應用改進隨機樹算法的無人艇局部路徑規(guī)劃

莊佳園，張 磊，孫寒冰，蘇玉民

(哈爾濱工程大學 水下機器人技術(shù)重點實驗室，15000哈爾濱)

水面無人艇(Unmanned Surface Vehicle，USV)簡稱無人艇，是一種具有自主規(guī)劃、自主航行能力，并可自主完成環(huán)境感知、目標探測及戰(zhàn)術(shù)攻擊等任務(wù)的小型水面船舶.其中，以色列“Protector”和美國“Spartan”無人艇引領(lǐng)著當今世界無人艇的發(fā)展方向，其他國家進行了無人艇的研究，如意大利的“Charlie”［1］、英 國 的“Springer”［2］和葡萄牙的“Delfim”等.文獻［3-5］總結(jié)了USV的發(fā)展歷史及現(xiàn)狀.USV的路徑規(guī)劃方法按對環(huán)境信息已知程度不同可分為兩類:環(huán)境信息完全已知的全局路徑規(guī)劃;環(huán)境信息完全未知或部分未知，通過傳感器實時地對USV的當前工作環(huán)境進行探測，以獲取障礙物的位置和尺寸等信息的局部路徑規(guī)劃［6］.當USV按電子海圖等已知環(huán)境信息規(guī)劃的全局路徑航行時，同時需要根據(jù)航海雷達等當前傳感器感知的局部環(huán)境進行動態(tài)局部路徑規(guī)劃［7］.

本文針對USV航速快、機動性強、對局部路徑規(guī)劃算法實時性要求高等特點，改進了快速擴展隨機樹(RRT)算法，以當前雷達圖像為環(huán)境模型完成了USV動態(tài)局部路徑規(guī)劃，通過對規(guī)劃航線的優(yōu)化處理，在保證生成航線安全的同時，使優(yōu)化后航線相對航程最短且光順可行.

1 路徑規(guī)劃算法的描述與實現(xiàn)

1.1 經(jīng)典的RRT算法

快速擴展隨機樹(rapidly-exploring random tree，RRT)算法［8］由Lavalle首次提出.用樹結(jié)構(gòu)代替有向圖結(jié)構(gòu)，可以在給定控制率的條件下，解決高維多自由度機器人的復雜約束下的運動規(guī)劃問題，適用于包含幾何和動力學約束的路徑規(guī)劃.

RRT算法的主要思想是逐步、快速地降低一個隨機選擇的節(jié)點與樹之間的距離，直至滿足預期要求.目標是搜索到一條從起點xinit到終點xgoal的可行路徑，基本的RRT構(gòu)建過程如圖1所示.

圖1 基本的RRT構(gòu)建過程

在航海雷達圖像范圍內(nèi)，采用經(jīng)典RRT算法分別擴展100、500、1000、2000步的效果見圖2.

圖2 RRT算法擴展過程

可以看出，RRT算法每次擴展傾向于探索未知部分，主體枝干會迅速擴散到空間的4個頂點，同時主干的分叉又會深入到其他局部區(qū)域，這種平衡的擴展方式是RRT算法具有快速性的主要原因［9］.

1.2 RRT算法的改進

RRT算法在完成對未知環(huán)境探索并完成規(guī)劃的同時，存在以下問題［10］:在全局空間內(nèi)均勻搜索，導致算法無謂耗費較大;先全局搜索構(gòu)建隨機樹，然后一次性規(guī)劃路徑，實時應用性較差;路徑的搜索樹由隨機采樣點生成，缺乏可重復性，導致規(guī)劃路徑不是最優(yōu)路徑.

針對以上基本RRT算法存在的問題，以提高算法的效率和性能為目標，出現(xiàn)了一些RRT算法的改進算法.Nik等［11］將粒子濾波引入到RRT算法中，提高了隨機樹擴展的自適應性.Aldahak等［12］提出了KD樹概念，提高了搜索效率.Yershova等［13］加入了擴展反饋信息用以抑制擴展點范圍，Jaillet等［14］在此基礎(chǔ)上增加了動態(tài)調(diào)整信息.Burns等［15］提出了以預測模型為基礎(chǔ)的動態(tài)RRT算法，減少了規(guī)劃時間.康亮等［16］將RRT算法與基于滾動窗口的路徑規(guī)劃相結(jié)合，以增強算法探索未知空間的能力.宋金澤等［17］將非完整約束條件與雙向多步RRT算法相結(jié)合，在提高搜索效率的同時保證了路徑的可行性.彭輝等［18］在無人機區(qū)域目標搜索中改進了RRT算法，提高了搜索效率.

綜合以上優(yōu)化算法，針對USV的運動特點，本文在兩個方面改進RRT算法:

1)改進生長點的選擇，限制陷入局部區(qū)域節(jié)點附近一定范圍內(nèi)的節(jié)點被選為生長點.假設(shè)，當前樹T中含有n個節(jié)點，T={xi}，i=1，2，…，n．以xi為生長點，對應的探索點xnew與障礙物發(fā)生碰撞，稱xi探索失敗;若未與障礙物發(fā)生碰撞，稱xi探索成功．記fi對節(jié)點xi探索失敗的次數(shù)，即節(jié)點xi探索失敗一次，則fi=fi+1;如果該節(jié)點探索成功，則重置fi為1．探索失敗節(jié)點xi與其余節(jié)點xj(xj∈T)的距離為rij=xj-xi，定義δj為節(jié)點xj的抑制因子:

式中:ε為探索步長，文中取USV在最大航速下的最小直航距離.樹中節(jié)點xj與xrand間的距離為Dj=xj-xrand，則節(jié)點xj的權(quán)值wj為

權(quán)值更新后，選取樹中權(quán)值最大的節(jié)點作為樹的生長點.

2)改進探索點的選擇，以USV最大轉(zhuǎn)角θmax為約束條件限制探索點的范圍，引入距離啟發(fā)信息，使得規(guī)劃出來的航跡接近最優(yōu)搜索航跡．在未搜索區(qū)域產(chǎn)生m個隨機點，k=1，2，…，m，以為目標點，分別按上述生長點改進方法計算探索點為使規(guī)劃后航跡滿足USV的可航性約束，需要根據(jù)當前位置和到的航向改變量θk，對于超出USV可達范圍內(nèi)的隨機點xkrand，以巡航速度下最大轉(zhuǎn)角θmax為限制條件來計算

圖3 改進的RRT節(jié)點擴展

根據(jù)上述策略，基于改進RRT算法的局部路徑規(guī)劃算法描述如圖4所示:

步驟1 初始化生長樹T，以初始點xinit為生長樹的根節(jié)點x1;

步驟2 在未搜索區(qū)域產(chǎn)生m個隨機點，k=1，2，…，m，作為樹的探索方向點;

步驟5 選擇最優(yōu)節(jié)點xnew，按式(4)計算Jk，選擇min{Jk}對應的探索點為最優(yōu)節(jié)點加入生長樹;

步驟6 更新生長樹T，若xnew未與任何障礙物發(fā)生碰撞，則xnew加入生長樹T中，新的生長樹更新為T=T+xnew，否則放棄xnew，T不變，返回步驟2;

步驟7 判斷是否到達目標點xgoal，如果xgoal-xnew＜τ(τ為目標點的范圍閾值)，則認為到達目標點xgoal，否則返回步驟2;

步驟8 從目標點xgoal回溯到初始點xinit，返回路徑．

圖4 改進RRT算法流程圖

由以上算法過程和圖4可以看出:通過對RRT算法生長點選取的改進，在選取生長點時不僅考慮隨機方向點和樹節(jié)點之間的距離，同時加入衡量節(jié)點探索失敗次數(shù)的抑制因子，實現(xiàn)自適應調(diào)整樹中節(jié)點的生長權(quán)值，使樹朝著最有利的方向生長;通過對探索點選取的改進，以最大轉(zhuǎn)角限制探索方向，使規(guī)劃航跡趨于實用，以距目標點的距離為啟發(fā)因子，削弱新增節(jié)點的隨機性，使得規(guī)劃航跡接近最優(yōu)航跡.

2 試驗結(jié)果及分析

2010年～2012年，某型USV在中國進行了多次外場試驗，完成了自主航行和無人自主避碰試驗，試驗結(jié)果驗證了本文算法的有效性.

選取3種自主避碰試驗中的典型實際雷達圖像，如圖5所示.

圖5 原始雷達圖像

試驗1 港口內(nèi)自主出港試驗，起始點為USV當前位置，目標點為港口外一點.

試驗2 湖泊內(nèi)一個障礙物規(guī)避試驗，起始點為USV當前位置，目標點為障礙物后一點.

試驗3 海上兩個障礙物規(guī)避試驗，起始點為USV當前位置，目標點為右前方兩個障礙物中間一點.

為驗證改進RRT算法有效性，以經(jīng)過處理的二值化雷達圖像為環(huán)境模型［19］，采用經(jīng)典RRT算法和本文改進算法進行航跡搜索，USV航速20節(jié)(搜索步長為10 m)，最大轉(zhuǎn)彎角為60°，試驗結(jié)果如圖6～8所示.

圖6 試驗1規(guī)劃航線

圖7 試驗2規(guī)劃航線

圖8 試驗3規(guī)劃航線

表1可以看出，經(jīng)典RRT算法和改進RRT算法均可以生成一條由起始點到目標點的安全航線，通過對該航線中航點的跟蹤，即可使USV達到自主航行和避碰的目的.但從搜索效率來看，改進RRT算法對規(guī)劃路徑的搜索方向性更明確，樹中的無用節(jié)點大幅減少，同時對陷入局部區(qū)域中的節(jié)點具有較好的限制.如試驗1中在第一次搜索不成功的情況下，能快速調(diào)整生長點并完成路徑搜索.從規(guī)劃時間來看，由于經(jīng)典RRT算法在整個模型空間內(nèi)均勻搜索可行路線，因此搜索節(jié)點較多，規(guī)劃時間較長，不能滿足USV的實時性要求.綜上，本文對探索點和生長點的選擇的改進，可以提高算法的效率，滿足USV嵌入式系統(tǒng)的實時性要求.

表1 3種試驗規(guī)劃結(jié)果比較

3 規(guī)劃路徑的優(yōu)化處理

文中提出的改進RRT算法雖然可以完成可行路徑的搜索，但規(guī)劃航線中都存在多余航點，使得規(guī)劃出來的路徑并不理想.多余航點是指那些去除后不會影響航線有效性和安全性的航點.如果將規(guī)劃結(jié)果直接作為USV航行路徑，多余航點造成的階梯形和鋸齒形線段不利于USV的運動控制，因此需要對規(guī)劃路徑進行優(yōu)化處理，僅保留轉(zhuǎn)向點，以適應USV的實際航行路線.本文采用二分查找法逐次判定線段安全性，對航點序列進行多余航點去除，過程如下［20］:在規(guī)劃出的路徑中依次取出連續(xù)的航點pi、pi+1、pi+2，若pi與pi+2之間的連線不與障礙物發(fā)生碰撞，則去除pi+1;繼續(xù)判斷pi與pi+2之后的航點連線，若其連線不與障礙物發(fā)生碰撞，則刪去pi+2;依次類推，直到pi與后面的某航點的連接線與障礙物發(fā)生碰撞，則在該航點后重新取出連續(xù)的3個航點并作為pi、pi+1、pi+2;重復上述過程，直至取完全部航點．

稱經(jīng)過多余航點去除后的規(guī)劃路徑為關(guān)鍵路徑.由于僅保留了少量必要航點(關(guān)鍵點)，使得關(guān)鍵路徑上存在許多拐角，USV到達關(guān)鍵路徑上關(guān)鍵點時要保持一定的速度，但在該關(guān)鍵點處艏向角要發(fā)生變化.由于USV在航行過程中慣性較大，經(jīng)常會沖出關(guān)鍵點，艏向控制也需要一定的時間達到穩(wěn)定，會造成USV偏離原規(guī)劃航線，對運動控制尤其是航跡跟蹤有不利影響，因此需要對關(guān)鍵路徑的拐點進行平滑處理.本文采用的處理方式為基于試驗數(shù)據(jù)的曲線擬合，在分析總結(jié)USV大量回轉(zhuǎn)試驗數(shù)據(jù)的基礎(chǔ)上，統(tǒng)計USV在不同航速下的戰(zhàn)術(shù)回轉(zhuǎn)直徑，以每個拐點處的轉(zhuǎn)艏角度對應的圓弧代替原路徑中的拐點，達到對規(guī)劃路徑進行平滑處理的目的，使處理后的航行路徑更接近于實際航行情況.本文以運動控制限定的最大舵角(10°)對應的回轉(zhuǎn)試驗數(shù)據(jù)作為擬合曲線.

經(jīng)過優(yōu)化處理后的規(guī)劃航線見圖9，優(yōu)化前后結(jié)果比較見表2.

圖9 優(yōu)化后規(guī)劃航線

表2 優(yōu)化前后規(guī)劃結(jié)果比較

由表2可以看出:本文采用的優(yōu)化方法可以有效地去除原規(guī)劃路徑中多余的航點，減少規(guī)劃后的航行距離;以USV實際回轉(zhuǎn)試驗數(shù)據(jù)光順處理后的路徑更加容易滿足USV運動控制系統(tǒng)要求，具有可跟蹤性.

4 結(jié) 論

1)改進了經(jīng)典RRT算法探索點和生長點的選擇方式，在保證樹生長方向有利的同時削弱新增節(jié)點的隨機性，使其更適用于USV的局部路徑規(guī)劃.

2)以海上和湖上實際雷達圖像為環(huán)境模型，完成了局部路徑規(guī)劃試驗，并與經(jīng)典RRT算法進行對比.本文的改進方法可以較好地提高搜素效率，同時對陷入局部區(qū)域的節(jié)點有一定的抑制作用.

3)通過對規(guī)劃航線的優(yōu)化和基于回轉(zhuǎn)試驗數(shù)據(jù)的平滑處理，去除了規(guī)劃路徑中的多余航點，減少了規(guī)劃距離，同時光順后的路徑可更好地適應USV控制系統(tǒng)的要求.

［1］CACCIA M，BIBULI M，BONO R，et al.Basic navigation，guidance and control of an unmanned surface vehicle［J］.Autonomous Robots，2008，25(4):349-365.

［2］XU T，CHUDLEY J，SUTTON R.Soft computing design of a multisensory data fusion system for unmanned surface vehicle navigation［C］//Proceedings of the 7th IFAC Conference on Maneuvering and Control of Marine Craft.Lisbon:IFAC，2006:124-156.

［3］YAN Rujian，PANG Shuo，SUN Hanbing，et al.Development and missions of unmanned surface vehicle［J］.Journal of Marine Science and Application，2010，9(4):451-457.

［4］MANLEY J E.Autonomous surface vessels，15 years of development［C］//Proceedings of Oceans 2008 MTS/IEEE Quebec Conference and Exhibition.Quebec:IEEE，2008:1-4.

［5］VEERS J，BERTRAM V.Development of the USV multi-mission surface vehicle III［C］//Proceedings of 5th Int Conference Computer and IT Application in the Maritime Industries(COMPIT).Leiden:COMPIT，2006:345-355.

［6］馬仁利，關(guān)正西.路徑規(guī)劃技術(shù)的現(xiàn)狀與發(fā)展綜述［J］.現(xiàn)代機械，2008(3):22-27.

［7］CAMPBELL S，NAEEM W，IRWIN G W.A review on improving the autonomy of unmanned surface vehicles through intelligent collsion avoidance manoeuvres［J］.Annual Reviews in Control，2012，36(9):167.283.

［8］LAVALLE S.Rapidly-exploring random trees:A new tool for path planning［D］.Iowa:Iowa State University，1998.

［9］王維.虛擬人運動規(guī)劃與運動合成關(guān)鍵技術(shù)研究［D］.長沙:國防科技大學，2011.

［10］康亮.自主移動機器人運動規(guī)劃的若干算法研究［D］.南京:南京理工大學，2009.

［11］NIK A M，REID S.Particle RRT for path planning with uncertainty［C］//Proceedings of IEEE International Conference on Robotics and Automation.Rome:IEEE，2007:1617-1624.

［12］ALDAHAK A，ELNAGAR A.Practical pursuit-evasion algorithm:detection and tracking［C］//Proceedings of IEEE International Conference on Robotics and Automation.Rome:IEEE，2007:343-348.

［13］YERSHOVA A，JAILLET L，SIMéON T，et al.Dynamic domain RRTs:Efficient exploration by controlling the sampling domain［C］//Proceedings of IEEE International Conference on Robotics and Automation.Barcelona:IEEE，2005:3856-3861.

［14］JAILLET L，YERSHOVA A，LAVALLE S，et al.Adaptive tuning of the sampling domain for dynamicdomain RRTs ［C］//Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems.Edmonton:IEEE，2005:2851-2856.

［15］BURNS B，BROCK O.Single-query motion planning with utility guided random trees［C］//Proceedings of IEEE International Conference on Robotics and Automation.Rome:IEEE，2007:3307-3312.

［16］康亮，趙春霞，郭劍輝.基于模糊滾動RRT算法的移動機器人路徑規(guī)劃［J］.南京理工大學學報:自然科學版，2010，34(5):642-648

［17］宋金澤，戴斌，單恩忠，等.一種改進的RRT路徑規(guī)劃算法［J］.電子學報，2010，32(2):225-228.

［18］彭輝，王林，沈林成.區(qū)域目標搜索中基于改進RRT的UAV實時航跡規(guī)劃［J］.國防科技大學學報，2009，31(5):86-91.

［19］莊佳園，徐玉如，萬磊，等.基于雷達圖像的水面無人艇目標檢測技術(shù)［J］.哈爾濱工程大學學報，2012，33(2):29-135.

［20］莊佳園，蘇玉民，廖煜雷，等.基于航海雷達的水面無人艇局部路徑規(guī)劃［J］.上海交通大學學報，2012，46(9):1371-1375.