王瑜瑜，劉少軍

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

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

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

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

圖1 軀干參考坐標示意圖

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

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

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

(1)

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

(2)

3)Kalman增益濾波：

(3)

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

4)由觀測的數(shù)據先驗估計值：

(4)

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

5)估計均方差：

(5)

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

由于機器人在前向和橫向兩個平面內的運動是相互獨立的，因此這里以前向平面為例進行介紹。

設加速計的測量值為：

a=[axayaz]

(6)

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

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

(7)

利用陀螺儀獲取軀干的角速度ω，對該角速度進行積分運算便同樣可獲得軀干傾角為：

(8)

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

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

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

(9)

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

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

(10)

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

(11)

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

(12)

2 傾角髖關節(jié)補償控制

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

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

圖3 髖關節(jié)的穩(wěn)定補償控制結構圖

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

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

圖中，Dzmp為機器人傾角的期望值，Rzmp為機器人傾角的實際值，髖關節(jié)角度補償計算公式為：

(13)

2.2 模糊控制器

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

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

(14)

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

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

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

3 實驗結果及分析

3.1 動態(tài)軌跡跟蹤控制實驗

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

圖6 位置軌跡跟蹤的平均相對誤差

圖7 平均相對控制量

3.2 傾角測試實驗

圖8 機器人行走過程中傾角值的變化

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

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

表2 補償前后軀干傾角誤差的統(tǒng)計結果

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

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

4 結束語

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