刘洪剑，王耀南*，谭建豪，李树帅，钟 杭

(1.湖南大学电气与信息工程学院，长沙410082;2.机器人视觉感知与控制技术国家工程实验室，长沙410082)

一种旋翼无人机组合导航系统设计及应用*

刘洪剑1，2，王耀南1，2*，谭建豪1，2，李树帅1，2，钟 杭1，2

(1.湖南大学电气与信息工程学院，长沙410082;2.机器人视觉感知与控制技术国家工程实验室，长沙410082)

针对传统组合导航算法相对复杂，对系统硬件要求太高，实时性不好的缺点，设计了一种基于线性卡尔曼滤波器的简单实用组合导航算法。考虑到传统卡尔曼滤波器在系统噪声变化时滤波精度变差甚至发散以及模糊控制器计算量大的缺陷，设计了线性迭代调整观测噪声参数的方法对滤波器进行自适应改进，提高了算法的鲁棒性和实时性。利用STM32微控制器和MEMS惯性单元以及UBLOX GPS定位模块搭建硬件平台进行实验验证。结果表明:在168 MHz时钟频率下，一次姿态数据读取和解算共耗时3.27 ms，一次组合导航滤波耗时2.18 ms，二者分别运行在100 Hz和5 Hz频率下。最终组合导航结果可以满足无人机在1 m的精度范围内自动悬停的需求，验证了所设计的组合导航算法的可靠性和实用性。

无人机;卡尔曼滤波;自适应;组合导航

如今旋翼无人机已经成为一个研究热点，位姿信息获取及位姿控制是无人机自主式飞行面临的基本问题。目前旋翼无人机的姿态解算及控制研究已经比较成熟，而导航控制问题仍处于研究阶段。相比于传统监测等飞行任务，本课题研究的面向任务的旋翼飞行机械臂自主作业与控制系统则需要更加准确的位置信息，以实现无人机悬停模式下准确的空中抓取作业［1］。

应用于旋翼无人机上集加速度计和陀螺仪于一体的微型惯性测量单元(MIMU)，不仅可以通过姿态解算得到姿态信息。还可以作为惯性导航系统(INS)，提供自主式无人机导航与制导所需的位置信息。但是其导航误差会随时间快速积累，不能单独用于长时间位置导航。在室外环境下，GPS能提供准确的位置信息，但是其测量误差不可避免，短时间测量精度较差。INS和GPS各有所长，优势互补，利用卡尔曼滤波器对INS与GPS数据进行融合，获得导航参数的最优估计值，可以得到高精度、高可靠的导航信息［2］。

目前对基于卡尔曼滤波器的组合导航算法的研究已有很多［3－4］。主要研究热点集中在针对系统模型非线性或系统噪声统计特性不明确引起的滤波器精度降低甚至发散这一问题的改进上。具体改进措施有:针对系统非线性模型的扩展卡尔曼滤波(EKF)算法［5－6］、粒子滤波(PF)算法［7－8］和无迹卡尔曼滤波(UKF)算法［9－11］等;针对系统噪声不确定性的自适应衰减记忆法卡尔曼滤波算法［12］、Sage-Husa自适应卡尔曼滤波算法［13－14］、模糊逻辑自适应卡尔曼滤波算法［15－16］等。

EKF算法将非线性函数直接忽略高阶项进行线性化，势必存在高阶项截断误差，而且雅可比矩阵求解的计算量比较大。PF算法大量粒子的随机产生，很难满足导航系统实时性需求。UKF算法对初始值的取值比较敏感，系统噪声不确定性会对滤波精度产生较大的影响;目前对于记忆衰减因子的选择没有完善的理论，只能根据经验进行确定。Sage-Husa自适应算法可以在一定程度上降低模型误差、提高滤波精度，但是计算量大，且对于阶次较高的系统可靠性不高。模糊逻辑的思想和其他自适应方法是一致的，都是用权值调整噪声参数。但是模糊逻辑将调整过程根据经验分为几个模糊空间，而其他方法可以在每一点上调整，所以模糊逻辑调整的精确性不如其他方法好。而且模糊控制作为一种人工智能技术，对系统硬件要求比较高。

以上方法虽在精度上都达到了较理想的效果，但是一个共同的缺点就是算法复杂导致实时性不理想，对系统硬件要求太高，严重限制了其在实际中的应用，大部分算法只能停留在仿真阶段。本文从工程应用角度出发，提出一种简洁有效的实时自适应卡尔曼滤波算法。首先设计INS/GPS组合导航系统模型框架，然后建立线性系统的离散卡尔曼滤波模型。使用线性迭代的方式代替模糊控制器来克服系统噪声的不确实性。最后设计实验平台对算法进行验证并得出结论。

1 组合导航系统模型设计

INS/GPS组合导航系统一般有两种信息融合方式:速度、位置组合(松耦合)和伪距、伪距率组合(紧耦合)，本文采用前一种方式，这种方法的测量模型是线性的，可以用标准卡尔曼滤波器进行信息的融合。

设计的INS/GPS组合导航系统框图如图1所示。惯性导航系统(INS)以牛顿力学为基础，首先由姿态解算得到姿态矩阵Tnb，建立起导航坐标系。然后利用加速度计来测量无人机的实时加速度，将实时加速度通过Tnb转换到导航坐标系中，经过一次积分运算得到实时速度，再一次积分运算得出实时位置。然后利用自适应卡尔曼滤波器结合GPS提供的速度和位置信息对上述预测值进行校正。这里采用输出校正法，利用估计值直接校正组合导航系统最终输出的速度和位置，工程上实现简单。

图1 组合导航系统框图

图1中INS为公共子系统，GPS为辅助子系统。两个子系统分别进行定位运算，然后将两者结果融合到一起。该算法的好处是:不需对子系统内部做任何改动，方便系统集成，比如在不方便使用GPS的情况下，可以使用其他定位系统如视觉传感器直接替换掉GPS模块，而程序不需做太大的改动。而且姿态解算是组合导航的基础，虽然两个过程同时进行，但是组合导航算法不会对姿态算法产生任何影响。

为了充分发挥惯性传感器更新速率快的优势，INS子系统运行在100 Hz的高频率上，而组合导航卡尔曼滤波器运行在5 Hz的GPS低定位频率上。当GPS出现故障时，系统可以直接输出INS子系统的定位结果，在短时间内可提供较准确的位置信息。

2 卡尔曼滤波器设计

步骤1 状态空间表达式的建立

旋翼无人机的运行状态可以用精简的6个状态变量来描述:3个速度分量和3个位置分量。为了简化计算量，本文选取状态向量为:

式中:vn，ve，vd分别表示北东地方向的速度分量，pn，pe，pd分别北东地方向的位置分量。则可以建立系统的状态方程:

式中:Tnb为已知的机体坐标系b到导航坐标系n的姿态矩阵:

dT为卡尔曼滤波周期，［dvx，dvy，dvz］T表示周期dT内无人机在机体坐标系b中测得的速度增量。由于惯导系统数据更新速率比GPS模块数据更新速率要快得多，而且在GPS数据更新时间间隔内，系统加速度不可能为一定值。为减小状态预测误差，本文采用在GPS更新周期内对加速度计多次采样求积分的方式得到速度增量:

式中:dt表示INS的更新周期。G表示导航坐标系n中的重力加速度值。过程噪声ωk=［ωdvx，ωdvy，ωdvz］T。

取GPS模块输出的速度和位置信息作为观测变量，则系统观测向量为:

观测方程为:

测量噪声υk=［υvn，υve，υvd，υpn，υpe，υpd］T。

到此，建立了系统的线性离散状态空间模型。

步骤2 滤波条件的建立

将式(2)写成线性离散状态方程的形式:

式中:“(－)”表示对应量的预测值，“(+)”表示对应量的估计值，下文表示方法类似。则系统一步状态转移矩阵和过程噪声分布矩阵分别为:

式中:I3×3表示3×3的单位矩阵，03×3表示3×3的0矩阵，下文表示方法类似。

将状态向量Xk代入式(6)观测方程，可知输出矩阵和观测噪声分布矩阵分别为:

卡尔曼滤波器是一个循环迭代过程，迭代需要一个初始条件。本文以无人机起飞点处作为位置坐标原点，为了让滤波器尽快收敛，此处初始条件设为:

为了简化模型，通常我们假设过程噪声ωk和观测噪声υk为零均值高斯白噪声，且其统计特性不随时间和环境而改变:

过程噪声协方差矩阵Qk和观测噪声协方差矩阵Rk分别代表了预测的不确定性和测量的不确定性。即使在上述假设成立的条件下，其值也是无法直接测量或者根据先验知识得到的，需要在实际应用时根据滤波效果不断调整得到一个合理值。根据本文调试经验，可初步设定:

步骤3 滤波器的运行

利用式(7)～式(17)，可以建立系统的卡尔曼滤波模型如下:

状态预测:

协方差预测:

卡尔曼增益:

状态修正:

协方差修正:

3 测量噪声自适应调整

对由式(2)和式(6)描述的系统，卡尔曼滤波器给出了最小协方差意义下状态向量的最优无偏估计。但是在实际中，过程噪声、测量噪声以及残差一般情况下不再是高斯白噪声［12，17］，此时用传统的卡尔曼滤波器很容易导致滤波发散，一旦产生发散则信息融合失败。

通常的解决方法是通过残差方差匹配技术，使滤波器根据环境的改变自适应调整统计参数，增强残差的理论协方差阵与实际协方差阵的一致性［17］。文献［14，17］介绍的方法都是通过模糊控制器来实现的。虽然仿真效果都不错，但是进行模糊化、模糊推理和去模糊化等复杂过程严重影响了实际应用时算法的实时性。所以本文设计如下的线性迭代自适应过程:

定义残差为:

残差的理论协方差阵计算公式［17］:

实际协方差阵计算公式:

式中:N为用来计算实际协方差阵窗口的大小，N取值过大会影响自适应调整的实时性，取值过小会降低Cr(k)的准确性，都不利于自适应性能的改善。根据调试结果，本系统取5＜N＜30较合适，本文取N=10进行实验。

从式(24)可以看出，改变观测噪声协方差阵Rk的大小，可以改变残差协方差的理论值Pr(k)。所以可以通过调整Rk来改变Pr(k)从而增强Pr(k)与Cr(k)的一致性。

用式(26)来表示理论值与实际值的一致程度:

设计Rk的线性迭代调整公式为:

式中:Ri(k)表示Rk主对角线上第i个元素。则根据上述分析可得如下关系:若DOMi(k)＞0，即理论值大于实际值，应减小Ri(k)，所以ΔRi＜0;若DOMi(k)=0，即理论值等于实际值，保持Ri(k)不变，所以ΔRi=0;若DOMi(k)＜0，即理论值小于实际值，应增大Ri(k)，所以ΔRi＞0。

|ΔRi|的大小可根据实际情况进行调整。取得太大，Rk的调整力度过大导致震荡;取得太小，Rk的自适应调整效果不明显，退化为常规卡尔曼滤波。一般可以参考式(17)，取Ri(k)×10－1数量级合适。如需进一步提高自适应性能，可以根据DOM(k)的大小，自动调整|ΔRi|，使两者成正向关关系，但是参数整定过程会变得繁琐。权衡利弊本文取:

进行参数自适应调整后，卡尔曼滤波器具体步骤需要进行修改，在上述滤波模型后面需加上如下步骤:① 计算式(20)时输出式(24)，得到Pr(k);②计算式(21)时输出式(23)，得到rk;③将rk代入式(25)，得到 Cr(k);④将Pr(k)和Cr(k)代入式(26)，得到DOM(k);⑤根据DOMi(k)求ΔRi;⑥将ΔRi代入式(27)，求得Rk作为下一次滤波迭代中式(20)的噪声参数。

图2 无人机实验平台

4 实验结果及分析

4.1 实验平台

图2为本文设计的无人机实验平台，轴距60 cm。飞行控制器采用STM32F405(Cortex－M4内核)为中央处理器，工作频率168 MHz。主要完成传感器数据采集、姿态解算、组合导航、实验数据传送、控制量输出等功能。惯性测量单元(IMU)MPU6050固连于载体上，该芯片内置一个三轴陀螺仪和一个三轴加速度计，实验使用±2 000(°)/s和±2 gn的量程，16 bit数字输出，采样数据最高输出速率可达1 kHz。GPS接收机使用UBLOX NEO－M8N定位模块，该模块定位精度PDOP数值能达到2.0 m左右。采用NMEA－0183协议通过串口输出GPS定位数据，并可通过UBX协议对模块进行配置。NMEA－0183协议利用ASCⅡ码传递GPS定位数据，一帧的最大长度可达600 byte左右，数据量比较大。为了数据传输的稳定性和快速性，本设计使用主控制器的DMA串口功能接收GPS数据。模块最高更新频率可达到10 Hz，但数据不够稳定，故本文使用5 Hz更新频率。经测试，一次姿态解算耗时约3.27 ms，一次组合导航滤波耗时2.18 ms，为留冗余，设计姿态解算频率为100 Hz。即每10 ms进行一次姿态解算和INS导航，每200 ms进行一次GPS数据读取和位置卡尔曼滤波计算。

4.2 实验测试与结果分析

使用搭建的硬件平台对算法进行实验验证，通过无线串口模块将数据实时传送到电脑端得到测试数据。实验结果如图3～图11所示。其中，图3～图8为静态滤波结果图，即电机静止状态下，手持无人机在水平面内快速移动时的位置滤波结果。图9～图11为动态滤波结果图，实验中给定无人机一个三维路径，在其自动飞行的过程中，实时传回滤波数据。

图3 静态东向位置残差曲线

图4 静态北向位置残差曲线

由图3和图4位置滤波的残差曲线可以看出，整个实验过程，残差一直保持着0均值白噪声特性不变。系统稳定后，残差最大值小于1.5 m。图5和图6分别表示东向和北向位置滤波的残差协方差曲线，实线表示实际值，虚线表示理论值。结果显示，测量噪声Rk的自适应调整，使残差协方差的理论值紧跟实际值变化，提高了滤波器在系统噪声统计特性改变时的滤波精度。图7和图8中实线和虚线分别表示静态位置滤波曲线的测量值和估计值，可以看出，滤波器对GPS的输出数据起到了良好的平滑和跟踪效果。为了更好地验证算法的实际应用效果，使用最优估计值对无人机进行定位实验。

图5 静态东向位置残差协方差曲线

图6 静态北向位置残差协方差曲线

图7 静态东向位置滤波曲线

图8 静态北向位置滤波曲线

图9和图10的实验结果证明，无人机可以平稳地执行起飞、按轨迹飞行、悬停和降落等任务，动态滤波残差在1.5 m以内。以图11所示的视角观察无人机的悬停效果和漂移范围，结果显示，在风力较小的环境下，无人机的空间位置漂移范围可在1 m以内。

图9 动态三维位置滤波曲线

图10 动态三维位置残差曲线

图11 无人机组合导航自动悬停实验

5 结束语

本文首先建立了组合导航系统模型，然后按照模型的思路，选取了精简的6个状态变量设计出系统线性离散卡尔曼滤波器。不仅避免了系统的非线性带来的问题，也减少了计算量，保证了算法的实时性要求。为克服系统噪声统计特性的不确定性给滤波器带来的负面影响，对系统测量噪声进行了自适应调整。使用线性迭代的方式代替模糊控制器，思路简单且易于实现。最后设计并制作了实验验证平台，结果证明，利用滤波器输出的最优位置估计信息，无人机可以实现长时间的稳定自动悬停，精度在1 m以内。说明本系统可以为无人机在室外环境下的自主作业与控制提供准确有效的位置信息。

［1］ 谭建豪，王耀南，王媛媛，等.旋翼飞行机器人研究进展［J］.控制理论与应用，2015，32(10):1278－1286.

［2］ 董绪荣，张守信，华仲春.GPS/INS组合导航定位及其应用［M］.北京:国防科技大学出版社，1998.

［3］ 秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航系统原理［M］.西安:西北工业大学出版社，1998.

［4］ 董健康，安东.自适应卡尔曼滤波在组合导航中的应用研究［J］.计算机技术与发展，2011，21(10):183－185.

［5］ 张品秀，黄操军，乔相伟.基于自适应扩展Kalman滤波的SINS/GPS深组合研究［J］.传感技术学报，2010，23(3):408－412.

［6］ Psiaki M L.The Super-Iterated Extended Kalman Filter［C］// AIAA Guidance，Navigation，and Control Conference，2004(8): AIAA－04－5418.

［7］ Rigatos G G.Nonlinear Kalman Filters and Particle Filters for integrated Navigation of Unmanned Aerial Vehicles［J］.Robotics and Autonomous Systems，2012，60(7):978－995.

［8］ 陈延军，梁新华，潘泉，等.两级量测更新的粒子滤波检测前跟踪算法［J］.传感技术学报，2015，28(1):115－120.

［9］ 杨波，秦永元，柴艳.UKF在INS/GPS直接法卡尔曼滤波中的应用［J］.传感技术学报，2007，20(4):842－846.

［11］欧县华，武宪青，何熊熊.基于AIUKF的WSN节点定位算法［J］.传感技术学报，2015，28(2):234－238.

［12］马忠孝，刘宗玉，陈明.基于模糊逻辑的自适应卡尔曼滤波在GPS/INS组合导航中的应用［J］.信息与控制，2006，35(4): 457－461.

［13］王冲，曾庆军.基于Sage-Husa自适应滤波算法的AUV组合导航系统设计［J］.电子设计工程，2013，21(15):75－77.

［14］张少军，王宏丽，蔡宗平.MIMU/GPS组合导航模糊自适应卡尔曼滤波研究［J］.电光与控制，2008，15(8):84－86.

［15］Sasiadek J Z，Wang Q，Zeremba M B.Fuzzy Adaptive Kalman Filtering For INS/GPS Data Fusion［C］//Proceedings of the 15th IEEE International Symposium on Intelligent Control.［s.l.］:［s.n.］.2000:181－186.

［16］段宇，徐晓苏.模糊逻辑自适应卡尔曼滤波技术在GPS/INS组合导航系统中的仿真研究［J］.传感技术学报，2003，16(2): 165－169.

［17］柏菁，刘建业，袁信.模糊自适应卡尔曼滤波技术研究［J］.信息与控制，2002，31(3):193－197.

刘洪剑(1991－)，男，湖南大学电气与信息工程学院硕士研究生，主要研究方向为无人机数据融合与控制，hnlhj @hnu.edu.cn;

王耀南(1957－)，男，湖南大学教授，博士生导师，主要研究方向为智能控制理论与智能信息处理，yaonan@hnu.edu.cn。

A Kind of Design and Application of Integrated Navigation Systems for UAVs*

LIU Hongjian1，2，WANG Yaonan1，2*，TAN Jianhao1，2，LI Shushuai1，2，ZHONG Hang1，2
(1.College of Electrical and Information Engineering，Hunan University，Changsha410082，China; 2.National Engineering Laboratory for Robot Visual Perception and Control Technology，Changsha410082，China)

Aiming at the disadvantages that the traditional integrated navigation algorithm has high complexity，high hardware requirements，and poor real-time performance，a simple and practical integrated navigation algorithm based on linear Kalman filter is designed.Considering the problems that the filtering accuracy will become terrible or even divergence when the system noise varies，and great computational burden of the fuzzy controller，a method to adjust observation noise parameters adaptively based on linear iteration is proposed to improve the adaptive performance and robustness of the filter.Then，the proposed method is verified in an experimental system that consists of the STM32 microcontroller，the MEMS inertial unit and the UBLOX GPS positioning module.The experimental results indicate that，the time of reading attitude data and attitude solution costs 3.27 ms in total running at 100 Hz，and the time of integrated navigation filtering costs 2.18 ms in total running at 5 Hz，both under 168 MHz clock frequency.The final integrated navigation results can meet the needs of the UAVs automatic hover in the precision of 1 m and verify the reliability and practicability of the designed integrated navigation algorithm.

UAV;Kalman filter;self-adaptive;integrated navigation

U666.11

A

1004－1699(2017)02－0331－06

C:6140B;6330;7220

10.3969/j.issn.1004－1699.2017.02.028

项目来源:国家自然科学基金重点项目(61433016);国家自然科学基金面上项目(61573134)

2016－07－03 修改日期:2016－09－06