石釗銘

（武漢市江夏區(qū)藏龍大道709號 武漢 430205）

1 引言

隨著科學(xué)技術(shù)的高速發(fā)展，各種機(jī)器人越來越多地應(yīng)用到人們的日常生活中，從事著與人們?nèi)粘Ｉ钕⑾⑾嚓P(guān)的服務(wù)工作，改善和提高了人們的生活質(zhì)量［1］。無人水面艇作為集多傳感器綜合于一體的科技產(chǎn)品，在諸如測繪、軍事演習(xí)等各行各業(yè)扮演著主要的角色。

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

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

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

圖1 算法流程

2 覆蓋式路徑規(guī)劃算法

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

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

圖2 切片劃分子細(xì)胞示意圖

遺傳根據(jù)適應(yīng)度函數(shù)對群體進(jìn)行遺傳更新，本文采用如下適應(yīng)度函數(shù)：

其中ωdist與ωtime分別代表總流程與總時間的權(quán)值。

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

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

2）依據(jù)初始種群計算最優(yōu)路徑，指定當(dāng)前最優(yōu)解為初始解；

3）隨機(jī)選擇兩個細(xì)胞作為父母細(xì)胞，利用交叉算子［6］產(chǎn)生后代，當(dāng)子代數(shù)目與初始數(shù)目相同時停止交叉，交叉后根據(jù)變異概率進(jìn)行變異，并將子代變異前后交換位置；

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

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

6）將步驟3）～5）進(jìn)行迭代，直到進(jìn)化與分配后代數(shù)目相同為止，得到遺傳算法的最優(yōu)解。

3 基于動態(tài)圓的路徑優(yōu)化

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

根據(jù)水面環(huán)境的柵格地圖建模得到地圖數(shù)據(jù)，將第二節(jié)規(guī)劃的路徑輸出為一系列路徑點，在柵格地圖中，路徑規(guī)劃后，發(fā)布的路徑如圖2所示，綠色線條即為發(fā)布的路徑。

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

圖3 鈍角情況示意圖

無人測試船先到達(dá)點A、轉(zhuǎn)彎后到達(dá)點B、最后到達(dá)C，在圖4中，假設(shè)AB=a，BC=b，AC=c，設(shè)定閾值為L，由此可得動態(tài)圓的半徑：

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

判斷點與直線位置關(guān)系步驟如下：

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

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

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

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

圖4 動態(tài)圓計算示意圖

4 平臺測試并比較效率

ROS（Robot Operating System）是一個機(jī)器人軟件平臺，它能為異質(zhì)計算機(jī)集群提供類似操作系統(tǒng)的功能［7］。在其可視化功能下RVIZ下可以發(fā)布路徑點，能夠?qū)崟r的可視化路徑，進(jìn)而實現(xiàn)機(jī)器人的仿真實驗。對于水面環(huán)境建模后利用本文算法得到的路徑規(guī)劃如圖5所示，紅色線條為本文算法規(guī)劃的路徑。

圖5 實際柵格地圖路徑規(guī)劃

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

5 結(jié)語

本文基于無人測試船平臺將水面環(huán)境建模為柵格地圖，提出了一種高效率的測試船全覆蓋路徑規(guī)劃策略，根據(jù)路徑規(guī)劃的路徑點進(jìn)行了轉(zhuǎn)角的動態(tài)圓優(yōu)化，最后搭建實驗環(huán)境加以驗證算法效果。首先在柵格地圖上進(jìn)行覆蓋式路徑規(guī)劃，目標(biāo)點實際位置關(guān)系計算出動態(tài)半徑，然后確定角速度方向，最后確定速度大小發(fā)布主題，運動控制節(jié)點訂閱主題數(shù)據(jù)，即實現(xiàn)無人測試船的路徑規(guī)劃。

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