一种因子图优化组合导航方法技术

本发明专利技术公开了一种因子图优化组合导航方法，采用ISAM2优化算法，在流型空间上，增量求解因子图中的状态变量，并基于当前状态和预积分增量进行了地球自转及有害加速度的补偿，然后随着系统运行时间的增长，因子图优化规模逐渐扩大，通过ISMA2算法对历史受影响的时刻进行多次的重补偿，当达到一定次数后历史节点趋于稳定，此时ISAM2算法便不再调用补偿算法对其进行重补偿。本发明专利技术采用在流型空间上的高精度的IMU预积分算法，其定位精度相对于传统预积分算法得到了提高，同时由于基于因子图优化的方式，其可扩展性强，可实现多传感器的即插即用，并且采用ISAM2的算法计算量小，实用性强。强。强。

【技术实现步骤摘要】
一种因子图优化组合导航方法


[0001]本专利技术属于组合导航
，涉及一种因子图优化组合导航方法，特别是一种融合ISAM2(Incremental Smoothing and Mapping Using the Bayes Tree)与流型空间上高精度惯性测量单元(Inertial Measurement Unit,IMU)预积分的因子图优化组合导航方法。

技术介绍

[0002]近年来随着导航技术的快速发展，组合导航所用传感器种类逐渐增多，一方面传统基于卡尔曼滤波技术的方法在一些场景应用受限，另一方面基于滤波技术对现有数据利用不够充分,且在滤波融合时数据易受到“污染”情况，导致其精度难以更进一步的提升。而因子图优化算法的出现有效解决滤波算法存在的问题，进一步提高了组合导航的定位精度。然而由于IMU的高频率测量，导致因子图优化算法难以实时计算，为解决IMU的高频测量，国外学者T.Lupton于2012年首次提出了IMU预积分算法。但是目前基于因子图优化的算法主要采用低精度的MEMS(Microelectro Mechanical Syste本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种因子图优化组合导航方法，其特征在于，包括以下步骤：步骤一：判断代码是否为执行第一次循环，若为是，获取并保存IMU输出的k时刻数据和和k
‑
1时刻数据和并初始化系统；否则获取并保存当前k时刻IMU输出的k时刻数据和其中分别为IMU在载体坐标系b下k时刻测得的载体相对于惯性系的角度增量和速度增量；定义预积分的起始时刻为i时刻，且0≤i≤k
‑
1；步骤二：用i时刻的陀螺和加速度计的零偏分别对和和进行校正，然后对校正后的速度增量补偿b系下k
‑
1时刻的速度的旋转误差补偿量和单子样加前一周期的b系下k
‑
1时刻的划桨误差补偿量最后得到补偿后的b系下k
‑
1时刻的结果对校正后的角度增量进行单子样加前一周期的圆锥运动误差补偿得到k
‑
1时刻圆锥运动补偿后的角度增量步骤三：和除以IMU的采样时间间隔，获得载体系下k
‑
1时刻和k时刻的平均加速度a
b(k)
和平均角速度w
b(k)
，然后求得k
‑
1时刻到k时刻的角度预积分增量φ
k
‑
1,k
、位置预积分增量速度预积分增量然后在流型空间上求得i时刻到k时刻的角度预积分增量位置预积分增量速度预积分增量并进行误差传递矩阵的计算；步骤四：判断是否存在除IMU以外的其他传感器的输入，若存在，则执行步骤五；否则，k＝k+1，则循环步骤一至四；步骤五：根据上述的步骤三中i时刻至k时刻的预积分增量创建预积分因子，令预积分终止时刻j＝k，把预积分因子加入因子图中i时刻和j时刻的状态变量之间；步骤六：在流型空间上，对补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对和补偿重力加速度、科氏力以及载体在地球表面运动产生的向心力，然后建模地球为椭球，递推计算出j时刻的状态加入因子图中；步骤七：进行ISAM2优化，计算i时刻和j时刻的预积分残差加入因子图中，假设需要线性化时刻设为i，下一时刻为j，对预积分增量进行加速度计和陀螺零偏的一阶重修正；步骤八：在流型空间上，首先对步骤七中的预积分增量和进行重力的更新与补偿，然后对补偿地球自转角速率以及载体运动引起的导航系旋转角速率，最后对和补偿科氏力以及向心力，递推计算出j时刻的状态，并更新i、j时刻状态之间的预积分残差；步骤九：根据ISAM2的判断规则，判断是否历史所有受影响时刻的状态已经全部重新计算完毕，若计算完毕则执行步骤十，否则继续循环步骤七和步骤九；步骤十：令i＝j，输出导航信息...

【专利技术属性】
技术研发人员：丁继成，黄承林，王峰，杨福鑫，杨皓，何欣庭，胡亚南，
申请(专利权)人：哈尔滨工程大学，
类型：发明
国别省市：