基于Kinect的七自由度空間機械臂體感控制方法

張博文，黃攀峰，劉正雄

(1. 西北工業(yè)大學(xué)航天學(xué)院智能機器人研究中心，西安 710072； 2. 西北工業(yè)大學(xué)航天飛行動力學(xué)技術(shù)重點實驗室，西安 710072)

1 引言

隨著機器人技術(shù)的發(fā)展，機械臂的應(yīng)用范圍越來越廣，在某些特殊的環(huán)境下，由機械臂代替人體完成任務(wù)將極大的降低危險性，例如空間、深海、輻射環(huán)境等[1]。使用傳統(tǒng)的機械臂控制方式控制難度較大，學(xué)習(xí)成本較高。因此，機器人的體感控制逐漸發(fā)展起來。與機械臂的傳統(tǒng)控制方式相比，體感控制具有許多優(yōu)點，如直觀、便捷、易用等。

Kinect是微軟發(fā)布的深度信息傳感器[2]，由于其能夠采集多個用戶信息，配置和使用簡單，精度高等優(yōu)點，近年來被逐漸使用在機械臂的體感控制上。使用Kienct傳感器對機械臂進(jìn)行體感控制可以充分發(fā)揮人手臂的靈活特點，操作者只需對自身手臂進(jìn)行控制，無需附加其他設(shè)備。

使用Kinect對機械臂進(jìn)行體感控制，關(guān)鍵問題是在操作者的手臂運動與機械臂的運動之間建立運動映射。人體與機械臂的運動映射方法主要有關(guān)節(jié)空間映射和末端笛卡爾空間映射兩種[3]。Nguyen等[4]針對三自由度機械臂，采用關(guān)節(jié)空間映射方法將人臂自由度映射到機械臂相應(yīng)關(guān)節(jié)上，并使用空間向量法計算機械臂關(guān)節(jié)角度，實現(xiàn)運動復(fù)現(xiàn)；但使用的機械臂自由度較低。衛(wèi)沅等[5]針對特定構(gòu)型七自由度機械臂，使用四個關(guān)節(jié)點求取逆運動學(xué)解，對機械臂進(jìn)行實時控制。王朝陽等[6]針對SRS(Spherical-Roll-Spherical)構(gòu)型的七自由度機械臂，提出了一種基于肘部關(guān)節(jié)約束的逆運動學(xué)解法，根據(jù)關(guān)節(jié)點位置直接解算出機械臂角度。上述兩種方法均針對特定結(jié)構(gòu)機械臂，而對肩、肘均存在偏置的冗余機械臂則無法求解。

為了解決現(xiàn)有方法無法使用人臂對肩、肘均存在偏置的七自由度空間機械臂進(jìn)行體感控制的問題，本文提出一種基于操作周期和運動周期的增量式關(guān)節(jié)角映射方法，在操作周期內(nèi)使用Kinect傳感器采集操作者手臂關(guān)節(jié)點坐標(biāo)，經(jīng)濾波處理后使用空間向量法計算關(guān)節(jié)角，并將關(guān)節(jié)角增量映射給機械臂關(guān)節(jié)角，作為機械臂控制指令實現(xiàn)體感控制。

2 人臂與機械臂建模

手臂關(guān)節(jié)由肩關(guān)節(jié)、肘關(guān)節(jié)、腕關(guān)節(jié)組成，肩關(guān)節(jié)和腕關(guān)節(jié)能夠繞三軸運動，具有三個自由度；肘關(guān)節(jié)可以視為一個轉(zhuǎn)動關(guān)節(jié)，具有一個自由度。在將人臂自由度向機械臂自由度映射時，可將球關(guān)節(jié)等效為三個軸線相交的串聯(lián)軸，以人體右臂為研究對象，七個自由度的劃分如圖1所示。

本文以如圖2所示七自由度空間機械臂作為被控對象。機械臂末端在上方，基座在下方并固定于服務(wù)航天器上，其結(jié)構(gòu)類似于人臂?；浇湍┒烁浇哂腥齻€自由度，可以視為肩關(guān)節(jié)和腕關(guān)節(jié)，但三軸不交于一點；中間有一個自由度，可以視為肘關(guān)節(jié)。

圖1 手臂關(guān)節(jié)自由度Fig.1 DOF of the human arm

圖2 使用的七自由度空間機械臂Fig.2 The 7-DOF space manipulator used

將人手臂垂直放下，手心向內(nèi)作為初始狀態(tài)，將機械臂展開，末端向內(nèi)作為初始狀態(tài)。該空間機械臂的結(jié)構(gòu)與人手臂是十分相似的，因此可以使用關(guān)節(jié)角映射的方法來進(jìn)行空間機械臂的體感控制。

3 體感控制方案

3.1 坐標(biāo)轉(zhuǎn)換

本文使用Kinect V2傳感器，能夠采集每個用戶25個關(guān)節(jié)，能夠采集的關(guān)節(jié)已由Kinect傳感器直接給定，能夠采集的部分關(guān)節(jié)角如圖3所示[7]。

圖3 部分人體骨骼關(guān)鍵點[7]Fig.3 Part of skeleton joint positions of the human[7]

本文使用空間向量法(該方法詳見文獻(xiàn)[8])求取操作者手臂關(guān)節(jié)角度，需處理的關(guān)節(jié)包括：脊柱O、右肩A、右肘B、右腕C、右手掌D、右手掌尖E、右拇指尖F，左肩G。

由于Kinect提供的人體關(guān)節(jié)點坐標(biāo)是基于Kinect相機坐標(biāo)系的，而Kinect相機坐標(biāo)系固連于Kinect傳感器。在計算關(guān)節(jié)角向量時需要關(guān)節(jié)點相對于固連于人體的基座標(biāo)系的坐標(biāo)，故定義人體右肩關(guān)節(jié)點作為人體基座標(biāo)系原點，將關(guān)節(jié)點坐標(biāo)轉(zhuǎn)換到與人體固連的基坐標(biāo)系中。

定義Kinect相機坐標(biāo)系為Ok，固連于人體的基坐標(biāo)系為Oh，定義人體基座標(biāo)系的Z軸方向為人體左肩關(guān)節(jié)指向右肩關(guān)節(jié)。坐標(biāo)轉(zhuǎn)換分兩步進(jìn)行，包括平移和旋轉(zhuǎn)：首先，記右肩關(guān)節(jié)A在Ok中的坐標(biāo)為A(xA,yA,zA)，平移變換即將待轉(zhuǎn)換關(guān)節(jié)點的坐標(biāo)減去右肩關(guān)節(jié)A在Ok中的坐標(biāo)A(xA,yA,zA)，易得轉(zhuǎn)換完成后關(guān)節(jié)點A的坐標(biāo)為A(0,0,0)。第二步為旋轉(zhuǎn)變換，記任意關(guān)節(jié)點V進(jìn)行平移變換之后，旋轉(zhuǎn)變換前的坐標(biāo)為V(xk,yk,zk)，進(jìn)行旋轉(zhuǎn)變換后的坐標(biāo)為Vh(xh,yh,zh)，兩者的關(guān)系如式(1)：

(1)

其中RT是正交旋轉(zhuǎn)矩陣，可以用式(2)表示[6]：

(2)

ix=[1,0,0]T,jy=[0,1,0]T,kz=[0,0,1]T

(3)

3.2 操作周期與運動周期定義

(4)

(5)

Kinect傳感器可直接輸出操作者手勢信息，因此可以方便地使用Kinect傳感器采集操作者手勢變化判斷操作周期的開始與結(jié)束：操作者由握拳到手張開且五指并攏為操作周期開始，操作者由手張開且五指并攏到握拳為操作周期結(jié)束。為避免手勢對關(guān)節(jié)角的影響，使用左臂作為手勢采集的目標(biāo)。操作周期開始后，操作者進(jìn)行手臂動作，機械臂得到關(guān)節(jié)角信號，開始運動，運動周期開始；操作周期結(jié)束后，機械臂運動停止，運動周期結(jié)束。即每個操作周期對應(yīng)一個運動周期，連續(xù)循環(huán)直至操作任務(wù)完成。這樣，操作者操縱機械臂完成任務(wù)的過程即可劃分為若干個操作周期，機械臂的動作過程依據(jù)操作周期劃分為若干個運動周期。在分析操作者操縱機械臂的過程時，具體分析每一個操作周期即可，在研究機械臂完成整個運動的過程時，具體分析每一個運動周期即可。

3.3 基于周期循環(huán)的控制方法

(6)

在第k個運動周期內(nèi)，機械臂關(guān)節(jié)角計算如式(7)：

(7)

圖4 操作周期與運動周期Fig.4 The action period and the movement period

經(jīng)過多個操作周期與運動周期，即可完成相應(yīng)任務(wù)。

4 體感控制策略設(shè)計

4.1 濾波器設(shè)計

Kinect傳感器的輸出數(shù)據(jù)存在一定的噪聲，某些幀甚至?xí)霈F(xiàn)數(shù)據(jù)的劇烈波動，使用帶有噪聲或這種不穩(wěn)定值的數(shù)據(jù)直接計算機械臂關(guān)節(jié)角會造成機械臂運動的不穩(wěn)定。因此，必須對Kinect傳感器的輸出數(shù)據(jù)進(jìn)行濾波處理，以得到平滑、穩(wěn)定的關(guān)節(jié)角信號。

王朝陽等[6]設(shè)計了一種基于限幅的實時消抖濾波算法，通過定義抖動閾值與計數(shù)器容量來消除七個關(guān)節(jié)角的抖動，但是濾波器導(dǎo)致了數(shù)據(jù)延遲，降低了操作的實時性。范勇濤[9]使用卡爾曼濾波對計算得到的關(guān)節(jié)角進(jìn)行平滑處理，消除了噪聲，但是只對數(shù)據(jù)進(jìn)行了平滑處理，沒有考慮數(shù)據(jù)跳變的情況。但是，上述方法均是直接對關(guān)節(jié)角數(shù)據(jù)進(jìn)行濾波處理，而忽略了對計算關(guān)節(jié)角的數(shù)據(jù)來源，即關(guān)節(jié)點坐標(biāo)的分析。因此，本文首先對關(guān)節(jié)點坐標(biāo)進(jìn)行識別精度分析，然后根據(jù)分析結(jié)果設(shè)計濾波器。

4.1.1 關(guān)節(jié)點坐標(biāo)識別精度分析

為了有針對性的設(shè)計識別算法，必須了解Kinect對人體關(guān)節(jié)點識別精度的影響因素，了解噪聲和數(shù)據(jù)跳變產(chǎn)生的原因。

在操作者的運動過程中，靠近身體的關(guān)節(jié)點，如肩關(guān)節(jié)，與遠(yuǎn)離身體的關(guān)節(jié)，如肘關(guān)節(jié)、腕關(guān)節(jié)等運動范圍與運動的劇烈情況顯然不同，關(guān)節(jié)點的識別精度也不相同。選取右肩關(guān)節(jié)點A，右肘關(guān)節(jié)點B及右拇指關(guān)節(jié)點F，對比其在一段時間內(nèi)，操作者進(jìn)行手臂操作運動時采集到的Z軸上坐標(biāo)數(shù)據(jù)如圖5，其中橫坐標(biāo)單位選取為幀，Kinect傳感器采集數(shù)據(jù)的頻率為30 FPS。

圖5 A、B、F三個關(guān)節(jié)點在Z軸上的坐標(biāo)數(shù)據(jù)Fig.5 Coordinate datain Z axis of three joint point

由圖5可見，在操作者運動過程中，右肩關(guān)節(jié)是最穩(wěn)定的，而肘部關(guān)節(jié)具有一定波動，在第220和360幀出現(xiàn)了較為嚴(yán)重的數(shù)據(jù)波動，而拇指關(guān)節(jié)的數(shù)據(jù)失真現(xiàn)象是最嚴(yán)重的，數(shù)據(jù)波動也最多。從圖中可以發(fā)現(xiàn)，越靠近手部的關(guān)節(jié)點，在操作時其數(shù)據(jù)波動越大，不可信數(shù)據(jù)越多。因此在濾波時，既需要平滑處理，也需要剔除失真的數(shù)據(jù)。

本文在計算手臂腕關(guān)節(jié)的三個關(guān)節(jié)角時，使用了腕部的關(guān)節(jié)點坐標(biāo)進(jìn)行計算。因此取右手部位三個關(guān)節(jié)點——右手掌關(guān)節(jié)點D、右手中指尖關(guān)節(jié)點E、右手拇指關(guān)節(jié)點F——的Z軸坐標(biāo)數(shù)據(jù)進(jìn)行對比，如圖6所示。

圖6 D、E、F三個關(guān)節(jié)點在Z軸上的坐標(biāo)數(shù)據(jù)Fig.6 Coordinate datain Z axis of three joint points

由圖可見，手掌部分三個關(guān)節(jié)的坐標(biāo)極為接近，且某些幀均存在較大的數(shù)據(jù)波動。在計算關(guān)節(jié)角時，使用這些關(guān)節(jié)點數(shù)據(jù)會造成誤差的累計和放大。

綜上，Kinect傳感器對手掌部位的三個關(guān)節(jié)點識別精度較低。這一方面是由于手部關(guān)節(jié)易于產(chǎn)生遮擋等情況，造成某些幀數(shù)據(jù)波動；另一方面由于手掌附近的三個關(guān)節(jié)點坐標(biāo)值差距很小，經(jīng)過計算導(dǎo)致誤差的放大。這三種原因會導(dǎo)致計算得到的腕關(guān)節(jié)三個關(guān)節(jié)角波動較大，難以反映真實情況。

4.1.2 濾波器設(shè)計

針對上文對Kienct采集關(guān)節(jié)角數(shù)據(jù)精度的分析，本文提出一種限幅平滑濾波算法，濾波過程分兩部進(jìn)行：

1)限幅濾波，主要目的是消除某些特殊幀產(chǎn)生的大幅度數(shù)據(jù)波動。定義兩次采樣之間允許的大偏差值T，每次采樣后進(jìn)行判斷。如果本次值與上次值之差小于最大偏差值T，則本次值有效；如果本次值與上次值之差大于最大偏差值T，則本次值無效，認(rèn)定本次采樣值為數(shù)據(jù)波動，放棄本次值，用上次值代替本次值。

2)五點三次平滑濾波，主要目的是消除采樣值在真實值附近微小范圍內(nèi)的抖動，使計算出的機械臂關(guān)節(jié)角更加穩(wěn)定、平滑。取五次連續(xù)采樣值進(jìn)行擬合，得到一條三次函數(shù)曲線，使用三次函數(shù)曲線上相應(yīng)點的值作為平滑結(jié)果，每次采樣進(jìn)行一次平滑處理。

設(shè)連續(xù)的五次采樣值為yi-2,yi-1,yi,yi+1,yi+2，設(shè)有式(8)所示三次多項式：

y=a0+a1x+a2x2+a3x3

(8)

(9)

綜上，限幅平滑濾波算法偽代碼如下：

/**********************/

Algorithm

input:realtimeValue[i-2],realtime Value[i-1],realtime Value[i],realtime Value[i+1],realtime Value[i+2],max Difference

output: steady Value[i]

/**********************/

If i<3Then

steady Value[i]=realtime Value[i]

Else

if|realtime Value[i]-realtime Value[i-1]| >max Difference

realtime Value[i]=realtime Value[i-1]

End

SteadyValue[i]=(-3*realtimeValue[i-2]+

12*realtimeValue[i-1]+17*realtimeValue[i]+12*realtimeValue[i+1]-3*realtimeValue[i+2])/35

End

return realtime Value[i]

/**********************/

4.1.3 濾波器效果驗證

針對右肩關(guān)節(jié)點G、右肘關(guān)節(jié)點B及右拇指關(guān)節(jié)點F對比其在一段時間內(nèi)，操作者進(jìn)行手臂操作運動時采集到的Z軸上坐標(biāo)的濾波前后數(shù)據(jù)，如圖7所示。

圖7 三個關(guān)節(jié)點在Z軸上濾波后的坐標(biāo)數(shù)據(jù)Fig.7 Coordinate data in Z axis of three joint points after filtering

與圖5進(jìn)行對比可以發(fā)現(xiàn)，關(guān)節(jié)坐標(biāo)數(shù)據(jù)的微小抖動現(xiàn)象抑制明顯，數(shù)據(jù)相比于濾波前更加穩(wěn)定流暢，且相對于未濾波前的數(shù)據(jù)延遲僅兩幀。

4.2 基于分步操作周期的體感控制策略

基于上述結(jié)果，本文提出一種將操作周期分兩步進(jìn)行，僅使用操作者手臂的前四個自由度完成體感操作的改進(jìn)策略。即將操作周期和運動周期都分為如下兩步：

1)當(dāng)操作者做出操作周期開始手勢(左手張開，五指并攏)時，采集操作者肩關(guān)節(jié)和肘關(guān)節(jié)的四個關(guān)節(jié)坐標(biāo)，經(jīng)濾波后計算得到機械臂關(guān)節(jié)角，映射給機械臂前四個關(guān)節(jié)，此時機械臂運動周期第一步開始，此時后三個關(guān)節(jié)角保持固定，當(dāng)操作者做出停止手勢(左手握拳)時，操作周期第一步結(jié)束，開始操作周期第二步；

2)采集操作者肩關(guān)節(jié)三個關(guān)節(jié)角，實時映射給機械臂末端三個關(guān)節(jié)，此時機械臂前四個關(guān)節(jié)停止運動，保持靜止。

圖8 分步操作周期Fig.8 The operation period by step

5 試驗驗證

針對上文提出的基于操作周期與運動周期的體感控制算法與濾波算法，設(shè)計仿真試驗進(jìn)行驗證，首先搭建仿真機械臂控制系統(tǒng)，然后在虛擬環(huán)境中構(gòu)建機械臂工作環(huán)境，實現(xiàn)對七自由度空間機械臂的體感控制。

5.1 平臺搭建

本文搭建的試驗平臺結(jié)構(gòu)如圖9所示，包括操作者、Kinect傳感器、數(shù)據(jù)處理與仿真系統(tǒng)三部分。

圖9 試驗平臺結(jié)構(gòu)Fig.9 The structure of experiment platform

在Unity3D物理引擎中搭建機械臂仿真試驗場景，作為仿真系統(tǒng)，通過調(diào)用Kinect SDK實時獲取Kinect傳感器讀取的數(shù)據(jù)。仿真試驗場景如圖10所示。

圖10 仿真試驗場景Fig.10 The scene of simulation experiment

5.2 空間機械臂體感控制仿真試驗

試驗通過使用上述基于分步操作周期的體感控制策略來實時控制仿真環(huán)境中的機械臂末端到達(dá)其工作空間內(nèi)的某一空間點。試驗過程分為兩步：

1) 操作者做出初始姿勢，開始第一個操作周期，空間機械臂從圖11所示的初始狀態(tài)開始第一個運動周期。

圖11 操作者初始姿勢Fig.11 The initial gesture of the operator

2) 操作者進(jìn)行動作，實時控制機械臂運動；同時通過左手手勢控制操作周期的開始與結(jié)束；操作周期與運動周期循環(huán)進(jìn)行，制導(dǎo)機械臂到達(dá)目標(biāo)狀態(tài)。

試驗結(jié)果如圖12所示，可見試驗經(jīng)歷了兩個操作周期。操作周期第一步時，操作者左臂處于固定姿勢，使用手勢進(jìn)行操作周期開始和結(jié)束的操作。第一個操作周期略過了第二步，因為此時機械臂末端還沒有到達(dá)目標(biāo)點附近，因此并未對機械臂的腕關(guān)節(jié)三個自由度進(jìn)行調(diào)整，直至第二個操作周期的第二步，機械臂腕關(guān)節(jié)到達(dá)指定位置之后，開始對機械臂后三個自由度進(jìn)行控制，以達(dá)到更加精細(xì)的末端位姿。

圖12 空間七自由度機械臂體感控制Fig.12 Somatosensory control of 7-DOF space manipulator

6 結(jié)論

1)提出的基于操作周期和運動周期的機械臂體感控制方法，使用關(guān)節(jié)角增量將人體手臂關(guān)節(jié)角映射給機械臂關(guān)節(jié)角，經(jīng)仿真驗證，可實現(xiàn)機械臂關(guān)節(jié)角穩(wěn)定的體感控制。

2)提出的針對關(guān)節(jié)點坐標(biāo)的限幅平滑濾波算法，經(jīng)仿真驗證，能有效剔除某些幀的較大失真值，并削減采樣值在真實值附近的微小抖動。