室内非平整地形下无人机定高控制方法研究*

王 聪， 张 华,2， 陈财富， 汪 双， 黎荣华

(1.西南科技大学 信息工程学院，四川 绵阳 621000；2.西南科技大学 特殊环境机器人技术四川省重点实验室，四川 绵阳 621000)

0 引 言

无人机具备机动性强、视角广等优点，近年来在室内应急救援、巡检等领域得到广泛应用。其高度控制作为纵向飞行控制的重要组成部分[1]，是确保稳定性的重要环节。

现有导航方案，如气压计、惯性导航设备，在室内定高精度差，无法感知地形变化及上下方障碍物[2,3]。在室内，常采用超声波、激光测距仪测高，这类传感器可用于地形估计，如地形跟随，即无人机随下方地形高低起伏而起伏，与障碍物保持恒定相对高度[4,5]。文献[6,7]分别使用激光雷达、毫米波雷达用于坡路和植保中的地形跟随。但在实际使用过程中，无人机常需保持恒定绝对高度，如定距跟踪目标[8]，定高监视、巡检[9]。文献[10] 加入卡尔曼滤波融合超声波与加速度计估计无人机高度，但地形变化会影响融合效果。文献[11,12]分别使用双目视觉、视觉即时定位与地图构建(simultaneous localization and mapping，SLAM)进行无人机定高及定位，但视觉在光照、纹理特征不足时表现较差。文献[13]采用三维激光SLAM用于无人驾驶，但由于传感器体积及重量限制，不适用于小型无人机平台。文献[14]搭建无人机自主飞行系统，使用飞行时间(time of flight,ToF)传感器获取高度，在非平整地面仍需进一步处理。文献[15]使用惯性测量单元(inertial measurement unit,IMU)、气压计融合进行绝对高度的测量，能够一定程度适应突变地形，但在室内应用还需提高精度。

针对无人机在室内由于地形变化导致的定高不恒定问题，本文提出室内非平整地形下无人机定高控制方法，所提方法在室内应急救援，空中机器人路径规划，地空协同跟踪等领域有广阔应用前景。

1 系统描述

无人机搭载ToF传感器测定高度信息，在非平整地形条件下，高度数据经过高度去突变模型处理后，再进行自适应加权递推均值滤波，最后经过无人机飞行控制器高度环处理后用作高度控制。系统框图如图1所示。

图1 系统框图

2 六旋翼数学建模

无人机定高去突变系统采用六旋翼飞行器为载体，如图2为六旋翼飞行器结构示意，其中,o(x,y,z)为机体坐标系，O(X,Y,Z)为地球固连坐标系。为简化建模过程，提出以下假设：1)六旋翼为刚体；2)质量与转动惯量不变；3)六旋翼几何中心与质心一致。

图2 六旋翼结构示意

由机体坐标系到地球固连坐标系的旋转矩阵为

(1)

式中C与S分别为cos()和sin()函数；θ，ψ，φ分别为俯仰角、偏航角与滚转角。系统输入如下

(2)

式中U为六旋翼4个独立通道的输入，U1,U2,U3,U4分别为升力、滚转、俯仰、偏航输入量；Ni(i=1,2,3,4,5,6)为各个电机的转速；L为旋翼中心到机体质心的距离；a和d分别为升力、阻力系数。

由牛顿—欧拉方程可得出所关注的飞行高度及决定多旋翼稳定性的姿态角的运动方程[16]

(3)

式中J为总转动惯量，Ix，Iy，Iz分别为x，y，z轴的转动惯量，m为飞行器质量，g为重力加速度，fi(i=1,2,3,4)为系统其他干扰量。

3 高度去突变模型

3.1 地形突变情况分析

ToF传感器测量无人机与地面之间的相对距离，在地形突变情况下该距离发生突变。如图3是无人机在地形突变情况下的高度变化情况。无人机先后处于突变前后的地面上空，产生的突变高度为Δd，通过建立高度去突变模型大幅削减地形突变对高度的影响。

图3 地形突变情况的无人机高度变化

考虑到地形存在高低起伏，设定高度突变阈值t，并提出以下判据判断地形是否产生突变

|dc-dl|>t

(4)

式中t为高度突变阈值，综合考虑传感器刷新速率及室内应用需求，本文中高度突变阈值取0.1 m；dc为当前高度；dl为上一时刻高度。若满足式(4)，则产生了地形突变，须进一步进行高度去突变处理。

3.2 单一地形突变

当满足式(4)，突变高度大于阈值，首先取得突变高度，再利用当前高度减去突变高度得到相对稳定高度，建立如下高度去突变模型

d=dc-Δd,Δd=dc-dl

(5)

式中 Δd为突变高度。d为去突变处理后的高度。如式(5)所示的高度去突变模型，在低复杂度的单一地形突变环境，突变过程固定，前后突变高度一致，能够满足运用需求。

3.3 连续地形突变

室内环境地形情况十分复杂，往往存在地形连续突变的情况。式(5)中的高度去突变模型不满足应用需求。在式(5)的基础上，对突变高度模型进行优化，首次产生突变后，对突变高度进行累加，随地形的连续变化而调整。建立如下高度去突变模型

(6)

4 自适应加权递推均值滤波

经过高度去突变模型的处理，无人机高度数据仍存在小幅波动，通过引入自适应加权递推均值滤波，减小高度误差，并使无人机高度变化更加平滑。

4.1 加权均值滤波

加权均值滤波取n个数据，根据数据时间序列先后等因素赋予相应权重，再进行加权平均，能够减小数据波动。加权均值滤波用于无人机高度数据处理过程如下

(7)

式中di为总数n的数据序列中第i个高度去突变模型处理后的数据，考虑传感器帧率及应用需求，本文中n取10；wi为第i个数据的加权系数；dstable为加权平均滤波后的稳定高度。

该方法存在时间滞后，加权系数适应性不强的缺点。为减少时间滞后，动态分配加权系数，在加权均值滤波的基础上，引入一种自适应加权递推均值滤波算法。

4.2 建立自适应加权递推均值滤波模型

首先,递推选取n个数据，确定对应加权系数，根据估计高度与期望高度之间的误差动态调整加权系数。误差越大，权值应越小，采用反比例函数确定加权因子，如式(8)所示，取误差的绝对值与正实数之和作为分母，避免误差为0及负数时导致权值无穷大的情况,即

(8)

然后进行归一化处理,得到加权系数为

(9)

式中wi为第i个数据对应的归一化加权系数。

最后得出自适应加权递推滤波后的高度数据

(10)

式中dstable为对高度去突变数据进行自适应加权递推均值滤波后的稳定高度。

5 高度控制模型

ToF传感器的测量数据先后经过高度去突变模型、自适应加权递推均值滤波处理后的稳定高度dstable，经无人机位置控制环中的高度通道处理后用作高度控制。

如图4所示的高度通道，采用双闭环串级比例—积分—微分(proportional-integral-differential,PID)控制算法，可分为外环高度控制环及内环高度速度控制环，外环输入为期望高度，内环输入为外环输出，即期望速度[17]。

图4 高度通道

高度控制环采用比例控制器得到期望速度用作高度速度控制环的输入

(11)

式中Velsp为期望速度；Zerr为高度误差；Zsp为期望高度;kp为比例系数。

高度速度控制环采用PID控制器，得到期望拉力输入控制分配器对无人机电机进行控制

(12)

式中fsp为期望拉力；m为无人机质量；g为重力加速度；kP，kI，kD为比例、积分、微分系数；Velerr为速度误差。

6 实验平台搭建与测试

ToF传感器采用北醒光子的低成本、高精度TFmini-Plus激光雷达，基于ToF即飞行时间原理，测量范围0.1～12 m，准确度为±5 cm，重量12 g；无人机采用X600-X6六轴机架；板载计算机NVIDIA TX2 用于高度数据的处理；飞行控制器使用开源PIXHAWK一代，固件版本为v1.11.0；ToF传感器朝下安装于无人机底部，用于获取无人机与地面之间的相对距离。

为定量分析无人机在室内地形突变环境下的定高效果，搭建4.5 m×3.7 m×3.1 m的钢架结构及缓冲网组成实验场地；使用不同高度的盒子模拟连续地形突变。如图5为实验平台。

图5 实验平台

无人机于定点模式保持与地面的相对距离不变，并飞越下方模拟地形突变环境，测试定高效果。如图6为地形突变状态下无人机飞越下方障碍物过程，t0时刻处于平坦地面上方；t1时刻飞越0.5 m高障碍物，高度发生突变；t2时刻处于0.5,0.25 m高障碍物之间；t3时刻飞越0.25 m高障碍物；t4时刻移出0.25 m高障碍物，位于平坦地面上方。

图6 地形突变状态下无人机飞行实验过程

7 数据分析

实验共分4组：期望高度分别相距平坦地面1，2 m，并以两组快慢不同的速度飞越下方高度为0.5，0.25 m左右的障碍物，速度大致为4，1 m/s；测试高度去突变模型及自适应加权递推均值滤波效果,分析计算高度误差。

期望高度为1，2m时不同速度下的高度估计曲线如图7(a)所示。如图7(b)为期望高度为1,2m时，不同速度下发生地形突变后的高度误差曲线，并比较加入自适应加权递推均值滤波前后的误差。

图7 实验结果

由图7(a)所示不同速度、不同期望高度下高度估计曲线得出：高度去突变模型能够大幅削减地形对高度的影响；自适应加权递推均值滤波使高度变化曲线更加平滑，相较于快速飞行，慢速飞行的高度去突变效果相对较差。

由图7(b)所示的发生地形突变后的高度误差曲线得出：相较于高度去突变模型，加入自适应加权递推均值滤波的高度误差波动幅度更小；相较于快速飞行，慢速飞行的高度误差更大。相较于快速飞行，慢速飞行去突变效果较差，是由于飞越下方障碍物边缘时，检测到边缘高度突变值小于阈值，须在实际使用过程中调整阈值。表1为不同飞行速度及高度下的误差对比情况，与期望高度相比，最大平均误差为7.97 cm，平均误差为2.92 cm。

表1 高度误差对比 cm

相比文献[15]提出的IMU气压计融合绝对高度估计方法，本文方法将高度平均误差从19.7 cm减小到了2.92 cm，高度精度提升85.2 %。

8 结束语

本文提出建立基于TOF传感器的高度去突变、自适应加权递推均值滤波模型，并应用于无人机室内非平整地形的定高。实验表明：本文方法能有效规避地形对高度的影响，满足执行飞行任务时保持恒定绝对高度的需求；高度估计精度达到2.82cm，满足室内飞行时精确度需求。所提方法已应用于应急救援环境下的地空协同机器人的定距离跟踪，满足实际应用需求。