CN115615437B - 一种因子图组合导航方法 - Google Patents
一种因子图组合导航方法 Download PDFInfo
- Publication number
- CN115615437B CN115615437B CN202211146118.6A CN202211146118A CN115615437B CN 115615437 B CN115615437 B CN 115615437B CN 202211146118 A CN202211146118 A CN 202211146118A CN 115615437 B CN115615437 B CN 115615437B
- Authority
- CN
- China
- Prior art keywords
- moment
- time
- integration
- increment
- compensation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 25
- 230000010354 integration Effects 0.000 claims abstract description 158
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 89
- 238000005457 optimization Methods 0.000 claims abstract description 49
- 230000001133 acceleration Effects 0.000 claims abstract description 39
- 239000011159 matrix material Substances 0.000 claims description 27
- 238000012937 correction Methods 0.000 claims description 25
- 230000005484 gravity Effects 0.000 claims description 21
- 230000001351 cycling effect Effects 0.000 claims description 13
- 238000013507 mapping Methods 0.000 claims description 13
- 238000012546 transfer Methods 0.000 claims description 11
- 238000005070 sampling Methods 0.000 claims description 10
- 238000004364 calculation method Methods 0.000 description 11
- 238000001914 filtration Methods 0.000 description 10
- 230000004927 fusion Effects 0.000 description 8
- 238000005259 measurement Methods 0.000 description 6
- 238000009795 derivation Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 2
- 238000009499 grossing Methods 0.000 description 2
- 238000005295 random walk Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005520 cutting process Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000009189 diving Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Mathematical Physics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Theoretical Computer Science (AREA)
- Computing Systems (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种因子图组合导航方法，采用ISAM1/ISAM2优化算法，增量求解因子图中的状态变量，对当前状态先基于预积分算法进行了一步地球自转以及有害加速度的粗补偿，然后随着传感器的不断输入，图优化的规模扩大，通过ISAM1/ISAM2算法对历史受影响的状态进行多次增量重补偿，当达到一定次数后历史节点趋于稳定，此时便不再对其进行重补偿。本发明采用高精度的IMU预积分算法，其定位精度相对于传统预积分算法得到了提高。同时由于基于因子图优化的方式，其具有强可扩展性，在不改变现有框架的基础上能更简易的融合更多传感器。最后采用ISAM1/ISAM2的算法计算量小，可实用性强。
Description
技术领域
本发明属于组合导航技术领域，涉及一种因子图组合导航方法，特别是一种融合ISAM1(Incremental Smoothing And Mapping)或ISAM2(Incremental Smoothing andMapping Using the Bayes Tree)与切空间上高精度惯性预积分的因子图组合导航方法。
背景技术
组合导航是指综合使用多种传感器由计算机控制的导航系统。如何有效的融合多个传感器一直是国内外研究的热点方向。目前，国内外众多研究学者对组合导航技术做了大量的研究和更新，并且随着制造业的发展传感器精度不断提高，组合导航的精度得到更进一步的提高。由于惯性器件具有极强的独立性并且能提供载体的全姿态信息，当下主流的组合系统大都以惯导为主导。基于分布式结构的联邦卡尔曼滤波由于其计算量小、扩展性强，容错性好，在多传感器融合上受到了应用广泛。但是目前基于分布式结构的联邦卡尔曼滤波以及其各类改进方法上依旧存在问题。由于导航传感器的测量频率不一致，而联邦卡尔曼滤波需要等待全部测量数据到达之后才能进行滤波融合，没有实时有效利用传感器的测量数据。而因子图优化的出现解决了滤波存在的一些问题。由于其“即插即用”的特性，使得因子图优化在多传感器融合时无需等待所有信息的到达，只要满足既定的优化条件即可进行信息的融合，大大提高了组合导航系统的实时性能，同时由于其强大的可扩展性使得其在组合导航领域上占据着重要地位。而由于因子图优化在组合导航领域的发展起步较滤波算法偏晚，且算法尚存在许多重要问题尚待探索和解决。高精度的IMU(InertialMeasurement Unit)具有极强的自主性和隐蔽性，在许多场景受到了广泛的应用，如深海的潜水器在其它传感器失效或者不能使用的情况下，为了不被敌人发现而仅依靠高精度IMU传感器进行自身的定位。目前，基于高精度IMU的因子图优化算法精度难以达到预期的定位精度，主要是由于其因子图优化中的预积分算法环节建模较为粗糙，忽略了大部分重要的影响因素，例如地球自转、科氏力等因素影响。
传统的预积分算法为了获得相邻优化变量之间的相对约束关系，将IMU的测量值在相邻两个优化时刻进行累积，避免了因为历史时刻状态的调整，导致后续因子图中的变量都需要重新进行复杂的IMU的递推积分计算。然而现有预积分算法较为粗糙，在计算相邻变量关系时未考虑到地球自转、科氏力等因素的影响。且预积分算法大都在图优化中使用，而在图优化中的非线性优化求解主要采用大规模的GN(Gauss Newton)或者LM(Levenberg-Marquard)算法，虽然采用了加速计算的方法，但是随着载体的运动时间增长，其传感器观测到的数据也会随时间增长，导致因子图优化的规模变大，计算时间变长。
发明内容
针对上述现有技术，本发明要解决的技术问题是提供一种融合ISAM1或ISAM2优化与切空间上高精度惯性预积分的因子图组合导航方法，提高定位精度和计算效率，提升可扩展性。
为解决上述技术问题，本发明的一种因子图组合导航方法，包括以下步骤：
步骤一：判断是否为执行第一次循环，当执行第一次循环时，获取并保存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时刻的平均加速度ab(k)和平均角速度wb(k)，把平均角速度wb(k)映射到切空间得到角速度的切向量w_tanb(k)，通过角速度的切向量w_tanb(k)和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，循环步骤一到十一，输出导航信息。
本发明还包括一种因子图组合导航方法，包括以下步骤：
步骤一：判断是否为执行第一次循环，当执行第一次循环时，获取并保存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时刻的平均加速度ab(k)和平均角速度wb(k)，把平均角速度wb(k)映射到切空间得到角速度的切向量w_tanb(k)，通过角速度的切向量w_tanb(k)和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，循环步骤一到十一，输出导航信息。
本发明还包括一种因子图组合导航方法，包括以下步骤：
步骤一：判断是否为执行第一次循环，当执行第一次循环时，获取并保存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时刻的平均加速度ab(k)和平均角速度wb(k)，把平均角速度wb(k)映射到切空间得到角速度的切向量w_tanb(k)，通过角速度的切向量w_tanb(k)和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时刻的状态加入因子图中；
步骤七：对因子图中的所有变量采用不带滑动窗口的ISAM2的批处理优化，假设历史需要重新线性化的时刻为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时刻的状态变量；
步骤十：根据不带滑动窗口的ISAM2算法的判断规则，判断是否所有历史时刻受影响的状态变量重新计算完毕，若计算完毕，则执行步骤十一，否则，则循环步骤七至步骤十；
步骤十一：令i＝j，循环步骤一到十一，输出导航信息。
本发明还包括一种因子图组合导航方法，包括以下步骤：
步骤一：判断是否为执行第一次循环，当执行第一次循环时，获取并保存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时刻的平均加速度ab(k)和平均角速度wb(k)，把平均角速度wb(k)映射到切空间得到角速度的切向量w_tanb(k)，通过角速度的切向量w_tanb(k)和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时刻的状态加入因子图中；
步骤七：对因子图中的所有变量采用带滑动窗口的ISAM2的批处理优化，在滑动窗口内，假设历史需要重新线性化的时刻为i，下一时刻为j，对预积分增量θi,j、pi,j、vi,j进行一阶加速度计和陀螺的零偏修正；
步骤八：根据带滑动窗口的ISAM2算法的判断规则，对历史需要重新线性的时刻i及下一时刻j之间预积分增量θi,j、pi,j、vi,j进行重力加速度的补偿；
步骤九：对补偿重力之后的i，j时刻之间的预积分增量θi,j补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对pi,j和vi,j补偿科氏力以及向心力，递推计算出j时刻的状态变量；
步骤十：根据带滑动窗口的ISAM2算法的判断规则，判断是否所有历史时刻受影响的状态变量重新计算完毕，若计算完毕，则执行步骤十一，否则，则循环步骤七至步骤十；
步骤十一：令i＝j，循环步骤一到十一，输出导航信息。
本发明的有益效果：本发明融合ISAM1/ISAM2优化与切空间上高精度IMU预积分的因子图优化导航算法，采用增量形式补偿地球自转、科氏力、向心力以及实时重力。其优点在于：采用了高精度的IMU预积分算法，其定位精度相对于传统预积分算法得到了提高。同时由于基于因子图优化的方式，其具有强可扩展性，在不改变现有框架的基础上能更简易的融合更多传感器。最后采用ISAM1/ISAM2的算法计算量小，可实用性强。本发明对于传感器异步输出能够实现实时且有效的融合，提高了算法的应用领域。由于本发明采用了融合ISAM1/ISAM2算法与高精度IMU预积分算法，优化速度较传统大规模的GN或者LM批处理算法更快速。同时为了保证长时间导航的计算效率，本发明还提出了一种在ISAM1/ISAM2中加入滑窗的算法，进一步提高了算法的实时性和实用性。
附图说明
图1是发明融合ISAM1/ISAM2与高精度惯性预积分算法的流程图；
图2是本发明的融合ISAM1/ISAM2与高精度惯性预积分的因子图结构图；
图3是仿真轨迹在二位平面的真值图；
图4是高精度IMU在XYZ轴输出的角增量；
图5是高精度IMU在XYZ轴输出的速度增量；
图6是传统因子图预积分算法的位置误差；
图7是传统因子图预积分算法的速度误差；
图8是传统因子图预积分算法的姿态误差；
图9是融合ISAM1改进预积分后的因子图算法的位置误差；
图10是融合ISAM1改进预积分后的因子图算法的速度误差；
图11是融合ISAM1改进预积分后的因子图算法的姿态误差；
图12是高精度卡尔曼滤波算法位置误差；
图13是高精度卡尔曼滤波与传统因子图优化算法位置误差；
图14是高精度卡尔曼滤波与融合ISAM1改进预积分后因子图优化算法位置误差；
图15是融合ISAM2改进预积分后的因子图算法的位置误差；
图16是融合ISAM2改进预积分后的因子图算法的速度误差；
图17是融合ISAM2改进预积分后的因子图算法的姿态误差；
图18是高精度卡尔曼滤波与融合ISAM2改进预积分后因子图优化算法位置误差。
具体实施方式
下面结合说明书附图和实施例对本发明做进一步说明。
本发明为提高高精度IMU在因子图优化导航定位时的精度，设计一种融合ISAM1/ISAM2优化与切空间上高精度IMU预积分的因子图优化导航算法，采用ISAM1/ISAM2优化算法，增量求解因子图中的状态变量，对当前状态先基于预积分算法进行了一步地球自转以及有害加速度的粗补偿，然后随着传感器的不断输入，图优化的规模扩大，通过ISAM1/ISAM2算法对历史受影响的状态进行多次增量重补偿，当达到一定次数后历史节点趋于稳定，此时便不再对其进行重补偿。同时为了保证实时性还加入了一种带滑窗的ISAM1/ISAM2算法可供选择。
结合图1，本发明包括如下步骤：
步骤一：判断代码是否为开机第一次执行步骤一，若为是则等待高精度IMU输出两个时刻的值，分别对应对k时刻和k-1时刻，并初始化系统。对于k时刻高精度IMU输出的值记为：和/>对于k-1时刻高精度IMU输出的值记为：/>和/>保存k时刻和k-1时刻的值。若判断为不是第一次执行步骤一，则获取当前k时刻高精度IMU的采集结果记为：/>和/>上述/>分别为IMU在载体坐标系b下k-1时刻所测得的载体相对于惯性系的角度增量和速度增量。/>分别为IMU在载体坐标系b下k时刻所测得的载体相对于惯性系的角度增量和速度增量。其中k时刻和k-1时刻为i时刻之后的某两个时刻，且0＜＝i＜＝k-1＜k。并且定义i时刻分别为每一次预积分的起始时刻，k时刻和k-1时刻为i时刻之后的某两个相邻时刻。
步骤二：在预积分的起始时刻i定义局部坐标系，在切空间上进行计算，把k时刻以及k-1时刻的和/>值，分别用i时刻的陀螺和加速度计的零偏进行校正。然后对速度增量补偿b系下k-1时刻的速度旋转误差补偿量/>和单子样加前一周期的b系下k-1时刻的划桨误差补偿量/>最后得到补偿后的b系下k-1时刻的结果/>对k时刻的角增量/>补偿单子样加前一周期b系下k-1时刻的圆锥误差得到k时刻圆锥运动补偿后的角度增量/>
步骤三：对补偿好后的和/>除以IMU的采样时间间隔，获得载体系下k-1时刻和k之间的平均加速度ab(k)和平均角速度wb(k)。在切空间中进行预积分的计算，需要把角速度wb(k)映射到切空间借助局部坐标得到角速度的切向量w_tanb(k)，通过角速度的切向量w_tanb(k)、和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时刻的值赋值给j时刻(j＝k)。把预积分因子加入因子图中i时刻和j时刻的状态变量之间。如图2所示，f1 IMU为0时刻和1时刻之间的预积分因子，而X0、V0、a0为0时刻的状态变量，由系统初始化时赋值，后面时刻状态变量的初值则由预积分算法递推计算得出。
步骤六：对θi,j补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对pi,j和vi,j补偿重力加速度、科氏力以及载体在地球表面运动产生的向心力，然后建模地球为椭球，递推计算出k时刻的状态加入因子图中
步骤七：在步骤七中分为以下四种情况，并且各个方案各自独立：
情况一为：对因子图中的所有变量采用ISAM1的批处理(不带滑动窗口)优化。
情况二为：根据开始选择滑窗的大小，用ISAM1算法对滑动窗口内的所有状态变量进行优化。
情况三为：对因子图中的所有变量采用ISAM2的批处理(不带滑动窗口)优化。
情况四为：对因子图中的所有变量采用ISAM2的批处理(不带滑动窗口)优化。
在优化过程中，在情况一下：根据ISAM1算法判断得出历史需要重新线性化的时刻(若需要线性化时刻设为i，则下一时刻为j)，对预积分增量(θi,j、pi,j、vi,j)进行加速度计和陀螺零偏的一阶重修正。情况二下：在滑动窗口内，根据ISAM1算法判断得出对历史需要重新线性化的时刻(若需要线性化时刻设为i，则下一时刻为j)，对预积分增量(θi,j、pi,j、vi,j)进行加速度计和陀螺零偏的一阶重修正。在情况三下：根据ISAM2算法判断得出历史需要重新线性化的时刻(若需要线性化时刻设为i，则下一时刻为j)，对预积分增量(θi,j、pi,j、vi,j)进行加速度计和陀螺零偏的一阶重修正。情况四下：在滑动窗口内，根据ISAM2算法判断得出历史需要重新线性化的时刻(若需要线性化时刻设为i，则下一时刻为j)，对预积分增量(θi,j、pi,j、vi,j)进行加速度计和陀螺零偏的一阶重修正。
步骤八：在优化过程中，情况一下：根据不带滑动窗口的ISAM1算法的判断规则，对历史需要重新线性时刻的状态变量(若假设需要线性化时刻设为i，i时刻的下一时刻设为j)，对i，j之间的预积分增量，进行重力加速度的更新与补偿。情况二下：根据带滑动窗口的ISAM1算法的判断规则，，对滑动窗口内历史需要重新线性化的时刻(若假设需要线性化时刻设为i，i时刻的下一时刻设为j)，对i，j时刻之间的预积分增量，进行重力加速度的更新与补偿。情况三下：根据不带滑动窗口的ISAM2算法的判断规则，对历史需要重新线性时刻的状态变量(若假设需要线性化时刻设为i，i时刻的下一时刻设为j)，对i，j之间的预积分增量，进行重力加速度的更新与补偿。情况四下：根据带滑动窗口的ISAM2算法的判断规则，对滑动窗口内历史需要重新线性化的时刻(若假设需要线性化时刻设为i，i时刻的下一时刻设为j)，对i，j时刻之间的预积分增量，进行重力加速度的更新与补偿。
步骤九：情况一：基于步骤八所得的补偿重力之后的i，j时刻之间的预积分增量θi,j、pi,j、vi,j，分别对θi,j补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对pi,j和vi,j补偿科氏力以及向心力，递推计算出j时刻的状态变量。情况二：基于步骤八所得的补偿重力之后的i，j时刻之间的预积分增量θi,j、pi,j、vi,j，分别对θi,j补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对pi,j和vi,j补偿科氏力以及向心力，递推计算出j时刻的状态变量。情况三：基于步骤八所得的补偿重力之后的i，j时刻之间的预积分增量θi,j、pi,j、vi,j，分别对θi,j补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对pi,j和vi,j补偿科氏力以及向心力，递推计算出j时刻的状态变量。情况四：基于步骤八所得的补偿重力之后的i，j时刻之间的预积分增量θi,j、pi,j、vi,j，分别对θi,j补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对pi,j和vi,j补偿科氏力以及向心力，递推计算出j时刻的状态变量。
步骤十：情况一：根据不带滑动窗口的ISAM1算法的判断规则，判断是否所有历史时刻受影响的状态变量重新计算完毕，若计算完毕，则执行步骤十一，否则，则循环步骤七至步骤十。情况二：根据带滑动窗口的ISAM1算法的判断规则，判断是否所有历史时刻受影响的状态变量重新计算完毕，若计算完毕，则执行步骤十一，否则，则循环步骤七至步骤十。情况三：根据不带滑动窗口的ISAM2算法的判断规则，判断是否所有历史时刻受影响的状态变量重新计算完毕，若计算完毕，则执行步骤十一，否则，则循环步骤七至步骤十。情况四：根据带滑动窗口的ISAM2算法的判断规则，判断是否所有历史时刻受影响的状态变量重新计算完毕，若计算完毕，则执行步骤十一，否则，则循环步骤七至步骤十。
步骤十一：令i＝j，循环步骤一到十二，形成一种融合ISAM1/ISAM2优化与切空间上高精度IMU预积分的因子图优化导航算法，输出导航信息。
本发明还包括以下一些结构特征：
所述步骤二中，其具体实现如下：
步骤二中和/>为上一时刻高精度IMU的输出，其计算方式如(1)所示。
步骤二中的b系下k-1时刻的速度旋转误差补偿量为：
步骤二中的单子样加前一周期b系下k-1时刻的速度划桨误差补偿量为：
经过速度的旋转误差补偿和单子样加前一周期的速度划桨误差补偿得 计算如下：
步骤二中的单子样加前一周期的圆锥运动误差补偿为：
步骤三中局部坐标系的定义如下：
定义预积分起点i时刻的初始姿态为Ri，定义一个局部映射从局部坐标映射到Ri的邻域：
步骤三中通过把补偿好后的和/>除以IMU采样时间间隔，获得载体系下k时刻的加速度ab(k)和角速度wb(k)具体如下：
其中，为载体系在k-1时刻到k时刻的旋转矩阵，/>计算如下：
其中角速度wb(k)映射到切空间借助局部坐标得到角速度的切向量w_tanb(k)如下：
w_tanb(k)＝H(θ)-1wb(k) (9)
其中H(θ)是以右扰动定义的雅克比矩阵，其计算如下：
其中θ为每次预积分传入的角度预积分增量。若为k时刻，则θ＝θi,k-1。
步骤三中的预积分增量递推计算如下：
其中当前时刻角度的预积分增量通过如下求得：
θi,k＝θi,k-1+w_tanb(k)Δtk,k-1 (11)
其中当前时刻位置的预积分增量通过如下求得：
其中当前时刻速度的预积分增量通过如下求得：
vk＝vk-1+Rkab(k)Δtk,k-1 (13)
其中θi,k、θi,k-1分别为当前时刻的角度预积分增量和上一时刻的角度预积分增量，pi,k、pi,k-1分别为当前时刻的位置预积分增量和上一时刻的位置预积分增量，vi,k、vi,k-1分别为当前时刻的速度预积分增量和上一时刻的速度预积分增量。Rk为当前时刻到初始预积分时刻i的旋转矩阵。Rk计算如下：
步骤三针对误差传递，需要分别计算预积分增量对于前一个时刻状态变量的偏导数，其求导结果如下：
当前预积分量对当前状态的姿态、位置、速度状态变量的偏导Ak为：
其中Ak一行一列位置元素的推导如下：
其中H(θi,k-1)-1wb(k)为角增量在切空间内的变化率，其中为：
其中Ak二行一列位置元素的推导如下：
其中Ak三行一列位置元素的推导同理可得。
当前预积分量对加速度计零偏状态变量的偏导Bk为：
当前预积分量对陀螺零偏状态变量的偏导Ck为：
步骤三的第k时刻的噪声可通过第k-1时刻的噪声计算得出，其噪声传递如下：
其中Σk为k时刻的噪声协方差矩阵，为9×9的方阵。Σk-1为k-1时刻的噪声协方差矩阵，为9×9的方阵。为k时刻加速度计的测量噪声，/>为k时刻测量角测速引入的噪声。通过噪声的传递可以获得预积分一段时间内的噪声矩阵。
步骤四的具体实现如下：判断是否存在除IMU以外的其他传感器的输入，若存在，则执行步骤五；否则，k＝k+1，则循环步骤一至四；
步骤五的具体实现如下：通过i，j之间的预积分增量θi,j、pi,j、vi,j和预积分残差的值来构建预积分因子。但是预积分残差不在步骤五中使用，而是在步骤七优化过程中计算出值。其通过步骤六基于i时刻的状态变量，计算出j时刻的状态变量记为然后计算已经存在于j时刻的状态变量记为预积分残差计算为：
/>
步骤六具体实现如下，其中重力更新计算如下：
g＝g0(1+5.27094e-3sin(lat)2+2.32718e-5sin(lat)4)-h(3.086e-6) (24)
其中lat为载体当前所在的纬度，h为载体当前所在的高度，g0为标准的地球重力加速度数值取为9.780325。载体当前所在的纬度值通过当前所在世界坐标系下的值反解获得。步骤六中的补偿地球自转角速率、载体运动引起的导航系旋转角速率、重力加速度、科氏力以及向心力和递推计算如下：
其中，Rn为导航系旋转补偿，其根据wie和wen计算得出，wie为在东北天导航系下，地球自转角速率。wen为东北天导航系下，载体运动引起的导航系旋转角速率。ΔRi,j由θi,j根据(14)计算得出。vmid为i、j之间的平均速度。
步骤七的零偏修正步骤如下：首先获得重新线性化时刻的预积分量增量，若为i时刻，则其预积分量增量为：θi,j、pi,j、vi,j。通过计算获得θi,j、pi,j、vi,j对加速度计零偏和陀螺零偏的雅克比矩阵。在获得最新时刻估计出的加速度计和陀螺零偏的值后，把预积分起始时刻的零偏与最新的零偏作差，求得零偏的变化量。通过加速度计零偏的雅克比乘以零偏的变化量获得预积分加速度计零偏修正量，通过陀螺零偏的雅克比乘以零偏的变化量获得预积分陀螺零偏修正量。最后分别把预积增量θi,j、pi,j、vi,j与预积分加速度计零偏修正量与预积分陀螺零偏修正量积分相加，得到修正后的预积分值。
步骤九与在本发明中为调用步骤六的公式(25)和步骤进行递推计算下一时刻。
本发明为一种融合ISAM1/ISMA2优化与切空间上高精度IMU预积分的因子图优化导航算法。其结构图如图2所示。实验轨迹真值如图3所示，通过西北工业大学严恭敏开源PSINS工具箱实现。其起始位置的纬度的值为0.597706293396019弧度，经度的值1.90083222404074弧度,高度的初始值为380米，具体运动为首先静止100s，然后向北以1米每秒的加速度加速飞行10秒，然后匀速保持100秒，接着左转弯(最终方向为正西，包括飞机先滚转、在转弯、最后反向滚转，最后恢复到滚转角为0度)，然后匀速保持100秒，接着右转弯450度(最终方向为正北，包括飞机先滚转、在转弯、最后反向滚转，最后恢复到滚转角为0度)，然后匀速保持100秒，接着飞机抬头10秒，然后匀速保持100秒，接着飞机低头10秒，然后匀速保持100秒，然后以2米每秒的加速度减速5秒减速至静止，最后静止100秒。仿真高精度惯导的参数指标如下：加速度计零偏为50ug，陀螺零偏为0.01deg/h，加速度计随机游走为10ug/sqrt(HZ)，陀螺随机游走为仿真时GNSS定位精度为1米。其加速度计和陀螺的增量输出如图4和图5所示。图6、图7、图8为传统预积分因子图优化的位置、速度、姿态与真值的误差。图9、图10、图11为使用高精度IMU预积分后的因子图优化结果，通过与的传统预积分的误差结果对比，本发明的高精度IMU预积分算法能够有效降低位置、速度、姿态在传统预积分算法上出现的突变，主要原因是高精度IMU算法更能有效适配高精度的IMU，由于高精度IMU能够测量到地球的转角速率，以及不能忽略时变重力和科里奥利力对其输出的影响。进一步的本发明还基于ISAM1/ISAM2在因子图相邻状态变量之间根据预积分递推上进行了补偿，进一步提高了算法精度。图12为高精度卡尔曼滤波算法的位置定位误差结果。图13和图14分别为未改进的预积分算法与本实验改进后的预积分算法在位置上误差的对比图，图15、图16、图17分别为融合ISAM2改进预积分后的因子图算法的位置、速度、误差，图18为高精度卡尔曼滤波和融合ISAM2改进预积分算法的位置误差。本发明提出的高精度IMU预积分算法进行了一系列有害加速度的补偿，因此算法精度较传统预积分算法有提升，通过仿真实验表明，本发明融合ISAM1/ISAM2与高精度惯性预积分的因子图组合导航算法有效解决了传统预积分算法在高精度IMU上表现不理想的情况。/>
Claims (4)
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时刻的平均加速度ab(k)和平均角速度wb(k)，把平均角速度wb(k)映射到切空间得到角速度的切向量w_tanb(k)，通过角速度的切向量w_tanb(k)和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，循环步骤一到十一，输出导航信息。
2.一种因子图组合导航方法，其特征在于，包括以下步骤：
步骤一：判断是否为执行第一次循环，当执行第一次循环时，获取并保存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时刻的平均加速度ab(k)和平均角速度wb(k)，把平均角速度wb(k)映射到切空间得到角速度的切向量w_tanb(k)，通过角速度的切向量w_tanb(k)和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，循环步骤一到十一，输出导航信息。
3.一种因子图组合导航方法，其特征在于，包括以下步骤：
步骤一：判断是否为执行第一次循环，当执行第一次循环时，获取并保存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时刻的平均加速度ab(k)和平均角速度wb(k)，把平均角速度wb(k)映射到切空间得到角速度的切向量w_tanb(k)，通过角速度的切向量w_tanb(k)和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时刻的状态加入因子图中；
步骤七：对因子图中的所有变量采用不带滑动窗口的ISAM2的批处理优化，假设历史需要重新线性化的时刻为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时刻的状态变量；
步骤十：根据不带滑动窗口的ISAM2算法的判断规则，判断是否所有历史时刻受影响的状态变量重新计算完毕，若计算完毕，则执行步骤十一，否则，则循环步骤七至步骤十；
步骤十一：令i＝j，循环步骤一到十一，输出导航信息。
4.一种因子图组合导航方法，其特征在于，包括以下步骤：
步骤一：判断是否为执行第一次循环，当执行第一次循环时，获取并保存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时刻的平均加速度ab(k)和平均角速度wb(k)，把平均角速度wb(k)映射到切空间得到角速度的切向量w_tanb(k)，通过角速度的切向量w_tanb(k)和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时刻的状态加入因子图中；
步骤七：对因子图中的所有变量采用带滑动窗口的ISAM2的批处理优化，在滑动窗口内，假设历史需要重新线性化的时刻为i，下一时刻为j，对预积分增量θi,j、pi,j、vi,j进行一阶加速度计和陀螺的零偏修正；
步骤八：根据带滑动窗口的ISAM2算法的判断规则，对历史需要重新线性的时刻i及下一时刻j之间预积分增量θi,j、pi,j、vi,j进行重力加速度的补偿；
步骤九：对补偿重力之后的i，j时刻之间的预积分增量θi,j补偿地球自转角速率以及载体运动引起的导航系旋转角速率，分别对pi,j和vi,j补偿科氏力以及向心力，递推计算出j时刻的状态变量；
步骤十：根据带滑动窗口的ISAM2算法的判断规则，判断是否所有历史时刻受影响的状态变量重新计算完毕，若计算完毕，则执行步骤十一，否则，则循环步骤七至步骤十；
步骤十一：令i＝j，循环步骤一到十一，输出导航信息。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211146118.6A CN115615437B (zh) | 2022-09-20 | 2022-09-20 | 一种因子图组合导航方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211146118.6A CN115615437B (zh) | 2022-09-20 | 2022-09-20 | 一种因子图组合导航方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115615437A CN115615437A (zh) | 2023-01-17 |
| CN115615437B true CN115615437B (zh) | 2024-04-30 |
Family
ID=84858670
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211146118.6A Active CN115615437B (zh) | 2022-09-20 | 2022-09-20 | 一种因子图组合导航方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115615437B (zh) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111238535A (zh) * | 2020-02-03 | 2020-06-05 | 南京航空航天大学 | 一种基于因子图的imu误差在线标定方法 |
| CN113175933A (zh) * | 2021-04-28 | 2021-07-27 | 南京航空航天大学 | 一种基于高精度惯性预积分的因子图组合导航方法 |
| CN113514064A (zh) * | 2021-04-23 | 2021-10-19 | 南京航空航天大学 | 一种鲁棒因子图多源容错导航方法 |
-
2022
- 2022-09-20 CN CN202211146118.6A patent/CN115615437B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111238535A (zh) * | 2020-02-03 | 2020-06-05 | 南京航空航天大学 | 一种基于因子图的imu误差在线标定方法 |
| CN113514064A (zh) * | 2021-04-23 | 2021-10-19 | 南京航空航天大学 | 一种鲁棒因子图多源容错导航方法 |
| CN113175933A (zh) * | 2021-04-28 | 2021-07-27 | 南京航空航天大学 | 一种基于高精度惯性预积分的因子图组合导航方法 |
Non-Patent Citations (2)
| Title |
|---|
| Improved Preintegration Method for GNSS/IMU/In-Vehicle Sensors Navigation Using Graph Optimization;Bai, SY et al.;IEEE TRANSACTIONS ON VEHICULAR TECHNOLOGY;20211130;全文 * |
| 基于滑动窗迭代最大后验估计的多源组合导航因子图融合算法;徐昊玮;廉保旺;刘尚波;;兵工学报;20190415(04);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115615437A (zh) | 2023-01-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108827299B (zh) | 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法 | |
| CN110398257B (zh) | Gps辅助的sins系统快速动基座初始对准方法 | |
| CN111024064B (zh) | 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法 | |
| CN112013836A (zh) | 一种基于改进自适应卡尔曼滤波的航姿参考系统算法 | |
| CN110702143B (zh) | 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法 | |
| CN105806363B (zh) | 基于srqkf的sins/dvl水下大失准角对准方法 | |
| CN103900574B (zh) | 一种基于迭代容积卡尔曼滤波姿态估计方法 | |
| CN109931955B (zh) | 基于状态相关李群滤波的捷联惯性导航系统初始对准方法 | |
| CN109945859B (zh) | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 | |
| CN109682377A (zh) | 一种基于动态步长梯度下降的姿态估计方法 | |
| CN112798021B (zh) | 基于激光多普勒测速仪的惯导系统行进间初始对准方法 | |
| CN108225370A (zh) | 一种运动姿态传感器的数据融合与解算方法 | |
| CN112665574B (zh) | 基于动量梯度下降法的水下机器人姿态采集方法 | |
| CN103674059A (zh) | 一种基于外测速度信息的sins水平姿态误差修正方法 | |
| CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
| CN116147624B (zh) | 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法 | |
| CN111121820B (zh) | 基于卡尔曼滤波的mems惯性传感器阵列融合方法 | |
| CN108645404A (zh) | 一种小型多旋翼无人机姿态解算方法 | |
| CN114216456A (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
| CN111190207B (zh) | 基于pstcsdref算法的无人机ins bds组合导航方法 | |
| CN110595434B (zh) | 基于mems传感器的四元数融合姿态估计方法 | |
| CN113175926B (zh) | 一种基于运动状态监测的自适应水平姿态测量方法 | |
| CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
| CN111207734B (zh) | 一种基于ekf的无人机组合导航方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |