機(jī)器人髖關(guān)節(jié)步態(tài)控制方法的研究

王瑜瑜，劉少軍

(西安航空職業(yè)技術(shù)學(xué)院通用航空學(xué)院，陜西 西安 710089)

仿人機(jī)器人在實(shí)際的運(yùn)動(dòng)行走過(guò)程中，由于自身以及環(huán)境等各種因素的共同作用，致使行走的實(shí)際運(yùn)動(dòng)軌跡和設(shè)定的軌跡沒(méi)有完全重合，各關(guān)節(jié)角度和設(shè)定角度之間亦存在偏差[1-2]。上述因素皆會(huì)對(duì)機(jī)器人行走的穩(wěn)定性產(chǎn)生影響，情況嚴(yán)重時(shí)會(huì)導(dǎo)致機(jī)器人跌倒乃至毀壞，故機(jī)器人研討探究的難點(diǎn)之一即是保持行走過(guò)程的穩(wěn)定性[3]。通過(guò)已有的研究可知，若機(jī)器人的零力矩點(diǎn)(ZMP)處于支撐多邊形的邊緣，此時(shí)機(jī)器人身處極易摔倒的臨界狀態(tài)，這種情況是應(yīng)該盡量避免的[4-5]。因此，應(yīng)該考慮加入適當(dāng)?shù)目刂品椒ǎ沟脵C(jī)器人的零力矩點(diǎn)由支撐多邊形的邊緣往中心移動(dòng)，以確保機(jī)器人平穩(wěn)行走，提高系統(tǒng)的穩(wěn)定性和魯棒性。通過(guò)分析研究，筆者提出了一種基于機(jī)器人姿態(tài)的髖關(guān)節(jié)補(bǔ)償控制方法。

1 機(jī)器人的姿態(tài)計(jì)算

仿人機(jī)器人的姿態(tài)參數(shù)主要表現(xiàn)為軀干的橫滾角和俯仰角，這兩個(gè)參數(shù)的變化直接反映機(jī)器人軀體姿態(tài)的改變。故要完成機(jī)器人軀干姿態(tài)的計(jì)算，首要的是建立軀干的參考坐標(biāo)系，具體如圖1所示，位于該參考坐標(biāo)系下的姿態(tài)角度如圖2所示。其中，AngleX和AngleY分別為機(jī)器人軀干繞X軸和Y軸的偏轉(zhuǎn)角，即分別代表軀干的橫滾角與俯仰角。

圖1 軀干參考坐標(biāo)示意圖

圖2 軀干坐標(biāo)系下的姿態(tài)角度

NAO機(jī)器人自身帶有慣性單元，該單元包含了一個(gè)用于計(jì)算姿態(tài)變化角速度值的三軸陀螺儀和一個(gè)用于獲取運(yùn)動(dòng)加速度值的三軸加速度計(jì)[6]。由于二者都存在測(cè)量誤差，若采用直接融合反饋值方式會(huì)致使偏差累加，故本文采用了卡爾曼(Kalman)濾波方式，實(shí)現(xiàn)兩種傳感器信號(hào)數(shù)據(jù)的有機(jī)融合，進(jìn)而估算出機(jī)器人的姿態(tài)角度[7-8]。Kalman濾波具體步驟為：

1)系統(tǒng)預(yù)估狀態(tài)方程：

(1)

2)預(yù)估協(xié)方差矩陣：

(2)

3)Kalman增益濾波：

(3)

式中：Kk為卡爾曼增益；H為觀測(cè)矩陣；R為測(cè)量的協(xié)方差。

4)由觀測(cè)的數(shù)據(jù)先驗(yàn)估計(jì)值：

(4)

式中：xk為k時(shí)刻的系統(tǒng)狀態(tài)；Zk為k時(shí)刻測(cè)量矩陣。

5)估計(jì)均方差：

(5)

式中：I為與狀態(tài)向量同維度的單位矩陣。

由于機(jī)器人在前向和橫向兩個(gè)平面內(nèi)的運(yùn)動(dòng)是相互獨(dú)立的，因此這里以前向平面為例進(jìn)行介紹。

設(shè)加速計(jì)的測(cè)量值為：

a=[axayaz]

(6)

式中：a為加速度測(cè)量值；ax，ay，az為x，y，z軸加速度值。

據(jù)此可得到機(jī)器人行走過(guò)程中的軀干傾角θ(t)為：

(7)

利用陀螺儀獲取軀干的角速度ω，對(duì)該角速度進(jìn)行積分運(yùn)算便同樣可獲得軀干傾角為：

(8)

式中：t為時(shí)間；T為機(jī)器人步態(tài)的單步周期。

考慮到實(shí)際噪聲影響，將式(8)實(shí)施離散化處理后可得：

θk+1=θk+ωkT+w

(9)

式中：θk+1為(k+1)時(shí)刻軀干傾角；ωk為k時(shí)刻軀干角速度；w為過(guò)程噪聲。

根據(jù)狀態(tài)轉(zhuǎn)換方程的定義選取系統(tǒng)狀態(tài)變量x：

(10)

由式(9)和式(10)可得，狀態(tài)方程為：

(11)

從慣性單元獲得俯仰角的角度及角速度為：

(12)

2 傾角髖關(guān)節(jié)補(bǔ)償控制

2.1 姿態(tài)補(bǔ)償控制器

機(jī)器人行走步態(tài)控制的主要作用就是調(diào)整各關(guān)節(jié)角度，以保證機(jī)器人處于動(dòng)態(tài)穩(wěn)定狀態(tài)。機(jī)器人髖關(guān)節(jié)是其上肢與下肢連接的樞紐，也是上半身質(zhì)量空間位置分布的主要控制關(guān)節(jié)。由ZMP定義可知，髖關(guān)節(jié)能直接影響機(jī)器人倒立擺模型的質(zhì)心位置，而該質(zhì)心位置分布會(huì)對(duì)機(jī)器人穩(wěn)定性直接造成影響[9-10]。故而為了提高系統(tǒng)穩(wěn)定性，本文采用了髖關(guān)節(jié)補(bǔ)償控制方法，其控制結(jié)構(gòu)如圖3所示。

圖3 髖關(guān)節(jié)的穩(wěn)定補(bǔ)償控制結(jié)構(gòu)圖

由圖可知，當(dāng)機(jī)器人軀干發(fā)生傾斜時(shí)，會(huì)將獲取的軀干傾角傳送給姿態(tài)補(bǔ)償控制器，控制器輸出髖關(guān)節(jié)補(bǔ)償角Δθ，從而實(shí)現(xiàn)對(duì)軀干的運(yùn)動(dòng)控制，其中控制單元采用模糊控制規(guī)律。補(bǔ)償角Δθhip的計(jì)算如圖4所示。

圖4 姿態(tài)補(bǔ)償控制器

圖中，Dzmp為機(jī)器人傾角的期望值，Rzmp為機(jī)器人傾角的實(shí)際值，髖關(guān)節(jié)角度補(bǔ)償計(jì)算公式為：

(13)

2.2 模糊控制器

控制器將補(bǔ)償后的角度偏差以及偏差變化率視為輸入信號(hào)，根據(jù)補(bǔ)償角度、角度誤差及其變化率的實(shí)際范圍，初定角度誤差的基本論域?yàn)?-6°,6°)。在此基礎(chǔ)上，為了將控制器進(jìn)行標(biāo)準(zhǔn)化設(shè)計(jì)，變化區(qū)間定為[-6,+6]，將該區(qū)間進(jìn)行離散化處理后可得由13個(gè)整數(shù)元素組成的集合：{-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6}。

若論域范圍為[a,b]，則可利用式(14)將變量x轉(zhuǎn)換成論域范圍為[-6,6]的變量y。

(14)

為了對(duì)變量的狀態(tài)進(jìn)行準(zhǔn)確描述，設(shè)定了以下7個(gè)符號(hào)：正大(PB)、正中(PM)、正小(PS)、零(ZE)、負(fù)小(NS)、負(fù)中(NM)、負(fù)大(NB)。將它們與前面的論域結(jié)合便可得模糊隸屬函數(shù)，具體如圖5所示。用上述7個(gè)符號(hào)將系統(tǒng)的兩個(gè)輸入變量進(jìn)行列寫(xiě)，可得如表1所示的模糊控制規(guī)則表，利用該表即可得與輸入量相統(tǒng)一的模糊控制規(guī)則。

表1 模糊控制規(guī)則表

圖5 模糊狀態(tài)變量隸屬函數(shù)

3 實(shí)驗(yàn)結(jié)果及分析

3.1 動(dòng)態(tài)軌跡跟蹤控制實(shí)驗(yàn)

首先進(jìn)行機(jī)器人軌跡跟蹤實(shí)驗(yàn)，由圖6可知，加入補(bǔ)償控制前，運(yùn)動(dòng)軌跡跟蹤的相對(duì)誤差在(-10%,10%)發(fā)生較大幅度變化，而加入補(bǔ)償控制后運(yùn)動(dòng)軌跡跟蹤誤差變小，降低至(-3%,3%)，且跟蹤的穩(wěn)定性明顯增強(qiáng)。由圖7平均相對(duì)控制量的輸出結(jié)果可知，補(bǔ)償控制的加入使得控制量明顯增加，相應(yīng)的控制效果明顯增強(qiáng)。

圖6 位置軌跡跟蹤的平均相對(duì)誤差

圖7 平均相對(duì)控制量

3.2 傾角測(cè)試實(shí)驗(yàn)

圖8 機(jī)器人行走過(guò)程中傾角值的變化

由補(bǔ)償前后的數(shù)據(jù)波形可知，加入補(bǔ)償后不僅使得機(jī)器人傾角擺動(dòng)幅度減小，而且極大縮小了角度的變化范圍，能有效防止傾角過(guò)大現(xiàn)象的出現(xiàn)。

計(jì)算獲得的補(bǔ)償前后軀干傾角誤差的統(tǒng)計(jì)結(jié)果見(jiàn)表2，軀干傾角補(bǔ)償前后ZMP偏移期望值的統(tǒng)計(jì)結(jié)果見(jiàn)表3。

表2 補(bǔ)償前后軀干傾角誤差的統(tǒng)計(jì)結(jié)果

表3 軀干傾角補(bǔ)償前后ZMP偏移期望值的統(tǒng)計(jì)結(jié)果

由表2和表3的統(tǒng)計(jì)數(shù)據(jù)可知：實(shí)施補(bǔ)償后能夠有效降低機(jī)器人運(yùn)動(dòng)過(guò)程中發(fā)生的傾斜角度；軀干的補(bǔ)償作用對(duì)于ZMP在y方向的作用效果尤為明顯；有效提高了機(jī)器人行走的穩(wěn)定性，同時(shí)極大增強(qiáng)了步態(tài)的魯棒性。

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

本文從機(jī)器人髖關(guān)節(jié)能直接影響倒立擺模型質(zhì)心軌跡的角度出發(fā)，設(shè)計(jì)、分析了基于模糊控制的姿態(tài)髖關(guān)節(jié)補(bǔ)償控制策略。利用加速度計(jì)和陀螺儀構(gòu)成慣性單元計(jì)算軀干的傾角，在此基礎(chǔ)上，采用姿態(tài)角度對(duì)髖關(guān)節(jié)進(jìn)行補(bǔ)償，并采用模糊控制算法進(jìn)行控制，達(dá)成了機(jī)器人穩(wěn)定步態(tài)行走的效果。最后的實(shí)驗(yàn)結(jié)果也充分證明了本文所提控制方法的有效性和可行性。