董慧芬，宋金海

(中國(guó)民航大學(xué)電子信息與自動(dòng)化學(xué)院，天津 300300)

1 引言

當(dāng)今機(jī)器人快速發(fā)展，各式各樣的機(jī)器人琳瑯滿目，醫(yī)療機(jī)器人、服務(wù)機(jī)器人、仿人機(jī)器人、平衡機(jī)器人等等[1-3]。目前平衡機(jī)器人是國(guó)內(nèi)外的研究熱點(diǎn)，隨著傳感器技術(shù)和微控制器技術(shù)發(fā)展，其中非同軸前后輪機(jī)器人因?yàn)槠浜?jiǎn)單結(jié)構(gòu)、成本低廉、能在地勢(shì)起伏較大的情況下仍能保持良好的性能，成為了當(dāng)前平衡機(jī)器人的主流[4-5]。由于該機(jī)器人是一種具有強(qiáng)耦合、欠驅(qū)動(dòng)的非線性系統(tǒng)[6]，關(guān)于其動(dòng)力學(xué)建模和提出新的控制方法引起了許多學(xué)者的興趣。Ham等人提出了一種簡(jiǎn)單的自行車機(jī)器人的動(dòng)力學(xué)模型，在此基礎(chǔ)上設(shè)計(jì)了一個(gè)內(nèi)部平衡控制器[7]；密歇根大學(xué)的Anouck等制作了非同軸的兩輪小車，利用拉格朗日方程分別建立了單陀螺和雙陀螺動(dòng)力學(xué)模型[8]，在平衡點(diǎn)附近進(jìn)行簡(jiǎn)單地線性化，對(duì)線性化后的模型進(jìn)行控制和分析，但是由于其線性化過(guò)于粗略，無(wú)法實(shí)現(xiàn)較大傾角的回正。武漢理工大學(xué)的趙燕教授對(duì)雙陀螺的非同軸前后輪機(jī)器人采用拉格朗日進(jìn)行動(dòng)力學(xué)建模[9]，設(shè)計(jì)了滑?？刂破鬟M(jìn)行機(jī)器人的平衡控制，但是對(duì)于該機(jī)器人的轉(zhuǎn)彎動(dòng)力學(xué)模型沒(méi)有深入研究；俄亥俄州立大學(xué)的Harun Yetkin 等人研發(fā)了一種基于陀螺進(jìn)動(dòng)效應(yīng)的自平衡車[10]，采用 Lagrange 方程建立自平衡車的動(dòng)力學(xué)模型，設(shè)計(jì)了一階滑模控制器和 PID 控制器，試驗(yàn)表明滑模控制器的效果比 PID 控制器效果好，但其采用的單陀螺布局會(huì)產(chǎn)生干擾自行車俯仰或者偏航運(yùn)動(dòng)的力矩，在自行車轉(zhuǎn)彎過(guò)程時(shí)由于單陀螺布局容易導(dǎo)致側(cè)傾發(fā)生，無(wú)法完成轉(zhuǎn)彎運(yùn)動(dòng)。2015年呂寬洲等人設(shè)計(jì)一款帶有陀螺裝置的無(wú)人自行車[11]，通過(guò)設(shè)計(jì)平衡控制算法直接作用于陀螺，產(chǎn)生陀螺力矩維持自行車平衡，實(shí)現(xiàn)了無(wú)人自行車在靜止?fàn)顟B(tài)能自動(dòng)恢復(fù)到豎直狀態(tài)，但對(duì)于受到撞擊干擾等能否快速恢復(fù)到平衡狀態(tài)沒(méi)有進(jìn)行深入展開(kāi)。北京凌云智能科技公司提出了一種基于陀螺效應(yīng)的兩輪電動(dòng)車的概念車[12]，其通過(guò)駕駛者人工控制方向盤和陀螺產(chǎn)生的陀螺力矩來(lái)平衡重力矩，維持兩輪汽車在行駛過(guò)程保持平衡狀態(tài)，沒(méi)有進(jìn)行自平衡的無(wú)人自動(dòng)控制方面的研究。

根據(jù)上述國(guó)內(nèi)外對(duì)于非同軸前后輪機(jī)器人的動(dòng)力學(xué)建模和控制器的研究，主要的側(cè)重點(diǎn)都在于車體在直線運(yùn)動(dòng)或者靜止時(shí)的自平衡控制方面，對(duì)于機(jī)器人的轉(zhuǎn)彎動(dòng)力學(xué)建模、一定轉(zhuǎn)向角下平衡車的姿態(tài)控制沒(méi)有深入的研究。因此本文在建立平衡車轉(zhuǎn)彎動(dòng)力學(xué)非線性模型的基礎(chǔ)上，采用反饋線性化方法進(jìn)行非線性模型的線性化，而不是在平衡點(diǎn)附近的線性化處理，對(duì)前后輪平衡車轉(zhuǎn)彎控制進(jìn)行研究，減小線性化誤差的影響。

2 非同軸前后輪平衡車轉(zhuǎn)彎動(dòng)力學(xué)建模

2.1 陀螺平衡原理

本文中非同軸前后輪平衡車采用陀螺作為平衡單元，如圖1所示，陀螺自轉(zhuǎn)軸為y軸，進(jìn)動(dòng)軸為Z軸。陀螺繞著自轉(zhuǎn)y軸以w0的角速度進(jìn)行自轉(zhuǎn)，陀螺相對(duì)于y軸轉(zhuǎn)動(dòng)慣量是J0，當(dāng)陀螺繞著Z軸以角速度進(jìn)動(dòng)時(shí)，根據(jù)右手定則陀螺將會(huì)產(chǎn)生一個(gè)沿著X軸方向的力矩M。

圖1 陀螺力矩產(chǎn)生原理

由于進(jìn)動(dòng)角度的產(chǎn)生，陀螺力矩的方向會(huì)改變，可以分解為偏航力矩和傾斜平衡力矩，前者是影響平衡車方向的干擾力矩，后者是平衡傾斜姿態(tài)的力矩。所以文中采用雙陀螺平衡方案，為了讓偏航力矩抵消、平衡力矩疊加，要求兩個(gè)陀螺的自轉(zhuǎn)方向和進(jìn)動(dòng)方向都相反，如圖2。

圖2 雙陀螺平衡原理

2.2 自平衡車轉(zhuǎn)彎原理分析

當(dāng)平衡車進(jìn)行轉(zhuǎn)彎時(shí)，會(huì)產(chǎn)生一個(gè)使平衡車側(cè)翻的離心力，通過(guò)改變車身的傾斜與車身的速度可使平衡車不產(chǎn)生側(cè)傾。利用這一現(xiàn)象，對(duì)平衡車一定傾斜角下的轉(zhuǎn)彎動(dòng)力學(xué)分析，圖3(a)為平衡車后視簡(jiǎn)化圖，圖3(b)為車身側(cè)傾一個(gè)角度進(jìn)行轉(zhuǎn)彎運(yùn)動(dòng)力學(xué)分析，其中，O為平衡車的重心，G為平衡車的重力，F(xiàn)N為地面對(duì)平衡車的支撐力，f為前后輪所受的摩擦力，h為平衡車質(zhì)心離地面的高度。

圖3 平衡車轉(zhuǎn)向力學(xué)分析示意圖

由圖3(a)可知，平衡車在向右進(jìn)行轉(zhuǎn)向時(shí)，受到與轉(zhuǎn)向方向相反的一個(gè)摩擦力f，導(dǎo)致平衡車在轉(zhuǎn)向時(shí)候向左翻轉(zhuǎn)。圖3(b)是通過(guò)平衡車傾斜一定角度β，轉(zhuǎn)彎所需的向心力由車體傾斜產(chǎn)生的分力提供，車體便不會(huì)發(fā)生傾倒。所以轉(zhuǎn)向時(shí)的向心力與自平衡車的橫滾角之間存在著耦合關(guān)系。如下式(1)

fh=FNhtan(β)

(1)

由式(1)可知

f=Gtan(β)

(2)

平衡車轉(zhuǎn)彎示意圖如圖4所示，表示平衡車的轉(zhuǎn)向角，L為平衡車前后輪心間距，O為平衡車的轉(zhuǎn)彎圓心，R為轉(zhuǎn)彎半徑，v為平衡車前進(jìn)線速度，則平衡車轉(zhuǎn)彎所需要的向心力為F向心力=2mv/R。

圖4 平衡車轉(zhuǎn)向俯視圖

假設(shè)機(jī)器人所提供的最大靜摩擦力等于所需要的向心力，則機(jī)器人傾斜角β與轉(zhuǎn)彎半徑的表達(dá)式如式(3)

(3)

根據(jù)正弦定理可知

(4)

式(3)與(4)可知平衡車平穩(wěn)地進(jìn)行轉(zhuǎn)彎運(yùn)動(dòng)需要對(duì)傾斜角與車身的速度進(jìn)行協(xié)調(diào)控制，下面將對(duì)平衡車轉(zhuǎn)向動(dòng)力學(xué)進(jìn)行建模。

2.3 自平衡車的轉(zhuǎn)彎動(dòng)力學(xué)模型

將前后輪平衡車系統(tǒng)可以看作由三個(gè)剛體部分組成：車體、高轉(zhuǎn)速的陀螺、前后輪。以β表示平衡車的傾斜角，α表示平衡車的轉(zhuǎn)向角，θ表示陀螺的進(jìn)動(dòng)角。如下圖5(a)和圖5(b)所示，車輪的質(zhì)量為m1且半徑為r1，車體的重量為m2、質(zhì)心高度為h1，車體沿經(jīng)過(guò)前后輪直線的轉(zhuǎn)動(dòng)慣量為Ib。陀螺的質(zhì)量為m3，質(zhì)心高度為h2，自轉(zhuǎn)軸的轉(zhuǎn)動(dòng)慣量為Jr，沿著x軸的轉(zhuǎn)動(dòng)慣量為Jp。假設(shè)平衡車初始線速度為v0，車輪和車體有2個(gè)方向的運(yùn)動(dòng)，分別是平動(dòng)和傾斜方向。

圖5 平衡車結(jié)構(gòu)簡(jiǎn)圖

從上述假設(shè)和平衡車參數(shù)可知平衡車的動(dòng)能

T=Tb+Tg+Tf+Tw

(5)

不考慮其它不確定的外部干擾，系統(tǒng)的所受廣義力中重力為有勢(shì)力，則系統(tǒng)的勢(shì)能為

V=2m1gr1cosθ+m2gh1cosθ+2m3gh2cosθ

(6)

對(duì)于廣義坐標(biāo)β，受到的非有勢(shì)廣義力矩為

(7)

其中α為平衡車的轉(zhuǎn)角，Wd為后輪驅(qū)動(dòng)電機(jī)的功，L是前后輪著地點(diǎn)的間距。

對(duì)于廣義坐標(biāo)θ，受到的非有勢(shì)廣義力矩為

(8)

其中Tt為進(jìn)動(dòng)電機(jī)的力矩。

對(duì)于廣義坐標(biāo)α，受到的非有勢(shì)廣義力矩為

(9)

其中Twh為轉(zhuǎn)向電機(jī)的力矩，μ轉(zhuǎn)向架與車體的摩擦系數(shù)。

選取平衡車的傾斜運(yùn)動(dòng)陀螺進(jìn)動(dòng)兩個(gè)自由度為系統(tǒng)的廣義坐標(biāo)，對(duì)于廣義坐標(biāo)β通過(guò)拉格朗日方程可以的到

+2sinθcosθ(Ip-Ir)-g(m2h1+m3h2)sinβ=

(10)

對(duì)于廣義坐標(biāo)θ通過(guò)拉格朗日方程可得

(11)

對(duì)于廣義坐標(biāo)α通過(guò)拉格朗日方程可得

(12)

設(shè)

u1=wd，u2=Tt，u3=Twh

由上式(10)-(12)可得平衡車的非線性轉(zhuǎn)彎動(dòng)力學(xué)模型如式(13)所示

(13)

其中

C=(2m1r+m2h)g，D=g(m2h1+m3h2)，E=2Ipw

I=Ip-Ir，e1=Ip，e2=Ir，

3 自平衡車轉(zhuǎn)彎非線性動(dòng)力學(xué)模型反饋線性化

將平衡車的轉(zhuǎn)彎動(dòng)力學(xué)模型式(11)轉(zhuǎn)化為矩陣形式，可以得到下面MIMO非線性模型

(14)

其中，x∈R6，u∈R3，y∈R3，f(x)為6維的光滑的向量場(chǎng)，G(x)是6*3的矩陣，具體如下公式

H(x)=[h(x1)，h(x2)，h3(x3)]

x=[x1，x2，x3，x4，x5，x6]，y=[y1，y2，y3]

G(x)=[g1(x)，g2(x)，g3(x)，g4(x)，g5(x)，g6(x)]

u=[u1，u2，u3]T

可以得到如下的矩陣

該矩陣在平衡點(diǎn)x0=(0，x2，0，x4，0，x6)T的鄰域非奇異，所以系統(tǒng)在此鄰域總相對(duì)階為6階，則有

其中v=[v1，v2]為新輸入向量。由A-1(x)存在可得

(15)

通過(guò)狀態(tài)反饋控制率得到非線性系統(tǒng)的線性化和對(duì)于新輸入的解耦，在結(jié)合控制率對(duì)上述的等效線性系統(tǒng)進(jìn)行研究。平衡車的線性化控制結(jié)構(gòu)框圖如圖6所示。

圖6 轉(zhuǎn)彎非線性動(dòng)力學(xué)模型線性化控制結(jié)構(gòu)框圖

4 基于反饋線性化轉(zhuǎn)彎平衡控制仿真及結(jié)果分析

對(duì)于前后輪自平衡車，應(yīng)用經(jīng)過(guò)反饋線性化后的轉(zhuǎn)彎動(dòng)力學(xué)模型，設(shè)計(jì)PID控制器，可使小車的傾斜角快速穩(wěn)定到達(dá)給定值?；贛atlab中Simulink搭建如圖6所示仿真模型，根據(jù)實(shí)驗(yàn)平臺(tái)準(zhǔn)確估計(jì)獲取小車模型參數(shù)如表1。

表1 平衡車參數(shù)

根據(jù)前面的公式計(jì)算得到A、B、C、D、E、I、e1、e2的值(其中g(shù)=9.8m/s2)，搭建Simulink模型進(jìn)行仿真驗(yàn)證。下面實(shí)驗(yàn)分析圖中虛線均為車身的傾斜角，實(shí)線為給定傾斜角的輸入。

4.1 未使用反饋線性化解耦控制仿真結(jié)果分析

仿真中，為了探究在轉(zhuǎn)向過(guò)程中平衡問(wèn)題，在模型未進(jìn)行反饋線性化的前提下使用PID控制器進(jìn)行轉(zhuǎn)彎控制，給定輸入車速為1m/s，外部干擾力為0，在運(yùn)動(dòng)2s后給定轉(zhuǎn)向角度為30°，根據(jù)式(3)，需要車身傾斜角保持在6°。

如圖7所示，虛線為期望的轉(zhuǎn)向角，實(shí)線為實(shí)際轉(zhuǎn)向角。在平衡車運(yùn)動(dòng)過(guò)程中，在0s時(shí)輸入一個(gè)幅值為30°的單位階躍信號(hào)作為轉(zhuǎn)向角度的輸入，由圖可知實(shí)際的轉(zhuǎn)向角無(wú)法達(dá)到預(yù)期要求。

圖7 普通pid控制器下α的響應(yīng)

如圖8所示，由陀螺的進(jìn)動(dòng)原理可知，由于陀螺的進(jìn)動(dòng)角一直在變化結(jié)果導(dǎo)致平衡車的傾斜角隨之改變。

圖8 普通pid控制器下θ的響應(yīng)

如圖9所示，平衡車不斷地在-5°到-20°之間晃動(dòng)，無(wú)法達(dá)到預(yù)期的6°。在實(shí)際中意味著平衡車無(wú)法在轉(zhuǎn)彎時(shí)保持自身的平衡，無(wú)法完成轉(zhuǎn)向過(guò)程。

圖9 普通pid控制器下的β的響應(yīng)

4.2 使用反饋線性化轉(zhuǎn)彎控制仿真結(jié)果分析

將非線性的轉(zhuǎn)彎系統(tǒng)進(jìn)行反饋線性化后，加入PID控制器保持車身速度1m/s，在0s時(shí)輸入一個(gè)幅值為30°的單位階躍信號(hào)作為轉(zhuǎn)向角度的輸入。

如圖10所示，平衡車在1s內(nèi)轉(zhuǎn)向角度達(dá)到預(yù)期的30°，超調(diào)量為0。

圖10 使用反饋線性化解耦控制的α的響應(yīng)

根據(jù)上面的式(3)，車身的傾斜角度需要到達(dá)6°，在下圖11中在2s內(nèi)達(dá)到預(yù)期傾斜角度，與上式(3)一致。

圖11 使用反饋線性化解耦控制的β的響應(yīng)

圖12中陀螺的進(jìn)動(dòng)角在在0-2s時(shí)，產(chǎn)生進(jìn)動(dòng)效應(yīng)對(duì)車身的傾斜角進(jìn)行改變，在2-3s時(shí)，由于車身達(dá)到預(yù)期的傾斜角度，進(jìn)動(dòng)角的變化趨于0并且回復(fù)初始位置。

圖12 使用反饋線性化解耦控制的θ的響應(yīng)

4.3 抗干擾仿真結(jié)果分析

在保持一定傾斜角穩(wěn)定轉(zhuǎn)彎運(yùn)行時(shí)，為了模擬受到碰撞等瞬時(shí)外部干擾，如下圖13所示，在5s時(shí)刻加入一個(gè)大小為100N，持續(xù)時(shí)間為0.1s的干擾力。

圖13 外部干擾力

仿真結(jié)果如圖14所示，在受到外力干擾下，平衡車的轉(zhuǎn)彎平衡狀態(tài)受到打破，傾斜角度由穩(wěn)態(tài)的6°變?yōu)樽畲笾?.6°，經(jīng)過(guò)3.9s回復(fù)到原來(lái)穩(wěn)態(tài)值6°。由圖15可知，陀螺的進(jìn)動(dòng)角發(fā)生變化，最大值為2.4°，產(chǎn)生進(jìn)動(dòng)力矩使平衡車在3.6s內(nèi)回復(fù)平衡狀態(tài)，進(jìn)動(dòng)角穩(wěn)態(tài)誤差為0，由此可知在一定干擾下平衡車能快速回復(fù)，保持自身的平衡穩(wěn)定。

圖14 干擾仿真傾斜角變化曲線

圖15 干擾仿真進(jìn)動(dòng)角θ變化曲線

5 結(jié)論

本文以自平衡車車體左右傾斜角、陀螺進(jìn)動(dòng)角和轉(zhuǎn)向角為廣義坐標(biāo)，利用拉格朗日法建立平衡車轉(zhuǎn)向的MIMO仿射非線性動(dòng)力學(xué)模型；提出一種基于非線性動(dòng)力學(xué)模型的轉(zhuǎn)彎平衡控制方法。仿真分析結(jié)果可知，采用基于反饋精確線性化理論設(shè)計(jì)的控制器，可以實(shí)現(xiàn)雙陀螺平衡車的轉(zhuǎn)彎平衡控制。在轉(zhuǎn)向角30°和速度為1m/s的情況下，該平衡車可以在保持一定傾角情況下，完成轉(zhuǎn)向運(yùn)動(dòng)以及在受到外界瞬時(shí)100N干擾力下保持轉(zhuǎn)彎平衡穩(wěn)定，驗(yàn)證了所建立的平衡車的轉(zhuǎn)彎動(dòng)力學(xué)模型的正確性。與一般的近似線性化方法對(duì)比，在使用多輸入多輸出系統(tǒng)的輸出反饋線性化算法設(shè)計(jì)控制算法，不需要做出其它線性近似處理，取得了更好的效果。