梁志遠(yuǎn),趙佳輝,李清都,陳媛媛

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

0 引 言

隨著機(jī)器人技術(shù)的發(fā)展和廣泛應(yīng)用,移動(dòng)機(jī)器人面臨著更加復(fù)雜的環(huán)境。因此,能夠?qū)崿F(xiàn)快速移動(dòng)、應(yīng)對復(fù)雜地形、克服障礙的移動(dòng)機(jī)器人是目前熱門的研究主題[1-2]。

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

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

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

圖1 機(jī)器人整體側(cè)視圖Fig.1 Concept of robot side view

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

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

1 機(jī)器人動(dòng)力學(xué)建模

1.1 等效倒立擺模型

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

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

表1 機(jī)器人模型參數(shù)

本文定義如下參數(shù)。

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

{M}:固連在車軸上的機(jī)械坐標(biāo)系。

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

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

θ0:不同擺臂關(guān)節(jié)角下機(jī)器人平衡點(diǎn)。

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

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

Ic1,Ic2,Ic3:等效倒立擺在其質(zhì)心系下的轉(zhuǎn)動(dòng)慣量。

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

根據(jù)質(zhì)點(diǎn)系定義得等效倒立擺質(zhì)心位置為

(1)

機(jī)器人處于平衡點(diǎn)時(shí),倒立擺質(zhì)心位置在X軸的分量Rcx=0,質(zhì)心距車軸距離則為在Z軸的分量,即lc=Rcz。

令θr=βr-θ0,θl=βl-θ0,可得平衡點(diǎn)θ0,質(zhì)心距車軸距離lc分別為

(2)

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

(3)

等效倒立擺質(zhì)心的轉(zhuǎn)動(dòng)慣量Ic1,Ic2,Ic3可根據(jù)動(dòng)能守恒推導(dǎo)出,假設(shè)機(jī)器人繞著坐標(biāo)系Y軸以wy角速度轉(zhuǎn)動(dòng),則倒立擺質(zhì)心動(dòng)能Ecy等于車身和擺臂的質(zhì)點(diǎn)組動(dòng)能Ey,表示為

(4)

(5)

記繞著X、Z軸的質(zhì)心動(dòng)能和質(zhì)點(diǎn)組動(dòng)能分別為Ecx,Ex和Ecz,Ez,可得

(6)

(7)

倒立擺質(zhì)心的轉(zhuǎn)動(dòng)慣量Ic1,Ic2,Ic3與左右擺臂關(guān)節(jié)位置關(guān)系為

(8)

1.2 拉格朗日方程建模

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

γl,γr:左右驅(qū)動(dòng)輪轉(zhuǎn)動(dòng)角度。

vl,vr:左右驅(qū)動(dòng)輪線速度。

vm,vc:機(jī)械原點(diǎn)M和倒立擺質(zhì)心線速度。

Tl,Tr:左右驅(qū)動(dòng)輪力矩。

fl,fr:左右驅(qū)動(dòng)輪阻尼力。

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

機(jī)器人運(yùn)動(dòng)過程由直線、俯仰和偏航運(yùn)動(dòng)共同組成[12],分別通過控制機(jī)器人位置x、俯仰角θ和偏航角φ來實(shí)現(xiàn)。為了完整描述機(jī)器人的運(yùn)動(dòng),總共需要13個(gè)廣義坐標(biāo):xm,ym,xl,yl,xr,yr,xc,yc,zc,γl,γr,θ,φ。對于參考系{W},每個(gè)剛體的質(zhì)心位置為

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 機(jī)器人俯視圖Fig.4 Top view of robot

假設(shè)機(jī)器人驅(qū)動(dòng)輪運(yùn)動(dòng)時(shí)與地面無相對滑動(dòng)[12],則機(jī)器人位移x和偏航角φ為

(10)

對(10)式微分后化簡,得驅(qū)動(dòng)輪角速度為

(11)

則從動(dòng)輪角速度為

(12)

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

(13)

拉格朗日方法需要將機(jī)器人剛體的動(dòng)能和勢能表示為廣義坐標(biāo)的函數(shù),機(jī)器人的平動(dòng)動(dòng)能可表示為

(14)

機(jī)器人的轉(zhuǎn)動(dòng)動(dòng)能可表示為

(15)

(15)式中:第2項(xiàng)為倒立擺質(zhì)心繞y軸的轉(zhuǎn)動(dòng)動(dòng)能,考慮了倒立擺俯仰角θ的影響;第5項(xiàng)為從動(dòng)輪的轉(zhuǎn)動(dòng)動(dòng)能,這是因?yàn)樵谟?jì)算質(zhì)心轉(zhuǎn)動(dòng)慣量時(shí)沒有考慮從動(dòng)輪自身轉(zhuǎn)動(dòng)。

當(dāng)機(jī)器人在平面運(yùn)動(dòng)時(shí),垂直運(yùn)動(dòng)僅與俯仰角θ有關(guān),因此勢能為

V=mcglccθ

(16)

拉格朗日函數(shù)定義為

L=Ttrans+Trot-V

(17)

機(jī)器人的廣義坐標(biāo)為位置x,俯仰角θ和偏航角φ,對應(yīng)的廣義力Fx,Fθ,Fφ可以表示為

(18)

則拉格朗日運(yùn)動(dòng)方程可表示為

(19)

將(11)—(18)式代入(19) 式可以得到機(jī)器人3個(gè)獨(dú)立的非線性運(yùn)動(dòng)方程,表示為

(20)

(21)

(22)

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

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

(23)

(24)

(25)

由(23)—(25)式,有

(26)

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

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

則有

(27)

描述機(jī)器人前后運(yùn)動(dòng)的四階系統(tǒng)的狀態(tài)方程為

(28)

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

(29)

描述機(jī)器人轉(zhuǎn)向運(yùn)動(dòng)的二階系統(tǒng)的狀態(tài)方程為

(30)

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

(31)

當(dāng)機(jī)器人處于默認(rèn)姿態(tài)時(shí),擺臂關(guān)節(jié)角βr=βl=0。由(2)式和(8)式可得,此時(shí)機(jī)器人平衡點(diǎn)θ0=0,質(zhì)心高度lc=0.206 452 m,質(zhì)心慣量Ic1=0.303 819 kg·m2,Ic2= 0.254 564 kg·m2,Ic3=0.055 684 9 kg·m2,代入?yún)?shù)后得

(32)

(33)

(34)

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

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

M=[BAB…AnB]

(35)

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

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

(36)

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

(37)

(38)

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

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

3 實(shí)驗(yàn)與仿真分析

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

3.1 階躍響應(yīng)

從圖6可以看出,姿態(tài)補(bǔ)償后的LQR控制器可以實(shí)現(xiàn)不同擺臂角度下參考指令的快速跟蹤,且響應(yīng)無超調(diào),調(diào)節(jié)時(shí)間短。擺臂0°時(shí),穩(wěn)定時(shí)間約為6 s;擺臂90°時(shí),穩(wěn)定時(shí)間為約為5 s。這是因?yàn)樵谕唤M控制器參數(shù)下,前者倒立擺質(zhì)心到車軸距離lc=0.206 5,而后者lc=0.167 5,質(zhì)心低,穩(wěn)定快。

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

3.2 履帶擺臂控制

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

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

3.3 輪履模式切換

基于平衡點(diǎn)姿態(tài)補(bǔ)償?shù)腖QR控制器經(jīng)過驗(yàn)證可以實(shí)現(xiàn)擺臂運(yùn)動(dòng)時(shí)穩(wěn)定的平衡控制,在此基礎(chǔ)上加入一定的切換策略可實(shí)現(xiàn)模式切換。模式切換過程包括平衡到履帶模式和履帶到平衡模式。

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

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

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

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

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

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

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

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

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

4 結(jié)束語

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