基于扩展Kalman滤波的无人机位姿校正

李 可

(河南工程学院 工程训练中心，河南 郑州 451191)

基于扩展Kalman滤波的无人机位姿校正

李 可

(河南工程学院 工程训练中心，河南 郑州 451191)

无人机姿态控制问题是无人机稳定性飞行的关键，针对无人机在位姿参数不确定条件下制导控制器姿态定位精度不高的问题，提出了一种基于扩展Kalman滤波的无人机位姿校正方法，进行制导系统稳定性控制律设计.建立无人机飞行动力学模型，构建飞行弹道方程，分析无人机制导系统被控对象的约束参量，采用加速度计、陀螺计和磁力计进行位姿参量测量.考虑到位姿参数的不确定性，采用扩展Kalman滤波算法进行姿态参数的整定性处理，实现角度校正.将校正后的位姿参数输入模糊神经网络系统中，实现无人机制导控制律的优化设计.仿真结果表明，采用该方法进行无人机位姿校正和飞行制导控制，定姿精度较高、抗干扰能力较强，实现了飞行的稳定性控制.

无人机；制导系统；稳定性控制；扩展Kalman滤波

无人机是一种采用遥控装备进行远程制导控制的无人驾驶飞行器，在军事和民用领域都展示了较好的应用价值.对无人机准确、可靠地定姿控制并通过位姿参数的优化解算和自整定性融合处理，输入制导控制系统中以实现飞行稳定性控制是保证飞行稳定性的关键，故研究无人机制导系统的稳定性控制技术在促进无人机技术革新方面具有重要意义[1].

无人机制导系统稳定性控制建立在位姿参数测量和姿态修正的基础上.无人机在飞行过程中容易受到电子雷达探测和电磁等干扰，导致位姿参数存在不确定性，需要进行稳定性控制设计.将无人机的姿态数据通过惯导系统实时采集并传输到姿控系统，与航向陀螺仪进行信息交互，输出控制指令，以实现飞行控制.传统方法中，对无人机的控制算法主要有模糊PID控制[2]、自适应反演控制[3]、滑膜积分控制和姿态融合滤波控制[4]等，通过姿态参量解算设计控制器取得了较好的姿态稳定性.文献[5]提出了一种基于姿态融合滤波的无人机抗干扰控制算法，构建无人机在飞行时的姿态参量约束运动方程，通过姿态信息融合，以捷联惯导传输到执行器实现飞行控制优化，但该方法对气流小扰动等不确定因素的抗干扰能力不强；文献[6]提出了一种基于推力矢量的飞机纵向鲁棒动态逆控制模型，通过推力矢量的坐标变换得到飞行纵向模型，在线性不确定系统中进行无人机动态逆控制，改进的控制方法回避了时标分离问题，进行小扰动耦合处理，提高了飞行控制的抗干扰能力，但该方法计算开销较大且控制的实时性不好.

本研究针对上述问题提出了一种基于扩展Kalman滤波(extended Kalman filter,EKF)的无人机制导系统稳定性控制律设计方法，首先进行飞行动力学和被控对象分析，然后进行控制约束参量分析，构建改进的控制律，最后进行仿真测试，得出有效性结论.

1 飞行动力学分析与被控对象描述

1.1无人机飞行动力学方程

(1)

(2)

(3)

在采用姿态传感器进行飞行位姿参量测量和采集时，综合考虑飞机自身重力、流体动力、飞行推力等惯性力矩的影响，得到飞行弹道的非线性方程组：

(4)

1.2被控对象描述

采用加速度计、陀螺计和磁力计进行位姿参量测量，得到位姿参数模型，针对位姿参数的不确定性进行误差补偿和滤波处理，进行位姿参数的自整定性控制，采用改进的Kalman滤波算法实现角度校正，得到被动对象结构，如图1所示.

图1 无人机稳定性控制被控对象结构模型Fig.1 Structural model of controlled object for UAV stability control

(5)

采用四阶龙格库塔法求解约束状态方程，选择最优控制性能指标，得到随机最优导引律：

(6)

无人机外形是轴对称的，根据瞬时平衡假设，在水平面范围内，当位姿参数为一组随机序列时，基于Kalman滤波进行姿态参量的最优状态估计设计控制律.

2 控制律设计

2.1扩展Kalman滤波算法

在飞行动力学分析与被控对象描述的基础上进行无人机稳定性控制律改进设计，提出了一种基于扩展Kalman滤波的无人机制导系统稳定性控制律设计方法.首先，采用Kalman滤波技术求出飞行状态参量的最小方差估计值[8]，得到Kalman滤波函数：

(7)

式中：φα，ψα，γ为无人机在横向、侧向和横滚运动的末弹道参数；b1，b2，b3，d3为被控姿态参量的已知系数；Δb1，Δb2，Δb3，Δd3为未确定系数；fd1，fd2，fd3为扰动力矩；δφ，δψ，δγ为加速度计、磁力计和陀螺仪角度的估计误差.

对融合Kalman方程进行扩展，构建最优线性递推滤波方差：

(8)

从式(8)得知，融合滤波后的最优姿态角具有动态加速稳定性，可将稳定性特征描述为

(9)

式中：b>0；φa为两时刻角度估计的差值；u为姿态角的输入；fd为外加的白噪声序列.

在扩展Kalman滤波模型中，当前的飞行位姿状态变量和误差协方差估计可用以下方程计算：

(10)

(11)

M=Mn+ΔM,

(12)

(13)

在扩展Kalman滤波模型下，被控系统的参数自整定性预测方程可表示为

(14)

图2 模糊神经网络Fig.2 Fuzzy neural network system structure

2.2模糊神经网络控制律设计

将校正后的位姿参数输入模糊神经网络系统，实现无人机制导控制律的优化设计[9]，模糊神经网络如图2所示.

图2给出的神经网络为3层网络结构，各层权值的迭代方程为

(15)

输入层至隐含层的权重值为

(16)

且学习步长η满足

(17)

(18)

考虑位姿参数存在不确定性因素，对测量组合系统进行修正，将融合Kalman扩展滤波后的最优姿态角输入测量系统，建立一个线性离散随机过程[10]，得到飞行控制的最优导引律：

(19)

最后,进行控制律的稳定性证明，定义Lyapunov函数为

(20)

(21)

根据Lyapunove稳定性原理[11]，控制系统的稳定性得证.

3 仿真测试分析

为了测试本设计的应用性能，进行仿真实验分析，仿真平台为Matlab 7.飞行姿态参量数据来自于前期通过数字磁力计和数字加速度计实际采集的数据，位姿参数测量的灵敏度为8.44/15.30/60 mdps/digit，无人机飞行控制传感器设备的工作温度为-40～85 ℃，能有效满足各种飞行条件要求.设置飞行位姿参数的采样周期为0.02 s，Kalman滤波的阶数为4，滤波周期为0.02 s,中心频率取1 200 Hz，扰动干扰强度为0～10 dB，飞行的俯仰角和航向角初始值分别为Δ1=5°，Δ2=8°,转弯半径为120 rad，下降速度为10 m/s.根据上述仿真环境和参量设定，进行无人机飞行姿态校正和稳定性控制仿真分析.采用本设计的EKF融合滤波方法和传统的直接计算方法进行无人机飞行控制俯仰角、横滚角和航向角的测量，结果如图3所示.

图3 飞行位姿参数参量结果Fig.3 Parameters of flight attitude

分析图3得知，采用本方法进行扩展Kalman滤波实现飞行位姿参数的整定性融合处理和角度校正，角度的振荡和偏差更小，采样曲线更加平滑，说明本方法在提高飞行姿态参量稳定性和准确性方面具有明显的效果.

以图3所示的EKF方法采集的飞行位姿参量数据为研究对象进行误差分析，得到结果如图4所示.

分析图4得知，采用本方法能够有效降低俯仰角和横滚角的误差，对磁力计采集的航向角的误差控制却相对较弱.

采用不同算法进行无人机控制，得到姿态角校正的解算结果对比，如图5所示.

图4 姿态角与真值的偏差Fig.4 Deviation of attitude angle and true value

图5 无人机飞行姿态角解算结果Fig.5 UAV flight attitude angle calculation results

图5给出的飞行姿态角解算结果分别描述了静态直航飞行阶段和大迎角飞行阶段的位姿参量.其中，前后两段相对较平稳的部分表示静态直航，中间段姿态角剧烈振荡阶段表示大迎角飞行.根据图5的校正结果，在相同实验参数下，对不同算法进行位姿参数校正的最大误差、平均误差和均方差统计，结果见表1.

表1 不同算法位姿参数校正统计分析Tab.1 Statistical analysis of different algorithms for pose parameter correction (°)

分析表1得知，在静态直航段，3种算法的性能相当，几乎没有轨迹跟踪误差和漂移，都具有较好的控制性能.在大迎角飞行阶段，由于存在不确定性扰动影响，稳定性控制难度较高，本方法能通过扩展Kalman滤波和模糊神经网络控制，使飞行姿态角变得相当稳定，能在短时间内进行位姿调整，提高定姿的精度.位姿校正确保了飞行的稳定性，仿真结果验证了本方法的有效性和优越性.

4 结语

本研究提出了一种基于扩展Kalman滤波的位姿校正方法，进行无人机制导系统稳定性控制律设计，建立了飞行运动学数学模型，构建被控对象并进行控制约束参量分析，采用扩展Kalman滤波算法进行位姿参量融合校正，并通过模糊神经网络构建优化的控制律.研究得知，本设计能提高飞行姿态角的稳定性且定姿精度较高，特别是在大迎角飞行中，飞行稳定性控制得到了很好的展现.

[1] 刘炜,陆兴华.飞行失衡条件下的无人机惯导鲁棒性控制研究[J].计算机与数字工程,2016,44(12):2380-2385.

[2] 潘加亮,熊智,王丽娜,等.一种简化的发射系下SINS/GPS/CNS组合导航系统无迹Kalman滤波算法[J].兵工学报,2015,36(3):484-491.

[3] 王勋,张代兵,沈林成.一种基于虚拟力的无人机路径跟踪控制方法[J].机器人,2016,38(3):329-336.

[4] 邸斌,周锐,董卓宁.考虑信息成功传递概率的多无人机协同目标最优观测与跟踪[J].控制与决策,2016,31(4):616-622.

[5] 陆兴华.姿态融合滤波的无人机抗干扰控制算法[J].传感器与微系统,2016,35(7):116-119.

[6] 刘凯,朱纪洪,余波.推力矢量飞机纵向鲁棒动态逆控制[J].控制与决策,2013,28(7):1113-1116.

[7] LEE W,BANG H,LEEGHIM H.Cooperative localization between small UAVs using a combination of heterogeneous sensors[J].Aerospace Science and Technology,2013,27(1):105-111.

[8] EVANGELIO R H,PATZOLD M,KELLER I.Adaptively splitted GMM with feedback improvement for the task of background subtraction[J].IEEE Transactions on Information Forensics and Security,2014,9(5):863-874.

[9] ST-CHARLES P,BILODEAU G,BERGEVIN R.SuBSENSE:a universal change detection method with local adaptive sensitivity[J].IEEE Transactions on Image Processing,2015,24(1):359-373.

[10]HELMY A,HEDAYAT A,AL-DHAHIR N.Robust weighted sum-rate maximization for the multi-stream MIMO interference channel with sparse equalization[J].IEEE Transactions on Communications,2015,60(10):3645-3659.

[11]谭中权.离散与连续时间强相依高斯过程最大值与和的渐近关系[J].应用数学学报,2015,38(1):27-36.

ResearchontheattitudecorrectionofunmannedaerialvehiclebasedonextendedKalmanfilter

LIKe

(EngineeringTrainingCenter,HenanUniversityofEngineering,Zhengzhou451191,China)

UAV attitude control is the key technology for UAV flight stability. Considering the lower accuracy of the guidance controller attitude positioning of UAV in the posture parameters under uncertainty problems, an UAV attitude correction method is proposed based on extended Kalman filter, and the navigation system stability control law design method is presented. The establishment of UAV flight dynamics model is taken, the flight trajectory equation is constructed, the UAV guidance system with constraint parameter object is analyzed, the accelerometer, gyroscope and magnetometer are used to obtain the position parameter measurement considering the pose parameter uncertainty, the qualitative treatment of extended Kalman filter algorithm of attitude parameters is conducted, realizing the angle correction. The position and orientation parameters are input into the fuzzy neural network to realize the optimal design of the guidance control law. The simulation results show that the proposed method can be used in the flight guidance control of UAV, and the attitude accuracy is higher.

unmanned aerial vehicle; guidance system; stability control; extended Kalman filter

TP273

A

1674-330X(2017)03-0044-06

2017-02-09

河南工程学院校级质量工程项目(508905)

李可(1978-)，男，河南郑州人，实验师，主要从事自动化控制研究.