張博文，黃攀峰，劉正雄

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

1 引言

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

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

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

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

2 人臂與機(jī)械臂建模

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

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

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

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

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

3 體感控制方案

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

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

圖3 部分人體骨骼關(guān)鍵點(diǎ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é)點(diǎn)坐標(biāo)是基于Kinect相機(jī)坐標(biāo)系的，而Kinect相機(jī)坐標(biāo)系固連于Kinect傳感器。在計(jì)算關(guān)節(jié)角向量時(shí)需要關(guān)節(jié)點(diǎn)相對于固連于人體的基座標(biāo)系的坐標(biāo)，故定義人體右肩關(guān)節(jié)點(diǎn)作為人體基座標(biāo)系原點(diǎn)，將關(guān)節(jié)點(diǎn)坐標(biāo)轉(zhuǎn)換到與人體固連的基坐標(biāo)系中。

定義Kinect相機(jī)坐標(biāo)系為Ok，固連于人體的基坐標(biāo)系為Oh，定義人體基座標(biāo)系的Z軸方向?yàn)槿梭w左肩關(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é)點(diǎn)的坐標(biāo)減去右肩關(guān)節(jié)A在Ok中的坐標(biāo)A(xA,yA,zA)，易得轉(zhuǎn)換完成后關(guān)節(jié)點(diǎn)A的坐標(biāo)為A(0,0,0)。第二步為旋轉(zhuǎn)變換，記任意關(guān)節(jié)點(diǎn)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 操作周期與運(yùn)動周期定義

(4)

(5)

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

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

(6)

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

(7)

圖4 操作周期與運(yùn)動周期Fig.4 The action period and the movement period

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

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

4.1 濾波器設(shè)計(jì)

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

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

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

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

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

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

由圖5可見，在操作者運(yùn)動過程中，右肩關(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é)點(diǎn)，在操作時(shí)其數(shù)據(jù)波動越大，不可信數(shù)據(jù)越多。因此在濾波時(shí)，既需要平滑處理，也需要剔除失真的數(shù)據(jù)。

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

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

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

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

4.1.2 濾波器設(shè)計(jì)

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

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

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

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

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 濾波器效果驗(yàn)證

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

圖7 三個(gè)關(guān)節(jié)點(diǎn)在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)行，僅使用操作者手臂的前四個(gè)自由度完成體感操作的改進(jìn)策略。即將操作周期和運(yùn)動周期都分為如下兩步：

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

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

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

5 試驗(yàn)驗(yàn)證

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

5.1 平臺搭建

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

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

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

圖10 仿真試驗(yàn)場景Fig.10 The scene of simulation experiment

5.2 空間機(jī)械臂體感控制仿真試驗(yàn)

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

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

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

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

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

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

6 結(jié)論

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

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