基于啟發(fā)式動(dòng)態(tài)規(guī)劃的履帶機(jī)器人路徑跟隨控制方法

宋 彥 張羊陽(yáng) 姚 琦 袁 勝 廖 娟 劉 路

(1.安徽農(nóng)業(yè)大學(xué)工學(xué)院， 合肥 230036; 2.安徽省智能農(nóng)機(jī)裝備工程實(shí)驗(yàn)室， 合肥 230036;3.合肥中科智馳科技有限公司， 合肥 230088)

0 引言

隨著移動(dòng)機(jī)器人在農(nóng)業(yè)領(lǐng)域的廣泛應(yīng)用，復(fù)雜的地形和多變的環(huán)境對(duì)移動(dòng)機(jī)器人的運(yùn)動(dòng)控制能力提出了更高的要求，如何提高移動(dòng)機(jī)器人在未知、復(fù)雜環(huán)境的路徑跟隨性能成為研究的熱點(diǎn)。

路徑跟隨的控制機(jī)理多采用“預(yù)瞄—跟隨”原理[1-2]，傳統(tǒng)的控制方法有純追蹤[3]和PID控制[4-5],但是上述方法均忽略了系統(tǒng)的數(shù)學(xué)模型，難以取得高精度的控制效果。一些學(xué)者提出了基于車(chē)輛動(dòng)力學(xué)模型[6]或運(yùn)動(dòng)學(xué)模型[7]的控制方法，例如：基于期望橫擺角速度[8]以及橫向速度[9]的狀態(tài)反饋控制方法，基于模型預(yù)測(cè)控制[10-11]的方法等。此類(lèi)方法在模型較為精確的條件下，提高了控制精度，但是需要調(diào)節(jié)和優(yōu)化的控制參數(shù)較多，尤其是當(dāng)機(jī)器人的工作環(huán)境發(fā)生變化時(shí)，需要重新選擇控制參數(shù)，優(yōu)化控制性能。

近年來(lái)，如何提高移動(dòng)機(jī)器人的環(huán)境適應(yīng)性和自主行為能力成為學(xué)界的研究熱點(diǎn)，出現(xiàn)了基于智能控制的方法[12-13]，如基于模糊控制[14-15]和神經(jīng)網(wǎng)絡(luò)方法[16]等，如文獻(xiàn)[17]采用模糊控制理論調(diào)整前視距離，文獻(xiàn)[18]提出了一種自適應(yīng)模糊變結(jié)構(gòu)控制方法。但上述方法仍然需要將專(zhuān)家經(jīng)驗(yàn)抽象為模糊控制規(guī)則，并調(diào)校模糊控制參數(shù)。文獻(xiàn)[19]提出了一種根據(jù)車(chē)輛速度和道路彎度動(dòng)態(tài)調(diào)整預(yù)瞄距離的改進(jìn)純追蹤方法。也有學(xué)者采用了一些智能優(yōu)化算法，如文獻(xiàn)[20]采用了遺傳算法自動(dòng)優(yōu)化控制參數(shù)，文獻(xiàn)[21]提出了一種基于增量Q學(xué)習(xí)的控制參數(shù)優(yōu)化方法。

自適應(yīng)動(dòng)態(tài)規(guī)劃[22](Adaptive dynamic programming, ADP)理論采用了強(qiáng)化學(xué)習(xí)機(jī)制，通過(guò)計(jì)算環(huán)境對(duì)不同策略的評(píng)價(jià)性反饋信號(hào)來(lái)優(yōu)化控制策略，使智能體具備了自主優(yōu)化能力[23]。ADP理論包含了不同的實(shí)現(xiàn)方法，如啟發(fā)式動(dòng)態(tài)規(guī)劃(Heuristic dynamic programming, HDP)、對(duì)偶啟發(fā)式規(guī)劃(Dual heuristic programming, DHP)等。ADP理論的實(shí)現(xiàn)框架有執(zhí)行器、評(píng)價(jià)器兩個(gè)模塊，評(píng)價(jià)器基于性能指標(biāo)函數(shù)評(píng)價(jià)當(dāng)前控制動(dòng)作的性能優(yōu)劣，執(zhí)行器根據(jù)評(píng)價(jià)結(jié)果優(yōu)化控制動(dòng)作，并通過(guò)“執(zhí)行—評(píng)價(jià)”迭代運(yùn)算尋找最優(yōu)控制策略。近年來(lái)，這一方法被用于過(guò)程控制、機(jī)器人控制等領(lǐng)域。文獻(xiàn)[24]提出了一種基于回聲狀態(tài)網(wǎng)絡(luò)的HDP控制方法，用于污水處理過(guò)程控制。在移動(dòng)機(jī)器人的應(yīng)用領(lǐng)域，文獻(xiàn)[25]提出了一種基于HDP的路徑規(guī)劃方法。文獻(xiàn)[26]較早地將ADP理論應(yīng)用于移動(dòng)機(jī)器人的路徑跟蹤控制問(wèn)題，但是其建立的回報(bào)函數(shù)僅考慮了速度誤差，且未對(duì)該方法進(jìn)行試驗(yàn)驗(yàn)證。文獻(xiàn)[27]采用一種基于DHP的方法用于機(jī)器人的路徑跟隨控制，該方法能夠估計(jì)性能指標(biāo)函數(shù)的導(dǎo)數(shù)，但是仍然缺乏相關(guān)的試驗(yàn)驗(yàn)證。

為提高移動(dòng)機(jī)器人路徑跟隨控制方法對(duì)環(huán)境的適應(yīng)性和自主優(yōu)化能力，本文提出一種基于HDP算法的路徑跟隨控制方法，構(gòu)建綜合跟隨精度指標(biāo)和穩(wěn)定性指標(biāo)的回報(bào)函數(shù)，采用神經(jīng)網(wǎng)絡(luò)模型逼近評(píng)價(jià)器和執(zhí)行器，通過(guò)開(kāi)展對(duì)不同線(xiàn)形路徑的跟隨試驗(yàn)，驗(yàn)證該方法對(duì)未知路徑線(xiàn)形的適應(yīng)性。

1 控制系統(tǒng)結(jié)構(gòu)與數(shù)學(xué)模型

1.1 控制系統(tǒng)結(jié)構(gòu)

本文研究對(duì)象為履帶式機(jī)器人，其路徑跟隨控制系統(tǒng)結(jié)構(gòu)如圖1所示，其中參考路徑為一系列離散的GPS坐標(biāo)，機(jī)器人通過(guò)車(chē)載GPS傳感器得到實(shí)際位姿。坐標(biāo)轉(zhuǎn)換模塊的功能是通過(guò)當(dāng)前實(shí)際位姿和預(yù)瞄點(diǎn)信息得到位姿誤差信息。路徑跟隨控制器根據(jù)位姿誤差由控制算法計(jì)算產(chǎn)生控制動(dòng)作?？刂屏哭D(zhuǎn)換模塊將控制動(dòng)作轉(zhuǎn)換為兩側(cè)電機(jī)的轉(zhuǎn)速，驅(qū)動(dòng)機(jī)器人穩(wěn)定跟隨參考路徑。

圖1 控制系統(tǒng)結(jié)構(gòu)Fig.1 Control system structure

1.2 系統(tǒng)數(shù)學(xué)模型

圖2 履帶機(jī)器人路徑跟隨示意圖Fig.2 Tracked robot path following diagram

本文建立的履帶機(jī)器人路徑跟隨示意圖如圖2所示，首先定義大地坐標(biāo)系為XOY，其中X軸為正東方向，Y軸為正北方向。定義機(jī)器人局部坐標(biāo)系為xoy，其中x軸方向?yàn)闄C(jī)器人前進(jìn)方向，將y軸方向定義為機(jī)器人的前進(jìn)方向逆時(shí)針旋轉(zhuǎn)90°。其中L為預(yù)瞄距離，(Xc,Yc)為移動(dòng)機(jī)器人在全局坐標(biāo)系中的位置坐標(biāo)，φc為車(chē)輛縱軸線(xiàn)與大地坐標(biāo)系橫坐標(biāo)的夾角，(Xc,Yc,φc)定義為機(jī)器人的當(dāng)前位姿。(Xp,Yp)為預(yù)瞄點(diǎn)在全局坐標(biāo)系中的位置坐標(biāo)，φp為預(yù)瞄點(diǎn)處的切線(xiàn)與大地坐標(biāo)系橫坐標(biāo)的夾角，(Xp,Yp,φp)定義為預(yù)瞄點(diǎn)處的期望位姿。

在不考慮履帶滑動(dòng)的情況下，定義履帶機(jī)器人的驅(qū)動(dòng)輪半徑為rd，履帶中心距為B，當(dāng)前機(jī)器人前進(jìn)速度為vc，角速度為ωc，兩側(cè)電機(jī)的轉(zhuǎn)速為nL、nR，vc和ωc與兩側(cè)電機(jī)轉(zhuǎn)速的轉(zhuǎn)換關(guān)系為

(1)

則履帶式機(jī)器人的運(yùn)動(dòng)學(xué)模型可表示為

(2)

移動(dòng)機(jī)器人相對(duì)預(yù)瞄點(diǎn)的誤差狀態(tài)方程為

(3)

式中xe——縱向偏差

ye——橫向偏差

φe——航向角偏差

根據(jù)圖2的幾何關(guān)系，由式(2)、(3)可得移動(dòng)機(jī)器人相對(duì)預(yù)瞄點(diǎn)Op的橫向偏差和航向角偏差的變化率為

(4)

式中ω——橫擺角速度

vx——移動(dòng)機(jī)器人縱向速度

vy——移動(dòng)機(jī)器人橫向速度

ρ——預(yù)瞄點(diǎn)處的道路曲率

2 基于HDP算法的路徑跟隨控制策略設(shè)計(jì)

首先采用馬爾可夫決策過(guò)程(Markov decision process, MDP)模型描述移動(dòng)機(jī)器人路徑跟隨控制問(wèn)題，然后采用HDP算法求解該控制問(wèn)題的近似最優(yōu)控制策略，并給出算法的實(shí)現(xiàn)框架，最后推導(dǎo)路徑跟隨控制器參數(shù)的在線(xiàn)學(xué)習(xí)規(guī)則。

2.1 路徑跟隨控制問(wèn)題的MDP模型

ADP算法[28]的理論框架是以馬爾可夫決策過(guò)程(MDP)為基礎(chǔ)的。一個(gè)MDP模型由四元組{S,U,r(s,u,s′),P(s,u,s′)}表示，其中S為狀態(tài)集合，U為動(dòng)作集合，r(s,u,s′)為機(jī)器人采用動(dòng)作u使其從狀態(tài)s轉(zhuǎn)移到s′時(shí)獲得的回報(bào)，P(s,u,s′)為機(jī)器人采用動(dòng)作u使其從狀態(tài)s轉(zhuǎn)移到s′時(shí)的概率。

針對(duì)路徑跟隨問(wèn)題，選取狀態(tài)s=(ye,φe,ωc,ρ)T，動(dòng)作u=ω。由式(4)離散化后可以得到狀態(tài)轉(zhuǎn)移方程為

(5)

式中Ts——系統(tǒng)的控制周期

基于MDP模型，將路徑跟隨最優(yōu)控制策略描述為：尋找合適的控制動(dòng)作u使得機(jī)器人從狀態(tài)s(k)轉(zhuǎn)移到s(k+1)，且使得性能指標(biāo)函數(shù)最小。性能指標(biāo)函數(shù)公式為

(6)

式中E()——數(shù)學(xué)期望

rt——t時(shí)刻的回報(bào)

γ——折扣因子

路徑跟隨過(guò)程中，不但期望減小橫向偏差，也期望盡量減少控制動(dòng)作波動(dòng)，因此選取的回報(bào)函數(shù)為

r(s(k),u(s(k)))=
s(k)TQs(k)+(u(s(k))-ωp)TR(u(s(k))-ωp)

(7)

其中

ωp=vxρ

式中Q——半正定對(duì)角矩陣，表征對(duì)橫向偏差和航向角偏差指標(biāo)的權(quán)重

R——正常數(shù)，表征對(duì)期望橫擺角速度補(bǔ)償?shù)臋?quán)重

2.2 路徑跟隨控制器設(shè)計(jì)

本文定義的性能指標(biāo)函數(shù)式(6)也可描述為

J(s(k))=r(s(k),u(s(k)))+γJ(s(k+1))

(8)

根據(jù)Bellman優(yōu)化原理，最優(yōu)的控制策略為

(9)

定義u*(s(k))為使性能指標(biāo)函數(shù)極小化的最優(yōu)控制動(dòng)作，式(8)可以寫(xiě)作

J*(s(k))=r(s(k),u*(s(k)))+γJ*(s(k+1))

(10)

式中J*(s(k))——最優(yōu)性能指標(biāo)函數(shù)

J*(s(k+1))——機(jī)器人在下一狀態(tài)s(k+1)時(shí)的最優(yōu)性能指標(biāo)函數(shù)

令?J*(s(k))/?u*(s(k))=0，則最優(yōu)控制動(dòng)作可表示為

(11)

為實(shí)現(xiàn)上述最優(yōu)控制，構(gòu)建如圖3所示基于HDP算法的控制框架。算法實(shí)現(xiàn)通常包括3部分[21]：評(píng)價(jià)器、模型以及執(zhí)行器，評(píng)價(jià)器的作用為根據(jù)狀態(tài)變量估計(jì)性能指標(biāo)函數(shù)J(s(k))的值；執(zhí)行器的作用為根據(jù)狀態(tài)變量計(jì)算控制動(dòng)作u(s(k))；模型對(duì)移動(dòng)機(jī)器人下一狀態(tài)s(k+1)進(jìn)行預(yù)測(cè)。評(píng)價(jià)器1和評(píng)價(jià)器2的結(jié)構(gòu)及參數(shù)均相同，但是分別用于估計(jì)當(dāng)前狀態(tài)的性能指標(biāo)函數(shù)值以及預(yù)測(cè)狀態(tài)的性能指標(biāo)函數(shù)值。為了按照時(shí)間正向求解控制動(dòng)作，HDP方法往往采用函數(shù)近似的方法估計(jì)性能指標(biāo)函數(shù)值[21]，本文采用多層前饋神經(jīng)網(wǎng)絡(luò)逼近評(píng)價(jià)器，同樣也采用這一模型逼近最優(yōu)動(dòng)作。算法執(zhí)行過(guò)程中，采用梯度下降算法依次對(duì)動(dòng)作網(wǎng)絡(luò)和評(píng)價(jià)網(wǎng)絡(luò)里的神經(jīng)網(wǎng)絡(luò)權(quán)值進(jìn)行反饋調(diào)節(jié)，最小化性能指標(biāo)函數(shù)J(s(k))，進(jìn)而獲得使J(s(k))最優(yōu)的控制策略u(píng)(s(k))。

圖3 基于HDP算法的路徑跟隨控制方法結(jié)構(gòu)圖Fig.3 Structural diagram of path following control method based on HDP algorithm

2.3 控制器參數(shù)的在線(xiàn)學(xué)習(xí)

控制器參數(shù)在線(xiàn)學(xué)習(xí)過(guò)程實(shí)質(zhì)上是對(duì)神經(jīng)網(wǎng)絡(luò)權(quán)值的在線(xiàn)調(diào)整過(guò)程，其目標(biāo)是尋求一組最優(yōu)權(quán)值使J(s(k))最小化，進(jìn)而獲取近似最優(yōu)控制動(dòng)作。HDP算法中評(píng)價(jià)器網(wǎng)絡(luò)的優(yōu)化目標(biāo)是對(duì)最優(yōu)性能指標(biāo)函數(shù)值的近似，執(zhí)行器網(wǎng)絡(luò)優(yōu)化的目標(biāo)是找到最優(yōu)控制動(dòng)作u*(s(k))使得目標(biāo)函數(shù)J(s(k))最小。

本文采用3層前饋神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)評(píng)價(jià)器和執(zhí)行器，激活函數(shù)為Sigmoid函數(shù)，從隱含層到輸出層的函數(shù)為線(xiàn)性函數(shù)，此時(shí)執(zhí)行器和評(píng)價(jià)器的表達(dá)式分別為

u(s(k))=Φa(Waσa(Vas(k)+ba1)+ba2)

(12)

Jc(s(k))=Φc(Wcσc(Vcs(k)+bc1)+bc2)

(13)

式中Jc(s(k))——評(píng)價(jià)器對(duì)狀態(tài)s(k)估計(jì)的性能指標(biāo)函數(shù)值

Va、Vc——神經(jīng)網(wǎng)絡(luò)從輸入層到隱含層的權(quán)值

ba1、bc1——神經(jīng)網(wǎng)絡(luò)從輸入層到隱含層的偏置

Wa、Wc——神經(jīng)網(wǎng)絡(luò)從隱含層到輸出層的權(quán)值

ba2、bc2——神經(jīng)網(wǎng)絡(luò)從隱含層到輸出層的偏置

σa、σc——Sigmoid函數(shù)

Φa、Φc——線(xiàn)性函數(shù)

2.3.1執(zhí)行器網(wǎng)絡(luò)參數(shù)優(yōu)化

執(zhí)行器的學(xué)習(xí)目標(biāo)是最小化誤差函數(shù)

(14)

定義ζd(k)=u(s(k))-u*(s(k))，則執(zhí)行器網(wǎng)絡(luò)的權(quán)值和偏置更新規(guī)則為

(15)

其中

Ψa(k)=(Wa,ba2,Va,ba1)

式中αa(k)——執(zhí)行器網(wǎng)絡(luò)的學(xué)習(xí)率

Ψa(k)——執(zhí)行器網(wǎng)絡(luò)的權(quán)值和偏置

由式(12)可得

(16)

(17)

(18)

(19)

其中

σ′a(x)=e-x/(1+e-x)2

式中σ′a(x)——Sigmoid函數(shù)導(dǎo)數(shù)

2.3.2評(píng)價(jià)器網(wǎng)絡(luò)參數(shù)優(yōu)化

定義誤差

δd(s(k))=Jc(s(k))-r(s(k),u(s(k)))-
γJc(s(k+1))

(20)

式中Jc(s(k+1))——評(píng)價(jià)器對(duì)狀態(tài)s(k+1)估計(jì)的性能指標(biāo)函數(shù)值

因此評(píng)價(jià)器網(wǎng)絡(luò)的學(xué)習(xí)目標(biāo)是最小化誤差函數(shù)

(21)

則評(píng)價(jià)器網(wǎng)絡(luò)的權(quán)值和偏置更新規(guī)則如下

(22)

其中

Ψc(k)=(Wc,bc2,Vc,bc1)

式中αc(k)——評(píng)價(jià)器網(wǎng)絡(luò)的學(xué)習(xí)率

Ψc(k)——評(píng)價(jià)器網(wǎng)絡(luò)的權(quán)值和偏置

由式(13)可得

(23)

(24)

(25)

(26)

其中

σ′c(x)=e-x/(1+e-x)2

式中σ′c(x)——Sigmoid函數(shù)導(dǎo)數(shù)

3 仿真分析

為了驗(yàn)證本文提出算法的有效性，在Matlab/Simulink環(huán)境下，通過(guò)數(shù)值仿真驗(yàn)證該方法的性能。定義跟隨軌跡點(diǎn)到參考路徑的垂線(xiàn)距離為橫向偏差，采用橫向偏差平均值的絕對(duì)值和橫向偏差的均方根值評(píng)價(jià)跟隨性能。

仿真參數(shù)設(shè)置為：機(jī)器人履帶中心距0.485 m，驅(qū)動(dòng)輪半徑0.2 m，控制周期Ts=0.2 s。為了對(duì)比控制效果，同時(shí)設(shè)計(jì)了基于LQR和基于傳統(tǒng)純追蹤算法的控制器，并且在不同速度下對(duì)3種控制器進(jìn)行仿真。參考路徑采用了組合線(xiàn)(直線(xiàn)和圓)和螺旋線(xiàn)。

為了觀察誤差收斂過(guò)程，將機(jī)器人的起點(diǎn)偏離軌跡起點(diǎn)一段距離，在跟隨組合線(xiàn)時(shí)，組合線(xiàn)起點(diǎn)為(0 m,-20 m)，機(jī)器人的起點(diǎn)為(2 m,-20 m)。當(dāng)速度v=0.3 m/s時(shí)，LQR算法和純追蹤算法的預(yù)瞄距離為2.5 m，HDP算法的預(yù)瞄距離為3 m。選取LQR算法的反饋矩陣K=[-0.15 -0.02]T，HDP算法的參數(shù)為Q=diag{4,1,0,0}，R=1，γ=0.9，學(xué)習(xí)步長(zhǎng)為0.2?；诩冏粉櫵惴?、HDP算法以及LQR算法的路徑跟隨軌跡如圖4所示，對(duì)應(yīng)的橫向偏差如圖5所示。為準(zhǔn)確評(píng)價(jià)其跟隨誤差，剔除初始誤差的收斂過(guò)程，分析機(jī)器人進(jìn)入穩(wěn)態(tài)跟隨時(shí)的橫向偏差，結(jié)果如表1所示。

在螺旋線(xiàn)的參考路徑中，螺旋線(xiàn)可表示為

(27)

螺旋線(xiàn)的起點(diǎn)為(53 m,-29 m)，也將機(jī)器人的起點(diǎn)偏離軌跡起點(diǎn)，設(shè)定為(53 m,-27 m)。基于純追蹤算法、HDP算法以及LQR算法的路徑跟隨軌跡如圖6所示，對(duì)應(yīng)的橫向偏差如圖7所示。表2為剔除初始誤差的收斂過(guò)程，進(jìn)入穩(wěn)態(tài)跟隨狀態(tài)后的誤差統(tǒng)計(jì)結(jié)果。

圖4 速度為0.3 m/s時(shí)不同控制器跟隨組合線(xiàn)的軌跡Fig.4 Trajectory of robot using different controllers on combination line at speed of 0.3 m/s

圖5 速度為0.3 m/s時(shí)不同控制器跟隨組合線(xiàn)的橫向偏差Fig.5 Lateral error of different controllers on combination line at speed of 0.3 m/s

表1 速度為0.3 m/s時(shí)不同控制器跟隨組合線(xiàn)的誤差Tab.1 Tracking error of different controllers on combination line at speed of 0.3 m/s m

圖6 速度為0.3 m/s時(shí)不同控制器跟隨螺旋線(xiàn)的軌跡Fig.6 Trajectory of robot using different controllers on helical line at speed of 0.3 m/s

圖7 速度為0.3 m/s時(shí)不同控制器跟隨螺旋線(xiàn)的橫向偏差Fig.7 Lateral error of different controllers on helical line at speed of 0.3 m/s

評(píng)價(jià)參數(shù)純追蹤算法LQR算法HDP算法平均誤差絕對(duì)值0.020.030.01均方根誤差 0.090.070.02

為了驗(yàn)證移動(dòng)機(jī)器人在不同速度下的跟隨效果，改變機(jī)器人移動(dòng)速度v=1 m/s進(jìn)行仿真。LQR算法、純追蹤算法以及HDP算法的預(yù)瞄距離均為3 m。選取LQR算法的反饋矩陣K=[-0.3 -0.005]T,HDP算法參數(shù)不變，純追蹤算法的預(yù)瞄距離不變，機(jī)器人起點(diǎn)不變。基于純追蹤算法、HDP算法以及LQR算法的路徑跟隨軌跡如圖8所示，對(duì)應(yīng)的橫向偏差如圖9所示。表3為剔除初始誤差的收斂過(guò)程，進(jìn)入穩(wěn)態(tài)跟隨狀態(tài)的誤差統(tǒng)計(jì)結(jié)果。

圖8 速度為1 m/s時(shí)不同控制器跟隨組合線(xiàn)的軌跡Fig.8 Trajectory of robot using different controllers on combination line at speed of 1 m/s

圖9 速度為1 m/s時(shí)不同控制器跟隨組合線(xiàn)的橫向偏差Fig.9 Lateral error of different controllers on combination line at speed of 1 m/s

在跟隨螺旋線(xiàn)時(shí)，機(jī)器人起點(diǎn)不變?；诩冏粉櫵惴?、HDP算法以及LQR算法的路徑跟隨軌跡如圖10所示，對(duì)應(yīng)的橫向偏差如圖11所示。表4為剔除初始誤差的收斂過(guò)程，進(jìn)入穩(wěn)態(tài)跟隨狀態(tài)的誤差統(tǒng)計(jì)結(jié)果。

從仿真結(jié)果可以得出，3種控制算法在2種跟隨速度條件下，均能夠?qū)崿F(xiàn)對(duì)兩種復(fù)雜線(xiàn)形的跟隨控制，設(shè)置的初始誤差均得到快速收斂。在跟隨組合線(xiàn)時(shí)，3種控制方法的穩(wěn)態(tài)平均誤差絕對(duì)值均在0附近，基于HDP控制方法的均方根誤差比純追蹤算法和LQR算法要小。組合線(xiàn)曲率在直線(xiàn)與半圓線(xiàn)交接處、兩條半圓曲線(xiàn)交接處均發(fā)生了變化，雖然基于HDP算法在此時(shí)的瞬態(tài)誤差較大，但隨即收斂到0附近，而純追蹤算法和LQR算法，在跟隨半圓線(xiàn)時(shí)，誤差基本不變。在跟隨螺旋線(xiàn)時(shí)，可以看出隨著螺旋線(xiàn)曲率的增大，3種控制算法的橫向偏差均有增大的趨勢(shì)，但是基于HDP算法的平均誤差絕對(duì)值依然比純追蹤算法和LQR算法小，基于HDP算法的均方根誤差小于或等于基于純追蹤算法和LQR算法的均方根誤差。上述現(xiàn)象說(shuō)明，相對(duì)于其他兩種方法，HDP算法可以通過(guò)參數(shù)的在線(xiàn)學(xué)習(xí)，自主優(yōu)化路徑跟隨性能，減小控制誤差。

表3 速度為1 m/s時(shí)不同控制器跟隨組合線(xiàn)的誤差Tab.3 Tracking error of different controllers on combination line at speed of 1 m/s m

圖10 速度為1 m/s時(shí)不同控制器跟隨螺旋線(xiàn)的軌跡Fig.10 Trajectory of robot using different controllers on helical line at speed of 1 m/s

圖11 速度為1 m/s時(shí)不同控制器跟隨螺旋線(xiàn)的橫向偏差Fig.11 Lateral error of different controllers on helical line at speed of 1 m/s

表4 速度為1 m/s時(shí)不同控制器跟隨螺旋線(xiàn)的誤差Tab.4 Tracking error of different controllers on helical line at speed of 1 m/s m

4 試驗(yàn)結(jié)果分析

4.1 路徑跟隨試驗(yàn)結(jié)果

本文使用的履帶機(jī)器人試驗(yàn)平臺(tái)如圖12所示，其長(zhǎng)度為0.8 m，寬度為0.57 m，質(zhì)量約為60 kg，履帶中心距B為0.485 m，驅(qū)動(dòng)輪半徑rd為0.2 m。采用48 V、20 A·h的鋰電池供電，驅(qū)動(dòng)電機(jī)功率為400 W。傳感器為北斗星通BDM680，工作在差分定位模式，移動(dòng)站與基準(zhǔn)站之間的距離小于5 km，位置類(lèi)型狀態(tài)為Narrow_Int，按照產(chǎn)品手冊(cè)，在該工作模式下定位誤差為0.01 m(RMS)，通過(guò)雙天線(xiàn)定位提供移動(dòng)平臺(tái)的航向角信息。計(jì)算平臺(tái)采用工控機(jī)和基于STM32單片機(jī)的控制卡組成，工控機(jī)計(jì)算控制動(dòng)作；單片機(jī)控制卡通過(guò)數(shù)學(xué)模型將控制動(dòng)作分解為兩側(cè)履帶轉(zhuǎn)速，并給電機(jī)驅(qū)動(dòng)器發(fā)送指令。

圖12 履帶機(jī)器人Fig.12 Tracked robot

試驗(yàn)路徑包括了直線(xiàn)、鈍角轉(zhuǎn)向曲線(xiàn)以及銳角轉(zhuǎn)向曲線(xiàn)，試驗(yàn)地點(diǎn)為安徽農(nóng)業(yè)大學(xué)農(nóng)萃園。機(jī)器人運(yùn)動(dòng)速度v=0.3 m/s，系統(tǒng)控制周期Ts=0.5 s。為驗(yàn)證HDP算法的路徑跟隨性能，同時(shí)采用LQR算法和純追蹤算法作為比較。為了獲得對(duì)不同線(xiàn)形的理想跟隨效果，對(duì)LQR算法和純追蹤算法的控制參數(shù)作了針對(duì)性調(diào)試。為了驗(yàn)證HDP算法對(duì)不同線(xiàn)形的自適應(yīng)能力，在跟隨不同的參考路徑時(shí)，不改變HDP算法的參數(shù)配置，其參數(shù)配置為Q=diag{2,1,0,0}，R=5，γ=0.9，控制器的神經(jīng)網(wǎng)絡(luò)隱含層的神經(jīng)元均為5個(gè)，學(xué)習(xí)步長(zhǎng)均為0.2。采用基于LQR算法獲得的路徑跟隨數(shù)據(jù)訓(xùn)練評(píng)價(jià)器和執(zhí)行器網(wǎng)絡(luò)，獲得初始網(wǎng)絡(luò)權(quán)值。

在跟隨直線(xiàn)時(shí)，3種方法的預(yù)瞄距離均為3 m，其中LQR算法的反饋矩陣K=[-0.11 -0.015]T。圖13為3種方法跟隨直線(xiàn)軌跡。圖14為不同控制器跟隨直線(xiàn)的橫向偏差，其誤差統(tǒng)計(jì)量如表5所示。

圖13 基于不同控制器跟隨直線(xiàn)的軌跡Fig.13 Trajectory of robot using different controllers on straight line

圖14 基于不同控制器跟隨直線(xiàn)的橫向偏差Fig.14 Lateral error of different controllers on straight line

m

在跟隨鈍角轉(zhuǎn)向曲線(xiàn)時(shí)，純追蹤算法的預(yù)瞄距離為2.5 m，LQR算法為3 m，其中LQR算法的反饋矩陣K=[-0.05 -0.055]T。圖15為3種方法跟隨鈍角轉(zhuǎn)向曲線(xiàn)的軌跡，圖16為不同控制器跟隨鈍角轉(zhuǎn)向曲線(xiàn)的橫向偏差，其統(tǒng)計(jì)量如表6所示。

在跟隨銳角轉(zhuǎn)向曲線(xiàn)時(shí)，預(yù)瞄距離均為3 m，其中LQR算法的反饋矩陣K=[-0.25 -0.075]T。圖17為3種方法跟隨銳角轉(zhuǎn)向曲線(xiàn)的軌跡，圖18為不同控制器跟隨銳角轉(zhuǎn)向曲線(xiàn)的橫向偏差，其統(tǒng)計(jì)量如表7所示。

圖15 基于不同控制器跟隨鈍角轉(zhuǎn)向曲線(xiàn)的軌跡Fig.15 Trajectory of robot using different controllers on obtuse angle steering curve

圖16 基于不同控制器跟隨鈍角轉(zhuǎn)向曲線(xiàn)的橫向偏差Fig.16 Lateral error of different controllers on obtuse angle steering curve

圖17 基于不同控制器跟隨銳角轉(zhuǎn)向曲線(xiàn)的軌跡Fig.17 Trajectory of robot using different controllers on acute angle steering curve

圖18 基于不同控制器跟隨銳角轉(zhuǎn)向曲線(xiàn)的橫向偏差Fig.18 Lateral error of different controllers on acute angle steering curve

4.2 結(jié)果分析

總結(jié)上述試驗(yàn)結(jié)果可以發(fā)現(xiàn)，3種方法均能夠順利跟隨直線(xiàn)、鈍角轉(zhuǎn)向曲線(xiàn)以及銳角轉(zhuǎn)向曲線(xiàn)。對(duì)比LQR和純追蹤算法，在跟隨直線(xiàn)時(shí)， HDP算法除均方根誤差等于LQR算法的同類(lèi)指標(biāo)外，其他統(tǒng)計(jì)指標(biāo)均為最?。辉诟S鈍角轉(zhuǎn)向曲線(xiàn)時(shí)，HDP算法除最大誤差絕對(duì)值略大于LQR算法同類(lèi)指標(biāo)0.03 m，其余兩項(xiàng)誤差指標(biāo)均最??；在跟隨銳角轉(zhuǎn)向曲線(xiàn)時(shí)，HDP算法除平均誤差絕對(duì)值略大于LQR算法同類(lèi)指標(biāo)0.01 m，其余兩項(xiàng)誤差指標(biāo)均最小。為了完成3種曲線(xiàn)的路徑跟隨任務(wù)并取得較好的控制效果，試驗(yàn)過(guò)程中對(duì)純追蹤算法和LQR算法都進(jìn)行了反復(fù)參數(shù)優(yōu)化。而HDP算法僅通過(guò)歷史跟隨數(shù)據(jù)對(duì)評(píng)價(jià)器和執(zhí)行器網(wǎng)絡(luò)進(jìn)行了離線(xiàn)訓(xùn)練，變換跟隨路徑時(shí)控制參數(shù)未做人工調(diào)試，僅通過(guò)HDP算法的參數(shù)優(yōu)化機(jī)制對(duì)控制網(wǎng)絡(luò)進(jìn)行在線(xiàn)優(yōu)化，即可取得較高精度的跟隨結(jié)果，說(shuō)明該方法具有較好的自主優(yōu)化能力。

表7 不同控制器跟隨銳角轉(zhuǎn)向曲線(xiàn)試驗(yàn)的誤差Tab.7 Tracking error of different controllers on acute angle steering curve m

5 結(jié)論

(1)針對(duì)移動(dòng)機(jī)器人傳統(tǒng)路徑跟隨控制算法環(huán)境適應(yīng)能力弱、需要人工優(yōu)化控制參數(shù)的問(wèn)題，以履帶式移動(dòng)機(jī)器人為研究對(duì)象，提出了一種基于啟發(fā)式動(dòng)態(tài)規(guī)劃的路徑跟隨控制方法，并試驗(yàn)驗(yàn)證了路徑跟隨性能，結(jié)果表明，跟隨直線(xiàn)的均方根誤差為0.06 m，跟隨鈍角轉(zhuǎn)向曲線(xiàn)的均方根誤差為0.06 m，跟隨銳角轉(zhuǎn)向曲線(xiàn)的均方根誤差為0.09 m。該方法可以實(shí)現(xiàn)對(duì)多種復(fù)雜線(xiàn)形的路徑跟隨，且具有較高的控制精度。

(2)基于啟發(fā)式動(dòng)態(tài)規(guī)劃算法框架，構(gòu)建了綜合跟隨精度指標(biāo)和穩(wěn)定性指標(biāo)的評(píng)價(jià)函數(shù)，通過(guò)對(duì)當(dāng)前控制動(dòng)作的評(píng)價(jià)性反饋信號(hào)優(yōu)化控制器參數(shù)，仿真和試驗(yàn)結(jié)果表明，在跟隨路徑線(xiàn)形變化時(shí)，該方法不需要對(duì)控制參數(shù)進(jìn)行調(diào)試，僅通過(guò)HDP算法的參數(shù)優(yōu)化機(jī)制對(duì)網(wǎng)絡(luò)權(quán)值進(jìn)行在線(xiàn)優(yōu)化就能夠獲得高精度的控制效果，具有較好的環(huán)境適應(yīng)性和自主優(yōu)化能力。