改進RRT 算法用于電磁驅(qū)動機器魚路徑規(guī)劃

朱紅秀，鄭 權(quán)，杜 闖，徐 亮

（中國礦業(yè)大學(xué)（北京）機電與信息工程學(xué)院，北京 100083）

0 引言

隨著各國對海洋的探測和研究活動日漸頻繁，高性能的水下機器人研發(fā)工作迫在眉睫。機器魚作為典型的水下機器人，具有作業(yè)范圍廣、機動性強等特點，可以高效地完成海洋軍事偵查、海底勘測、海洋生物誘導(dǎo)、海洋救援等多種任務(wù)［1-2］。機器魚的作業(yè)環(huán)境往往是復(fù)雜的、隨機變化的，因此，合理的路徑規(guī)劃對于機器魚作業(yè)十分重要［3-6］。

在路徑規(guī)劃方面，許多專家和學(xué)者進行了相關(guān)研究。李連鵬等應(yīng)用遺傳算法提升了機器魚路徑規(guī)劃的速度，但是受限于復(fù)雜環(huán)境，路徑規(guī)劃精確性難以保證［7］。王建等提出基于改進AFSA 的機器魚群路徑規(guī)劃算法，但是該算法研究尚處于理論階段，靈敏度等參數(shù)難以滿足機器魚的控制要求［8］。胡靜波等基于改進的強化學(xué)習(xí)算法實現(xiàn)了機器魚的路徑規(guī)劃，但是該算法需要大量的樣本數(shù)據(jù)作為支撐［9］。洪強等通過改進A*算法實現(xiàn)已知環(huán)境的機器魚最優(yōu)路徑規(guī)劃，但是算法搜索范圍大，效率較低［10］。除此以外，應(yīng)用較多的算法還有模糊邏輯法［11］、圖搜索法［12］和道路圖規(guī)劃法［13］等。目前應(yīng)用于機器魚路徑規(guī)劃的算法在處理路徑規(guī)劃問題時具有一定的優(yōu)越性，但是當(dāng)機器魚處于復(fù)雜工作環(huán)境時，這些算法的計算量會隨之增加，這嚴(yán)重限制了機器魚的作業(yè)能力［14］。在快速擴展隨機樹（RRT）算法［15］的基礎(chǔ)上對其改進，并將其應(yīng)用在一種以電磁驅(qū)動器作為動力源的機器魚的路徑規(guī)劃上，通過仿真和實驗驗證了該算法應(yīng)用在電磁驅(qū)動機器魚路徑規(guī)劃上的合理性。

1 電磁驅(qū)動機器魚

1.1 電磁驅(qū)動原理

圖1 機器魚總體裝配

圖2 尾部裝配圖

電磁驅(qū)動機器魚的總體裝配如圖1 所示，動力源為尾部的電磁驅(qū)動器，尾部裝配圖如圖2 所示。電磁驅(qū)動器結(jié)構(gòu)簡單，無需傳動機構(gòu)，可以直接與機器魚本體相連。電磁驅(qū)動器工作時始終處于通電狀態(tài)，通過輸入雙極性正弦交流電的控制信號，驅(qū)動器帶動尾柄從一側(cè)擺動到另一側(cè)位置［16］。

圖3 電磁驅(qū)動器結(jié)構(gòu)示意圖

圖3 所示為電磁驅(qū)動器的結(jié)構(gòu)示意圖，機器魚初始工作時，線圈處于左側(cè)的極限位置，當(dāng)正弦控制信號處于正半周期時，通入正向電流，線圈左端磁化為N 極右端磁化為S 極，使其與左側(cè)永磁體相互排斥，與右側(cè)永磁體相互吸引，在二者合力作用下線圈繞旋轉(zhuǎn)軸順時針擺動，到達(dá)右側(cè)極限位置后，正弦控制信號處于負(fù)半周期，通入反向電流，線圈左端磁化為S 極右端磁化為N 極，使線圈與右側(cè)永磁體相互排斥，與左側(cè)永磁體相互吸引，在二者的合力作用下，永磁體推動線圈繞旋轉(zhuǎn)軸進行逆時針擺動。永磁體和線圈之間的氣隙是電磁驅(qū)動器的主要工作氣隙，該處磁場主要由永磁鐵磁場和線圈磁場共同組成，在氣隙磁場力的作用下線圈帶動魚尾繞旋轉(zhuǎn)軸擺動，從而控制機器魚實現(xiàn)往復(fù)擺動。

在路徑規(guī)劃需要機器魚進行轉(zhuǎn)彎時，電磁驅(qū)動器通電線圈的擺動方式為由單側(cè)極限位置向中間位置往復(fù)擺動，從而實現(xiàn)尾鰭的單側(cè)擺動。單側(cè)擺動信號設(shè)置為正負(fù)周期具有不同的占空比如圖4 所示，左圖為電磁驅(qū)動機器魚左轉(zhuǎn)時通電線圈信號，右圖為電磁驅(qū)動機器魚左轉(zhuǎn)時通電線圈信號。

圖4 電磁驅(qū)動控制信號

1.2 電磁驅(qū)動機器魚控制系統(tǒng)

為了實現(xiàn)機器魚的自主游動，首先，通過控制模塊讀取傳感器模塊的融合姿態(tài)角信息；然后，控制模塊根據(jù)已知信息進行機器魚路徑規(guī)劃并輸出對應(yīng)的左轉(zhuǎn)、右轉(zhuǎn)，或直線游動的機器魚數(shù)字控制信號。最后驅(qū)動模塊將數(shù)字信號轉(zhuǎn)換為模擬信號并進行功率放大，輸出可以負(fù)載電磁驅(qū)動器線圈的控制信號，控制機器魚不斷地調(diào)整位姿使其逼近期望值直至到達(dá)目標(biāo)點?？刂葡到y(tǒng)的各個模塊配合流程如下頁圖5 所示。

2 RRT 算法

2.1 RRT 算法描述

圖5 控制系統(tǒng)

快速擴展隨機樹（RRT）算法是一種基于采樣的路徑規(guī)劃算法，通過隨機采樣的方式探索空間生成規(guī)劃路徑，可以顯著降低計算量。同時，算法無需進行任何預(yù)處理步驟，只需對其產(chǎn)生的隨機節(jié)點采用樹型結(jié)構(gòu)存儲，因此，算法被廣泛應(yīng)用于機器人路徑規(guī)劃。

算法分為隨機樹的構(gòu)建和路徑查詢兩個過程［17-18］。

2.1.1 隨機樹構(gòu)建

空間為C∈RN，其中包括機器魚的位置坐標(biāo)與姿態(tài)角度。假設(shè)狀態(tài)空間C 中存在一組復(fù)雜的約束條件，則機器魚的任何規(guī)劃路徑都必須保持處于此約束內(nèi)。

定義Ctree∈RN為所有滿足約束條件的狀態(tài)空間集合。為了確保機器魚在水下環(huán)境的安全性，RRT算法只能在Ctree∈RN中進行擴展構(gòu)建。機器魚是否會發(fā)生碰撞可以通過檢測選取的狀態(tài)點q 是否滿足狀態(tài)空間C 的全局約束來檢測，即通過驗證q∈Ctree是否成立來決定狀態(tài)點q 是否可以抵達(dá)，以及是否可以成為快速擴展隨機樹的新節(jié)點。

設(shè)Tk是一個擁有k 個節(jié)點的隨機樹，且Tk∈Ctree，即快速擴展隨機樹Tk生成樹枝所產(chǎn)生的規(guī)劃路徑為無碰撞路徑。設(shè)q 是Tk的節(jié)點且q∈Tk，定義qinit是狀態(tài)起始點，qgoal是狀態(tài)目標(biāo)點，Cgoal∈Ctree為目標(biāo)點區(qū)域。路徑規(guī)劃任務(wù)可以看作是在狀態(tài)空間Ctree中搜索一條連續(xù)的路徑，該路徑從qinit出發(fā)，到qgoal或Cgoal截止。

RRT 算法的構(gòu)建如圖6 所示［19］:

圖6 RRT 算法構(gòu)建過程

RRT 算法構(gòu)建流程圖如圖7 所示。

2.1.2 路經(jīng)查詢

圖7 RRT 算法構(gòu)建流程圖

路徑查詢的具體步驟:完成隨機樹的構(gòu)建后，停止添加新的狀態(tài)點，從qgoal開始，逆向遍歷隨機樹Tk，依次遍歷其父節(jié)點，反復(fù)迭代直到找到qinit為止，這樣就可以找到一條從qinit到qgoal的規(guī)劃路徑，并且該路徑能夠滿足機器魚的全局約束條件。

因隨機樹在隨機采樣狀態(tài)點的過程中，其添加的新狀態(tài)點符合機器魚的約束條件，因此，最終得到的規(guī)劃路徑也是合理的、符合約束條件的。

2.2 RRT 算法分析

由于RRT 算法采用隨機采樣方式對狀態(tài)空間進行探索，可以通過采樣法將隨機樹的生長引向未知空白的區(qū)域，避免了對空間環(huán)境的建模，因此，RRT 算法更加適用于高維度機器魚在復(fù)雜環(huán)境中的路徑規(guī)劃。但是因為RRT 算法的隨機性，同樣也會導(dǎo)致一些問題［20］:

1）不確定性:隨機采樣具有隨機性，算法最終擴展形成的隨機樹也是不同的，在相同環(huán)境下同一起始點和目標(biāo)點的多次重復(fù)規(guī)劃得到的規(guī)劃路徑也可能會各不相同。

2）偏差性:利用RRT 得到的規(guī)劃路徑不一定是最優(yōu)的。

3）緩慢性:隨機樹的擴展是通過隨機選取狀態(tài)點的方式，擴展方向是隨機的，因此，隨機樹對目標(biāo)狀態(tài)點的收斂速度可能會十分緩慢。

2.3 RRT 算法的改進

2.3.1 收斂加速

隨機樹擴展節(jié)點是固定步長的，所以擴展方向決定了算法的效率，針對RRT 算法隨機樹擴展中的特點，可以采用目標(biāo)導(dǎo)向的方法減小隨機采樣的隨機性，使隨機樹更加傾向于目標(biāo)區(qū)域的方向生長。

目標(biāo)導(dǎo)向的具體操作方法是:人為地引導(dǎo)隨機點的生成，在產(chǎn)生隨機點qrand時，先以均勻概率原則獲得一個隨機概率值，如果這個概率值小于預(yù)先設(shè)置的閾值，則選取目標(biāo)狀態(tài)點作為循環(huán)中的qrand，即qrand=qgoal，如果概率值大于預(yù)先設(shè)置的閾值，則隨機選取狀態(tài)點qrand。

qrand在隨機樹構(gòu)建的過程中相當(dāng)于一個生長的方向，以一定的概率將狀態(tài)目標(biāo)點qgoal作為qrand，就可以讓隨機樹更加快速地朝向目標(biāo)區(qū)域擴展，達(dá)到快速收斂的效果。

2.3.2 路徑優(yōu)化

2）路徑點優(yōu)化

通過之前的分析可知，快速擴展隨機樹算法得到的規(guī)劃路徑往往不是最優(yōu)結(jié)果，為此，提出基于起始點導(dǎo)向的約束檢測原理以縮短隨機樹的規(guī)劃路徑。

路徑點優(yōu)化原理如圖8 所示:

圖8 路徑點的重新選取

其具體實現(xiàn)流程圖如圖9 所示:

圖9 路徑點優(yōu)化流程圖

2.4 仿真實驗與分析

為驗證改進RRT 算法在路徑規(guī)劃中的快速性、高效性和有效性，在仿真環(huán)境下設(shè)置機器魚為質(zhì)點，對RRT 算法和改進RRT 算法進行驗證。

2.4.1 RRT 仿真實驗

仿真環(huán)境設(shè)置為500×500 的平面區(qū)域，步長20。起始狀態(tài)點設(shè)置為［450 450］，用綠色圓標(biāo)識，目標(biāo)狀態(tài)點設(shè)置為［1 1］，用紅色圓標(biāo)識。在地圖中，障礙物的分布是簡單而有序的，為了驗證算法的可靠性，進行平面障礙的路徑規(guī)劃仿真，仿真實驗次數(shù)為100 次。

下頁圖10 所示為RRT 算法在地圖中的部分仿真結(jié)果，紅色路線為算法的規(guī)劃路徑，仿真結(jié)果驗證了RRT 算法作為全局搜索算法的優(yōu)點，由于是基于隨機采樣的路徑搜索算法，該算法即使在復(fù)雜地形中也不會出現(xiàn)陷入局部極小點的情況，但是其路徑的隨機性、不平滑性以及非最優(yōu)性也十分明顯。

2.4.2 改進RRT 仿真實驗

在相同地圖環(huán)境和參數(shù)下進行改進RRT 算法的仿真實驗進行對比，在改進算法中設(shè)置機器魚最大航偏角為60°，實驗次數(shù)為100 次。

圖10 RRT 算法路徑規(guī)劃仿真

圖11 改進RRT 算法路徑規(guī)劃仿真

圖11 所示為改進RRT 算法的部分仿真結(jié)果，紅色路線為算法的初步規(guī)劃路徑，黑色路線為算法的優(yōu)化路徑。由仿真結(jié)果可得，在改進的RRT 算法中隨機樹擴展效率極高，尤其通過路徑點的優(yōu)化，規(guī)劃路徑長度明顯下降且規(guī)劃路徑趨于穩(wěn)定。

2.5 仿真結(jié)果分析

在總共進行的200 次仿真實驗中，無論是基礎(chǔ)RRT 算法還是改進RRT 算法，均能夠完成路徑規(guī)劃的任務(wù)，圖12 和圖13 分別是200 次仿真實驗的路徑長度和規(guī)劃時間對比數(shù)據(jù)，橫坐標(biāo)為實驗的次數(shù)。

圖12 路徑長度對比數(shù)據(jù)

圖13 規(guī)劃時間對比數(shù)據(jù)

圖14 機器魚路徑規(guī)劃實驗

通過路徑長度和規(guī)劃時間對比數(shù)據(jù)可以發(fā)現(xiàn)，改進RRT 相比RRT 算法，路徑長度和規(guī)劃時間顯著下降，數(shù)據(jù)的波動幅度下降，算法穩(wěn)定性極大提升。

下頁表1 為具體的對比數(shù)據(jù)。

表1 仿真實驗數(shù)據(jù)對比

3 路徑規(guī)劃實驗

為了進一步驗證改進算法取得有效性，在搭建好的電磁驅(qū)動機器魚游動實驗平臺上進行路徑規(guī)劃實驗。實驗場地為2 m×2 m 的平面水域環(huán)境，障礙物布局如圖14 所示。

圖14 所示為機器魚在起始點、路徑點和目標(biāo)點位置的運動狀態(tài)，其中綠色圓點表示機器魚在起始點，藍(lán)色圓點表示路徑點，紅色圓點表示目標(biāo)點。通過多次實驗驗證，改進的RRT 算法可以快速、高效地規(guī)劃出避開障礙物，同時滿足機器魚運動性能約束的較優(yōu)路徑。

4 結(jié)論

通過對電磁驅(qū)動機器魚驅(qū)動器工作原理的分析，得到了電磁驅(qū)動器在機器魚不同工況下的驅(qū)動信號類型，并以此為根據(jù)設(shè)計機器魚路徑規(guī)劃方案。在機器魚的路徑規(guī)劃設(shè)計中，分析了經(jīng)典RRT算法路徑規(guī)劃的不足，引入目標(biāo)導(dǎo)向、路徑點優(yōu)化和路徑約束的方法對RRT 算法進行改進。最后，分別對兩種算法的路徑規(guī)劃進行了仿真驗證，實驗結(jié)果證明，所提出的改進RRT 算法比經(jīng)典RRT 算法在規(guī)劃時間和規(guī)劃路徑長度上均有明顯減小。但是算法仍存在許多問題及需要進一步改進的地方，在下一步研究中將嘗試結(jié)合局部優(yōu)化算法改進RRT算法，提升其動態(tài)規(guī)劃能力。