基于IMU的主動伴舞機(jī)器人人機(jī)協(xié)作控制算法*

劉 召，宋立濱，于 濤，郭 凱，王增喜，耿美曉

(1.清華大學(xué)天津高端裝備研究院，天津 300304;2.清研華宇智能機(jī)器人(天津)有限責(zé)任公司，天津 300304)

1 引言

自傳統(tǒng)工業(yè)機(jī)器人誕生以來，機(jī)器人技術(shù)在短短的60年內(nèi)便取得了顯著的進(jìn)步。如今，機(jī)器人已經(jīng)不再受限于工業(yè)應(yīng)用，它的研究重點(diǎn)和發(fā)展方向更趨向于日常生活的應(yīng)用，如餐飲、迎賓、娛樂、陪伴等[1]。同時，機(jī)器人技術(shù)的發(fā)展對人機(jī)協(xié)作的主動性要求也越來越高，也就是說，機(jī)器人不僅能夠根據(jù)人的施力情況而被動運(yùn)動，而且能根據(jù)預(yù)先學(xué)習(xí)的知識與人協(xié)調(diào)作業(yè)，即主動協(xié)調(diào)控制[2]。

本文以主動型舞蹈機(jī)器人作為人機(jī)協(xié)作機(jī)器人的研究背景，機(jī)器人的主干外觀如圖1所示。主動型舞蹈機(jī)器人可以按照預(yù)先學(xué)習(xí)的舞步規(guī)則帶領(lǐng)人完成舞蹈動作，還可以根據(jù)人在舞蹈過程中對機(jī)器人上身施加的力實(shí)時調(diào)整舞蹈軌跡，避免人受到傷害，實(shí)現(xiàn)人與機(jī)器人之間的協(xié)調(diào)運(yùn)動，具有更高的主動性和智能性。

傳統(tǒng)的人機(jī)協(xié)作機(jī)器人控制系統(tǒng)中大多采用多維力傳感器來感受外力的大小和方向，以此作為控制算法的輸入[3]。這種控制系統(tǒng)對外力的感知簡便、準(zhǔn)確，但價格十分昂貴。本文采用一個安裝在機(jī)器人腰部的慣性測量單元IMU(Inertial Measurement Unit)模塊來取代力傳感器，通過感知機(jī)器人上身相對于穩(wěn)定姿態(tài)的角度變化來判斷機(jī)器人的受力大小和方向，識別人的意圖，再結(jié)合預(yù)先學(xué)習(xí)的舞步規(guī)則進(jìn)行運(yùn)動協(xié)調(diào)，以此作為對機(jī)器人擬定軌跡的修正依據(jù)，保證帶領(lǐng)人跳舞動作的協(xié)調(diào)性。

Figure 1 Appearance of active dancing robot圖1 主動伴舞機(jī)器人外觀

2 基于Kalman濾波的IMU數(shù)據(jù)融合

IMU模塊包含1個3軸陀螺儀和1個3軸加速度計(jì)。加速度計(jì)對載體的加速度變化敏感，取瞬時值計(jì)算傾角的誤差比較大；陀螺儀數(shù)據(jù)通過積分得到的傾角不受加速度的影響，短時間內(nèi)精度較高，但隨時間的增加，積分累積誤差和溫度漂移帶來的誤差較大，甚至有可能導(dǎo)致結(jié)果發(fā)散。因此在舞蹈機(jī)器人實(shí)時姿態(tài)判斷算法中，需要使用Kalman濾波算法，利用加速度計(jì)和陀螺儀各自的特點(diǎn)將數(shù)據(jù)進(jìn)行融合，以抑制誤差，提高測量精度[4,5]。

本文采用陀螺儀建立的機(jī)器人上身姿態(tài)模型如下所示：

αk=αk-1+(uk-1-βk-1)T

其中，α為機(jī)器人上半身的姿態(tài)角，u為陀螺儀角速度測量值，β為陀螺儀穩(wěn)態(tài)漂移，T為采樣時間。

狀態(tài)估計(jì)時，認(rèn)為陀螺儀穩(wěn)態(tài)漂移為固定值，即：

βk=βk-1

將這兩個方程寫成矩陣形式，即得到：

(1)

由此可得采用卡爾曼濾波進(jìn)行數(shù)據(jù)融合算法過程[6]：

(1)建立系統(tǒng)狀態(tài)方程：

X(k)=AX(k-1)+Buk-1

(2)

(2)由上一次的誤差協(xié)方差P(k-1)和過程噪聲預(yù)測新的誤差P(k)：

P(k)=AP(k-1)AT+Q

(3)

其中，Q_acce為加速度計(jì)的協(xié)方差，Q_gyro為陀螺儀測量協(xié)方差，其數(shù)值代表卡爾曼濾波器對其傳感器數(shù)據(jù)的信任程度，值越小，信任度越高。

(3)計(jì)算卡爾曼增益。

Kg(k)=P(k)HT(HP(k)HT+R)-1

(4)

(4)進(jìn)行校正更新，得到狀態(tài)k的最優(yōu)估算值X(k)：

X(k)=X(k-1)+Kg(k)(Z(k)-HX(k-1))

(5)

其中，Z(k)為k時刻的系統(tǒng)測量值。

(5)為下一步估計(jì)k+1狀態(tài)最優(yōu)值迭代進(jìn)行更新操作，并更新k狀態(tài)協(xié)方差P(k)值：

P(k)=(I-Kg(k)H)P(k-1)

(6)

其中，I為單位矩陣。

計(jì)算完時間更新和測量更新后，再次重復(fù)上一次計(jì)算得到的后驗(yàn)估計(jì)，作為下一次計(jì)算的先驗(yàn)估計(jì)，如此循環(huán)運(yùn)算直到找到最優(yōu)的結(jié)果為止，由此便得到了主動伴舞機(jī)器人判斷人施力狀況的姿態(tài)角。

3 舞蹈機(jī)器人人機(jī)協(xié)作控制算法

主動型伴舞機(jī)器人的運(yùn)動狀態(tài)由兩部分合成，第一部分是機(jī)器人根據(jù)預(yù)先學(xué)習(xí)的舞步規(guī)則得到的目標(biāo)坐標(biāo)和期望速度矢量；另一部分是人類舞伴對機(jī)器人的實(shí)時作用力產(chǎn)生的人為速度矢量。系統(tǒng)根據(jù)兩個速度矢量的和對機(jī)器人執(zhí)行開環(huán)速度控制。

(7)

Figure 2 Composite velocity of the robot圖2 機(jī)器人受合速度示意圖

3.1 機(jī)器人主動運(yùn)動軌跡

主動型舞蹈機(jī)器人按照預(yù)先學(xué)習(xí)的舞步規(guī)則，配合預(yù)定的伴奏音樂，確定軌跡坐標(biāo)序列及時間序列。假定在某一段軌跡上，機(jī)器人原始的運(yùn)動規(guī)律可表示為：

ydi=fdi(x)

θdi=gdi(x)

(8)

3.2 人力對機(jī)器人運(yùn)動影響算法

機(jī)器人的上身和下身通過裝有彈簧的圓盤連接，將IMU水平放置在腰部的圓盤上，當(dāng)人對機(jī)器人上身產(chǎn)生推拉或扭轉(zhuǎn)的作用力時，IMU的實(shí)時輸出值就會發(fā)生變化，經(jīng)Kalman濾波后就會反應(yīng)到橫滾角、俯仰角和扭轉(zhuǎn)角的變化上，由此來判斷人對機(jī)器人作用力的大小和方向。機(jī)器人則在帶領(lǐng)人跳舞的同時，根據(jù)人力的大小判斷人的意圖，向人力做出妥協(xié)，避免人受到傷害。

3.2.1 相對姿態(tài)角求解

由于機(jī)械結(jié)構(gòu)誤差和安裝誤差，機(jī)器人上身姿態(tài)角的靜態(tài)值不可能永遠(yuǎn)為零，因此就需要人為對姿態(tài)角進(jìn)行校正。

當(dāng)機(jī)器人靜止，不受到外力作用時，連續(xù)測量、求取N次機(jī)器人三個方向姿態(tài)角，取平均值，得到靜止?fàn)顟B(tài)下機(jī)器人的橫滾角Rool0、俯仰角Pitch0和扭轉(zhuǎn)角Twist0。機(jī)器人運(yùn)動時，實(shí)時計(jì)算三個方向姿態(tài)角，并與靜止姿態(tài)角做差，得到理想狀態(tài)下的相對姿態(tài)角：

Rollre=Roll-Roll0

Pitchre=Pitch-Pitch0

Twistre=Twist-Twist0

(9)

但是,在實(shí)際的機(jī)器人模型上，連接彈簧的剛度值要同時兼顧對外力的響應(yīng)變化和保持運(yùn)動過程的相對穩(wěn)定，因此機(jī)器人運(yùn)動過程中上身的小幅度擺動不可避免。綜合考慮以上情況，要根據(jù)機(jī)器人獨(dú)立運(yùn)動時三個姿態(tài)角的抖動，選擇合適的閾值調(diào)整相對姿態(tài)角的大小。調(diào)整后的相對姿態(tài)角表達(dá)式如式(10)所示：

(10)

其中，p、q、m分別為姿態(tài)角的抖動誤差。

3.2.2 相對姿態(tài)角到運(yùn)動速度矢量的映射

在人力作用下，機(jī)器人上身姿態(tài)角隨作用力的增大而單調(diào)遞增，但力過大時會造成彈簧的剛性形變，機(jī)器人結(jié)構(gòu)會被損壞；同時，為避免機(jī)器人運(yùn)動過程中的劇烈晃動，選擇的彈簧剛度較大，一般的人力無法使彈簧發(fā)生十分大的形變，所以作用力必須小于一個最大值，姿態(tài)角也相應(yīng)地收斂。在數(shù)學(xué)函數(shù)表達(dá)式中，指數(shù)函數(shù)可以很好地表達(dá)這一特性，因此，人作用力大小與姿態(tài)角的關(guān)系可以用指數(shù)函數(shù)表示，具體由式(11)來表示：

Fhuman=Fmax(1-e-b θ),|θ|≤θmax

(11)

其中,θ為機(jī)器人上身受外力作用下的姿態(tài)角(橫滾角/俯仰角/扭轉(zhuǎn)角)，θmax為彈簧系統(tǒng)允許的最大姿態(tài)角度的大小，b決定指數(shù)函數(shù)曲線斜率變化規(guī)律，通過實(shí)驗(yàn)獲得最佳取值。

由此可得機(jī)器人在人類舞伴作用力下的動力學(xué)方程：

(12)

3.2.3 基于狀態(tài)判斷的運(yùn)動決策

在得到相對姿態(tài)角與機(jī)器人動力學(xué)關(guān)系后，并不能直接用于人機(jī)協(xié)作的運(yùn)動計(jì)算，需要結(jié)合機(jī)器人當(dāng)前狀態(tài)來判斷此刻的數(shù)據(jù)是否有效，因?yàn)槔缭跈C(jī)器人靜止?fàn)顟B(tài)下，我們并不希望一個突然的、瞬時的力影響機(jī)器人的狀態(tài)。

3.3 人力對機(jī)器人運(yùn)動影響算法

機(jī)器人在運(yùn)動過程中，我們以50 Hz的頻率向機(jī)器人發(fā)送速度控制信號。根據(jù)式(7)的合速度矢量表達(dá)式，可得Δt時間段內(nèi)機(jī)器人的運(yùn)動軌跡長度：

(13)

則機(jī)器人運(yùn)動到第i個目標(biāo)點(diǎn)時的位姿表達(dá)式為：

(14)

由于人類舞伴的作用力使原始軌跡坐標(biāo)有所改變，因此第i個目標(biāo)點(diǎn)后的軌跡點(diǎn)坐標(biāo)都要同時做出修正。

4 實(shí)驗(yàn)與測試

針對本文提出的人機(jī)協(xié)作控制算法，進(jìn)行了華爾茲中CCL(Closed Change Left)舞步的人機(jī)協(xié)作實(shí)驗(yàn)，機(jī)器人自主運(yùn)動且無人力作用時，CCL舞步的期望軌跡如圖3所示。

Figure 3 Diagram of desired CCL trajectory圖3 CCL舞步期望軌跡示意圖

兩次實(shí)驗(yàn)中，在機(jī)器人走CCL舞步中途，人類舞伴對機(jī)器人分別施加一段時間的“推動力”和“拉力”，IMU采集到的加速度、角速度數(shù)據(jù)經(jīng)卡爾曼濾波后，得到的橫滾角、俯仰角和扭轉(zhuǎn)角曲線分別如圖4所示。

Figure 4 Curves of attitude angle when pushing the robot圖4 人推動機(jī)器人時姿態(tài)角變化曲線

由圖4中可以看出，推動、拉動機(jī)器人時的敏感變量為俯仰角，其隨人力呈現(xiàn)較為明顯的變化，但其余兩個姿態(tài)角也有不同幅度的變化，但變化相對較小。經(jīng)閾值濾波后，相對姿態(tài)角曲線如圖5所示。

Figure 5 Curves of relative attitude angle圖5 相對姿態(tài)角變化曲線

在CCL舞步中，在人推動或者拉動機(jī)器人時，希望機(jī)器人向前幅度變小或變大，但并不希望其旋轉(zhuǎn)，因此通過狀態(tài)判斷，就可以剔除扭轉(zhuǎn)角的瞬時變化。在人機(jī)協(xié)作狀態(tài)下，機(jī)器人實(shí)際運(yùn)動軌跡與期望軌跡的對比曲線分別如圖6所示。

Figure 6 Curve comparison of the desired trajectory and man-machine collaboration trajectory圖6 期望軌跡與人機(jī)協(xié)作軌跡對比曲線

實(shí)驗(yàn)結(jié)果表明，本文算法在機(jī)器人實(shí)際運(yùn)動中有效且效果較好。

5 結(jié)束語

本文針對主動型伴舞機(jī)器人的人機(jī)協(xié)作問題進(jìn)行了研究，提出了基于IMU的人機(jī)協(xié)作控制方式。通過卡爾曼濾波算法將加速度計(jì)和陀螺儀產(chǎn)生的實(shí)時加速度和角速度進(jìn)行融合，得出舞蹈機(jī)器人上身的實(shí)時姿態(tài)角，通過姿態(tài)角的變化判斷人類舞伴對機(jī)器人的作用力，從而機(jī)器人可以根據(jù)人力的作用實(shí)時修改軌跡，完成人和機(jī)器人的協(xié)調(diào)動作，避免人受到傷害。實(shí)驗(yàn)結(jié)果驗(yàn)證了算法的有效性。

[1] Tan Min, Wang Shuo. Research progress on robotics [J].ACTA Automatica Sinica,2013,39(7):963-972.(in Chinese)

[2] Wang Ting,Wang Yue-chao.Human-machine system and its application to mobile robot [J].Robot,2004,26(6):553-557.(in Chinese)

[3] Kosuge K,Hayashi T,Hirata Y,et al.Dance partner robot:MS Dancer [C]∥Proc of the 2003 IEEE/ RSJ International Conference on Intelligent Robots and Systems,2003:3459-3464.

[4] Feng Zhi-yong, Zeng Han,Zhang Li,et al.Angle measurement based on gyroscope and accelerometer signal fusion [J].Journal of Southwest China Normal University (Natural Science Edition),2011,36(4):137-141.(in Chinese)

[5] Wang Shuai,Wei Guo.Application of Kalman filter in attitude measurement for four-rotor aircraft flight[J].Ordnance Industry Automation,2011,30(1):73-74.(in Chinese)

[6] Peng Ding-cong.Basic principle and application of Kalman filter [J].Software Guide,2009,8(11):32-34.(in Chinese)

附中文參考文獻(xiàn)：

[1] 譚民，王碩.機(jī)器人技術(shù)研究進(jìn)展[J].自動化學(xué)報,2013,39(7):963-972.

[2] 王挺，王越超.人機(jī)系統(tǒng)在移動機(jī)器人平臺的應(yīng)用[J].機(jī)器人,2004,26(6):553-557.

[4] 馮志勇，曾瀚，張力，等.基于陀螺儀及加速度計(jì)信號融合的姿態(tài)角度測量[J].西南師范大學(xué)學(xué)報(自然科學(xué)版),2011,36(4):137-141.

[5] 王帥，魏國.卡爾曼濾波在四翼飛行器姿態(tài)測量中的應(yīng)用[J].兵工自動化,2011,30(1):73-74.

[6] 彭丁聰.卡爾曼濾波的基本原理及應(yīng)用[J].軟件導(dǎo)刊,2009,8(11):32-34.