重載輪式移動(dòng)機(jī)器人路徑跟蹤滑模預(yù)測(cè)控制

陳 勇，劉 哲，喬 健，盧清華

(佛山科學(xué)技術(shù)學(xué)院 機(jī)電工程與自動(dòng)化學(xué)院，廣東 佛山 528225)

1 引言

重載移動(dòng)機(jī)器人在動(dòng)態(tài)建模[1]、導(dǎo)航技術(shù)[2]、能耗計(jì)算[3]、運(yùn)動(dòng)控制[4]等領(lǐng)域的研究深受國(guó)內(nèi)外學(xué)者關(guān)注。與普通輪式移動(dòng)機(jī)器人不同，重載移動(dòng)機(jī)器人具有較大承載能力的特點(diǎn)，使得其在變壓器、鋁型材等重物運(yùn)輸過程中具有廣泛的應(yīng)用前景。

重載移動(dòng)機(jī)器人結(jié)構(gòu)特點(diǎn)導(dǎo)致的路徑跟蹤速度和穩(wěn)定性問題相比于一般輪式機(jī)器人顯得更為復(fù)雜[5]，預(yù)測(cè)控制用于移動(dòng)機(jī)器人的運(yùn)動(dòng)過程調(diào)節(jié)，能夠提升計(jì)算速度，加快移動(dòng)機(jī)器人軌跡跟蹤的速度，已經(jīng)應(yīng)用到移動(dòng)機(jī)器人控制的各個(gè)領(lǐng)域[6]。文獻(xiàn)[7]提出了一種預(yù)測(cè)控制器來控制兩輪式移動(dòng)機(jī)器人軌跡跟蹤，采用飽和技術(shù)優(yōu)化最大和最小控制值，簡(jiǎn)化控制器算法。文獻(xiàn)[8]將重型車輛的行駛?cè)蝿?wù)描述為一個(gè)最優(yōu)控制問題，通過模型預(yù)測(cè)控制器實(shí)現(xiàn)能耗節(jié)省。文獻(xiàn)[9]提出了一種模型預(yù)測(cè)控制基于WMR軌跡規(guī)劃的控制算法，解決了其他控制算法只能處理部分機(jī)器人約束點(diǎn)的問題。文獻(xiàn)[10]提出了一種基于神經(jīng)網(wǎng)絡(luò)優(yōu)化的魯棒模型預(yù)測(cè)控制方法來穩(wěn)定具有物理約束的移動(dòng)機(jī)器人，通過神經(jīng)網(wǎng)絡(luò)算法增加模型預(yù)測(cè)控制的穩(wěn)定性。文獻(xiàn)[11]提出了一種基于量子粒子群優(yōu)化的模型預(yù)測(cè)控制方法，用以解決機(jī)器人速度跳變和速度跳變引起的驅(qū)動(dòng)飽和問題。

作為一種結(jié)構(gòu)簡(jiǎn)單的新型控制方法，滑模控制在移動(dòng)機(jī)器人的運(yùn)動(dòng)控制中得到了廣泛應(yīng)用。文獻(xiàn)[12]針對(duì)欠驅(qū)動(dòng)兩輪移動(dòng)機(jī)器人，提出了一種引入線性控制器的新積分滑模控制器。文獻(xiàn)[13]針對(duì)移動(dòng)機(jī)器人的參數(shù)不確定性，設(shè)計(jì)了基于神經(jīng)網(wǎng)絡(luò)的模型預(yù)測(cè)控制器，實(shí)現(xiàn)軌跡跟蹤控制。文獻(xiàn)[14]應(yīng)用動(dòng)態(tài)環(huán)境下諧波勢(shì)場(chǎng)的在線修正，提出了一種多級(jí)滑模控制器移動(dòng)機(jī)器人的動(dòng)態(tài)避障策略，使機(jī)器人在非已知軌跡上以時(shí)變速度運(yùn)動(dòng)時(shí)避免與障礙物的碰撞。文獻(xiàn)[15]設(shè)計(jì)了基于指數(shù)趨近律的滑模控制方法，保證在大階躍情況下系統(tǒng)仍然能以較大的速度趨近于滑動(dòng)模態(tài)。文獻(xiàn)[16]針對(duì)移動(dòng)機(jī)器人的路徑跟蹤控制問題，提出了一種全局穩(wěn)定定理與指數(shù)趨近律相結(jié)合的滑模控制方法，采用雙環(huán)閉環(huán)控制結(jié)構(gòu)提高了機(jī)器人在路徑跟蹤過程中的線速度與角速度的穩(wěn)定性，削弱了指數(shù)控制律的抖振現(xiàn)象。

本文首先分析了重載移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)特性，建立了重載輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)模型，基于動(dòng)力學(xué)模型設(shè)計(jì)了預(yù)測(cè)控制器，并加入位置滑模控制，結(jié)合滑模控制和預(yù)測(cè)控制的優(yōu)點(diǎn)，提出了基于動(dòng)力學(xué)的滑模預(yù)測(cè)控制方法，提高了路徑跟蹤速度和穩(wěn)定性。

2 重載輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)建模

如式(1)所示，在不考慮摩擦力、向心力、側(cè)滑等外部擾動(dòng)因素下，可以建立輪式移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)方程。

(1)

3 重載輪式移動(dòng)機(jī)器人動(dòng)力學(xué)建模

應(yīng)用于物流運(yùn)輸中的移動(dòng)機(jī)器人載重量通常為幾十公斤甚至超過1 000 kg，本文重載移動(dòng)機(jī)器人最大載荷量可達(dá)8 000 kg，其關(guān)鍵技術(shù)問題為重載移動(dòng)機(jī)器人舵輪的控制，單輪承重可達(dá)2 t，使用4個(gè)同樣載荷量的輪子，針對(duì)其中兩個(gè)舵輪進(jìn)行控制。

系統(tǒng)進(jìn)行轉(zhuǎn)向時(shí)，左后輪不產(chǎn)生偏轉(zhuǎn)角，需要分析右前方驅(qū)動(dòng)輪的縱向力和側(cè)向力，考慮到四輪式結(jié)構(gòu)的繞z軸轉(zhuǎn)動(dòng)慣量分析復(fù)雜，垂直于軸心的力大小不能用準(zhǔn)確的力學(xué)分析得到，簡(jiǎn)化模型假設(shè)后方兩個(gè)輪子都沒有偏轉(zhuǎn)角，并且分析軸心轉(zhuǎn)動(dòng)動(dòng)力學(xué)時(shí)近似成單軌模型，如圖1所示。

圖1 輪式移動(dòng)機(jī)器人動(dòng)力學(xué)模型示意圖

在x軸方向上有：

在y軸方向上有：

則z軸的方向上的動(dòng)力學(xué)方程為

通過側(cè)向力和縱向力列出驅(qū)動(dòng)輪在x軸、y軸方向受到的合力：

移動(dòng)機(jī)器人進(jìn)行轉(zhuǎn)向運(yùn)動(dòng)時(shí)，輪子受到的縱向力和側(cè)向摩擦力與輪子的偏轉(zhuǎn)角α、滑移率s、縱向負(fù)載Mg+F、輪子與路面的摩擦系數(shù)μ等參數(shù)構(gòu)成如下函數(shù)關(guān)系：

其一般方法由以下公式給出：

式中，x=s+Sh,S是縱向滑移率，Sh和Sν分別代表曲線水平方向漂移和縱向水平方向漂移。C為曲線外形系數(shù),D為曲線最大值系數(shù),B為剛度系數(shù)，E為曲率系數(shù)。

由于速度與位置的約束，輪子的偏轉(zhuǎn)角α可以由三角函數(shù)算出：

αr1=tan-1(vcr1/vlr1)

輪子速度為

滑移率s可由經(jīng)驗(yàn)公式計(jì)算：

驅(qū)動(dòng)輪所受垂向載荷可由下面公式計(jì)算：

機(jī)器人轉(zhuǎn)向運(yùn)動(dòng)時(shí)的動(dòng)力學(xué)模型為：

由上述分析得知，縱向力和側(cè)向力的函數(shù)關(guān)系中，只有驅(qū)動(dòng)輪偏轉(zhuǎn)角α為未知，在實(shí)際的機(jī)器人運(yùn)動(dòng)控制中，可以測(cè)得偏轉(zhuǎn)角的動(dòng)態(tài)變化，因此可以把偏轉(zhuǎn)角作為控制變量當(dāng)作系統(tǒng)的輸入，得系統(tǒng)的動(dòng)力學(xué)方程的空間狀態(tài)方程如式(2)所示。

(2)

4 預(yù)測(cè)控制設(shè)計(jì)

4.1 動(dòng)力學(xué)方程線性化與離散化

采用泰勒公式法對(duì)非線性系統(tǒng)進(jìn)行線性化。首先要保證系統(tǒng)在該時(shí)刻有n階導(dǎo)數(shù)，并且在該時(shí)刻的開區(qū)間上n+1階可導(dǎo)。針對(duì)式(2)所示的動(dòng)力學(xué)的空間狀態(tài)方程，使用泰勒級(jí)數(shù)展開，只保留泰勒級(jí)數(shù)的一階項(xiàng)，則有：

上式可寫成：

u=γ(ηt,ut)+Jγ(η)(η-ηt)+Jγ(u)(u-ut)

式中Jγ(x)為泰勒展開式的近似雅可比矩陣。

誤差線性化得到：

模型預(yù)測(cè)控制是針對(duì)狀態(tài)方程采用模型預(yù)測(cè)控制算法，建立步長(zhǎng)，并根據(jù)其當(dāng)前狀態(tài)計(jì)算下一步的動(dòng)作，需要狀態(tài)方程是離散化的，采用歐拉法對(duì)線性方程進(jìn)行離散處理。

狀態(tài)空間表達(dá)式如式(3)所示。

(3)

其中：A1=I+TA(t)，B1=TB(t)

4.2 滑模預(yù)測(cè)控制器

針對(duì)如下離散化狀態(tài)方程：

定義線性誤差方程：

E=ηk+T-ηk

其中：ηk+T為η的在k+T時(shí)刻的參考值。

(4)

利用全局穩(wěn)定指數(shù)趨近律設(shè)計(jì)滑模切換函數(shù)，如式(5)所示。

(5)

為使系統(tǒng)盡可能地達(dá)到每一時(shí)刻的參考值，有：

min{(ηk+T-ηk)TQ(ηk+T-ηk)+(uk+T-uk)TR(uk+T-uk)}

其中：Q為權(quán)重因子。對(duì)一般二次型規(guī)劃問題有以下形式：

對(duì)于時(shí)間步長(zhǎng)的周期內(nèi)得到求解后，可以得到控制時(shí)域內(nèi)的控制量和控制變化量如式(6)和式(7)所示。

U=[u(k),u(k+1),u(k+2),…]T

(6)

ΔU=u(k+T)-u(k)

(7)

將u(k)作為實(shí)際的輸入用于系統(tǒng)，在下個(gè)周期更新，以此類推，完成系統(tǒng)預(yù)測(cè)跟蹤控制。

4.3 控制器約束條件

在實(shí)際的移動(dòng)機(jī)器人動(dòng)力學(xué)運(yùn)動(dòng)控制中，其環(huán)境通常是復(fù)雜的，為適應(yīng)場(chǎng)合的多樣性，需要對(duì)機(jī)器人建立約束條件，如式(8)～式(10)所示。

(8)

(9)

(10)

式(8)為速度和角速度約束；式(9)為加速度與角加速度約束；式(10)為位置約束與姿態(tài)角約束。

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

對(duì)四輪式對(duì)角雙驅(qū)動(dòng)移動(dòng)機(jī)器人動(dòng)力學(xué)滑模預(yù)測(cè)控制進(jìn)行CarSim和Matlab聯(lián)合仿真，通過動(dòng)力學(xué)模型建立，控制移動(dòng)機(jī)器人后輪和前輪作為驅(qū)動(dòng)輪，分析移動(dòng)機(jī)器人在固定速度、不同速度、滑模預(yù)測(cè)控制與預(yù)測(cè)控制對(duì)比、障礙干擾等情況下的移動(dòng)機(jī)器人位置、姿態(tài)角、前驅(qū)動(dòng)輪偏轉(zhuǎn)角的跟蹤情況。

滑模預(yù)測(cè)流程如圖2所示。移動(dòng)機(jī)器人4個(gè)輪子采用四角對(duì)稱放置，左右間距為2 m，前輪與后輪到質(zhì)心距離均為3.5 m，車體及荷載總質(zhì)量為7 000 kg，仿真路徑為經(jīng)典雙移線(DLC)軌跡。

圖2 滑模預(yù)測(cè)控制流程框圖

5.1 固定速度下雙移線路徑跟蹤仿真

選擇移動(dòng)機(jī)器人速度為1 m/s，路徑為雙移線運(yùn)動(dòng)軌跡，路徑跟蹤運(yùn)動(dòng)時(shí)間為15 s，移動(dòng)機(jī)器人的路徑跟蹤過程中的位置、姿態(tài)角、驅(qū)動(dòng)輪偏轉(zhuǎn)角如圖3～圖5所示。

圖3中實(shí)線部分為預(yù)設(shè)路徑，虛線部分為速度是1 m/s時(shí)沿預(yù)設(shè)路徑的跟蹤路徑，機(jī)器人路徑基本可以跟蹤預(yù)設(shè)路徑。圖4為路徑跟蹤時(shí)機(jī)器人的姿態(tài)角變化，能夠吻合雙移線中的姿態(tài)角。圖5為機(jī)器人運(yùn)動(dòng)時(shí)轉(zhuǎn)向驅(qū)動(dòng)輪的偏轉(zhuǎn)角，其控制移動(dòng)機(jī)器人的轉(zhuǎn)向，移動(dòng)機(jī)器人姿態(tài)角的變化隨機(jī)器人轉(zhuǎn)向輪偏轉(zhuǎn)角的變化而變化。

圖3 固定速度下移動(dòng)過程中機(jī)器人位置曲線

圖4 固定速度下移動(dòng)過程中機(jī)器人姿態(tài)角曲線

圖5 固定速度下移動(dòng)過程中驅(qū)動(dòng)輪偏轉(zhuǎn)角曲線

5.2 不同速度下雙移線路徑跟蹤仿真

取移動(dòng)機(jī)器人速度與運(yùn)動(dòng)時(shí)間條件分別為：1)速度為0.5 m/s、仿真時(shí)間25 s；2)速度為0.75 m/s、仿真時(shí)間20 s；3)速度為1 m/s、仿真時(shí)間15 s。

移動(dòng)機(jī)器人在不同移動(dòng)速度時(shí)的路徑跟蹤過程中的位置、姿態(tài)角、右前輪偏轉(zhuǎn)角如圖6～圖8所示，分別為移動(dòng)機(jī)器人速度為重載移動(dòng)機(jī)器人速度分別為0.5 m/s、1 m/s和0.75 m/s時(shí)路徑跟蹤狀態(tài)。

圖6 不同速度下移動(dòng)過程中機(jī)器人位置曲線

圖7 不同速度下移動(dòng)過程中機(jī)器人姿態(tài)角曲線

圖8 不同速度下移動(dòng)過程中機(jī)器人驅(qū)動(dòng)輪偏轉(zhuǎn)角曲線

從機(jī)器人3種不同速度的響應(yīng)可以看出，機(jī)器人的速度改變對(duì)其路徑和姿態(tài)角的有一定的影響，機(jī)器人速度越大，路徑跟蹤過程越快，在轉(zhuǎn)角路徑時(shí)的偏差就越小，能夠快速地完成轉(zhuǎn)向。

5.3 固定速度下滑模預(yù)測(cè)控制與預(yù)測(cè)控制對(duì)比試驗(yàn)

取移動(dòng)機(jī)器人速度為1 m/s，仿真時(shí)間為15 s，預(yù)測(cè)步長(zhǎng)為0.3 s，控制步長(zhǎng)為0.075 s，觀察對(duì)移動(dòng)機(jī)器人分別在滑模預(yù)測(cè)控制(SDMPC)和預(yù)測(cè)控制(MPC)下的軌跡變化、姿態(tài)角變化和偏轉(zhuǎn)角變化，如圖9～圖11所示。

圖9 移動(dòng)過程中機(jī)器人位置曲線Fig.9 Comparison of robot position in moving process

從圖9～圖11可以看出：滑模預(yù)測(cè)控制(SDMPC)和預(yù)測(cè)控制(MPC)的對(duì)比仿真實(shí)驗(yàn)中，二者路徑和姿態(tài)角變化并無(wú)明顯差別，但二者的驅(qū)動(dòng)輪偏轉(zhuǎn)角的對(duì)比中，在路徑轉(zhuǎn)彎處和運(yùn)行到80 m后，預(yù)測(cè)控制明顯存在較大的擾動(dòng)現(xiàn)象，而滑模預(yù)測(cè)控制下的驅(qū)動(dòng)輪偏轉(zhuǎn)角更加穩(wěn)定且無(wú)抖震現(xiàn)象，具有更好的跟蹤性能。

圖10 移動(dòng)過程中機(jī)器人姿態(tài)角曲線

圖11 移動(dòng)過程中機(jī)器人驅(qū)動(dòng)輪偏轉(zhuǎn)角變化曲線

5.4 擾動(dòng)狀態(tài)下的路徑跟蹤

取移動(dòng)機(jī)器人速度為1 m/s，仿真時(shí)間為15 s，對(duì)移動(dòng)機(jī)器人進(jìn)行路徑存在擾動(dòng)情況下動(dòng)力學(xué)滑模預(yù)測(cè)控制器的雙移線軌跡路徑，其運(yùn)動(dòng)路徑、驅(qū)動(dòng)輪偏轉(zhuǎn)角變化如圖12～圖14所示，表明擾動(dòng)狀態(tài)下滑模預(yù)測(cè)控制器能保持有效性。

圖12顯示：移動(dòng)機(jī)器人在運(yùn)動(dòng)過程中設(shè)計(jì)一處路徑障礙(X軸76 m處)，移動(dòng)機(jī)器人的控制器可以很好并及時(shí)地做出避障處理。圖13為存在障礙路徑中的姿態(tài)角變化，圖14為路徑跟蹤時(shí)控制輸入的轉(zhuǎn)向驅(qū)動(dòng)輪偏轉(zhuǎn)角。

圖12 存在擾動(dòng)時(shí)的路徑曲線

圖13 存在擾動(dòng)時(shí)的機(jī)器人姿態(tài)角曲線

圖14 存在擾動(dòng)時(shí)的驅(qū)動(dòng)輪偏轉(zhuǎn)角曲線

6 結(jié)論

根據(jù)重載移動(dòng)機(jī)器人結(jié)構(gòu)，建立重載移動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)建模。通過CarSim和Matlab聯(lián)合仿真進(jìn)行運(yùn)動(dòng)控制驗(yàn)證，分別分析了固定速度、不同速度、固定速度下滑模預(yù)測(cè)控制與預(yù)測(cè)控制及擾動(dòng)干擾等情況下的移動(dòng)機(jī)器人路徑跟蹤情況。仿真實(shí)驗(yàn)結(jié)果表明，滑模預(yù)測(cè)控制應(yīng)用于機(jī)器人的路徑控制過程，能較好地進(jìn)行路徑跟蹤并進(jìn)行有效避障，驗(yàn)證了相同路徑下控制器的有效性。本文設(shè)計(jì)的控制器可以作為設(shè)計(jì)真實(shí)重載移動(dòng)機(jī)器人控制系統(tǒng)的有效參考。