一种用于航姿系统的自适应姿态估计方法及其平滑切换算法与流程

1.本发明涉及一种用于航姿系统的自适应姿态估计方法及其平滑切换算法，适用于航空导航技术领域。
技术背景
2.随着微机电技术的发展，三轴mems陀螺仪、加速度计、磁力计成为最主要的惯性测量元件。但现有的mems器件均存在某些缺陷：mems陀螺仪存在漂移，单独使用陀螺仪估计姿态存在累积误差；mems加速度计受线性加速度和震动影响较大，单独利用加速度计估计姿态角，会出现较大误差；mems磁力计易受周围环境所产生的磁场干扰，从而影响航向角的输出。由于此类误差是随机的，无法预先消除，因此不能单独通过上述方法获取准确的姿态角，必须通过合理的数据融合方法来补偿各类误差带来的影响，才能获得稳定可靠的姿态信息。


技术实现要素：

3.发明目的
4.本发明的目的在于为了提高系统在gps信号丢失情况下姿态估计的准确性，确保无人机在丢失gps信号器件仍能够稳定飞行。
5.技术方案
6.为了实现上述目的，本发明采用以下技术方案：一种用于航姿系统的自适应姿态估计方法，所述方法的实现步骤包括：
7.步骤1，在gps状态良好的情况下，基于四元数的低阶ekf姿态估计算法和基于gps速度、位置的高阶组合导航融合算法同时运行，实际采用高阶组合导航算法估计姿态信息；
8.步骤2，在gps信号受到干扰时，停止高阶组合导航算法并切换为采用低阶姿态估计算法；
9.步骤3，当gps信号恢复后，将低阶姿态估计算法得到的四元数值作为高阶滤波算法初值，实现姿态融合算法的平滑切换。
10.进一步的，所述基于四元数的低阶ekf姿态估计算法，具体为采用三轴加速度计，三轴陀螺仪、磁力计，应用低阶卡尔曼滤波器实现各传感器数据融合，解算无人机姿态与航向信息。
11.更进一步的，所述的基于四元数的低阶ekf姿态估计算法包括以下步骤：在基于四元数的低阶卡尔曼滤波器中，设系统状态量x为姿态四元数：
12.x＝[q
0 q
1 q
2 q3]
t
ꢀꢀꢀ
(1)
[0013]
其中，q0、q1、q2、q3为姿态四元数；
[0014]
根据卡尔曼滤波器原理，定义离散系统的状态方程如下：
[0015]
x
k
＝a
k
x
k
‑1 ω
k
‑1ꢀꢀꢀ
(2)
[0016]
其中，x
k
、x
k
‑1分别表示第k、k
‑
时刻的系统状态量，a
k
为从k
‑
1时刻到k时刻的状态转
移矩阵，ω
k
‑1表示k
‑
1时刻的系统过程噪声序列，则对应于卡尔曼滤波器的状态方程为：
[0017][0018]
表示系统k时刻先验估计状态，表示系统k
‑
1时刻后验估计状态，其中：
[0019][0020]
其中，表示角速率矢量矩阵：
[0021][0022]
式中：ω
x
表示绕横滚轴角速率，ω
y
表示绕俯仰轴角速率，ω
z
表示绕航向轴角速率。
[0023]
更进一步的，对所述卡尔曼滤波器状态的先验估计过程，仅使用陀螺仪进行角度更新；然后开展后验估计，所述后验估计包括两个阶段：
[0024]
第一阶段基于加速度计数据，第二阶段基于磁力计数据，根据卡尔曼滤波器原理，校正方程的公式如下所示：
[0025][0026]
式中，z
k
表示k时刻实际量测值，具体为加速度计与磁力计所检测的数据；是根据系统先验估计状态计算的k时刻期望量测值；k
k
表示k时刻卡尔曼增益，由公式(7)得出：
[0027][0028]
式中，表示先验估计协方差矩阵，q
k
‑1为过程噪声矩阵，p
k
‑1和p
k
分别表示上一次和本次后验估计协方差矩阵，r
k
表示量测噪声矩阵，h
k
有两种形式，分别表示期望的重力矢量或磁场矢量对四元数求偏导的雅可比矩阵h
k1
和h
k2
，i表示单位矩阵；
[0029]
h1表示体轴系下期望的重力矢量量测值，由方向余弦矩阵与ned坐标系下重力矢量计算得出，设重力加速度的模值|g|为常值9.807m/s2，则：
[0030][0031]
由上可得：
[0032]
[0033]
h
k1
表示重力矢量量测值h1对状态x求偏导的雅可比矩阵；
[0034]
同理，h2表示体轴系下期望的磁场矢量量测值，查询当地磁偏角ψ
mag
和磁倾角θ
mag
，并将磁场归一化得出：
[0035][0036]
则：
[0037][0038]
h
k2
表示磁场矢量量测值h2对状态x求偏导的雅可比矩阵。
[0039]
进一步的，所述姿态估计算法具体为根据gps接收机测量的位置、速度，应用卡尔曼滤波器不断修正惯性导航系统，以达到无人机的姿态、速度、位置估计的目的。
[0040]
进一步的，基于gps量测的高阶组合导航算法的过程具体包括以下步骤：
[0041]
在基于gps量测的高阶卡尔曼滤波器中，设系统状态量为：
[0042][0043]
式中，q0、q1、q2、q3为姿态四元数，v
n
、v
e
、v
d
分别为ned坐标系下北向速度，东向速度，地向速度，p
n
、p
e
、p
d
分别为ned坐标系下北向位置、东向位置和地向位置，bias
x
、bias
y
、bias
z
分别为绕横滚轴、俯仰轴和航向轴的陀螺仪随机漂移估计值；
[0044]
利用公式(4)更新状态量中的前四个状态的先验估计x
k(1∶4)
，x
k(1:4)
为由q0、q1、q2、q3构成的状态；利用运动学方程，机体加速度和方向余弦矩阵得到先验估计x
k(5:10)
，更新过程如下所示：
[0045]
x
k(1
:
4)
为由q0、q1、q2、q3构成的状态
[0046][0047]
式中，a
x
、a
y
、a
z
分别为机体坐标系下的加速度量测值，即进行量纲变换以后的加速度计量测值，dt表示系统先验估计的递推时间；
[0048]
此时，系统状态转移矩阵为：
[0049][0050]
量测信号为ned坐标系下的三轴速度v
gnss
，ned坐标系下的三轴位置p
gnss
，体轴系下的三轴地磁场量测值为m
b
，并由此计算出的航向角ψ
m
，其中ψ
m
由公式(16)得出：
[0051][0052]
则对应的量测矩阵为：
[0053][0054]
当进行速度、位置融合时，量测值为gps接收机量测的速度v
gnss
和位置p
gnss
，量测矩阵为h
vel
(k)和h
pos
(k)；当进行航向角融合时，量测值为磁力计测量的航向角ψ
m
，量测矩阵为h
mag
(k)。
[0055]
本技术还提供用于航姿系统的自适应姿态估计平滑切换算法，所述切换算法包括以下步骤：
[0056]
首先，在系统初始运行时，同时构造基于四元数的低阶卡尔曼滤波器和基于gps量测的高阶卡尔曼滤波器，并让其同步运行，等待其收敛；
[0057]
然后，在有速度、位置量测信号的情况下，系统输出的状态为基于gps量测的高阶卡尔曼滤波器的估计值，如公式(18)所示：
[0058]
[φ θ ψ]＝[φ
2 θ
2 ψ2]
ꢀꢀꢀ
(18)
[0059]
式中，φ、θ、ψ分别表示系统输出的横滚角、俯仰角和航向角，φ2、θ2、ψ2表示基于gps量测的高阶卡尔曼滤波器估计值；
[0060]
在失去速度、位置量测信号的情况下，停止基于gps量测的高阶卡尔曼滤波器的后验估计过程，同时停止其先验估计过程；输出基于四元数的低阶卡尔曼滤波器的估计值，如公式(19)：
[0061]
[φ θ ψ]＝[φ
1 θ
1 ψ1]
ꢀꢀꢀ
(19)
[0062]
式中，φ1、θ1、ψ1表示基于四元数的低阶卡尔曼滤波器；
[0063]
在重新获得速度、位置量测信号时，恢复运行基于gps量测的高阶卡尔曼滤波器的先验估计过程和后验估计过程，此时，系统估计的协方差矩阵处于系统稳定时的协方差矩阵附近，并将基于四元数的低阶卡尔曼滤波器的姿态四元数估计值作为基于gps量测的高阶卡尔曼滤波器恢复运行时姿态四元数初始估计值，gps当前量测速度、位置信号作为基于gps量测的高阶卡尔曼滤波器恢复运行时速度、位置初始估计值，此时，系统输出的状态为基于gps量测的高阶卡尔曼滤波器的估计值，如公式(20)：
[0064][0065]
式中，x1表示基于四元数的低阶卡尔曼滤波器状态量，x2表示基于gps量测的高阶卡尔曼滤波器状态量。
[0066]
进一步的，所述切换算法应用于如下航姿系统：所述航姿系统包含导航计算机，所述导航计算机以arm cortexm7为主处理器，用于获取传感器数据并进行两套姿态估计滤波算法运算，用于当检测gps发生故障/恢复时完成算法的自适应平滑切换；
[0067]
还包括惯性传感器，所述惯性传感器为adis16470惯性测量单元，分别用于测量三轴角速率和三轴加速度信息；
[0068]
还包括磁力计，所述磁力计为rm3100，用于测量三轴地磁场信息；
[0069]
以及gps卫星导航系统，所述gps卫星导航系统为neo
‑
m8n，用于测量三轴位置与三轴速度信息。
[0070]
有益效果
[0071]
本发明具有以下优点：该算法确保了在失去速度、位置量测情况下姿态估计的准确性，并且当重新获得速度、位置观测时，系统估计值切换过程准确、平稳，不会出现很大的突变，有利于系统稳定。
[0072]
附图表说明
[0073]
为了更清楚地说明本发明中的技术方案，下面将对本发明中所需要使用的附图进行简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据这些附图获得其它附图。
[0074]
图1为基于arm架构的航姿系统框图；
[0075]
图2为有加减速情况下基于四元数的低阶卡尔曼滤波器估计值；
[0076]
图3为gps异常情况高阶卡尔曼滤波器估计值；
[0077]
图4为自适应平滑切换算法流程图；
[0078]
图5为自适应平滑切换算法仿真图；
[0079]
图6为基于gps量测的高阶组合导航算法估计值和参考值；
[0080]
图7为基于四元数的低阶组合导航算法估计值和参考值。
具体实施方式
[0081]
为了使本发明的目的、技术方案和优点更加清楚，下面将结合说明书附图对本发明的实施方式做进一步地详细叙述。
[0082]
实施例1
[0083]
本发明采用以下技术方案：一种用于航姿系统的自适应姿态估计方法，所述方法的实现步骤包括：
[0084]
步骤1，在gps状态良好的情况下，基于四元数的低阶ekf姿态估计算法和基于gps速度、位置的高阶组合导航融合算法同时运行，实际采用高阶组合导航算法估计姿态信息；
[0085]
步骤2，在gps信号受到干扰时，停止高阶组合导航算法并切换为采用低阶姿态估计算法；
[0086]
步骤3，当gps信号恢复后，将低阶姿态估计算法得到的四元数值作为高阶滤波算法初值，实现姿态融合算法的平滑切换。
[0087]
进一步的，所述的航姿系统如图1所示，选用stm32系列mcu作为系统的导航计算机；陀螺仪和加速度计采用adis16470，采用spi接口输出数据和接收配置信息；gps采用neo
‑
m8n，通过spi接口输出基于nema格式的导航数据和基于ubx格式的导航数据；磁力计采
用rm3100，通过控制芯片的spi接口输出三轴地磁信号。由于选用的无线电台为了降低受到的干扰，其接口电平为rs
‑
232电平，故与上位机通信的串口需将ttl电平转化为rs
‑
232电平。
[0088]
进一步的，所述的基于四元数的低阶姿态估计算法包括以下步骤：在基于四元数的低阶卡尔曼滤波器中，设系统状态量为姿态四元数：
[0089]
x＝[q
0 q
1 q
2 q3]
t
ꢀꢀꢀ
(1)
[0090]
根据卡尔曼滤波器原理，定义离散系统的状态方程如下：
[0091]
x
k
＝a
k
x
k
‑1 ω
k
‑1ꢀꢀꢀ
(2)
[0092]
其中，ω
k
‑1表示k
‑
1时刻的系统过程噪声序列。则对应于卡尔曼滤波器的状态方程为：
[0093][0094]
表示系统k时刻先验估计状态，表示系统k
‑
1时刻后验估计状态。其中：
[0095][0096]
其中，可表示为：
[0097][0098]
式中：ω
x
表示绕横滚轴角速率，ω
y
表示绕俯仰轴角速率，ω
z
表示绕航向轴角速率。
[0099]
公式(3)描述的是先验估计过程，仅使用陀螺仪更新角度，后续需要使用卡尔曼滤波器校正方程。在本系统中有两个阶段，每一个阶段一个矫正方程：第一阶段使用加速度计数据，第二阶段使用磁力计数据。根据卡尔曼滤波器原理，校正方程的公式如下所示：
[0100][0101]
式中，z
k
表示k时刻实际量测值，在本系统中为加速度计与磁力计。是根据系统先验估计状态计算的k时刻期望量测值；k
k
表示k时刻卡尔曼增益，可由公式(7)得出。
[0102][0103]
式中，表示先验估计协方差矩阵，q
k
‑1为过程噪声矩阵，p
k
‑1和p
k
分别表示上一次和本次的后验估计协方差矩阵，r
k
表示量测噪声矩阵，h
k
有两种形式，分别表示期望的重力矢量或磁场矢量对四元数求偏导的雅可比矩阵h
k1
和h
k2
，i表示单位矩阵。
[0104]
h1表示体轴系下期望的重力矢量量测值，可由方向余弦矩阵与ned坐标系下重力矢量计算得出，设重力加速度的模值|g|为常值9.807m/s2，则：
[0105][0106]
由上可得：
[0107][0108]
同理，h2表示体轴系下期望的磁场矢量量测值，可查询当地磁偏角ψ
mag
和磁倾角θ
mag
，并将磁场归一化得出：
[0109][0110]
则：
[0111][0112]
对六自由度模型同时施加三轴作用力和三轴转动力矩，使六自由度模型产生周期性转动的同时，还产生周期性的加减速，采用基于四元数的低阶卡尔曼滤波器估计其姿态，仿真情况如图2所示。由仿真结果可知，在无人机有加减速过程时，系统加速度计量测中不仅有重力加速度，还叠加了机体加速度，导致系统的量测过程出现较大误差，从而导致后验估计过程出现较大误差，最终使整个姿态估计过程出现了较大偏差，最大达到了
±8°
，此时已无法满足系统需求。
[0113]
基于四元数的低阶卡尔曼滤波器在任何情况下都会一直更新，确保了系统失去速度、位置量测的情况下仍然能准确估计无人机姿态。同时在重新获得速度、位置量测时，由于将低阶卡尔曼滤波器估计的姿态四元数作为高阶卡尔曼滤波器恢复运行时的初值，切换过程不会出现跳变；且暂停运行高阶卡尔曼滤波器后，系统协方差会处于期望收敛值附近，使得高阶卡尔曼滤波器的收敛过程平稳，收敛速度较快，解决了有、无速度、位置量测情况下的姿态估计问题。
[0114]
但由于系统的俯仰角和横滚角的量测依赖于加速度计，而加速度计量测会叠加机体加速度和机体振动，在机体持续加、减速时，量测的俯仰角和横滚角误差较大，进而导致估计的俯仰角和横滚角出现较大误差，故需要引入带有gps量测的高阶组合导航算法。
[0115]
进一步的，所述的基于gps量测的高阶组合导航算法包括以下步骤：
[0116]
在基于gps量测的高阶卡尔曼滤波器中，设系统状态量为：
[0117][0118]
式中，q0、q1、q2、q3为姿态四元数，v
n
、v
e
、v
d
分别为ned坐标系下北向速度，东向速
度，地向速度，p
n
、p
e
、p
d
分别为ned坐标系下北向位置、东向位置和地向位置，bias
x
、bias
y
、bias
z
分别为绕横滚轴、俯仰轴和航向轴的陀螺仪随机漂移估计值。
[0119]
同基于四元数的低阶姿态估计算法中所述，利用公式(4)可更新状态量中的前四个状态的先验估计x
k(1∶4)
(即四元数)。利用运动学方程，机体加速度和方向余弦矩阵c
″
b
得到先验估计x
k(5:10)
，更新过程如下所示：
[0120][0121]
式中，a
x
、a
y
、a
z
分别为机体坐标系下的加速度量测值，即进行量纲变换以后的加速度计量测值，dt表示系统先验估计的递推时间。
[0122]
此时，系统状态转移矩阵应为：
[0123][0124]
此时，量测信号为ned坐标系下的三轴速度v
gnss
，ned坐标系下的三轴位置p
gnss
，体轴系下的三轴地磁场量测值为m
b
，并由此计算出的航向角ψ
m
，其中ψ
m
可由公式(16)得出。
[0125][0126]
则对应的量测矩阵为：
[0127][0128]
同基于四元数的低阶姿态估计算法中所述，当进行速度、位置融合时，量测值为gps接收机量测的速度v
gnss
和位置p
gnss
，量测矩阵为h
vel
(k)和h
pos
(k)；当进行航向角融合时，量测值为磁力计测量的航向角ψ
m
，量测矩阵为h
mag
(k)。
[0129]
使模型在一定时间内失去gps速度、位置量测时，基于gps量测的高阶卡尔曼滤波器后验估计环节会受到较大影响，仿真结果如图3所示。系统在第450s时，丢失gps速度、位置量测，此时系统后验估计无法更新，系统估计的状态量出现了较大误差，姿态估计误差最大可达35
°
。同时在第750s重新捕获gps速度、位置量测，收敛过程中出现了较大的跳变值，最大跳变值达到80
°
，且收敛时间大于10s。
[0130]
实施例2
[0131]
本技术还公开一种用于航姿系统的自适应姿态估计平滑切换算法，即一种基于四元数的低阶ekf姿态估计算法和基于gps速度、位置的高阶组合导航融合算法包括以下步骤：
[0132]
首先，在系统初始运行时，同时构造基于四元数的低阶卡尔曼滤波器和基于gps量测的高阶卡尔曼滤波器，并让其同步运行，等待其收敛。
[0133]
然后，在有速度、位置量测情况下，系统输出的状态为基于gps量测的高阶卡尔曼滤波器的估计值，如公式(18)所示：
[0134]
[φ θ ψ]＝[φ
2 θ
2 ψ2]
ꢀꢀꢀ
(18)
[0135]
式中，φ、θ、ψ分别表示系统输出的横滚角、俯仰角和航向角，φ2、θ2、ψ2表示基于gps量测的高阶卡尔曼滤波器估计值。
[0136]
在失去速度、位置量测的情况下，不仅停止基于gps量测的高阶卡尔曼滤波器的后验估计过程，同时停止其先验估计过程，防止其估计的协方差矩阵发散。并输出基于四元数的低阶卡尔曼滤波器的估计值，如公式(19)：
[0137]
[φ θ ψ]＝[φ
1 θ
1 ψ1]
ꢀꢀꢀ
(19)
[0138]
式中，φ1、θ1、ψ1表示基于四元数的低阶卡尔曼滤波器。
[0139]
在重新获得速度、位置量测时，恢复运行基于gps量测的高阶卡尔曼滤波器的先验估计过程和后验估计过程，此时，系统估计的协方差矩阵处于系统稳定时的协方差矩阵附近，并将基于四元数的低阶卡尔曼滤波器的姿态四元数估计值作为基于gps量测的高阶卡尔曼滤波器恢复运行时姿态四元数初始估计值，gps当前量测速度、位置作为基于gps量测的高阶卡尔曼滤波器恢复运行时速度、位置初始估计值，此时，系统输出的状态为基于gps量测的高阶卡尔曼滤波器的估计值，如公式(20)：
[0140][0141]
式中，x1表示基于四元数的低阶卡尔曼滤波器状态量，x2表示基于gps量测的高阶卡尔曼滤波器状态量。此时，由于基于gps量测的高阶卡尔曼滤波器的估计协方差矩阵处于稳定状态的协方差矩阵附近，且系统状态量也处于真实值附近，系统不会产生较大修正量，且不会有较长的收敛时间，切换后的状态立刻就处于真实值附近，保证切换后的状态随时可以用来控制无人机而不会产生较大偏差，同时保证了切换瞬间的平稳性。
[0142]
上述平滑切换算法应用于如下航姿系统中：
[0143]
选用stm32h743系列mcu作为系统的导航计算机，该型处理器为st公司的基于armcortex
‑
m7内核，封装为lqfp144，主频高达480mhz，具有2m的片上flash和1m的片内ram。同时，具有一个l1 cache一级缓存，用来为低速存储器加速；具有6个spi接口，方便与传感器进行通信；同时还支持sdio接口，便于将数据存储进sd卡。
[0144]
本技术航姿系统中的传感器选用亚德诺半导体公司的adis16470，该型mimu中内置的每个惯性传感器都实现了业界领先的技术与信号调理技术的完美结合，可提供优化的动态性能，出厂时针对各传感器的灵敏度、偏置、对准和线性加速度(陀螺偏置)进行校准。并且，该mimu抗冲击能力较强，运动中的偏置稳定性和噪声密度等性能较好。
[0145]
本技术航姿系统选用pni公司的rm3100型磁力计，其量程为
±
800ut，噪声小于15nt，分辨率高于13nt，被强磁干扰后能自动恢复，不用再次进行校准，且其使用的spi通信接口，速度快，占用系统时间较短。
[0146]
本技术航姿系统选用gps卫星导航系统为u
‑
blox公司的neo
‑
m8n型卫星导航芯片。该型芯片可同时接受gps信号和bds信号，且其体积小、功耗低、搜星快，适合应用于各种环境。其通信接口可设置为uart、iic和spi等，可输出基于nema格式的导航数据和基于ubx格式的导航数据，具有较好的灵活性，故选用的是neo
‑
m8n型gps卫星导航芯片。
[0147]
结合所附图表，对本发明的技术方案作具体说明，仿真基于matlab/simulink的六自由度模型，算法试验验证基于arm架构的航姿系统。
[0148]
如图4为自适应平滑切换流程图，对六自由度模型同时施加三轴作用力和三轴转动力矩，使六自由度模型产生周期性转动的同时，还产生周期性的加减速，并在第150s时断开gps速度、位置观测，在第450s时恢复gps速度、位置观测，仿真结果如图5所示。由图5右仿真结果可知，在0～150s阶段，gps速度、位置观测正常，输出高阶卡尔曼姿态估计值；在150～450s这一时间段里，gps速度、位置观测异常，输出低阶卡尔曼姿态估计值；450s后，gps速度、位置观测恢复，低阶姿态估计值作为高阶初值计算并输出高阶卡尔曼姿态估计值。由图5左可知，图5的左图是真值与估计值对比，因为误差比较小，所以两幅图几乎重合在一起了，所以增加了右图，右图即表示真值与估计值的差值。在该算法切换的过程中，姿态估计值的跳变小于0.5
°
，收敛时间小于0.2s，完全满足无人机自主飞行的要求，并且保证了在丢失速度、位置观测的情况下，无人机能准确估计自身状态。根据整个仿真结果可知，本章提出的无人机自适应平滑切换姿态融合算法不仅保证了系统在持续加减速情况下姿态估计的准确性，同时在gps数据异常期间的姿态估计精度能够得到维持；而且提高了基于gps量测的高阶组合导航算法在gps信号恢复时的收敛速度和平稳性，为无人机系统的增稳控制提供重要保障。
[0149]
为了验证本发明的有效性，进行的对比实验及分析如下：
[0150]
算法验证的试飞试验采用某六轴多旋翼无人机，自研的航姿测量设备由陀螺仪和加速度计(adis16470)，gps(neo
‑
m8n)，磁力计(rm3100)构成，并利用某商用航姿传感器作为参考对比，利用遥控器切换是否丢失gps量测状态。实验数据如图6和图7所示。图中φ1、θ1、ψ1表示商用航姿传感器的估计姿态，φ2、θ2、ψ2表示丢失gps信号后暂停运行的高阶卡尔曼滤波器的估计姿态，φ3、θ3、ψ3表示丢失gps信号丢失后只停止速度、位置量测更新的高阶卡尔曼滤波器的估计姿态，φ4、θ4、ψ4表示低阶卡尔曼滤波器的估计姿态。采用人为断开gps速度、位置观测的方式，约407s左右断开gps速度、位置量测信号，约482s左右恢复gps信号。由仿真结果可知，在gps速度、位置观测正常阶段，φ3、θ3、ψ3相较于φ4、θ4、ψ4估计精度高，因此，在这一阶段输出高阶估计值；在断开gps速度、位置阶段，φ3、θ3、ψ3开始出现较大偏差，最大可达40
°
，而φ4、θ4、ψ4能够跟上φ1、θ1、ψ1，因此，在这一阶段输出低阶估计值；在恢复gps速度、位置量测的瞬间，φ3、θ3、ψ3出现了较大跳变，最大跳变可达50
°
，且收敛时间大于5s，而φ2、θ2、ψ2立刻恢复到参考值附近，误差小于0.5
°
，收敛时间小于0.2s，且未出现大的姿态估计跳变，满足无人机状态估计换切换的要求，因此，在这一阶段以低阶估计值作为高阶初值并输出高阶估计值。
[0151]
根据实验结果可知，本专利提出的航姿系统自适应姿态估计平滑切换算法不仅保证了系统在持续加减速情况下姿态估计的准确性，同时在gps信号异常期间，姿态估计能够得到维持；而且提高了基于gps量测的高阶组合导航算法在gps信号恢复时的收敛速度和平稳性，为无人机系统的增稳控制提供重要保障。
[0152]
本领域普通技术人员可以理解实现上述实施例的全部或部分步骤可以通过硬件来完成，也可以通过程序来指令相关的硬件完成，该程序可以存储于一种计算机可读存储介质中，上述提到的存储介质可以是只读存储器，磁盘或光盘等。
[0153]
上述仅为本技术的较佳实施例，并不用以限制本技术，凡在本技术的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本技术的保护范围之内。