章志誠，杜昌平

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

?

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

章志誠，杜昌平

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

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

多旋翼飛行器；激光雷達(dá)；改進(jìn)勢場法；避障

0 引言

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2 實(shí)時避障算法模型

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

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

2.1 理論模型

1) 勢場函數(shù)：

設(shè)飛行器受目標(biāo)點(diǎn)引力作用，且距離越遠(yuǎn)，引力越大；受障礙物斥力作用，且距離越近，斥力越大，則可以得到引力勢場為[5]：

(1)

斥力勢場為：

(2)

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

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

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

(3)

斥力函數(shù)為：

(4)

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

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

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

(5)

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

(6)

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

4) 控制量映射：

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

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

(7)

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

(8)

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

(9)

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

(10)

5) 飛控算法：

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

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

2.2 激光雷達(dá)強(qiáng)光下干擾的解決方案

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

圖5 強(qiáng)光下激光雷達(dá)檢測到的點(diǎn)云圖(中間部分點(diǎn)云為野值點(diǎn))

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

實(shí)現(xiàn)步驟：

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

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

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

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

3 實(shí)驗(yàn)結(jié)果分析

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

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

圖6 試驗(yàn)環(huán)境的激光雷達(dá)點(diǎn)云圖

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

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

圖7 障礙物與多旋翼飛行器之間的距離

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

圖9 X方向位置期望與實(shí)際值，DPOSX為位置期望，POSX為實(shí)際位置，單位為cm

圖10 X方向速度期望與實(shí)際值，DVELX為速度期望，VELX為實(shí)際速度，單位為cm/s

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

圖12 Y方向位置期望與實(shí)際值，DPOSY為位置期望，POSY為實(shí)際位置，單位為cm

圖13 Y方向速度期望與實(shí)際值，DVELX為速度期望，VELX為實(shí)際速度，單位為cm/s

4 結(jié)論

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

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

[2] 蔡卓凡. 基于多超聲波傳感器避障機(jī)器人小車的設(shè)計[J]. 自動化技術(shù)與應(yīng)用, 2014, 33(5): 85-89.

[3] 何 燚, 張翼飛. 基于雙目視覺的移動機(jī)器人避障算法仿真研究[J]. 計算機(jī)仿真, 2013, 30(2): 282-285.

[4] 劉 杰, 閆清東, 唐正華. 基于激光雷達(dá)的移動機(jī)器人避障控制研究[J]. 計算機(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ìn)人工勢場法的無人機(jī)路徑規(guī)劃算法[J]. 計算機(jī)應(yīng)用, 2015.

[7] 雷艷敏, 馮志彬. 改進(jìn)的勢場柵格法在機(jī)器人路徑規(guī)劃中的應(yīng)用[J]. 長春大學(xué)學(xué)報, 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] 李云翀, 何克忠. 基于激光雷達(dá)的室外移動機(jī)器人避障與導(dǎo)航新方法[J]. 機(jī)器人, 2006, 28(3): 275-278.

[10] 石為人, 黃興華, 周 偉. 基于改進(jìn)人工勢場法的移動機(jī)器人路徑規(guī)劃[J]. 計算機(jī)應(yīng)用, 2010, 30(8): 2021-2023.

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

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

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

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

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

[16] 王醒策, 張汝波, 顧國昌. 基于勢場柵格法的機(jī)器人全局路徑規(guī)劃[J]. 哈爾濱工程大學(xué)學(xué)報, 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-)，男，碩士，主要從事飛行控制，慣性導(dǎo)航方向的研究。

杜昌平(1978-)，男，博士，副教授，主要從事導(dǎo)航制導(dǎo)與控制，復(fù)雜系統(tǒng)建模與仿真、多傳感器數(shù)據(jù)融合方向的研究。

1671-4598(2016)09-0117-05

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

TP29

A