仇 翔 陳 旭 劉安東 俞 立 周敏龍

(*浙江工業(yè)大學(xué)信息工程學(xué)院 杭州 310023)(** 諾力智能裝備股份有限公司 湖州 313100)

0 引 言

隨著科技發(fā)展，移動(dòng)機(jī)器人因其適應(yīng)性強(qiáng)、可操作性廣等優(yōu)點(diǎn)，已在軍事、工業(yè)、科學(xué)研究、物流倉儲、移動(dòng)舞臺等領(lǐng)域廣泛應(yīng)用。移動(dòng)機(jī)器人路徑跟蹤控制作為運(yùn)動(dòng)控制中的三大基本問題之一，一直備受關(guān)注。針對路徑跟蹤技術(shù)的研究，不只適用于移動(dòng)機(jī)器人路徑跟蹤[1]控制，還涉及無人機(jī)路徑跟蹤[2]、車輛路徑規(guī)劃[3]、自主水下航行器[4]、車床跟蹤切削[5]、移動(dòng)舞臺組合[6]等方面，具有重要的理論與實(shí)際意義。

近年來，國內(nèi)外學(xué)者提出了多種路徑跟蹤控制算法，部分研究取得重大成果。其中，Jiang等人[7]基于反演控制方法實(shí)現(xiàn)實(shí)時(shí)反步控制，在初始誤差未知的情況下，使系統(tǒng)收斂和穩(wěn)定。Chen等人[8]提出了一種基于自適應(yīng)積分滑?？刂品椒?，有效降低干擾對系統(tǒng)的影響，實(shí)現(xiàn)系統(tǒng)全局漸近穩(wěn)定跟蹤效果。Rossomando等人[9]提出了在線學(xué)習(xí)的神經(jīng)網(wǎng)絡(luò)算法，該神經(jīng)網(wǎng)絡(luò)算法充當(dāng)傳統(tǒng)逆控制器的補(bǔ)償器，在系統(tǒng)受到動(dòng)態(tài)和運(yùn)動(dòng)學(xué)變化影響時(shí)改善控制性能，有效地對常規(guī)逆控制器進(jìn)行補(bǔ)償。上述方法解決了初始狀態(tài)未知、外界干擾等問題，然而，并沒有考慮約束問題，本文采用模型預(yù)測控制算法在約束條件下實(shí)現(xiàn)移動(dòng)機(jī)器人精確的路徑跟蹤效果。

模型預(yù)測控制又稱為預(yù)測控制，通常包含預(yù)測模型、滾動(dòng)時(shí)域優(yōu)化、反饋優(yōu)化3個(gè)部分，相較于其他的控制方法，預(yù)測控制能夠及時(shí)地校正模型失配以及干擾等引起的不確定性，具有建模方便、系統(tǒng)穩(wěn)定、擴(kuò)展性好等優(yōu)點(diǎn)，在移動(dòng)機(jī)器人路徑跟蹤控制問題上受到廣泛關(guān)注。González等人[10]在具有縱向滑動(dòng)的越野條件下驅(qū)動(dòng)受約束移動(dòng)機(jī)器人，基于模型預(yù)測控制算法設(shè)計(jì)控制律，保證移動(dòng)機(jī)器人路徑跟蹤的魯棒性和穩(wěn)定性。Yu等人[11]將路徑跟蹤控制問題轉(zhuǎn)化為依賴參數(shù)的調(diào)節(jié)問題，結(jié)合模型預(yù)測控制方法，選擇合適終端懲罰項(xiàng)以及響應(yīng)的終端約束，實(shí)現(xiàn)車載類移動(dòng)機(jī)器人路徑跟蹤。Li等人[12]通過非完整移動(dòng)機(jī)器人跟蹤誤差模型以及模型預(yù)測控制方法迭代變換為二次規(guī)劃問題，并使用原始-對偶神經(jīng)網(wǎng)絡(luò)獲取控制量，解決移動(dòng)機(jī)器人路徑跟蹤問題。Sun等人[13]基于虛擬結(jié)構(gòu)，采用預(yù)測控制方法解決具有輸入耦合約束的獨(dú)輪機(jī)器人跟蹤控制問題。然而移動(dòng)機(jī)器人在實(shí)際運(yùn)行過程中，存在設(shè)定速度與實(shí)際速度無法快速匹配的問題。顯然，當(dāng)控制量輸入到移動(dòng)機(jī)器人端時(shí)，機(jī)器人運(yùn)行時(shí)受加速、減速、慣性、電池電量等因素影響，導(dǎo)致移動(dòng)機(jī)器人設(shè)定速度與實(shí)際速度無法快速匹配的問題。

針對移動(dòng)機(jī)器人路徑跟蹤過程中存在設(shè)定速度與實(shí)際速度不匹配的問題，本文提出一種解決速度失配的模型預(yù)測控制方法。首先，基于運(yùn)動(dòng)學(xué)模型建立了移動(dòng)機(jī)器人路徑跟蹤誤差模型，給出了參數(shù)化期望路徑的更新方程。其次，通過定義與路徑參數(shù)相關(guān)的預(yù)測性能指標(biāo)，并結(jié)合狀態(tài)空間方程推導(dǎo)出預(yù)測模型向量描述，得到了具有不等式約束的二次規(guī)劃優(yōu)化問題。進(jìn)而，采用有效集二次規(guī)劃方法求解具有不等式約束的優(yōu)化問題獲得最優(yōu)控制量。為了驗(yàn)證本文提出算法的可行性，先通過仿真說明算法的有效性，再搭建移動(dòng)機(jī)器人實(shí)驗(yàn)平臺，驗(yàn)證算法的有效性。

1 問題描述

移動(dòng)機(jī)器人路徑跟蹤涉及移動(dòng)機(jī)器人坐標(biāo)系{B}、虛擬移動(dòng)機(jī)器人坐標(biāo)系{T}、世界坐標(biāo)系{W}，如圖1所示。

圖1 移動(dòng)機(jī)器人路徑跟蹤坐標(biāo)系

(1)

(2)

其中，v為機(jī)器人實(shí)際線速度，ω為機(jī)器人實(shí)際角速度，vr為虛擬機(jī)器人參考線速度，ωr為參考角速度。

根據(jù)文獻(xiàn)[14]中移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)誤差模型在平衡點(diǎn)處線性化，可得：

(3)

對式(3)采用零階保持器進(jìn)行離散化得：

(4)

路徑跟蹤是典型的運(yùn)動(dòng)控制問題，與軌跡跟蹤的本質(zhì)區(qū)別在于不受時(shí)間約束，而是跟蹤一個(gè)參數(shù)化路徑[15]。定義參數(shù)化期望路徑為

P={r(k)∈Rn|r(k)=p(θr(k))}

(5)

(6)

(7)

由以上分析可知，若r(k)的更新方式只與時(shí)間有關(guān)，則為軌跡跟蹤問題，需要嚴(yán)格的時(shí)間管理。而本文為了解決實(shí)際速度與設(shè)定速度短時(shí)間失配問題，選擇路徑跟蹤方式，將控制量誤差信號引入到路徑更新方式中，使得實(shí)際移動(dòng)機(jī)器人的速度與虛擬機(jī)器人快速匹配。

本文路徑跟蹤的目的是使移動(dòng)機(jī)器人實(shí)際位姿與虛擬機(jī)器人位姿的誤差為0，并且虛擬移動(dòng)機(jī)器人的運(yùn)動(dòng)路徑最終要與期望路徑一致。實(shí)際移動(dòng)機(jī)器人運(yùn)動(dòng)狀態(tài)逼近虛擬移動(dòng)機(jī)器人運(yùn)動(dòng)狀態(tài)，而虛擬移動(dòng)機(jī)器人的運(yùn)動(dòng)狀態(tài)逼近期望路徑相對應(yīng)的運(yùn)動(dòng)狀態(tài)，如此形成過渡過程。因此，該過程包含有狀態(tài)誤差信息、控制輸入誤差信息、控制輸入誤差增益信息，從而可定義預(yù)測性能指標(biāo)函數(shù)為

(8)

2 模型預(yù)測控制

2.1 預(yù)測控制

(9)

(10)

對整個(gè)系統(tǒng)而言，結(jié)合3個(gè)懲罰項(xiàng)可得優(yōu)化目標(biāo)函數(shù)為

s.t. ‖u(k)‖2≤umax(11)

2.2 約束預(yù)測控制

將式(9)、(10)代入到式(11)，可得：

其中:

D=HTQH+GT(k)G(k)+R

W=I

I是單位矩陣。從而將優(yōu)化目標(biāo)函數(shù)式(11)化簡為標(biāo)準(zhǔn)的二次規(guī)劃問題式(12)。

(13)

式(13)的最優(yōu)解方程為

3 仿真結(jié)果

為了證明所提算法的有效性，本節(jié)給出了數(shù)值算例進(jìn)行仿真驗(yàn)證。基于圓形參考路徑，定義其參數(shù)化路徑如下：

xr=0.8cos(θr)

yr=0.8sin(θr)

(17)

(1) 給出參考機(jī)器人初始位姿[xr(1),yr(1),θr(1)]T，實(shí)際機(jī)器人位姿[x(1),y(1),θ(1)]T，并設(shè)置k=0；

(2) 計(jì)算各誤差向量，并通過有效集方法二次規(guī)劃求解出k時(shí)刻的控制輸入偏差量，進(jìn)而利用預(yù)測模型求出k+1時(shí)刻狀態(tài)誤差量；

(3) 路徑參數(shù)θr更新，求解k+1時(shí)刻虛擬參考移動(dòng)機(jī)器人狀態(tài)；

(4) 根據(jù)狀態(tài)誤差量以及參考狀態(tài)解算出k+1時(shí)刻實(shí)際移動(dòng)機(jī)器人的狀態(tài)；

(5)k=k+1，返回到步驟(2)。

圓形參考路徑跟蹤仿真效果如圖2所示，其中實(shí)線為虛擬參考路徑，三角所示為移動(dòng)機(jī)器人初始位姿，虛線為移動(dòng)機(jī)器人實(shí)際路徑跟蹤曲線。盡管在初始狀態(tài)下，實(shí)際移動(dòng)機(jī)器人位姿與參考位姿存在一定偏差，但經(jīng)過所提預(yù)測控制算法多次迭代后實(shí)際移動(dòng)機(jī)器人跟蹤路徑能漸近收斂于圓形參考路徑，即位姿狀態(tài)偏差均能漸近收斂于零，實(shí)現(xiàn)跟蹤效果。

為了更加直觀觀察跟蹤性能，這里給出了狀態(tài)誤差波形曲線。如圖3所示，其由3個(gè)誤差波形圖組成，包括橫軸誤差波形、縱軸誤差波形、角度誤差波形，各誤差量均能漸近收斂于零，表明實(shí)際移動(dòng)機(jī)器人跟蹤上了虛擬參考機(jī)器人，而虛擬移動(dòng)機(jī)器人路徑即圓形參考路徑，實(shí)現(xiàn)移動(dòng)機(jī)器人的路徑跟蹤。

圖2 圓形參考路徑跟蹤

圖3 圓形參考路徑跟蹤誤差狀態(tài)

4 實(shí)驗(yàn)結(jié)果

4.1 實(shí)驗(yàn)平臺介紹

第3節(jié)通過仿真驗(yàn)證了所提算法的有效性。為了進(jìn)一步說明該算法的實(shí)際意義，本研究搭建了基于Kobuki移動(dòng)機(jī)器人底盤的實(shí)驗(yàn)平臺。針對Kobuki移動(dòng)機(jī)器人底盤存在設(shè)定速度與實(shí)際速度無法快速匹配的問題，使用本文所提的預(yù)測控制算法設(shè)計(jì)控制器完成圓形路徑跟蹤實(shí)驗(yàn)，并對不同取值的補(bǔ)償增益量分析實(shí)驗(yàn)結(jié)果。

實(shí)驗(yàn)設(shè)備包括Optitrack位姿定位系統(tǒng)，無線數(shù)據(jù)傳輸系統(tǒng)，Kobuki移動(dòng)機(jī)器人驅(qū)動(dòng)控制系統(tǒng)。其中，Optitrack位姿定位系統(tǒng)是利用8個(gè)光感攝像頭獲取Mark點(diǎn)對剛體進(jìn)行位姿估計(jì)，可作為本實(shí)驗(yàn)中移動(dòng)機(jī)器人的狀態(tài)數(shù)據(jù)反饋。為了實(shí)驗(yàn)的可行性，將原本離線的Optitrack位姿定位系統(tǒng)設(shè)計(jì)成在線模式，無線狀態(tài)數(shù)據(jù)反饋系統(tǒng)架構(gòu)如圖4所示，實(shí)物圖如圖5所示。

圖4 移動(dòng)機(jī)器人狀態(tài)反饋通道

圖5 Optitrack系統(tǒng)下的移動(dòng)機(jī)器人平臺

移動(dòng)機(jī)器人上有4個(gè)Mark點(diǎn)，8個(gè)攝像頭與臺式電腦通過有線網(wǎng)絡(luò)進(jìn)行數(shù)據(jù)交互，攝像頭采集Mark點(diǎn)數(shù)據(jù)傳給臺式電腦求解位姿，Socket通信將位姿信息反饋給機(jī)器人端筆記本，從而完成數(shù)據(jù)狀態(tài)反饋。

4.2 實(shí)驗(yàn)結(jié)果與分析

本實(shí)驗(yàn)基于實(shí)時(shí)操作系統(tǒng)，為了直觀地得到移動(dòng)機(jī)器人的狀態(tài)量，基于Qt5.8.0設(shè)計(jì)上位機(jī)，包含3個(gè)界面：無線數(shù)據(jù)接收界面、實(shí)時(shí)狀態(tài)及2D機(jī)器人路徑顯示界面、實(shí)時(shí)波形圖顯示界面。該上位機(jī)開啟2個(gè)并行線程——主線程和子線程，主線程基于模型預(yù)測控制算法給出移動(dòng)機(jī)器人控制量，子線程實(shí)時(shí)接收Optitrack系統(tǒng)的狀態(tài)反饋數(shù)據(jù)并解析。

移動(dòng)機(jī)器人硬件驅(qū)動(dòng)差異性很大，驅(qū)動(dòng)性能與電機(jī)控制精度、電池電壓穩(wěn)定程度、輪子摩擦系數(shù)、移動(dòng)機(jī)器人本身重量等因素密切相關(guān)，這些不可避免的影響導(dǎo)致移動(dòng)機(jī)器人的設(shè)定速度與實(shí)際運(yùn)行速度短時(shí)間失配問題，使用本文方法可以進(jìn)行補(bǔ)償。對于不同驅(qū)動(dòng)設(shè)備，λ的取值各不相同。

本文基于移動(dòng)機(jī)器人底盤Kobuki對不同λ取值形成的路徑跟蹤效果進(jìn)行分析。首先給出移動(dòng)機(jī)器人數(shù)據(jù)狀態(tài)反饋界面，涉及TCP/IP通信協(xié)議參數(shù)IP地址、端口，接收窗口，數(shù)據(jù)顯示等，數(shù)據(jù)編碼以字母“b”開頭，字母“e”結(jié)尾，數(shù)據(jù)本體以空格隔開，反饋界面如圖6所示。

當(dāng)λ=0誤差增益補(bǔ)償時(shí)，即控制輸入誤差量不參與路徑更新，不對移動(dòng)機(jī)器人的實(shí)際運(yùn)行狀態(tài)進(jìn)行補(bǔ)償，移動(dòng)機(jī)器人的路徑跟蹤結(jié)果如圖7所示，其中實(shí)線表示虛擬參考位置，虛線為移動(dòng)機(jī)器人位置，在采樣1 000次之后，實(shí)線與虛線明顯存在較大的角度誤差，路徑跟蹤效果較差。其誤差量波形如圖8所示，移動(dòng)機(jī)器人的線速度和角速度波動(dòng)較大，橫軸誤差與縱軸誤差最大值接近300 mm，角度誤差最大值接近0.4 rad，總體趨勢呈現(xiàn)收斂狀態(tài)，但收斂速度很慢，誤差量很大，無法滿足要求。該狀態(tài)即移動(dòng)機(jī)器人的設(shè)定速度與實(shí)際的運(yùn)行速度無法快速匹配，在動(dòng)態(tài)輸入控制量時(shí)，始終存在誤差，從而影響路徑跟蹤效果，也即本文需要解決的問題。

圖6 狀態(tài)數(shù)據(jù)反饋界面

圖7 λ=0機(jī)器人實(shí)時(shí)路徑及參數(shù)

圖8 λ=0參數(shù)波形圖

當(dāng)λ=0.02的誤差增益補(bǔ)償時(shí)，如圖9所示，實(shí)線代表的虛擬參考位置與虛線代表的實(shí)際移動(dòng)機(jī)器人位置重合，路徑跟蹤軌跡較λ=0時(shí)有明顯改善，但還存在一定誤差。如圖10所示，機(jī)器人線速度與角速度在長時(shí)間內(nèi)仍存在較大波動(dòng)，橫軸誤差最大值接近80 mm，縱軸誤差最大值接近100 mm，角度誤差最大值接近0.12 rad。引入λ后，移動(dòng)機(jī)器人路徑跟蹤效果無論是路徑軌跡還是誤差量均有重大改善，但未趨向完美。

圖9 λ=0.02機(jī)器人實(shí)時(shí)路徑及參數(shù)

圖10 λ=0.02參數(shù)波形圖

當(dāng)λ=0.05的誤差增益補(bǔ)償時(shí)，如圖11所示，虛線所示的移動(dòng)機(jī)器人跟蹤軌跡與實(shí)線所示的虛擬參考路徑幾乎重合，路徑跟蹤效果很好。如圖12所示，其橫軸誤差最大值接近40 mm，縱軸誤差最大值接近40 mm，角度誤差能快速到0附近。在該次實(shí)驗(yàn)過程中，移動(dòng)機(jī)器人能很快地達(dá)到理想的動(dòng)態(tài)穩(wěn)定狀態(tài)，能完美地跟蹤上虛擬參考路徑，各項(xiàng)誤差均近似為0。與λ=0.02相比，λ=0.05使跟蹤性能有更好的效果，路徑軌跡和誤差狀態(tài)都趨向于完美。

圖11 λ=0.05機(jī)器人實(shí)時(shí)路徑及參數(shù)

當(dāng)λ=0.3的誤差增益補(bǔ)償時(shí)，如圖13所示，路徑跟蹤效果較λ=0.05時(shí)變差。如圖14所示，其橫軸誤差達(dá)40 mm，縱軸誤差達(dá)80 mm，角度誤差達(dá)0.1 rad，較λ=0.05時(shí)有所變差。顯然，過度補(bǔ)償導(dǎo)致誤差量變大。

綜上所述，本文基于模型預(yù)測控制算法，結(jié)合控制輸入偏差量的增益補(bǔ)償解決移動(dòng)機(jī)器人設(shè)定速度與實(shí)際速度不能快速匹配的問題。而λ的取值與設(shè)備硬件有關(guān)，λ取值小不足以補(bǔ)償，λ取值大又過度補(bǔ)償。λ取值對跟蹤性能的影響如表1所示。

圖13 λ=0.3機(jī)器人實(shí)時(shí)路徑及參數(shù)

圖14 λ=0.3參數(shù)波形圖

表1 λ取值對跟蹤性能的影響

5 結(jié) 論

本文針對實(shí)際移動(dòng)機(jī)器人存在設(shè)定速度與實(shí)際速度無法快速匹配的問題，提出了基于模型預(yù)測控制的移動(dòng)機(jī)器人路徑跟蹤方法。首先，基于運(yùn)動(dòng)學(xué)模型建立了移動(dòng)機(jī)器人路徑跟蹤誤差模型，給出了參數(shù)化期望路徑的更新方程。其次，通過定義與路徑參數(shù)相關(guān)的預(yù)測性能指標(biāo)，并結(jié)合狀態(tài)空間方程推導(dǎo)出預(yù)測模型向量描述，得到了具有不等式約束的二次規(guī)劃優(yōu)化問題。進(jìn)而，采用有效集二次規(guī)劃方法求解具有不等式約束的優(yōu)化問題獲得最優(yōu)控制量。通過仿真驗(yàn)證了算法的可行性，并設(shè)計(jì)移動(dòng)機(jī)器人平臺，多次調(diào)節(jié)λ值，完成圓形路徑跟蹤實(shí)驗(yàn)，跟蹤效果理想，充分證明了所提算法的有效性及實(shí)際意義。