CN109916395A - 一种姿态自主冗余组合导航算法 - Google Patents
一种姿态自主冗余组合导航算法 Download PDFInfo
- Publication number
- CN109916395A CN109916395A CN201910271058.2A CN201910271058A CN109916395A CN 109916395 A CN109916395 A CN 109916395A CN 201910271058 A CN201910271058 A CN 201910271058A CN 109916395 A CN109916395 A CN 109916395A
- Authority
- CN
- China
- Prior art keywords
- algorithm
- attitude
- navigation
- data
- coordinate system
- 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.)
- Granted
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 149
- 238000005259 measurement Methods 0.000 claims abstract description 48
- 230000004927 fusion Effects 0.000 claims abstract description 20
- 238000001914 filtration Methods 0.000 claims abstract description 14
- 239000011159 matrix material Substances 0.000 claims description 35
- 238000004364 calculation method Methods 0.000 claims description 32
- 238000012937 correction Methods 0.000 claims description 22
- 238000000034 method Methods 0.000 claims description 20
- 230000001133 acceleration Effects 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 9
- 238000005295 random walk Methods 0.000 claims description 6
- 230000007704 transition Effects 0.000 claims description 6
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 4
- 230000005484 gravity Effects 0.000 claims description 4
- 239000011541 reaction mixture Substances 0.000 claims description 4
- 230000003044 adaptive effect Effects 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000012886 linear function Methods 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 150000001875 compounds Chemical class 0.000 claims 1
- 238000010586 diagram Methods 0.000 description 4
- 230000005389 magnetism Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 2
- 238000007796 conventional method Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000002035 prolonged effect Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
本发明提供一种姿态自主冗余组合导航算法，包含一套自主式的AHRS算法和一套组合导航算法，两套算法在同一导航周期内相继完成解算。AHRS算法采用自适应矢量叉乘滤波，利用MEMS IMU和磁力计数据进行解算出载体航向和姿态数据；组合导航算法基于载体动力学模型，利用IMU、磁力计、气压高度计、GNSS数据，采用高维度扩展卡尔曼滤波完成数据融合，解算出位置、速度和姿态信息。本算法中AHRS算法属于纯自主算法，与组合导航算法中的姿态数据互为冗余、热备份，在GNSS失效后依然为载体提供可靠的航向、姿态信息，在GNSS重新定位后为组合导航算法提供初始姿态，使组合导航算法快速收敛。
Description
技术领域
本发明涉及组合导航算法技术领域，特别是涉及一种姿态自主冗余组合导航算法。
背景技术
现有组合导航算法中，不存在姿态数据冗余，在卫星导航(GNSS)信号失效后，处于纯惯导状态，位置、速度和姿态误差很快发散至无法使用，无法为载体提供可靠的航向、姿态信息；当GNSS重新定位后，无初始姿态信息，组合导航算法需要重新初始对准，收敛时间更长，甚至算法发散。
发明内容
本发明所要解决的技术问题是：为了克服现有技术中的不足，本发明提供一种基于微机电惯性测量组合/卫星导航/磁力计/气压高度计(MEMS IMU/GNSS/Magnetometer/Baro)的姿态自主冗余组合导航算法，解决纯组合导航算法中不存在姿态数据冗余，以及GNSS重新定位后收敛时间长的问题。向AHRS状态姿态算法的切换问题，使算法切换由硬切换转换到软切换，确保算法在切换间快速收敛。
本发明解决其技术问题所要采用的技术方案是：
在基于微机电惯性测量组合/卫星导航/磁力计/气压高度计(MEMS IMU/GNSS/Magnetometer/Baro)的组合导航算法中，其中，MEMS IMU可简写为IMU，当GNSS失效时，廉价且低精度的MEMS IMU无法维持纯惯性导航定位精度，GNSS失效后，在短时间内组合导航算法就必须退化到AHRS状态，确保为载体继续提供姿态测量。组合导航算法切换到AHRS状态，这涉及到状态方程和量测方程的切换，本发明的算法中包含一套自主式的AHRS算法和一套组合导航算法，两套算法在同一导航周期内相继完成解算，以保证快速切换和收敛。
本发明所涉及的组合导航系统包括安装于载体上的MEMS IMU、GNSS、磁力计和气压高度计，其中，
IMU包含三轴正交陀螺仪和三轴正交加速度计，分别用于测量角速度和加速度(比力)；GNSS接收机用于输出卫星导航的位置和速度；三轴正交磁力计，用于地磁测量；气压高度计用于测量大气压强，从而换算出气压高度。
本发明涉及到的坐标系包括载体坐标系、站心坐标系、东北天地理坐标系、导航坐标系、地磁东北天坐标系，其中，载体坐标系指以装载导航系统的载体的右前上坐标系，安装于载体上的具有坐标的传感器(IMU、磁力计)其安装与载体坐标系重合；站心坐标系为载体导航起始点为原点的东北天地理坐标系，组合导航的位置估计以站心坐标系表示；东北天地理坐标系的原点为载体质心，GNSS的速度量测以及状态中的速度估计表示在本坐标系中，且认为等同于表示于站心坐标系中；在本算法中导航坐标系采用站心坐标系；地磁东北天坐标系以磁北为北向，东北天地理坐标系天向轴旋转一个地磁偏角即与地磁东北天坐标系重合。
一种姿态自主冗余组合导航算法，包括一套自主式的AHRS算法和一套组合导航算法，两套算法在同一导航周期内相继完成解算，还包括以下步骤：
S1：由导航计算机读取安装于载体上的IMU、磁力计、气压高度计以及卫GNSS接收机的数据；
S2：在导航周期内，利用步骤S1中获得的IMU数据和磁力计数据进行自适应矢量叉乘滤波数据融合，解算出载体航向和姿态信息，实现独立且完全自主的AHRS算法；
S3：完成载体姿态解算后，判断GNSS是否更新，如果GNSS数据没有更新，则进入步骤S4；如果卫星导航GNSS数据有更新，则进入步骤S5；具体的GNSS接收机会输出导航更新标志位，判断该标志位即可。
S4：在GNSS未更新情况下，对捷联纯惯性导航时间t1进行累加计时，根据IMU的仪表精度可以确定捷联纯惯性导航误差累积到失效的门限时间T1，当捷联纯惯性导航时间t1超过该门限时间T1时，即t1＞T1，则认为捷联纯惯性导航误差偏大，捷联纯惯性导航结果失效，导航系统仅输出纯AHRS算法的姿态数据；当捷联纯惯性导航时间t1未超过该门限时间T1时，即t1≤T1，则认为捷联纯惯性导航误差在可接受的范围内，载体同时输出AHRS算法姿态以及捷联纯惯性导航算法的位置、速度和姿态；
S5：判断是否为GNSS捕获0时刻，GNSS捕获时刻可以通过GNSS接收机获取，如果是GNSS定位计时t2清零，以纯AHRS算法的姿态数据、GNSS的位置和速度数据作为组合导航的初始数据，重置组合导航算法参数，然后再利用IMU数据、GNSS数据、磁力计数据和气压高度计数据进行组合导航数据融合，解算载体的位置、速度和姿态，并对GNSS定位计时t2累加一次，且将捷联纯惯性导航累加时间t1清零；如果不是，则直接利用IMU数据、GNSS数据、磁力计数据和气压高度计数据进行组合导航数据融合，解算载体的位置、速度和姿态，并对GNSS定位计时t2累加一次，且将捷联纯惯性导航累加时间t1清零，如果t2达到组合导航重新收敛的门限值T2后，全套导航参数有效，否则输出纯AHRS算法姿态数据。
在整个算法过程中AHRS算法是处于一直有效状态，当GNSS重新定位时，AHRS算法姿态可以为GNSS提供一个初始姿态；如果没有AHRS提供初始姿态，GNSS重新定位后组合导航需要重新对准，算法收敛时间变长，且对准环境复杂，可能出现算法发散。
具体的，姿态数据融合算法，即AHRS算法，是一种纯自主式的姿态算法，为载体提供精度可靠的航向角、俯仰角和横滚角。为缩短整个算法计算时间，姿态算法在满足精度和载体抗机动的前提下，要求计算量尽可能小。
步骤S2中AHRS算法以适应矢量叉乘滤波算法作为数据融合工具，首先，通过加速度计输出计算出当前k时刻角速度校正量eak，以及通过磁力计输出计算当前k时刻角速度校正量emk，然后，通过计算获得的角速度校正量eak和emk，对陀螺仪角速度进行校正，上述处理过程中的数据均需转化到载体坐标系中。
具体计算过程如下：
(1)通过加速度计输出计算出当前k时刻角速度校正量eak。
IMU中加速度计输出的原始加速度记为[ax ay az]T，对加速度计输出进行归一化处理，归一化后的加速度记为[axk ayk azk]T，则在载体坐标系中计算当前k时刻角速度校正量eak：
其中，[0 0 1]T为归一化后的重力加速度；为导航坐标系到载体坐标系的姿态矩阵；Pacc为加速度计对姿态校正的权重系数。
(2)通过磁力计输出计算当前k时刻角速度校正量emk。
磁力计输出的原始地磁矢量记为[mx my mz]T，对磁力计输出进行归一化处理，归一化后的地磁矢量记为[mxk myk mzk]T，则在载体坐标系中计算当前k时刻角速度校正量emk：
其中，Pmag为磁力计对角速度校正的权重系数；[mE mN mU]T为归一化的当地地理坐标系(东北天)下的地磁矢量。
归一化的当地地理坐标系下的地磁矢量[mE mN mU]T的计算方法如下：
首先，将载体坐标系下，归一化的磁力计地磁矢量[mxk myk mzk]T转换到地理坐标系下：
其中，[mE1 mN1 mU1]T为地理坐标系下地磁矢量；为载体坐标系到导航坐标系的姿态矩阵。
接着，利用地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T：
最后，补偿磁偏角，将地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到地理坐标系下的地磁矢量[mE mN mU]T：
其中，Mag_dec为以弧度为单位的地磁偏角，根据当地经纬度查询得到。
(3)对陀螺仪角速度进行校正。
由上述计算获得的角速度校正量eak和emk，对陀螺仪角速度进行校正，则k时刻校正后的陀螺仪角速度输出[gxk gyk gzk]T计算：
其中，[gx gy gz]T为IMU中陀螺仪输出的原始角速度数据；Ki为积分系数；Kp为比例系数。
式(1)、(2)、(6)中，Pacc、Pmag、Ki和Kp为可调参数，该四项参数根据加速度计输出得到的振动量级以及陀螺仪输出角速度大小(姿态角变化)进行自适应调整，确保在不同振动量级以及不同姿态角变化快慢情况下的姿态解算精度和姿态收敛速度；通过式(6)计算后的陀螺仪角速度即可按照常规方法，如：解四元数微分方程或等效旋转矢量算法，进行四元数更新并解算出载体姿态。本算法优点：计算量少、收敛性好、鲁棒性好、完全自主。
组合导航算法采用站心坐标系，即载体初始位置的东北天坐标系，组合导航算法状态方程为：
其中，为系统状态，为系统噪声；
式(7)具体表示如下：
其中，系统状态包括：3维位置3维速度4维姿态四元数3维陀螺零偏和3维加速度计零偏共16维；系统噪声包括：3维陀螺仪白噪声3维加速度计白噪声3维陀螺零偏随机游走噪声和3维加速度计零偏随机游走噪声共12维；为四元数的乘法矩阵；为IMU中加速度计输出加速度(比力)、为IMU中陀螺仪输出角速度；根据状态微分方程式(8)可求解状态一步预测。
组合导航算法量测方程：
其中，表示量测；为量测由状态量表示的非线性函数；表示量测噪声。
式(9)中，量测为：
式(10)中，量测由3维GNSS位置量测3维GNSS速度量测3维磁力计量测1维气压高度HBaro，共10维量测构成.
式(9)中，可以表示为：
其中，为归一化的当地地理坐标系下的地磁矢量，其计算方法以及过程参见公式(3)-公式(5)；Pz为状态中的高度；将状态一步预测值代入式(11)，即得到量测预测值；
式(9)中，量测噪声包括3维GNSS位置量测噪声、3维GNSS速度量测噪声、3维归一化后的磁力计输出量测噪声、1维气压高度输出量测噪声。
式(9)中位置和速度的量测预测采用纯惯性递推值，磁力计输出的预测与AHRS算法中计算方法相同，气压高度的预测同样根据纯惯性高度方向递推。每一次气压高度的量测值，需要扣除站心位置静态下气压高度值。
对式(8)和式(11)，计算雅克比矩阵，得到状态转移矩阵Φ、系统噪声驱动矩阵Γ以及量测矩阵H。
状态一步预测值通过式(8)解微分方程获得：
式(12)中为上一导航周期状态估计值，T代表导航周期。
状态转移矩阵Φ计算：
式(13)中I为单位矩阵。
系统噪声驱动矩阵Γ计算：
量测矩阵H计算：
完成以上计算，即可利用扩展卡尔曼滤波即可完成数据融合，解算出载体的位置、速度和姿态信息。
其中，需要注意的是，在AHRS算法和组合导航算法中均涉及到了导航坐标系到载体坐标系的姿态矩阵由于本算法为姿态冗余算法，在AHRS算法和组合导航算法中都存在姿态解算，在组合导航算法中涉及到的由组合导航解算的姿态四元数构成，而在AHRS算法中所涉及到的由AHRS算法计算得到的姿态四元数构成；仅在组合导航算法需要重新对准时，组合导航使用AHRS姿态数据进行对准。
本发明的有益效果是：本发明提供的一种基于微机电惯性测量组合/卫星导航/磁力计/气压高度计(MEMS IMU/GNSS/Magnetometer/Baro)的姿态自主冗余组合导航算法。算法包含一套自主式的AHRS算法和一套组合导航算法，两套算法在同一导航周期内相继完成解算。AHRS算法采用自适应矢量叉乘滤波，利用IMU和磁力计数据进行解算出载体航向和姿态数据，属于纯自主AHRS算法。组合导航算法基于载体动力学模型，利用IMU、磁力计、气压高度计、GNSS数据，采用高维度扩展卡尔曼滤波完成数据融合，解算出位置、速度和姿态信息。本算法具备姿态数据完全自主、冗余、热备份；在卫星导航失效，纯惯性累积误差变大后AHRS算法继续为载体提供高精度的航向姿态信息；在卫星导航信号恢复后，为AHRS算法为导航算法提供初始姿态，使导航算法快速收敛；本算法中AHRS算法计算量极小，组合导航算法采用站心坐标系，滤波方程简单，计算量也小，本算法导航周期可以达到5ms以内，同时导航算法收敛性好、鲁棒性好。
附图说明
下面结合附图和实施例对本发明作进一步说明。
图1是本发明姿态自主冗余组合导航算法原理框图。
图2是本发明AHRS数据融合算法原理框图。
图3是本发明组合导航算法原理框图。
图4是本发明姿态自主冗余组合导航算法运行流程图。
具体实施方式
现在结合附图对本发明作详细的说明。此图为简化的示意图，仅以示意方式说明本发明的基本结构，因此其仅显示与本发明有关的构成。
如图1所示，本发明所涉及的组合导航系统包括安装于载体上的IMU、GNSS、磁力计和气压高度计，其中，
IMU包含三轴正交陀螺仪和三轴正交加速度计，分别用于测量角速度和加速度(比力)；GNSS接收机用于输出卫星导航的位置和速度；三轴正交磁力计，用于地磁测量；气压高度计用于测量大气压强，从而换算出气压高度。
如图1-图4所示，一种姿态自主冗余组合导航算法，包括一套自主式的AHRS算法和一套组合导航算法，两套算法在同一导航周期内相继完成解算，还包括以下步骤：
S1：由导航计算机读取安装于载体上的IMU、磁力计、气压高度计以及卫星导航GNSS接收机的数据；
S2：在导航周期内，利用步骤S1中获得的IMU数据和磁力计数据进行自适应矢量叉乘滤波数据融合，解算出载体航向和姿态信息，实现独立且完全自主的AHRS算法；
S3：完成载体姿态解算后，判断GNSS是否失效，如果卫星导航GNSS数据没有更新，则说明GNSS失效，进入步骤S4；如果卫星导航GNSS数据有更新，则说明GNSS有效，进入步骤S5；具体的GNSS接收机会输出导航更新标志位，判断该标志位即可。
S4：在GNSS未更新情况下，对捷联纯惯性导航时间t1进行累加计时，根据IMU的仪表精度可以确定捷联纯惯性导航误差累积到失效的门限时间T1，当捷联纯惯性导航时间t1超过该门限时间T1时，即t1＞T1，则认为捷联纯惯性导航误差偏大，捷联纯惯性导航结果失效，导航系统仅输出纯AHRS算法的姿态数据；当捷联纯惯性导航时间t1未超过该门限时间T1时，即t1≤T1，则认为捷联纯惯性导航误差在可接受的范围内，载体同时输出AHRS算法姿态以及捷联纯惯性导航算法的位置、速度和姿态；T1取值大小即为捷联纯惯性导航精度维持时间，由IMU中陀螺仪和加速度计精度以及实际应用平台决定，应通过仿真及在实际应用平台上对捷联纯惯性维持时间进行测试验证后确定T1的取值。
S5：判断是否为GNSS捕获0时刻，GNSS捕获时刻可以通过GNSS接收机获取，如果是GNSS定位计时t2清零，以纯AHRS算法的姿态数据、GNSS的位置和速度数据作为组合导航的初始数据，重置组合导航算法参数，然后再利用IMU数据、GNSS数据、磁力计数据和气压高度计数据进行组合导航数据融合，解算载体的位置、速度和姿态，并对GNSS定位计时t2累加一次，且将捷联纯惯性导航累加时间t1清零；如果不是，则直接利用IMU数据、GNSS数据、磁力计数据和气压高度计数据进行组合导航数据融合，解算载体的位置、速度和姿态，并对GNSS定位计时t2累加一次，且将捷联纯惯性导航累加时间t1清零，如果t2达到组合导航重新收敛的门限值T2后，全套导航参数有效，否则输出纯AHRS算法姿态数据。T2取值大小由传感器噪声特性、实际应用平台环境等因素确定，应通过仿真及在实际应用平台上进行测试验证后确定。
在整个算法过程中AHRS算法是处于一直有效状态，当GNSS重新定位时，AHRS算法姿态可以为GNSS提供一个初始姿态；如果没有AHRS提供初始姿态，GNSS重新定位后组合导航需要重新对准，算法收敛时间变长，且对准环境复杂，可能出现算法发散。
如图2所示，姿态数据融合算法，即AHRS算法，是一种纯自主式的姿态算法，为载体提供精度可靠的航向角、俯仰角和横滚角。为缩短整个算法计算时间，姿态算法在满足精度和载体抗机动的前提下，要求计算量尽可能小。
步骤S2中AHRS算法以适应矢量叉乘滤波算法作为数据融合工具，首先，通过加速度计输出计算出当前k时刻角速度校正量eak，以及通过磁力计输出计算当前k时刻角速度校正量emk，然后，通过计算获得的角速度校正量eak和emk，对陀螺仪角速度进行校正，上述处理过程中的数据均需转化到载体坐标系中。
具体计算过程如下：
(1)通过加速度计输出计算出当前k时刻角速度校正量eak。
IMU中加速度计输出的原始加速度记为[ax ay az]T，对加速度计输出进行归一化处理，归一化后的加速度记为[axk ayk azk]T，则在载体坐标系中计算当前k时刻角速度校正量eak：
其中，[0 0 1]T为归一化后的重力加速度；为导航坐标系到载体坐标系的姿态矩阵；Pacc为加速度计对姿态校正的权重系数。
(2)通过磁力计输出计算当前k时刻角速度校正量emk。
磁力计输出的原始地磁矢量记为[mx my mz]T，对磁力计输出进行归一化处理，归一化后的地磁矢量记为[mxk myk mzk]T，则在载体坐标系中计算当前k时刻角速度校正量emk：
其中，Pmag为磁力计对角速度校正的权重系数；[mE mN mU]T为归一化的当地地理坐标系(东北天)下的地磁矢量。
归一化的当地地理坐标系下的地磁矢量[mE mN mU]T的计算方法如下：
首先，将载体坐标系下，归一化的磁力计地磁矢量[mxk myk mzk]T转换到地理坐标系下：
其中，[mE1 mN1 mU1]T为地理坐标系下地磁矢量；为载体坐标系到导航坐标系的姿态矩阵。
接着，利用地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T：
最后，补偿磁偏角，将地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到地理坐标系下的地磁矢量[mE mN mU]T：
其中，Mag_dec为以弧度为单位的地磁偏角，根据当地经纬度查询得到。
(3)对陀螺仪角速度进行校正。
由上述计算获得的角速度校正量eak和emk，对陀螺仪角速度进行校正，则k时刻校正后的陀螺仪角速度输出[gxk gyk gzk]T计算：
其中，[gx gy gz]T为IMU中陀螺仪输出的原始角速度数据；Ki为积分系数；Kp为比例系数。
式(1)、(2)、(6)中，Pacc、Pmag、Ki和Kp为可调参数，该四项参数根据加速度计输出得到的振动量级以及陀螺仪输出角速度大小(姿态角变化)进行自适应调整，确保在不同振动量级以及不同姿态角变化快慢情况下的姿态解算精度和姿态收敛速度；通过式(6)计算后的陀螺仪角速度即可按照常规方法，如：解四元数微分方程或等效旋转矢量算法，进行四元数更新并解算出载体姿态。本算法优点：计算量少、收敛性好、鲁棒性好、完全自主。
如图3所示，组合导航算法采用站心坐标系，即载体初始位置的东北天坐标系，组合导航算法状态方程为：
其中，为系统状态，为系统噪声；
式(7)具体表示如下：
其中，为3维位置，为3维速度，为4维姿态四元数，为3维陀螺零偏，为3维加速度计零偏，共16维；系统噪声由3维陀螺仪白噪声3维加速度计白噪声3维陀螺零偏随机游走噪声3维加速度计零偏随机游走噪声构成，共12维。；为四元数的乘法矩阵；为IMU中加速度计输出加速度(比力)、为IMU中陀螺仪输出角速度；根据状态微分方程式(8)求解状态一步预测。
组合导航算法量测方程：
其中，表示量测；为量测由状态量表示的非线性函数；表示量测噪声；
式(9)中，量测可以表示为：
式(10)中，量测由3维GNSS位置量测3维GNSS速度量测3维磁力计量测1维气压高度HBaro，共10维量测构成。
式(9)中，可以表示为：
其中，为归一化的当地地理坐标系下的地磁矢量，其计算方法以及过程参见公式(3)-公式(5)；Pz为状态中的高度；将状态一步预测值代入式(11)，即得到量测预测值；
式(9)中，量测噪声包括3维GNSS位置量测噪声、3维GNSS速度量测噪声、3维归一化后的磁力计输出量测噪声、1维气压高度输出量测噪声。
式(9)中位置和速度的量测预测采用纯惯性递推值，磁力计输出的预测与AHRS算法中计算方法相同，气压高度的预测同样根据纯惯性高度方向递推。每一次气压高度的量测值，需要扣除站心位置静态下气压高度值。
对式(8)和式(11)，计算雅克比矩阵，得到状态转移矩阵Φ、系统噪声驱动矩阵Γ以及量测矩阵H。
状态一步预测值通过式(8)解微分方程获得：
式(12)中为上一导航周期状态估计值，T代表导航周期。
状态转移矩阵Φ计算：
式(13)中I为单位矩阵。
系统噪声驱动矩阵Γ计算：
量测矩阵H计算：
完成以上计算，即可利用扩展卡尔曼滤波即可完成数据融合，解算出载体的位置、速度和姿态信息。
其中，需要注意的是，在AHRS算法和组合导航算法中均涉及到了导航坐标系到载体坐标系的姿态矩阵由于本算法为姿态冗余算法，在AHRS算法和组合导航算法中都存在姿态解算，在组合导航算法中涉及到的由组合导航解算的姿态四元数构成，而在AHRS算法中所涉及到的由AHRS算法计算得到的姿态四元数构成；仅在组合导航算法需要重新对准时，组合导航使用AHRS姿态数据进行对准。
以上述依据本发明的理想实施例为启示，通过上述的说明内容，相关的工作人员完全可以在不偏离本发明的范围内，进行多样的变更以及修改。本项发明的技术范围并不局限于说明书上的内容，必须要根据权利要求范围来确定其技术性范围。
Claims (4)
1.一种姿态自主冗余组合导航算法，其特征在于：包含一套自主式的AHRS算法和一套组合导航算法，两套算法在同一导航周期内相继完成解算，还包括以下步骤：
S1：由导航计算机读取安装于载体上的IMU、磁力计、气压高度计以及GNSS的数据；
S2：在导航周期内，利用步骤S1中获得的IMU数据和磁力计数据进行自适应矢量叉乘滤波数据融合，解算出载体航向和姿态信息，实现独立自主的AHRS算法；
S3：完成载体姿态解算后，判断GNSS是否更新，如果GNSS数据没有更新，进入步骤S4；如果GNSS数据有更新，则进入步骤S5；
S4：在GNSS未更新情况下，对捷联纯惯性导航时间t1进行累加计时，根据IMU的仪表精度可以确定捷联纯惯性导航误差累积到失效的门限时间T1，当捷联纯惯性导航时间t1超过该门限时间T1时，即t1＞T1，则认为捷联纯惯性导航误差偏大，捷联纯惯性导航结果失效，导航系统仅输出纯AHRS算法的姿态数据；当捷联纯惯性导航时间t1未超过该门限时间T1时，即t1≤T1，则认为捷联纯惯性导航误差在可接受的范围内，载体同时输出AHRS算法姿态以及捷联纯惯性导航算法的位置、速度和姿态；
S5：判断是否为GNSS捕获0时刻，如果是GNSS定位计时t2清零，以纯AHRS算法的姿态数据、GNSS的位置和速度数据作为组合导航的初始数据，重置组合导航算法参数，然后再利用IMU数据、GNSS数据、磁力计数据和气压高度计数据进行组合导航数据融合，解算载体的位置、速度和姿态，并对GNSS定位计时t2累加一次，且将捷联纯惯性导航累加时间t1清零；如果不是，则直接利用IMU数据、GNSS数据、磁力计数据和气压高度计数据进行组合导航数据融合，解算载体的位置、速度和姿态，并对GNSS定位计时t2累加一次，且将捷联纯惯性导航累加时间t1清零，如果t2达到组合导航重新收敛的门限值T2后，全套导航参数有效，否则输出纯AHRS算法姿态数据。
2.如权利要求1所述的姿态自主冗余组合导航算法，其特征在于：步骤S2中AHRS算法以适应矢量叉乘滤波算法作为数据融合工具，计算过程如下：
载体坐标系下，IMU中加速度计输出的原始加速度记为[ax ay az]T，对加速度计输出进行归一化处理，归一化后的加速度记为[axk ayk azk]T，则在载体坐标系中计算当前k时刻角速度校正量eak：
其中，[0 0 1]T为归一化后的重力加速度，为导航坐标系到载体坐标系的姿态矩阵；Pacc为加速度计对姿态校正的权重系数；
载体坐标系下，磁力计输出的原始地磁矢量记为[mx my mz]T，对磁力计输出进行归一化处理，归一化后的地磁矢量记为[mxk myk mzk]T，则在载体坐标系中计算当前k时刻角速度校正量emk：
其中，Pmag为磁力计对角速度校正的权重系数；[mE mN mU]T为归一化的当地地理坐标系下的地磁矢量；
由上述计算获得的角速度校正量eak和emk，对陀螺仪角速度进行校正，则k时刻校正后的陀螺仪角速度输出[gxk gyk gzk]T计算：
其中，[gx gy gz]T为IMU中陀螺仪输出的原始角速度数据；Ki为积分系数；Kp为比例系数；
根据校正后的陀螺仪角速度采用解四元数微分方程或等效旋转矢量算法进行四元数更新并解算出载体姿态。
3.如权利要求2所述的姿态自主冗余组合导航算法，其特征在于：归一化的当地地理坐标系下的地磁矢量[mE mN mU]T的计算方法为：
首先，将载体坐标系下，归一化的磁力计地磁矢量[mxk myk mzk]T转换到地理坐标系：
其中，[mE1 mN1 mU1]T为地理坐标系下地磁矢量；为载体坐标系到导航坐标系的姿态矩阵；
接着，利用地理坐标系下地磁矢量[mE1 mN1 mU1]T，重构出地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T：
最后，将地磁东北天坐标系下的地磁矢量[0 mN2 mU2]T，通过地磁偏角的补偿转换到地理坐标系下的地磁矢量[mE mN mU]T：
其中，Mag_dec为以弧度为单位的地磁偏角，根据当地经纬度查询得到。
4.如权利要求3所述的姿态自主冗余组合导航算法，其特征在于：所述组合导航算法采用站心坐标系，即载体初始位置为原点的东北天地理坐标系，组合导航算法状态方程为：
其中，为系统状态，为系统噪声；
式(7)具体表示如下：
其中，系统状态包括：3维位置3维速度4维姿态四元数3维陀螺零偏和3维加速度计零偏共16维；系统噪声包括：3维陀螺仪白噪声3维加速度计白噪声3维陀螺零偏随机游走噪声和3维加速度计零偏随机游走噪声共12维；为四元数的乘法矩阵；为IMU中加速度计输出加速度(比力)、为IMU中陀螺仪输出角速度；根据状态微分方程式(8)求解状态一步预测；
组合导航算法量测方程：
其中，表示量测；为量测由状态量表示的非线性函数；表示量测噪声；
式(9)中，表示10维量测，由以下量测量构成：
式(10)中，表示3维GNSS位置量测、表示3维GNSS速度量测、表示3维由磁力计输出归一化后的磁力计量测、HBaro表示1维气压高度；
式(9)中，可以表示为：
其中，为归一化的当地地理坐标系下的地磁矢量；Pz为状态中的高度；将状态一步预测值代入式(11)，即得到量测预测值；
式(9)中，量测噪声包括3维GNSS位置量测噪声、3维GNSS速度量测噪声、3维归一化后的磁力计输出量测噪声、1维气压高度输出量测噪声；
对式(8)和式(11)，计算雅克比矩阵，得到状态转移矩阵Φ、系统噪声驱动矩阵Γ以及量测矩阵H；
状态一步预测值通过式(8)解微分方程获得：
式(12)中为上一导航周期状态估计值，T代表导航周期；
状态转移矩阵Φ计算：
式(13)中I为单位矩阵；
系统噪声驱动矩阵Γ计算：
量测矩阵H计算：
完成以上计算，即可利用扩展卡尔曼滤波即可完成数据融合，解算出载体的位置、速度和姿态信息。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910271058.2A CN109916395B (zh) | 2019-04-04 | 2019-04-04 | 一种姿态自主冗余组合导航算法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910271058.2A CN109916395B (zh) | 2019-04-04 | 2019-04-04 | 一种姿态自主冗余组合导航算法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109916395A true CN109916395A (zh) | 2019-06-21 |
| CN109916395B CN109916395B (zh) | 2023-06-23 |
Family
ID=66968701
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910271058.2A Active CN109916395B (zh) | 2019-04-04 | 2019-04-04 | 一种姿态自主冗余组合导航算法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109916395B (zh) |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110824524A (zh) * | 2019-11-13 | 2020-02-21 | 西安通航装备科技开发有限公司 | 一种基于机载Ka波段的卫星视频传输系统 |
| CN110954097A (zh) * | 2019-12-13 | 2020-04-03 | 陕西瑞特测控技术有限公司 | 一种用于机器人组合的导航定位方法 |
| CN111158363A (zh) * | 2019-12-27 | 2020-05-15 | 苏州博田自动化技术有限公司 | 基于控制时序组合的宏微导航方法 |
| CN111964688A (zh) * | 2020-07-10 | 2020-11-20 | 北京航空航天大学 | 结合无人机动力学模型和mems传感器的姿态估计方法 |
| WO2021027638A1 (zh) * | 2019-08-09 | 2021-02-18 | 深圳市道通智能航空技术有限公司 | 一种偏航角的融合方法、装置及飞行器 |
| CN112650281A (zh) * | 2020-12-14 | 2021-04-13 | 一飞(海南)科技有限公司 | 多传感器三余度系统、控制方法、无人机、介质及终端 |
| CN113063416A (zh) * | 2021-02-05 | 2021-07-02 | 重庆大学 | 一种基于自适应参数互补滤波的机器人姿态融合方法 |
| CN113108779A (zh) * | 2021-03-11 | 2021-07-13 | 上海航天控制技术研究所 | 一种基于1553b总线三冗余的自主箭测系统 |
| CN116952309A (zh) * | 2023-09-20 | 2023-10-27 | 中国船舶集团有限公司第七〇七研究所 | 一种应用于定向测量设备的姿态和振动监测方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030149528A1 (en) * | 2002-02-06 | 2003-08-07 | Ching-Fang Lin | Positioning and navigation method and system thereof |
| CN103363992A (zh) * | 2013-06-29 | 2013-10-23 | 天津大学 | 基于梯度下降的四旋翼无人机姿态航向参考系统解算方法 |
| CN103744101A (zh) * | 2014-01-02 | 2014-04-23 | 上海大学 | 一种低成本ahrs辅助gps确定整周模糊度装置及方法 |
| CN104864865A (zh) * | 2015-06-01 | 2015-08-26 | 济南大学 | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 |
| CN106443746A (zh) * | 2016-07-19 | 2017-02-22 | 招商局重庆交通科研设计院有限公司 | 一种低成本双天线gnss/ahrs组合测姿方法 |
| CN107861522A (zh) * | 2017-12-15 | 2018-03-30 | 北京盈想东方科技股份有限公司 | 无人靶机控制系统 |
-
2019
- 2019-04-04 CN CN201910271058.2A patent/CN109916395B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20030149528A1 (en) * | 2002-02-06 | 2003-08-07 | Ching-Fang Lin | Positioning and navigation method and system thereof |
| CN103363992A (zh) * | 2013-06-29 | 2013-10-23 | 天津大学 | 基于梯度下降的四旋翼无人机姿态航向参考系统解算方法 |
| CN103744101A (zh) * | 2014-01-02 | 2014-04-23 | 上海大学 | 一种低成本ahrs辅助gps确定整周模糊度装置及方法 |
| CN104864865A (zh) * | 2015-06-01 | 2015-08-26 | 济南大学 | 一种面向室内行人导航的ahrs/uwb无缝组合导航方法 |
| CN106443746A (zh) * | 2016-07-19 | 2017-02-22 | 招商局重庆交通科研设计院有限公司 | 一种低成本双天线gnss/ahrs组合测姿方法 |
| CN107861522A (zh) * | 2017-12-15 | 2018-03-30 | 北京盈想东方科技股份有限公司 | 无人靶机控制系统 |
Cited By (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2021027638A1 (zh) * | 2019-08-09 | 2021-02-18 | 深圳市道通智能航空技术有限公司 | 一种偏航角的融合方法、装置及飞行器 |
| CN110824524B (zh) * | 2019-11-13 | 2021-10-26 | 西安通航装备科技开发有限公司 | 一种基于机载Ka波段的卫星视频传输系统 |
| CN110824524A (zh) * | 2019-11-13 | 2020-02-21 | 西安通航装备科技开发有限公司 | 一种基于机载Ka波段的卫星视频传输系统 |
| CN110954097A (zh) * | 2019-12-13 | 2020-04-03 | 陕西瑞特测控技术有限公司 | 一种用于机器人组合的导航定位方法 |
| CN111158363A (zh) * | 2019-12-27 | 2020-05-15 | 苏州博田自动化技术有限公司 | 基于控制时序组合的宏微导航方法 |
| CN111964688A (zh) * | 2020-07-10 | 2020-11-20 | 北京航空航天大学 | 结合无人机动力学模型和mems传感器的姿态估计方法 |
| CN112650281A (zh) * | 2020-12-14 | 2021-04-13 | 一飞(海南)科技有限公司 | 多传感器三余度系统、控制方法、无人机、介质及终端 |
| CN112650281B (zh) * | 2020-12-14 | 2023-08-22 | 一飞(海南)科技有限公司 | 多传感器三余度系统、控制方法、无人机、介质及终端 |
| CN113063416A (zh) * | 2021-02-05 | 2021-07-02 | 重庆大学 | 一种基于自适应参数互补滤波的机器人姿态融合方法 |
| CN113063416B (zh) * | 2021-02-05 | 2023-08-08 | 重庆大学 | 一种基于自适应参数互补滤波的机器人姿态融合方法 |
| CN113108779B (zh) * | 2021-03-11 | 2022-10-28 | 上海航天控制技术研究所 | 一种基于1553b总线三冗余的自主箭测系统 |
| CN113108779A (zh) * | 2021-03-11 | 2021-07-13 | 上海航天控制技术研究所 | 一种基于1553b总线三冗余的自主箭测系统 |
| CN116952309A (zh) * | 2023-09-20 | 2023-10-27 | 中国船舶集团有限公司第七〇七研究所 | 一种应用于定向测量设备的姿态和振动监测方法 |
| CN116952309B (zh) * | 2023-09-20 | 2023-12-15 | 中国船舶集团有限公司第七〇七研究所 | 一种应用于定向测量设备的姿态和振动监测方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109916395B (zh) | 2023-06-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109916395B (zh) | 一种姿态自主冗余组合导航算法 | |
| CN109931926B (zh) | 一种基于站心坐标系的小型无人机无缝自主式导航方法 | |
| CN108051866B (zh) | 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法 | |
| CN111678538B (zh) | 一种基于速度匹配的动态水平仪误差补偿方法 | |
| CN106500693B (zh) | 一种基于自适应扩展卡尔曼滤波的ahrs算法 | |
| CN101706281B (zh) | 惯性/天文/卫星高精度组合导航系统及其导航方法 | |
| CN104698485B (zh) | 基于bd、gps及mems的组合导航系统及导航方法 | |
| CN104422948A (zh) | 一种嵌入式组合导航系统及其方法 | |
| CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航系统及方法 | |
| CN111121766B (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
| CN112432642B (zh) | 一种重力灯塔与惯性导航融合定位方法及系统 | |
| CN109916394A (zh) | 一种融合光流位置和速度信息的组合导航算法 | |
| CN111722295B (zh) | 一种水下捷联式重力测量数据处理方法 | |
| CN109612460B (zh) | 一种基于静止修正的垂线偏差测量方法 | |
| US11408735B2 (en) | Positioning system and positioning method | |
| CN113834483A (zh) | 一种基于可观测度的惯性/偏振/地磁容错导航方法 | |
| CN116147624B (zh) | 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法 | |
| CN111189474A (zh) | 基于mems的marg传感器的自主校准方法 | |
| CN116448145A (zh) | 一种基于偏振矢量空间差分的航姿确定方法 | |
| CN111307114B (zh) | 基于运动参考单元的水面舰船水平姿态测量方法 | |
| CN110058324B (zh) | 利用重力场模型的捷联式重力仪水平分量误差修正方法 | |
| JP2018052489A (ja) | 機首方位測定システムにおけるセンサ測定の欠如を補償するシステムと方法 | |
| CN110095117A (zh) | 一种无陀螺惯性量测系统与gps组合的导航方法 | |
| CN110514201B (zh) | 一种惯性导航系统及适用于高转速旋转体的导航方法 | |
| CN114509071B (zh) | 一种风洞试验模型姿态测量方法 |
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 |