基于激光雷達的多旋翼飛行器實時避障系統(tǒng)

章志誠，杜昌平

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

?

基于激光雷達的多旋翼飛行器實時避障系統(tǒng)

章志誠，杜昌平

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

針對多旋翼飛行器的障礙物規(guī)避問題,提出一種基于激光雷達的自主飛行多旋翼飛行器避障系統(tǒng)，實現(xiàn)多旋翼飛行器自主飛行的實時避障；該避障系統(tǒng)針對靜態(tài)、低速運動障礙物，綜合飛行器本體姿態(tài)、速度、加速度等狀態(tài)信息，建立基于改進勢場法的避障模型和算法；在機器人操作系統(tǒng)(ROS，Robot Operating System)平臺進行該避障系統(tǒng)的軟件實現(xiàn)，其通過串口與飛控進行通信，完成多旋翼飛行器的自主避障飛行；同時，為了使該系統(tǒng)能在強光環(huán)境正常工作，在不影響系統(tǒng)實時性的前提下，對激光雷達的干擾問題進行優(yōu)化設計；大量實驗表明：該避障算法計算量小，能夠保證避障系統(tǒng)的實時性，在機體慢速以及低速運動(機體與障礙物之間的相對運動速度小于等于3 m/s)的場景中能夠正確檢測范圍6 m內(nèi)，并迅速規(guī)避障礙物。

多旋翼飛行器；激光雷達；改進勢場法；避障

0 引言

近年來，隨著計算機、傳感器、人工智能技術(shù)的發(fā)展，多旋翼無人機越來越廣泛地被應用于航拍、監(jiān)控、探測、電力巡線、安保、農(nóng)業(yè)等領(lǐng)域。然而，在以上這些行業(yè)應用中，若沒有避障算法輔助作業(yè)，則多旋翼無人機極易發(fā)生事故，甚至損毀。例如：電力巡線中，其工作環(huán)境通常伴有電線桿、電線轉(zhuǎn)接等多種障礙物影響多旋翼無人機巡線飛行。因此，多旋翼無人機的自主避障受到越來越多的關(guān)注[1]。

目前常見的避障方案主要分為三類：基于超聲波傳感器的避障方案，基于雙目視覺的避障方案以及基于激光雷達的避障方案。

(1)基于超聲波傳感器的避障方案：

超聲波傳感器被廣泛地應用于許多場景[2]，如倒車雷達、測距儀等。超聲波傳感器通常具有一定測量束角(一般介于10度至70度之間)，測距范圍一般為4至10 m之間。由于超聲波為機械波，在多旋翼飛行器上使用時，易衰減或受干擾，從而導致測量精度不高，甚至產(chǎn)生大量野值點的問題。其次，超聲波傳感器測量得到的數(shù)據(jù)量較少，并不利于實現(xiàn)精確平滑的避障控制。

(2)基于雙目視覺的避障方案：

該方案通過雙目視覺在獲得豐富的圖像信息的同時，對障礙物的距離進行計算[3]。優(yōu)點為測量束角較大，精度較高。缺點為計算量、數(shù)據(jù)吞吐量較大，測量范圍有限，對光線環(huán)境有著極為嚴苛的要求。

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

該方案采用激光雷達傳感器獲取飛行環(huán)境信息，對障礙物的相對距離、角度等進行計算。其優(yōu)點為精度高，弱光環(huán)境下表現(xiàn)較好；缺點為易受強光干擾。

為此，文中采用二維激光雷達傳感器實時獲取當前飛行環(huán)境信息，進而采用基于改進勢場法的自主避障飛行實時路徑規(guī)劃。再與多旋翼飛行器飛行控制系統(tǒng)綜合完成多旋翼飛行器的自主避障飛行。文中采用的激光雷達提供測量角精度1度、測量范圍360度、測量頻率5～10 Hz、測量距離為10 cm至6 m之間的二維點云數(shù)據(jù)。為克服激光雷達易受強光干擾的缺陷，將激光雷達的理論測量分辨率作為閾值參考，根據(jù)二維點云中相鄰點距離的變化率濾除強光下產(chǎn)生的干擾點，使其可在強光下正常工作[4]。

1 實時避障系統(tǒng)方案

該實時避障系統(tǒng)主要由五部分組成：激光雷達、ODROID U3、飛控、ROS、多旋翼飛行器，如圖1所示。

圖1 避障系統(tǒng)組成圖

激光雷達：用于檢測障礙物，主要獲得相對障礙物的距離與方位角信息。

ODROID U3：避障算法的執(zhí)行硬件，基于ARM CORTEX-A9 四核處理器，其上運行LUBUNTU 14.04 LTS操作系統(tǒng)。

飛控：完成多旋翼飛行器的姿態(tài)、速度、位置控制。

ROS：機器人操作系統(tǒng)，是一個機器人軟件平臺，它能為異質(zhì)計算機集群提供類似操作系統(tǒng)的功能。ROS的前身是斯坦福人工智能實驗室為了支持斯坦福智能機器人STAIR而建立的交換庭(switchyard)項目，在該避障系統(tǒng)中，承擔各個消息節(jié)點之間的消息傳遞與分配任務。

多旋翼飛行器：避障系統(tǒng)的執(zhí)行部件，用于執(zhí)行基本的飛行任務。

如圖2為避障系統(tǒng)的每個子系統(tǒng)之間的依賴關(guān)系圖。其中，MAVROS與Obstacle Avoidance為ROS中兩個不同的消息節(jié)點。MAVROS負責處理ODROID與飛控之間的通信，同時將各個數(shù)據(jù)包的數(shù)據(jù)通過ROS轉(zhuǎn)發(fā)給Obstacle Avoidance節(jié)點。Obstacle Avoidance節(jié)點從激光雷達處獲取數(shù)據(jù)，經(jīng)過避障算法之后將控制指令通過ROS轉(zhuǎn)發(fā)給MAVROS。

圖2 避障系統(tǒng)與各個子系統(tǒng)之間的依賴關(guān)系

該方案的優(yōu)點：

1)精度高。角分辨率較高(360度，角分辨率為1度)，且可以利用的數(shù)據(jù)較多，控制效果更為柔和連續(xù)，不容易產(chǎn)生突變。

2)測距延遲低。隨著被測物體距離增大，增加的測距延時可忽略不計。根據(jù)處理器性能優(yōu)劣，測距頻率可從5 Hz至40 Hz，這對實時控制系統(tǒng)具有重要意義。

3)可見光越弱，測量效果越好。

2 實時避障算法模型

所提出的實時避障算法首先根據(jù)激光雷達測得的二維點云信息，經(jīng)濾波處理，獲取有效點云數(shù)據(jù)；然后，針對傳統(tǒng)勢場法的局部最小點問題，改進傳統(tǒng)的啟發(fā)函數(shù)，最后，通過啟發(fā)函數(shù)轉(zhuǎn)換為飛控系統(tǒng)的導引控制指令，進而實現(xiàn)自主避障飛行。算法流程如圖3所示。該避障算法主要分為4個模塊：勢場函數(shù)、引、斥力函數(shù)、啟發(fā)函數(shù)、控制量映射。

圖3 避障算法實現(xiàn)流程圖

2.1 理論模型

1) 勢場函數(shù)：

設飛行器受目標點引力作用，且距離越遠，引力越大；受障礙物斥力作用，且距離越近，斥力越大，則可以得到引力勢場為[5]：

(1)

斥力勢場為：

(2)

式中，Ugf(d)表示引力勢場，Urf(d)表示斥力勢場，k表示比例系數(shù)，d為距離。

2) 引、斥力函數(shù)：

由式(1)可得引力函數(shù)為：

(3)

斥力函數(shù)為：

(4)

式中，引力與斥力分別為其對應勢場函數(shù)的負梯度，F(xiàn)gf(d)、Frf(d)分別為引力與斥力，d為距離。

3) 啟發(fā)函數(shù)：

啟發(fā)函數(shù)描述了多旋翼飛行器運動軌跡的約束關(guān)系。令啟發(fā)函數(shù)為：

(5)

考慮到當飛行器合力為0時，即目標附近存在障礙物時，容易出現(xiàn)局部最小點，為此對斥力作出改進[5]，則:

(6)

式中，drfmin為安全距離，drfmax為斥力作用的最大距離。由于本文討論的重點為避障算法，故此對啟發(fā)函數(shù)進行了一定的簡化，即將Frf(d)置零。其中,由于激光雷達距離旋翼外側(cè)最短距離約為40 cm，且本文使用的激光雷達傳感器測量死區(qū)為10 cm，同時考慮到避障需要預留一定的安全剎車距離，因此，實際使用時，將最短距離設為1 m。

4) 控制量映射：

盡管激光雷達給出的點云數(shù)據(jù)包含了目標點的距離與方位角信息，在實際使用當中，由于障礙物體積的原因，激光雷達檢測到同一障礙物的點云數(shù)據(jù)通常存在多個數(shù)據(jù)點，因此，需要對所有的點云數(shù)據(jù)進行處理，并進行控制量映射，最終作為多旋翼飛行器位置環(huán)的控制輸入量。

通過幾何關(guān)系可將斥力分解到機體系X、Y方向上，其中Y軸負方向為機頭方向，Y軸向右，Z軸豎直向下，由此可得：

(7)

式中，θ為方位角。則誤差斥力函數(shù)為：

(8)

進而可得所有障礙物對于飛行器的平均斥力誤差為：

(9)

將計算得到的平均斥力誤差除以最大斥力誤差即為歸一化后的控制量。

(10)

5) 飛控算法：

飛控部分避障算法實現(xiàn)如圖4所示，由于環(huán)境中障礙物對于多旋翼飛行器的斥力可以表征為加速度環(huán)的參考指令值。由此，將歸一化后的平均斥力誤差映射為對應的加速度環(huán)期望值，同時積分得到期望速度與期望位置，將其分別輸入加速度、速度以及位置環(huán)進行閉環(huán)控制。其中，速度與位置觀測量由慣導系統(tǒng)、光流傳感器和GPS經(jīng)數(shù)據(jù)融合得到。同時進行傳感器故障診斷，當光流傳感器或GPS傳感器失效時，將失效傳感器對應的觀測量從擴展卡爾曼濾波器中去除。

圖4 飛控部分避障算法實現(xiàn)

2.2 激光雷達強光下干擾的解決方案

如圖5所示為強光下的激光雷達點云圖，中間部分數(shù)據(jù)為野值點。由于雷達激光管發(fā)射的激光波長約為785 nm，而激光雷達采用了全波段CMOS傳感器，故自然光中波長約為785 nm的光線會對激光雷達測量產(chǎn)生干擾[5]，由此產(chǎn)生大量野值點。

圖5 強光下激光雷達檢測到的點云圖(中間部分點云為野值點)

大量實驗分析表明，強光下激光雷達檢測到的野值點大部分分布在中心范圍2 m內(nèi)，且沿極坐標軸線性密集分布。為此，本文采用將相鄰點距與對應距離下的傳感器分辨率比較的方式濾除該野值點。

實現(xiàn)步驟：

Step1：獲取原始二維點云數(shù)據(jù)。

Step2：由于距離激光雷達約1 m左右距離時，野值點較多，且該距離對于避障沒有太多實際意義，因此，將該段點云數(shù)據(jù)去除。

Step3：使用余弦定理計算目標點前后相鄰5個點之間的平均距離，若該距離大于對應距離下的傳感器測量分辨率時，則認為該點為野值點，將其濾除。

經(jīng)過實驗測試，該方案在強光時，仍然可以較好地濾除野值點，使得該用于室內(nèi)的低成本激光雷達能夠在戶外強光下正常工作。

3 實驗結(jié)果分析

本文試驗平臺——650 cm軸距的四旋翼飛行器，飛行最大速度可達12 m/s。為了便于室內(nèi)試驗，多旋翼飛行器上配備了光流傳感器與超聲波傳感器，用于室內(nèi)定位與懸?？刂芠7]。

本文實驗測試環(huán)境為陽光較強的室外，圖6為實驗時，激光雷達采集到的點云數(shù)據(jù)。

圖6 試驗環(huán)境的激光雷達點云圖

實驗環(huán)境為室外，飛行器靜止懸停在四周為墻面的環(huán)境中，通過光流傳感器實現(xiàn)測速定位，懸停高度約為2 m。由于該環(huán)境GPS信號質(zhì)量差，因此，采用光流傳感器輔助懸停。當測試人員由遠處接近至距離多旋翼飛行器約4 m處時，多旋翼飛行器開始執(zhí)行避障任務，且距離越近避障速度越快，直至飛行器距離環(huán)境中障礙物4 m以上后繼續(xù)懸停。

圖7至圖13分別為障礙物與多旋翼飛行器之間的距離，X、Y方向歸一化后的避障控制量(放大1 000倍)，X、Y方向位置期望與實際值，X、Y方向速度期望與實際值。如圖7、圖8所示實驗結(jié)果表明，當障礙物從4 m以外靠近時，多旋翼飛行器能夠迅速給出避障控制量，達到較好的實時性(飛控端位置環(huán)控制周期為200 ms，避障控制周期在100 ms～200 ms之間)。如圖9、圖12所示實驗結(jié)果表明，該系統(tǒng)的位置環(huán)響應速度較快，能夠較好地跟隨避障指令。如圖7、圖9和圖12所示實驗結(jié)果表明該系統(tǒng)在障礙物低速靠近時最小距離為3.3 m，遠未達到飛行器機動極限，由此可知該避障系統(tǒng)能夠在更高速情況下穩(wěn)定規(guī)避障礙物。上述實驗結(jié)果及分析表明：該避障算法能夠較好地對障礙物進行規(guī)避，實時性較好，位置與速度環(huán)跟隨速度較快，沒有出現(xiàn)明顯時延。

圖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 結(jié)論

文中采用二維激光雷達傳感器和改進勢場法設計實現(xiàn)了基于激光雷達的多旋翼飛行器實時避障系統(tǒng)。激光雷達傳感器實時獲取當前飛行環(huán)境中障礙物的相對距離和角度信息；改進勢場法則是實時規(guī)劃多旋翼飛行器最優(yōu)飛行路徑；且針對激光雷達易受戶外強光干擾的缺陷進行了優(yōu)化設計。該實時避障系統(tǒng)與多旋翼飛行器飛行控制系統(tǒng)綜合完成多旋翼飛行器的自主避障飛行。所設計的避障系統(tǒng)在軸距為650 cm的多旋翼飛行器上進行了試驗驗證。結(jié)果表明在低速運動情況下，表現(xiàn)良好，可以正常規(guī)避移動障礙物；且所設計的激光雷達避障系統(tǒng)能應用于多旋翼飛行器戶外避障。

[1] 張 侃. 多旋翼無人機在輸電線路巡檢中的運用及發(fā)展[J]. 現(xiàn)代工業(yè)經(jīng)濟和信息化, 2013 (16): 72-73.

[2] 蔡卓凡. 基于多超聲波傳感器避障機器人小車的設計[J]. 自動化技術(shù)與應用, 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] 丁家如, 杜昌平, 趙 耀, 等. 基于改進人工勢場法的無人機路徑規(guī)劃算法[J]. 計算機應用, 2015.

[7] 雷艷敏, 馮志彬. 改進的勢場柵格法在機器人路徑規(guī)劃中的應用[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] 石為人, 黃興華, 周 偉. 基于改進人工勢場法的移動機器人路徑規(guī)劃[J]. 計算機應用, 2010, 30(8): 2021-2023.

[11] 王 銘, 王 瑞, 李曉娟, 等. 非確定性環(huán)境中移動機器人實時避障的概率模型檢測[J]. 小型微型計算機系統(tǒng), 2014, 35(9): 2104-2109.

[12] 張 祺, 楊宜民. 基于改進人工勢場法的足球機器人避碰控制[J]. 機器人, 2002, 24(1): 12-15.

[13] 肖 雪, 秦貴和, 陳筠翰. 基于光流的自主移動機器人避障系統(tǒng)[J]. 計算機工程, 2013, 39(10): 305-308.

[14] 楊 明, 王 宏, 何克忠, 等. 基于激光雷達的移動機器人環(huán)境建模與避障[J]. 清華大學學報 (自然科學版), 2000, 40(7): 112-116.

[15] 張建英, 劉 暾. 基于人工勢場法的移動機器人最優(yōu)路徑規(guī)劃[J]. 航空學報, 2007, 28(B08): 183-188.

[16] 王醒策, 張汝波, 顧國昌. 基于勢場柵格法的機器人全局路徑規(guī)劃[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-)，男，博士，副教授，主要從事導航制導與控制，復雜系統(tǒng)建模與仿真、多傳感器數(shù)據(jù)融合方向的研究。

1671-4598(2016)09-0117-05

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

TP29

A