傷員運載機器人樓梯環(huán)境運動穩(wěn)定性

紀兆俊, 李艷杰*, 卜春光, 郎智明, 佟福奇

(1.沈陽理工大學(xué)機械工程學(xué)院, 沈陽 110159; 2.中國科學(xué)院沈陽自動化研究所機器人學(xué)國家重點實驗室, 沈陽 110016;3.中國科學(xué)院機器人與智能制造創(chuàng)新研究院, 沈陽 110169)

隨著機器人技術(shù)的不斷發(fā)展,機器人越來越多地代替人類來完成一些危險作業(yè)任務(wù),各類特種機器人不斷發(fā)展,對性能的需求也在日益增長[1]。結(jié)構(gòu)穩(wěn)定問題是安全與經(jīng)濟的主要問題之一[2]。機器人自主爬樓梯的穩(wěn)定性是傷員運載工作的一項重要指標。機器人在遇到樓梯、臺階等復(fù)雜地形時,沒有做好從普通運動模式到越障模式之間的切換,連續(xù)翻越樓梯或臺階難免會產(chǎn)生顛簸、姿態(tài)不穩(wěn)定等問題[3]。若在運載傷員的過程中穩(wěn)定性無法保證,發(fā)生傾覆、翻轉(zhuǎn)等危險行為,那么對傷員造成的危險后果是無法想象的。

機器人的運動穩(wěn)定性問題對于機器人的運動安全意義重大。多年來,也有不少學(xué)者對機器人的運動穩(wěn)定性進行了研究。夏建強等[4]通過零力矩點動態(tài)穩(wěn)定判據(jù)方法,針對雙臂護理機器人抱舉運動的重心穩(wěn)定性進行了穩(wěn)定性分析。郝亮超等[5]分析康復(fù)機器人的重心位置,對康復(fù)機器人的傾覆問題進行校核判斷,驗證了康復(fù)機器人的結(jié)構(gòu)科學(xué)性。但上述研究中所研究的機器人關(guān)節(jié)自由度都相對較少,且運動環(huán)境相對簡單。

現(xiàn)提出一種針對傷員運載機器人的運動穩(wěn)定性判定方法,結(jié)合質(zhì)心位置與構(gòu)型姿態(tài),為傷員運載機器人的各越障階段搭建對應(yīng)的穩(wěn)定錐模型[6],通過穩(wěn)定錐判定理論制定傷員運載機器人的重心穩(wěn)定衡量方法[7-8],該方法服務(wù)于運載機器人上樓梯時的重心穩(wěn)定性分析,為運載機器人的安全、穩(wěn)定的工作提供理論支撐。

1 傷員運載機器人結(jié)構(gòu)

目前爬樓機器人有多種類型,它們在功能與實用性上各有優(yōu)劣[9]。研究對象是中國科學(xué)院沈陽自動化研究所研制的傷員運載機器人,如圖1所示。

圖1 傷員運載機器人實體圖Fig.1 Physical diagram of a casualty carrying robot

該機器人底盤部分為履腿復(fù)合式結(jié)構(gòu),底盤通過可移動滑臺連接上方的可變形式擔架。傷員運載機器人搭載的可變形式擔架主要有“擔架式”和“椅式”兩種極限形態(tài)和一些中間形態(tài)。所研究的傷員運載過程中的重心調(diào)整問題,主要是通過可移動滑臺的前后移動與可變形式擔架的姿態(tài)變化對機器人的重心進行調(diào)整。

2 傷員運載機器人質(zhì)心運動學(xué)建模

2.1 建立全局坐標系

得益于機器人結(jié)構(gòu)的對稱性,在機器人上樓梯這一場景下,可將問題考慮為一個二維問題。為傷員運載機器人建立坐標系如圖2所示,不考慮圖2中的垂直紙面方向。設(shè)置基礎(chǔ)直角坐標系XOY1,及局部坐標系XOY2和XOY3,通過x、y坐標來描述位姿信息。

圖2 傷員運載機器人坐標系簡圖Fig.2 Sketch of the coordinate system of the casualty carrier robot

圖2所涉及的各參數(shù)及釋義見表1。

表1 坐標系參數(shù)表Table 1 Coordinate system parameter table

若是將上述坐標系看作是三維坐標系,指定Z軸的方向是垂直于紙面的方向。那么由坐標系XOY2到坐標系XOY1的變換矩陣為

(1)

坐標系XOY2到坐標系XOY1的變換矩陣為

(2)

式中:Rot、Trans分別為機器人的3×3旋轉(zhuǎn)矩陣與3×1位置平移矩陣;c1、s1分別為θ1余弦值和正弦值;c2、s2分別為θ2余弦值和正弦值;x3=0.69+S,S為擔架的水平位移,由機械結(jié)構(gòu)實際設(shè)計尺寸測量得,S取值范圍為0～0.58 m。

2.2 整體質(zhì)心運動學(xué)建模

由于機器人自由度較高,關(guān)節(jié)眾多,為得到其整體質(zhì)心位置,采用先計算單獨個體質(zhì)心位置,再將它們通過以個體質(zhì)量為權(quán)重計算其加權(quán)平均值,即可得到整體質(zhì)心坐標位置,即

(3)

式(3)中:Px、Py、Pz為整體質(zhì)心的三軸位置坐標;xi、yi、zi為各部分質(zhì)心的三軸位置坐標;mi為各部分的質(zhì)量;i為系統(tǒng)部件數(shù)目;k為系統(tǒng)總部件數(shù)。

分別計算三大部分在基礎(chǔ)坐標系XOY1下的質(zhì)心位置坐標。

XOY1坐標系下的底盤車體的質(zhì)心坐標為

(4)

XOY2坐標系下的擺臂質(zhì)心坐標為

(5)

XOY3坐標系下的擔架的質(zhì)心坐標為

(6)

式中:3Pxi和3Pyi為擔架的重心在XOY3坐標系下的位置,由擔架構(gòu)型決定,需要分析計算。

由空間位置變換矩陣式(1)和式(2)可得,基礎(chǔ)坐標系下擺臂的質(zhì)心坐標為

1P(x2,y2)=1T22P(x2,y2)

(7)

基礎(chǔ)坐標系下的擔架質(zhì)心坐標為

1P(x3,y3)=1T33P(x3,y3)

(8)

由式(3)結(jié)合式(4)、式(7)、式(8),可得傷員運載機器人整體質(zhì)心坐標計算公式為

(9)

式(9)中:m1為傷員運載機器人底盤部分的質(zhì)量;m2為傷員運載機器人擺臂部分質(zhì)量;m3為傷員運載機器人擔架部分質(zhì)量。

綜上所述,在機器人坐標系下,傷員運載機器人的質(zhì)心運動學(xué)模型為

(10)

式(10)中:M=m1+m2+m3。

2.3 傷員運載機器人擔架部分質(zhì)心位置分析

擔架部分的自由度較高,部件的相互位置關(guān)系變化較大,不僅有“擔架式”構(gòu)型,還有“椅子式”構(gòu)型以及中間位置構(gòu)型等,因此,傷員運載機器人擔架部分的質(zhì)心坐標位置需要結(jié)合運動學(xué)進行分析。

如圖3所示,將擔架機械結(jié)構(gòu)抽象為一個四連桿機構(gòu),分別將擔架對應(yīng)人的上半身、臀部和小腿部分設(shè)定為Ⅰ、Ⅱ和Ⅲ部分,Ⅱ部分和Ⅲ部分連接處有變形電機,電機輸入扭矩,推動Ⅱ部分沿著Ⅰ部分末端的弧形導(dǎo)軌產(chǎn)生角位移,但是由于Ⅰ部分是固定在傷員運載機器人本體上的,Ⅰ部分不動,那么在反力的作用下,電機反向驅(qū)使擔架Ⅱ帶動Ⅲ部分進行旋轉(zhuǎn),于是Ⅱ部分和Ⅲ部分以及電機關(guān)節(jié)三部分同時運動,Ⅲ部分的回轉(zhuǎn)中心為變形電機軸回轉(zhuǎn)中心,而Ⅱ部分的回轉(zhuǎn)中心是虛擬的,為Ⅰ部分末端弧形槽的圓心。

圖3 擔架機構(gòu)簡圖Fig.3 Stretcher mechanism sketch

對擔架機構(gòu)進行運動分析,如圖4所示。Ⅰ、Ⅲ部分回轉(zhuǎn)中心連線分別與L1、Ⅱ部分、Ⅰ、Ⅱ部分連接處和Ⅱ部分的回轉(zhuǎn)中心連線間的初始角度分別為θ1、θ2、θ3。假設(shè)電機位置不動,當θ1變化量為Δθ1時,對應(yīng)的θ3變化量為Δθ3。因此,可得出以下關(guān)系。

圖4 擔架運動分析簡圖Fig.4 Stretcher movement analysis sketch

L1sinθ1+L2sinθ2=L3sinθ3

(11)

L2cosθ2-L1cosθ1=L4+L3cosθ3

(12)

為擔架各變形關(guān)節(jié)設(shè)置坐標系來描述其在空間坐標系中的位置,如圖5所示。

θ4、θ5分別為擔架Ⅰ部分與擔架Ⅱ部分之間夾角、擔架Ⅱ部分與擔架Ⅲ部分之間夾角的補角

將機械結(jié)構(gòu)尺寸測量后,在XOY3坐標系下?lián)堍虿糠值闹匦淖鴺藶?/p>

(13)

式(13)中:c4、s4分別為θ4的余弦值和正弦值。在XOY3坐標系下?lián)堍蟛糠值闹匦淖鴺藶?/p>

3PⅢ=3T44T55PⅢ=

(14)

式(14)中:c5、s5分別為θ5的余弦值和正弦值。在XOY3坐標系下?lián)堍癫糠值闹匦淖鴺藶?/p>

(15)

則在XOY3坐標系下,擔架的質(zhì)心x坐標為

(16)

則在XOY3坐標系下,擔架的質(zhì)心y坐標為

(17)

式中:3PⅠx3、3PⅡx3、3PⅢx3與3PⅠy3、3PⅡy3、3PⅢy3可由式(13)～式(15)求得;m1、m2、m3為擔架三部分的質(zhì)量,分別是80.2、48.4、16.8 kg;θ4=Δθ3;θ5為電機角度變化量。

3 傷員運載機器人的構(gòu)型穩(wěn)定性判定

傷員運載機器人在工作過程中,面對樓梯與地面環(huán)境的不斷改變,需要不斷改變機器人各關(guān)節(jié)構(gòu)型來應(yīng)對環(huán)境變化對機器人重心穩(wěn)定性的影響。由于機器人的對稱性結(jié)構(gòu),故只需考慮機器人在上樓過程中的傾覆的危險情況。而最有可能發(fā)生危險的階段就是機器人從平地上樓梯的階段和從樓梯到平地的過程,故對這兩個危險階段進行分析。

3.1 傷員運載機器人地面到樓梯階段構(gòu)型穩(wěn)定性分析

機器人在運動和越障的過程中必須要保持一定的穩(wěn)定性,不能發(fā)生側(cè)翻或傾倒等現(xiàn)象,為此,可以采用穩(wěn)定錐方法對機器人越障過程中的穩(wěn)定性進行分析[10-11]。對機器人該階段進行分析,如圖6所示。

圖6 傷員運載機器人駛上樓梯階段穩(wěn)定性分析簡圖Fig.6 Sketch of the stability analysis of the casualty carrying robot during the drive up the stairs

圖6中,P為機器人整體質(zhì)心位置,對機器人建立穩(wěn)錐模型,并建立穩(wěn)定錐坐標系。由幾何關(guān)系可知,有

(18)

式(18)中:φ為基礎(chǔ)坐標系與穩(wěn)定錐坐標系之間的變化關(guān)系角;α為機器人坐標系的x軸與水平地面間的夾角;β為穩(wěn)定錐三角形垂線與重力方向夾角。因?qū)嶒灅翘萁嵌葹?0°,故取值范圍為0°～30°。l1為傷員運載機器人底盤前后輪中心距,m;l2為傷員運載機器人擺臂前后輪中心距,m;x為底盤主履帶位移,m。

已知φ,可求由機器人坐標系到穩(wěn)定錐坐標系的變換矩陣為

(19)

式(19)中:R為傷員運載機器人底盤后輪回轉(zhuǎn)中心到地面接觸點的距離。

故質(zhì)心坐標為

(20)

其中,由式(10)可知,有

1Px=

(21)

(22)

則在穩(wěn)定錐坐標系下,傷員運載機器人整體質(zhì)心的x坐標位置計算公式為

0Px=cosφ1Px+sinφ1Py+Rsinφ

(23)

在穩(wěn)定錐坐標系下,傷員運載機器人整體質(zhì)心的y坐標位置計算公式為

0Py=cosφ1Py-sinφ1Px+Rcosφ

(24)

由穩(wěn)定錐定義可知,穩(wěn)定錐的各條邊線與重力方向的夾角即為邊線穩(wěn)定角,可得該階段傷員運載機器人各邊線穩(wěn)定角計算公式如下。

(25)

(26)

設(shè)該階段傷員運載機器人的重心穩(wěn)定角為γ,由穩(wěn)定錐算法可知,取邊線穩(wěn)定角中的最小角,即

γ=min(γ1,γ2)

(27)

當γ<0時,傷員運載機器人將會重心失穩(wěn),發(fā)生危險。當γ>0時,證明傷員運載機器人的重心是處于安全位置,機器人屬于重心穩(wěn)定狀態(tài)。

3.2 傷員運載機器人樓梯到地面階段構(gòu)型穩(wěn)定性分析

該階段同理地面到樓梯階段的穩(wěn)定角計算方法,針對傷員運載機器人從樓梯到地面階段的重心穩(wěn)定性分析如圖7所示。

圖7 傷員運載機器人樓梯到地面階段穩(wěn)定性分析簡圖Fig.7 Sketch of the stability analysis of the stair-to-surface phase of the Casualty Carrier Robot

從機器人坐標系到穩(wěn)定錐坐標系的變換矩陣為

(28)

式(28)中:C為cos(30°-θ1)的簡寫形式;S為sin(30°-θ1)的簡寫形式;θ1為機器人擺臂關(guān)節(jié)旋轉(zhuǎn)角度。

則該階段穩(wěn)定錐坐標系下的傷員運載機器人重心坐標位置為

(29)

由穩(wěn)定錐定義可知,穩(wěn)定錐的各條邊線與重力方向的夾角即為邊線穩(wěn)定角,可得該階段傷員運載機器人各邊線穩(wěn)定角計算公式為

(30)

式(30)中:γ為該階段傷員運載機器人的重心穩(wěn)定角。

同理,當γ<0時,傷員運載機器人將會重心失穩(wěn),發(fā)生危險。當γ>0時,證明傷員運載機器人的重心是處于安全位置,機器人屬于重心穩(wěn)定狀態(tài)。

4 仿真與實驗

4.1 MATLAB仿真

利用上述方法對機器人各階段構(gòu)型進行穩(wěn)定性分析,并利用MATLAB進行仿真計算。

在準備上樓階段,為了傷員的舒適性與安全性,擔架應(yīng)由“擔架式”變?yōu)椤耙问健?。如圖8所示,隨著變形角度的變大。機器人的穩(wěn)定性逐漸降低。

圖8 擔架變形調(diào)整下的重心穩(wěn)定角變化圖Fig.8 Graph of the change in the angle of stability of the centre of gravity under stretcher deformation adjustment

在地面到樓梯階段。若“椅式”構(gòu)型下,不進行重心調(diào)整。此時主履帶位移是影響穩(wěn)定錐變化的唯一變量。如圖9和圖10所示,機器人的穩(wěn)定性隨著主履帶位移的增加而降低。

圖9 無重心調(diào)整機構(gòu)作用下邊線穩(wěn)定角1的變化圖Fig.9 Change of edge stability angle 1 under the action of no center of gravity adjustment mechanism

圖10 無重心調(diào)整機構(gòu)作用下邊線穩(wěn)定角2的變化圖Fig.10 Change of edge stability angle 2 without center of gravity adjustment mechanism

在地面到樓梯階段,若只對擔架俯仰進行重心調(diào)整,結(jié)果如圖11所示。只對擔架位移進行調(diào)整,結(jié)果如圖12所示。

圖11 擔架俯仰機構(gòu)作用下的穩(wěn)定性分析圖Fig.11 Stability analysis diagram under the action of the stretcher pitching mechanism

圖12 擔架位移機構(gòu)作用下的重心穩(wěn)定性分析圖Fig.12 Stability analysis diagram of the centre of gravity under the action of the stretcher displacement mechanism

綜合分析可知,在主履帶位移最大的情況下,機器人的穩(wěn)定性最差。針對此時刻對擔架俯仰與擔架位移同時調(diào)整,調(diào)整結(jié)果如圖13所示?？梢?在擔架俯仰與位移的雙重作用下,效果是優(yōu)于擔架位移或者擔架俯仰單一的調(diào)整效果的,使傷員運載機器人的重心穩(wěn)定角大幅度提升,可滿足該階段機器人該階段的重心穩(wěn)定要求。

圖13 擔架位移與俯仰共同作用下的穩(wěn)定性分析圖Fig.13 Stability analysis diagram for the combined effect of stretcher displacement and pitch

從樓梯到平地階段,機器人重心穩(wěn)定性如圖14所示,隨著擺腿角度變化,穩(wěn)定性降低,但機器人在此構(gòu)型下始終都是穩(wěn)定的。

圖14 樓梯到地面穩(wěn)定性分析圖Fig.14 Stair-to-floor stability analysis diagram

4.2 真實樓梯環(huán)境實驗

通過MATLAB進行仿真分析,得出各個重心調(diào)整機構(gòu)運動對機器人重心的影響及各階段的最優(yōu)姿態(tài)。將最優(yōu)姿態(tài)應(yīng)用到樣機,并搭載實驗假人(75 kg)進行樓梯環(huán)境實驗,樓梯傾斜角度為30°。實驗結(jié)果如圖15所示,可以看出,機器人與實驗假人安全上樓。

圖15 傷員運載機器人樣機實地實驗Fig.15 Field experiments with a prototype casualty-carrying robot

5 結(jié)論

(1)通過邊線穩(wěn)定角的大小來判斷傷員運載機器人重心穩(wěn)定性的方案是可行的。

(2)本文方法計算出的最優(yōu)姿態(tài)可輕松應(yīng)對傷員運載機器人在上樓過程中重心的變化,為保證傷員運載機器人的安全性具有重大意義。