四旋翼无人机自主避障技术研究——基于数据融合的恒定高度姿态控制
在避障方案的设计中，我们期望无人机从起点飞到目标点，必须通过各种传感器不断获取无人机当前的位置坐标，并根据无人机的位置调整无人机的姿态，最终到达目的地。四旋翼无人机飞行时将有六个自由度，性能灵活，移动迅速。路径中的障碍物也来自四面八方，而不仅仅是前方，所以无人机在避障过程中检测障碍物和规划安全路径的难度更大。为了便于算法的实验验证，减少障碍物检测的方向性，本文拟采用四旋翼无人机恒高控制下的避障飞行实验，即四旋翼无人机在期望高度飞行。通过预探测装置探测障碍物，通过算法避开四旋翼无人机前方的障碍物，使三维空间的避障转化为二维平面的避障飞行。本章主要分析了四旋翼无人机的高度计算和姿态计算，然后通过PID控制方法恢复四旋翼无人机的控制器。
定高飞行是指当无人机不接受遥控器的飞行指令时，飞控板会自动控制无人机的朋友，使无人机的飞行高度保持不变，无人机的升力等于其自身重力。在固定高度模式下，遥控器的油门输入已经不能控制无人机的高度，但仍然可以控制无人机的俯仰、偏航和滚转运动，即没有人可以在期望的高度平面内自由移动。几种常用的无人机高度信息排列如下:
绝对高度:当前位置与平均海平面之间的垂直距离，也称为海拔高度。
相对高度:指两个测量地点的绝对高度之差。
真高度:无人机飞行过程中，飞控离地面的实际高度为真高度，也叫几何高度。
2.基于互补滤波的信息融合。
关于四旋翼无人机的高度和姿态计算，需要进行数据融合，也叫信息融合。它是对来自多个传感器的信息进行处理，从而得出更加全面可靠的结论。本节采用互补滤波器进行数据融合，多传感器信息融合计算得到高度和姿态信息。互补滤波要求融合信号的干扰噪声处于不同的频率。通过设置两个滤波器的截止频率，保证融合后的信号能够覆盖所需的频率，通过预测-校正融合两个信息源，通常是预测其中一个，然后用另一个信息进行校正。
2.1基于互补滤波的高度计算
高度控制需要获取无人机的高度信息。在大多数情况下，飞行控制的高度信息由飞行控制内部的气压计提供，气压计测量绝对高度。测量公式如下:
所以气压计高度的测量可以表示为:
即气压计测得的高度等于实际高度加上测量误差的高度。
在实际飞行控制板中计算气压计数据时，会多次采集数据进行平均后再进行计算，但单个传感器提供的信息似乎无法满足实际飞行的要求，气压计有其不可忽视的缺陷:
(1)气压计测量时，噪声干扰大，数据不够平滑；
(2)气压计测得的数据会漂移；
(3)实验表明气压计测量受温度和气流干扰严重，在低温强气流环境下气压计无法测得准确数值。
加速度计也可以获得飞行控制器的位置信息。飞行控制器通过加速度计获得当前加速度后，通过积分可以获得垂直速度信息，然后通过积分可以获得高度信息，如下:
然而，加速度计也有固有的缺陷。多次积分会造成结果的累积误差，加速度计的瞬时测量误差会比较大。
显然，仅仅依靠气压计或加速度计提供准确的高度信息反馈给实际控制是不可能的，所以我们应该考虑与其他传感器和气压计的数据进行数据融合处理，以便获得良好和准确的高度信息。
互补滤波算法是将气压计和加速度计测得的高度信息按权重进行融合，并在此基础上对高度信息进行结算。用高通滤波器对加速度进行精细处理，用低通滤波器对气压计信息进行处理，其中加速度计可以获得飞控垂直方向的加速度，还可以综合垂直速度信息。整个算法的核心思想是通过在地理坐标系中积分加速度来获得速度和位置信息。经过两次校正后，仍然可用的信息被校正。第一次，李忠传感器计算校正系数以校正加速度，第二次，通过校正系数校正位置。最后用加速度修正可用速度，用加速度和位置修正可用位置。加速度修正过程是通过减去偏差，将机体测得的加速度转换到地理坐标系。
气压计的主要作用是计算出一个修正系数来修正加速度偏移。数据融合过程如下所示:
加速度计测量无人机的加速度，测量值在机体坐标系中，所以需要利用旋转矩阵将加速度值转换成地面参考坐标系中的加速度。具体融合信息的实现过程如下:
(2)通过旋转矩阵将加速度计测得的加速度转换到地面参考坐标系。转换前注意需要去除加速度计的失调，因为地理坐标系中Z轴加速度包含重力加速度，所以需要对重力加速度进行补偿；
(3)计算气压计的校准系数，这是校准加速度计所需的系数。具体公式如下
(4)利用获得的气压计修正系数计算加速度计的偏移矢量。？
(5)将加速度偏移向量转换回机身坐标系，对转换后的加速度进行积分，得到融合后的速度信息，再对速度信息进行积分，得到最终的高度估计值，最后对气压计修正系数进行第二次修正。
通过Matlab软件采集飞行数据并进行仿真，结果如图所示。可以看出，融合后的高度比单独用加速度计和气压计测得的高度更准确。
2.2基于互补滤波的姿态计算
从飞行原理可以看出，无人机飞行过程中最终控制要回到姿态控制，通过特定的欧拉角调整可以控制无人机的飞行姿态。要完成无人机的E姿态控制，需要采集无人机的当前姿态，然后通过控制算法将无人机的当前姿态调整到期望姿态。姿态采集主要依靠飞控的惯性测量单元IMU，姿态计算的精度直接关系到无人机飞行位置的精度。
飞行过程中，陀螺测量无人机的角速度，动态性能高。角速度对时间积分可以得到三个欧拉角。陀螺数据在积分过程中会形成累积误差，累积误差会随着时间的推移而增大，所以短时间内陀螺的测量值更可靠。磁强计主要测量当前的磁场分布，也就是无人机与磁场的夹角，也就是偏航角。但磁力仪受周围磁场干扰严重，实际测量误差较大。加速度计之前已经介绍过了，就不赘述了。
三个传感器的特性在频域上是互补的，因此本文考虑使用互补滤波来融合三个传感器的数据。实际上，在加速度计和磁强计融合后，陀螺仪测量的姿态信息被补偿，以提高测量精度和系统的动态性能。
三个传感器的数据融合过程如图所示。陀螺仪通过高通滤波器消除低频噪声，加速度计和磁力计通过低通滤波器消除高频噪声。
利用旋转矩阵将三个传感器测得的欧拉角转换成四元数形式，进而计算出磁场的参考方向。
计算重力分量v和磁场分量w:
使用加速度和磁强计、重力分量和磁场分量的值来计算误差；
对上一步得到的误差进行比例积分处理，然后用得到的值补偿陀螺仪引起的零漂现象，最后通过解算得到当前姿态信息。
收集飞行数据并滤波后的结果如图5.4俯仰角、图5.5滚转角和图5.6偏航角所示。
3.PID控制器设计
无人机定高飞行主要有两种情况。一种是手动控制定高模式，无人机飞控仍然接收并执行遥控器的指令信号。另一种是无人机自主飞行时，比如航路点飞行或者机外模式，设置无人机在一定高度执行预设的飞行任务，不依靠遥控器的指令控制自身运动。本文研究了第二种高度设定模式。
在位置控制的背景下，本文的串级双环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姿态控制器。