深海著陸車路徑規(guī)劃及跟蹤控制方法

周球，周悅，孫洪鳴，郭威，吳凱，蘭彥軍

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

0 引言

深海作為人類亟待探索的重要資源寶庫，蘊(yùn)藏著豐富的生物資源和化學(xué)能源，尤其是研究生活在深海熱液和冷泉區(qū)的噬壓、噬氧以及化能合成微生物等特殊的生命現(xiàn)象，對探索地球地質(zhì)演化和生命起源具有重要的意義［1-2］。深海著陸車(DSLV) 作為一種將履帶底盤與傳統(tǒng)水下機(jī)器人相結(jié)合的新型水下裝備，具備大范圍移動考察、定點(diǎn)精細(xì)化和長周期自主作業(yè)的能力［2］，可完成對海底資源的自主探測。

DSLV 要求在自攜帶能源有限條件下實現(xiàn)長距離作業(yè)，為此需要規(guī)劃較短參考路徑以節(jié)省能耗、提高作業(yè)范圍。同時，為實時、高效跟蹤規(guī)劃的路徑，需要采用合適的跟蹤算法。模型預(yù)測控制(MPC)算法由于可以根據(jù)車輛當(dāng)前狀態(tài)量信息計算得到預(yù)測時域內(nèi)的狀態(tài)量信息、實現(xiàn)對目標(biāo)函數(shù)的在線滾動優(yōu)化等優(yōu)點(diǎn)，逐步引起學(xué)術(shù)界重視［3-5］。文獻(xiàn)［6］基于輪胎非線性特性影響，提出考慮輪胎側(cè)向力計算誤差的自適應(yīng)MPC 算法，提高了未知工況下路徑跟蹤控制的性能;文獻(xiàn)［7］綜合考慮動靜態(tài)障礙物和人類駕駛行為，提出了一種用于車輛軌跡跟蹤控制的線性時變MPC 控制方案，提高了路徑跟蹤控制的穩(wěn)定性;文獻(xiàn)［8］基于級聯(lián)控制策略和MPC 算法設(shè)計了自主水下航行器(AUV) 控制器，減少了路徑跟蹤時舵角飽和現(xiàn)象;文獻(xiàn)［9］結(jié)合經(jīng)驗公式和和神經(jīng)網(wǎng)絡(luò)算法調(diào)整MPC 的預(yù)瞄步數(shù)和步長，降低了跟蹤誤差。上述方案均在不同方面提高了路徑跟蹤的性能，但未考慮到目標(biāo)函數(shù)中固定的權(quán)重矩陣無法同時兼顧穩(wěn)定性和跟蹤精度，難以在跟蹤誤差不斷變化的情況下取得最佳跟蹤效果。

根據(jù)海底具體環(huán)境，本文提出一種變參數(shù)蟻群優(yōu)化(VACO) 算法和根據(jù)跟蹤距離誤差進(jìn)行自適應(yīng)權(quán)重調(diào)節(jié)的模型預(yù)測控制(AWMPC) 算法，進(jìn)行DSLV 自主路徑規(guī)劃與跟蹤，降低了規(guī)劃路徑長度，提高了路徑跟蹤的精度。

1 DSLV 運(yùn)動學(xué)模型

本文研制的DSLV 采用雙電機(jī)后驅(qū)方式，通過差動方式控制DSLV 前進(jìn)、后退、轉(zhuǎn)向運(yùn)動。雙側(cè)電機(jī)驅(qū)動示意圖如圖1 所示。

圖1 雙側(cè)電機(jī)驅(qū)動示意圖Fig.1 Schematic diagram of the dual motor drive

在笛卡爾坐標(biāo)系OXYZ 下，令xc、yc分別為DSLV 幾何中心點(diǎn)的橫坐標(biāo)和縱坐標(biāo)，φ 為方向角，vl和vr分別為左右履帶線速度，v 為爬行速度，d 為左右履帶的中心距，D 為車身寬度，rd為驅(qū)動輪半徑，rl和rr為左右履帶瞬時轉(zhuǎn)向半徑，oc為瞬心，ω 為轉(zhuǎn)向角速度。其差速運(yùn)動示意圖如圖2 所示。

圖2 DSLV 差速運(yùn)動示意圖Fig.2 Diagram of DSLV differential motion

據(jù)“深海勇士”號和“奮斗者”號載人潛水器的作業(yè)經(jīng)驗，較平坦的深海海底主要分為沙質(zhì)底和泥質(zhì)底兩種，除馬里亞納海溝外，其他海域這兩種底質(zhì)潛水器沉降不大。項目組設(shè)計的DSLV 在海底的負(fù)浮力質(zhì)量約為50 kg，為應(yīng)對DSLV 著底時受到的沖擊力，對履帶結(jié)構(gòu)的張緊輪支架進(jìn)行了改進(jìn)［10］。為此，DSLV 選擇在較為平坦的沙質(zhì)底作業(yè)，且爬行速度較慢(約為0.5 m/s)，忽略Z 軸方向的運(yùn)動，并假設(shè)DSLV 質(zhì)心與幾何中心重合［11］，則由圖2 可得

式中:vx和vy分別為DSLV 在x 軸和y 軸的速度分量;sl、sr分別為DSLV 左右兩側(cè)履帶的滑轉(zhuǎn)率;ωl和ωr分別為DSLV 左右兩側(cè)驅(qū)動輪角速度。

DSLV 在做轉(zhuǎn)向運(yùn)動時，由于左右履帶的瞬心相同，瞬時圓周運(yùn)動的角速度ω 相同，旋轉(zhuǎn)半徑不同，則d 可表示為

進(jìn)而可得

選取狀態(tài)量ξ=［xcycφ］T，控制量u=［v ω］T，構(gòu)建其運(yùn)動學(xué)模型為

其一般形式可表示為

2 DSLV 路徑規(guī)劃與跟蹤控制

DSLV 路徑規(guī)劃與跟蹤控制是根據(jù)海底信息，在兩個作業(yè)點(diǎn)之間采用VACO 算法規(guī)劃出符合行駛條件的最短路徑，經(jīng)過4 階B 樣條曲線平滑后作為參考路徑;將實際狀態(tài)量ξ 與參考狀態(tài)量ξr輸入到AWMPC 控制器中，并根據(jù)跟蹤距離誤差E 的大小對目標(biāo)函數(shù)的控制量誤差增量權(quán)重矩陣R 進(jìn)行自適應(yīng)調(diào)整，通過求解目標(biāo)函數(shù)得出最優(yōu)控制序列ΔU*，并將第一個時刻的控制增量輸入AWMPC 控制器中［12］。通過實時控制DSLV 的爬行速度和角速度，使之平穩(wěn)、安全的跟蹤規(guī)劃的路徑。系統(tǒng)總體設(shè)計框架如圖3 所示。

圖3 VACO-AWMPC 總體設(shè)計框架Fig.3 Overall design framework of VACO-AWMPC

2.1 VACO 路徑規(guī)劃

蟻群優(yōu)化(ACO)算法具有魯棒性強(qiáng)和分布式計算的特點(diǎn)［13-14］，在進(jìn)行路徑規(guī)劃時，螞蟻?在t 時刻從點(diǎn)i 轉(zhuǎn)移到點(diǎn)j 的概率為

式中: ?={1，2，3，…，σ}，σ 為蟻群數(shù)量;τij(t) 表示螞蟻在t 時刻從點(diǎn)i 到點(diǎn)j 路徑上殘留的信息素濃度;μij(t)=1/dij為啟發(fā)函數(shù)，表示螞蟻從點(diǎn)i 到點(diǎn)j的期望程度，dij為點(diǎn)i 到點(diǎn)j 的距離;α 為殘留信息素的重要程度;β 為啟發(fā)函數(shù)的重要程度;Ω?為螞蟻?在t 時刻從點(diǎn)i 允許到達(dá)點(diǎn)的集合。考慮到μij(t)的取值使得ACO 在路徑規(guī)劃時轉(zhuǎn)移概率差異不大，在尋優(yōu)過程中需要較多次迭代，本文將啟發(fā)函數(shù)μij(t) 的取值修改為(1/dij)2，增大不同步長的信息素差異，以提高ACO 的尋優(yōu)效率。

信息素更新公式如下:

式中:ρ 為揮發(fā)因子，取(0，1) 之間的常數(shù);Δτij表示本次迭代中所有螞蟻在點(diǎn)i 到點(diǎn)j 路徑上釋放的信息素濃度之和;表示螞蟻?在點(diǎn)i 到點(diǎn)j 的路徑上釋放的信息素濃度;G 為信息素增加強(qiáng)度系數(shù);L?為本次循環(huán)中螞蟻?走過的路徑總長度。

由于信息素?fù)]發(fā)因子ρ 的大小決定了蟻群在尋找最短路徑的過程中信息素?fù)]發(fā)的快慢程度，針對ACO 中ρ 恒定，無法兼顧收斂速度和易陷入局部最優(yōu)解的問題，本文提出將反正切函數(shù)進(jìn)行平移、伸縮和翻轉(zhuǎn)變換后替代ACO 中的恒值ρ。由于反正切函數(shù)經(jīng)過翻轉(zhuǎn)后隨自變量的增加而減小。在算法迭代次初期，ρ 較大，前面螞蟻留下的信息素會很快揮發(fā)，后續(xù)螞蟻受到前面螞蟻留下信息素的影響較小，會以更高的概率選擇未走過的路徑，能夠擴(kuò)大搜索范圍，降低陷入局部最短路徑的概率。在算法迭代后期，ρ 較小，前面螞蟻留下的信息素?fù)]發(fā)較慢，后續(xù)螞蟻受到前面螞蟻留下信息素的影響較大，使得算法以更快的速度收斂，減少找到最短路徑所需的迭代次數(shù)，VACO 中揮發(fā)因子ρ 為

式中: e 為縱坐標(biāo)平移系數(shù);a 為縱坐標(biāo)放大倍數(shù);ζ 為迭代次數(shù);b 為橫坐標(biāo)平移系數(shù);c 為橫坐標(biāo)縮放比例。VACO 中ρ 的值能夠隨著ζ 的變化進(jìn)行動態(tài)調(diào)整，使得算法能夠較快收斂且不易陷入局部最優(yōu)。

路徑規(guī)劃與路徑長度、所需時間、可行性、及安全性等多種因素有關(guān)。由于深海著陸車在海底作業(yè)時自身攜帶能源有限，需要在安全的前提下重點(diǎn)考慮降低規(guī)劃路徑的長度以節(jié)省能耗，而安全性通過與障礙間預(yù)留安全距離來保證，其他因素不做考慮。為獲取最短路徑，設(shè)路徑規(guī)劃目標(biāo)函數(shù)為

式中:Lz為第z 次迭代的最短路徑長度，Lz=min L?，其中為目標(biāo)點(diǎn)，MO為規(guī)劃路徑的起始點(diǎn);γ 為算法迭代次數(shù)。

2.2 MPC 算法

MPC 是將DSLV 的運(yùn)動學(xué)模型經(jīng)過線性化、離散化后遞推得到預(yù)測模型，然后在k 時刻通過求解帶約束條件的目標(biāo)函數(shù)得到最優(yōu)控制序列ΔU*，并將控制序列的第一個控制增量輸入控制器中，然后至k+1 時刻，重復(fù)上述過程，滾動完成路徑跟蹤過程［15-16］，MPC 路徑跟蹤控制示意圖如圖4 所示。圖4 中: 令Np為預(yù)測時域，Nc為控制時域(Np≥Nc)，ξmax和ξmin為狀態(tài)量約束為控制增量約束。

圖4 MPC 路徑跟蹤示意圖Fig.4 Schematic diagram of MPC path tracking

2.2.1 非線性模型線性化

由式(2) 可知，DSLV 運(yùn)動學(xué)模型是一個典型的非線性系統(tǒng)，考慮到線性模型具有易于求解、計算速度快等優(yōu)點(diǎn)［17-18］，將式(3) 進(jìn)行線性化，使用Taylor公式將式(3) 在VACO 規(guī)劃的參考路徑上某點(diǎn)r 處進(jìn)行1 階Taylor 展開并忽略高階項［19-20］，可得

2.2.2 離散化

采用Forward Euler 方法對式(11) 進(jìn)行離散化，設(shè)采樣時間為T，則式(11) 可表示為

由式(12) 移項可得離散化線性時變模型為

為了使DSLV 平穩(wěn)運(yùn)行，需對控制量誤差增量進(jìn)行約束，為此引入控制量誤差增量，構(gòu)建增廣狀態(tài)量如下:

則由式(13)、式(14) 得

2.2.3 預(yù)測模型

令Np為預(yù)測時域，Nc為控制時域(Np≥Nc)，則預(yù)測時域內(nèi)的狀態(tài)量誤差為

2.2.4 目標(biāo)函數(shù)

本文建立的跟蹤目標(biāo)函數(shù)為

式中:第1 項為狀態(tài)誤差，表示路徑跟蹤能力，其中Q1為狀態(tài)誤差加權(quán)矩陣為單個狀態(tài)誤差加權(quán)矩陣;第2 項為輸出控制增量，表示系統(tǒng)的能量消耗，其中R1為控制增量加權(quán)矩陣，R1=R?為單個控制增量加權(quán)矩陣。

考慮到兩個權(quán)重參數(shù)的相對性，為簡化起見，本文取Q1為固定參數(shù)的主對角線矩陣?？紤]到恒定矩陣R 無法兼顧不同跟蹤距離誤差情況下的跟蹤性能，本文提出根據(jù)跟蹤距離誤差對R 進(jìn)行自適應(yīng)調(diào)節(jié)，即當(dāng)跟蹤距離誤差E 小于閾值L 時，R 能夠隨著E 增大而增大;否則，若初始時刻DSLV 不在規(guī)劃路徑上，則此時E 會比較大，將會使R 很大，可能造成系統(tǒng)難以收斂，跟蹤失效。為此，根據(jù)系統(tǒng)控制輸出能力和提高穩(wěn)定性的需求，對R 進(jìn)行限幅，即當(dāng)E大于設(shè)定閾值L 時，將其設(shè)為一個固定值。R 調(diào)節(jié)公式如下:

式中:a1、b1、L 均為常數(shù);e 為自然常數(shù)。

為便于計算機(jī)求解，將式(18) 變換為如下二次規(guī)劃問題:

式中:H=ΘTQ1Θ+R1;g=ETQ1Θ，E=ψh(k) 。

對于控制量誤差及其增量，有如下遞推式:

考慮到DSLV 由左右兩側(cè)電機(jī)通過驅(qū)動輪帶動履帶進(jìn)行運(yùn)動，在設(shè)計跟蹤目標(biāo)函數(shù)時為防止控制量劇烈變化引起系統(tǒng)的機(jī)械沖擊以及跟蹤過程中偏離參考控制量［21］，為此加入控制量誤差增量約束和控制量誤差約束。設(shè)控制量誤差增量約束的上下限分別為ΔUmax和ΔUmin，控制量誤差約束的上下限分別為Umax和Umin，控制量和控制增量約束分別為同理。

2.2.5 反饋機(jī)制

根據(jù)式(20) 進(jìn)行求解后，得到如下控制增量序列:

在每一個控制周期內(nèi)，實時獲取參考路徑與DSLV 狀態(tài)量信息，然后重復(fù)上述過程，實現(xiàn)對規(guī)劃路徑的有效跟蹤。

3 算法仿真與分析

DSLV 在復(fù)雜海底通過定制的深海相機(jī)和MD750D 前視聲納系統(tǒng)獲得海底環(huán)境信息，并借助定制的深海電機(jī)驅(qū)動履帶進(jìn)行差速運(yùn)動，然后通過Evologics 超短基線結(jié)合Waterlink A50 型多普勒和電機(jī)配備的模擬霍爾傳感器計算自己的位置信息和速度，基于美國霍尼韋爾公司生產(chǎn)的HMR3200 型電子羅盤獲取方向角信息，結(jié)合VACO-AWMPC 算法完成路徑規(guī)劃與跟蹤。

在路徑跟蹤時，采用柵格圖法構(gòu)建海底環(huán)境的二維簡化模型。為便于計算將DSLV 視作質(zhì)點(diǎn)，考慮到DSLV 容易因自身體積與障礙物發(fā)生碰撞，為此根據(jù)DSLV 物理尺寸與預(yù)留安全距離之和來膨脹障礙物［22］，即障礙物膨脹增量為DSLV 車身寬度一半與安全距離之和，以簡化路徑規(guī)劃時的計算量。

為驗證VACO 的有效性，將ACO 的尋優(yōu)效果作為對照組，在含有靜態(tài)障礙物30 m ×30 m 區(qū)域進(jìn)行仿真實驗，設(shè)置路徑規(guī)劃的起始坐標(biāo)點(diǎn)為Mo(0.5，29.5)，目標(biāo)坐標(biāo)點(diǎn)為Me(29.5，0.5)，預(yù)留安全距離為1 m，取G=10，σ=50，α=1，β=7，γ=100，ρ=0.3。VACO 中ρ 的取值和μij(t) 的取值如2.1 節(jié)所示，其中參數(shù)a、b、c、e 取值分別為0.15、30、10、0.5，其余同ACO。某次路徑規(guī)劃試驗結(jié)果對比如圖5 所示，為不失一般性，分別對ACO 與VACO 進(jìn)行30 次仿真實驗，實驗結(jié)果的平均值如表1 所示。

圖5 路徑規(guī)劃結(jié)果對比Fig.5 Comparison of path planning results

表1 仿真實驗結(jié)果平均值對比Table 1 Comparison of mean values of simulation results

由圖5 與表1 可見，采用VACO 規(guī)劃的路徑長度明顯降低，收斂所需的迭代次數(shù)更少，路徑長度和收斂時迭代次數(shù)分別降低4.60%和56.57%。

考慮到VACO 規(guī)劃的路徑在轉(zhuǎn)向處角度較大，不利于DSLV 在路徑跟蹤過程中的平穩(wěn)運(yùn)行，對蟻群算法規(guī)劃的路徑進(jìn)行4 階B 樣條曲線平滑處理作為參考路徑［23］。設(shè)平滑后參考路徑上某點(diǎn)坐標(biāo)為(xr，yr)，其下一個采樣點(diǎn)坐標(biāo)為(xr+1，yr+1)，根據(jù)公式arctan ［(yr+1-yr)/(xr+1-xr) ］計算參考點(diǎn)(xr，yr) 處的參考方向角。

設(shè)給定的 DSLV 初始狀態(tài)量 ξo=［0m 28 m π/3 m］T，按照平滑處理后的規(guī)劃路徑及參考位置方向角，進(jìn)行DSLV 跟蹤控制。預(yù)測模型參數(shù)及約束列于表2 中。

表2 預(yù)測模型參數(shù)及配置Table 2 Parameters and configuration of the predictive model

當(dāng)采用MPC 算法時R 為固定矩陣，取8I2×2;當(dāng)采用AWMPC 算法時自適應(yīng)權(quán)重矩陣R 取值見式(19)，分別對兩種情況進(jìn)行仿真實驗。為便于觀察，對仿真結(jié)果進(jìn)行了局部放大，路徑跟蹤仿真實驗結(jié)果如圖6～圖10 所示。

圖6 路徑跟蹤結(jié)果對比Fig.6 Comparison of path tracking results

圖7 狀態(tài)量跟蹤控制效果對比Fig.7 Comparison of state quantity tracking control effects

圖8 狀態(tài)量誤差變化對比Fig.8 Comparison of state quantity error change

圖9 控制量變化效果對比Fig.9 Comparison of control quantity change effect

圖10 跟蹤距離誤差對比Fig.10 Comparison of tracking distance error

由圖6 可見，起始時刻DSLV 并不在規(guī)劃路徑上，DSLV 開始運(yùn)行后，逐漸向規(guī)劃路徑靠近，最終都能達(dá)到較好的跟蹤效果，但采用AWMPC 算法跟蹤精度明顯更高。

由圖7 和圖8 可見，采用AWMPC 算法跟蹤路徑時橫向、縱向位置誤差以及方向角跟蹤誤差更小。

由圖9 可以看出，DSLV 在轉(zhuǎn)向時速度會出現(xiàn)波動，但完成轉(zhuǎn)向后會迅速恢復(fù)穩(wěn)定，采用AWMPC算法在自適應(yīng)權(quán)重矩陣R 切換初期出現(xiàn)了線速度和角速度波動，但始終保持在可接受范圍內(nèi)。

由圖10 可見，采用AWMPC 算法的跟蹤距離誤差更低。在包含起始位置誤差的情況下，兩種方式下平均跟蹤距離誤差分別為0.215 9 m 和0.113 1 m，降低47.6%。

4 結(jié)論

本文用反正切函數(shù)取代ACO 中信息素?fù)]發(fā)因子ρ，同時考慮控制系統(tǒng)約束構(gòu)建AWMPC 系統(tǒng)，對跟蹤目標(biāo)函數(shù)中權(quán)重矩陣R 進(jìn)行自適應(yīng)調(diào)節(jié)。并對DSLV 采用VACO 和AWMPC 算法進(jìn)行路徑規(guī)劃和跟蹤效果進(jìn)行了仿真實驗。得出主要結(jié)論如下:

1) 通過用反正切函數(shù)取代ACO 中信息素?fù)]發(fā)因子ρ，使VACO 中的信息素?fù)]發(fā)因子能夠隨著迭代次數(shù)的變化進(jìn)行調(diào)整，達(dá)到了降低規(guī)劃路徑長度和更快收斂的目的。

2) 通過修改跟蹤目標(biāo)函數(shù)中權(quán)重矩陣R，使矩陣R 隨著跟蹤距離誤差的變化進(jìn)行自適應(yīng)調(diào)整，提高了路徑跟蹤的穩(wěn)定性和跟蹤精度。

3) 通過結(jié)合VACO 和AWMPC 算法，使得DSLV 能夠在復(fù)雜海底環(huán)境下安全、高效、自主地進(jìn)行路徑規(guī)劃及跟蹤。

本文的研究方法為DSLV 及相關(guān)深海智能裝備路徑規(guī)劃與跟蹤實現(xiàn)提供了理論支撐。但研究過程中對履帶滑轉(zhuǎn)進(jìn)行了簡化處理，后續(xù)將考慮采用黑箱建模方法對復(fù)雜三維海底環(huán)境下的滑轉(zhuǎn)率進(jìn)行實時估計，提高運(yùn)動模型的精確性;在路徑跟綜過程中，仍存在一定的穩(wěn)態(tài)誤差，后續(xù)工作將針對該部分進(jìn)行更為深入的研究，進(jìn)一步完善DSLV 的跟蹤控制系統(tǒng)設(shè)計。