，，

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

0 引言

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

1 機(jī)器人的動(dòng)力學(xué)分析

如圖1所示，在機(jī)器人的運(yùn)動(dòng)空間建立慣性坐標(biāo)系I，并在機(jī)體質(zhì)心處建立機(jī)體坐標(biāo)系B。機(jī)體受到的合力包括重力,腿部對(duì)機(jī)體作用力以及機(jī)體受到的外界沖擊力。以坐標(biāo)系B作為參考坐標(biāo)系，機(jī)體所受合力可以用下式表示：

BFs=BFl+BFe+BG

(1)

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

(2)

式中,m為機(jī)器人腿的數(shù)量，Bfi為腿i對(duì)機(jī)體的作用力。機(jī)體運(yùn)動(dòng)加速度Bac如下：

(3)

式中,M為機(jī)體的質(zhì)量，Bg為重力加速度在系{B}中對(duì)應(yīng)的向量。

圖1 足式機(jī)器人模型

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

(4)

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

(6)

(7)

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

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

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

(8)

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

(9)

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

(10)

2.2 卡爾曼濾波器設(shè)計(jì)

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

(11)

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

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

(12)

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

2.2.1 狀態(tài)方程

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

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

(13)

式中，Ct為t時(shí)刻的方向余弦矩陣，△t為時(shí)間間隔。[ωt-1]為ωt-1的斜對(duì)稱矩陣。根據(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)方程中各部分相對(duì)應(yīng)，可知Φ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表示對(duì)變量求期望，將式(15)中的wt-1代入可得Qt-1的表達(dá)式：

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

(17)

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

2.2.2 量測(cè)方程

Bg=g×Z

(18)

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

(19)

其中:下標(biāo)t表示各向量在t時(shí)刻的值。將式(19)與量測(cè)方程相對(duì)應(yīng)，可知zt、Ht和vt的表達(dá)式分別如下所示：

(20)

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

(21)

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

(22)

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

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

3 仿真和算法驗(yàn)證

3.1 仿真條件設(shè)置

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

圖3 仿真中使用的足式機(jī)器人

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

表1 機(jī)器人的結(jié)構(gòu)和尺寸參數(shù)

表2 仿真實(shí)驗(yàn)的條件設(shè)置

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

圖4 機(jī)器人機(jī)體外界沖擊力

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

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

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

圖5 姿態(tài)角計(jì)算結(jié)果

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

4 結(jié)論

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

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

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