自适应误差四元数无迹卡尔曼滤波四旋翼飞行器姿态解算方法

崔培林,周翟和,吕品,胡斌

(南京航空航天大学自动化学院,211106,南京)

四旋翼飞行器(后文简称飞行器)是控制科学和无人机领域内备受关注的研究对象,飞行器姿态信息的测量精度直接影响了飞行器速度、位置的导航精度[1-2]。在小型飞行器中,常使用陀螺仪、加速度计以及磁强计作为姿态测量器件,陀螺仪存在累积误差,而加速度计和磁强计受外界干扰较大,需要对其进行有效的数据融合以保证获得较为精确的姿态角[3-4]。

在姿态解算方法中,文献[5-6]使用卡尔曼滤波(Kalman Filter,KF)算法进行数据融合,该方法建模简单,实时性较好,但是忽略了非线性因素对滤波带来的影响。文献[7-8]使用扩展卡尔曼滤波(Extended Kalman Filter,EKF)方法,这是一种关于噪声均值和协方差的线性化方法,用这种方法对状态方程和量测方程进行线性化处理,必然会引入误差。对于弱非线性,EKF可获得较为准确的结果,然而,对于强非线性,线性化模型的误差可能导致滤波器性能下降甚至发散。文献[9]提出了基于四元数无迹卡尔曼滤波(Unscented Kalman Filter,UKF)算法,该方法使用四元数法进行姿态解算,保证了系统可以全姿态工作,具有很好的非线性姿态估计精度与滤波稳定性,但是具有较大的计算量。文献[10-11]中提出了误差四元数卡尔曼滤波(Error Quaternion Kalman Filter,EQKF)方法,使用误差四元数的方法构建系统的状态方程与量测方程,具有计算量小,响应快的优点,但是也忽略了非线性因素的影响。

针对惯性传感器在飞行器姿态解算中存在随机漂移误差和易受到非重力加速度影响的问题,本文在以上研究的基础上,提出了一种自适应误差四元数无迹卡尔曼滤波(Adaptive Error Quaternion Unscented Kalman Filter,AEQUKF)的飞行器姿态解算方法。选用误差四元数和陀螺仪的关系,并结合陀螺仪漂移误差模型构建系统的状态方程,利用加速度计和磁强计数据构建系统的量测方程,最后选用误差四元数无迹卡尔曼滤波(Error Quaternion Unscented Kalman Filter,EQUKF)框架实现数据融合,通过判断系统是否存在非重力加速度,自适应调整量测噪声协方差矩阵,减小非重力加速度的影响。该方法抑制了传感器随机漂移误差,自适应补偿了非重力加速度对姿态估计的影响,相比较EKF算法进一步提高了姿态系统检测的精度。实验表明,当飞行器有非重力加速度时,该算法比传统算法具有更好的姿态解算精度。

1 误差四元数无迹卡尔曼滤波建模

1.1 状态方程的建立

陀螺仪输出的信号与运动条件无关,但是陀螺仪信号具有偏置误差[12-13]。因此,陀螺仪测量模型为角速度、偏置误差、白噪声的总和,即

ωo=ω+εg+ng

(1)

式中:ωo表示陀螺仪的输出;ω表示实际角速度;εg表示陀螺仪的偏置误差;ng表示陀螺仪的零均值白噪声。

若用⊗表示四元数乘法,则计算真实四元数的微分方程为

⊗ω

(2)

计算四元数的估计值的微分方程为

⊗ωo

(3)

(4)

式中

Qe=[qe1qe2qe3]T

(5)

其中qe1、qe2、qe3为误差四元数的3个参数。

(6)

对式(6)两边求导可得

(7)

将式(1)(2)代入式(7)可得

(8)

将式(6)代入式(8)可得

(9)

(10)

进一步地,将式(1)代入式(10)可得

(11)

对于3阶向量,定义[p×]为

(12)

因为误差四元数第一项为常数项1,对其求导为常数0,对后面模型的搭建没有影响。式(11)可简化为

0.5(εg+ng)+[Qe×](εg+ng)

(13)

又因为

[Qe×]ωo=-[ωo×]Qe

(14)

而Qe、εg和ng很小,所以[Qe×](εg+ng)为高阶小项,可以忽略。将式(14)代入式(13),忽略掉[Qe×]·(εg+ng)可得

(15)

若nεg为陀螺仪漂移零均值白噪声,则陀螺仪漂移模型为

(16)

选取系统状态量为

x=[qe1qe2qe3εgxεgyεgz]T

(17)

式中:εgx、εgy、εgz表示陀螺仪漂移三轴分量。

根据式(15)～(17),可得误差四元数UKF的状态方程为

xk=f(xk-1)+wk-1

(18)

式中:f(xk-1)为系统的状态转换方程;wk-1为系统过程噪声。f(xk-1)和wk-1的具体公式为

(19)

(20)

式中T表示采样时间。

系统协方差矩阵为

(21)

式中Qg和Qεg分别为陀螺仪噪声协方差矩阵和陀螺仪漂移噪声协方差矩阵,具体值由实验获得。

1.2 观测方程的建立

加速度输出数据信号模型为重力、非重力加速度、零均值白噪声的总和,磁强计输出数据信号模型为地磁场和零均值白噪声的总和,公式为

(22)

(23)

选取观测量z=[fafm]T,根据式(18)(19),误差四元数UKF的量测方程为

zk=h(xk)+vk

(24)

式中:h(xk)为系统的量测转换方程;vk为系统量测噪声。h(xk)和vk的具体公式为

(25)

(26)

由于ab和na互不相关,所以量测噪声协方差矩阵公式为

(27)

式中:Rb为非重力加速度协方差矩阵,具体值参考本文2.2节;Ra和Rm分别为加速度计噪声协方差矩阵和磁强计噪声协方差矩阵,具体值由实验获得。

2 自适应误差四元数无迹卡尔曼滤波算法

2.1 对称采样

无迹变换(Unscented Transformation,UT)的主要思想是使用近似概率分布来代替非线性函数,在原先状态分布中按照某一规则选取确定性的点集S(即Sigma点),使这些点集的均值和方差等于原状态分布的均值和方差。通过非线性变换,加权计算得到变换后的均值和方差。

UT变换算法中采样策略是关键部分,本文的采样策略为对称采样[15],具体如下。

(1)Sigma点的产生

(28)

(2)权值计算

(29)

2.2 非重力加速度补偿

当飞行器在上升、下降、加速、减速等阶段存在着较大的非重力加速度时,加速度计不能将其区分出来,由加速度计解算得到的姿态角度信息将受到非重力加速度的影响,姿态角包含较大误差,将直接影响飞行器的飞行控制。本文通过自适应调整协方差矩阵来减少非重力加速度带来的影响。

判断是否存在非重力加速度的公式为

|‖f‖-g|<δg

(30)

式中:‖f‖为加速度输出数据的范数;g为重力加速度;δg为阈值常数,其取值和加速度计噪声有关。

若上式满足,则飞行器不存在非重力加速度,即

Rb=0

(31)

反之,则存在非重力加速度,即

(32)

式中:ρ为常数,由多次实验获得,用于调节滤波精度;ra为加速度计的残差信息,公式为

ra=zk-h(xk)

(33)

2.3 自适应误差四元数无迹卡尔曼滤波算法

(34)

(35)

(2)状态更新。根据对称采样策略生成Sigma点,公式为

(36)

计算Sigma点的下一步预测值,对每个Sigma点非线性变换,公式为

χk+1,k=f(χk)

(37)

对变换后的Sigma点进行加权得到下一步预测和协方差矩阵,公式为

(38)

(39)

根据非线性观测方程对Sigma点集χk进行非线性变换,公式为

zk+1,k=h(χk+1,k)

(40)

使用加权计算,得到预测的观测值为

(41)

(3)非重力加速度补偿。根据式(30)判断飞行器系统是否存在非重力加速度。当不存在非重力加速度时,加速度输出的数据为当地重力加速度值,非重力加速度协方差矩阵为式(31),代入本文算法求解姿态数据;当系统存在非重力加速度时,残差信息式(33)中包含了最新时刻的加速度计数据,也保留了非重力加速度的信息,非重力加速度越大,残差信息也就越大,相应的非重力加速度协方差矩阵也就越大。利用式(32)调整协方差矩阵Rb,代入本文算法求解姿态数据。非重力加速度协方差矩阵是根据非重力加速度大小自适应更新的,非重力加速度越大,补偿效果越明显。

(4)量测更新。协方差矩阵更新公式[19-20]为

(42)

量测输出协方差矩阵更新公式为

(43)

卡尔曼增益矩阵更新公式为

(44)

状态更新后,滤波值更新公式为

(45)

后验方差矩阵更新公式为

Pk+1=Pk+1,k-Kk+1Pzk+1Kk+1

(46)

3 仿真与实验验证

使用Matlab仿真平台和飞行器平台对提出的算法进行验证。算法验证分为静态实验验证和动态非重力加速度仿真验证。静态实验主要是验证改进的算法具有良好的滤波效果,可以抑制陀螺漂移,获得较高的姿态解算的精度。动态非重力加速度仿真主要是为了验证飞行器在存在非重力加速度的情况下,该方法可以有效地减小非重力加速度影响,获得更为准确的滤波值。

具体实验和仿真参数如下:T=0.01 s,Ra=0.000 1I3,Rm=0.000 1I3,Qg=0.000 1I3,Qεg=0.000 001I3,P=0.000 1I6,ρ=10,δg=0.02。

3.1 静态验证

利用飞行器平台进行静态实验验证本文方法。飞行器实验平台如图1所示,姿态测量器件为MPU6050(三轴陀螺仪和三轴加速度计)和HMC5883L(三轴磁强计),飞行器轴长45 cm,数据通过UART通信,将传感器数据发送到上位机。飞行器在静止状态下的俯仰角、航向角和横滚角均为0°,飞行器不存在非重力加速度。

图1 飞行器实验平台

图2和表1给出了静态实验中AEQUKF的解算结果,图和表中“加速度计”、“陀螺仪”和“磁强计”表示该类传感器数据经过四元数解算方法直接解算得到的姿态数据,可以看出:相较于加速度计和磁强计的解算结果,AEQUKF具有较高的姿态解算精度;相较于陀螺仪的解算结果,AEQUKF滤除了陀螺漂移带来的影响。

(a)俯仰角误差

(b)横滚角误差

(c)航向角误差图2 静态条件下姿态解算误差

解算方法姿态角解算误差标准差/10-4(°)解算方法姿态角解算误差标准差/10-4(°)AEQ-UKF俯仰角横滚角航向角1.073.174.24加速度计俯仰角横滚角航向角2733 陀螺仪俯仰角横滚角航向角373935磁强计俯仰角横滚角航向角30

3.2 动态验证

飞行器在加速上升、减速下降等过程中存在较大的非重力加速度。为了验证提出的方法对非重力加速度有较好的抑制效果,利用Matlab平台对算法进行仿真验证。图3为Matlab仿真中加速度计的输出噪声数据,nx、ny、nz分别为加速度计在x、y、z方向的噪声,可以看出加速度计噪声部分在阈值范围外,说明仿真中存在非重力加速度影响。

(a)加速度计x方向噪声

(b)加速度计y方向噪声

(c)加速度计z方向噪声图3 动态条件下加速度计的输出噪声

图4和表2给出了动态非重力加速度仿真中AEQUKF解算结果,可以看出:EQUKF受到非重力加速度影响,滤波精度比较低,而本文提出的AEQUKF可以有效地减小非重力加速度影响,获得较高的姿态解算精度。

(a)俯仰角解算误差

(b)横滚角解算误差

(c)航向角解算误差图4 动态条件下姿态解算误差

解算方法姿态角解算误差标准差/10-3(°)俯仰角1.8AEQUKF横滚角2.1航向角2.4俯仰角52EQUKF横滚角62航向角67

4 总 结

通过研究飞行器姿态解算时存在非重力加速度影响的问题,提出了AEQUKF飞行器姿态估计算法。在AEQUKF算法中,基于误差四元数方法构建UKF状态方程和量测方程。当检测到存在非重力加速度时,利用残差信息自适应调整协方差矩阵,减小非重力加速度的影响,获得较为准确的姿态解算值,有效地减小了陀螺仪漂移误差带来的影响。实验表明,AEQUKF算法对非重力加速度具有很好的补偿作用,为飞行器提供了一个低成本、高精度的姿态检测系统。