基于激光雷达的多旋翼飞行器实时避障系统

章志诚，杜昌平

(浙江大学 航空航天学院， 杭州 310027)



基于激光雷达的多旋翼飞行器实时避障系统

章志诚，杜昌平

(浙江大学 航空航天学院， 杭州 310027)

针对多旋翼飞行器的障碍物规避问题,提出一种基于激光雷达的自主飞行多旋翼飞行器避障系统，实现多旋翼飞行器自主飞行的实时避障；该避障系统针对静态、低速运动障碍物，综合飞行器本体姿态、速度、加速度等状态信息，建立基于改进势场法的避障模型和算法；在机器人操作系统(ROS，Robot Operating System)平台进行该避障系统的软件实现，其通过串口与飞控进行通信，完成多旋翼飞行器的自主避障飞行；同时，为了使该系统能在强光环境正常工作，在不影响系统实时性的前提下，对激光雷达的干扰问题进行优化设计；大量实验表明：该避障算法计算量小，能够保证避障系统的实时性，在机体慢速以及低速运动(机体与障碍物之间的相对运动速度小于等于3 m/s)的场景中能够正确检测范围6 m内，并迅速规避障碍物。

多旋翼飞行器；激光雷达；改进势场法；避障

0 引言

近年来，随着计算机、传感器、人工智能技术的发展，多旋翼无人机越来越广泛地被应用于航拍、监控、探测、电力巡线、安保、农业等领域。然而，在以上这些行业应用中，若没有避障算法辅助作业，则多旋翼无人机极易发生事故，甚至损毁。例如：电力巡线中，其工作环境通常伴有电线杆、电线转接等多种障碍物影响多旋翼无人机巡线飞行。因此，多旋翼无人机的自主避障受到越来越多的关注[1]。

目前常见的避障方案主要分为三类：基于超声波传感器的避障方案，基于双目视觉的避障方案以及基于激光雷达的避障方案。

(1)基于超声波传感器的避障方案：

超声波传感器被广泛地应用于许多场景[2]，如倒车雷达、测距仪等。超声波传感器通常具有一定测量束角(一般介于10度至70度之间)，测距范围一般为4至10 m之间。由于超声波为机械波，在多旋翼飞行器上使用时，易衰减或受干扰，从而导致测量精度不高，甚至产生大量野值点的问题。其次，超声波传感器测量得到的数据量较少，并不利于实现精确平滑的避障控制。

(2)基于双目视觉的避障方案：

该方案通过双目视觉在获得丰富的图像信息的同时，对障碍物的距离进行计算[3]。优点为测量束角较大，精度较高。缺点为计算量、数据吞吐量较大，测量范围有限，对光线环境有着极为严苛的要求。

(3)基于激光雷达的避障方案：

该方案采用激光雷达传感器获取飞行环境信息，对障碍物的相对距离、角度等进行计算。其优点为精度高，弱光环境下表现较好；缺点为易受强光干扰。

为此，文中采用二维激光雷达传感器实时获取当前飞行环境信息，进而采用基于改进势场法的自主避障飞行实时路径规划。再与多旋翼飞行器飞行控制系统综合完成多旋翼飞行器的自主避障飞行。文中采用的激光雷达提供测量角精度1度、测量范围360度、测量频率5～10 Hz、测量距离为10 cm至6 m之间的二维点云数据。为克服激光雷达易受强光干扰的缺陷，将激光雷达的理论测量分辨率作为阈值参考，根据二维点云中相邻点距离的变化率滤除强光下产生的干扰点，使其可在强光下正常工作[4]。

1 实时避障系统方案

该实时避障系统主要由五部分组成：激光雷达、ODROID U3、飞控、ROS、多旋翼飞行器，如图1所示。

图1 避障系统组成图

激光雷达：用于检测障碍物，主要获得相对障碍物的距离与方位角信息。

ODROID U3：避障算法的执行硬件，基于ARM CORTEX-A9 四核处理器，其上运行LUBUNTU 14.04 LTS操作系统。

飞控：完成多旋翼飞行器的姿态、速度、位置控制。

ROS：机器人操作系统，是一个机器人软件平台，它能为异质计算机集群提供类似操作系统的功能。ROS的前身是斯坦福人工智能实验室为了支持斯坦福智能机器人STAIR而建立的交换庭(switchyard)项目，在该避障系统中，承担各个消息节点之间的消息传递与分配任务。

多旋翼飞行器：避障系统的执行部件，用于执行基本的飞行任务。

如图2为避障系统的每个子系统之间的依赖关系图。其中，MAVROS与Obstacle Avoidance为ROS中两个不同的消息节点。MAVROS负责处理ODROID与飞控之间的通信，同时将各个数据包的数据通过ROS转发给Obstacle Avoidance节点。Obstacle Avoidance节点从激光雷达处获取数据，经过避障算法之后将控制指令通过ROS转发给MAVROS。

图2 避障系统与各个子系统之间的依赖关系

该方案的优点：

1)精度高。角分辨率较高(360度，角分辨率为1度)，且可以利用的数据较多，控制效果更为柔和连续，不容易产生突变。

2)测距延迟低。随着被测物体距离增大，增加的测距延时可忽略不计。根据处理器性能优劣，测距频率可从5 Hz至40 Hz，这对实时控制系统具有重要意义。

3)可见光越弱，测量效果越好。

2 实时避障算法模型

所提出的实时避障算法首先根据激光雷达测得的二维点云信息，经滤波处理，获取有效点云数据；然后，针对传统势场法的局部最小点问题，改进传统的启发函数，最后，通过启发函数转换为飞控系统的导引控制指令，进而实现自主避障飞行。算法流程如图3所示。该避障算法主要分为4个模块：势场函数、引、斥力函数、启发函数、控制量映射。

图3 避障算法实现流程图

2.1 理论模型

1) 势场函数：

设飞行器受目标点引力作用，且距离越远，引力越大；受障碍物斥力作用，且距离越近，斥力越大，则可以得到引力势场为[5]：

(1)

斥力势场为：

(2)

式中，Ugf(d)表示引力势场，Urf(d)表示斥力势场，k表示比例系数，d为距离。

2) 引、斥力函数：

由式(1)可得引力函数为：

(3)

斥力函数为：

(4)

式中，引力与斥力分别为其对应势场函数的负梯度，Fgf(d)、Frf(d)分别为引力与斥力，d为距离。

3) 启发函数：

启发函数描述了多旋翼飞行器运动轨迹的约束关系。令启发函数为：

(5)

考虑到当飞行器合力为0时，即目标附近存在障碍物时，容易出现局部最小点，为此对斥力作出改进[5]，则:

(6)

式中，drfmin为安全距离，drfmax为斥力作用的最大距离。由于本文讨论的重点为避障算法，故此对启发函数进行了一定的简化，即将Frf(d)置零。其中,由于激光雷达距离旋翼外侧最短距离约为40 cm，且本文使用的激光雷达传感器测量死区为10 cm，同时考虑到避障需要预留一定的安全刹车距离，因此，实际使用时，将最短距离设为1 m。

4) 控制量映射：

尽管激光雷达给出的点云数据包含了目标点的距离与方位角信息，在实际使用当中，由于障碍物体积的原因，激光雷达检测到同一障碍物的点云数据通常存在多个数据点，因此，需要对所有的点云数据进行处理，并进行控制量映射，最终作为多旋翼飞行器位置环的控制输入量。

通过几何关系可将斥力分解到机体系X、Y方向上，其中Y轴负方向为机头方向，Y轴向右，Z轴竖直向下，由此可得：

(7)

式中，θ为方位角。则误差斥力函数为：

(8)

进而可得所有障碍物对于飞行器的平均斥力误差为：

(9)

将计算得到的平均斥力误差除以最大斥力误差即为归一化后的控制量。

(10)

5) 飞控算法：

飞控部分避障算法实现如图4所示，由于环境中障碍物对于多旋翼飞行器的斥力可以表征为加速度环的参考指令值。由此，将归一化后的平均斥力误差映射为对应的加速度环期望值，同时积分得到期望速度与期望位置，将其分别输入加速度、速度以及位置环进行闭环控制。其中，速度与位置观测量由惯导系统、光流传感器和GPS经数据融合得到。同时进行传感器故障诊断，当光流传感器或GPS传感器失效时，将失效传感器对应的观测量从扩展卡尔曼滤波器中去除。

图4 飞控部分避障算法实现

2.2 激光雷达强光下干扰的解决方案

如图5所示为强光下的激光雷达点云图，中间部分数据为野值点。由于雷达激光管发射的激光波长约为785 nm，而激光雷达采用了全波段CMOS传感器，故自然光中波长约为785 nm的光线会对激光雷达测量产生干扰[5]，由此产生大量野值点。

图5 强光下激光雷达检测到的点云图(中间部分点云为野值点)

大量实验分析表明，强光下激光雷达检测到的野值点大部分分布在中心范围2 m内，且沿极坐标轴线性密集分布。为此，本文采用将相邻点距与对应距离下的传感器分辨率比较的方式滤除该野值点。

实现步骤：

Step1：获取原始二维点云数据。

Step2：由于距离激光雷达约1 m左右距离时，野值点较多，且该距离对于避障没有太多实际意义，因此，将该段点云数据去除。

Step3：使用余弦定理计算目标点前后相邻5个点之间的平均距离，若该距离大于对应距离下的传感器测量分辨率时，则认为该点为野值点，将其滤除。

经过实验测试，该方案在强光时，仍然可以较好地滤除野值点，使得该用于室内的低成本激光雷达能够在户外强光下正常工作。

3 实验结果分析

本文试验平台——650 cm轴距的四旋翼飞行器，飞行最大速度可达12 m/s。为了便于室内试验，多旋翼飞行器上配备了光流传感器与超声波传感器，用于室内定位与悬停控制[7]。

本文实验测试环境为阳光较强的室外，图6为实验时，激光雷达采集到的点云数据。

图6 试验环境的激光雷达点云图

实验环境为室外，飞行器静止悬停在四周为墙面的环境中，通过光流传感器实现测速定位，悬停高度约为2 m。由于该环境GPS信号质量差，因此，采用光流传感器辅助悬停。当测试人员由远处接近至距离多旋翼飞行器约4 m处时，多旋翼飞行器开始执行避障任务，且距离越近避障速度越快，直至飞行器距离环境中障碍物4 m以上后继续悬停。

图7至图13分别为障碍物与多旋翼飞行器之间的距离，X、Y方向归一化后的避障控制量(放大1 000倍)，X、Y方向位置期望与实际值，X、Y方向速度期望与实际值。如图7、图8所示实验结果表明，当障碍物从4 m以外靠近时，多旋翼飞行器能够迅速给出避障控制量，达到较好的实时性(飞控端位置环控制周期为200 ms，避障控制周期在100 ms～200 ms之间)。如图9、图12所示实验结果表明，该系统的位置环响应速度较快，能够较好地跟随避障指令。如图7、图9和图12所示实验结果表明该系统在障碍物低速靠近时最小距离为3.3 m，远未达到飞行器机动极限，由此可知该避障系统能够在更高速情况下稳定规避障碍物。上述实验结果及分析表明：该避障算法能够较好地对障碍物进行规避，实时性较好，位置与速度环跟随速度较快，没有出现明显时延。

图7 障碍物与多旋翼飞行器之间的距离

图8 X方向归一化后的避障控制量(放大1 000倍)

图9 X方向位置期望与实际值，DPOSX为位置期望，POSX为实际位置，单位为cm

图10 X方向速度期望与实际值，DVELX为速度期望，VELX为实际速度，单位为cm/s

图11 Y方向归一化后的避障控制量(放大1 000倍)

图12 Y方向位置期望与实际值，DPOSY为位置期望，POSY为实际位置，单位为cm

图13 Y方向速度期望与实际值，DVELX为速度期望，VELX为实际速度，单位为cm/s

4 结论

文中采用二维激光雷达传感器和改进势场法设计实现了基于激光雷达的多旋翼飞行器实时避障系统。激光雷达传感器实时获取当前飞行环境中障碍物的相对距离和角度信息；改进势场法则是实时规划多旋翼飞行器最优飞行路径；且针对激光雷达易受户外强光干扰的缺陷进行了优化设计。该实时避障系统与多旋翼飞行器飞行控制系统综合完成多旋翼飞行器的自主避障飞行。所设计的避障系统在轴距为650 cm的多旋翼飞行器上进行了试验验证。结果表明在低速运动情况下，表现良好，可以正常规避移动障碍物；且所设计的激光雷达避障系统能应用于多旋翼飞行器户外避障。

[1] 张 侃. 多旋翼无人机在输电线路巡检中的运用及发展[J]. 现代工业经济和信息化, 2013 (16): 72-73.

[2] 蔡卓凡. 基于多超声波传感器避障机器人小车的设计[J]. 自动化技术与应用, 2014, 33(5): 85-89.

[3] 何 燚, 张翼飞. 基于双目视觉的移动机器人避障算法仿真研究[J]. 计算机仿真, 2013, 30(2): 282-285.

[4] 刘 杰, 闫清东, 唐正华. 基于激光雷达的移动机器人避障控制研究[J]. 计算机测量与控制, 2015, 23(3): 787-790.

[5] Konolige K, Augenbraun J, Donaldson N, et al. A low-cost laser distance sensor[A].Robotics and Automation, 2008. ICRA 2008. IEEE International Conference on[C]. IEEE, 2008: 3002-3008.

[6] 丁家如, 杜昌平, 赵 耀, 等. 基于改进人工势场法的无人机路径规划算法[J]. 计算机应用, 2015.

[7] 雷艳敏, 冯志彬. 改进的势场栅格法在机器人路径规划中的应用[J]. 长春大学学报, 2009, 19(2): 38-42.

[8] Honegger D, Meier L, Tanskanen P, et al. An open source and open hardware embedded metric optical flow CMOS camera for indoor and outdoor applications[A].Robotics and Automation (ICRA), 2013 IEEE International Conference on[C]. IEEE, 2013: 1736-1741.

[9] 李云翀, 何克忠. 基于激光雷达的室外移动机器人避障与导航新方法[J]. 机器人, 2006, 28(3): 275-278.

[10] 石为人, 黄兴华, 周 伟. 基于改进人工势场法的移动机器人路径规划[J]. 计算机应用, 2010, 30(8): 2021-2023.

[11] 王 铭, 王 瑞, 李晓娟, 等. 非确定性环境中移动机器人实时避障的概率模型检测[J]. 小型微型计算机系统, 2014, 35(9): 2104-2109.

[12] 张 祺, 杨宜民. 基于改进人工势场法的足球机器人避碰控制[J]. 机器人, 2002, 24(1): 12-15.

[13] 肖 雪, 秦贵和, 陈筠翰. 基于光流的自主移动机器人避障系统[J]. 计算机工程, 2013, 39(10): 305-308.

[14] 杨 明, 王 宏, 何克忠, 等. 基于激光雷达的移动机器人环境建模与避障[J]. 清华大学学报 (自然科学版), 2000, 40(7): 112-116.

[15] 张建英, 刘 暾. 基于人工势场法的移动机器人最优路径规划[J]. 航空学报, 2007, 28(B08): 183-188.

[16] 王醒策, 张汝波, 顾国昌. 基于势场栅格法的机器人全局路径规划[J]. 哈尔滨工程大学学报, 2003, 24(2): 170-174.

A Real-Time Obstacle Avoidance System for MultiCopter Based on Laser Radar

Zhang Zhicheng, Du Changping

(School of Aeronautics and Astronautics, Zhejiang University, Hangzhou 310027,China)

In order to solve the problem of obstacle avoidance of the multicopter,an obstacle avoidance system is studied based on autonomous flight of laser radar vehicle, which can realize the real-time obstacle avoidance for autonomous flight of the multicopter. According to avoid the static or low speed obstacle, an improved potential field algorithm is proposed, which considers the attitude angle, velocity and acceleration of the multicopter and runs in the Robot Operating System (ROS). This algorithm plays the important role in the obstacle avoidance system, as it can generate the flight control commands for the flight control system to complete obstacle. Then in order to work in normal light environment, a laser radar interference optimization of the obstacle avoidance system is also studied in the paper in detail. At last, a large number of experiments are carried out. The results show that the proposed obstacle avoidance system can quickly avoid the obstacle in the range of 6 meters with the low computational cost.

multicopter; laser radar; improved potential field method; obstacle avoidance

2016-04-10；

2016-05-18。

章志诚(1992-)，男，硕士，主要从事飞行控制，惯性导航方向的研究。

杜昌平(1978-)，男，博士，副教授，主要从事导航制导与控制，复杂系统建模与仿真、多传感器数据融合方向的研究。

1671-4598(2016)09-0117-05

10.16526/j.cnki.11-4762/tp.2016.09.032

TP29

A