李斯雨，閔華松，王 琪

武漢科技大學 機器人與智能系統(tǒng)研究院，武漢 430080

隨著工業(yè)技術的發(fā)展，支持操作員遠程控制的協(xié)作機器人在遠程醫(yī)療[1-2]、空間探測[3]、抗災救援[4]等復雜環(huán)境下的任務都取得了一定的成果。其中機器人從人類演示中學習技能，通常比代碼編程更加容易[5]。將人的動作或技能遷移到機器人身上的挑戰(zhàn)之一是運動學映射[6]。目前根據機械臂類型不同將映射方案廣義分成了兩大類：人臂到工業(yè)機械臂[7]，人臂到擬人機械臂[8]。其中廣泛應用的是人臂到擬人機械臂的映射，該類型系統(tǒng)結合了人臂的高度靈活性以及冗余七自由度機械臂的適應性。但由于人臂與擬人機械臂的結構有差異以及關節(jié)的驅動方式不同，人臂到擬人機械臂的運動學映射算法的開發(fā)較為困難。

人臂到擬人機械臂的運動學映射算法主要有基于關節(jié)空間的映射算法和任務空間的映射算法。在關節(jié)空間內，Savevska等[9]和Gong等[10]將人臂等效為七自由度機械臂，獲取人臂關節(jié)位姿后，以幾何方法求解人臂關節(jié)角，將擁有相同自由度的關節(jié)一一對應映射。李家霖等[11]構建手臂外骨骼，進而將人臂轉換成機械臂模型，完成運動映射。將人臂等效為七自由度機械臂模型避免擬人機械臂復雜的逆運動學，具有高效的映射效率，但人臂等效為機械臂模型后喪失了人臂關節(jié)的靈活性與自由度，導致機械臂位形相似度與末端位姿精度較差。為此在相同人臂模型下，Tomi? 等[12]加入關鍵幀數據，優(yōu)化末端精度。Wang 等[13]以機械臂連桿方向與人臂的相似性，優(yōu)化機械臂位形相似度，但在末端姿態(tài)上仍有較大誤差。關節(jié)空間映射能直接快速完成運動學映射，但關節(jié)空間映射的人臂模型提取喪失了部分人臂運動信息，難以在末端位姿精度和位形相似度上得到良好效果。

為優(yōu)化末端位姿精度和位形相似度，一部分學者在任務空間內將人臂看作整體，以人臂末端位姿和臂形建立人臂模型，并基于此完成運動學映射。Luo 等[14]通過Kinect 相機獲取人臂末端坐標，建立人臂模型，采用笛卡爾阻抗控制機械臂臂形。Shi等[15]提取人臂末端的位置和姿態(tài)，并設計一種相似度控制算法篩選符合要求的機械臂逆解。然而先求機械臂逆解，后對解進行篩選，優(yōu)化機械臂運動，完成映射。此類方法映射時間長，穩(wěn)定性差，無法完成實時模仿。為此Li等[16]和趙京等[17]通過人臂末端位姿與臂形角，建立人臂模型，提取人臂運動特征，在機械臂逆解完成前進行約束，提高映射效率。在任務空間內，以人臂末端位姿與臂形約束建立人臂模型，完成運動學映射，具有高末端精度，但在位形相似度上表現不好，以及求擬人機械臂逆解效率低，在實時映射中機器人響應速度慢。

為了完整地將人臂運動信息傳遞給擬人機械臂，提高擬人機械臂末端位姿精度和位形相似度，及在擬人機械臂對人臂運動的模仿中具有較好實時性，提出了一種基于等效關節(jié)的運動學映射方法。該方法將機械臂的多個關節(jié)融合等效為人臂生理關節(jié)，使得多個機械關節(jié)在一定條件下保持約束，重點解決了人臂關節(jié)與擬人機械臂關節(jié)之間驅動方式和關節(jié)旋轉軸線不同的問題，以此為基礎提取人臂到機械臂的轉換模型。其次針對擬人機械臂逆運動學求解復雜的問題，根據人臂到擬人機械臂的轉換模型，以及球關節(jié)的關節(jié)角的集合組成ZYZ歐拉角集合的特點，將擬人機械臂運動學逆解分解成多個子問題，分別求得各關節(jié)角。為驗證提出算法的性能，設計了單臂跟隨實驗和雙臂協(xié)調實驗并分別與文獻[10]的基于關節(jié)空間的運動學映射算法和文獻[16]的基于任務空間的運動學映射方法進行了比較。

1 擬人機械臂正運動學

為直觀表示機械臂關節(jié)旋轉軸線，使用旋量法對機械臂進行運動學建模，如圖1 所示，S1 到S7 分別表示擬人機械臂七個關節(jié)的旋轉軸線。

圖1 擬人機械臂運動學模型Fig.1 Kinematic model of anthropomorphic robotic arm

當剛體繞某一單位矢量軸ω=[ω1,ω2,ω3]T純轉動θ角時，剛體的運動變換可以用矩陣指數形式表示：

式（1）中，為反對稱矩陣：

由羅德里格斯公式可得：

式（3）中，I為單位矩陣。

擬人機械臂的運動本質上可以看成各關節(jié)的運動旋量復合而成，可以采用指數積公式進行表示：

式（4）、（5）、（6）中，gst(θ)是擬人機械臂末端位姿，為關節(jié)運動旋量，qi為關節(jié)轉動位置。

聯(lián)立式（4）、（5）得：

式（7）中，ωi≠0。

HR2 擬人機械臂旋量坐標ξi=[wi vi]T見表1，其中機械臂連桿長度為d1=174 mm，d3=292 mm，d5=242 mm，d7=234 mm，機械臂初始位姿gst(0)可用式（8）表達：

表1 擬人機械臂旋量坐標Table 1 Coordinates of anthropomorphic robotic arm screwtheory

2 基于等效關節(jié)的運動學映射

2.1 人臂模型到擬人機械臂模型轉換

根據機器人學研究[18]，可以將人臂的小臂的旋前/旋后和腕關節(jié)的2個關節(jié)進行合并，從而在腕關節(jié)處形成一個球關節(jié)，因此人類手臂可以等效為一個S-R-S構型的七自由度機械臂，擬人機械臂和人類手臂具有相同的構型，在功能實現上能夠完成人類手臂的所有動作。

人類手臂關節(jié)是靠骨骼和肌肉驅動的，以肩關節(jié)為例，肩關節(jié)以整體的形式進行運動，在完成任務時該關節(jié)自由度減少，關節(jié)旋轉軸線會發(fā)生變化，如圖2 所示在肩關節(jié)中心點位置建立右手笛卡爾坐標系，O為坐標原點，E點是肘關節(jié)中心點。當肘關節(jié)中心點E運動到E′位置時，在人體上表現為兩個旋轉軸運動，一個繞軸運動負責改變位置，一個繞軸運動負責調整姿態(tài)，其中改變位置的軸線可以隨運動的改變而變化，使得人臂具有很強的靈活性。而擬人機械臂的關節(jié)轉動軸線與機械結構相關，無法隨任務變化，在運動中與人臂具有較大差異，給運動映射帶來挑戰(zhàn)。

圖2 簡化肩關節(jié)物理模型Fig.2 Simplifying physical model of shoulder joint

擬人機械臂的各個關節(jié)滿足固定的運動學關系，每一個關節(jié)都是獨立驅動的，可以通過合理的配合和控制來實現各個關節(jié)的聯(lián)動，實現人臂到機械臂的運動映射。為完成各個關節(jié)的聯(lián)合配置，暫時拋開擬人機械臂的機械結構，將其轉化成三個生理關節(jié)，在生理結構上完成映射，確定擬人機械臂整體構型然后再映射回關節(jié)空間執(zhí)行。定義機械臂第2個關節(jié)中心為肩關節(jié)中心，機械臂第4 個關節(jié)中心為肘關節(jié)中心，機械臂第6 個關節(jié)中心為腕關節(jié)中心。

圖3 為人臂到機械臂在生理結構上的運動映射模型，SH、E H、WH和HH分別代表人臂的肩、肘、腕關節(jié)和手掌的中心，SR、E R、WR和HR分別是機械臂轉化后的生理關節(jié)中心，lup和d3分別代表人臂和機械臂肩關節(jié)中心點到肘關節(jié)中心點的上臂長度，lfore和d5分別代表人臂和機械臂肘關節(jié)中心點到腕關節(jié)中心點的小臂長度，lh和d7分別代表人臂與機械臂代表腕關節(jié)中心點到手掌中心點長度。βH和βR分別是人臂與機械臂的上臂與小臂的夾角。

圖3 運動學映射模型Fig.3 Kinematic mapping model

人臂運動特征函數為可用公式（9）進行表示：

式（9）中，qE、qW、qH分別是肘關節(jié)、腕關節(jié)和手掌中心的相對于肩部基坐標系的單位四元數。

將機械臂轉化為生理關節(jié)后，以生理關節(jié)為映射單元，確定人臂與機械臂的比例，保證機械臂的特征姿態(tài)跟隨人臂特征姿態(tài)，因此可以定義映射函數為：

式（10）中，i表示不同的關節(jié)，K為人臂與擬人機械臂連桿長度的比例，、分別表示人臂關節(jié)與擬人機械臂等效的關節(jié)的中心位置，(t)、(t)分別表示人臂關節(jié)與擬人機械臂等效的關節(jié)在某一時刻下的單位四元數。

2.2 運動映射算法

（1）坐標系轉換

映射函數確定映射關系，將人臂運動信息傳遞給機械臂，通過映射信息求解出機械臂關節(jié)角。在機器人中間軀干建立機器人系統(tǒng)基坐標系{B}，同時坐標系{B}也為世界坐標系，在擬人機械臂的基座處分別建立機械臂基坐標{Rl}和{Rr}。選擇在人體腰部軀干上建立人體基坐標系{HB}，以避免人上肢部分在運動過程中導致的基坐標系漂移。在人臂左肩關節(jié)中心和右肩關節(jié)中心建立人臂基坐標系{Hl}和{Hr}。坐標建立與轉換關系如圖4所示，下文計算均在世界坐標系{B}下完成。

圖4 人體基坐標系與基坐標轉換關系Fig.4 Human base coordinate system and base coordinate conversion relationship

人體基坐標系到世界坐標系轉換關系約束方程如下：

在統(tǒng)一基座標系下，由人臂到機械臂的轉換模型可以求解出擬人機械臂的三個關節(jié)中心的位姿，因此擬人機械臂關節(jié)角可以分成子問題求解。

（2）求解θ1、θ2和θ3

肩關節(jié)決定了肘關節(jié)中心的姿態(tài)并且球形關節(jié)的關節(jié)角是構成基座標系下的ZYZ歐拉角的集合，因而可以根據肘關節(jié)中心的旋轉矩陣R計算擬人機械臂的前三個關節(jié)角θ1、θ2、θ3：

式（12）是右手坐標系下單位四元數q=[w,x,y,z]到旋轉矩陣的變化公式，式（13）、（14）分別表示θ2在不同的取值范圍下的解。

（3）求解θ4

如圖4所示肘關節(jié)只有一個自由度，通過四元數的變換即可求得肘關節(jié)夾角βR：

式（15）中Δq是在基坐標系下肘關節(jié)姿態(tài)qE到腕關節(jié)姿態(tài)qW的中間旋轉變換。式（16）是單位四元數轉軸角公式。

聯(lián)立（15）、（16）求得βR，再由幾何關系求得擬人機械臂第四關節(jié)的關節(jié)角θ4：

（4）求解θ5、θ6和θ7

由于腕關節(jié)角的配置與肩關節(jié)類似，只需求出人臂手掌旋轉矩陣相對于腕關節(jié)旋轉矩陣，就可由式（13）、（14）求得擬人機械臂后三個關節(jié)θ5、θ6、θ7的角度。

上述人臂到擬人機械臂的運動映射方法最多可以求出四組解，解的選取和驗證在3.1.3小節(jié)詳細說明。

3 實驗

3.1 實驗設置

3.1.1 實驗平臺

為驗證提出映射算法，搭建遙操作系統(tǒng)的框架如圖5所示。系統(tǒng)由擬人雙臂機器人和慣性動作捕捉系統(tǒng)組成。

圖5 實驗系統(tǒng)結構圖Fig.5 Experimental system structure diagram

3.1.2 實驗流程

為驗證人臂到擬人機械臂運動學映射算法的性能，設計單臂軌跡跟隨和雙臂協(xié)調任務實驗，分別與文獻[10]的運動學映射算法和文獻[16]的運動學映射算法進行了對比，其中操作者的上臂和小臂參數分別為lup=280 mm和lfore=265 mm。

單臂軌跡跟隨中，操作者通過手臂運動改變手臂末端位姿與姿態(tài)，擬人機械臂復現操作者手臂動作實驗。雙臂協(xié)調任務中，操作者控制雙臂協(xié)調完成端盆，機器人實時精準復現人臂動作完成任務。

3.1.3 實驗評價指標

（1）解的選取。

機械臂關節(jié)速度的限制，實驗中將關節(jié)速度大于3.14 rad/s 的數據將會拋棄，因此在遙操作過程中，需要操作者對于動作速度進行控制，避免人為誤差。

人臂的初始位姿及構型與機械臂的初始位姿及構型可能不盡相同，為保持運動的一致性，需要將機械臂的初始位姿和構型運動到人臂的初始位姿和構型，稱該過程為映射對齊。擬人機械臂各關節(jié)轉動的角度，直接反映了映射對齊的響應速度，并通過關節(jié)角度轉動權重來減少靠近基座的關節(jié)運動對后面關節(jié)的影響。因此映射對齊過程需要對解進行選擇，采用擬人機械臂關節(jié)轉動角度的權重和進行衡量，衡量標準Mθ表達為下式：

式（18）中，ki表示機械臂各個轉動關節(jié)的轉動權值，由關節(jié)參數確定。θi_goal表示機械臂需要運動到的目標位姿的各個關節(jié)角，θi_ref表示機械臂初始位姿的各個關節(jié)角。

（2）位形相似度

其次通過以下標準進行評估人臂與機械臂位形相似度，在三維空間內，可將手臂到機械臂的相似程度轉化成空間向量的相似度，通過擬人機械臂上臂向量和人臂上臂向量之間的夾角的均方根誤差進行衡量整個運動過程的相似度。

式（19）中，d3、lup分別表示機械臂上臂和人臂上臂的方向向量，eupper(i)表示一幀動作的位形相似度。

對于小臂位形相似度衡量標準同上臂類似。從位形相似度進行約束保證機器人各個生理關節(jié)中心位置跟隨人臂生理關節(jié)中心運動，同時保證機械臂末端姿態(tài)與人臂末端姿態(tài)保持一致。為直觀顯示，可將人臂與機械臂末端姿態(tài)轉化成ZYZ歐拉角。

式（20）中，rot=α,β,γ,erot(i)表示人臂末端與機械臂末端歐拉角度的誤差。

（3）運動學映射耗時時間

運動學映射方法的耗時時間包含人體數據轉換時間和擬人機械臂關節(jié)角求解時間，直接影響擬人機械臂的響應速度。耗時時間采用式（21）進行計算。

式（21）中，tover為映射任務結束時間，tstart為映射任務開始時間，Sframe為數據總幀數。

3.2 實驗結果

3.2.1 解的選取

一幀人臂運動數據映射到機械臂上可以求得四組解。相同的人臂位姿映射到擬人機械臂，求得四組解，其中每一組解的關節(jié)角度見表2。圖6為同一個人臂動作姿態(tài)映射到擬人機械臂上，仿真得到的四種映射解?？梢钥吹讲煌慕庀聶C械臂肘關節(jié)中心的位姿和末端的位姿是相同的，而圖6中虛線框標注出的擬人機械臂肩關節(jié)（圖6 中藍色虛線框）和腕關節(jié)（圖6 中紅色虛線框）的關節(jié)角不同。

表2 不同解結果Table 2 Different results

圖6 人臂位形到擬人機械臂位形的映射解Fig.6 Mapping solution of human arm bit shape to anthropomorphic robot arm bit shape

以圖6為例，得到四組映射解，通過式（18）計算Mθ的值進行解的選擇。首先需要人為確定ki值，其中越靠近擬人機械臂基座的關節(jié)對后面關節(jié)影響越大，其ki也越大。最終計算得到四組Mθ分別為5.043 6、4.316 5、5.758 3 和3.601 9，其中第四組解的Mθ是最小的，因此使用第四組解進行映射對齊的響應時間和關節(jié)變化幅度較少。ki的值與計算結果在表2。

3.2.2 單臂跟隨實驗

在單臂跟隨實驗中，提出方法、文獻[16]方法和文獻[10]方法的實驗結果見圖7 和表3。圖8 為三種不同運動學映射方法得到的末端運動軌跡對比圖。其中圖（a）為三維空間下的末端軌跡對比，其XYZ軸坐標為位移；圖（b）、（c）和（d）分別為末端軌跡在不同坐標軸下的投影對比，橫軸坐標代表動捕數據幀數，縱坐標為不同坐標軸下的位移。從圖8 中可以看出在Y軸方向上的誤差比沿著X,Z方向上稍大。位形相似度上RMSEupper與RMSEfore的值為2%～4%，RMSErot小于1%，表明使用提出方法，擬人機械臂與人臂位形相似度高，機械臂末端較完美地跟蹤了人臂末端姿態(tài)，提出方法成功復現了人臂動作。運動學映射消耗時間Mtime為0.082 ms。

表3 實驗結果Table 3 Experimental results

圖7 人臂與擬人機械臂末端運動軌跡對比圖Fig.7 Comparison of trajectory of human arm and end of robot arm

圖8 遙操作雙臂端盆任務Fig.8 Teleoperated dual-arms to carry basin task

文獻[16]運動學映射方法，采用末端位姿和臂形角進行運動學映射，實驗結果表明完美復現了人臂的位姿和末端軌跡，RMSErot小于1%，但RMSEupper與RMSEfore的值均接近20%，這意味著增加臂形角參數也無法完全保證位形相似度。其運動學映射消耗時間Mtime為0.096 ms。

文獻[10]運動學映射方法，將人臂等效為擬人機械臂，并在關節(jié)空間里一一對應映射，實驗結果表明，機械臂復現人臂軌跡存在一定誤差，RMSErot在40%左右，RMSEupper與RMSEfore的值均接近10%，意味著末端位姿精度與位形相似度較低，其運動學映射消耗時間Mtime為0.078 ms。

由圖7和表3實驗結果分析得到：

在末端精度上，軌跡跟隨提出方法和文獻[16]方法都有良好的表現，而文獻[10]方法末端軌跡精度較差。其中文獻[16]方法中的末端位置軌跡是由計算誤差得到的，提出方法和文獻[10]方法的位置跟隨誤差主要是由人體運動過程中肩關節(jié)漂移帶來的，可以看出提出方法肩關節(jié)漂移較文獻[10]方法得到抑制。在提出方法和文獻[10]方法的末端運動軌跡得到驗證，在圖8 子圖（b）、（c）和（d）中可以看出擬人機械臂軌跡開始和終點與人臂軌跡重合，中間部分軌跡產生分離，其中文獻[10]方法肩關節(jié)中心漂移未抑制，末端軌跡中間部分偏離嚴重。末端姿態(tài)通過RMSErot指標可以看出，提出方法和文獻[16]方法產生的誤差是由計算帶來，而文獻[10]方法喪失了末端姿態(tài)精度。

在位形相似度上，提出方法遠遠大于文獻[10]方法和文獻[16]方法，其中提出方法和文獻[16]方法相比，都保持擬人機械臂臂形角與人臂臂形角一致，但文獻[16]方法未對肘關節(jié)中心的姿態(tài)進行約束，使得在位形相似度上產生了較大的誤差。提出方法與文獻[10]方法相比，提出方法從整體出發(fā)考慮到人臂與機械臂的結構差異，文獻[10]方法直接將關節(jié)一一對應映射，因此提出方法在位形相似度上優(yōu)于文獻[10]。

在運動學消耗時間上，運動學算法映射耗時時間與計算機性能擁有很大關系，但是在同一實驗平臺下，提出方法較文獻[10]方法基本持平，較文獻[16]方法計算效率提升了14%。其中提出方法將運動學映射問題分成三個階段分開計算機械臂的逆解，從而避免了七自由度機械臂的逆運動學問題。實驗結果表明提出的算法也具有較好的實時性。

綜上所述，提出方法能夠更好復現人臂的運動，計算效率高，在末端軌跡與位形相似度上，都達到了較好效果。

3.2.3 雙臂協(xié)調端盆實驗

完成了單臂的運動映射實驗后，還進行了雙臂端盆實驗。實驗過程如圖8所示，每一步完成示意圖：（a）初始位姿，（b）端上盆子，（c）端至一定高度。在運動過程中準確地復現了臂型和位姿，成功地完成了端盆任務。其中機械臂夾爪姿態(tài)復現人手掌心位姿而并非人手握取時手指姿態(tài)。

在此次任務中，統(tǒng)計了擬人機械臂各個關節(jié)角的變化。如圖9 所示，端盆任務中雙臂各個關節(jié)角變化曲線，關節(jié)角變化曲線可以看出關節(jié)運動具有連續(xù)性且平滑并且各個關節(jié)的角度變化范圍都處于擬人機械臂關節(jié)可達范圍中，可以有效地滿足復雜任務。

圖9 雙臂各關節(jié)角變化曲線Fig.9 Change curve of each joint angle of both arms

4 結束語

研究了基于等效關節(jié)的擬人機械臂運動學映射算法，該方法以人臂運動特點出發(fā)，將擬人機械臂等效為人臂生理關節(jié)，提出人臂到擬人機械臂的轉換模型，重點解決了人臂關節(jié)與機械臂關節(jié)之間驅動方式和旋轉軸線不同的問題。針對擬人機械臂關節(jié)角求解復雜的問題，將其分解成多個子問題，簡化擬人機械臂逆運動學求解。通過仿真和實際實驗表明，所提出映射算法能使操作者以更自然的方式控制擬人雙臂機器人，同時機器人可以完美復現人臂動作，完成復雜的任務。

所提出方法仍然具有一定局限性，首先，未能完全通過算法抑制肩部漂移帶來的誤差，導致末端位姿有一定偏差。其次，可穿戴設備固定在人體表面，當操作者做一些特殊動作時，會出現傳感器偏移造成誤差，不能準確反映人體動作，從而造成擬人機械臂任務失敗。

在未來工作中，希望設計一種更加合理的映射算法，抑制人臂運動過程中肩關節(jié)漂移帶來的誤差，以及更進一步將所提出算法推廣到非擬人機械臂上，使其擁有更大的應用場景。