基于路徑評(píng)價(jià)模型的自適應(yīng)軌跡跟蹤控制

許 萬(wàn)，周 航

湖北工業(yè)大學(xué) 機(jī)械工程學(xué)院，武漢 430068

移動(dòng)機(jī)器人因其具有機(jī)動(dòng)性能靈活、承載性能優(yōu)越等特點(diǎn)被應(yīng)用在室內(nèi)建筑施工中的自動(dòng)化放線領(lǐng)域[1]。其中，軌跡跟蹤控制是移動(dòng)機(jī)器人自主導(dǎo)航過(guò)程的關(guān)鍵一環(huán)，控制器的性能將直接影響行駛精度[2]。

當(dāng)前，主流控制方法包括：PID 控制、Backstepping控制、模糊控制、滑模控制、自抗擾控制、MPC 控制、神經(jīng)網(wǎng)絡(luò)控制等[3]。然而，上述控制器在控制增益的選取、模糊規(guī)則表的確定、論域的設(shè)立等方面普遍需要通過(guò)人為試湊的方式，存在控制器調(diào)試?yán)щy等缺點(diǎn)，同時(shí)控制器魯棒性較差，容易受到外界擾動(dòng)的干擾，并且當(dāng)外界環(huán)境發(fā)生變化時(shí)，控制增益需要進(jìn)行重新設(shè)定[4]。考慮到復(fù)雜環(huán)境下的控制需求較為嚴(yán)格，采用固定的控制增益難以確保跟蹤精度，文獻(xiàn)[5]提出了一種自適應(yīng)模糊PID 控制方法，實(shí)現(xiàn)了對(duì)控制增益的自整定，同時(shí)也提高了移動(dòng)機(jī)器人的抗擾能力；文獻(xiàn)[6]利用神經(jīng)網(wǎng)絡(luò)非線性建模能力強(qiáng)的特點(diǎn)，將該方法與Backstepping 控制器進(jìn)行結(jié)合，其中Backstepping 方法能將高階系統(tǒng)進(jìn)行降階處理，簡(jiǎn)化控制模型，提升控制性能，具有較高的響應(yīng)速度，同時(shí)利用神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)控制增益的自適應(yīng)調(diào)整；文獻(xiàn)[7]提出了一種改進(jìn)自適應(yīng)模型預(yù)測(cè)控制，能有效地建模和控制非線性系統(tǒng)，在曲線行駛、圓弧轉(zhuǎn)彎時(shí)提供更精確的控制策略；文獻(xiàn)[8]設(shè)計(jì)了一種自適應(yīng)滑?？刂破鳎瑴p少了傳統(tǒng)滑?？刂频母哳l振蕩現(xiàn)象，提高了控制器的平滑性和穩(wěn)定性。考慮到機(jī)器人模型不確定會(huì)對(duì)跟蹤精度產(chǎn)生影響，文獻(xiàn)[9]將移動(dòng)機(jī)器人實(shí)際運(yùn)行中不可避免的車輪磨損、主軸形變、加工誤差、裝配誤差等因素考慮在內(nèi)，自適應(yīng)調(diào)整運(yùn)動(dòng)學(xué)模型，基于動(dòng)態(tài)運(yùn)動(dòng)學(xué)模型設(shè)計(jì)出的控制器跟蹤精度更高。考慮到動(dòng)力學(xué)對(duì)控制器的影響，文獻(xiàn)[10]針對(duì)移動(dòng)機(jī)器人在行駛的過(guò)程中支撐輪的扭轉(zhuǎn)會(huì)對(duì)機(jī)器人產(chǎn)生擾動(dòng)，影響軌跡跟蹤性能，進(jìn)而將該擾動(dòng)作為前饋輸入設(shè)計(jì)自適應(yīng)控制器；文獻(xiàn)[11]針對(duì)機(jī)器人行駛在顛簸路面、濕滑路面以及松軟路面時(shí)車輪會(huì)發(fā)生縱向或側(cè)向滑動(dòng)，設(shè)計(jì)了一種能夠在未知滑動(dòng)參數(shù)情況下自動(dòng)調(diào)整增益的控制器。

綜上所述，采用自適應(yīng)機(jī)制能夠有效地增強(qiáng)軌跡跟蹤的性能。但是，上述研究成果均未考慮路徑曲率對(duì)軌跡跟蹤控制的影響。

文獻(xiàn)[12-13]指出，移動(dòng)機(jī)器人的軌跡跟蹤控制會(huì)受到路徑信息的影響，尤其是路徑的曲率及曲率變化率這兩個(gè)物理量會(huì)直接影響機(jī)器人的控制精度。在理論跟蹤軌跡彎道較多、曲率變化較大的路況下，移動(dòng)機(jī)器人軌跡跟蹤控制難度顯著提升。文獻(xiàn)[14-15]通過(guò)研究人類駕駛員的行車經(jīng)驗(yàn)，總結(jié)出駕駛技術(shù)高超的人類駕駛員會(huì)通過(guò)綜合分析前方道路彎曲程度與車身目前狀態(tài)之間的關(guān)系，提前做出控制決策，從而提升駕駛安全性。基于上述研究成果，為了進(jìn)一步提高智能移動(dòng)放線機(jī)器人軌跡跟蹤控制精度，借鑒人類駕駛員在控制車輛時(shí)會(huì)綜合考慮前方道路彎曲程度與汽車位姿的駕駛經(jīng)驗(yàn)，設(shè)計(jì)了一種基于路徑評(píng)價(jià)模型的自適應(yīng)軌跡跟蹤控制器。首先，建立移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型以及位姿誤差模型；其次，建立了路徑評(píng)價(jià)模型，定義了路徑復(fù)雜度的概念，并設(shè)計(jì)一種路徑評(píng)價(jià)函數(shù)，該函數(shù)能對(duì)當(dāng)前路徑曲率及車輛跟蹤誤差進(jìn)行量化分析；然后，為了驗(yàn)證路徑評(píng)價(jià)模型的可行性，設(shè)計(jì)了一種自適應(yīng)Backstepping控制器，將路徑評(píng)價(jià)函數(shù)的結(jié)果作為BP神經(jīng)網(wǎng)絡(luò)的輸入，控制增益作為BP神經(jīng)網(wǎng)絡(luò)的輸出，進(jìn)而實(shí)現(xiàn)Backstepping控制器的自適應(yīng)控制；最后，通過(guò)仿真和實(shí)驗(yàn)證明了路徑評(píng)價(jià)模型能有效提高軌跡跟蹤精度，同時(shí)還通過(guò)實(shí)驗(yàn)法給出了路徑評(píng)價(jià)模型中核心參數(shù)的合理取值范圍。

1 移動(dòng)機(jī)器人模型

1.1 運(yùn)動(dòng)學(xué)模型

以智能移動(dòng)放線機(jī)器人為研究對(duì)象，其輪系結(jié)構(gòu)為麥克納姆輪，假設(shè)移動(dòng)機(jī)器人在行駛過(guò)程中車輪與地面間不發(fā)生打滑現(xiàn)象，麥克納姆輪各輥?zhàn)又鬏S線與車輪主軸線呈45°夾角，構(gòu)建如圖1所示的運(yùn)動(dòng)學(xué)模型。

圖1 運(yùn)動(dòng)學(xué)模型Fig.1 Kinematic model

圖1中，XOY為全局坐標(biāo)系，xo′y為局部坐標(biāo)系，o′為移動(dòng)機(jī)器人底盤的幾何中心，坐標(biāo)(x,y)為機(jī)器人的全局位置，vx、vy為機(jī)器人線速度v在局部坐標(biāo)系xo′y中的速度分量，ωz為機(jī)器人角速度，φ為車輪轉(zhuǎn)速，W和L分別為車輪軸線中點(diǎn)到底盤幾何中心的水平和垂直間距，R為車輪的最大半徑，其運(yùn)動(dòng)學(xué)方程可表示為[16]：

1.2 位姿誤差模型

位姿誤差模型如圖2所示，在全局坐標(biāo)系XOY中，定義Q=[x y θ]T為機(jī)器人的真實(shí)位姿向量；Qr=[xr yr θr]T為機(jī)器人的參考位姿向量；D=[Xe Ye θe]T為全局位姿誤差變量；d=[xe ye θe]T為局部位姿誤差變量。

圖2 位姿誤差模型Fig.2 Pose error model

圖2中，x、y、θ分別為機(jī)器人的全局實(shí)際坐標(biāo)值及車身橫擺角度，xr、yr、θr分別為機(jī)器人的全局參考坐標(biāo)值及車身橫擺角度，移動(dòng)機(jī)器人底盤的幾何中心與參考軌跡上目標(biāo)點(diǎn)之間的位姿誤差為：

對(duì)式（2）微分得：

其中，實(shí)際點(diǎn)上的速度與角速度分別描述為v和ω；參考點(diǎn)上的速度與角速度分別描述為vr和ωr。

2 路徑評(píng)價(jià)函數(shù)

在傳統(tǒng)控制器的設(shè)計(jì)過(guò)程中，通常將當(dāng)前時(shí)刻下的位姿誤差信息作為控制器的輸入，從而控制機(jī)器人抵達(dá)目標(biāo)點(diǎn)。但是該方法未考慮路徑曲率對(duì)跟蹤控制的影響，當(dāng)路徑的復(fù)雜度較高時(shí)，控制難度將顯著增大。

為了描述移動(dòng)機(jī)器人實(shí)際位姿與理論跟蹤軌跡之間的關(guān)系，建立如圖3 所示的路徑評(píng)價(jià)模型，其中實(shí)線為“理論跟蹤軌跡”，虛線為“實(shí)時(shí)規(guī)劃路徑”。該模型在描述移動(dòng)機(jī)器人跟蹤位姿誤差的同時(shí)，直觀反映了移動(dòng)機(jī)器人與理論跟蹤軌跡之間的幾何關(guān)系。

圖3 路徑評(píng)價(jià)模型Fig.3 Trajectory evaluation model

圖3中，點(diǎn)O0(x0,y0,θ0)為底盤的幾何中心，并且為當(dāng)前時(shí)刻T下的實(shí)際位姿點(diǎn)；Oi(xi,yi,θi)(i∈(1,n))為理論跟蹤軌跡上的離散參考點(diǎn)，且O1(x1,y1,θ1)為當(dāng)前時(shí)刻T下的理論位姿點(diǎn)。

圖3 所示的理論跟蹤軌跡是一條與時(shí)間相關(guān)的曲線，在實(shí)際應(yīng)用中通常將理論軌跡進(jìn)行離散化處理，從而演變?yōu)閷?duì)參考離散點(diǎn)進(jìn)行跟蹤的問(wèn)題。假設(shè)離散化間隔時(shí)間為Δt,則Oi(xi,yi,θi)(i∈(1,n))為T+(i-1)Δt(i∈(1,n))時(shí)刻下的理論位姿點(diǎn)。

將由點(diǎn)O0(x0,y0,θ0)及點(diǎn)Oi(xi,yi,θi)(i∈(1,n))連接而成的曲線定義為實(shí)時(shí)規(guī)劃路徑E，同時(shí)定義與實(shí)時(shí)規(guī)劃路徑重合的離散參考點(diǎn)為“路況特征點(diǎn)”，其數(shù)量為n。實(shí)時(shí)規(guī)劃路徑E包含有當(dāng)前時(shí)刻T下，O0(x0,y0,θ0)與O1(x1,y1,θ1)之間的位姿誤差信息，以及未來(lái)時(shí)刻理論跟蹤軌跡的曲率和曲率變化率信息。

2.1 路徑評(píng)價(jià)函數(shù)的設(shè)計(jì)

根據(jù)文獻(xiàn)[12-13]描述，移動(dòng)機(jī)器人當(dāng)前的位姿誤差以及理論跟蹤軌跡的曲率及曲率變化率信息會(huì)直接影響控制的效果。如圖3所示，當(dāng)前時(shí)刻T下O0與O1之間的位姿誤差D能夠直接描述位姿偏差情況，理論跟蹤軌跡的曲率及曲率變化率信息能夠量化反映道路的彎曲程度及變化程度。

因此，從路徑曲率和車輛跟蹤誤差的角度入手，基于曲率因素及位姿誤差因素設(shè)計(jì)了一種路徑評(píng)價(jià)函數(shù)，能對(duì)實(shí)時(shí)規(guī)劃路徑E進(jìn)行定量分析，具體描述如下：

式（4）描述了路況特征點(diǎn)Oi處的路徑評(píng)價(jià)情況。其中，η(η∈(0,1))為評(píng)價(jià)函數(shù)的權(quán)值系數(shù)，權(quán)值系數(shù)的選取將直接影響評(píng)價(jià)函數(shù)的輸出值，為此實(shí)驗(yàn)部分將定性分析權(quán)值系數(shù)的選取依據(jù)。針對(duì)實(shí)時(shí)規(guī)劃路徑E的路徑評(píng)價(jià)函數(shù)運(yùn)行流程如圖4所示。

圖4 路徑評(píng)價(jià)函數(shù)運(yùn)行流程Fig.4 Flowchart of trajectory evaluation function operation

圖4中，評(píng)價(jià)函數(shù)f(i)可以系統(tǒng)且直觀地反映了路徑曲率froad(i)和車輛跟蹤誤差fvehicle(i)信息，為后文自適應(yīng)控制器的設(shè)計(jì)提供了控制輸入。其中，froad(i)及fvehicle(i)分別代表曲率因素以及位姿誤差因素，具體描述如下。

2.2 路徑曲率因素

圖3中，剔除O0點(diǎn)，路況特征點(diǎn)O1至On處的曲率及其變化率可以描述未來(lái)(n-1)Δt時(shí)間內(nèi)理論跟蹤軌跡的復(fù)雜度。為了量化分析未來(lái)(n-1)Δt時(shí)間內(nèi)的路徑彎曲程度，引入曲率Ρ以及曲率變化率Ρ′。假設(shè)理論跟蹤軌跡上一共有m個(gè)離散點(diǎn)，路況特征點(diǎn)i處歸一化處理后的曲率值和曲率變化率的值ρ(i)和ρ′(i)分別表示如下：

歸一化處理后的數(shù)值能夠保證曲率及其變化率數(shù)量級(jí)的一致，在點(diǎn)Oi處，未來(lái)(n-1)Δt時(shí)間內(nèi)Oi至Oi+n-1段的路徑復(fù)雜度可以定義如下：

路況特征點(diǎn)Oi處的未來(lái)路徑曲率評(píng)價(jià)函數(shù)如下：

2.3 位姿誤差因素

圖3中，O0O1段描述了移動(dòng)機(jī)器人的位姿誤差，為了量化分析當(dāng)前時(shí)刻T下點(diǎn)O1處的誤差大小，引入跟蹤誤差dline和橫擺誤差dangle，表示如下：

路況特征點(diǎn)Oi處的當(dāng)前車輛跟蹤誤差評(píng)價(jià)函數(shù)如下：

3 基于路徑評(píng)價(jià)模型的控制器設(shè)計(jì)

為了驗(yàn)證上文中路徑評(píng)價(jià)模型的可行性以及路徑評(píng)價(jià)函數(shù)的準(zhǔn)確性，并進(jìn)一步討論引入上述機(jī)制能否提高移動(dòng)機(jī)器人的跟蹤精度，為此設(shè)計(jì)了一種基于路徑評(píng)價(jià)模型的控制器，該控制器由兩部分組成：Backstepping運(yùn)動(dòng)學(xué)控制器、BP 神經(jīng)網(wǎng)絡(luò)增益自整定控制器?？刂破飨到y(tǒng)結(jié)構(gòu)如圖5所示。

圖5 TEMC系統(tǒng)結(jié)構(gòu)Fig.5 TEMC system structure

其中，運(yùn)動(dòng)學(xué)控制器是以移動(dòng)機(jī)器人當(dāng)前時(shí)刻T下，實(shí)際位姿Q與參考位姿Qr之間的誤差d以及期望速度μr作為輸入，以速度μ作為輸出，經(jīng)過(guò)運(yùn)動(dòng)學(xué)逆解，控制四輪轉(zhuǎn)速=[φ1φ2φ3φ4]T,從而使控制增益自整定控制器是基于BP 神經(jīng)網(wǎng)絡(luò)，以路徑評(píng)價(jià)函數(shù)的值froad(i)和fvehicle(i)作為輸入，以運(yùn)動(dòng)學(xué)控制器的增益K作為輸出，從而實(shí)現(xiàn)自適應(yīng)Backstepping控制。

3.1 Backstepping運(yùn)動(dòng)學(xué)控制器

基于1.1 節(jié)所建立的運(yùn)動(dòng)學(xué)模型及位姿誤差模型，設(shè)計(jì)軌跡跟蹤控制器如下：

當(dāng)xe→0 時(shí)，將xe作為虛擬控制輸入，取部分Lyapunov函數(shù)為：

對(duì)式（13）求導(dǎo)可得：

取虛擬控制輸入xe的虛擬反饋為：

其中，k1為大于0的常數(shù)，f(ω)=sin(arctanω)。

將式（15）代入式（14）可得：

其中，ωsin(arctanω)在其定義域內(nèi)恒為非負(fù)常數(shù)，從而可得≤0。V1正定，負(fù)定，由Lyapunov相關(guān)理論可知，在t→+∞時(shí)，ye是漸進(jìn)穩(wěn)定的。

對(duì)式（17）求導(dǎo)可得：

綜上，設(shè)計(jì)控制器的核心是在輸入量[v,ω]的作用下，在t→+∞時(shí)：xe=k1sin(arctanω)ye。據(jù)此，構(gòu)造全局Lyapunov函數(shù)為：

對(duì)式（19）求導(dǎo)可得：

將式（18）代入式（20）得：

根據(jù)式（21）所設(shè)計(jì)的控制率為：

其中，k1、k2、k3為任意的正常數(shù)，分別對(duì)xe、ye、θe進(jìn)行調(diào)節(jié)。

3.2 BP神經(jīng)網(wǎng)絡(luò)增益自整定控制器

式（4）所描述的路徑評(píng)價(jià)函數(shù)f(i)可以量化路徑曲率和車輛跟蹤誤差信息，也即“路徑復(fù)雜度”和“位姿誤差”信息。由式（22）可知，控制器中的三個(gè)增益參數(shù)k1、k2、k3會(huì)直接影響控制效果。當(dāng)系統(tǒng)的跟蹤誤差發(fā)生變化，以及跟蹤不同復(fù)雜度的理論軌跡時(shí)，原始控制增益K是無(wú)法保證較好的跟蹤精度。

為此，綜合考慮理論軌跡復(fù)雜度和位姿誤差這兩個(gè)輸入信息，基于BP神經(jīng)網(wǎng)絡(luò)實(shí)現(xiàn)對(duì)控制增益K的自整定，從而提高控制器跟蹤復(fù)雜路徑時(shí)的跟蹤精度與穩(wěn)定性。所設(shè)計(jì)的BP神經(jīng)網(wǎng)絡(luò)模型輸入?yún)?shù)為froad(i)和fvehicle(i)，輸出為控制增益K。

神經(jīng)網(wǎng)絡(luò)的輸出層和各隱藏層激活函數(shù)都為Relu函數(shù)：

取性能指標(biāo)函數(shù)為：

利用梯度下降法修正BP 神經(jīng)網(wǎng)絡(luò)的權(quán)系數(shù)，基于BP神經(jīng)網(wǎng)絡(luò)控制增益自整定控制器運(yùn)行流程如下：

（1）建立網(wǎng)絡(luò)模型，其中隱藏層為3層，每層神經(jīng)元為20個(gè)，并對(duì)各層權(quán)系數(shù)賦初始值。

（2）計(jì)算當(dāng)前時(shí)刻“froad(i)”和“fvehicle(i)”。

（3）將路徑評(píng)價(jià)函數(shù)f(i)的值作為BP 神經(jīng)網(wǎng)絡(luò)的輸入，更新網(wǎng)絡(luò)中各層參數(shù)的計(jì)算值，其中輸出即為Backstepping控制器的增益K。

（4）根據(jù)式（22）計(jì)算輸出速度。

（5）進(jìn)行神經(jīng)網(wǎng)絡(luò)學(xué)習(xí)，不斷調(diào)整權(quán)系數(shù)，將損失函數(shù)值梯度下降到局部最優(yōu)解，從而實(shí)現(xiàn)對(duì)Backstepping控制器增益的自適應(yīng)調(diào)節(jié)。

（6）返回第（2）步。

3.3 穩(wěn)定性分析

定理1移動(dòng)機(jī)器人在Backstepping運(yùn)動(dòng)學(xué)控制器、BP 神經(jīng)網(wǎng)絡(luò)增益自整定控制器的共同作用下，所設(shè)計(jì)的TEMC控制系統(tǒng)是漸進(jìn)穩(wěn)定的。

證明對(duì)于式（19）所描述的全局Lyapunov 函數(shù)，可以看出V2≥0，當(dāng)且僅當(dāng)(,ye,θe)T=0 時(shí)，V2=0。將所設(shè)計(jì)的控制率式（22）代入式（21）中得：

綜上，在控制率式（22）及自整定控制增益K的作用下，位姿誤差d=[xe ye θe]T是漸進(jìn)穩(wěn)定的，能達(dá)到的目的，即能在有限時(shí)間內(nèi)控制機(jī)器人收斂于期望軌跡。

4 仿真與實(shí)驗(yàn)

為了驗(yàn)證移動(dòng)機(jī)器人在室內(nèi)建筑施工中的有效性以及上述路徑評(píng)價(jià)模型和路徑評(píng)價(jià)函數(shù)的可行性，實(shí)驗(yàn)采用分段函數(shù)和雙扭線作為參考軌跡，取跟蹤誤差和橫擺誤差θe來(lái)分析控制器的性能，并與傳統(tǒng)反演法控制器（TIC）[17]及自適應(yīng)反演法控制器（AIC）[18]進(jìn)行對(duì)比，同時(shí)采用誤差的均方根Xrms(Le)、Xrms(θe)和最大誤差Emax(Le)、Emax(θe)來(lái)評(píng)估各個(gè)控制器的性能。實(shí)驗(yàn)中，移動(dòng)機(jī)器人底盤的長(zhǎng)L=0.183 m、寬W=0.132 m、車輪半徑R=0.037 5 m，取離散化采樣時(shí)間間隔Δt=0.01 s，考慮到移動(dòng)機(jī)器人主要工作在室內(nèi)低速場(chǎng)景，在實(shí)驗(yàn)中采用恒定且較低的期望速度μr=0.5 m/s。

4.1 仿真實(shí)驗(yàn)

在仿真實(shí)驗(yàn)中，TEMC 控制器的各個(gè)參數(shù)選取如表1所示。

表1 TEMC參數(shù)選?。ǚ抡妫㏕able 1 TEMC parameter selection（simulation）

4.1.1 軌跡跟蹤實(shí)驗(yàn)

實(shí)驗(yàn)1分段軌跡跟蹤實(shí)驗(yàn)：采用直線Xd=-2π 與正弦曲線Xd=t,Yd=-5 sin(t/2)的組合作為參考軌跡。參考軌跡的初始位姿為Q=[-2π-13 π/2]T，實(shí)際起始位姿為Qr=[-2π-1-14 3π/20]T。同時(shí)，用于路徑復(fù)雜度評(píng)價(jià)的路況特征點(diǎn)數(shù)量n=30。實(shí)驗(yàn)結(jié)果如圖6所示，實(shí)驗(yàn)數(shù)據(jù)如表2所示。

表2 分段軌跡評(píng)價(jià)指標(biāo)（仿真）Table 2 Segmented trajectory evaluation index（simulation）

圖6 分段軌跡結(jié)果對(duì)比（仿真）Fig.6 Comparison of segmented trajectory results（simulation）

實(shí)驗(yàn)1 結(jié)果分析：在參考軌跡中，直線軌跡和正弦軌跡的銜接處存在路徑曲率及曲率變化率突變的情況。在銜接處，TIC和AIC的跟蹤曲線出現(xiàn)了明顯的跟蹤誤差，而TEMC 在曲線銜接處能快速適應(yīng)，其跟蹤曲線無(wú)劇烈波動(dòng)且響應(yīng)速度高。在正弦軌跡的上凹處，參考軌跡曲率發(fā)生了快速波動(dòng)，TIC和AIC出現(xiàn)了沒(méi)有跟蹤上的情況，軌跡跟蹤誤差較大，而TEMC 跟蹤效果良好。實(shí)驗(yàn)1驗(yàn)證了上文所提出路徑評(píng)價(jià)機(jī)制的可行性，將路徑評(píng)價(jià)模型引入到軌跡跟蹤控制器中能有效地提高跟蹤精度，同時(shí)能夠適應(yīng)曲率變化率突變的路況，在曲率波動(dòng)較大的軌跡上也能快速且準(zhǔn)確地跟蹤。

實(shí)驗(yàn)2雙扭線軌跡跟蹤實(shí)驗(yàn)。采用雙扭線作為參考軌跡，其表達(dá)式為Xd=5 cos(πt/15),Yd=-5 sin(2πt/15)。參考軌跡的初始位姿為Q=[5 0 π/2]T，實(shí)際起始位姿為Qr=[-3-1 3π/20]T。同時(shí)，用于路徑復(fù)雜度評(píng)價(jià)的路況特征點(diǎn)數(shù)量n=30。實(shí)驗(yàn)結(jié)果如圖7 所示，實(shí)驗(yàn)數(shù)據(jù)如表3所示。

表3 雙扭線軌跡評(píng)價(jià)指標(biāo)（仿真）Table 3 Double helix trajectory evaluation index（simulation）

圖7 雙扭線實(shí)驗(yàn)結(jié)果對(duì)比（仿真）Fig.7 Comparison of double helix trajectory results（simulation）

實(shí)驗(yàn)2結(jié)果分析：參考軌跡雙扭線上的曲率及其變化率波動(dòng)更加明顯，也能更好地測(cè)試各控制器的跟蹤性能。在起始位姿誤差存在的情況下，TEMC控制器的收斂速度要快于TIC 和AIC 控制器。由于TEMC 控制器引入了路徑評(píng)價(jià)機(jī)制，對(duì)于理論軌跡曲率變化較大的路段，TEMC 控制器能進(jìn)行快速穩(wěn)定地跟蹤，且跟蹤誤差無(wú)明顯震蕩現(xiàn)象。而TIC 和AIC 控制器主要考慮自身的位姿誤差，無(wú)法對(duì)理論軌跡的復(fù)雜度進(jìn)行分析，對(duì)于曲率及其變化率波動(dòng)較大的路段不能進(jìn)行較好地跟蹤，跟蹤誤差較大。實(shí)驗(yàn)2 進(jìn)一步論證了針對(duì)復(fù)雜軌跡的跟蹤，考慮未來(lái)路徑曲率對(duì)控制決策的影響，將有效提高移動(dòng)機(jī)器人的跟蹤精度。

4.1.2 路徑評(píng)價(jià)函數(shù)權(quán)值系數(shù)η 選取實(shí)驗(yàn)

由人類駕駛員駕駛汽車的經(jīng)驗(yàn)可知，如何操控汽車平穩(wěn)地駕駛?cè)Q于前方道路的彎曲程度，例如當(dāng)汽車行駛在崎嶇的道路上時(shí)，通常需要提前調(diào)整汽車的位姿，以便順利地切入每一個(gè)急彎。同時(shí)由式（4）以及TEMC系統(tǒng)結(jié)構(gòu)可知，權(quán)值系數(shù)η的選取將直接影響路徑評(píng)價(jià)函數(shù)對(duì)路徑曲率froad(i)及車輛跟蹤誤差fvehicle(i)重要性評(píng)估的側(cè)重點(diǎn)，從而間接影響軌跡跟蹤的性能。針對(duì)不同的應(yīng)用場(chǎng)景如何選取合適的權(quán)值系數(shù)，將通過(guò)對(duì)比實(shí)驗(yàn)進(jìn)行說(shuō)明。

為了初步評(píng)估整體理論跟蹤軌跡，在式（5）及式（6）的基礎(chǔ)上，定義整體路徑復(fù)雜度的概念，表示如下：

實(shí)驗(yàn)3分別采用上文中的分段軌跡和雙扭線軌跡作為實(shí)驗(yàn)對(duì)象，其中分段軌跡的整體路徑復(fù)雜度Ψsegment=0.602 3，雙扭線軌跡的整體路徑復(fù)雜度Ψtwist=0.828 2，實(shí)驗(yàn)過(guò)程中通過(guò)調(diào)整權(quán)值系數(shù)η的取值，觀察誤差值，找出權(quán)值系數(shù)η與整體路徑復(fù)雜度Ψ之間的關(guān)系。實(shí)驗(yàn)結(jié)果如圖8、圖9所示，實(shí)驗(yàn)數(shù)據(jù)如表4、表5所示。

表4 分段軌跡評(píng)價(jià)指標(biāo)（仿真）Table 4 Segmented trajectory evaluation index（simulation）

表5 雙扭線軌跡評(píng)價(jià)指標(biāo)（仿真）Table 5 Double helix trajectory evaluation index（simulation）

圖8 分段軌跡η 的敏感性分析（仿真）Fig.8 Sensitivity analysis of segmented trajectory η（simulation）

圖9 雙扭線軌跡η 的敏感性分析（仿真）Fig.9 Sensitivity analysis of double helix trajectory η（simulation）

實(shí)驗(yàn)3結(jié)果分析：首先通過(guò)大量的對(duì)比實(shí)驗(yàn)找出了對(duì)應(yīng)于兩段不同復(fù)雜度軌跡的最優(yōu)η值，分別為η=0.52和η=0.65。在此基礎(chǔ)上，分別將η值增大和縮小30%。實(shí)驗(yàn)結(jié)果表明，針對(duì)同一軌跡在一定范圍內(nèi)，η值越大跟蹤效果越好，但η值過(guò)大將降低車輛跟蹤誤差fvehicle(i)的權(quán)重，從而導(dǎo)致跟蹤誤差增大的情況。經(jīng)過(guò)實(shí)驗(yàn)分析，η的取值不宜大于70%，并且針對(duì)整體路徑復(fù)雜度不同的情況，復(fù)雜度越大，η的取值越大。

4.2 實(shí)物實(shí)驗(yàn)

為了進(jìn)一步分析上述路徑評(píng)價(jià)模型和路徑評(píng)價(jià)函數(shù)的可行性，將上述控制方法應(yīng)用于自主搭建的麥克納姆輪移動(dòng)機(jī)器人中。其上位機(jī)基于ROS 操作系統(tǒng)開(kāi)發(fā)，運(yùn)行在Ubuntu 18.04 上，核心算法功能包均采用C++/Python 進(jìn)行編寫，下位機(jī)核心代碼基于C 語(yǔ)言開(kāi)發(fā)。機(jī)器人系統(tǒng)框架如圖10所示。

圖10 機(jī)器人系統(tǒng)框架Fig.10 Robot system framework

其中，底盤運(yùn)動(dòng)控制板為基于STM32的驅(qū)動(dòng)板，集成了4 路直流電機(jī)驅(qū)動(dòng)、MPU9250 陀螺儀、藍(lán)牙以及OLED 等模塊，同時(shí)可以實(shí)現(xiàn)串口和CAN 通信。ROS平臺(tái)運(yùn)行在Raspberry_Pi 4B 上，主要承擔(dān)著激光雷達(dá)信息的處理、軌跡跟蹤控制器的運(yùn)行以及與STM32 控制命令的交互。實(shí)物機(jī)器人的具體結(jié)構(gòu)如圖11 所示，控制器的各個(gè)參數(shù)選取如表6所示。

表6 TEMC參數(shù)選?。▽?shí)驗(yàn)）Table 6 TEMC parameter selection（experiment）

圖11 機(jī)器人實(shí)物圖Fig.11 Actual diagram of robot

實(shí)驗(yàn)結(jié)果分析：從圖12以及表7的實(shí)驗(yàn)結(jié)果可以看出，針對(duì)雙扭線這類跟蹤軌跡曲率及曲率變化率較大的參考軌跡，基于路徑評(píng)價(jià)模型設(shè)計(jì)的TEMC控制器跟蹤效果明顯優(yōu)于TIC和AIC控制器。同時(shí)，實(shí)物實(shí)驗(yàn)也進(jìn)一步證明了前文所設(shè)計(jì)的路徑評(píng)價(jià)模型的可行性與先進(jìn)性，論證了考慮未來(lái)路徑信息對(duì)控制決策的影響將有效提升移動(dòng)機(jī)器人跟蹤精度，且機(jī)器人跟蹤精度滿足建筑施工的作業(yè)要求。

表7 雙扭線軌跡評(píng)價(jià)指標(biāo)（實(shí)驗(yàn)）Table 7 Double helix trajectory evaluation index（experiment）

圖12 雙扭線實(shí)驗(yàn)結(jié)果對(duì)比（實(shí)驗(yàn)）Fig.12 Comparison of double helix trajectory experimental results（experiment）

5 結(jié)論

針對(duì)現(xiàn)有軌跡跟蹤控制器主要考慮自身的位姿誤差，未考慮路徑曲率對(duì)跟蹤控制的影響，將路徑曲率引入到軌跡跟蹤控制當(dāng)中，提出了一種基于路徑評(píng)價(jià)模型的跟蹤控制方法。首先，針對(duì)機(jī)器人運(yùn)動(dòng)學(xué)模型和位姿誤差模型進(jìn)行分析；其次，建立了一種路徑評(píng)價(jià)模型，并提出了一種路徑評(píng)價(jià)函數(shù)，該函數(shù)能對(duì)未來(lái)一定時(shí)間內(nèi)的路徑曲率和車輛跟蹤誤差進(jìn)行綜合分析；然后，為了驗(yàn)證上述路徑評(píng)價(jià)模型的可行性，基于Backstepping 方法設(shè)計(jì)了一種參數(shù)自整定控制器，并證明了該控制器是漸近穩(wěn)定的；然后，通過(guò)實(shí)驗(yàn)法給出了路徑評(píng)價(jià)模型中關(guān)鍵參數(shù)的合理取值范圍；最后，通過(guò)仿真及實(shí)物實(shí)驗(yàn)進(jìn)一步論證了未來(lái)路徑曲率會(huì)對(duì)控制決策產(chǎn)生影響，將路徑評(píng)價(jià)模型引入到控制系統(tǒng)中能有效提高軌跡跟蹤的精度。在實(shí)物對(duì)比實(shí)驗(yàn)中，TEMC控制方法與其他前饋控制器相比，軌跡跟蹤精度提升了48%以上。