足式機器人在非結構地形中的姿態(tài)求解算法

，，

(1.天津商業(yè)大學 機械工程學院，天津 100081； 2.北京理工大學 機電學院，北京 100081)

0 引言

仿生機器人一直是機器人領域的研究熱點，足式機器人是其中的重要組成部分[1]。隨著 “Big Dog”和 “Wild cat”等液壓驅動機器人的出現(xiàn)，在野外環(huán)境應用的足式機器人逐漸增多，因而對適應非結構地形的姿態(tài)求解算法進行研究具有重要意義。機器人的姿態(tài)求解是指對機體相對于重力方向的俯仰角和橫滾角進行求解。陀螺儀、加速度計等慣性測量單元(IMU)廣泛地應用于飛行器的姿態(tài)求解中[2-3]，近年來也開始在足式機器人中使用。P.C.Lin等[4]在研究RHex六足機器人時，最早提出將機器人運動學與IMU相結合用于足式機器人的姿態(tài)解算。這種方法只適用于在平地上運動的機器人，并且要求機器人在任意時刻至少有三條腿與地面接觸，具有較大的局限性。為了對四足機器人在平地上的姿態(tài)進行求解，S.P.N.Singh等[5]通過在機器人機體上設置三個距離傳感器，獲得機體與地面之間的相對位姿關系，并將其與IMU進行融合以獲得最終的位姿。M.Reinstein等[6]提出了一種基于經驗的姿態(tài)求解算法，利用關節(jié)編碼器、壓力傳感器的信息得到實時步長，并將其作為量測向量對IMU的求解結果進行補償，實現(xiàn)了機器人的姿態(tài)和速度的估計。由于足端與地面之間的相對滑動會大大降低算法的精度，因而要通過試驗以確定補償滑動的參數(shù)。以上方法僅解決了足式機器人在平地運動中的姿態(tài)求解問題，不能在非結構地形中使用。為此本文提出了一種新型的姿態(tài)求解算法，采用卡爾曼濾波器將機器人動力學與IMU相融合，實現(xiàn)了足式機器人在非結構地形中的姿態(tài)求解。

1 機器人的動力學分析

如圖1所示，在機器人的運動空間建立慣性坐標系I，并在機體質心處建立機體坐標系B。機體受到的合力包括重力,腿部對機體作用力以及機體受到的外界沖擊力。以坐標系B作為參考坐標系，機體所受合力可以用下式表示：

BFs=BFl+BFe+BG

(1)

式中,左上標B表示向量以坐標系{B}為參考坐標系，BFs為所受合力，BFl為腿部對機體作用力，BFe機體受到的外界沖擊力，BG為機體所受重力。BFl可以用下式表示：

(2)

式中,m為機器人腿的數(shù)量，Bfi為腿i對機體的作用力。機體運動加速度Bac如下：

(3)

式中,M為機體的質量，Bg為重力加速度在系{B}中對應的向量。

圖1 足式機器人模型

將機體視為桿件0并在腿部各桿件上建立坐標系。機器人腿i上桿件由機體到足端依次編號為桿件1，2…n。將桿件j的上關節(jié)編號為關節(jié)j，并將桿件j的坐標系{j}固定在關節(jié)j處。其中坐標系{1}各坐標軸與機體坐標系{B}各坐標軸平行。由圖1可知，由于腿部通過關節(jié)1與機體相連，因而Bfi即為關節(jié)1受到的腿部作用力。腿足式機器人的每條腿都可以看作是以機體為浮動基座的串聯(lián)機器人[7]。將牛頓-歐拉方程進行變形[8]，得到腿i的關節(jié)1受力Bfi的計算公式如下所示：

(4)

對于腿i來說Fi,j可以用下式所示：

(6)

(7)

2 姿態(tài)求解算法

2.1 機器人機體姿態(tài)的描述

機器人機體的橫滾角(roll)、俯仰角(pitch)和偏航角(yaw)，分別為機體相對慣性坐標系x、y、z三個軸的旋轉角度，其中橫滾角和俯仰角稱為機器人的姿態(tài)角。機體坐標系B與慣性坐標系I之間的變換關系可以采用方向余弦矩陣C表示：

(8)

式中,c代表cos，s代表sin，α、β和γ分別表示機器人機體的偏航角、俯仰角和橫滾角。將C改寫成如下形式:

(9)

注意到Z只包括β、γ兩個變量，且形式較為簡單，僅使用Z中的元素就能計算出橫滾角和俯仰角，如下式所示：

(10)

2.2 卡爾曼濾波器設計

本文采用三軸陀螺儀和加速度計，建立傳感器輸出信號模型，如下所示：

(11)

yG和yA分別為陀螺儀和加速度計的輸出信號，Bw為機體的角速度。nG和nA分別表示陀螺儀和加速度計的測量誤差，均為零均值的高斯白噪聲。

卡爾曼濾波器包括狀態(tài)方程和量測方程[9]，如下所示：

(12)

其中:xt和zt分別為狀態(tài)向量和量測向量，Φt,t-1為t-1時刻到t時刻的一步轉移矩陣，Ht為量測矩陣，wt和vt分別為系統(tǒng)激勵噪聲和量測噪聲。將狀態(tài)向量xt定義為Z。下面分別確定狀態(tài)方程和量測方程。

2.2.1 狀態(tài)方程

卡爾曼濾波器的狀態(tài)方程基于捷聯(lián)式慣導算法[10]，如下所示：

Ct=Ct-1(I+Δt[ωt-1])

(13)

式中，Ct為t時刻的方向余弦矩陣，△t為時間間隔。[ωt-1]為ωt-1的斜對稱矩陣。根據(jù)式(9)可以將式(13)整理成如下形式：

Zt=(I+Δt[ωt-1])ΤZt-1

(14)

由式(10)可知[ωt - 1]=[yG, t - 1]-[nG,t],且[ωt-1]=-[ωt-1]Τ，代入式(14)可得：

Zt= (I-Δt[yG ,t - 1])Zt - 1-Δt[Zt-1]nG,t-1

(15)

將式(15)和的狀態(tài)方程中各部分相對應，可知Φt,t-1和wt可以用下式表示：

Φt ,t - 1=I-Δt[yG, t - 1]

wt-1= -Δt[Zt-1]nG

(16)

狀態(tài)方程的噪聲協(xié)方差可以定義為Qt-1=E(wt-1wt-1Τ)，式中E表示對變量求期望，將式(15)中的wt-1代入可得Qt-1的表達式：

Qt-1=Δt2[Zt-1]∑G[Zt-1]

(17)

式中,∑G是陀螺儀測量噪聲的協(xié)方差矩陣，可以用E(nGnGΤ)表示。

2.2.2 量測方程

Bg=g×Z

(18)

將式(2)、(7)及(18)代入式(11)中的加速度計輸出信號模型，并進行整理得:

(19)

其中:下標t表示各向量在t時刻的值。將式(19)與量測方程相對應，可知zt、Ht和vt的表達式分別如下所示：

(20)

與狀態(tài)方程類似，量測方程噪聲協(xié)方差矩陣Mt由Mt=E(vtvtΤ)定義。當BFe,t不存在時，Mt為nA的噪聲協(xié)方差E(nAnAΤ)。當BFe,t存在時，量測方程會產生很大的量測誤差，不能用于姿態(tài)求解，可以通過將噪聲協(xié)方差Mt設為無窮大來實現(xiàn)，如下式表示:

(21)

2.2.3 卡爾曼濾波器的參數(shù)確定

(22)

通過卡爾曼濾波器的遞歸運算就能獲得Z的估計值，再通過(10)式的計算就得到橫滾角β和俯仰角γ。整個姿態(tài)求解算法的流程如圖2所示。

圖2 姿態(tài)求解算法的流程圖

3 仿真和算法驗證

3.1 仿真條件設置

ADAMS是一款出色的虛擬樣機仿真軟件，在對機構進行仿真的過程中，ADAMS能夠獲得各構件任意點的速度、角速度、方位角等運動學參數(shù)和力、力矩等動力學參數(shù)，并且可以將這些參數(shù)在不同的坐標系下進行表示，因而在本文中使用ADAMS進行仿真實驗以對提出算法進行驗證。在本文中選取了兩款典型的足式機器人的簡化模型進行仿真(如圖3所示)，其中四足機器人具有與哺乳類動物類似的腿部結構，六足機器人具有與昆蟲類似的腿部結構，兩款機器人腿部都由三個桿件組成，桿件之間采用旋轉關節(jié)進行連接，但關節(jié)采用不同的布置方式。機器人機體采用鋼制材料，腿部采用鋁制材料，各桿件的質量和尺寸參數(shù)如表1所示。

圖3 仿真中使用的足式機器人

如圖3所示，在仿真中使用不規(guī)則的階梯地面作為非結構地形，其中每個階梯的長度范圍為160～300 mm，高度范圍為10～40 mm，相鄰階梯隨機凸起或凹陷。在仿真中四足機器人采用對角步態(tài)(對角兩腿同時抬起或著地)，六足機器人采用三角步態(tài)(不相鄰的三腿同時抬起或著地)沿直線運動。仿真共進行三組，具體的條件設置如表2所示。

表1 機器人的結構和尺寸參數(shù)

表2 仿真實驗的條件設置

在前兩組仿真中，兩款機器人分別在階梯地面上運動。機器人開始運動時階梯地面較為平緩，隨著運動的進行，階梯高度的變化越來越劇烈。由于在階梯地面運動時，機器人很容易傾倒，因而前兩組仿真中沒有給機器人機體施加外界沖擊力。 為了檢驗機器人機體受到外界沖擊時算法的效果，在第3組仿真中四足機器人在平地上運動，在保證機器人不傾倒的情況下，對機體Y軸正方向施加了如圖4所示的沖擊力。

圖4 機器人機體外界沖擊力

3.2 數(shù)據(jù)處理分析

根據(jù)上節(jié)所示設置的條件進行仿真后，進行數(shù)據(jù)采集，數(shù)據(jù)的采樣頻率為100 Hz。這些數(shù)據(jù)包括機體的真實俯仰角和橫滾角(用于與計算值對比)、角速度、加速度、方向余弦矩陣，各桿件質心的加速度和方向余弦矩陣，關節(jié)角度及足端沖擊力等。根據(jù)關節(jié)角度采用D-H法可以得到各桿件坐標系到機體坐標系的旋轉變化矩陣。采用文獻[12]的方法，根據(jù)關系傳感器信號的特點，在真實值的基礎上加上高斯白噪聲來模擬慣性傳感器輸出信號。算法中使用的陀螺儀的輸出為在角速度基礎上加上協(xié)方差∑G=0.056I的零均值高斯白噪聲，各加速度計的輸出信息則是在運動和重力加速度之和的基礎上加上協(xié)方差∑A=0.03I的零均值高斯白噪聲。各桿件坐標系下的重力加速度用向量[0 0 g]T與桿件的方向余弦矩陣的乘積獲得。

在對采集的信息進行處理后，使用本文提出的算法對姿態(tài)角進行計算，結果如圖5、圖6所示。從圖中可以看出，在前兩組仿真開始時，由于地面較為平緩，兩款機器人姿態(tài)角比較穩(wěn)定。隨著運動的進行，階梯變化越來越劇烈，姿態(tài)角的變化也越來越大。但整個仿真過程中姿態(tài)角計算值始終能夠準確反應真實值的變化。由于這兩款具有不同的結構，并采用不同的步態(tài)運動，說明算法具有較好的通用性。在第3組仿真中，當機體沖擊力存在時，只能依靠陀螺儀進行姿態(tài)求解，由于用陀螺儀的角速度求解姿態(tài)時需要積分，噪聲在積分后產生了較大的計算偏差。當沖擊力消失后，加速度的信息又能用于姿態(tài)求解，從而使計算偏差迅速減小。從圖6中可以看出，即使在沖擊力存在的情況下，計算值的偏差仍能控制在較小水平，能夠滿足機器人平衡控制的需要。

圖5 姿態(tài)角計算結果

圖6 姿態(tài)角計算偏差

4 結論

針對以往機器人姿態(tài)求解方法存在的不足，提出了一種適用于非結構地形的姿態(tài)求解方法，該算法通過動力學分析得到機器人機體運動加速度，并將其引入到卡爾曼濾波器的量測方程中，通過與慣性測量單元(IMU)的信息相融合來獲得準確的姿態(tài)信息。通過兩款典型的足式機器人在非結構地形中的仿真分析，驗證了所提算法的有效性和通用性。與之前研究相比，本文提出的方法具有以下特點：

1)提出的算法采用動力學與IMU相結合的方法，將慣性傳感器、力傳感器和角度傳感器等多傳感器的信息相融合，實現(xiàn)了足式機器人在非結構地形中的姿態(tài)求解。

2)當機體存在外界沖擊力作用時，算法的計算值偏差將會增大，但仍能保證平衡控制的需要，當沖擊力消失后，計算值的偏差又將迅速變小。