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

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

0 引言

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

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

1 電磁驅(qū)動(dòng)機(jī)器魚(yú)

1.1 電磁驅(qū)動(dòng)原理

圖1 機(jī)器魚(yú)總體裝配

圖2 尾部裝配圖

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

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

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

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

圖4 電磁驅(qū)動(dòng)控制信號(hào)

1.2 電磁驅(qū)動(dòng)機(jī)器魚(yú)控制系統(tǒng)

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

2 RRT 算法

2.1 RRT 算法描述

圖5 控制系統(tǒng)

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

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

2.1.1 隨機(jī)樹(shù)構(gòu)建

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

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

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

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

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

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

2.1.2 路經(jīng)查詢

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

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

因隨機(jī)樹(shù)在隨機(jī)采樣狀態(tài)點(diǎn)的過(guò)程中，其添加的新?tīng)顟B(tài)點(diǎn)符合機(jī)器魚(yú)的約束條件，因此，最終得到的規(guī)劃路徑也是合理的、符合約束條件的。

2.2 RRT 算法分析

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

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

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

3）緩慢性:隨機(jī)樹(shù)的擴(kuò)展是通過(guò)隨機(jī)選取狀態(tài)點(diǎn)的方式，擴(kuò)展方向是隨機(jī)的，因此，隨機(jī)樹(shù)對(duì)目標(biāo)狀態(tài)點(diǎn)的收斂速度可能會(huì)十分緩慢。

2.3 RRT 算法的改進(jìn)

2.3.1 收斂加速

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

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

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

2.3.2 路徑優(yōu)化

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

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

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

圖8 路徑點(diǎn)的重新選取

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

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

2.4 仿真實(shí)驗(yàn)與分析

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

2.4.1 RRT 仿真實(shí)驗(yàn)

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

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

2.4.2 改進(jìn)RRT 仿真實(shí)驗(yàn)

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

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

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

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

2.5 仿真結(jié)果分析

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

圖12 路徑長(zhǎng)度對(duì)比數(shù)據(jù)

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

圖14 機(jī)器魚(yú)路徑規(guī)劃實(shí)驗(yàn)

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

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

表1 仿真實(shí)驗(yàn)數(shù)據(jù)對(duì)比

3 路徑規(guī)劃實(shí)驗(yàn)

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

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

4 結(jié)論

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