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

張博文，黃攀峰，劉正雄

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

1 引言

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

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

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

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

2 人臂與機械臂建模

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

本文以如圖2所示七自由度空間機械臂作為被控對象。機械臂末端在上方，基座在下方并固定于服務航天器上，其結構類似于人臂。基座附近和末端附近均具有三個自由度，可以視為肩關節和腕關節，但三軸不交于一點；中間有一個自由度，可以視為肘關節。

圖1 手臂關節自由度Fig.1 DOF of the human arm

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

將人手臂垂直放下，手心向內作為初始狀態，將機械臂展開，末端向內作為初始狀態。該空間機械臂的結構與人手臂是十分相似的，因此可以使用關節角映射的方法來進行空間機械臂的體感控制。

3 體感控制方案

3.1 坐標轉換

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

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

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

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

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

(1)

其中RT是正交旋轉矩陣，可以用式(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傳感器采集操作者手勢變化判斷操作周期的開始與結束：操作者由握拳到手張開且五指并攏為操作周期開始，操作者由手張開且五指并攏到握拳為操作周期結束。為避免手勢對關節角的影響，使用左臂作為手勢采集的目標。操作周期開始后，操作者進行手臂動作，機械臂得到關節角信號，開始運動，運動周期開始；操作周期結束后，機械臂運動停止，運動周期結束。即每個操作周期對應一個運動周期，連續循環直至操作任務完成。這樣，操作者操縱機械臂完成任務的過程即可劃分為若干個操作周期，機械臂的動作過程依據操作周期劃分為若干個運動周期。在分析操作者操縱機械臂的過程時，具體分析每一個操作周期即可，在研究機械臂完成整個運動的過程時，具體分析每一個運動周期即可。

3.3 基于周期循環的控制方法

(6)

在第k個運動周期內，機械臂關節角計算如式(7)：

(7)

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

經過多個操作周期與運動周期，即可完成相應任務。

4 體感控制策略設計

4.1 濾波器設計

Kinect傳感器的輸出數據存在一定的噪聲，某些幀甚至會出現數據的劇烈波動，使用帶有噪聲或這種不穩定值的數據直接計算機械臂關節角會造成機械臂運動的不穩定。因此，必須對Kinect傳感器的輸出數據進行濾波處理，以得到平滑、穩定的關節角信號。

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

4.1.1 關節點坐標識別精度分析

為了有針對性的設計識別算法，必須了解Kinect對人體關節點識別精度的影響因素，了解噪聲和數據跳變產生的原因。

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

圖5 A、B、F三個關節點在Z軸上的坐標數據Fig.5 Coordinate datain Z axis of three joint point

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

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

圖6 D、E、F三個關節點在Z軸上的坐標數據Fig.6 Coordinate datain Z axis of three joint points

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

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

4.1.2 濾波器設計

針對上文對Kienct采集關節角數據精度的分析，本文提出一種限幅平滑濾波算法，濾波過程分兩部進行：

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

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

設連續的五次采樣值為yi-2,yi-1,yi,yi+1,yi+2，設有式(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 濾波器效果驗證

針對右肩關節點G、右肘關節點B及右拇指關節點F對比其在一段時間內，操作者進行手臂操作運動時采集到的Z軸上坐標的濾波前后數據，如圖7所示。

圖7 三個關節點在Z軸上濾波后的坐標數據Fig.7 Coordinate data in Z axis of three joint points after filtering

與圖5進行對比可以發現，關節坐標數據的微小抖動現象抑制明顯，數據相比于濾波前更加穩定流暢，且相對于未濾波前的數據延遲僅兩幀。

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

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

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

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

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

5 試驗驗證

針對上文提出的基于操作周期與運動周期的體感控制算法與濾波算法，設計仿真試驗進行驗證，首先搭建仿真機械臂控制系統，然后在虛擬環境中構建機械臂工作環境，實現對七自由度空間機械臂的體感控制。

5.1 平臺搭建

本文搭建的試驗平臺結構如圖9所示，包括操作者、Kinect傳感器、數據處理與仿真系統三部分。

圖9 試驗平臺結構Fig.9 The structure of experiment platform

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

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

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

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

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

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

2) 操作者進行動作，實時控制機械臂運動；同時通過左手手勢控制操作周期的開始與結束；操作周期與運動周期循環進行，制導機械臂到達目標狀態。

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

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

6 結論

1)提出的基于操作周期和運動周期的機械臂體感控制方法，使用關節角增量將人體手臂關節角映射給機械臂關節角，經仿真驗證，可實現機械臂關節角穩定的體感控制。

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