基于四元数衍生无迹卡尔曼滤波的二段式多旋翼无人机姿态估计算法

蔡安江,刘凯峰,郭师虹,舒 展

(1.西安建筑科技大学机电工程学院,陕西西安 710055;2.西安建筑科技大学土木工程学院,陕西西安 710055)

1 引言

多旋翼无人机姿态估计是多旋翼无人机姿态控制、导航定位的基础,姿态估计的精度直接影响着姿态控制和导航定位的精度.考虑到尺寸、功耗以及成本等原因,多旋翼无人机通常不会使用传统的体积和重量比较大的高精度的传感器,而是使用价格相对低廉的微机电系统(micro-electro-mechanical system,MEMS)惯性传感器来进行姿态估计.但是目前的MEMS惯性传感器相较于军用级别的高精度惯性传感器来说精度低、稳定性较差并且其系统误差随时间积累较快,其测量值无法满足多旋翼无人机姿态控制的要求.同时多旋翼无人机常使用低成本的单片机或者微控制器作为控制单元,整个无人机控制系统需具有较强的实时性.因此,必须采用合理的姿态估计算法来处理传感器的数据,所以设计精度高、迭代速度快且性能稳定的姿态估计算法一直是多旋翼无人机领域的研究难点和热点之一.

低成本惯性陀螺存在较大的零点漂移,直接对陀螺仪数据进行积分会使得姿态角发散,因此通常使用加速度计数据作为参考去估计横滚角和俯仰角,使用磁力计数据作为参考去估计航向角.低成本陀螺仪、加速度计和磁力计测量值都包含较大的噪声,现有研究人员对于如何使用存在噪声干扰的陀螺仪、加速度计和磁力计数据获得精确的姿态角的问题进行了大量的研究[1–2].由于四元数能进行全姿解算并具有较小的计算量,卡尔曼滤波器能有效的降低噪声,均被广泛用于无人机姿态解算中[3–5].由于姿态估计模型具有非线性的特征,通常使用非线性卡尔曼滤波器来处理[6–8].

刘兴川等[9]提出一种基于四元数的扩展卡尔曼滤波(extended Kalman filter,EKF)算法来降低陀螺仪、加速度计和磁力计的测量噪声,但并未考虑加速度计数据会对航向角的估计造成干扰以及磁力计数据会对横滚角和俯仰角的估计造成干扰的问题.针对这个问题,Suh等[10–11]在基于四元数的扩展卡尔曼滤波器基础上,将加速度计和磁力计数据分为两个阶段进行处理,提出一种二阶段EKF滤波算法,以降低磁力计数据对横滚角和俯仰角估计的干扰,但是该算法需要改变卡尔曼滤波增益系数,这样会使得卡尔曼滤波器失去最优性.Valenti等[12]在二阶段EKF滤波算法的基础上提出一种代数四元数算法,将状态四元数分为两个辅助四元数以降低磁力计数据对横滚角和俯仰角估计的干扰.但在算法推导过程中,存在一些简化过程,影响算法的可靠性;且在算法实现过程中加入了一些外部计算和协方差估计过程,增加了整个算法的计算量.同时,目前大多数姿态估计算法均是在EKF基础上设计的姿态估计算法.EKF是针对非线性系统的一种最广泛的状态估计方法,但EKF仅仅是将非线性方程使用泰勒公式展开的一阶近似,在系统存在较强的非线性时,EKF算法存在较大的截断误差.无迹卡尔曼滤波(unscented Kalman filter,UKF)也是卡尔曼滤波的延伸,它降低了扩展卡尔曼滤波线性化带来的误差,与EKF相比,UKF带来了巨大的改进,Garcia,Cao等[13–14]就曾将UKF应用于小型卫星和航天器的姿态估计中.虽然UKF减小了线性化误差,拥有较高的状态估计精度,但UKF 计算过程中的无迹变换(unscented transformation,UT)会带来大量的计算,这给使用低成本处理器的多旋翼无人机实时解算带来很大的困难.

本文从四元数本质考虑,推测四元数各元素分别包含着不同的姿态角信息,通过仿真实验证明了该推测,由此设计了一种可以分离干扰的二段式算法,且针对传统UKF算法计算量大的问题,参考了一种相对计算量较小的衍生无迹卡尔曼滤波算法(derivative unscented Kalman filter,DUKF)[15–16],提出了一种基于四元数DUKF的二段式多旋翼姿态估计算法.最后本文通过提取静止状态PIXHAWK飞控中的惯性传感器和磁力计数据,进行仿真实验,验证了本文所提算法的有效性.

2 基于四元数的姿态估计系统模型

2.1 姿态估计系统的状态方程

则可以建立连续时间的状态方程为[17]

其中

为了方便迭代计算,将连续时间的状态方程转换为离散时间状态方程,令AT=T为陀螺仪数据采样间隔,对连续时间方程离散化:

令Ak=(I+ATT),则有离散时间状态方程为

其中:I为四阶单位矩阵;为了描述方便,本文后续内容中符号将用q代替.

2.2 姿态估计系统的量测方程

本文采用三轴加速度计和三轴磁力计数据来对四元数状态向量进行校正,校正过程分两个阶段进行,须建立两个量测方程.

2.2.1 基于三轴加速度计数据的量测方程的建立

三轴加速度计可测得重力场在机体坐标系下的3个分量,重力场在地球坐标系的3个分量为[0 0|g|]T,其中|g|为重力加速度的绝对值,则有基于三轴加速度计数据的量测方程为[18]

2.2.2 基于三轴磁力计数据的量测方程的建立

三轴磁力计可测得地磁场在机体坐标系下的3个分量,假设当地磁场在地球坐标系的3 个分量为[bxbybz]T,则有基于三轴磁力计数据的量测方程为

3 基于四元数DUKF的二段式姿态估计算法

3.1 衍生无迹卡尔曼滤波算法(DUKF)

假设非线性系统状态方程和量测方程为

针对上述状态空间模型的滤波问题,由于其状态方程具有线性特征,文献[15]提出一种衍生无迹卡尔曼滤波算法(DUKF).与标准的无迹卡尔曼滤波算法(UKF)相比,DUKF在时间更新的过程中采用与卡尔曼滤波(Kalman filter,KF)相同的形式,这样可以简化计算过程,从而避免标准UKF因无迹变换(UT)带来的额外计算量;而在量测更新的过程中则使用无迹变换,以获得与UKF算法相同的非线性滤波精度.因此与UKF算法相比,DUKF算法在保持精度的同时,很好的缩短了算法每个周期的迭代时间.DUKF算法的具体步骤可参考文献[15–16].

3.2 二段式四元数状态向量校正策略

无人机姿态估计中,加速度计数据用来校正由陀螺仪漂移造成的姿态角累积误差,但由于加速度计的数据不能用于测量航向角,通常引入磁力计来校正航向角的估计.但这样导致传感器配置冗余,磁力计的数据不仅可以测得航向角,对横滚角、俯仰角也会有影响,同时,错误的加速度计数据也会影响航向角的估计[12].横滚角和俯仰角的估计的精确度是无人机稳定飞行的基础,在无人机作业过程中可能会出现一些电磁干扰污染磁力计数据,以至于严重影响横滚角、俯仰角的估计,造成无人机失稳甚至坠机.因此,消除磁力计数据对于横滚角、俯仰角估计的影响尤为重要,本文通过对衍生无迹卡尔曼滤波过程中的四元数校正向量进行处理来解决这一问题.

如式(15)所述,衍生无迹卡尔曼滤波使用残差乘以卡尔曼滤波增益系数得到相应的校正向量来校正先验估计值,

其中:a代表旋转角度,e=[exeyez]代表旋转轴.使用四元数表示姿态角时,四元数中有一个元素是冗余的,从式(17)可以看出,p1,p2,p3已经代表了旋转轴和旋转角度,元素p0是冗余的,p0的存在是为了保证四元数p为单位四元数,即

p1,p2,p3三个元素分别与绕x轴、y轴和z轴的旋转有关,由此可设想p1主要包含横滚角信息,p2主要包含俯仰角信息,p3主要包含航向角信息,这样可以在量测更新阶段中求得校正向量∆x后,保留需要的信息,去掉不需要的信息以消除干扰.现有姿态估计算法常将三轴加速度计数据和三轴磁力计数据作为一个六维量测向量进行姿态解算,这样计算出的校正向量耦合了加速度计和磁力计的数据,使得横滚角、俯仰角和航向角的估计均受到这两种传感器数据的影响,无法分离相互间的干扰.为消除相互干扰,本文将加速度计和磁力计数据分为两个量测更新阶段处理.第1阶段中,由加速度计数据计算出的四元数校正向量乘以相应的校正系数得到修改后的校正向量,以去除原来校正向量中关于校正航向角的信息,消除加速度数据对航向角的影响,修改后的校正向量被代入式(15)中计算得到经过加速度计数据校正的第1阶段后验状态估计值,完成第1阶段量测校正;第2阶段中,由磁力计数据计算出的四元数校正向量经过乘以相应的校正系数的得到修改后的校正向量,以去除校正横滚角、俯仰角的信息,消除对横滚角和俯仰角的影响,修改后的校正向量和第1阶段后验状态估计值也被代入式(15)中计算得到经过磁力计数据校正的第2阶段后验状态估计值,完成第2阶段量测校正.这样使得加速度计数据只用以校正横滚角、俯仰角,磁力计数据只用以校正航向角,达到分离相互干扰的目的.由此,提出一种基于四元数的DUKF二段式姿态估计算法,算法具体实施过程如第2.3节所示.

3.3 基于四元数DUKF 的二段式姿态估计算法(two-step DUKF,TDUKF)

结合前面的系统状态方程和量测方程,以姿态四元数为状态向量的非线性系统为

其中:qk为k时刻系统的四元数状态向量;z1k和z2k分别为k时刻三轴加速度计数据和三轴磁力计数据组成的量测向量;Ak−1为式(7)中的状态转移矩阵;wk,v1k和v2k为互不相关的零均值高斯白噪声过程,其方差分别为

则基于四元数DUKF的二段式姿态估计算法流程为

a)系统初始化.

设置初始状态四元数为q0=[1 0 0 0]T,初始状态误差协方差阵为

计算系统先验估计.

1)读取三轴陀螺仪数据,获得wx,wy和wz,将其代入式(4)中计算得到Ω.

2)计算离散时间状态转移矩阵.

3)时间更新.

由于系统状态方程具有线性特征,系统先验状态估计值及其误差协方差阵的计算与线性卡尔曼滤波相同,则k −1时刻系统先验状态估计值为

k −1时刻系统先验状态估计误差协方差为

4)Sigma点选取.

根据先验状态估计值及其误差协方差选择一组加权Sigma点,选取的Sigma点为

b)开始第1阶段校正.

1)第1阶段量测更新.

经过非线性量测方程hg(·)变换后的Sigma点为

2)计算量测预测值及其协方差

计算先验状态估计值与量测预测值间的互协方差

3)确定卡尔曼增益矩阵.

4)读取加速度计数据zk1=[axayaz]T.

5)计算第1阶段校正向量

其中:qa1为与∆q1维度相同的校正系数,⊙表示向量间对应元素相乘的运算符.

c)开始第2阶段校正.

1)第2阶段量测更新.

经过非线性量测方程hm(·)变换后的Sigma点为

2)计算量测预测值及其协方差

计算先验状态估计值与量测预测值间的互协方差

其中:

3)确定卡尔曼增益矩阵

4)读取磁力计数据zk2=[mxmymz]T.

5)计算第2阶段校正向量

其中:qa2为与∆q2维度相同的校正系数,⊙表示向量间对应元素相乘的运算符.

d)进入下一个迭代周期,重复步骤2)–4)的过程.

图1 TDUKF算法结构框图Fig.1 TDUKF algorithm block diagram

3.4 状态四元数校正系数的选取

为证明第2.2节中四元数状态向量校正策略的可行性,本文将qa1和qa2按不同取值分为表1中的7种情况,通过从水平放置的PIXHAWK飞控中提取陀螺仪、加速度计及磁力计数据,利用MATLAB软件进行仿真实验,仿真实验结果如图2所示.如图2(a)所示,只使用陀螺仪数据更新姿态角时,由于陀螺仪漂移的存在,更新的姿态角会随着时间发散;图2(b)–2(c)与图2(a)出现了相同的现象,表明∆q1和∆q2的第1个元素并未影响姿态四元数,即其不包含姿态信息;图2(d)中俯仰角和航向角发散,但横滚角收敛;图2(e)中横滚角和航向角发散,但俯仰角收敛;图2(f)中航向角发散,但横滚角和俯仰角收敛,表明∆q1中第2和第3个元素分别起了校正了横滚角和俯仰角的作用,即表明四元数的第2,3个元素分别主要包含着横滚角和俯仰角的信息;图2(g)中横滚角和俯仰角发散;图2(h)中航向角收敛,图2(i)中所有姿态角都收敛,表明∆q2的第4个元素对校正航向角产生作用,即表明四元数第4个元素主要包含着航向角信息;综上可知本文之前设想正确.本文所提姿态解算算法中,加速度计数据只用来校正横滚角和俯仰角,所以在加速度校正阶段,对于校正参数∆q1,置其第4个元素为零,以消除加速度计数据对航向角估计的干扰;同理在磁力计校正阶段,为消除磁力计数据对横滚角和俯仰角估计的干扰,将校正参数∆q2的第2和第3个元素置为零,即qa1和qa2按Case7取值.

表1 qa1,qa2取值表Table 1 value table of qa1,qa2

图2 不同情况下的的姿态角Fig.2 Attitude angle under different conditions

4 仿真实验及结果分析

为了验证本文所提算法在计算精度和解算时间方面的有效性,本文利用二段式EKF算法(two-step EKF,TEKF)和二段式UKF算法(two-step UKF,TUKF)与本文所提算法进行仿真实验对比,仿真结果见图3 和表2–3.为了验证本文所提算法的抗干扰性能,使用仅包含加速度计数据的DUKF校正的算法(acceleration-DUKF,A–DUKF)以及传统的将加速度数据和磁力计数据的六维量测向量一起处理的DUKF算法(normal-DUKF,N–DUKF)与本文所提算法进行仿真实验对比,仿真结果见图4(a)–(b);室内外环境中经常会存在各种物体造成的电磁场干扰,如高压电塔,各种电器等,这些干扰多为0.1 Gs级别[20],为了验证本文所提算法在环境出现磁场异常时的分离相互干扰的能力,人为的在三轴磁力计40∼80 s 的数据上均加入一个约为0.1 Gs的常值偏移以模拟环境中出现的磁异常,使用传统的将加速度数据和磁力计数据的六维量测向量一起处理的DUKF算法(N–DUKF)与本文所提算法进行仿真实验对比,仿真结果见图4(c)–(e);在执行一般任务时,多旋翼无人机加速比较缓慢,为了验证本文所提算法在存在载体加速度时的分离相互干扰的能力,人为的在三轴加速度计40∼80 s的数据上均加入一个约为0.05 g的常值偏移以模拟存在的外部加速度干扰,使用传统的将加速度数据和磁力计数据的六维量测向量一起处理的DUKF算法(N–DUKF)与本文所提算法进行仿真实验对比,仿真结果见图4(f)–(h).

由图3(a)–(b)可以看出,本文所提算法(TDUKF)解算精度明显高于TEKF算法,与TUKF算法的解算精度相当,由图3(c)可以看出,这3种算法估计航向角时,均能较快的收敛.

图3 不同算法姿态角对比Fig.3 Attitude angle comparison of different algorithms

由表2 的数据可以看出TEKF 算法的解算精度最低,TUKF算法的解算精度高于TEKF算法,本文所提的TDUKF算法的解算精度略微高于TUKF算法.

表2 姿态误差(◦)Table 2 Attitude error(◦)

表3 的数据是在装有MATLAB 的PC 机上经过100次蒙特卡洛仿真计算出的平均值,PC配置如下:处理器Intel Core i5--2450 M 2.5 GHz,内存4 GB.由表3可知,与TUKF算法相比,本文所提算法(TDUKF)的运算时间约缩短了40%左右,这为姿态解算实时性带来很大的提高.

表3 平均每个算法周期解算时间(ms)Table 3 Average calculation time per algorithm cycle(ms)

由图4(a)–(b)可以看出本文所提算法(TDUKF)的横滚角和俯仰角的估计和只使用加速度计数据的DUKF算法(A–DUKF)的相当,而将加速度数据和磁力计数据一起处理的DUKF算法(N–DUKF)由于磁力计数据的影响,横滚角和俯仰角的估计都出现了一定的偏差;由图4(c)–(e)可以看出,加入人为磁干扰后,传统N–DUKF算法和本文所提TDUKF 算法的航向角估计都受到影响,与图4(a)–(b)相比,由于加入了人为磁干扰,N–DUKF算法的横滚角和俯仰角估计均受到影响,但TDUKF算法的横滚角和俯仰角估计并未受到加入的磁干扰的影响;由图4(f)–(h)可以看出,加入人为加速度干扰后,传统N–DUKF算法的横滚角、俯仰角和航向角估计都受到影响,但本文所提TDUKF算法只有横滚角和俯仰角的估计受到影响,航向角估计并未受到影响.说明本文所提算法能很好的消除磁力计数据对俯仰角和横滚角估计的影响以及加速度计数据对航向角估计的影响,分离了相互之间的干扰,具有良好的抗干扰能力.

综上所述,本文所提算法(TDUKF)具有良好的抗干扰能力,且在解算时间上又明显少于传统UKF算法,略高于传统EKF算法,但解算精度高于传统EKF算法.

图4 不同算法姿态角对比Fig.4 Attitude angle comparison of different algorithms

5 结论

为了提高多旋翼无人机姿态估计的精度、系统实时性以及抗干扰能力,本文从四元数本质出发,推测四元数的第2、第3和第4个元素分别主要包含着横滚角、俯仰角和姿态角的信息,并通过仿真实验证明该推测.根据四元数的这种性质,本文结合一种计算量较常规UKF算法小的DUKF算法,将加速度数据和磁力计数据分别在两个校正过程中进行处理,并将校正过程中的校正四元数乘以相应的系数来设置所需要的姿态角信息,以消除加速度计数据对航向角估计的干扰以及磁力计数据对横滚角和俯仰角估计的干扰,由此提出一种基于四元数DUKF的二段式姿态估计算法(TDUKF).并利用从水平放置的PIXHAWK飞控中提取的传感器数据,将本文所提算法(TDUKF)与二段式EKF 算法(TEKF)以及二段式UKF 算法(TUKF)进行仿真对比实验,从解算精度、解算时间方面证明了本文所提算法的有效性;将本文算法所提算法与使用仅包含加速度计数据的DUKF 校正的算法(A–DUKF)以及常规的将加速度数据和磁力计数据的六维量测向量一起处理的DUKF算法(N–DUKF)进行仿真实验对比,从抗干扰能力方面证明了本文所提算法的有效性,对工程实践具有一定的参考意义.