輪式移動(dòng)機(jī)器人軌跡跟蹤控制與仿真

尹澤成，魏惠芳，胡高山，康朔

（沈陽(yáng)城市建設(shè)學(xué)院，遼寧 沈陽(yáng) 110167）

機(jī)器人逐漸走進(jìn)人們的視野并愈發(fā)先進(jìn)，越來(lái)越多的被運(yùn)用到人們的生活和工作之中。在眾多類型的機(jī)器人當(dāng)中，輪式移動(dòng)機(jī)器人是一種集環(huán)境感知、動(dòng)態(tài)決策與規(guī)劃、行為控制與執(zhí)行等多功能于一體的綜合系統(tǒng)，它具有良好的避障能力和運(yùn)動(dòng)性能，應(yīng)用范圍愈發(fā)廣泛。但是對(duì)于輪式移動(dòng)機(jī)器人來(lái)說(shuō)，其運(yùn)行軌跡的跟蹤控制是目前研究的一個(gè)重要方向。對(duì)于移動(dòng)機(jī)器人而言，它的軌跡跟蹤主要包括位置于速度跟蹤等方面。此外，隨著移動(dòng)機(jī)器人被賦予的任務(wù)及其接觸的環(huán)境愈發(fā)復(fù)雜，任務(wù)執(zhí)行當(dāng)中所面臨的隨機(jī)因素越來(lái)越多，其運(yùn)動(dòng)性能將表現(xiàn)出更大的復(fù)雜性，其運(yùn)動(dòng)控制方面也有更大的挑戰(zhàn)性。所以，當(dāng)下針對(duì)輪式移動(dòng)機(jī)器人的軌跡跟蹤控制進(jìn)行深入的研究有利于進(jìn)一步提高我國(guó)移動(dòng)機(jī)器人的整體性能，使其更好完成較為復(fù)雜的任務(wù)，具有深遠(yuǎn)意義。

1 數(shù)學(xué)模型的建立

為了使輪式移動(dòng)機(jī)器人能夠按照預(yù)設(shè)軌跡運(yùn)動(dòng)到執(zhí)行任務(wù)的目的地，需要對(duì)機(jī)器人的運(yùn)行軌跡進(jìn)行跟蹤控制。目前大多數(shù)學(xué)者在對(duì)移動(dòng)機(jī)器人進(jìn)行軌跡跟蹤研究時(shí)都是采用動(dòng)力學(xué)與運(yùn)動(dòng)學(xué)模型進(jìn)行的，而且還需要根據(jù)不同的模型設(shè)計(jì)與其對(duì)應(yīng)的控制器，以便可以更好的實(shí)現(xiàn)軌跡跟蹤控制。控制器是實(shí)現(xiàn)軌跡跟蹤的核心，在對(duì)其進(jìn)行設(shè)計(jì)時(shí)可以將機(jī)器人視為一個(gè)質(zhì)點(diǎn)，這樣不僅可以忽略一些不必要的因素，而且有利于分析機(jī)器人的運(yùn)行速度及角速度等物理特性。由于是實(shí)際應(yīng)用時(shí)，機(jī)器人會(huì)受到多方面不確定因素的影響，所以對(duì)機(jī)器人進(jìn)行受力分析并獲取相應(yīng)的動(dòng)力學(xué)模型可以更好地實(shí)現(xiàn)對(duì)其軌跡的跟蹤控制。

本文是基于MPC算法對(duì)移動(dòng)機(jī)器人進(jìn)行數(shù)學(xué)模型的建立，對(duì)于MPC算法而言，它屬于反饋控制策略，一方面模型中的第一個(gè)元素在第一個(gè)周期中可以用于被控對(duì)象并進(jìn)行開環(huán)求解，另一方面在下一個(gè)采樣周期中，系統(tǒng)會(huì)自動(dòng)的使用下一刻的測(cè)量值進(jìn)行求解下一刻的控制值，由此可見(jiàn)MPC算法屬于反饋控制。而且采用該算法進(jìn)行求解時(shí)需要遵循預(yù)測(cè)系統(tǒng)未來(lái)動(dòng)態(tài)、（數(shù)值）求解開環(huán)優(yōu)化問(wèn)題、將優(yōu)化解的第一個(gè)元素（或者說(shuō)第一部分）作用于系統(tǒng)三步驟原則。

本次研究的是由一個(gè)前轉(zhuǎn)向輪與兩個(gè)后驅(qū)動(dòng)輪組成的輪式移動(dòng)機(jī)器人，且其運(yùn)動(dòng)學(xué)模型如圖1所示，圖中機(jī)器人的定位都是以慣性坐標(biāo)系（X，O，Y）進(jìn)行的，而（x，y，θ）代表機(jī)器人的固定坐標(biāo)系。在對(duì)機(jī)器人是時(shí)刻處于運(yùn)動(dòng)狀態(tài)，為避免其發(fā)生損壞，對(duì)控制器進(jìn)行合理的設(shè)計(jì)并給出相應(yīng)的約束條件，即：

圖1 輪式機(jī)器人運(yùn)動(dòng)學(xué)模型

對(duì)于整個(gè)慣性坐標(biāo)系來(lái)說(shuō)，機(jī)器人的參考軌跡為：

在改變機(jī)器人的運(yùn)動(dòng)軌跡時(shí)，需要對(duì)參考進(jìn)行相應(yīng)的輸入?yún)?shù)。

機(jī)器人的期望線速度為：

式中，+代表機(jī)器人前進(jìn)；-代表機(jī)器人后退。

參考軌跡上每個(gè)點(diǎn)的切線角為：

式中，k為驅(qū)動(dòng)方向，且k=0表示正方向，k=1表示反方向；

對(duì)式（4）求導(dǎo)可得期望角速度為：

所以，通過(guò)集合關(guān)系變化將全局坐標(biāo)系的機(jī)器人與參考軌跡的位置誤差轉(zhuǎn)變?yōu)榫植孔鴺?biāo)系下的跟蹤誤差，即：

2 控制器設(shè)計(jì)

本次控制器的設(shè)計(jì)是基于模型預(yù)測(cè)控制原理進(jìn)行的，它又被稱為滾動(dòng)優(yōu)化控制，通俗地說(shuō)就是充分地將先前與當(dāng)前的多個(gè)時(shí)刻參數(shù)進(jìn)行合理的利用起來(lái)，借助求解優(yōu)化法則來(lái)得到滿足條件的控制參數(shù)并將其應(yīng)用到整個(gè)控制系統(tǒng)中去，所以它屬于在線優(yōu)化過(guò)程。模型預(yù)測(cè)控制的優(yōu)點(diǎn)在于對(duì)模型的要求不高，不用精確的模型也能進(jìn)行較好的控制，它是處理約束問(wèn)題最有效的方法之一。

由圖2分析可知，在當(dāng)前時(shí)刻k預(yù)測(cè)未來(lái)的Np個(gè)狀態(tài)量并借助開環(huán)優(yōu)化的方法進(jìn)行函數(shù)求解，最后獲得滿足條件的最小控制參數(shù)，獲取到該參數(shù)之后將其第一個(gè)控制量應(yīng)用到被控對(duì)象上去進(jìn)行下一計(jì)算。因?yàn)闄C(jī)器人在時(shí)刻運(yùn)動(dòng)時(shí)會(huì)受到不確定因素的干擾使得實(shí)際的數(shù)據(jù)與預(yù)測(cè)的數(shù)據(jù)存在一定程度的偏差。為更好的優(yōu)化此類問(wèn)題，在k+1時(shí)刻需要對(duì)當(dāng)前的狀態(tài)量再次重復(fù)以上操作，以獲取更加精準(zhǔn)的未來(lái)狀態(tài)量并進(jìn)行優(yōu)化分析。由于時(shí)間處于變化的過(guò)程，而預(yù)測(cè)時(shí)域N保持不變，故稱之為滾動(dòng)優(yōu)化原則。

圖2 模型預(yù)測(cè)控制原理圖

對(duì)于模型預(yù)測(cè)控制原理來(lái)說(shuō)，模型預(yù)測(cè)、滾動(dòng)優(yōu)化、反饋矯正是其三大核心步驟，也是其基本組成要素。而對(duì)于模型的預(yù)測(cè)是預(yù)先設(shè)計(jì)一個(gè)目標(biāo)對(duì)象并用于未來(lái)狀態(tài)的預(yù)測(cè)，因此叫做預(yù)測(cè)模型。我們主要看的是模型在預(yù)測(cè)控制中的作用，而不是模型的表達(dá)形式。

由圖3分析可知，在受到不確定因素的干擾之后，使得優(yōu)化求解得到的狀態(tài)量與實(shí)際存在一定程度的偏差，所以為發(fā)生異?，F(xiàn)象，優(yōu)化求解得到的數(shù)據(jù)不能全部直接應(yīng)用到系統(tǒng)中去，而是僅僅將當(dāng)前時(shí)刻的狀態(tài)量應(yīng)用到系統(tǒng)中去。當(dāng)進(jìn)行到下一個(gè)采樣周期時(shí)，為保證優(yōu)化結(jié)果的準(zhǔn)確度，先對(duì)實(shí)際測(cè)量的參數(shù)進(jìn)行分析并對(duì)目標(biāo)模型進(jìn)行校正，最后將校正之后的模型進(jìn)行先一步優(yōu)化求解，這樣可以將偏差縮小到最小。

圖3 系統(tǒng)反饋矯正示意圖

由上述內(nèi)容可知，預(yù)測(cè)未來(lái)狀態(tài)量、優(yōu)化求解、將最優(yōu)解應(yīng)用到系統(tǒng)是模型預(yù)測(cè)控制的三大步驟，在實(shí)際操作過(guò)程中需要對(duì)每個(gè)采樣時(shí)刻都進(jìn)行多次的重復(fù)操作，獲取到的最佳狀態(tài)量也就是系統(tǒng)預(yù)測(cè)的未來(lái)狀態(tài)起點(diǎn)，也就是說(shuō)將采樣時(shí)獲取到的測(cè)量值視為模型預(yù)預(yù)測(cè)的初始條件。隨著采樣時(shí)刻不斷增加，模型預(yù)測(cè)控制也不斷地進(jìn)行優(yōu)化求解。模型預(yù)測(cè)控制相比較離線優(yōu)化控制來(lái)說(shuō)最大的區(qū)別就是通過(guò)采樣時(shí)刻的不斷增加，進(jìn)行多次在線求解優(yōu)化問(wèn)題，獲得每個(gè)采樣時(shí)刻的最優(yōu)解。這也表明了優(yōu)化求解并沒(méi)有離線計(jì)算，而是反復(fù)在線求解優(yōu)化問(wèn)題。將跟蹤誤差系統(tǒng)進(jìn)行離散化，得：

式中，T為采樣周期。

3 仿真驗(yàn)證與分析

如圖4所示預(yù)設(shè)路徑效果圖。為了驗(yàn)證模型預(yù)測(cè)控制對(duì)移動(dòng)機(jī)器人軌跡跟蹤控制的有效性，針對(duì)移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型所設(shè)計(jì)的未加入終端代價(jià)函數(shù)的模型預(yù)測(cè)控制進(jìn)行仿真對(duì)比。圖5為移動(dòng)機(jī)器人進(jìn)行軌跡跟蹤時(shí)跟蹤誤差曲線。圖6為移動(dòng)機(jī)器人速度和角速度控制量變化曲線，圖中上下兩條虛線分別表示執(zhí)行器約束的上下界。

圖4 預(yù)設(shè)路徑效果圖

圖5 仿真路徑結(jié)果圖

圖6 移動(dòng)機(jī)器人角速度與線速度仿真結(jié)果圖

從圖5中可以看出本章所提算法能夠在2s內(nèi)完成軌跡跟蹤，而模糊跟蹤算法完成軌跡跟蹤所需要的時(shí)間大約為7s。因此，通過(guò)對(duì)比可以發(fā)現(xiàn)增加終端代價(jià)函數(shù)的模型預(yù)測(cè)控制不僅能夠進(jìn)行精確地軌跡跟蹤而且收斂的速度也比較快。

由圖6可以看出移動(dòng)機(jī)器人的速度和角速度都被限制飽和約束范圍之內(nèi)，并且線速度和角速度分為維持在0.4m/s和0.4rad/s。在仿真程序中，上方為各項(xiàng)實(shí)時(shí)數(shù)據(jù)以及各項(xiàng)控制數(shù)據(jù)，中間為路線軌跡跟蹤情況，下方為緯度誤差曲線，最下方為測(cè)量路徑轉(zhuǎn)向角以及控制路徑轉(zhuǎn)向角。按仿真程序如圖所示，在MPC仿真方式下，軌跡跟蹤效果良好，基本貼合預(yù)定軌跡，轉(zhuǎn)向角基本貼合預(yù)定軌跡轉(zhuǎn)向角，軌跡跟蹤維度誤差基本與水平線重合，預(yù)測(cè)控制效果良好，驗(yàn)證此方法的有效性。

4 結(jié)語(yǔ)

本文主要研究了輪式移動(dòng)機(jī)器人的軌跡跟蹤控制，運(yùn)用模型預(yù)測(cè)控制方法對(duì)預(yù)定軌跡進(jìn)行跟蹤控制，本文主要針對(duì)輪式移動(dòng)機(jī)器人軌跡跟蹤控制展開研究，采用模型預(yù)測(cè)控制方法進(jìn)行主要研究與仿真，通過(guò)MATLAB實(shí)現(xiàn)仿真，對(duì)軌跡進(jìn)行跟蹤控制，實(shí)現(xiàn)對(duì)算法的驗(yàn)證。主要對(duì)目前軌跡跟蹤控制的現(xiàn)狀進(jìn)行闡述，對(duì)各種主流軌跡跟蹤方法進(jìn)行闡述分析，以及對(duì)模型預(yù)測(cè)方法優(yōu)勢(shì)進(jìn)行闡述。針對(duì)模型建立進(jìn)行了研究，對(duì)模型問(wèn)題及進(jìn)行了主要描述，以及對(duì)基礎(chǔ)知識(shí)進(jìn)行了描述。針對(duì)控制器問(wèn)題進(jìn)行了主要研究，進(jìn)行了MATLAB驗(yàn)證，進(jìn)行仿真分析。為了驗(yàn)證所提方法的有效性，進(jìn)行了MATLAB模擬仿真測(cè)試與分析，并取得了良好的跟蹤路徑曲線與誤差曲線，驗(yàn)證了此方法的合理性和有效性。