輪履模式可切換的移動機器人建模與控制

梁志遠,趙佳輝,李清都,陳媛媛

(1.上海理工大學 機器智能研究院,上海 200093;2.上海理工大學 光電信息與計算機工程學院,上海 200093;3.海軍軍醫(yī)大學 海軍軍醫(yī)系,上海 200433)

0 引 言

隨著機器人技術的發(fā)展和廣泛應用,移動機器人面臨著更加復雜的環(huán)境。因此,能夠實現快速移動、應對復雜地形、克服障礙的移動機器人是目前熱門的研究主題[1-2]。

按照運動形式,移動機器人大致分為4類:足式、履帶式、輪式和復合式[3]。雖然雙足[4]、四足[5]機器人在克服障礙,爬樓梯等方面表現性能良好,但是以犧牲穩(wěn)定性作為前提,控制器復雜需要大量的計算去執(zhí)行動作,而且負載低,續(xù)航也短。相比之下,履帶式機器人[6]依靠結構上的設計便能夠輕松克服障礙,但是身體笨重、靈活性差、能效低、履帶磨損大。輪式機器人[7]移動快、能效高、續(xù)航久,但一般無法處理崎嶇地形,特別是超過輪子半徑的障礙等情形。復合式機器人則將2種或2種以上運動形式結合在一起,2種常見的結合形式為輪足式和輪履式[8],可以兼具不同運動形式的優(yōu)點,環(huán)境適應能力強,這使得復合式機器人成為移動機器人領域的熱點之一。

近年來,復合式機器人的研究取得了不少成果。蘇黎世聯邦理工學院的雙輪足式機器人Ascento[9]利用線性二次型調節(jié)器(linear quadratic regulator, LQR)來實現平地動態(tài)平衡控制,采用跳躍的方式克服障礙。四輪足式機器人ANYmal[10]利用模型預測控制(model predictive control,MPC)實現在陡峭的地面快速移動以及爬樓梯等復雜任務。傳統(tǒng)輪履機器人采用結構疊加的方式來實現輪式與履帶式的結合,而文獻[11]的輪履式機器人,利用履帶伸縮結構切換輪式與履帶式運動。上述機器人均存在一些不足之處。Ascento和ANYmal采用腿式結構,負載能力差,復雜路面需要視覺輔助,盲適應性差。前者跳躍式越障犧牲系統(tǒng)穩(wěn)定能力,無法應對崎嶇地形;后者足式運動速度慢、控制器復雜、運動效率低。傳統(tǒng)的變形輪履結構設計冗余復雜,不易擴展其他功能。文獻[11]的伸縮機構則導致履帶長度受限,車輪較大,車身較短,履帶越障能力下降。

針對上述復合式機器人的不足之處,本文提出一種具有雙輪平衡模式和履帶模式的新型輪履移動機器人模型,其側視圖如圖1所示。該機器人由車身、驅動輪和履帶擺臂構成;采用輪履耦合結構,整體結構簡單而緊湊;車身可以安裝機械爪,配合車身執(zhí)行抓取任務;履帶擺臂包括履帶、連桿和從動輪。機器人共有4個驅動電機,其中2個驅動左右驅動輪,另外2個驅動左右履帶擺臂。

圖1 機器人整體側視圖Fig.1 Concept of robot side view

本文機器人具有2種運動模式,可通過控制擺臂調整機器人質心,實現模式間切換:①雙輪平衡模式,實現平坦路面快速移動,具有高能效和長續(xù)航的能力;②履帶模式,應對復雜路面及障礙,且具有高負載能力。新型機器人融合輪式和履帶式優(yōu)點,地形適應性強,可用于執(zhí)行巡檢安防、勘探救援等任務。

目前,對于雙輪平衡和履帶式這類單一模式機器人的理論研究已較為成熟,本文針對具有雙輪平衡和履帶模式這一新型結構,在控制上實現不同姿態(tài)下的動態(tài)平衡和輪履模式的快速穩(wěn)定切換,以提高機器人的地形適應能力。

1 機器人動力學建模

1.1 等效倒立擺模型

在雙輪平衡模式下,輪履移動機器人的擺臂運動會改變機器人質心和平衡點位置,因此,可先將其等效為變質心倒立擺系統(tǒng),使模型降階,簡化動力學方程。機器人倒立擺等效模型如圖2所示,主要的模型參數如表1所示。

圖2 平面倒立擺等效模型Fig.2 Equivalent model of inverted pendulum

表1 機器人模型參數

本文定義如下參數。

{I}:慣性測量單元(IMU)坐標系。

{M}:固連在車軸上的機械坐標系。

αl,αr:左右擺臂相對前進方向的姿態(tài)角。

βl,βr:車身與左右擺臂的關節(jié)角。

θ0:不同擺臂關節(jié)角下機器人平衡點。

θl,θr:左右擺臂與等效倒立擺夾角。

θ,lc,mc:等效倒立擺俯仰角、質心距車軸距離、質量。

Ic1,Ic2,Ic3:等效倒立擺在其質心系下的轉動慣量。

機器人擺臂由連桿、履帶和驅動輪組成,其中履帶質量忽略不計,擺臂質量為ma=ms+msw,擺臂質心距車軸距離為la=(msls+mswlsw)/(ms+msw),本文定義sθ=sinθ,cθ=cosθ,左右擺臂和車身質心在{M}下的位置為Rr=[lasθrlacθr],Rl=[lasθllacθl],Rb=[-lbsθ0lbcθ0]。

根據質點系定義得等效倒立擺質心位置為

(1)

機器人處于平衡點時,倒立擺質心位置在X軸的分量Rcx=0,質心距車軸距離則為在Z軸的分量,即lc=Rcz。

令θr=βr-θ0,θl=βl-θ0,可得平衡點θ0,質心距車軸距離lc分別為

(2)

令θr=π/2-αr,θl=π/2-αl,可得在期望擺臂姿態(tài)角αl,αr下的擺臂關節(jié)角逆解為

(3)

等效倒立擺質心的轉動慣量Ic1,Ic2,Ic3可根據動能守恒推導出,假設機器人繞著坐標系Y軸以wy角速度轉動,則倒立擺質心動能Ecy等于車身和擺臂的質點組動能Ey,表示為

(4)

(5)

記繞著X、Z軸的質心動能和質點組動能分別為Ecx,Ex和Ecz,Ez,可得

(6)

(7)

倒立擺質心的轉動慣量Ic1,Ic2,Ic3與左右擺臂關節(jié)位置關系為

(8)

1.2 拉格朗日方程建模

輪履移動機器人經過變換后可以看作一個兩輪倒立擺機器人,其原理如圖3所示,俯視圖如圖4所示,其中,{W}為世界坐標系,{M}為固連在車軸中心的機械坐標系,{C}為位于倒立擺質心的質心坐標系。圖3中部分參數定義如下。

γl,γr:左右驅動輪轉動角度。

vl,vr:左右驅動輪線速度。

vm,vc:機械原點M和倒立擺質心線速度。

Tl,Tr:左右驅動輪力矩。

fl,fr:左右驅動輪阻尼力。

圖3 倒立擺機器人原理圖Fig.3 Schematic diagram of inverted pendulum robot

機器人運動過程由直線、俯仰和偏航運動共同組成[12],分別通過控制機器人位置x、俯仰角θ和偏航角φ來實現。為了完整描述機器人的運動,總共需要13個廣義坐標:xm,ym,xl,yl,xr,yr,xc,yc,zc,γl,γr,θ,φ。對于參考系{W},每個剛體的質心位置為

xc=xm+lcsθcφ,yc=ym+lcsθsφ,zc=lccθ,

xl=xm-0.5Dsφ,yl=ym+0.5Dcφ,

xr=xm+0.5Dsφ,yr=ym-0.5Dcφ

(9)

圖4 機器人俯視圖Fig.4 Top view of robot

假設機器人驅動輪運動時與地面無相對滑動[12],則機器人位移x和偏航角φ為

(10)

對(10)式微分后化簡,得驅動輪角速度為

(11)

則從動輪角速度為

(12)

對(9)式微分后得到各個剛體的質心速度為

(13)

拉格朗日方法需要將機器人剛體的動能和勢能表示為廣義坐標的函數,機器人的平動動能可表示為

(14)

機器人的轉動動能可表示為

(15)

(15)式中:第2項為倒立擺質心繞y軸的轉動動能,考慮了倒立擺俯仰角θ的影響;第5項為從動輪的轉動動能,這是因為在計算質心轉動慣量時沒有考慮從動輪自身轉動。

當機器人在平面運動時,垂直運動僅與俯仰角θ有關,因此勢能為

V=mcglccθ

(16)

拉格朗日函數定義為

L=Ttrans+Trot-V

(17)

機器人的廣義坐標為位置x,俯仰角θ和偏航角φ,對應的廣義力Fx,Fθ,Fφ可以表示為

(18)

則拉格朗日運動方程可表示為

(19)

將(11)—(18)式代入(19) 式可以得到機器人3個獨立的非線性運動方程,表示為

(20)

(21)

(22)

1.3 狀態(tài)空間方程

平衡模式下,機器人通常工作在平衡點附近。當俯仰角|θ|≤10°時,系統(tǒng)在平衡點處線性化后的狀態(tài)方程與原非線性方程的誤差很小[13],此時可以近似地認為sinθ≈θ,cosθ≈1,在忽略所有的二次項和交叉項后, (20)—(22)式可整理為

(23)

(24)

(25)

由(23)—(25)式,有

(26)

由線性化后的運動方程(24)—(26)式可看出,機器人位置x和俯仰角θ相互耦合,偏航角φ則相對獨立,表現為機器人前后運動和俯仰角有關,轉向運動獨立[13],因此,可以將機器人的狀態(tài)方程解耦為2個單獨系統(tǒng)。

定義解耦后的2個系統(tǒng)輸入轉矩為Tx和Tφ,其中,Tφ=(Tr-Tl)/2。

則有

(27)

描述機器人前后運動的四階系統(tǒng)的狀態(tài)方程為

(28)

(28)式中:u1=Tx;C1為4×4單位矩陣;狀態(tài)變量x1選取為機器人位移、速度、俯仰角度和角速度,表示為

(29)

描述機器人轉向運動的二階系統(tǒng)的狀態(tài)方程為

(30)

(30)式中:u2=Tφ;C2為2×2單位矩陣;狀態(tài)變量x2選取為機器人偏航角和角速度,表示為

(31)

當機器人處于默認姿態(tài)時,擺臂關節(jié)角βr=βl=0。由(2)式和(8)式可得,此時機器人平衡點θ0=0,質心高度lc=0.206 452 m,質心慣量Ic1=0.303 819 kg·m2,Ic2= 0.254 564 kg·m2,Ic3=0.055 684 9 kg·m2,代入參數后得

(32)

(33)

(34)

2 LQR控制器設計

本文機器人的開環(huán)系統(tǒng)在雙輪平衡模式下具有明顯的不穩(wěn)定性,經計算得前后運動系統(tǒng)的特征值為[0,-0.905 14,-0.072 3,8.719 9],在平衡點附近是不穩(wěn)定的。系統(tǒng)的完全能控性是反饋控制器設計的前提[14],系統(tǒng)能控性矩陣為

M=[BAB…AnB]

(35)

經分析可知,rank(M1)=4,rank(M2)=2,能控矩陣滿秩,系統(tǒng)完全能控。

平衡模式下機器人需要一個穩(wěn)定的控制器來保證其在保持平衡的同時,能夠穩(wěn)定前進、可動態(tài)調整擺臂姿態(tài)以及在輪履模式間進行平滑切換。機器人還應該能夠處理外部干擾,同時應使用盡可能小的空間進行模式切換。為此,本文基于LQR方法設計控制器,這是一種能夠以最小代價實現線性系統(tǒng)調節(jié)的最優(yōu)控制策略,二次型最優(yōu)函數為

(36)

(36)式中:Q是半正定矩陣,表示對x的每個狀態(tài)的動態(tài)誤差的懲罰程度;R是正定矩陣,表示對控制輸出所消耗能量的懲罰[14]。

(37)

(38)

(38)式中,ep,wp分別為IMU的俯仰角度和俯仰角速度。

圖5 系統(tǒng)控制回路Fig.5 Control loop of the system

3 實驗與仿真分析

本文選用Webots仿真平臺對機器人進行實時仿真。Webots是一款開源的機器人仿真軟件,主要用于機器人建模、控制與仿真和驗證機器人控制算法,其內核基于開源物理引擎(open dynamics engine,ODE),動力學仿真結果較為真實。本實驗使用Webots自帶的模型庫、零件庫等搭建了輪履移動機器人仿真模型,并對設計的控制器效果與輪履模式切換策略進行仿真驗證。

3.1 階躍響應

從圖6可以看出,姿態(tài)補償后的LQR控制器可以實現不同擺臂角度下參考指令的快速跟蹤,且響應無超調,調節(jié)時間短。擺臂0°時,穩(wěn)定時間約為6 s;擺臂90°時,穩(wěn)定時間為約為5 s。這是因為在同一組控制器參數下,前者倒立擺質心到車軸距離lc=0.206 5,而后者lc=0.167 5,質心低,穩(wěn)定快。

圖6 不同擺臂位置階躍響應下狀態(tài)曲線對比Fig.6 Comparison of state curves under step response of different swing arm positions

3.2 履帶擺臂控制

圖7 擺臂周期運動時狀態(tài)曲線對比Fig.7 Comparison of state curves during periodic motion of the swing arm

從圖7可以看出,基于姿態(tài)補償后的LQR控制器雖然不能完全消除擺臂運動帶來的干擾,但從幅值上看狀態(tài)數據波動很小,其中,位置x的振幅在±0.02 m之間,俯仰角θ的振幅在±0.007 4 rad之間,由此可以推斷姿態(tài)補償對擺臂運動的干擾有很強的抑制能力。

3.3 輪履模式切換

基于平衡點姿態(tài)補償的LQR控制器經過驗證可以實現擺臂運動時穩(wěn)定的平衡控制,在此基礎上加入一定的切換策略可實現模式切換。模式切換過程包括平衡到履帶模式和履帶到平衡模式。

圖8為平衡到履帶模式的切換過程。圖8a機器人處于平衡模式下,此時為姿態(tài)補償的LQR控制;圖8b擺臂關節(jié)按5次多項式軌跡,在0～2 s時從0°運動到105°,使得擺臂接近地面,避免觸地時沖擊過大;圖8c在2 s時退出平衡控制,驅動輪改為速度控制,并施加給驅動輪反向速度使得機器人前傾,擺臂與地面接觸;圖8d擺臂關節(jié)按5次多項式軌跡從105°運動到30°,完成模式切換。

圖8 平衡模式到履帶模式切換過程Fig.8 Switching process from balance mode to track mode

圖9為履帶到平衡模式切換過程機器人實時數據。從圖9可以看出,在2 s時,機器人狀態(tài)和驅動輪扭矩的抖動是由切換過程圖8c中驅動輪反向速度引起的;2.72 s左右的數據抖動是由擺臂碰撞地面引起的,除此之外,整個切換過程都十分平穩(wěn)。

圖9 平衡到履帶模式切換過程實時數據Fig.9 Real time data of the switching process from balance to track mode

圖10為履帶到平衡模式的切換過程。圖10a機器人處于履帶模式下,此時驅動輪為速度控制;圖10 b擺臂關節(jié)按5次多項式軌跡,在0～2 s時從30°運動到105°,此時擺臂展開質心抬高,機器人狀態(tài)進入控制器調節(jié)范圍;圖10c在2 s時將速度控制改為姿態(tài)補償LQR控制,機器人進入平衡模式;圖10d將擺臂關節(jié)按5次多項式軌跡從105°運動到0°,完成模式切換。

圖10 履帶模式到平衡模式切換過程Fig.10 Switching process from track mode to balance mode

圖11為履帶到平衡模式切換過程機器人實時數據。從系統(tǒng)狀態(tài)和驅動輪扭矩可以看出,機器人在6 s內實現了平穩(wěn)的運動模式切換。

圖11 履帶到平衡模式切換過程實時數據Fig.11 Real time data of the switching process from track to balance mode

綜上所述,本文提出的新型輪履移動機器人在基于平衡點姿態(tài)補償的LQR控制器下具有良好的動態(tài)平衡控制性能,對擺臂運動具有較強的抗擾能力,該方法能夠滿足輪履模式快速穩(wěn)定切換的要求,在6 s內實現模式切換,使機器人可以充分發(fā)揮輪式和履帶式的優(yōu)勢,靈活適應不同環(huán)境。

4 結束語

針對目前復合式移動機器人存在的缺陷,本文提出一種具有雙輪平衡和履帶2種模式的輪履復合式機器人,其結構簡單緊湊且兼具輪履2種模式優(yōu)點,為復合式機器人研發(fā)提供另一種思路。針對這一新型輪履機器人結構,采用基于姿態(tài)補償的LQR方法設計了平衡控制規(guī)律,實現不同擺臂角度下對參考位置的無超調快速響應。仿真結果表明,該控制器對擺臂運動造成的狀態(tài)擾動有很強的抑制能力,且能夠實現輪履模式快速穩(wěn)定切換,提升了機器人機動性能,使其可于城市地區(qū)巡檢安防、災區(qū)勘探救援、輻射環(huán)境監(jiān)測等任務。