基于蟻群算法的深海著陸車路徑規(guī)劃

郭威，吳凱，周悅，孫洪鳴，徐高飛，高森

(1.中國(guó)科學(xué)院 深?？茖W(xué)與工程研究所，海南 三亞 572000；2.中國(guó)科學(xué)院大學(xué)，北京 100049；3.上海海洋大學(xué) 工程學(xué)院，上海 201306)

0 引言

海洋蘊(yùn)藏著豐富的資源，是維持人類社會(huì)可持續(xù)發(fā)展的重要戰(zhàn)略空間。自主式深海著陸車是一種能夠在海底環(huán)境下完成大范圍位置序列探測(cè)、定點(diǎn)位時(shí)間序列測(cè)量及精細(xì)化重載作業(yè)的可移動(dòng)海底探測(cè)平臺(tái)。深海著陸車受自身體積、質(zhì)量等限制，其所攜帶電池電量有限，從而對(duì)著陸車能否在有限能源條件下實(shí)現(xiàn)長(zhǎng)距離航行提出了挑戰(zhàn)。因此，如何在復(fù)雜三維海底環(huán)境中規(guī)劃出一條著陸車安全、能耗低、用時(shí)短的路徑，具有重要的工程應(yīng)用意義。

目前，水下機(jī)器人路徑規(guī)劃方法主要有遺傳算法、粒子群優(yōu)化算法、神經(jīng)網(wǎng)絡(luò)算法、蟻群算法等，而蟻群算法因具有反饋性好、魯棒性強(qiáng)、分布式計(jì)算能力強(qiáng)等優(yōu)點(diǎn)，在水下機(jī)器人路徑規(guī)劃領(lǐng)域得到廣泛應(yīng)用。劉貴杰等提出基于能耗最優(yōu)改進(jìn)蟻群算法，通過(guò)對(duì)自主式水下機(jī)器人(AUV)進(jìn)行水動(dòng)力分析得到能耗計(jì)算公式，以能耗作為啟發(fā)因子和評(píng)價(jià)函數(shù)指標(biāo)，一定程度上提高了AUV的續(xù)航能力。潘昕等提出基于遺傳螞蟻混合算法的AUV全局路徑規(guī)劃，以航路消耗、上浮或下潛風(fēng)險(xiǎn)等作為評(píng)價(jià)指標(biāo)，提高了路徑的經(jīng)濟(jì)性與安全性。張汝波等提出基于改進(jìn)蟻群算法的AUV路徑規(guī)劃，通過(guò)運(yùn)用蟻群間信息素調(diào)整方法避免了算法的停滯及信息素過(guò)于集中問(wèn)題，提高了航路的搜索能力；張楠楠等研究AUV在三維環(huán)境中改進(jìn)蟻群算法路徑規(guī)劃，以路徑距離、通行性及姿態(tài)角作為啟發(fā)函數(shù)來(lái)搜索最優(yōu)路徑，并將路徑長(zhǎng)度和光滑度作為評(píng)價(jià)指標(biāo)，一定程度上減少作業(yè)時(shí)的能耗。

本文針對(duì)自主式深海著陸車長(zhǎng)續(xù)航應(yīng)用需求，以能耗低和路徑短為優(yōu)化目標(biāo)，通過(guò)行駛阻力分析和驅(qū)動(dòng)電機(jī)能耗測(cè)量與分析來(lái)建立著陸車運(yùn)動(dòng)能耗模型，進(jìn)而開展基于蟻群尋優(yōu)的能耗- 距離路徑規(guī)劃算法研究。

1 深海三維空間抽象建模與路徑生成規(guī)則

三維海底空間復(fù)雜，建立合理的環(huán)境模型是開展深海著陸車路徑規(guī)劃的關(guān)鍵。

1.1 三維空間抽象建模

海底三維環(huán)境數(shù)據(jù)通過(guò)科考船測(cè)繪獲得，將其用于三維空間抽象建模：首先以三維空間環(huán)境(見圖1)左下角頂點(diǎn)作為原點(diǎn)建立坐標(biāo)系，其中軸、軸、軸分別對(duì)應(yīng)緯度、經(jīng)度、垂直于海平面的方向，且′、、分別為深海著陸車作業(yè)規(guī)劃空間3個(gè)方向的最大尺度，深海著陸車起點(diǎn)和終點(diǎn)分別位于平面和′′′′上，三維環(huán)境路徑規(guī)劃空間如圖1(a)所示；然后對(duì)三維路徑規(guī)劃空間采用柵格等分法劃分出路徑規(guī)劃所需的網(wǎng)格點(diǎn)，即沿著′將規(guī)劃的空間-′′′′進(jìn)行等分，得到+1個(gè)平面，再對(duì)+1個(gè)平面沿著、分別進(jìn)行和等分，獲得三維空間劃分點(diǎn)集，該集合中每一個(gè)點(diǎn)對(duì)應(yīng)著序號(hào)坐標(biāo)(,,)和位置坐標(biāo)(,,)，其中、、分別為沿′、、的劃分序號(hào)，∈0,1,…,-1,；∈0,1,…,-1,；∈0,1,…,-1,，平面劃分如圖1(b)所示。

圖1 三維空間抽象建模Fig.1 Three-dimensional abstract modeling

1.2 路徑生成規(guī)則

首先，規(guī)定著陸車搜索的可行點(diǎn)，以保證其安全爬行于海底表面。著陸車從當(dāng)前節(jié)點(diǎn)搜索至下一可行節(jié)點(diǎn)時(shí)，如果節(jié)點(diǎn)位于海底地形坡面，則該節(jié)點(diǎn)視為可行點(diǎn)(即安全點(diǎn))；其次，設(shè)置可達(dá)區(qū)域，以減少算法復(fù)雜度，具體可達(dá)區(qū)域設(shè)計(jì)如下：以軸為前進(jìn)方向，著陸車每向前爬行一步，允許最大橫移為max，最大爬坡或下坡高度為max。

著陸車坐于海底后，通過(guò)所攜帶的超短基線水聲定位獲得其初始位置點(diǎn)，操作員使用上位機(jī)通過(guò)水聲通信設(shè)置著陸車終點(diǎn)，著陸車控制系統(tǒng)在預(yù)先導(dǎo)入該海域三維海底環(huán)境的程序中載入起點(diǎn)、終點(diǎn)坐標(biāo)，執(zhí)行路徑規(guī)劃：著陸車從起點(diǎn)出發(fā)，首先到達(dá)平面可達(dá)域的某一可行點(diǎn)，然后從該點(diǎn)出發(fā)到達(dá)可達(dá)域的某一可行點(diǎn)，依次運(yùn)行至最后平面終點(diǎn)，連接起點(diǎn)與終點(diǎn)之間經(jīng)過(guò)的每一個(gè)網(wǎng)格點(diǎn)形成一條規(guī)劃路徑，如圖2所示。

圖2 路徑點(diǎn)選取規(guī)則Fig.2 Waypoint selection rules

2 能耗計(jì)算方法

2.1 行駛阻力計(jì)算

深海著陸車航行于環(huán)境惡劣的深海，行駛阻力除了由海流引起的流體阻力外，還將受到來(lái)自履帶底盤系統(tǒng)引起的內(nèi)摩擦阻力，各輪轉(zhuǎn)動(dòng)過(guò)程中與履帶接觸及履帶卷繞過(guò)程產(chǎn)生的阻力，以及深海著陸車在海底底質(zhì)作業(yè)過(guò)程中土壤受履帶擠壓而產(chǎn)生的土壤壓實(shí)阻力的水平分力。著陸車行駛受力如圖3所示。圖3中，為水阻力，為泥土阻力，為爬坡阻力，為驅(qū)動(dòng)力，為支撐力，為海底坡度角，為著陸車航行時(shí)的行駛速度，為著陸車潛伏深海的負(fù)浮力質(zhì)量，為重力加速度。

圖3 著陸車航行時(shí)受力圖Fig.3 Force diagram of landing vehicle during cruising

本文針對(duì)所在研究團(tuán)隊(duì)設(shè)計(jì)的深海著陸車樣機(jī)航行時(shí)路徑規(guī)劃問(wèn)題開展研究，著陸車主要技術(shù)參數(shù)見表1。

表1 主要技術(shù)參數(shù)Tab.1 Main technical parameters

211 水阻力

著陸車水下航行受到水阻力為

=2

(1)

式中：為水動(dòng)力系數(shù)；為海水密度。

212 泥土阻力

著陸車水下航行時(shí)受到泥土阻力為

=···cos

(2)

式中：為泥土阻力系數(shù)。

213 爬坡阻力

著陸車水下航行受到爬坡阻力為

=··sin

(3)

由表2參數(shù)，則行駛阻力為

表2 阻力參數(shù)Tab.2 Resistance parameters

=++=
2+···cos±··sin=
538125+98cos±392sin

(4)

式中：±表示著陸車爬坡與下坡時(shí)受到的阻力。由(4)式可知，當(dāng)行駛阻力≥0 N時(shí)，表示著陸車受到的合阻力與其前進(jìn)方向相反，阻礙著陸車運(yùn)動(dòng)；當(dāng)行駛阻力<0 N時(shí)，表示著陸車受到的合阻力與其前進(jìn)方向相同，此時(shí)合阻力將為著陸車提供動(dòng)力。

2.2 電機(jī)效率建模

本文著陸車采用雙側(cè)直流無(wú)刷電機(jī)后驅(qū)方式，通過(guò)差動(dòng)原理控制其運(yùn)動(dòng)。電機(jī)選用美國(guó)科爾摩根公司生產(chǎn)的TBM(S)型無(wú)框力矩電機(jī)，該電機(jī)為三相兩級(jí)繞組接無(wú)刷直流電機(jī)。由無(wú)刷直流電機(jī)機(jī)械特性可知，在一定轉(zhuǎn)速范圍內(nèi)，電機(jī)隨轉(zhuǎn)速提高機(jī)械效率逐漸提高，而無(wú)刷直流電機(jī)轉(zhuǎn)速又決定著陸車航行速度。在著陸車勻速航行時(shí)，考慮著陸車自身傳遞效率、打滑率以及減速器傳遞效率，著陸車消耗在驅(qū)動(dòng)電機(jī)的能量為著陸車克服行駛阻力、履帶打滑、自身傳遞損耗和減速器損耗做功，因此電機(jī)效率也決定了著陸車工作效率。

搭建TBM-12941B電機(jī)測(cè)試平臺(tái)，測(cè)得電機(jī)在任意轉(zhuǎn)速、不同轉(zhuǎn)矩下的機(jī)械輸出效率關(guān)系如圖4所示。經(jīng)查其技術(shù)手冊(cè)得知減速比為10∶1的減速器傳遞效率為93，著陸車在稀軟海底上航行時(shí)打滑率13，以及考慮著陸車自身傳遞效率，則電機(jī)在額定轉(zhuǎn)矩75 N·m下著陸車任意速度的工作效率曲線如圖5所示。

圖4 電機(jī)轉(zhuǎn)速、轉(zhuǎn)矩與輸出效率關(guān)系圖Fig.4 Relation diagram of motor speed，torque and output efficiency

圖5 著陸車速度與工作效率關(guān)系圖Fig.5 Velocity and working efficiencyof landing vehicle

采用4次函數(shù)擬合圖5散點(diǎn)線，得出著陸車航行速度(0 m/s<<095 m/s)與工作效率的函數(shù)為

=-16580+32548-
21955+6179+0031

(5)

2.3 能耗計(jì)算

忽略深海環(huán)境海流、著陸車航行時(shí)轉(zhuǎn)彎、打滑、加減速等因素對(duì)能耗的影響，假設(shè)著陸車航行始終以勻速度從起始點(diǎn)出發(fā)，經(jīng)過(guò)爬坡越障最終行駛至終點(diǎn)。著陸車勻速航行消耗能量為

=

(6)

式中：為著陸車到達(dá)終點(diǎn)行駛的里程。

將(4)式、(5)式代入(6)式，得

(7)

式中：為著路車到達(dá)終點(diǎn)所遍歷的節(jié)點(diǎn)數(shù)；表示從當(dāng)前點(diǎn)到下一可行節(jié)點(diǎn)的能源消耗；表示當(dāng)前點(diǎn)到下一可行節(jié)點(diǎn)的距離。

由(7)式可知，航行時(shí)著陸車能耗僅與行駛速度、里程、地形坡度有關(guān)。根據(jù)(7)式可得如圖6和圖7所示單位里程能耗關(guān)系。

圖6 爬坡時(shí)速度、坡度和能耗關(guān)系圖Fig.6 Relationship between speed，slope and energy consumption during climbing

圖7 下坡時(shí)速度、坡度和能耗關(guān)系圖Fig.7 Relationship between speed，slope and energy consumption during going downhill

由圖6和圖7可知：當(dāng)著陸車爬坡時(shí)，單位里程能耗隨爬坡角度、車速的增加而增加，其中車速對(duì)著陸車能耗影響較大；下坡時(shí)，著陸車單位里程能耗雖然隨下坡角度增大而降低，但車速增加著陸車能耗依然增加。依據(jù)(7)式，綜合考慮深海著陸車工作的安全性、效率與能耗，將車速設(shè)定為05 m/s，爬坡角度限制在30°以內(nèi)。

3 蟻群算法路徑規(guī)劃

3.1 信息素更新

蟻群算法使用信息素誘導(dǎo)螞蟻搜索下一可行節(jié)點(diǎn)。由于僅采用全局信息素更新方式的傳統(tǒng)蟻群算法，正反饋機(jī)制存在算法易陷入局部最優(yōu)的缺點(diǎn)，本文采用局部信息素和全局信息素更新方式，通過(guò)局部信息素更新，使螞蟻有更大概率選擇上輪搜索路徑之外的可行節(jié)點(diǎn)，同時(shí)限制信息素值區(qū)間，增強(qiáng)算法全局尋優(yōu)能力，具體更新規(guī)則如下：

局部信息素更新是指螞蟻從節(jié)點(diǎn)到達(dá)下一節(jié)點(diǎn)后，對(duì)該段路徑信息素更新，公式為

()=(1-)(-1)

(8)

式中：()為時(shí)刻路徑上的信息素值；為局部信息素衰減系數(shù)。

蟻群完成第次迭代后，通過(guò)評(píng)價(jià)函數(shù)對(duì)全局信息素進(jìn)行更新，公式為

(9)

(10)

式中：為全局信息素衰減系數(shù)；為信息素濃度常數(shù)；()為第次迭代蟻群找到最佳路徑的評(píng)價(jià)函數(shù)。另外，每個(gè)節(jié)點(diǎn)信息素值限定在[,]之間，用以避免算法過(guò)早收斂，用以避免算法擴(kuò)散。

3.2 啟發(fā)函數(shù)設(shè)計(jì)

蟻群算法使用信息素和啟發(fā)函數(shù)進(jìn)行轉(zhuǎn)移概率計(jì)算來(lái)決定蟻群的路徑選擇。在時(shí)刻螞蟻從當(dāng)前節(jié)點(diǎn)到達(dá)下一節(jié)點(diǎn)的轉(zhuǎn)移概率公式為

(11)

式中：()為啟發(fā)函數(shù)；、分別為信息素和啟發(fā)信息權(quán)重；為螞蟻從點(diǎn)到下一步可達(dá)域內(nèi)可行點(diǎn)點(diǎn)集。

本文啟發(fā)函數(shù)設(shè)計(jì)考慮局部距離、全局距離、安全性和路徑能耗4個(gè)因素，啟發(fā)函數(shù)設(shè)計(jì)如下：

(12)

式中：為當(dāng)前點(diǎn)到下一點(diǎn)路徑長(zhǎng)度的倒數(shù)值；為安全性因素，當(dāng)選擇為可行節(jié)點(diǎn)時(shí)值為1，否則為0；為下一點(diǎn)到目標(biāo)點(diǎn)路徑長(zhǎng)度的倒數(shù)值；為當(dāng)前點(diǎn)到下一點(diǎn)能耗的倒數(shù)值；、、、分別為相應(yīng)權(quán)重。

1) 局部距離啟發(fā)信息：促使蟻群選擇距離最短的點(diǎn)，計(jì)算公式如下：

(13)

2) 安全性啟發(fā)信息：使得蟻群選擇可達(dá)域內(nèi)的可行點(diǎn)，計(jì)算公式如下：

(14)

3) 全局距離啟發(fā)信息：使得蟻群搜索具有方向性，增強(qiáng)算法搜索速度，計(jì)算公式如下：

(15)

4) 路徑能耗啟發(fā)信息：使得蟻群選擇低能耗的點(diǎn)，計(jì)算公式如下：

=1=1[(2+
···cos±··sin)/]

(16)

3.3 適應(yīng)度函數(shù)設(shè)計(jì)

傳統(tǒng)路徑規(guī)劃多以最短路徑作為規(guī)劃目標(biāo)。然而，著陸車路徑規(guī)劃過(guò)程中不僅考慮路徑長(zhǎng)度，還要考慮能耗和運(yùn)行時(shí)間等，為此本文提出能耗- 距離路徑規(guī)劃算法，即采用加權(quán)的方法，在保證著陸車安全行駛的同時(shí)實(shí)現(xiàn)路徑里程與能耗問(wèn)題的均衡，設(shè)計(jì)如下適應(yīng)度函數(shù)：

()=()+()

(17)

式中：()和()為螞蟻第次迭代路徑總長(zhǎng)度和總能耗因素；、為相應(yīng)權(quán)重。下面分別對(duì)路徑長(zhǎng)度和能耗進(jìn)行討論。

1) 路徑總長(zhǎng)度因素：反映了算法對(duì)最短路徑的追求，設(shè)計(jì)如下：

(18)

2) 路徑總能耗因素：反映了算法對(duì)低能耗的追求，設(shè)計(jì)如下：

(19)

3.4 蟻群算法路徑規(guī)劃步驟

三維環(huán)境建模。導(dǎo)入三維海底數(shù)據(jù)及起點(diǎn)和終點(diǎn)坐標(biāo)，對(duì)著陸車工作空間進(jìn)行抽象建模與劃分。

參數(shù)初始化。初始化信息素權(quán)重，信息素濃度常數(shù)，啟發(fā)信息權(quán)重，啟發(fā)函數(shù)系數(shù)、、、，信息素衰減系數(shù)，評(píng)價(jià)函數(shù)權(quán)重、，初始信息素，螞蟻數(shù)量只，最大迭代次數(shù)。

蟻群搜索最優(yōu)路徑。依據(jù)(11)式計(jì)算螞蟻從當(dāng)前節(jié)點(diǎn)到達(dá)下一可行節(jié)點(diǎn)的轉(zhuǎn)移概率值，只螞蟻根據(jù)轉(zhuǎn)移概率值依次從起點(diǎn)處出發(fā)搜索下一個(gè)可達(dá)域內(nèi)的可行節(jié)點(diǎn)。

更新局部信息素值。每次螞蟻到達(dá)下一個(gè)可行節(jié)點(diǎn)后，按(8)式進(jìn)行局部信息素值更新。

每代蟻群找出的最佳路徑。第次迭代只螞蟻完成一次搜索后，根據(jù)(17)式對(duì)當(dāng)代蟻群經(jīng)歷的所有路徑進(jìn)行評(píng)價(jià)，保存最佳評(píng)價(jià)值和最佳路徑的所有節(jié)點(diǎn)。

更新全局信息素值。對(duì)所有節(jié)點(diǎn)按(9)式和(10)式進(jìn)行全局信息素值更新。

完成路徑規(guī)劃。每迭代一次迭代器加1，若未達(dá)到，則重復(fù)步驟1～步驟6。最終得到最優(yōu)路徑。

4 算法仿真及分析

為驗(yàn)證能耗- 距離路徑規(guī)劃算法的有效性，本文將10 km×10 km×50 m的三維海底模型劃分為100×100×50大小相同的柵格，設(shè)置相同起始點(diǎn)和終點(diǎn)進(jìn)行多次仿真，仿真參數(shù)見表3。

表3 仿真參數(shù)取值Tab.3 Simulation parameter selection

在MATLAB 2018a軟件環(huán)境中，分別用傳統(tǒng)路徑最短、傳統(tǒng)能耗最低和路徑能耗- 距離優(yōu)化3種方案進(jìn)行50次仿真。其中某次3種方案路徑規(guī)劃結(jié)果如圖8所示，50次仿真路徑長(zhǎng)度與能耗對(duì)比分別如圖9和圖10所示，仿真數(shù)據(jù)平均值統(tǒng)計(jì)結(jié)果見表4。

圖8 路徑規(guī)劃結(jié)果Fig.8 Path planning results

圖9 路徑長(zhǎng)度對(duì)比圖Fig.9 Path length comparison chart

圖10 路徑能耗對(duì)比圖Fig.10 Path energy consumption comparison chart

表4 3種算法仿真數(shù)據(jù)統(tǒng)計(jì)對(duì)比Tab.4 Statistical comparison of algorithm simulation data

在勻速運(yùn)動(dòng)情況下，作業(yè)時(shí)間與路徑長(zhǎng)度呈正比。根據(jù)深海著陸車作業(yè)需求，本文算法通過(guò)合理選取路徑權(quán)重和能耗權(quán)重來(lái)均衡作業(yè)的里程與能耗。仿真結(jié)果表明：本文能耗- 距離路徑規(guī)劃算法相對(duì)于傳統(tǒng)路徑最短規(guī)劃算法，其搜索效率提升2581，路徑長(zhǎng)度雖增加400，但能耗卻降低673；相對(duì)于傳統(tǒng)能耗最低路徑規(guī)劃算法，能耗- 距離路徑規(guī)劃算法搜索效率提升1154，能耗雖增加了386，但路徑長(zhǎng)度卻減少226。以上結(jié)果表明，本文算法在實(shí)際應(yīng)用中通過(guò)合理選取評(píng)價(jià)函數(shù)權(quán)重參數(shù)，能有效均衡著陸車航行時(shí)能源消耗與作業(yè)里程，可以很好滿足著陸車長(zhǎng)續(xù)航作業(yè)需求。

5 結(jié)論

本文針對(duì)自主式深海著陸車所面臨的作業(yè)需求，建立了三維海底環(huán)境模型和著陸車航行能耗模型，進(jìn)而提出了一種基于蟻群優(yōu)化的能耗- 距離路徑規(guī)劃算法。得出主要結(jié)論如下：

1) 本文所設(shè)計(jì)的路徑規(guī)劃算法能夠保證著陸車在較短路徑下有效減少路徑能耗，滿足著陸車長(zhǎng)時(shí)間、大范圍探測(cè)與作業(yè)需求，促進(jìn)我國(guó)對(duì)深海資源開發(fā)和海底觀測(cè)網(wǎng)建設(shè)的進(jìn)程。

2) 由于水下復(fù)雜多變的三維地理環(huán)境，著陸車海底路徑規(guī)劃在后續(xù)研究中將充分考慮海底底質(zhì)、打滑率、海流干擾、航行轉(zhuǎn)角、加減速等因素對(duì)路徑能耗的影響。