基于無人測試船水面路徑規劃策略?

石釗銘

（武漢市江夏區藏龍大道709號 武漢 430205）

1 引言

隨著科學技術的高速發展，各種機器人越來越多地應用到人們的日常生活中，從事著與人們日常生活息息相關的服務工作，改善和提高了人們的生活質量［1］。無人水面艇作為集多傳感器綜合于一體的科技產品，在諸如測繪、軍事演習等各行各業扮演著主要的角色。

機器人在封閉環境中的全覆蓋路徑規劃一直是較為熱門的話題，而應用場景也較為豐富，例如掃地機器人的全覆蓋清掃路徑，無人機的農藥噴灑路徑，國內外有眾多學者對此展開了大量的研究，賀嘉等在《基于貪心蟻群算法的無人水面艇全局路徑規劃》提出了雙向搜索法優化傳統貪心算法，使得迭代時間和次數減少，并取得了較好的路徑規劃結果［2］。簡毅等在《一種室內掃地機器人全遍歷路徑規劃方法研究》中，提出了先學習子區域，然后由邊緣向中靠攏的路徑規劃算法［3］。

本文首先對水面進行柵格化，然后采用旅行商算法進行全覆蓋式路徑規劃，根據路徑規劃發布的路徑點來動態的優化運動軌跡，采取了轉角與直行同步的策略。提高了無人測試船的巡航效率，避免規劃路徑中銳角調整時間過長的現象，減少過多減速以及轉折的次數，保證了無人測試船運動位姿的美觀性。最后在ROS操作系統下，利用系統工具快速搭建實驗平臺，以便于測試算法的可行性。

本文算法流程如圖1所示。

圖1 算法流程

2 覆蓋式路徑規劃算法

在進行覆蓋式路徑規劃之前，本文首先運用占據柵格地圖（Occupancy Grid Map）法［4］對已知水域進行建模。柵格法建模，是采用一系列同樣大小的柵格對水域進行建模的方法。在柵格地圖中，白色方塊代表水域中的可行區域，黑色方塊代表水域中的不可行區域，例如邊界或者水中高地，需要繞開進行路徑規劃。

根據切片法進行細胞單元劃分，如圖2所示地圖區域劃分為共7個細胞區域，然后對于每個單元格有兩種出入方式。則對于若干個子細胞而言，全覆蓋路徑規劃是旅行商問題［5］。個體基因由數字1～N的排列組成，本文將個體基因確定為4N個細胞路徑組合。單元id和出入點通過4N的模進行解碼，基因之間不可重復。

圖2 切片劃分子細胞示意圖

遺傳根據適應度函數對群體進行遺傳更新，本文采用如下適應度函數：

其中ωdist與ωtime分別代表總流程與總時間的權值。

根據遺傳算法模型，本文采用全覆蓋路徑規劃算法描述如下。

1）首先建立解碼和編碼的數學模型，采用洗牌算法隨機打亂細胞排列進行初始化，然后隨機選擇一個細胞的入口和出口點；

2）依據初始種群計算最優路徑，指定當前最優解為初始解；

3）隨機選擇兩個細胞作為父母細胞，利用交叉算子［6］產生后代，當子代數目與初始數目相同時停止交叉，交叉后根據變異概率進行變異，并將子代變異前后交換位置；

4）對于步驟3）的子代，將規定突變概率最為交換規則，隨機變換細胞的進入和退出點，然后找到每個細胞的最佳進入退出點；

5）用步驟4）的后代取代初始種群，計算更新后的種群單體的路徑總長度，如果更新后的總長度比更新前的總長度短，則更新計算結果；

6）將步驟3）～5）進行迭代，直到進化與分配后代數目相同為止，得到遺傳算法的最優解。

3 基于動態圓的路徑優化

無人測試船在水面的運動模型解析為角速度和線速度，可無人測試船在水面的運動狀態，實現轉彎或直行等運動。通過控制兩電機的轉速差來實現機器人在二維平面里的運動。

根據水面環境的柵格地圖建模得到地圖數據，將第二節規劃的路徑輸出為一系列路徑點，在柵格地圖中，路徑規劃后，發布的路徑如圖2所示，綠色線條即為發布的路徑。

圖2路徑規劃中，可歸納無人測試船的三種運動情況，測試船在遇到邊界前進行直線運動，轉角有銳角、直角、鈍角三種情況。取其中鈍角時如圖3所示。

圖3 鈍角情況示意圖

無人測試船先到達點A、轉彎后到達點B、最后到達C，在圖4中，假設AB=a，BC=b，AC=c，設定閾值為L，由此可得動態圓的半徑：

根據C點相對直線AB的位置，即可確定出角速度的方向，已知兩點 p1(x1，y1)，p2(x2，y2)，假設p1與 p2不重合，則任意點 p(x，y)相對直線L的位置，可根據Temp，可由以下模型得到：

判斷點與直線位置關系步驟如下：

1）若x比Temp小，則點p位于直線L左側；

2）若x比Temp大，則點p位于直線L右側；

3）若x與Temp相等，則直線L穿過點p。

如圖4所示，由半徑R根據線速度和角速度的數學關系，計算轉彎比例大小。則在轉彎的路徑單元內，無人測試船從點A到點C的過程為先由點A直線運動到點M，然后以點O為圓心，R為半徑，弧線運動到點G，最后直線運動到達點C。

圖4 動態圓計算示意圖

4 平臺測試并比較效率

ROS（Robot Operating System）是一個機器人軟件平臺，它能為異質計算機集群提供類似操作系統的功能［7］。在其可視化功能下RVIZ下可以發布路徑點，能夠實時的可視化路徑，進而實現機器人的仿真實驗。對于水面環境建模后利用本文算法得到的路徑規劃如圖5所示，紅色線條為本文算法規劃的路徑。

圖5 實際柵格地圖路徑規劃

在機器人的控制中，坐標系統是非常重要的，在ROS使用TF軟件庫進行坐標轉換［8］。在不同節點中不斷發布和接受TF轉換信息，則可以得到父坐標系和子坐標系的轉換關系，從而模擬實際機器人的移動，轉換信息還包含一些調試需要的發送頻率、運行時間等信息。利用ROS的工具，利用CLion集成軟件編寫程序，分別發布拉鏈等路徑規劃信息，來測試算法的效果。實際測試中用了多個基礎單元，以減小誤差，以拉鏈路徑為例，如圖6所示，其中兩個正交坐標系分別對應為地圖坐標map和無人測試船坐標base_footprint。

5 結語

本文基于無人測試船平臺將水面環境建模為柵格地圖，提出了一種高效率的測試船全覆蓋路徑規劃策略，根據路徑規劃的路徑點進行了轉角的動態圓優化，最后搭建實驗環境加以驗證算法效果。首先在柵格地圖上進行覆蓋式路徑規劃，目標點實際位置關系計算出動態半徑，然后確定角速度方向，最后確定速度大小發布主題，運動控制節點訂閱主題數據，即實現無人測試船的路徑規劃。

下階段的研究中將嘗試用其他的路徑優化方法［9］，同時會嘗試使用貝塞爾曲線［10］或者橢圓來代替轉角圓弧優化，以期得到更好的效果。