买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：西北工业大学

摘要：本发明公开了一种视觉惯性里程计与间断性RTK融合的绝对定位方法，首先进行RTK‑VIO系统初始化，完成RTK所在地球地理坐标系以及VIO坐标系和ENU坐标系之间的转换；然后获取当前位置的RTK值和VIO值，并转换到ENU坐标系；接下里利用VIO所估计位姿与RTK所估计位姿构建观测残差优化问题；构建滑动窗口，将步骤3所得残差按优质RTK信号发生事件的时间顺序加入到滑动窗口中，进行滚动优化，得到一个最新的校准事件；最终将校准事件平滑化，保证无人机的安全稳定飞行。本发明弥补了视觉惯性里程计误差累积和RTK受间歇性遮挡无法准确定位的缺点，实现了相对地理坐标系切空间的实时鲁棒的绝对定位。

主权项：1.一种视觉惯性里程计与间断性RTK融合的绝对定位方法，其特征在于，包括如下步骤：步骤1：RTK-VIO系统初始化；步骤1-1：RTK以地球地理坐标系作为参考系，以经度、纬度和椭球高度表示；VIO坐标系为局部笛卡尔坐标系，以无人机起始点为坐标原点，坐标系Z轴与水平面垂直，以三维向量表示位置，以四元数表示姿态；步骤1-2：选择待检测设施周围任一点作为原点，建立东北天坐标系，即ENU坐标系；将ENU坐标系作为RTK-VIO系统的全局参考坐标系；ENU坐标系的原点在地球地理坐标系中具有固定的经度、纬度和椭球高度；步骤1-3：使用公式组1和2将地球地理坐标系下的经度、纬度和椭球高度转化到以地球为中心的地球固定坐标系下，得到地球固定坐标系的坐标 其中，λ、h分别地球为地理坐标系的经度、纬度和椭球高度；a和b分别是地球的赤道半径和极半径； 采用公式3将地球固定坐标系的转化为ENU坐标系下的坐标 其中，为ENU坐标系的原点在地球固定坐标系的坐标，λr分别为ENU坐标系的原点在地球地理坐标系下的经度和纬度；步骤1-4：设在无人机运动过程中，ENU坐标系下的RTK在n个观测点的位置估计为VIO坐标系下相同时刻的VIO位置估计为则有： 其中，GpV为VIO坐标系到ENU坐标系的平移向量，为VIO坐标系到ENU坐标系的旋转矩阵； 表示如式5： 其中，γ为VIO坐标系到ENU坐标系的偏航角；步骤1-5：定义和表示两个相邻观测点i和j的RTK位置估计，和表示两个相邻观测点i和j在VIO坐标系下对应时刻的VIO位置估计；设置权重因子ΔTj表示第j个采样时间；根据式6计算第i个观测点VIO坐标系到ENU坐标系的偏航角γi的正弦值和余弦值： 取无人机开始运动后的最初N1个观测点，得到N1个偏航角的正弦值和余弦值，使用权重因子ηj对N1个偏航角的正弦值和余弦值分别加权平均并归一化，得到估计量sinγ和cosγ，再利用反正切函数得到最终偏航角γ；将γ代入式5得到旋转矩阵的估计量再将旋转矩阵的估计量多次代入式4，取平均求出GpV的估计值完成RTK-VIO系统的初始化；步骤2：观测值构建；获取当前无人机的RTK定位值，使用式1到式3转换到ENU坐标系中，得到RTK观测量设定滑动窗口长度为N2，将RTK观测量与滑动窗口内时间最早的RTK观测量作差求得位移观测量获取当前无人机的VIO定位值，并利用步骤1得到的将VIO坐标系下的VIO定位值转换到ENU坐标系中，得到VIO观测量再将VIO观测量与滑动窗口内时间最早的VIO观测量作差求得位移观测量步骤3：构建优化问题；待估计的状态量为由第i次位置观测和位移观测构造残差，如式7所示： 将所有残差进行组合，构建能量函数Eχ如式8： 使用Ceres最小二乘求解器迭代求解χ，对残差进行扰动，推导残差雅可比如式9： 其中，·∧表示求反对称阵算子，Exp·＝exp·∧，为定义在上的指数映射；δp表示位移扰动，δφ表示旋转扰动；选择χ的优化初始值为步骤1所得令δφ的更新方式为：其中M的形式如式10： 步骤4：滚动优化；在能量函数中加入边缘化先验残差rprior： 其中，先验残差rprior分为两项，即姿态变换残差和位置变换残差，定义为： 其中，Log·＝log·∨，为定义在SO3上的对数映射；使用最小二乘优化求解优化问题，按照RTK定位观测点的时间顺序在滑动窗口中计算，得到一个最新的待估计状态量，即一个校准事件；每当有新的状态量被获取，则剔除滑动窗口中时间最旧的状态量；步骤5：校准事件平滑化；定义每个采样周期的最小偏航角矫正量ωmin和最小位置矫正量vmin，最大偏航角矫正量ωmax和最大位置矫正量vmax；当偏航角矫正量和位置矫正量均小于每个采样周期的最小偏航角矫正量ωmin和最小位置矫正量vmin，则直接予以更新；当偏航角矫正量介于最小和最大偏航角量矫正量之间，位置矫正量介于最小和最大位置矫正量之间，使用式13更新矫正量； 其中，γcur代表当前的偏航角估计，分别代表估计得到的最新偏航角和上一次估计得到的偏航角，即pcur代表当前的位移量估计，分别代表估计得到的最新位移量和上一次估计得到的位移量；ω表示偏航角矫正量，v表示位置矫正量；实现校准事件的平滑化，保证无人机的安全稳定飞行。

全文数据：

权利要求：

百度查询： 西北工业大学 一种视觉惯性里程计与间断性RTK融合的绝对定位方法