申请/专利权人：哈尔滨工程大学

申请日：2022-09-20

公开（公告）日：2024-04-30

公开（公告）号：CN115615437B

主分类号：G01C21/20

分类号：G01C21/20;G01C21/16;G06F18/25;G06F17/16

优先权：

专利状态码：有效-授权

法律状态：2024.04.30#授权;2023.02.10#实质审查的生效;2023.01.17#公开

摘要：本发明公开了一种因子图组合导航方法，采用ISAM1ISAM2优化算法，增量求解因子图中的状态变量，对当前状态先基于预积分算法进行了一步地球自转以及有害加速度的粗补偿，然后随着传感器的不断输入，图优化的规模扩大，通过ISAM1ISAM2算法对历史受影响的状态进行多次增量重补偿，当达到一定次数后历史节点趋于稳定，此时便不再对其进行重补偿。本发明采用高精度的IMU预积分算法，其定位精度相对于传统预积分算法得到了提高。同时由于基于因子图优化的方式，其具有强可扩展性，在不改变现有框架的基础上能更简易的融合更多传感器。最后采用ISAM1ISAM2的算法计算量小，可实用性强。

主权项：1.一种因子图组合导航方法，其特征在于，包括以下步骤：步骤一：判断是否为执行第一次循环，当执行第一次循环时，获取并保存IMU输出的k时刻数据和和k-1时刻数据和并初始化系统；否则获取并保存当前k时刻IMU输出的k时刻数据和分别为IMU在载体坐标系b下k时刻所测得的载体相对于惯性系的角度增量和速度增量；预积分的起始时刻为i时刻，且0≤i≤k-1；步骤二：在预积分的起始时刻i定义局部坐标系，在切空间上进行计算，利用i时刻的陀螺和加速度计的零偏分别对进行校正，然后对速度增量补偿b系下k-1时刻的速度旋转误差补偿量和单子样加前一周期的b系下k-1时刻的划桨误差补偿量最后得到补偿后的b系下k-1时刻的结果对角度增量进行单子样加前一周期的圆锥运动误差补偿得到k时刻圆锥运动补偿后的角度增量步骤三：和除以IMU的采样时间间隔，获得载体系下k-1时刻和k时刻的平均加速度abk和平均角速度wbk，把平均角速度wbk映射到切空间得到角速度的切向量w_tanbk，通过角速度的切向量w_tanbk和i时刻到k时刻的旋转矩阵Rk和i时刻到k-1时刻的角度预积分增量θi,k-1、位置预积分增量pi,k-1、速度预积分增量vi,k-1求得k时刻的预积分增量θi,k、pi,k、vi,k，并进行误差传递矩阵计算；步骤四：判断是否存在除IMU以外的其他传感器的输入，若存在，则执行步骤五；否则，k＝k+1，则循环步骤一至四；步骤五：根据上述的步骤三中i时刻至k时刻的预积分增量创建预积分因子，令预积分终止时刻j＝k，把预积分因子加入因子图中i时刻和j时刻的状态变量之间；步骤六：对θi,j补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对pi,j和vi,j补偿重力加速度、科氏力以及载体在地球表面运动产生的向心力，然后建模地球为椭球，递推计算出k时刻的状态加入因子图中；步骤七：对因子图中的所有变量采用不带滑动窗口的ISAM1的批处理优化，假设历史需要重新线性化的时刻为i，下一时刻为j，对预积分增量θi,j、pi,j、vi,j进行一阶加速度计和陀螺的零偏修正；步骤八：根据不带滑动窗口的ISAM1算法的判断规则，对历史需要重新线性的时刻i及下一时刻j之间预积分增量θi,j、pi,j、vi,j进行重力加速度的补偿；步骤九：对补偿重力之后的i，j时刻之间的预积分增量θi,j补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对pi,j和vi,j补偿科氏力以及向心力，递推计算出j时刻的状态变量；步骤十：根据不带滑动窗口的ISAM1算法的判断规则，判断是否所有历史时刻受影响的状态变量重新计算完毕，若计算完毕，则执行步骤十一，否则，则循环步骤七至步骤十；步骤十一：令i＝j，循环步骤一到十一，输出导航信息。

全文数据：

权利要求：

百度查询： 哈尔滨工程大学 一种因子图组合导航方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD