03 四旋翼无人机自主避障技术的研究---基于数据融合的定高于姿态控制

1. 引言

避障方案设计中，我们期望无人机从起始点飞到目标点，就要不断通过各种传感器获取无人机当下的位置坐标，并根据无人机的位置调整无人机的姿态，最终到达目的地。四旋翼无人机飞行时会有六个自由度，性能灵活，移动迅速，路径中的障碍物也是来自于四面八方，不仅仅局限于正前方，所以增加了无人机避障过程中检测障碍物以及规划安全路径的难度，为方便实验验证算法，减少障碍物检测方向，本文计划采取四旋翼无人机定高控制下的避障飞行实验，即四旋翼无人机在期望高度下飞行，通过前置检测装置检测障碍物，利用算法实现躲避四旋翼无人机前方的障碍物，以此将三维空间中的避障转化为二维平面中的避障飞行，本章主要分析四旋翼无人机的高度解算以及姿态解算，然后利用PID控制方法简历四旋翼无人机的控制器。

定高飞行指无人机在不接受遥控器飞行指令的情况下，飞控板会自动控制无人机的友们，从而保持无人机飞行高度不变，无人机所受升力等于自身重力，定高模式下，遥控器油门输入不再控制无人机的高度，但是仍然可以控制无人机的俯仰、偏航、横滚运动，即无人机会在期望高度平面自由运动，无人机常用的几种高度信息整理如下：

绝对高度：当期位置于平均海平面的垂直距离，也叫做海拔高度。

相对高度：指两个测量地之间的绝对高度之差。

真实高度：无人机飞行过程中，飞控距离地面的实际高度即为真实高度，又称几何高度。

2. 基于互补滤波的信息融合

关于四旋翼无人机的高度以及姿态解算，需要用到数据融合，数据融合也成为信息融合，是将来自多个传感器信息进行处理，从而得出更为全面、可靠的结论，本节采用互补滤波器进行数据融合，将多传感器信息融合解算得到高度以及姿态信息，互补滤波法要求融合的信号的干扰噪声处在不同的频率，通过设置两个滤波器的截止频率，确保融合后的信号能够覆盖需求频率，通过预测---矫正融合两种信息来源，一般是预测其中一种信息，然后利用另外一种信息进行校正。

2.1 基于互补滤波的高度解算

定高控制需要获取无人机的高度信息，绝大多数情况下，飞控的高度信息是由飞控内部的气压计来提供的，气压计测量的是绝对高度，利用大气气压伴随高度的增加而降低的原理测量，测量公式：

所以气压计高度测量可以表示为：

即气压计所测高度等于实际高度加上测量误差高度。

实际飞控板内计算气压计数据时，会采集多次数据求均值然后进行计算，但是单一的传感器所提供的信息似乎不能够满足实际飞行的要求，而且气压计有其难以忽视的缺陷：

（1）气压计测量时，噪声干扰很大，数据不够平滑；

（2）气压计所测数据会存在漂移现象；

（3）经实验证明，气压计测量受温度以及气流干扰严重，低温、强气流环境下，气压计均无法测得准确数值。

加速度计也可以获取飞控的位置信息，飞控通过加速度计获取到当前的加速度以后，通过积分得到垂直速度信息，再积分即可获取高度信息，如下：

但是加速度计同样存在固有的缺陷问题，多次积分会使结果产生累积误差，且加速度计的瞬时测量值误差会比较大。

显然，无法单独依靠气压计或者加速度计提供准确的高度信息反馈到实际地控制中，考虑通过其他传感器与气压计的数据进行数据融合处理，以期望得到良好精确的高度信息。

互补滤波算法是通过将气压计于加速度计测量得到的高度信息按照权重进行融合，以此为基础结算高度信息，采用高通滤波器处理加速度细心，低通滤波器处理气压计信息，其中加速度计可以获取飞控的垂直方向上的加速度，经过积分可以化的垂直方向的速度信息，整个算法的核心思想是由地理坐标系下的加速度通过积分，来获得速度、位置信息；经过2次修正陈尚可利用的信息，第一次是李忠传感器计算修正系数产生加速度的偏差修正加速度，第二次是利用修正系数修正位置；最后可利用速度经过加速度修正，可利用的位置经过了加速度和位置修正，加速度的修正过程是由机体测量的加速度通过减去偏差，再转换到地理坐标系。

气压计主要的作用就是计算一个校正系数来对加速度偏移量进行校正。数据融合过程如图所示：

加速度计测量的是无人机的加速度，测量值是机体坐标系下的，所以需将加速度值利用旋转矩阵转换为地面参考坐标系下的加速度。具体融合信息的实现过程如下：

（2）将加速度计测量的加速度通过旋转矩阵转换到地面参考坐标系下，转换之前注意需要先去除加速度计的偏移量，因为地理坐标系下 z 轴加速度包含重力加速度，所以需要将重力加速度补偿上去；

（3）计算气压计的校正系数，这个系数也就是需要用来校正加速度计的系数，具体公式为

（4）利用所求的气压计校正系数计算加速度计的偏移向量。 

（5）将加速度偏移向量转换回机体坐标系，将转换后的加速度积分，得到融合后的速度信息，再对速度信息积分，即可得到最终的高度估计值，最后将气压计矫正系数二次校正。

采集飞行数据并通过 Matlab 软件仿真以后的结果如图所示，可见融合以后的高 度较加速度计以及气压计单独测的高度准确。

2.2 基于互补滤波的姿态解算

从飞行原理可以看出，无人机飞行过程中，最终的控制要回到姿态控制上面，通过具体的欧拉角度调整，从而控制无人机的飞行姿态。要完成无人机的e姿态控制，就需要采集到无人机当前的姿态，然后经过控制算法，将无人机当前姿态调整到期望的姿态，姿态采集主要依靠飞控的惯性测量单元IMU，姿态解算精确与否直接关联到无人机飞行位置精确与否。

飞行过程中，陀螺仪测量无人机的角速度，具有高动态性能，将角速度对时间积分可以得到三个欧拉角角度，陀螺仪数据在积分过程中，会形成累计误差，累计误差随着时间增加不断变大，所以短时间内陀螺仪测量值比较可靠。磁力计主要测量当前的磁场分布，即无人机与磁场之间的角度，这个角度即为偏航角，但是磁力计受周围磁场干扰严重，实际测量中误差较大。加速度计之前已经介绍过，不再赘述。

三种传感器再频域上特性互补，所以本文考虑采用互补滤波融合这三种传感器的数据，实际是利用加速度计与磁力计融合后补偿陀螺仪所测的姿态信息，提高测量精度和系统的动态性能。

三种传感器的数据融合过程如图所示，陀螺仪经过高通滤波器，消除低频噪声，加速度计与磁力计经过低通滤波器，消除高频噪声。

利用旋转矩阵将三个传感器所测量的欧拉角转换为四元数形式，然后计算磁场的参考方向

计算重力分v与磁场分量w：

利用加速度，磁力计的值与重力分量，磁场分量求取误差：

利用比例-积分处理上步所求误差，然后利用所求的值补偿陀螺仪产生的零漂现象， 最终结算得到当前姿态信息。

采集飞行数据并通过滤波以后的结果如图 5.4 俯仰角，图 5.5 滚转角，图 5.6 偏航角。

3. PID控制器设计

无人机定高飞行主要分两种情况，一种是手动控制定高模式，此种模式下，无人机飞控仍然接收并执行遥控器指令信号，另一种是无人机自主飞行时，如航点飞行或者 offboard 模式等，设定无人机在一定高度下执行预设飞行任务，而不依靠遥控器信号指 令控制自身运动，而本文研究的是第二种定高模式。

在位置控制的背景下，本文中串级双环 PID 控制系统专为实现避障系统而设计，保证四旋翼无人机可以准确的到达目标位置，并且在悬停时保持四旋翼的稳定性。整个双回路控制系统分为内环控制（姿态控制）和外环控制（位置控制）两部分，其中外环控制中主要研究定高控制部分。

3.1 PID控制原理

PID 控制器是控制理论中最经典的控制算法，PID 算法简单，可靠性高，被广泛应用于过程控制与运动控制，PID 控制主要由比例，积分以及微分三个环节组成，通过这三个环节对输入值与输出值形成的差值分别做比例运算，积分运算和微分运算，将控制结果发送到被控对象以实现对系统的控制作用，闭环 PID 控制系统原理图如图所示。

PID 的三种环节中比例环节 P 的作用是直接将误差的比例作为输出，加快系统的响应速度，提高系统调节精度，但是较大的比例作用会使对象的输出产生较大波动，太小的比例作用会使对象的输出变换缓慢。积分用于将之前的误差值与时间的比例累加起来作为输出，积分环节 I 主要用于消除对象输出稳定时的稳态误差，但是会存在积分饱和情况。微分环节 D 将误差随时间的变化的斜率以比例的形式输出，改善系统的动态性能， 主要用于缩短对象的上升时间，加快响应速度，达到超前调节的作用。使用 PID 控制器的过程中，既可以使用 PID 控制，也可以单独使用 P、PI、PD 等控制，使用的过程即

3.2 串级PID控制器设计

本文把避障研究简化到二维平面以后，整体的位置控制就被分为了两部分：定高控制与平面位置控制。其中平面的位置控制即由机载设备发送平面位置，然后由飞控执行。

（1）高度控制器

因为高度信息是三维位置的垂直方向信息，所以在实际的飞控控制的无人机飞行的过程中，高度控制属于位置控制的一部分，其中关于高度控制器的流程图可以总结为下图所示。

（2）姿态控制器

4. 本文小结

本文主要介绍避障过程中相关姿态以及位置高度的控制设计过程，要想控制效果好，首先解算要准确，再飞控资深所带传感器具有固有缺陷的前提下，通过互补滤波算法融合传感器数据，通过融合加速度计与气压结算无人机实际高度，融合加速度计、磁力计与陀螺仪数据解算无人机当前姿态信息；最后利用PID控制算法，设计了串级PID高度控制器与串级PID姿态控制器。

简介

随着科技的进步，导航的方案也层出不穷，尤其是SLAM技术极大的促进了导航方案在机器人、无人驾驶等领域的发展，绝大多数导航方案都会使用惯性测量单元（IMU）来融合其他传感器来实现更加精确的导航。然而在体积、成本、性能、功耗等诸多因素的限制下，如何选择一款适合客户应用的姿态传感器便成了客户最大的难点。

HI2XX系列姿态模块是综合了体积、成本、性能、功耗等诸多因素后推出的工业级姿态传感器，这篇文章旨在帮助设计人员理解IMU的基础知识及相关应用，并为用户提供选择方案。

术语

IMU惯性测量单元(Inertial Measurement Unit) 是测量物体三轴角速度和加速度的设备。一个IMU内可能会装有三轴陀螺仪和三轴加速度计，来测量物体在三维空间中的角速度和加速度。严格意义上的IMU只为用户提供三轴角速度以及三轴加速度数据。

VRU 垂直参考单元(Vertical Reference Unit)是在IMU的基础上，以重力向量作为参考，用卡尔曼或者互补滤波等算法为用户提供有重力向量参考的俯仰角、横滚角以及无参考标准的航向角。通常所说的6轴姿态模块就属于这类系统。航向角没有参考，不管模块朝向哪里，启动后航向角都为0°(或一个设定的常数)。随着模块工作时间增加，航向角会缓慢累计误差。俯仰角，横滚角由于有重力向量参考，低机动运动情况下，长时间不会有累积误差。

AHRS 航姿参考系统(Attitude and Heading Reference System)AHRS系统是在VRU的基础上增加了磁力计或光流传感器，用卡尔曼或者互补滤波等算法为用户提供拥有绝对参考的俯仰角、横滚角以及航向角的设备，这类系统用来为飞行器提供准确可靠的姿态与航行信息。我们通常所说的9轴姿态传感器就属于这类系统，因为航向角有地磁场的参考，所以不会漂移。但地磁场很微弱，经常受到周围带磁物体的干扰，所以如何在高机动情况下抵抗各种磁干扰成为AHRS研究的热门。

GNSS/INS 组合导航系统，顾名思义这种系统是利用全球卫星导航系统(Global Navigation Satellite System 简称GNSS，它是GPS，北斗，GLONASS、GALILEO等系统的统称) 与惯性导航(Inertial Navigation System)各自的优势进行算法融合，为用户提供更加精准的姿态及位置信息。

下图是组合导航系统的一个基本的框图，它以加速度计、陀螺仪、磁力计、气压计、GNSS等作为基本输入，利用融合算法输出用户所需要的姿态信息、位置信息以及速度信息。

HI2XX系列传感器包含HI226和HI229，其中HI226是VRU，HI229是AHRS。它们都是工业级姿态传感器。

关于几轴

我们生活在三维世界，人们理所当然的认为只有三个轴。那么为什么会出来6轴，9轴以及10轴？在这里我们不能简单地把6轴，9轴向物理世界对应: 它实际的意思是表示N种测量值。一个典型的姿态测量系统可以测得加速度和角速度，总共是6维向量，这是我们通常所说的6轴IMU，除此之外系统还可能包括三维的地磁场以及一维的气压计，就成了我们通常称的9轴和10轴系统。

自由度(Degree of Freedom, DoF) : 6DoF，9DoF也是经常听到的说法。空间中的刚体只有平移和旋转。其中平移三个自由度，旋转三个自由度，因此用9Dof与10Dof来描述姿态系统实际上是不恰当的，但是目前国内有一些厂商依然会用9DoF，10DoF来描述系统，我们理解就可以，不必过于深究。

HI226与HI229分别属于6轴姿态传感器与9轴姿态传感器。

下表为HI2XX系列的产品特性总结：

IMU性能指标及误差源

量程(Range) 它指的是IMU可以测量的加速度角速度的范围，意味着只要传感器运动范围如果不超过IMU的量程，那么便可以提供准确的数据，通常角速度的单位是 °/s，加速度单位是g。正常情况下, 机器人和无人驾驶角速度一般不会超过200 °/s，加速度不会超过4g，但是大量程可以在产品使用的过程中经受偶发性冲击，鲁棒性好。

随机游走(Random Walk) IMU系统通过对角速度与角加速度积分来获得姿态角与速度，但是原始测量值中含有噪声，这些噪声被积分后便形成了随机游走，并随着时间的平方根变化而变化。角度随机游走（ARW）的单位是°/sqrt(hr),速度随机游走噪声（VRW）的单位是m/s/ sqrt(hr)。

随机游走(Random Walk) 它体现了在输出带宽内，频率对噪声的影响。角速度的噪声频谱密度单位是°/s/√Hz，加速度的噪声频谱密度是g/√Hz。

零偏(Bias) 当IMU保持静止时, 它依然会有一个很小输出，这个输出的数值就是零偏。它会受到IMU的上电状态、温度、内部结构等因素影响，比如陀螺仪理论上静止时应该是0°/s，实际上陀螺仪静止的输出是一个均值不为零的噪声。

零偏稳定性(Bias Stability/In-run bias ) 这是评价低成本IMU非常重要的性能指标，它可以被定义为IMU相对于其输出速率平均值的偏差或漂移量。陀螺仪零偏稳定性的单位是°/h，加速度计的零偏稳定性单位是g/h。

比例因子(Scale Factor) 比例因子描述了输入与输出的相关性，比如载体实际旋转的角速度是100 °/s，但是陀螺仪输出的确是98°/s，真实值和测量值之间受到了比例的影响，可以被描述为下公式：

y=SF(x)+b+v

其中y为传输器输出，x为真实值，SF为比例因子，b为零篇，v为传感器噪声。

非线性度(Nonlinearity) 也叫做比例因子的高次性。比例因子实际上不是常量。它本身还会随着加速度或角速度的变化而变化(一般变化程度很小)，非线性度越小越好。

非正交性(Misalignment) 在理想的情况下，坐标系的轴与轴之间是绝对正交的，但是现实情况下IMU的坐标轴之间却不是完全正交的。这个指标会对剧烈无规则高机动运动产生很大的影响。

加计敏感度(G- Sensitivity) 陀螺仪是感应角速度的器件，但也会受到加速度的影响。这是微机电陀螺仪最常见的现象。

并不是上述所有误差源都会对系统造成很严重的影响，通常每种应用对应着不同的典型工作环境。但是了解哪些误差源对系统的影响大才有可能在系统设计阶段尽量减小误差。一般来讲，零偏，比例因子，非正交性可以通过出厂前校准得到改善。而零偏稳定性，噪声和非线性度对校准后的IMU使用影响比较大。除此之外，温度对系统影响也很大，但是鉴于高性价比的一般中低端IMU受温度影响的特性非常复杂，批量级温度补偿对于中低端工业级IMU来说还是一个挑战。影响IMU性能的主要因素如下图所示：

针对上述误差，我们专门为HI2XX系列定制了校准设备，出厂之前每一颗都经过了严格的校准，校准之后各方面指标如下。

精度指标如下表：

物理尺寸以及电气特性如下表所示：

应用场景

导航

很多应用场景都需要监测位置以及方向，比如无人驾驶汽车、移动机器人、无人船等，HI226最典型的应用就是移动机器人，比如扫地机器人，送餐机器人，大型商用机器人。性能优异的IMU有助于机器人用户降低激光雷达成本，缩短开发时间，下图是某机器人公司利用HI226以及低成本激光雷达做的导航方案效果图。

控制

很多应用场景会用到IMU的原始加速度与角速度数据，缓慢的航向漂移对这种应用不重要，可以接受一些噪声以及误差比如摄像机稳台、云台、关节的动作捕捉、体育训练等，这些领域对于IMU的要求必须是低成本的。HI226低延时与高速率特性可以很好的与这些场合相结合。

总结

首先你要知道四元数的解算过程（需要一定数学功底的），其实我推荐你也可以试试互补滤波算法，也是非常常见的算法，你要是要资料我这里很多，我就是做四轴飞行器的。如果你要简单一点，用MPU6050，他自带的DMP解算出的姿态我看过，效果挺好的。

另外看论文看英文的，中文很多实验数据有造假情况。不过一般985高校的研究生，博士生毕业论文还是可以看得

---