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

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

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

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

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

1 路徑規劃算法的描述與實現

1.1 經典的RRT算法

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

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

圖1 基本的RRT構建過程

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

圖2 RRT算法擴展過程

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

1.2 RRT算法的改進

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

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

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

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

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

權值更新后，選取樹中權值最大的節點作為樹的生長點.

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

圖3 改進的RRT節點擴展

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

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

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

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

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

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

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

圖4 改進RRT算法流程圖

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

2 試驗結果及分析

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

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

圖5 原始雷達圖像

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

試驗2 湖泊內一個障礙物規避試驗，起始點為USV當前位置，目標點為障礙物后一點.

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

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

圖6 試驗1規劃航線

圖7 試驗2規劃航線

圖8 試驗3規劃航線

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

表1 3種試驗規劃結果比較

3 規劃路徑的優化處理

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

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

經過優化處理后的規劃航線見圖9，優化前后結果比較見表2.

圖9 優化后規劃航線

表2 優化前后規劃結果比較

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

4 結 論

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

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

3)通過對規劃航線的優化和基于回轉試驗數據的平滑處理，去除了規劃路徑中的多余航點，減少了規劃距離，同時光順后的路徑可更好地適應USV控制系統的要求.

［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］馬仁利，關正西.路徑規劃技術的現狀與發展綜述［J］.現代機械，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］王維.虛擬人運動規劃與運動合成關鍵技術研究［D］.長沙:國防科技大學，2011.

［10］康亮.自主移動機器人運動規劃的若干算法研究［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算法的移動機器人路徑規劃［J］.南京理工大學學報:自然科學版，2010，34(5):642-648

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

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

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

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