基于加速度信息修正的四旋翼位置估计算法研究*

方家豪，叶 欣，董 伟，盛鑫军，朱向阳

（上海交通大学机械系统与振动国家重点实验室，上海200240）

基于加速度信息修正的四旋翼位置估计算法研究*

方家豪，叶 欣，董 伟，盛鑫军*，朱向阳

（上海交通大学机械系统与振动国家重点实验室，上海200240）

针对单一的微传感器无法准确进行四旋翼无人机空间定位的问题，设计了一种多元信息融合的互补滤波算法用于无人机空间位置估计。该算法的核心思想为利用一类通用的辅助传感器如气压传感器，全球定位系统（GPS）以及微基站的测量信息对加速度传感器的测量信息进行实时修正，然后利用修正后的加速度信息积分估计四旋翼无人机的空间位置。本文在自主研制的飞行控制平台上验证了这种互补滤波算法的有效性。通过对比实验验证，利用本文设计的互补滤波算法可以使得无人机运动速度估计值以及空间位置估计值无偏差的收敛。飞行实验证明，将该互补滤波算法输出的速度以及位置估计信息应用于位置控制器中，可以实现无人机稳定的位置控制。

多元信息融合；位置估计；互补滤波；加速度传感器；气压传感器；GPS

近年来随着各类廉价的高性能微控制器以及微传感器系统的兴起，四旋翼无人机UAV（Unmanned Aerial Vehicle）的研究也得到国内外研究机构的广泛重视［1］。考虑到四旋翼无人机的控制特点，为了实现无人机准确的位置控制需要实时地获得无人机当前的运动状态，其中包括了无人机当前的空间运动速度信息以及空间位置信息［2］。这两项信息的获得通常依赖于3类传感器：MEMS（Micro Electro-Mechanical System）加速度传感器，气压传感器以及GPS。然而这三类传感器信号都存在固有的缺陷，单一的传感器信号无法直接应用于无人机的位置控制［3］。因此需要通过合适的算法融合这三类传感器信号，达到互相补偿各自信号缺陷的目的。

近些年来，针对于这几类传感器信号的信息融合问题，国内外许多研究机构开展了大量的研究工作。在传感器信号分析方面，文献［4-5］中提到多种影响MEMS加速度传感器信号质量的因素，其中包括了传感器设计与加工工艺等内在因素以及传感器工作环境等外在因素。在文献［6］中提到，GPS传感器测量的位置信息存在时间的滞后，这种测量的滞后会影响到组合导航的精度。在空间位置估计算法方面，目前公开的算法主要是基于卡尔曼滤波的思想［7-9］。在文献［7］中使用了一种扩展卡尔曼滤波（EKF）算法用于惯性传感器与GPS传感器的信息融合。相比于使用单独的GPS测量值作为位置估计值，通过该EKF算法有效地减小了位置估计值的噪声。同时，在文献［8］中使用了一种SPKF（Sigma-Point Kalman Filters）算法，和EKF算法相比，该SPKF算法减少了30%的位置估计误差。在文献［9］中使用了一种UKF（Unscented Kalman Filter）算法，相比于EKF算法，该UKF算法的位置估计均方根（RMS）值更小，位置估计精度更高。

尽管上述的这些基于卡尔曼滤波的位置估计算法都在仿真环境中或者商业平台上验证了算法的有效性，但是这一类算法并不适用于廉价的微控制系统中。这些算法的运算量很大，对执行算法的控制器有很高的性能要求。在廉价的微控制系统中执行这类算法时，并不能达到文献中提及的更新频率，从而也无法保证位置估计的精度。因此，在工程应用中，这一类算法的效果有限。

考虑到实际的工程应用，本文使用了一种适用于廉价微控制器的互补滤波算法用于无人机空间位置的估计。相比于卡尔曼滤波算法，该互补滤波算法迭代简单，更适用于工程实践。该算法的核心思想为实时地获得加速度传感器测量误差的无偏估计值，从而对加速度传感器的测量值进行动态修正，最后利用修正后的加速度信息进行积分估计无人机的空间位置。这种算法具有很强的通用性，不仅仅适用于解决气压传感器以及GPS与加速度传感器的信息融合问题，同样也适用于其它具有类似信号特点的传感器与加速度传感器的信息融合。在本文的第五节中将讨论一种利用该互补滤波算法融合微基站与加速度传感器的信息进行无GPS环境下的无人机空间定位问题的解决方案。

本文的内容将以如下顺序介绍，第二节介绍了无人机的连体坐标系与惯性坐标系的定义。第三节介绍了本文设计的一款无人机飞行控制平台的硬件结构，同时对该平台连接的相关传感器信号特点进行了分析。第四节介绍了针对这些传感器信号特点所设计的一种用于空间位置估计的互补滤波算法。实验与结果在第五节中进行简要的讨论。最后一节是对本文所有内容的总结。

1 坐标系定义

本节主要介绍了四旋翼无人机的连体坐标系与惯性坐标系的定义以及坐标系的转化关系。如图1定义两个坐标系，坐标系{B}定义为无人机的连体坐标系。坐标系{G}定义为固连在地面上的惯性坐标系。惯性坐标系的原点O定义为无人机解锁时刻所处的空间位置。地理正东方向定义为惯性坐标系的X轴，地理正北方向定义为惯性坐标系的Y轴，竖直向上定义为惯性坐标系的Z轴。

图1 坐标系定义

无人机的三个姿态角定义如下，以绕着体坐标系X轴旋转定义为横滚角ϕ。以绕着体坐标系Y轴旋转定义为俯仰角θ。以绕着体坐标系Z轴旋转定义为偏航角ψ。据此得到从惯性坐标系到连体坐标系的旋转矩阵R

其中s为sin运算，c为cos运算。

2 传感器信号分析

本文自主研制了一款四旋翼飞行控制硬件平台。图2为该硬件平台以及其传感器的硬件连接关系框图，下文所有的实验都是基于这个硬件平台完成的。该硬件平台使用一款廉价的微处理器AT91SAM7S256芯片作为微控制单元（MCU）。这一型号的芯片不具备当前先进控制芯片常具有的硬件浮点数运算单元（FPU），也不支持数字信号处理（DSP）指令集。其较弱的运算处理能力提高了对算法简洁性的要求。

图2 四旋翼飞行控制器硬件系统

2.1 加速度传感器信号分析

主芯片通过集成电路总线接口（IIC）连接MPU9150型号的MEMS惯性传感器。加速度传感器测量信号的数学模型可以表达为

其中aIMU为加速度的测量值，R为旋转矩阵，a为加速度的真实值，g为重力加速度，ba为测量的静态偏差，ηa为测量的随机噪声。

图3为静止状态下，该加速度传感器分别在图1所示的体坐标系{B}的X，Y，Z三轴方向上测量得到的原始加速度数据。实验结果显示该加速度传感器的测量随机噪声的大小为0.05 m/s2。同时该加速度传感器信号测量的静态偏差因各个轴而异。在文献［10］中提到即使对加速度传感器进行合理的校正也无法完全消除测量的静态偏差。因此直接利用加速度传感器积分估计无人机的空间位置，估计值的误差将随着时间而逐步累积［11］。

图3 加速度传感器X，Y，Z三轴原始加速度数据

2.2 气压传感器信号分析

主控制芯片通过串行外设接口（SPI）连接MS5611型号的气压传感器。气压与相对高度的转换关系为

其中P为当前高度的气压测量值，RP为高度零点的气压值，H为相对高度值。气压传感器测量信号的数学模型表达为

其中Hbaro为当前气压传感器测量高度值，H为当前的真实高度，bbaro为气压传感器的时变漂移，ηbaro为随机测量噪声。如图4所示，该气压传感器的随机测量噪声为0.5 m。

图4 气压传感器测量压强及对应测量高度

表1 传感器信号特点

2.3 GPS信号分析

主控制芯片通过串口（USART）连接M8N型号的GPS模块，通过配置修改该模块的测量更新频率为5 Hz。GPS信号测量的数学模型可以表达为

其中PGPS(t)为当前时刻GPS传感器的测量值。假设GPS测量值存在一个dt大小的时间滞后［6］，所以将P(t-dt)定义为dt时刻之前无人机所处的真实位置。bGPS(t)为GPS传感器测量的时变漂移。

图5为静止状态下，在空旷的室外环境中该GPS模块测量得到的当前无人机所处位置的原始经纬度数据。将该经纬度测量数据投影到X-Y平面坐标系中得到如图6所示的位置分布。投影后的位置数据显示该GPS模块在X轴与Y轴方向上的测量精度为2 m。同时由于无线通信以及测量与计算的时间要求，GPS模块输出的测量信息通常存在时间滞后。

最后，表1总结了这3类传感器的信号的特点。

图5 静止状态下GPS模块测量经纬度数据

图6 X-Y平面投影图

3 多元信息融合算法

根据上一小节中所述的三类传感器信号的特点，本小节设计了一种基于互补滤波器的多元信息融合算法用于估计四旋翼无人机的相对高度以及水平位置。

3.1 高度估计算法

在惯性坐标系的Z轴方向上，采用融合气压传感器以及加速度传感器的数据进行相对高度的估计。图7为该方向上传感器数据融合算法框图。

图7 高度方向传感器数据融合算法框图

图7所示的高度估计算法可以描述为：

加速度传感器以500 Hz的频率更新得到运动加速度测量值au，减去加速度的静态误差估计值，得到修正后的运动加速度值。修正后的运动加速度值积分得到速度的预测值，减去当前速度的比例修正值，得到修正后的速度估计值。积分得到高度的预测值，减去当前高度的比例修正值，得到修正后的高度估计值。气压传感器更新测量得到测量高度值z，更新下一时刻速度的比例修正值，高度的比例修正值。同时利用积分环节计算下一时刻加速度的静态误差值修正量，积分更新下一时刻的加速度静态误差。该算法数学表达式可以表达为：

其中Kp，Kvp，Ki为比例以及积分修正系数值，修正系数在实验调试过程中调整至最佳值，参数选择依据参考第4.1小节。

3.2 水平位置估计算法

GPS/MEMS的传感器组合导航是四旋翼无人机获得位置信息的一种重要方法［12］。因此，本文采用融合GPS以及MEMS加速度传感器的信号进行水平位置的估计。图8为在惯性坐标系的X轴与Y轴方向上的信息融合算法框图，X轴与Y轴方向上算法一致。

图8 水平方向传感器数据融合算法框图

图8所示的水平位置估计算法可以描述为：

加速度传感器更新测量得到运动加速度测量值au，减去

其中Kpr，Kpv与Kir，Kiv分别为比例以及积分修正系数，修正系数在实验调试过程中调整至最佳值，参数选择依据参考第4.1小节。

4 实验与结果

本节首先进行一组对比实验，比较无人机在是否执行上述互补滤波算法的不同情况下的位置估计值。然后，为了证明上述互补滤波算法的有效性，本节将讨论利用上述互补滤波算法进行室外飞行测试的结果。最后，为了证明该算法的通用性，在本节的末尾将讨论一种利用上述互补滤波算法融合微基站与加速度传感器的信息进行无GPS环境下无人机定位问题的解决方案。

4.1 算法验证

为了验证上述算法的有效性，本文设计了一个对比实验比较3种不同的高度预测算法。在该对比实验中，无人机都处于静止状态下。算法1仅仅依靠加速度传感器进行积分预测高度。算法2在算法1的基础上添加了利用气压传感器对高度预测值的比例修正。算法3为本文所采用的互补滤波算法，在算法2的基础上添加了利用气压传感器对高度预测值的积分修正。在算法3中为了获得最优的位置以及速度估计值，需要对比例参数以及积分参数值进行调整。参数的调整依据为：最优参数应使得位置估计值以及速度估计值无偏差的收敛，同时应避免引入超调和震荡。上述对比实验的实验结果如图9、图10所示。

图9 3种算法得到的高度估计值

图10 3种算法得到的运动速度估计值

上述实验结果证明，由于加速度传感器数据存在测量的静态误差，在仅仅利用加速度传感器进行积分无修正的情况下，位置估计值以及速度估计值将随着时间而逐步发散。在引入了气压传感器数据的比例修正项之后估计值逐步收敛，但是存在明显的稳态误差，因此还需要引入积分修正项，积分修正项的作用为消除这个稳态误差。在使用积分环节后，随着时间的增加，误差的累积项会增大，从而可以推动对加速度测量误差的进一步修正，进而通过对修正后的加速度进行积分，可以消除估计值的稳态误差。

4.2 飞行测试（Flight Test）

由于准确的位置估计是实现无人机位置控制的基础［2］，所以本文采用飞行控制实验验证该空间位置预测算法的有效性。本文利用自主研制的控制器搭建了如图11的测试平台，该测试平台总重量 0.905 kg。控制算法采用常用的一组串级PID控制器。其中包含了一个位置控制器，一个速度控制器，一个角度控制器以及一个角速度控制器［13］。

图11 飞行测试平台

为了验证该位置估计算法的实时性与有效性，在室外环境中分别进行高度控制以及位置控制飞行测试。测试实验的结果如图 12～图 14所示。

图12 目标高度追踪效果

如图12所示，实线为利用本文的高度估计算法融合了加速度传感器以及气压传感器数据估计到的无人机当前所处高度，虚线为目标高度。实验结果证明，基于本文的高度估计算法，利用绝对精度为0.5 m气压传感器，可以实现四旋翼飞行器在高度方向上0.2 m的控制精度。

图13 X方向目标位置追踪效果

图14 Y方向目标位置追踪效果

如图13、图14所示，实线为基于上述的水平位置估计算法分别在惯性坐标系的X轴以及Y轴方向上估计到的飞行器的水平位置，虚线为在这两个方向上的目标位置。实验结果证明，基于本文的多元信息融合的水平位置估计算法，利用绝对精度为2 m的GPS传感器，可以实现四旋翼飞行器在水平方向上1 m的控制精度。

4.3 微基站系统

在无人机的众多应用场景中不可避免会遇到在无GPS环境下如何进行空间定位的问题。如前文所述，仅仅依靠加速度传感器估计的位置信息是不可信的，因此在无GPS的环境下也需要一类辅助的定位传感器。本文提出的一种解决方案为使用一类微基站定位系统作为辅助的定位系统。如图15、图16所示，该微基站系统由两部分组成，第一部分为安装在地面的基站系统，采用4个基站为一组安装在一个矩形区域的四个顶点位置。第二部分为无人机机载的定位标签，该定位标签可以通过与地面基站系统的通信计算出当前标签所处的空间位置。同时，该机载标签通过串口与无人机的控制器进行定位数据的通信。

图15 地面基站

图16 机载标签

图17 微基站系统X-Y平面定位信息

如图17所示，在实际测试中发现，该微基站系统输出的定位信息存在一定的测量噪声，在X与Y轴上的定位精度分别为0.5 m。这样的定位信息无法直接应用于无人机的位置控制器。然而，由于微基站的定位信息与GPS的定位信息有许多共性，因此可以利用本文的互补滤波器融合微基站与加速度传感器的信息进行无GPS环境下的无人机空间定位。

如图18、图19所示，分别为X与Y方向上原始的微基站测量数据与利用本文的互补滤波算法输出的滤波数据。实验结果显示，利用本文的互补滤波算法有效的抑制了原始测量数据中的高频噪声。

图18 微基站X方向测量数据与滤波数据

图19 微基站Y方向测量数据与滤波数据

5 结论

根据四旋翼无人机位置控制器的要求，本文设计了一种适用于廉价微控制器的互补滤波算法用于无人机运动速度以及空间位置的估计。为了验证该算法的有效性，本文以一块不具备FPU单元，不支持DSP指令集的廉价微控制芯片作为主控制单元，自主研制了一款四旋翼飞行控制平台。通过对该平台所连接的传感器信号特点的分析，本文设计了一种简单有效的互补滤波算法用于融合这些传感器的信号。通过一系列的实验验证，本文得出了如下结论：

①通过比较不同的位置估计算法证明，利用本文所设计的互补滤波算法可以实时修正加速度传感器的测量误差，从而使得空间位置估计值无偏差的收敛。

②由于准确的位置估计是实现无人机位置控制的基础，所以本文采用室外飞行控制实验证明该位置估计算法的有效性。实验结果显示采用本文的互补滤波算法，使用垂直方向上0.5 m测量精度的廉价气压传感器以及水平方向上2 m测量精度的民用GPS，可以实现无人机垂直方向上0.2 m，水平方向上1 m的控制精度。

③此类互补滤波算法具有很强的通用性，不仅仅适用于解决气压传感器以及GPS与加速度传感器的信息融合问题，也适用于其它具有类似信号特点的传感器与加速度传感器融合的组合导航问题。在未来的工程应用中，该互补滤波算法可以进一步扩展应用于微基站系统与加速度传感器的组合定位问题，从而可以解决无GPS环境下的无人机空间定位问题。

［1］陈海滨，殳国华.四旋翼飞行器的设计［J］.实验室研究与探索，2013，03：41-44.

［2］Mahony R，Kumar V，Corke P.Multirotor Aerial Vehicles：Model⁃ing，Estimation，and Control of Quadrotor［J］.IEEE Robotics& Automation Magazine，2012，19（3）：20-32.

［3］Tailanian M，Paternain S，Rosa R，et al.Design and Implemen⁃tation of Sensor Data Fusion for an Autonomous Quadrotor［C］//Instrumentation and Measurement Technology Conference（I2MTC）Proceedings，2014 IEEE International.IEEE，2014：1431-1436.

［4］李荣冰，刘建业，段方，等.MEMS-INS微型飞行器姿态确定系统的实现研究［J］.应用科学学报，2006（6）：618-622.

［5］于婷，孙伟，文剑.MEMS惯性组件的误差特性分析与标定［J］.传感技术学报，2016，29（6）：859-864.

［6］杨波，王跃钢，徐洪涛.高速飞行环境下卫星导航信息滞后补偿方法［J］.中国惯性技术学报，2011，19（3）：293-297.

［7］Mammarella M，Campa G，Napolitano M R，et al.Machine Vision/GPS Integration Using EKF for the UAV Aerial Refueling Problem［J］.Systems，Man，and Cybernetics，Part C：Applications and Re⁃views，IEEE Transactions on，2008，38（6）：791-801.

［8］Van Der Merwe R，Wan E A.Sigma-Point Kalman Filters for Inte⁃grated Navigation［C］//Proceedings of the 60th Annual Meeting of the Institute of Navigation（ION）.2004：641-654.

［9］Oh S M，Johnson E N.Development of UAV Navigation System Based on Unscented Kalman Filter［C］//AIAA Guidance，Naviga⁃tion，and Control Conference and Exhibit.Paper Number AIAA-2006-6763，Keystone，Colorado.2006.

［10］Lim C H，Lim T S，Koo V C.A MEMS Based，Low Cost GPS-Aid⁃ed INS for UAV Motion Sensing［C］//Advanced Intelligent Mecha⁃tronics（AIM），2014 IEEE/ASME International Conference on.IEEE，2014：576-581.

［11］李耀军，潘泉，赵春晖，等.基于INS/SMNS紧耦合的无人机导航［J］.中国惯性技术学报，2010（3）：302-306.

［12］蔡磊，牛小骥，张提升，等.MEMS惯导在旋翼飞行器中导航性能的实验分析［J］.传感技术学报，2016，29（5）：711-715.

［13］Dong W，Gu G Y，Zhu X，et al.Modeling and Control of a Quadro⁃tor UAV with Aerodynamic Concepts［J］.World Academy of Sci⁃ence，Engineering and Technology，2013，7：377-382.

方家豪（1992-），男，浙江省衢州人。现为上海交通大学机械与动力工程学院硕士研究生。主要从事四旋翼无人机飞行控制系统开发相关的研究，fangjia⁃hao@sjtu.edu.cn；

盛鑫军（1978-），男，江苏省江阴人。分别于2000年，2003年，2014年于上海交通大学获得学士学位，硕士学位以及博士学位。现为上海交通大学机器人研究所副研究员，长期从事无人机，智能机器人与生机电一体化的研究，xjsh⁃eng@sjtu.edu.c

朱向阳（1966-），男，江苏省扬州人。1992年于东南大学获得控制理论与应用专业博士学位。2002年起担任上海交通大学机械与动力工程学院教授，博士生导师。现为上海交通大学机器人研究所所长，长期从事机器人运动规划，人机接口以及生机电一体化方向的研究，mexyzhu@sjtu.edu.cn。

Quadrotor Position Estimation Algorithm Research Based on the Acceleration Correction*

FANG Jiahao，YE Xin，DONG Wei，SHENG Xinjun*，ZHU Xiangyang
（State Key Laboratory of Mechanical System and Vibration，Shanghai Jiaotong University，Shanghai 200240，China）

Single sensor couldn't provide accurate position estimation for the quadrotor.To solve this problem，a complementary filter based on multisensory data fusion is proposed to precisely estimate the position of the quadro⁃tor.The key point of this algorithm is to correct acceleration measurement with a class of auxiliary sensors such as the barometer，GPS and the micro base station.The complementary filter is validated by experiments on a quadrotor control platform.The result shows that，with this complementary filter，the speed estimation and the position estima⁃tion of the quadrotor is able to converge without steady error.Finally，outdoor flight test is carried out to prove that，with the position and speed estimation provided by the complementary filter，the quadrotor is capable of navigating through user-defined way-points.

multisensory data fusion；position estimation；complementary filter；accelerometer；barometer；GPS

TP212.9

A

1004-1699（2016）11-1684-08

EEACC：7230 10.3969/j.issn.1004-1699.2016.11.010

项目来源：上海张江国家自主创新示范区专项发展资金重点项目（201411-PD-JQ-B108-009）

2016-05-12 修改日期：2016-07-07