基于模型預(yù)測(cè)控制的雙輪平衡輪椅軌跡跟蹤

常宏 王賾

摘要：為實(shí)現(xiàn)雙輪平衡輪椅的自主循跡出行，根據(jù)歐拉-拉格朗日方程建立雙輪平衡輪椅的動(dòng)力學(xué)模型，通過(guò)差動(dòng)幾何模型建立運(yùn)動(dòng)關(guān)系，采用Pure Pursuit算法規(guī)劃系統(tǒng)的目標(biāo)運(yùn)動(dòng)狀態(tài)，分別采用模型預(yù)測(cè)控制（model predictive control，MPC）及線性二次型最優(yōu)控制（linear quadratic regulator，LQR）對(duì)雙輪平衡輪椅進(jìn)行平衡控制及運(yùn)動(dòng)狀態(tài)追蹤。通過(guò)軟件Simulink搭建雙輪平衡輪椅控制仿真系統(tǒng)，驗(yàn)證雙輪平衡輪椅系統(tǒng)分別采用LQR和MPC控制器時(shí)的軌跡跟蹤效果及跟蹤平穩(wěn)性。仿真結(jié)果表明：MPC控制器的動(dòng)態(tài)響應(yīng)速度較LQR控制器快，MPC控制器作用下車體能快速恢復(fù)穩(wěn)定狀態(tài)；軌跡跟蹤時(shí)，MPC控制器在啟動(dòng)轉(zhuǎn)向及終點(diǎn)位置對(duì)車體傾角及速度的穩(wěn)定控制優(yōu)于LQR控制器。

關(guān)鍵詞：雙輪平衡輪椅；LQR控制器；MPC控制器；軌跡追蹤

中圖分類號(hào)：U489;TP242.6文獻(xiàn)標(biāo)志碼：A文章編號(hào)：1672-0032（2024）02-0001-09

引用格式：常宏，王賾.基于模型預(yù)測(cè)控制的雙輪平衡輪椅軌跡跟蹤［J］.山東交通學(xué)院學(xué)報(bào)，2024，32（2）：1-9.

CHANG Hong，WANG Ze. Trajectory tracking of two-wheel balanced wheelchair based on model predictive control［J］.Journal of Shandong Jiaotong University，2024，32（2）：1-9.

0?引言

輪椅是出行不便人員的輔助行動(dòng)工具之一，一般通過(guò)人工協(xié)助、手柄遙控等方式完成輪椅的行動(dòng)動(dòng)作。隨輪椅使用者的自主性出行需求和出行質(zhì)量要求的提高，使輪椅適應(yīng)更復(fù)雜的道路環(huán)境，提高輪椅的安全性及乘坐舒適性變得至關(guān)重要。

常宏等[1]采用雙輪平衡輪椅系統(tǒng)結(jié)合圓弧齒架設(shè)計(jì)能自助上下樓的輪椅，但未進(jìn)行完整路徑追蹤控制。雙輪平衡輪椅系統(tǒng)有高度非線性特征，受力復(fù)雜，茅力非[2]采用拉格朗日運(yùn)動(dòng)方程建立座椅自平衡雙輪機(jī)器人在平坦路面和顛簸路面的動(dòng)力學(xué)模型；Yang等[3]引入邊界函數(shù)，建立雙機(jī)械臂非線性復(fù)雜內(nèi)力系統(tǒng)的動(dòng)力學(xué)模型。為實(shí)現(xiàn)平衡系統(tǒng)的控制與狀態(tài)跟蹤，劉學(xué)明等[4]基于后退方法，采用運(yùn)動(dòng)學(xué)跟蹤控制器和動(dòng)力學(xué)跟蹤控制器，實(shí)現(xiàn)雙輪平衡車在不確定運(yùn)動(dòng)下的參數(shù)自動(dòng)調(diào)節(jié)；黃用華等[5]采用線性二次型最優(yōu)控制（linear quadratic regulator，LQR）設(shè)計(jì)雙輪自行車系統(tǒng)點(diǎn)對(duì)點(diǎn)運(yùn)動(dòng)的軌跡控制器。針對(duì)平衡系統(tǒng)模型與控制的結(jié)合及參數(shù)調(diào)優(yōu)等問題，黃鶴等[6]采用拉格朗日方程構(gòu)建雙輪平衡機(jī)器人系統(tǒng)動(dòng)力學(xué)方程，采用改進(jìn)的食肉植物算法（improved carnivorous plant algorithm，ICPA）優(yōu)化LQR權(quán)重系數(shù)，對(duì)雙輪平衡機(jī)器人進(jìn)行高精度軌跡跟蹤；高志偉等[7]采用拉格朗日方程構(gòu)建雙輪自平衡小車系統(tǒng)的動(dòng)力學(xué)方程，通過(guò)LQR優(yōu)化比例積分微分（proportional integral derivative，PID）控制，提高小車的軌跡跟蹤性能，降低能量消耗；Murcia等[8]、Yue等[9]基于PID控制器控制雙輪倒立擺機(jī)器人的運(yùn)動(dòng)軌跡；寧一高[10]考慮避障運(yùn)動(dòng)問題，采用LQR控制器對(duì)雙輪平衡車進(jìn)行路徑規(guī)劃。模型預(yù)測(cè)控制（model predictive control，MPC）不僅適用于線性系統(tǒng)，在非線性系統(tǒng)中也有較好的適應(yīng)性，已在自動(dòng)駕駛領(lǐng)域得到廣泛應(yīng)用，梁棟[11]、龔大為[12]基于阿克曼車輛動(dòng)力學(xué)模型，將MPC控制器應(yīng)用于車輛控制及軌跡追蹤；Yuan等[13]通過(guò)將LQR控制器與MPC控制器結(jié)合實(shí)現(xiàn)對(duì)車輛不確定狀態(tài)的適應(yīng)性控制。PID控制器通常不需對(duì)系統(tǒng)建模，通過(guò)調(diào)節(jié)PID參數(shù)即可實(shí)現(xiàn)期望動(dòng)作，但易出現(xiàn)超調(diào)、控制量過(guò)大等問題；LQR控制器基于線性化后的動(dòng)力學(xué)模型，在平衡點(diǎn)附近實(shí)現(xiàn)車體的控制響應(yīng)，但僅結(jié)合當(dāng)前的狀態(tài)反饋進(jìn)行控制，易使被控對(duì)象陷入當(dāng)前最優(yōu)狀態(tài)；MPC控制器基于被控對(duì)象當(dāng)前狀態(tài)及未來(lái)時(shí)間的預(yù)測(cè)結(jié)果實(shí)施控制，控制效果較好。

雙輪平衡輪椅為典型的非線性、欠驅(qū)動(dòng)模型[14-16]，僅依靠運(yùn)動(dòng)幾何關(guān)系，在期望狀態(tài)附近線性化構(gòu)建控制律難以實(shí)現(xiàn)對(duì)車體的平衡控制和規(guī)劃軌跡的穩(wěn)定跟蹤。為改善車體平衡控制及軌跡追蹤效果，本文采用MPC控制器改進(jìn)雙輪平衡輪椅的控制系統(tǒng)，使雙輪平衡輪椅實(shí)現(xiàn)自主路徑追蹤，通過(guò)軟件Simulink搭建雙輪平衡輪椅控制仿真系統(tǒng)，驗(yàn)證系統(tǒng)的軌跡跟蹤效果。

1?動(dòng)力學(xué)建模

Ow—車輪旋轉(zhuǎn)中心；v—車速；α—車體偏航角；

θ—車體傾角；R—車輪半徑；lb—Ow與O的距離；

lc—輪椅質(zhì)心到O的距離；ψl、ψr—左、右輪的轉(zhuǎn)速。

為避免分析雙輪平衡輪椅系統(tǒng)內(nèi)部運(yùn)動(dòng)副約束，采用拉格朗日方程求解系統(tǒng)的動(dòng)力學(xué)問題[2，17-18]。建立雙輪平衡輪椅系統(tǒng)的動(dòng)力學(xué)模型，如圖1所示，以輪椅2個(gè)車輪旋轉(zhuǎn)中心連線的中點(diǎn)為坐標(biāo)原點(diǎn)O，車體處于平衡位置時(shí)，以垂直于車軸和車體組成的平面，水平向前為xG軸，以垂直于大地豎直向上為zG軸，以垂直于xG和zG組成的平面，由右輪指向左輪為yG軸，建立大地笛卡爾坐標(biāo)系OxGyGzG，固定在輪椅起始位置，不隨車體移動(dòng)；以垂直車軸和車體所構(gòu)成的平面，指向車體前進(jìn)方向?yàn)閤b軸，以垂直于xb軸，由右輪指向左輪方向?yàn)閥b軸，以垂直于xb軸和yb軸構(gòu)成的平面，向上方向?yàn)閦b軸，建立車體笛卡爾坐標(biāo)系Oxbybzb，其隨車體移動(dòng)而移動(dòng)。雙輪平衡輪椅的結(jié)構(gòu)參數(shù)如表1所示。

注：mw、mb分別為車輪、車體的質(zhì)量，Iwa為車輪轉(zhuǎn)動(dòng)慣量，Iwd為車輪繞直徑方向的轉(zhuǎn)動(dòng)慣量，Ixx、Iyy、Izz分別為車體對(duì)xb、yb、zb軸的轉(zhuǎn)動(dòng)慣量，fb為車身運(yùn)動(dòng)粘滯阻力的耗散能系數(shù)，fw為地面粘滯阻力的耗散能系數(shù)。

雙輪平衡輪椅車體轉(zhuǎn)動(dòng)動(dòng)能

TRb=IbGω2/2，

式中：IbG為車體相對(duì)大地笛卡爾坐標(biāo)系的慣性矩，ω為車體轉(zhuǎn)動(dòng)角速度。

雙輪平衡輪椅車體平動(dòng)動(dòng)能

TTb=mbvbG2/2，

式中vbG為車體平動(dòng)速度。

選取雙輪平衡輪椅車軸所在平面為零勢(shì)能面，車體勢(shì)能

Vb=mbglccos θ，

式中g(shù)為重力加速度。

雙輪平衡輪椅車輪轉(zhuǎn)動(dòng)動(dòng)能

TRw=Iwaψr·2+Iwaψl·2+2Iwdθ·2/2，

式中：ψl·、ψr·分別為左、右輪的轉(zhuǎn)動(dòng)角速度，θ·為車體傾斜角速度。

雙輪平衡輪椅車輪平動(dòng)動(dòng)能

TTw=mwR2（ψr·2+ψl·2）/2。

阻力會(huì)造成能量損失，雙輪平衡輪椅系統(tǒng)運(yùn)轉(zhuǎn)的能量損失

D=fw（ψr·2+ψl·2）+fbθ·2/2。

拉格朗日方程從能量角度求解動(dòng)力學(xué)模型，計(jì)算雙輪平衡輪椅系統(tǒng)的機(jī)械能

L=TRb+TTb+TRw+TTw－Vb。

選取初始狀態(tài)變量q=x?y?θ?α?ψrψlT，建立雙輪平衡輪椅系統(tǒng)的拉格朗日方程為：

ddt-L-q·－-L-q+-D-q·=Eqτ+AT（q）λ，

式中：τ為車輪轉(zhuǎn)矩輸入矩陣，τ=τrτlT，其中τl、τr分別為左、右轉(zhuǎn)軸的轉(zhuǎn)矩；A（q）為系統(tǒng)的非完整約束矩陣；λ為拉格朗日乘子；Eq為簡(jiǎn)化后轉(zhuǎn)矩矩陣的系數(shù)矩陣，表達(dá)式為：

E（q）=000000－110－101T。

考慮車輪處于純滾動(dòng)狀態(tài)，滿足A（q）q·=0，則：

A（q）=－sin αcos αcos αsin αcos αsin α00b0－b000－R00－R。

A（q）的零空間矩陣S（q）使A（q）S（q）=0，則：

S（q）=00cos αsin α00010010001/R1/Rb/R－b/RT。

q·與Sq間滿足

q·=Sqυ，

式中：υ為車體運(yùn)動(dòng)狀態(tài)向量，υ=θ·?v?α·T，其中α·為車體偏航角速度。

根據(jù)歐拉-拉格朗日法建立雙輪平衡輪椅的動(dòng)力學(xué)方程為：

Mqq··+Vq，q·=Eqτ+AT（q）λ，（1）

式中：Mq為狀態(tài)變量二階項(xiàng)系數(shù)矩陣，Vq，q·為狀態(tài)變量及狀態(tài)變量一階項(xiàng)描述矩陣，q··為狀態(tài)變量的二階導(dǎo)數(shù)。

式（1）兩邊同乘ST（q）得：

［ST（q）M（q）S（q）］υ·+ST（q）［M（q）S·（q）υ+V（q，q·）］=ST（q）E（q）τ，

式中S·（q）為S（q）對(duì)q的一階導(dǎo)數(shù)。

ψr、ψl可通過(guò)θ·、v表達(dá)，初始狀態(tài)變量修正為q=（xyθα）T，為實(shí)現(xiàn)系統(tǒng)的平衡及速度控制，選取狀態(tài)變量為x=（θαθ·vα·）T，控制量u=（τrτl）T，建立系統(tǒng)狀態(tài)方程為：

x·=g（x）u+f（x），（2）

式中：g（x）為系統(tǒng)輸入矩陣，g（x）=02×2ST（q）M（q）S（q）－1ST（q）E（q）；f（x）系統(tǒng)狀態(tài)矩陣，f（x）=θ·α·－ST（q）M（q）S（q）－1ST（q）［M（q）S·（q）υ+V（q，q·）］。

2?模型預(yù)測(cè)控制

MPC依據(jù)系統(tǒng)模型狀態(tài)預(yù)測(cè)未來(lái)時(shí)間狀態(tài)，通過(guò)優(yōu)化系統(tǒng)狀態(tài)與目標(biāo)狀態(tài)的誤差求解控制量。軌跡跟蹤中已知目標(biāo)路徑，MPC可結(jié)合更多的狀態(tài)因素優(yōu)化控制量。

由式（2）可知：雙輪平衡輪椅動(dòng)力學(xué)系統(tǒng)為非線性系統(tǒng)，正常工作時(shí)狀態(tài)基本穩(wěn)定在平衡點(diǎn)，即θ=0，θ·=0，對(duì)該系統(tǒng)在平衡點(diǎn)處進(jìn)行麥克勞林展開，忽略高次項(xiàng)，得狀態(tài)偏差的一階導(dǎo)數(shù)

x·=Ax+Bu，

式中：x為狀態(tài)偏差，x=x－xd，其中xd為期望目標(biāo)狀態(tài)；u為輸入偏差，u=u－ud，其中ud為預(yù)期輸入狀態(tài)；A、B為線性系數(shù)矩陣，A=00100

00001

a310a33a340

a410a43a440

a510000a55，

B=00b31b41b5100b32b42b52，A、B中各元素的表達(dá)式分別為：

a31=［2（2mwIxx+mbIxx－mbIzz－2mwIzz）R2α·2－4mwmbR2glc+4Iwambl2cα·2－4Iwamblcg+4mwmbR2l2cα·2+

4（Ixx－Izz）Iwaα·2－2m2bR2glc］/K1，K1=－4IyymwR2－4mbmwl2cR2－4IyyIwa－4mbl2cIwa－2IyymbR2，a41=

［（Iyy－Ixx+Izz）mbR2lcα·2+m2bR2l2cg+m2bR2l3cα·2］/K2，K2=－IyymbR2－2IyyIwa－2IyymwR2－2mbl2cIwa－

2mbmwl2cR2，a51=mblcR2vα·/K3，K3=IzzR2+2IwdR2+2Iwdb2+2mwb2R2，a33=［－4Iwafb－（4mw+

2mb）R2 fb］/K1，a34=mbR2lcfb/K2，a43=4mblcfw/K1，a44=（－2fwmbl2c－2fwIyy）/K2，a55=2fwb2/K3，b31=b32=（mbR2+2Iwa+2mwR2+mblcR）/K1，b41=b42=－R（mblcR+Iyy+mbl2c）/K2，b51=－b52=bR/K3。

通過(guò)前向歐拉法將連續(xù)系統(tǒng)轉(zhuǎn)換為離散系統(tǒng)，采樣周期為T，k+1時(shí)刻的狀態(tài)誤差

xk+1=A—xk+B—uk，

式中：xk為k時(shí)刻的狀態(tài)誤差；uk為k時(shí)刻的輸入變化量；A—、B—為系統(tǒng)離散化后線性系數(shù)矩陣，A—=I+AT，I為單位矩陣，B—=BT。

取預(yù)測(cè)區(qū)間長(zhǎng)度為N，x（k+i|k）、u（k+i|k）為系統(tǒng)在k時(shí)刻對(duì)k+i（i=1，2，…，N）時(shí)刻的預(yù)測(cè)狀態(tài)，系統(tǒng)的預(yù)測(cè)誤差為e=x－xr，預(yù)期目標(biāo)xr=0，則e=x。為保證系統(tǒng)在預(yù)測(cè)區(qū)間內(nèi)運(yùn)行的狀態(tài)誤差、終末誤差及控制量之和最小，構(gòu)造系統(tǒng)的代價(jià)函數(shù)

J（x，u）=∑N－1i=0［x（k+i|k）TQx（k+i|k）+u（k+1|k）TRu（k+1|k）］+x（k+N|k）TFx（k+N|k），（3）

式中：Q為衡量系統(tǒng)狀態(tài)誤差的增益矩陣，R為系統(tǒng)輸入變化量的增益矩陣，F(xiàn)為終態(tài)增益矩陣。

通過(guò)建立的離散系統(tǒng)方程可解得預(yù)測(cè)區(qū)間內(nèi)系統(tǒng)狀態(tài)預(yù)測(cè)為：

x（k|k）=xkx（k+1|k）=A—xk+B—u（k|k）x（k+2|k）=A—2xk+A—B—u（k|k）+B—u（k+1|k）?x（k+N|k）=A—Nxk+A—N－1B—u（k|k）+…+B—u（k+N－1|k）。（4）

由式（4）得系統(tǒng)在k時(shí)刻的預(yù)測(cè)區(qū)間狀態(tài)方程為：

Xk=Mxk+CUk，（5）

式中：Xk為預(yù)測(cè)N步的狀態(tài)誤差矩陣，M為k時(shí)刻狀態(tài)誤差的系數(shù)矩陣，Uk為預(yù)測(cè)N步的輸入變化量，C為預(yù)測(cè)N步輸入變化量的系數(shù)矩陣，表達(dá)式分別為：

Xk=x（kk）x（k+1|k）x（k+N|k），M=I5×5A—A—2A—N，Uk=u（kk）u（k+1|k）u（k+N－1|k），C=05×205×2…05×2

B—05×2…05×2

A—B—B—…05×2

A—N－1B—

A—N－2B—

…B—。

將式（5）代入式（3）得：

J（x，u）=XkTQ—Xk+UkTR—Uk=Mxk+CUkTQ—Mxk+CUk+UkTR—Uk=

xkTMTQ—Mxk+2xkTMTQ—CUk+UkT（CTQ—C+R—）Uk，（6）

式中：Q—為由狀態(tài)增益矩陣與終態(tài)增益矩陣組成的對(duì)角陣，Q—=Q0…0

0Q…0

00…F；R—為輸入變化量增益矩陣組成的對(duì)角陣，R—=R…00…R。

式（6）中xkTMTQ—Mxk為初始狀態(tài)，不影響代價(jià)函數(shù)，可忽略，即將問題轉(zhuǎn)化為求解令J（x，u）最小的預(yù)測(cè)N步的輸入變化量Uk，可通過(guò)二次規(guī)劃求解，最終可得系統(tǒng)在k時(shí)刻的控制量為Uk（1）。

圖2?雙輪平衡輪椅差動(dòng)幾何運(yùn)動(dòng)模型

構(gòu)建雙輪平衡輪椅差動(dòng)幾何運(yùn)動(dòng)模型如圖2所示。用車體差動(dòng)幾何模型中心速度描述車體的行進(jìn)速度

v=vl+vr/2，

式中vl、vr分別為左、右車輪的速度。

車體的偏航角速度α·可看作右輪相對(duì)左輪旋轉(zhuǎn)的角速度，即

α·=vr－vl/l，

式中l(wèi)為輪距。

采用Pure Pursuit算法[19]規(guī)劃雙輪平衡輪椅的目標(biāo)軌跡，估計(jì)車體當(dāng)前位置與目標(biāo)點(diǎn)的距離偏差、角度偏差，得到車體轉(zhuǎn)動(dòng)角速度，即規(guī)劃系統(tǒng)的α、α·及v，將運(yùn)動(dòng)規(guī)劃結(jié)果作為動(dòng)力學(xué)模型的期望結(jié)果，使雙輪平衡輪椅系統(tǒng)依據(jù)自身的約束特性跟蹤運(yùn)動(dòng)狀態(tài)，實(shí)現(xiàn)雙輪平衡輪椅對(duì)預(yù)期位置的軌跡跟蹤，并保證響應(yīng)速度與跟蹤平穩(wěn)性。

3?仿真與分析

3.1?仿真系統(tǒng)搭建

采用軟件Simulink搭建雙輪平衡輪椅控制仿真系統(tǒng)，包括雙輪平衡輪椅動(dòng)力學(xué)模型、LQR和MPC控制器、運(yùn)動(dòng)規(guī)劃模塊及顯示模塊，如圖3所示。將既定行進(jìn)軌跡輸入運(yùn)動(dòng)規(guī)劃模塊得到雙輪平衡輪椅的期望狀態(tài)，通過(guò)LQR和MPC控制器進(jìn)行期望狀態(tài)跟蹤。分析雙輪平衡輪椅系統(tǒng)分別采用LQR、MPC控制器時(shí)的軌跡跟蹤效果及跟蹤平穩(wěn)性。

圖3?雙輪平衡輪椅控制仿真系統(tǒng)

為使雙輪平衡輪椅系統(tǒng)在控制器作用下穩(wěn)定運(yùn)行，設(shè)定LQR控制器的系統(tǒng)狀態(tài)增益矩陣QLQR及系統(tǒng)輸入增益矩陣RLQR分別為：

QLQR=1 000?0?0?0??0

0100?0?0??0

0?0100?0??0

0?0?0100??0

0?0?0?01 000，RLQR=0.01?00??0.01。

設(shè)定MPC控制器的系統(tǒng)狀態(tài)增益矩陣QMPC及系統(tǒng)輸入增益矩陣RMPC分別為：

QMPC=100000

050000

001000

0001000

00001 000，RMPC=0.05?00??0.05。

通過(guò)MATLAB中的lqr函數(shù)求解LQR控制器的反饋增益K，得到LQR控制律為u=－Kx。

借助MATLAB二次規(guī)劃函數(shù)quadprog求解式（6），可得預(yù)測(cè)區(qū)間內(nèi)的Uk，則當(dāng)前時(shí)刻MPC控制律為u=Uk（1）。

3.2?系統(tǒng)動(dòng)態(tài)響應(yīng)

設(shè)定雙輪平衡輪椅系統(tǒng)初始狀態(tài)x=0.090010T，即初始時(shí)刻θ=009 rad，v=1 m/s，車體期望狀態(tài)為平衡位置即自平衡狀態(tài)（θ=0，v=0），驗(yàn)證雙輪平衡輪椅系統(tǒng)分別在LQR和MPC控制器作用下的穩(wěn)定性，仿真結(jié)果如圖4所示。

a）LQR控制器????????????????????????b）MPC控制器

圖4?LQR和MPC控制器對(duì)雙輪平衡輪椅系統(tǒng)的穩(wěn)定性控制

由圖4可知：LQR和MPC控制器均可使車體從初始狀態(tài)恢復(fù)至平衡穩(wěn)定狀態(tài)，MPC控制器作用下車體可較快達(dá)到穩(wěn)定狀態(tài)。

設(shè)定雙輪平衡輪椅系統(tǒng)初始狀態(tài)為平衡位置，車體目標(biāo)狀態(tài)為xd=00010T，即以v=1 m/s穩(wěn)定前進(jìn)，驗(yàn)證LQR和MPC控制器對(duì)雙輪平衡輪椅系統(tǒng)速度的控制情況，仿真結(jié)果如圖5所示。由圖5可知：LQR和MPC控制器均可實(shí)現(xiàn)對(duì)雙輪平衡輪椅系統(tǒng)速度的跟蹤控制，在MPC控制器作用下車體較快響應(yīng)至目標(biāo)速度附近。

a）LQR控制器b）MPC控制器

圖5?LQR和MPC控制器對(duì)雙輪平衡輪椅系統(tǒng)的速度控制

雙輪平衡輪椅系統(tǒng)的動(dòng)態(tài)響應(yīng)指標(biāo)如表2所示。由表2可知：MPC控制器的動(dòng)態(tài)響應(yīng)速度較LQR控制器快，二者在速度跟蹤中均因系統(tǒng)線性化引入的模型偏差而無(wú)法準(zhǔn)確追蹤速度，但MPC控制器的速度跟蹤穩(wěn)態(tài)誤差較LQR控制器小。

3.3?系統(tǒng)軌跡追蹤

設(shè)定目標(biāo)軌跡為半徑0.4 m的圓形，循跡速度為0.2 m/s，雙輪平衡輪椅系統(tǒng)初始狀態(tài)為x=（0－1.5000）T，初始位置坐標(biāo)為（0.4 m，0），輪椅初始正方向與x軸夾角為－90°。分別采用Pure Pursuit算法、LQR控制器和MPC控制器跟蹤目標(biāo)軌跡，仿真結(jié)果如圖6所示。由圖6可知：Pure Pursuit算法僅通過(guò)運(yùn)動(dòng)幾何關(guān)系跟蹤目標(biāo)軌跡，初始階段需較大的轉(zhuǎn)彎半徑，LQR和MPC控制器的轉(zhuǎn)彎半徑明顯減小。

a）Pure Pursuit算法?????????????b）LQR控制器??????????????c）MPC控制器

LQR和MPC控制器跟蹤目標(biāo)軌跡時(shí)對(duì)雙輪平衡輪椅系統(tǒng)的狀態(tài)控制如圖7所示。由圖7可知：LQR和MPC控制器均對(duì)雙輪平衡輪椅系統(tǒng)狀態(tài)有較好的控制，車體速度逐步達(dá)到并穩(wěn)定在目標(biāo)速度附近，傾角始終穩(wěn)定在0°附近，由于要保持一定行進(jìn)速度，車體略向前傾，即θ>0°；當(dāng)輪椅抵達(dá)終點(diǎn)位置時(shí)，速度急劇減小，由于慣性車體短時(shí)間內(nèi)前傾角較大，但二者均可較好的控制車體平衡，恢復(fù)至平衡狀態(tài)。MPC控制器可較快完成規(guī)劃路徑的循跡，用時(shí)225 s，LQR控制器用時(shí)250 s，前者提前25 s達(dá)到指定速度，響應(yīng)速度較快；LQR控制器在經(jīng)過(guò)200 s（輪椅行進(jìn)至270°附近）時(shí)車體傾角狀態(tài)出現(xiàn)抖動(dòng)（圖7虛線框選位置），MPC控制器在整個(gè)循跡周期內(nèi)傾角和速度狀態(tài)較平穩(wěn)，控制效果較好。

a）LQR控制器????????????????????????b）MPC控制器

在終點(diǎn)位置，雙輪平衡輪椅系統(tǒng)因自身慣性無(wú)法形成Pure Pursuit算法的瞬間停止機(jī)制，LQR與MPC控制器的動(dòng)力學(xué)控制更接近實(shí)際運(yùn)行情況。LQR與MPC控制器對(duì)雙輪平衡輪椅系統(tǒng)的終點(diǎn)控制如圖8所示。

a）LQR控制器??????????????????????b）MPC控制器

圖8?LQR與MPC控制器對(duì)雙輪平衡輪椅系統(tǒng)的終點(diǎn)控制

由圖8可知：當(dāng)輪椅到達(dá)終點(diǎn)位置（0.4 m，0）時(shí)，車速?zèng)]有瞬間為0而是保持當(dāng)前的運(yùn)動(dòng)狀態(tài)，在控制器的作用下逐步修正車體狀態(tài)后停止。從終點(diǎn)處的實(shí)際追蹤軌跡來(lái)看，輪椅在LQR控制器作用下，越過(guò)終點(diǎn)位置后，未能及時(shí)到達(dá)目標(biāo)狀態(tài)，徘徊于目標(biāo)位置附近；MPC控制器相較于LQR控制器可較快到達(dá)目標(biāo)狀態(tài)，原因是MPC控制器在控制過(guò)程中引入未來(lái)一段時(shí)間的信息，獲得與目標(biāo)路徑更適配的控制量。

LQR和MPC控制器完成目標(biāo)路徑跟蹤的平均誤差分別為0.047、0.046 m，跟蹤精度相似。MPC控制器引入未來(lái)時(shí)間的狀態(tài)信息進(jìn)行跟蹤優(yōu)化，在跟蹤穩(wěn)定性及終點(diǎn)位置的控制表現(xiàn)較好。

4?結(jié)束語(yǔ)

通過(guò)歐拉-拉格朗日方程建立雙輪平衡輪椅系統(tǒng)非線性動(dòng)力學(xué)模型，對(duì)其進(jìn)行線性化及離散化，建立預(yù)測(cè)區(qū)間內(nèi)狀態(tài)誤差、輸入誤差及終點(diǎn)誤差的代價(jià)函數(shù)，采用二次規(guī)劃求解當(dāng)前時(shí)刻的最優(yōu)控制輸入。通過(guò)與幾何模型追蹤控制和LQR控制器追蹤控制對(duì)比，MPC控制器對(duì)雙輪平衡輪椅系統(tǒng)控制有較快的動(dòng)態(tài)響應(yīng)，在軌跡跟蹤過(guò)程引入系統(tǒng)內(nèi)部約束及未來(lái)狀態(tài)信息，在追蹤啟動(dòng)、終點(diǎn)處有效控制系統(tǒng)，優(yōu)化跟蹤路徑。本文僅針對(duì)平坦水平路面進(jìn)行動(dòng)力學(xué)建模分析，可繼續(xù)研究在斜面及顛簸路面的軌跡跟蹤。

參考文獻(xiàn)：

[1]?常宏，常鵬，蔣維，等.基于平衡車的自助上下樓梯輪椅[J].山東交通學(xué)院學(xué)報(bào)，2017，25（4）：71-75.

[2]?茅力非.兩輪自平衡移動(dòng)機(jī)器人建模與控制研究[D].武漢：華中科技大學(xué)，2013.

[3]?YANG C， JIANG Y， LI Z， et al. Neuralcontrol of bimanual robots with guaranteed global stability and motion precision[J].IEEE Transactions on Industrial Informatics， 2017，13（3）：1162-1171.

[4]?劉學(xué)明，梁春蘭，王學(xué)東.兩輪驅(qū)動(dòng)非完整約束機(jī)器人軌跡跟蹤研究[J].制造業(yè)自動(dòng)化，2013，35（16）：84-87.

[5]?黃用華，郭騫，莊未，等.兩輪車自行車機(jī)器人的點(diǎn)對(duì)點(diǎn)位置控制[J].機(jī)械設(shè)計(jì)與研究，2022，38（2）：38-43.

[6]?黃鶴，李文龍，楊瀾，等.ICPA-LQR優(yōu)化的兩輪平衡機(jī)器人自穩(wěn)定與軌跡跟蹤PID控制器設(shè)計(jì)[J/OL].哈爾濱工業(yè)大學(xué)學(xué)報(bào).（2022-11-09）[2023-02-15].http：//kns.cnki.net/kcms/detail/23.1235.T.20221108.1333.002.html.

[7]?高志偉，代學(xué)武.自平衡小車LQR-PID平衡與路徑跟蹤控制器設(shè)計(jì)[J].控制工程.2020，27（4）：708-714.

[8]?MURCIA H F， GONZALEZ A E. Performance comparison between PID and LQR control on a 2-wheel inverted pendulum robot[C]//2016 IEEE Colombian Conference on Robotics and Automation （CCRA）. Bogota， Colombia：IEEE，2016：1-6.

[9]?YUE M， NING Y G， ZHAO X D， et al. Point stabilization control method for WIP vehicles based on motion planning[J].IEEE Transactions on Industrial Informatics， 2019，15（6）：3368-3378.

[10]?寧一高.考慮內(nèi)穩(wěn)定與外避障的兩輪自平衡車軌跡規(guī)劃與跟蹤控制[D].大連：大連理工大學(xué)，2021.

[11]?梁棟.基于模型預(yù)測(cè)控制的無(wú)人駕駛汽車橫縱向綜合控制[D].重慶：重慶理工大學(xué)，2022.

[12]?龔大為.基于模型預(yù)測(cè)控制的自動(dòng)駕駛汽車橫向控制策略研究[D].重慶：重慶交通大學(xué)，2021.

[13]?YUAN T F， ZHAO R C.LQR-MPC-based trajectory-tracking controller of autonomous vehicle subject to coupling effects and driving state uncertainties[J].Sensors，2022，22（15）：5556.

[14]?陳素霞，黃全振，高繼勛.非完整約束移動(dòng)機(jī)器人自適應(yīng)軌跡跟蹤控制設(shè)計(jì)[J].中國(guó)測(cè)試，2021，47（11）：80-84.

[15]?張震.非完整約束移動(dòng)機(jī)器人的運(yùn)動(dòng)規(guī)劃與跟蹤控制技術(shù)研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2021.

[16]?徐慧，孫宏圖.全自主雙輪平衡車數(shù)學(xué)建模與控制系統(tǒng)研究[J].機(jī)械設(shè)計(jì)，2022，39（7）：111-116.

[17]?PATHAK K， FRANCH J， AGRAWAL S K. Velocity control of a wheeled inverted pendulum by partial feedback linearization[C]//2004 43rd IEEE Conference on Decision and Control. Nassau， Bahamas：IEEE， 2004：3962-3967.

[18]?KUMAR E V， JEROME J. Robust LQR controller design for stabilizing and trajectory tracking of inverted pendulum[J].Procedia Engineering， 2013， 64：169-178.

[19]?劉延彬，姜媛媛.基于路標(biāo)點(diǎn)追蹤的差速驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃[J].控制與決策，2023，38（9）：2529-2536.

Trajectory tracking of two-wheel balanced wheelchair based on

model predictive control

CHANG Hong，WANG Ze

School of Mechanical Engineering， Inner Mongolia University of Technology， Hohhot 010051， China

Abstract：In order to achieve the autonomous tracking travel of the two-wheel balanced wheelchair， a dynamic model of the two-wheel balanced wheelchair is established based on the Euler-Lagrange equation， and the motion relationship is established through the differential geometric model. The Pure Pursuit algorithm is used to plan the target motion state of the system， and model predictive control（MPC） and linear quadratic optimal control（LQR）are used to balance and track the motion state of the two-wheel balanced wheelchair. The simulation system for the control of the two-wheel balanced wheelchair is built through software Simulink to verify the trajectory tracking effect and tracking stability of the two-wheel balanced wheelchair system when LQR and MPC controllers are used. The simulation results show that the dynamic response speed of the MPC controller is faster than that of the LQR controller， and the MPC controller can quickly restore the stable state of the vehicle body when acting on the vehicle body; when tracking the trajectory， the MPC controller has better stable control of the vehicle body inclination angle and speed at the starting and ending positions than the LQR controller.

Keywords： two-wheel balanced wheelchair; LQR controller; MPC controller; trajectory tracking

（責(zé)任編輯：趙玉真）

收稿日期：2023-04-28

基金項(xiàng)目：內(nèi)蒙古自治區(qū)關(guān)鍵技術(shù)攻關(guān)計(jì)劃項(xiàng)目（2021GG0258）

第一作者簡(jiǎn)介：常宏（1992—），男，呼和浩特人，工學(xué)碩士，主要研究方向?yàn)闄C(jī)器人控制，E-mail：changhong@imut.edu.cn。

*通信作者簡(jiǎn)介：王賾（1974—），男，呼和浩特人，主要研究方向?yàn)轱w行器制造，E-mail：wangze@imut.edu.cn。

DOI：10.3969/j.issn.1672-0032.2024.02.001