CN102692225B - 一种用于低成本小型无人机的姿态航向参考*** - Google Patents

Abstract

本发明涉及一种用于低成本小型无人机的姿态航向参考***，包括角速率陀螺、加速度计、GPS、角速率陀螺运算模块、校正模块和卡尔曼滤波器，其中角速率陀螺测量飞机的滚转角速率、俯仰角速率和偏航角速率；加速度计测量重力在机体坐标轴系下的分量；GPS测量飞机的航迹方位角；角速率陀螺运算模块得到带偏移的滚转角φ₁、俯仰角θ₁和偏航角ψ₁；校正模块得到估计的滚转角φ₂、俯仰角θ₂，并将GPS测量得到的航迹方位角作为估计的偏航角ψ₂；卡尔曼滤波器将角速率陀螺运算模块和校正模块产生的数据进行融合，得到最终的姿态角[φ θ ψ]^T。本发明降低了小型无人机***的成本；减小了航向参考***的复杂程度；提高了航姿估计的精度。

Description

一种用于低成本小型无人机的姿态航向参考***

技术领域

本发明涉及一种姿态航向参考***，特别涉及一种基于传感器融合技术的用于低成本小型无人机的姿态航向参考***。

背景技术

姿态航向参考***(Attitude Heading Reference System,AHRS)，简称航姿***，用于确定运动载体在空间的方位。由姿态航向参考***提供的姿态信息可以被广泛应用于导航、制导与控制。对于自主飞行的无人机，航姿***尤为重要，它为控制***的内回路提供必要的姿态反馈。通常，姿态信息的确定依赖于高精度的角速率陀螺，然而高精度陀螺存在着成本高、重量大、结构复杂等缺陷，显然不能适用于低成本小型无人机***。因此，在低成本传感器的制约条件下，为小型无人机***提供一套计算量小、可靠性高的姿态航向参考***一直都是国内外相关领域广泛研究的问题。

通常情况下，运动载体的姿态是用欧拉角(φ，θ，ψ)描述的。根据载体运动的角速率同欧拉角变化率之间的关系，可以将角速率陀螺的测量值进行积分，从而得到载体的姿态信息，但这种方法对于陀螺精度的要求较高(输出误差小于0.1°/h)。低成本小型无人机通常采用MEMS(Micro Electrical MechanicalSystem)传感器，虽然具备成本低、体积小等特点，此类传感器的精度较差，输出误差在10°～100°/h之间，积分陀螺输出值时会产生较大漂移，因而不能直接用于姿态估计。

姿态信息也可以不依赖于角速率陀螺，而采用观测矢量确定。采用观测矢量确定姿态的基本原理是：用分别在两个坐标系下两个不共线的观测矢量，通过三次旋转使两个坐标系重合，再计算得到欧拉角。一些文献针对这种无陀螺***(Gyro-Free Systems)进行了研究。例如，使用加速度计、磁强计和GPS(Global Positioning System)作为传感器，通过求解Wahba问题获得姿态信息；也可以通过多天线GPS，采用GPS载波相位观测量进行姿态估计。然而，在很多实际情况下，这种无陀螺航姿***不能单独使用。首先，无陀螺航姿***不能提供高带宽的的姿态信息；其次，当载体做特定机动时，使用该方法估计的姿态信息会存在较大误差。例如当无人机倾斜转弯时，加速度计会敏感到除重力加速度以外的的向心加速度，导致计算姿态不准确；最后，无陀螺航姿***给出的姿态信息包含较大的噪声。

基于上述原因，可以使用传感器融合(Sensor Fusion)技术将角速率陀螺同加速度计、磁强计及GPS等组成的无陀螺***进行融合，使它们互为补充，取长补短。由于角速率陀螺能够提供高带宽有偏的姿态信息，可用来“平滑”无陀螺***；而无陀螺***则用于校正角速率陀螺产生的漂移，因此也成为校正***(Aiding System)。这样，利用适当的滤波器将二者结合后，就可获得高带宽且无偏的姿态信息解决方案。传感器融合技术的核心是用于融合不同传感器的滤波器，使用滤波器融合角速率陀螺和无陀螺***进行测姿的方法数十年来被广泛应用于卫星的姿态确定问题中。

对于低成本小型无人机，虽然传感器融合技术已经被广泛应用于姿态确定，但是仍存在一些问题：首先，小型无人机***中用于姿态估计的传感器一般为角速率陀螺、加速度计、磁航向计(用于测量航向角)，而磁航向计的造价较高，不适合于低成本无人机***；其次，虽然一些小型无人机姿态估计***采用GPS代替磁航向计，降低了成本，但是由于GPS输出的是角度，此时的姿态估计算法是建立在欧拉角法的基础之上，这种方法需要在计算中实时求解三角函数，某些时候还会出现奇点，使得问题无解，因而不适用于计算能力较差的低成本飞控计算机。目前尚未见到能够较好地解决当无人机出现机动过载时姿态估计的解决方案。

发明内容

本发明的目的是针对现有技术的缺点，提供一套姿态航向参考***方案，仅依靠包括角速率陀螺、GPS、加速度计在内的低成本传感器，实现小型无人机姿态信息估计。

本发明提供了一种用于低成本小型无人机的姿态航向参考***，包括角速率陀螺、加速度计、GPS、角速率陀螺运算模块、校正模块和卡尔曼滤波器，其中：

角速率陀螺测量飞机的滚转角速率、俯仰角速率和偏航角速率；

加速度计测量重力在机体坐标轴系下的分量；

GPS测量飞机的航迹方位角；

角速率陀螺运算模块通过角速率陀螺测量的滚转角速率、俯仰角速率和偏航角速率，得到带偏移的滚转角φ₁、俯仰角θ₁和偏航角ψ₁；

校正模块通过加速度计测量的重力加速度的体轴分量得到校正模块估计的滚转角φ₂、俯仰角θ₂，并将GPS测量得到的航迹方位角作为估计的偏航角ψ₂；

卡尔曼滤波器将角速率陀螺运算模块和校正模块产生的数据进行融合，得到最终的姿态角[φ θ ψ]^T。

有益效果

本发明的优点在于：通过使用低成本传感器，降低了小型无人机***的成本；减小了传统姿态航向参考***的复杂程度；依靠传感器融合技术，提高了***航姿估计的精度和可靠性；解决了无人机出现过载时姿态估计不准确的问题。

附图说明

图1为姿态航向参考***的结构图；

图2为静态条件下滚转角解算；

图3为静态条件下俯仰角解算；

图4为角速率陀螺漂移估计；

图5为滚转角误差及其标准差；

图6为滚转角速率陀螺漂移误差及其标准差；

图7为估计滚转角同真实滚转角φ之间的误差对比；

图8为基于飞行试验的滚转角估计，其中(a)为时间窗口法估计滚转角，(b)为常规滤波及陀螺积分估计滚转角；

图9为基于飞行试验的俯仰角估计，其中(a)为时间窗口法估计俯仰角，(b)为常规滤波及陀螺积分估计俯仰角；

图10为滤波器开关标志量；

图11为滤波器开关触发量及门限值。

具体实施方式

下面结合附图，具体说明本发明的优选实施方式。

本实施方式实现了一种用于低成本小型无人机的姿态航向参考***，该***由角速率陀螺子***、校正子***和卡尔曼滤波器三个部分组成。图1显示了姿态航向参考***的结构图。本实施方式采用误差四元数法建立模型，缓解了机载处理器的运算负荷。

1)角速率陀螺运算模块

三维空间内刚体的姿态通常是用三个欧拉角描述的，它们分别为滚转角φ、俯仰角θ和偏航角ψ。姿态角描述了两个不同坐标系之间的相对关系。在导航、制导与控制中，对于近地运动的飞行器，通常将机体坐标轴S_b–Oxyz系选为动坐标系，导航坐标轴系S_n–Ox_ny_nz_n选为参考坐标系，定义为北东地(North-East-Down,NED)参考***，x_n为北向，y_n为东向，z_n指向地心。

定义任意矢量u，其在导航坐标轴系和机体坐标轴系分别表示为uⁿ和u^b。矢量u在上述两个不同坐标轴系下的投影转换是通过方向余弦矩阵实现的：

$u^b=C_n^b{u}^n---\left(1\right)$

式中为方向余弦矩阵(Direction Cosine Matrix,DCM)，也称为姿态矩阵，通常以欧拉角的形式定义。

在实际应用中，使用欧拉角表示姿态存在着若干问题。首先，在某些特殊情况下，个别姿态角成为不确定的，运动学方程出现奇异性，例如当俯仰角θ＝±90°时，偏航角ψ为不确定，且方程dψ/dt有奇异性。其次，在实际解算中，欧拉角法需要求解大量三角函数，必然给处理器带来计算负担。如果使用姿态四元数法，一方面能避免运动方程的奇异性，另一方面无需计算三角函数，节省了处理器资源，提高了计算效率。为了克服上述缺点，本实施方式使用四元数法建模。

定义姿态四元数q：

$q=\left[\begin{array}{c}{q}_0\\ \stackrel{\→}{q}\end{array}\right]---\left(2\right)$

其中q₀称为四元数q的标量部分， $\stackrel{\→}{q}={\left[\begin{array}{ccc}{q}_1& q_2& q_3\end{array}\right]}^T$ 称为四元数q的矢量部分。

用四元数表示的方向余弦阵定义如下：

$C_n^b\left(q\right)=\left[\begin{array}{ccc}1-2(q_2^2+q_3^2)& 2(q_1{q}_2+q_3{q}_0)& 2(q_1{q}_3-q_2{q}_0)\\ 2(q_1{q}_2-q_3{q}_0)& 1-2(q_1^2+q_3^2)& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_2{q}_0)& 2(q_2{q}_3-q_1{q}_0)& 1-2(q_1^2+q_2^2)\end{array}\right]---\left(3\right)$

四元数微分方程由下式确定：

$\stackrel{\·}{q}=\frac{1}{2}\mathrm{\Ω}\·q=\frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]---\left(4\right)$

其中，p为滚转角速率，q俯仰角速率，r为偏航角速率。

由四元数微分方程式(4)可知，通过角速率陀螺的测量值可实时更新四元数，然后根据姿态四元数表示的方向余弦矩阵(3)得到载体姿态角

$\begin{array}{c}\mathrm{\φ}=\arctan \frac{2(q_1{q}_3+q_0{q}_1)}{1-2(q_1^2+q_2^2)}\\ \mathrm{\θ}=\arcsin 2(-q_1{q}_3+q_2{q}_0)\\ \mathrm{\ψ}=\arctan \frac{2(q_1{q}_2+q_3{q}_0)}{1-2(q_2^2+q_3^2)}\end{array}---\left(5\right)$

2)校正模块

尽管通过角速率陀螺测量值积分得到的姿态角存在着无界误差，但由于角速率陀螺可以提供高带宽的输出，因而是大多数AHRS***不可或缺的。由陀螺积分产生的漂移可以通过校正***进行抑制，因为组成校正***的传感器具备输出误差有界的特性。校正***可以周期性地“重置”经角速率陀螺提供的姿态信息以达到校正的目的。本实施方式选取加速度计和GPS共同组成校正***。

通过加速度计对重力矢量在机体坐标轴系下的投影进行观测可得到姿态角，根据加速度计的原理，假设其三轴的输出矢量为f^b，可以表示

$f^b=a_{\mathrm{ib}}^b-g_b---\left(6\right)$

其中，为载体相对于惯性系的运动加速度在机体坐标轴系下的分量，g_b为重力加速度矢量在机体坐标轴系下的分量

$g_b=C_n^b{g}_n=\left[\begin{array}{c}-g\sin \mathrm{\θ}\\ g\sin \mathrm{\φ}\cos \mathrm{\θ}\\ g\cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(7\right)$

其中，为重力加速度矢量在导航坐标轴系下的投影，可以表示为g_n＝[0 0 g]^T，g为重力加速度常数。

在时，即载体静止或做匀速直线运动时，f^b变为

$f^b=\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=-g_b=\left[\begin{array}{c}g\sin \mathrm{\θ}\\ -g\sin \mathrm{\φ}\cos \mathrm{\θ}\\ -g\cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]---\left(8\right)$

此时，俯仰角θ和滚转角φ可以通过上式确定

$\mathrm{\θ}=\arcsin \frac{f_x}{g}$

                                                      (9)

$\mathrm{\φ}=\arcsin \frac{{-f}_y}{g\cos \mathrm{\θ}}$

式(8)不包含偏航角ψ，因而仅通过加速度计只能确定俯仰角θ和滚转角φ。为了确定偏航角ψ，还需引入GPS提供的航向信号。需要指出的是，GPS只能提供航迹方位角但是在侧风较小的情况下可以用来替代偏航角ψ。

3)卡尔曼滤波器

本实施方式选择卡尔曼滤波器作为将角速率陀螺和校正***结合在一起的传感器融合算法。

滤波器工作步骤为：在确定初始条件后，就可以利用角速率陀螺的测量数据积分，进行姿态估计，这一步骤称为“时间更新”，提供了高带宽的姿态信息。在时间更新阶段，由于陀螺数据存在噪声，使得估计姿态的误差随时间累积，因而不能靠单纯的陀螺数据积分确定姿态。为了抑制陀螺误差，需要引入校正***对陀螺数据进行周期性的修正，这一步骤称为“量测更新”。状态误差的协方差矩阵以及陀螺漂移估计也在该步骤校正。接下来，进行新一轮的时间更新，如此周期性重复。

真实姿态四元数q可以表示为估计四元数和误差四元数q_e相乘的形式，即认为误差四元数q_e是估计四元数到真实四元数q的旋转。由于角速率陀螺的误差，误差四元数是非零的小量。q_e，和q的关系由下式确定：

$q=\hat{q}\⊗q_e---\left(10\right)$

误差四元数可近似表示为：

$q_e=\left[\begin{array}{c}1\\ {\stackrel{\→}{q}}_e\end{array}\right]---\left(11\right)$

对式(11)求导，并利用四元数微分方程式(4)，经过线性化可得四元数误差微分方程

${\stackrel{\stackrel{\·}{\→}}{q}}_e=-[\stackrel{\→}{\mathrm{\ω}}\×]{\stackrel{\→}{q}}_e-\frac{1}{2}\left(\widehat{\mathrm{\ω}}-\mathrm{\ω}\right)---\left(12\right)$

其中，表示角速率陀螺测量值与真实值之差，由下式确定

$[\stackrel{\→}{\mathrm{\ω}}\×]=\left[\begin{array}{ccc}0& r& -q\\ -r& 0& p\\ q& -p& 0\end{array}\right]---\left(13\right)$

校正子***的观测量取为误差角，可由如下方法获得：首先利用加速度计和GPS的测量信息根据式(9)计算出载体姿态角，然后将此姿态角同式(5)计算得到的姿态角估计值相减，即可得到角度误差

$\left[\begin{array}{c}\mathrm{\δ\φ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\φ}}_{\mathrm{AS}}\\ {\mathrm{\θ}}_{\mathrm{AS}}\\ {\mathrm{\ψ}}_{\mathrm{AS}}\end{array}\right]-\left[\begin{array}{c}\widehat{\mathrm{\φ}}\\ \widehat{\mathrm{\θ}}\\ \widehat{\mathrm{\ψ}}\end{array}\right]---\left(14\right)$

式中，[δφ δθ δψ]^T为误差角，[φ_AS θ_AS ψ_AS]^T为通过校正***得到的姿态角， ${\left[\begin{array}{ccc}\widehat{\mathrm{\φ}}& \widehat{\mathrm{\θ}}& \widehat{\mathrm{\ψ}}\end{array}\right]}^T$ 为利用四元数法计算出的姿态角。

角误差同姿态四元数之间的关系由下式确定

${\stackrel{\→}{q}}_e={\left[\begin{array}{ccc}\frac{\mathrm{\δ\φ}}{2}& \frac{\mathrm{\δ\θ}}{2}& \frac{\mathrm{\δ\ψ}}{2}\end{array}\right]}^T---\left(15\right)$

校正子***的状态量选取为误差四元数的矢量部分和三个角速率陀螺的漂移δb_p，δb_q和δb_r

$X={\left[\begin{array}{cccc}{\stackrel{\→}{q}}_e& {\mathrm{\δb}}_p& {\mathrm{\δb}}_q& {\mathrm{\δb}}_r\end{array}\right]}^T---\left(16\right)$

则可以构造***的状态方程

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(17\right)$

其中，A(t)为***矩阵，定义如下

$A\left(t\right)=\left[\begin{array}{cc}-[\stackrel{\→}{\mathrm{\ω}}\×]& -\frac{1}{2}{I}_{3\×3}\\ 0_{3\×3}& -\frac{1}{\mathrm{\τ}}{I}_{3\×3}\end{array}\right]---\left(18\right)$

输入矩阵B(t)和噪声矩阵W(t)定义如下

$B\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& I_{3\×3}\end{array}\right]W\left(t\right)\left[\begin{array}{c}{b}_w\\ w_{b_1}\end{array}\right]---\left(19\right)$

式(17)可离散化为如下形式

X_k+1＝F_kX_k+G_kW_k   (20)

其中，F_k为状态转移矩阵，G_k为***噪声驱动矩阵

F_k＝I+A(t)△t

                                                  (21)

G_k＝B(t)△t

将观测方程写成下列离散化形式

Z_k＝H_kX_k+V_k   (22)

其中，Z＝[δφ δθ δψ]^T为观测量，V为观测噪声，H为观测矩阵，可以由式(15)确定

H＝[2I_3×3 0_3×3]   (23)

在卡尔曼滤波的量测更新阶段，通过加速度计和GPS对角速率陀螺***提供的姿态信息进行校正。使用加速度计修正陀螺漂移的前提条件是载体处于静止或匀速直线运动状态，由于不存在附加加速度，加速度计敏感到的只有重力加速度在机体坐标轴上的分量，此时利用式(9)便可以计算出俯仰角θ和滚转角φ。然而，当载体机动时，由于加速度计感受到了附加加速度而导致姿态估计出现偏差。因此，必须采用一定的补偿方法，以减小载体机动时出现的姿态角误差。下面提出两种解决方案并进行分析。

a)过载补偿

当飞机进行倾斜转弯时，由于出现向心加速度，会导致加速度计出现测量误差，使得姿态估计不准确。为了便于分析，可将飞机的协调转弯看做圆周运动。

质点做圆周运动的向心加速度由下式确定

a＝ω·V            (24)

式中，ω代表质点绕圆心的角速度，V代表质点速度。显然，速度矢量、角速度旋转轴和加速度矢量正交。在过渡过程之外，认为飞机的速度矢量沿机体x轴方向。这样，由于在机体y轴和z轴方向不存在速度分量，机体x轴方向的加速度计将不能感受到向心加速度。角速率陀螺和加速度计都的安装都是沿机体坐标轴的，而且空速管测量的真空速V_TAS也是沿机体x轴方向的，因此角速率陀螺和空速管的测量数据可以直接用来校正加速度计。由于在卡尔曼滤波算法中没有使用沿机体z轴方向的加速度计a_z，这里只需考虑对机体y轴方向的加速度计a_y的校正。

机体x轴和z轴方向的角速率p和r都会引起机体y轴方向的加速度a_y。然而，如果将向心加速度公式写成绕圆心的角速度和圆半径的形式(如下式)就可以看出，如果半径很小，那么引起的加速度也将很小。

a＝ω²R            (25)

由于副翼偏转而引起的机体滚转总是沿着机体x轴方向的，同时，加速度计a_y同x轴方向距离较近。这样，绕x轴的角速度p引起的y轴方向的加速度也可以忽略不计，由转弯产生的附加加速度完全由绕z轴方向的偏航角速度r和真空速V_TAS决定

a_y＝r·V_TAS          (26)

因此，经过修正后的式(9)就可以写成如下形式

$\mathrm{\θ}=\arcsin \frac{f_x}{g}$

                                                      (27)

$\mathrm{\φ}=\arcsin \frac{{-f}_y+{\mathrm{rV}}_{\mathrm{TAS}}}{g\cos \mathrm{\θ}}$

执行探测任务的一类小型无人机，大部分时间处于巡航状态，当需要改变航向时则进行倾斜转弯，对于此种无人机，该补偿方案可以取得满意的效果。

b)时间窗口

当飞机飞行时，会在巡航和机动两种状态之间转换。巡飞时，可以近似认为飞机不受过载，此时的校正***可以用来修正陀螺漂移。飞机做机动时，校正***由于受到附加加速度干扰而不可用。通常情况下，飞机做连续机动的时间不会很长，这期间陀螺误差的积累较小，因此可以考虑将卡尔曼滤波器关闭，当结束机动时，再行开启。

飞机转弯时，角速率会有显著变化，因此选择角速率的范数作为触发卡尔曼滤波器的标志，时间窗口的开闭由下式确定

$\left\{\begin{array}{c}u\left(t\right)\≤\mathrm{\β},\mathrm{flag}=1\\ u\left(t\right)>\mathrm{\β},\mathrm{flag}=0\end{array}\right.---\left(28\right)$

其中，β为时间窗口门限值，flag为窗口标志。

当u(t)≤β时，flag＝1，窗口打开，使用卡尔曼滤波器进行定姿；当u(t)>β时，flag＝0，窗口关闭，此时断开滤波器，停止校正***的修正。

使用本实施方式所述的姿态航向参考***进行姿态信息估计的具体实施步骤如下：

第一步：利用角速率陀螺测量数据计算姿态四元数

${\stackrel{\stackrel{\·}{^}}{q}}^{-}=\frac{1}{2}\mathrm{\Ω}\·{\hat{q}}^{-}---\left(29\right)$

第二步：根据式(21)中的F和G预测状态误差协方差矩阵

$P_k^{-}=F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^T+G_{k-1}{Q}_{k-1}{G}_{k-1}^T---\left(30\right)$

式中，Q_k-1为过程噪声协方差矩阵。

第三步：在校正***中，利用(9)及GPS测量的航向角ψ_GPS，得到由校正***估计的姿态角信息[φ_AS θ_AS ψ_AS]^T。

第四步：利用上一时间步长估计的***姿态角信息 ${\left[\begin{array}{ccc}\widehat{\mathrm{\φ}}& \widehat{\mathrm{\θ}}& \widehat{\mathrm{\Ψ}}\end{array}\right]}^T$ 同校正***得到的姿态角信息作差，得到卡尔曼滤波器的观测量

$Z=\left[\begin{array}{c}\mathrm{\δ\φ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\φ}}_2\\ {\mathrm{\θ}}_2\\ {\mathrm{\ψ}}_2\end{array}\right]-\left[\begin{array}{c}\widehat{\mathrm{\φ}}\\ \widehat{\mathrm{\θ}}\\ \widehat{\mathrm{\ψ}}\end{array}\right]---\left(31\right)$

第五步：计算卡尔曼增益矩阵

$K_k=P_{k-1}{H}_k^T{(H_k{P}_{k-1}{H}_k^T+R_k)}^{-1}---\left(32\right)$

式中，R_k为量测噪声协方差矩阵。

第六步：更新状态误差协方差矩阵

$P_k^{+}=(I-K_k{H}_k)P_k^{-}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(33\right)$

第七步：更新状态向量

${\hat{X}}_k^{+}={\left[\begin{array}{cccc}{\stackrel{\widehat{\→}}{q}}_{\mathrm{ek}}^{+}& {\mathrm{\δ}\hat{b}}_{\mathrm{pk}}^{+}& {\mathrm{\δ}\hat{b}}_{\mathrm{qk}}^{+}& {\mathrm{\δ}\hat{b}}_{\mathrm{rk}}^{+}\end{array}\right]}^T=K_k{Z}_k---\left(34\right)$

第八步：使用估计的四元数及陀螺漂移修正时间更新步骤中所得姿态四元数及角速率陀螺测量值

${\hat{q}}_k^{+}={\hat{q}}_k^{-}\⊗{\left[\begin{array}{cc}1& {\stackrel{\widehat{\→}}{q}}_{\mathrm{ek}}^{+}\end{array}\right]}^T$

${\hat{b}}_k^{+}={\hat{b}}_k^{-}{+\mathrm{\δ}\hat{b}}_k^{+}---\left(35\right)$

${\widehat{\mathrm{\ω}}}_k^{+}={\widehat{\mathrm{\ω}}}_k^{-}-{\hat{b}}_k^{+}$

第九步：由姿态四元数即可计算出姿态角[φ θ ψ]^T。

下面通过静态实验和飞行试验对本发明的综合性能进行验证。

1)静态实验

在静态试验中，利用Crossbow Technology出品的AHRS-400CC惯性测量单元(Inertial Measurement Units,IMUs)辅助完成算法测试。AHRS-400CC具有三轴加速度计、角速率陀螺和磁强计，通过内置的姿态估计算法可以输出滚转、俯仰和偏航三个欧拉角。本实验使用AHRS-400CC采集的陀螺和加速度计数据进行姿态解算，并将解算出的出的姿态角同AHRS-400CC输出的“真实姿态”进行对比。由于本文提出的姿态算法使用了GPS提供的航向信息校正偏航角，而AHRS-400CC并不具备GPS传感器，因此在静态实验中只进行俯仰角和滚转角的验证。将AHRS-400CC水平静止放置，采集传感器数据和姿态角信息，实验时间为160s。

图2和图3显示了静态条件下姿态角的解算结果。实线为卡尔曼滤波算法得到的姿态角，虚线为陀螺积分得到的姿态角。可以看出，由于存在测量噪声，单纯使用角速率陀螺测量值积分得到的姿态角会随时间增加而漂移。滚转角产生的漂移(约为0.0156°/s)大于俯仰角漂移(约为0.0031°/s)，这是由于机体x轴方向陀螺的漂移大于z轴方向陀螺的漂移引起的。由卡尔曼滤波算法估计的姿态角没有发生漂移，且具有较小的振荡幅度。表1给出了三种算法的均方根误差(Root Mean Square Error,RMSE)对比。由此可见，经过加速度计的修正，卡尔曼滤波算法估计的姿态角可以有效抑制陀螺漂移。

表1 姿态角估计算法RMSE对比

对角速率陀螺漂移b_p，b_q的估计如图4所示。实线为b_p，虚线为b_q。由图可知，滚转角速率陀螺的漂移略大于俯仰角速率陀螺的漂移，这与图2和图3显示的结果一致。陀螺漂移最终会稳定于常值。

图5和图6分别显示了卡尔曼滤波算法得到的滚转角误差(实线)δφ、滚转角速率陀螺漂移误差(实线)δb_p以及相应的1σ标准差界(虚线)。同前述分析一样，两图表明卡尔曼滤波***的误差是有界的。稳态的滚转角误差的1σ标准差小于校正***的噪声标准差(0.2636°)，再次证明了采用传感器融合算法进行姿态角估计优于单纯的角速率陀螺***或校正***。俯仰角误差同滚转角误差情况类似，这里不再赘述。

估计滚转角同真实滚转角φ之间的误差e_φ如图7所示。虚线为仅用角速率陀螺估计产生的e_φ，虽然噪声很小，但是漂移较大。实线为卡尔曼滤波算法估计产生的e_φ，可见，由传感器融合技术估计的姿态角结合了陀螺***低噪声的校正***不漂移的优点。

2)飞行试验

为了进一步验证航姿***在真实飞行环境中的性能，在数学仿真以及地面实验之后，还需进行飞行试验。试验过程如下：将商用惯性测量单元AHRS-400CC置于载体无人机上，在开始飞行后记录飞行数据，包括三轴加速度、三轴角速度以及飞行时间。为验证航姿***在载体机动状态下的姿态估计能力，由操控手操纵飞机进行大机动飞行，滚转角φ变化范围为±80°之间，俯仰角θ变化范围在±40°之间。飞行结束后，使用本章提出的姿态估计算法对AHRS-400CC采集的数据进行离线滤波计算和分析，得到姿态角信息。最后，将滤波得到的姿态角同AHRS-400CC自身内置算法计算的姿态角比较分析。由于AHRS-400CC航姿估计算法中具备对载体机动条件下的解决方案，故而认为其输出的姿态角为真实的飞机姿态。

根据仿真分析的结论，在大动态飞行条件下过载补偿法不可用，因此本节仅采用时间窗口法处理飞行数据。图8和图9显示了估计姿态角同真实姿态角之间的对比，分别采用了三种不同的姿态角估计方法进行对比，包括时间窗口法(虚线)、陀螺积分(划线)以及常规卡尔曼滤波(虚线)。由于本次飞行试验中没有使用GPS模块，不能对偏航角ψ校正，这里只估计了无人机的滚转角φ和俯仰角θ。

由图8(a)和图9的(a)可知，采用时间窗口法估计的姿态角可以很好地符合真实值。在图(b)中，陀螺积分得到的姿态角会有一定的漂移。而依靠常规卡尔曼滤波计算的姿态角失真严重，这是因为在飞行试验中，无人机大部分时间处于过载状态，加速度计的量测数据已经不能如实地反应姿态信息，从而使其不能正确地对陀螺积分值进行修正。表2给出了三种方法进行姿态估计的RMSE。由此可见，时间窗口法较好地解决了载体机动条件下的姿态估计问题。

表2 基于飞行试验的姿态估计值RMSE对比

图10给出了姿态估计过程中控制滤波器开关的标志量，图11则显示了滤波器开关的触发量和门限值(图中标号1所示)，这里触发量取为偏航角速度r。可以看出，由于航向运动比较剧烈，引起了较大的偏航角速度。当触发量大于门限值时，标志量为零，滤波器断开，此时完全依靠角速率陀螺估计姿态。当触发量低于门限值时，标志量置一，启动卡尔曼滤波器，此时由于机动过载很小，可以利用加速度计修正估计值，从而完成姿态确定。

综上可见，本发明提出了一种了基于低成本传感器(角速率陀螺、加速度计、GPS模块)的姿态航向估计算法。首先推导了运动载体姿态航向参考***基于误差四元数的数学模型，采用基于卡尔曼滤波的传感器融合技术以提高航姿***的精度。对于在空间运动的小型无人机，当其处于机动条件时，校正***由于存在附加加速度而不能准确地估计姿态。这对这一问题，提出了时间窗口法和过载补偿法两种解决方案。在确定了姿态航向参考***方案之后，分别进行了静态实验以及飞行试验以验证姿态算法的正确性与可行性。结果分析表明，本发明能准确地估计无人机飞行过程中的空间姿态，可以应用于低成本小型无人机***。

Claims (4)

1.一种用于低成本小型无人机的姿态航向参考***，其特征在于，包括角速率陀螺、加速度计、GPS、角速率陀螺运算模块、校正模块和卡尔曼滤波器，其中：

角速率陀螺测量飞机的滚转角速率、俯仰角速率和偏航角速率；

加速度计测量重力在机体坐标轴系下的分量；

GPS测量飞机的航迹方位角；

角速率陀螺运算模块通过角速率陀螺测量的滚转角速率、俯仰角速率和偏航角速率，得到带偏移的滚转角φ₁、俯仰角θ₁和偏航角ψ₁；

校正模块通过加速度计测量的重力在机体坐标轴系下的分量得到校正模块估计的滚转角φ₂、俯仰角θ₂，并将GPS测量得到的航迹方位角作为估计的偏航角ψ₂；

卡尔曼滤波器将角速率陀螺运算模块和校正模块产生的数据进行融合，得到最终的姿态角[φ θ ψ]^T；

所述角速率陀螺运算模块使用四元数法建模，首先定义姿态四元数q：

$q=\left[\begin{array}{c}{q}_0\\ \stackrel{\→}{q}\end{array}\right]$

其中q₀称为四元数q的标量部分， $\stackrel{\→}{q}={\left[\begin{array}{ccc}{q}_1& q_2& q_3\end{array}\right]}^T$ 称为四元数q的矢量部分，通过角速率陀螺的测量值实时更新四元数：

$\stackrel{\·}{q}=\frac{1}{2}\mathrm{\Ω}\·q=\frac{1}{2}\left[\begin{array}{cccc}0& -p& -q& -r\\ p& 0& r& -q\\ q& -r& 0& p\\ r& q& -p& 0\end{array}\right]\left[\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$

其中，p为滚转角速率，q俯仰角速率，r为偏航角速率；

然后通过以下公式得到载体姿态角

$\begin{array}{c}{\mathrm{\φ}}_1=\arctan \frac{2(q_1{q}_3+q_0{q}_1)}{1-2(q_1^2+q_2^2)}\\ {\mathrm{\θ}}_1=\arcsin 2(-q_1{q}_3+q_2{q}_0)\\ {\mathrm{\ψ}}_1=\arctan \frac{2(q_1{q}_2+q_3{q}_0)}{1-2(q_2^2+q_3^2)}\end{array};$

所述卡尔曼滤波器工作步骤为：在确定初始条件后，就利用角速率陀螺的测量数据积分，进行姿态估计，这一步骤称为“时间更新”，提供了高带宽的姿态信息；在时间更新阶段，由于陀螺数据存在噪声，使得估计姿态的误差随时间累积，因而不能靠单纯的陀螺数据积分确定姿态，为了抑制陀螺误差，需要引入校正***对陀螺数据进行周期性的修正，这一步骤称为“量测更新”，状态误差的协方差矩阵以及陀螺漂移估计也在该步骤校正；接下来，进行新一轮的时间更新，如此周期性重复；

所述卡尔曼滤波器进行数据融合的具体方法为：

第一步：利用角速率陀螺测量数据计算姿态四元数

${\stackrel{\stackrel{\·}{^}}{q}}^{-}=\frac{1}{2}\mathrm{\Ω}\·{\hat{q}}^{-}$

第二步：预测状态误差协方差矩阵P

$P_k^{-}=F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^T+G_{k-1}{Q}_{k-1}{G}_{k-1}^T$

式中，Q_k-1为过程噪声协方差矩阵，F和G为雅克比矩阵；

第三步：在校正***中，利用加速度计及GPS，得到由校正***估计的姿态角信息[φ₂ θ₂ ψ₂]^T；

第四步：利用上一时间步长估计的***姿态角信息 ${\left[\begin{array}{ccc}\widehat{\mathrm{\φ}}& \widehat{\mathrm{\θ}}& \widehat{\mathrm{\Ψ}}\end{array}\right]}^T$ 同校正***得到的姿态角信息作差，得到卡尔曼滤波器的观测量

$Z=\left[\begin{array}{c}\mathrm{\δ\φ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{\mathrm{\φ}}_2\\ {\mathrm{\θ}}_2\\ {\mathrm{\ψ}}_2\end{array}\right]-\left[\begin{array}{c}\widehat{\mathrm{\φ}}\\ \widehat{\mathrm{\θ}}\\ \widehat{\mathrm{\ψ}}\end{array}\right]$

第五步：计算卡尔曼增益矩阵

$K_k=P_{k-1}{H}_k^T{(H_k{P}_{k-1}{H}_k^T+R_k)}^{-1}$

式中，R_k为量测噪声协方差矩阵，H为雅克比矩阵；

第六步：更新状态误差协方差矩阵

$P_k^{+}=(I-K_k{H}_k)P_k^{-}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

第七步：更新状态向量

${\hat{X}}_k^{+}={\left[\begin{array}{cccc}{\stackrel{\widehat{\→}}{q}}_{\mathrm{ek}}^{+}& {\mathrm{\δ}\hat{b}}_{\mathrm{pk}}^{+}& {\mathrm{\δ}\hat{b}}_{\mathrm{qk}}^{+}& {\mathrm{\δ}\hat{b}}_{\mathrm{rk}}^{+}\end{array}\right]}^T=K_k{Z}_k$

第八步：使用估计的误差四元数及陀螺漂移和修正时间更新步骤中所得姿态四元数及角速率陀螺测量值

${\hat{q}}_k^{+}={\hat{q}}_k^{-}\⊗{\left[\begin{array}{cc}1& {\stackrel{\widehat{\→}}{q}}_{\mathrm{ek}}^{+}\end{array}\right]}^T$

${\hat{b}}_k^{+}={\hat{b}}_k^{-}{+\mathrm{\δ}\hat{b}}_k^{+}$

${\widehat{\mathrm{\ω}}}_k^{+}={\widehat{\mathrm{\ω}}}_k^{-}-{\hat{b}}_k^{+}$

第九步：由姿态四元数即可计算出姿态角[φ θ ψ]^T。

2.根据权利要求1所述的姿态航向参考***，其特征在于，所述校正模块通过以下公式确定估计的滚转角φ₂和俯仰角θ₂：

${\mathrm{\θ}}_2=\arcsin \frac{f_x}{g}$

${\mathrm{\φ}}_2=\arcsin \frac{{-f}_y}{g\cos \mathrm{\θ}}$

其中，g为重力加速度常数，f_x、f_y为加速度计读数。

3.根据权利要求1所述的姿态航向参考***，其特征在于，当飞机进行倾斜转弯时，所述校正模块通过以下公式确定估计的滚转角φ₂和俯仰角θ₂

${\mathrm{\θ}}_2=\arcsin \frac{f_x}{g}$

${\mathrm{\φ}}_2=\arcsin \frac{{-f}_y+{\mathrm{rV}}_{\mathrm{TAS}}}{g\cos \mathrm{\θ}}$

其中，g为重力加速度常数，f_x、f_y为加速度计读数，r为绕z轴方向的偏航角速度，V_TAS为真空速。

4.根据权利要求1-3任一项所述的姿态航向参考***，其特征在于，当飞机进行倾斜转弯时，选择角速率的范数即绝对值作为触发卡尔曼滤波器的标志，时间窗口的开闭由下式确定

$\left\{\begin{array}{c}u\left(t\right)\≤\mathrm{\β},\mathrm{flag}=1\\ u\left(t\right)>\mathrm{\β},\mathrm{flag}=0\end{array}\right.$

其中，β为时间窗口门限值，flag为窗口标志；

当u(t)≤β时，flag＝1，窗口打开，使用卡尔曼滤波器进行定姿；当u(t)>β时，flag＝0，窗口关闭，此时断开滤波器，停止校正***的修正。