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

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

摘要：本申请属于机器人定位技术领域。本申请提供一种基于视觉惯性与RTK松耦合的室内外无缝切换定位方法。该方法包括：通过对IMU信息进行激励检测，选取受噪声影响小的GPS信息参与初始化。通过构建误差状态向量进行第一扩展卡尔曼滤波，补偿第一后验误差状态，求解第一全局位姿。考虑到GPS信息变化时出现的位置突变，设计加入负反馈的第一扩展卡尔曼滤波方式实现定位结果的平稳过渡。在GPS信息不可用的情况下，退化为视觉惯性定位系统，锁定第一误差状态向量，调整滤波策略，实现载体进出拒止环境的无缝全局定位。本公开实施例可以降低了运算量，实现了载体进出拒止环境过程过渡稳定的恒精度无缝衔接定位。

主权项：1.一种基于视觉惯性与RTK松耦合的室内外无缝切换定位方法，其特征在于，该方法包括：对图像信息和IMU信息进行处理，以得到载体在相对坐标系下的局部位姿；具体包括：对图像信息进行特征点提取和匹配，且基于图像信息进行纯视觉SFM，以得到各帧间的相对运动S；对IMU信息进行预积分处理，并对预积分处理后的IMU信息和各帧间的相对运动进行视觉惯性对齐，以得到陀螺仪零偏；根据陀螺仪零偏对IMU信息进行预积分处理，得到各帧的速度、重力矢量和尺度信息，以完成视觉惯性系统的初始化；基于初始化的视觉惯性系统计算IMU预积分残差、视觉重投影误差、边缘化的先验信息，并根据IMU预积分残差、视觉重投影误差、边缘化的先验信息构建紧耦合优化模型；根据关键帧构建滑动窗口对紧耦合优化模型进行优化求解，以得到载体在相对坐标系下的局部位姿；利用GPS信息获取ENU坐标系下的ENU位置，并对ENU位置进行可靠性筛选，以得到可用的ENU位置信息；具体包括：将GPS信息中的经度信息、纬度信息和高度信息从经纬高坐标系转换到地心地固坐标系下；将GPS信息中的经度信息、纬度信息和高度信息从地心地固坐标系转换到ENU坐标系下，以得到ENU位置；通过GPS信息中的误差协方差矩阵对GPS信息的可靠性进行筛选，当GPS信息可用时，将GPS信息转化为可用的ENU位置信息；对可用的ENU位置信息对应时刻的IMU信息进行激励检测，再将通过检测的可用的ENU位置信息和局部位姿进行时间对齐，以得到从相对坐标系到ENU坐标系的转移矩阵；具体包括：根据广义似然比检验构建激励检测函数，利用激励检测函数对可用的ENU位置信息对应时刻的IMU信息进行激励检测；将激励检测函数的值大于预设的阈值所对应的可用的ENU位置信息记为通过检测的可用的ENU位置信息；对通过检测的可用的ENU位置信息和局部位姿进行时间对齐，利用时间对齐后的位置对构建最小二乘问题，并求取从相对坐标系到ENU坐标系的转移矩阵；将局部位姿经转移矩阵转换到ENU坐标系下，以得到ENU坐标系下的视觉惯性定位结果；将ENU坐标系下的视觉惯性定位结果与ENU位置做差，以得到第一误差状态向量，利用第一扩展卡尔曼滤波器对第一误差状态向量进行误差滤波，以得到第一后验误差状态，并将第一后验误差状态补偿到视觉惯性定位结果中，以得到载体在ENU坐标系下的第一全局位姿；具体包括：将旋转得到的视觉惯性定位结果与GPS观测得到的ENU位置做差，以得到第一误差状态向量；其中，第一误差状态向量包括位置误差、速度误差、姿态误差、加速度偏差误差和陀螺仪偏差误差；利用第一扩展卡尔曼滤波器对第一误差状态向量进行误差滤波，以得到第一后验误差状态；将第一后验误差状态补偿到视觉惯性定位结果中，平滑滤波得到载体在ENU坐标系下的第一全局位姿；当GPS信息不可用后，锁定第一误差状态向量，利用第二扩展卡尔曼滤波器进行滤波，以得到载体在ENU坐标系下的第二全局位姿；具体包括：利用第二扩展卡尔曼滤波器对第一误差状态向量进行滤波，以得到第二后验误差状态；将第二后验误差状态补偿到视觉惯性定位结果中，以得到载体在ENU坐标系下的第二全局位姿；当GPS信息恢复后跳变时，将第二全局位姿与当前的ENU位置进行处理后，反馈到第一扩展卡尔曼滤波器中，以得到载体在ENU坐标系下的第三全局位姿；具体包括：当GPS信息恢复后跳变时，保持跳变前的第一误差状态向量作为第二误差状态向量；将第二全局位姿与当前的ENU位置进行负反馈得到反馈值；利用第一扩展卡尔曼滤波器对第二误差状态向量和反馈值进行误差滤波，以得到第三后验误差状态；将第三后验误差状态补偿到视觉惯性定位结果中，以得到载体在ENU坐标系下的第三全局位姿。

全文数据：

权利要求：

百度查询： 西北工业大学 基于视觉惯性与RTK松耦合的室内外无缝切换定位方法