球面手腕康復機構逆運動學解新方法及應用

焦文杰，姬帥旭，郝惠敏，黃家海，李利娜，李詩雨

(太原理工大學 機械與運載工程學院，山西 太原 030024)

隨著國內(nèi)人口老齡化的加速，中風發(fā)病率呈逐年增加的趨勢.據(jù)統(tǒng)計可知，國內(nèi)中風患者目前已達1 300多萬，預計2030年可能會超過3 000萬人[1].中風患者的高效康復訓練是當前的關注點之一，相對于傳統(tǒng)理療和藥物治療，康復機器人可以節(jié)約大量人力、物力，手腕康復訓練裝置是其中之一[2-3].

近年來，國內(nèi)外學者對腕部康復機器人/裝置開展了較多的研究工作，如氣動人工肌肉手腕康復裝置[4]、繩索驅(qū)動腕部康復機構[5]、混聯(lián)式腕關節(jié)康復機構[6]以及基于3RRR球面并聯(lián)機構(spherical parallel mechanism，SPM)的手腕康復裝置[7].其中，SPM手腕康復裝置具有結構緊湊、貼合手腕運動特征的優(yōu)點.

SPM具有俯仰、偏航和滾動的運動特點[8]，Gosselin等[9-11]對該機構的運動學特性開展了一定的研究工作.通常將該機構逆運動學的研究轉(zhuǎn)換為3個一元二次方程的求解問題，由于方程組最多存在8個解，給驅(qū)動系統(tǒng)控制器的設計帶來了一定的挑戰(zhàn)[12].目前，3RRR球面并聯(lián)機構逆運動學唯一解的求解方案有2個：方案1是建立3RRR球面并聯(lián)機構逆運動學方程，選取一元二次方程正根對應的解作為機構逆運動學解[13]；方案2是在8組解中選取與初始姿態(tài)相同工作模式的一組解作為唯一的逆運動學解[14-15].上述2種方案都存在不足，方案1存在部分動平臺姿態(tài)無逆運動學解的問題，方案2不能得到具體的逆運動學解析解.

針對共軸3RRR球面并聯(lián)機構（CSPM）逆運動學唯一解求解方法的不足，提出基于歐拉角的逆運動學分步求解方法.該方法將基于歐拉角描述的動平臺姿態(tài)分解為繞Z軸和繞X、Y軸旋轉(zhuǎn)的子姿態(tài)A和子姿態(tài)B，并分別求解出子姿態(tài)A的逆解和子姿態(tài)B的最優(yōu)解，將2個子姿態(tài)最優(yōu)解線性相加，可以獲得CSPM唯一逆解.將該方法與單位四元數(shù)相結合，并應用于球面手腕康復裝置的軌跡規(guī)劃中，以提高裝置運動控制的實時性，這對CSPM控制器設計具有實際的工程意義.

1 球面手腕康復機構的運動學模型

1.1 人體腕關節(jié)的簡介

人體腕部具有3個自由度，以人體腕部建立坐標系，腕部分別可實現(xiàn)繞Z軸轉(zhuǎn)動的屈曲(F)/伸展(E)運動、繞Y軸轉(zhuǎn)動的內(nèi)收(A)/外展(O)和繞X軸轉(zhuǎn)動的旋內(nèi)(M)/旋外(L)運動，如圖1所示.由GB/T 15499—1995及其他資料[16-17]可知，正常情況下實現(xiàn)所述6個運動時手腕轉(zhuǎn)動角度γ的范圍，如表1所示.

表1 手腕活動范圍Tab.1 Wrist range of motion

圖1 人體腕關節(jié)運動模型Fig.1 Human wrist joint movement model

1.2 坐標系設定

共軸3RRR球面并聯(lián)機構如圖2所示，由3個近端連桿（彎曲弧度為α1）、3個遠端連桿（彎曲弧度為α2）、動平臺和底座組成，其中近端連桿為驅(qū)動桿.近端連桿與底座、近端連桿與遠端連桿、遠端連桿與動平臺均采用轉(zhuǎn)動副連接，使得CSPM動平臺在空間中只能作旋轉(zhuǎn)運動，與手腕運動相貼合.在初始條件下，CSPM動平臺和底座呈水平狀態(tài)，3根近端連桿呈120°均勻分布.CSPM的特點是所有關節(jié)軸線相交于O點，后文將其稱為旋轉(zhuǎn)中心.

以圖2的O點為坐標原點建立固定坐標系，Z軸垂直于底座，方向向上.在共軸3RRR球面并聯(lián)機構中，3根驅(qū)動軸存在共軸關系，驅(qū)動桿軸向量、近端連桿與遠端連桿相連的軸向量、動平臺姿態(tài)向量分別采用Ui、Wi、Vi（i= 1，2，3）表示.Vi與Z軸的夾角為β.定義Z軸與Wi所組成的平面為測量平面Pi（i= 1，2，3），Y軸位于P1平面內(nèi)，利用右手定理可以確定X軸方向.

設定驅(qū)動軸關節(jié)轉(zhuǎn)角θ=[θ1,θ2,θ3]是測量平面Pi轉(zhuǎn)動的角度，逆時針方向為正，順時針為負，如圖3所示.在工作空間內(nèi)，使用XYZ歐拉角表示動平臺姿態(tài)，初始姿態(tài)下，動平臺呈水平狀態(tài)，設定驅(qū)動桿轉(zhuǎn)角為θ=[θ1,θ2,θ3].

圖3 CSPM驅(qū)動軸正方向的設定Fig.3 Setting of CSPM drive shaft positive direction

1.3 運動學模型

1) 初始姿態(tài).在固定坐標系下，3個驅(qū)動軸單位向量相同，

在初始時刻，Wi*(i=1,2,3)為

式中：α1為機構參數(shù)；ηi=2(i-1)π/3,i=1,2,3.

在初始姿態(tài)下，V1水平投影與平面P1的夾角為δ，如圖3所示.根據(jù)幾何關系可知，在初始時刻，

式中：β為機構參數(shù).

2) 非初始姿態(tài).當輸入角度為θi時，Wi為

動平臺姿態(tài)Vi為

式中：

其中，φ1、φ2、φ3為歐拉角參數(shù).

2 基于歐拉角逆運動學求解的新方法

使用歐拉角()表示動平臺姿態(tài)，CSPM最多存在8組逆運動學解，但只有一組解滿足與初始姿態(tài)工作模式相同的要求，稱為手腕康復裝置的唯一逆運動學解.

2.1 共軸球面并聯(lián)機構求解的新方法

聯(lián)立式(4)、(5)，根據(jù)幾何約束列出方程：

將式(4)、(5)代入式(7)，可以簡化為

由于CSPM具有3根輸入軸重合的特征，當動平臺繞Z軸旋轉(zhuǎn)φ時，3個驅(qū)動桿也會旋轉(zhuǎn)φ.由此可知，在CSPM逆運動學的求解過程中，驅(qū)動桿旋向差異主要取決于動平臺姿態(tài)角φ1、φ2.基于上述特性，提出基于分步計算CSPM逆運動學求解的新方法.將動平臺姿態(tài)分解為繞Z軸和繞X、Y軸旋轉(zhuǎn)的2個部分，分別計算出2個部分姿態(tài)近端連桿所需的最優(yōu)輸入角度及；根據(jù)輸入軸重合的特點，將進行加法運算后，得到完整的逆運動學解.

1)根據(jù)CSPM的特點，將動平臺姿態(tài)分解為繞X、Y軸旋轉(zhuǎn)的和繞Z軸旋轉(zhuǎn)的，動平臺姿態(tài)向量使用Vi表示.

3)根據(jù)式(10)計算：

4)結合式(9)，可得

5)利用式(9)、(10)，計算Ti對應的關節(jié)轉(zhuǎn)角.

式中：min表示取最小值，abs表示取絕對值.

7)當動平臺繞Z軸旋轉(zhuǎn)0,0,φ3時，近端連桿所需的輸入角度為

將θ*與進行加法運算后，可得唯一的逆運動學解，即當CSPM動平臺旋轉(zhuǎn)角度φ1,φ2,φ3時，近端連桿輸入角度為

對上述過程歸納整理后，得到如圖4所示的唯一逆運動學解流程圖.

圖4 求解唯一逆運動學解的流程圖Fig.4 Flow chart for solving unique inverse kinematics

2.2 結果驗證與討論

以圖2所示的共軸3RRR球面并聯(lián)機構為例，機構參數(shù)為α1=50°,α2=57.6°,β=75°,δ=60°.機構的裝配方式為遠端連桿均在近端連桿的左側(cè)，如圖3所示.設動平臺的位姿角為(30°, 15°, 20°)，利用式(5)求解得到

根據(jù)式(14)～(19)，求解得到

利用文獻[14]的正運動學方法對逆運動學新解法進行驗證，將得到的逆運動學解θ代入式(4)，可得

根據(jù)CSPM動平臺任意2個姿態(tài)向量之間的幾何約束關系，存在

式中：φ3為相鄰姿態(tài)向量Vi之間的夾角，有α3=2arcsin(cosβcos(π/6)).

將式(5)、(27)代入式(7)，結合式(27)、(28)可得

式中 ：v1x、v1y、v1z為V1的X、Y、Z軸分量.

根據(jù)機構初始姿態(tài)下的裝配方式，確定方程的初值為

使用matlab中的fsolve函數(shù)，求解得到

將式（31）的正運動學解與式（25）所示的初始值進行對比可知，兩者的誤差非常小，驗證了所提方法的正確性.

3 基于逆解新方法的手腕康復機構工作空間

3.1 奇異位形分析

手腕康復裝置的奇異性是指CSPM處于某些特殊姿態(tài)時會失去固有剛度且機構可控性變差.球面并聯(lián)機構運動路徑應該遠離奇異位形，以免機構喪失自由度，降低機構的可控性.

對式(7)求導，得到

將式(33)、(34)代入式(32)，化簡得到

式中：J為雅可比矩陣，J=[J1,J2,J3]T，

條件系數(shù)定義為

通過ζ(J)可以判斷機構姿態(tài)是否處于奇異位形.當ζ(J)→0時，CSPM姿態(tài)處于奇異位形附近；反之，CSPM姿態(tài)遠離奇異位形.定義ζmin(J)來判定機構位姿點是否位于工作空間內(nèi)，參考眾多研究， 將ζmin(J)定義為0.25.

3.2 連桿碰撞檢測

根據(jù)文獻[18]可知，CSPM連桿碰撞的原因是相鄰驅(qū)動桿之間的夾角過大或者過小，本文采用歐式距離判斷碰撞是否發(fā)生.如圖5所示，假設近端連桿和遠端連桿的連接點為Bi(i=1,2,3)，動平臺與上端連桿的連接點為Ci(i=1,2,3).通過幾何距離測量可知，共軸上點Ai、Bi、Ci與O點的歐式距離都為100 mm，即OAi=OBi=OCi=100mm，計算Ai、Bi、Ci在固定坐標系下的坐標為

圖5 3RRR球面并聯(lián)機構連接點位置Fig.5 3RRR spherical parallel mechanism connection point position

CSPM在運動過程中只能發(fā)生以下2種類型的碰撞[18]：1)在相鄰2個驅(qū)動桿與遠端連桿的連接點處發(fā)生碰撞；2)動平臺與底座之間發(fā)生碰撞.

對于所研究的CSPM手腕康復裝置而言，第1類碰撞是近端連桿末端之間發(fā)生碰撞.設定碰撞點為D，如圖6(a)所示，此時測量BiBj=22mm,i、j=1,2,3,i≠j，因此碰撞邊界條件為

圖6 不同碰撞的碰撞點位置Fig.6 Collision point position of different collisions

對于所研究的CSPM手腕康復裝置而言，第2類碰撞是動平臺與底座邊緣之間發(fā)生了碰撞，碰撞點為F，如圖6(b)所示.此時測量∠AiOCi=22°，ACi=38mm（i=1,2,3），碰撞的邊界條件為

3.3 手腕康復機構的姿態(tài)角空間與實際工作空間

對于CSPM 手腕康復裝置而言，設定不發(fā)生奇異和碰撞的歐拉角=為姿態(tài)角工作空間Ω.采用蒙特卡洛法進行工作空間計算，采樣點數(shù)為200 000，假設繞X軸轉(zhuǎn)動的姿態(tài)角φ1的采樣范圍為[-90°,90°]；繞Y軸轉(zhuǎn)動的姿態(tài)角φ2的采樣范圍為[-90°,90°]；繞Z軸轉(zhuǎn)動的姿態(tài)角φ3的范圍為[-180°,180°].以每一個采樣點姿態(tài)角對應的ζ(J)是否大于0.25以及是否滿足第1、2碰撞條件作為判斷依據(jù)，若某一采樣點姿態(tài)角滿足上述條件，則判定該點屬于歐拉角工作空間Ω.由于CSPM在工作空間可以繞旋轉(zhuǎn)軸無限旋轉(zhuǎn)，求解得到工作空間中繞Z軸轉(zhuǎn)動角度φ3仍為[-180°,180°]，如圖7所示.

圖7 CSPM歐拉角工作空間Fig.7 Euler angle workspace of CSPM

為了確保中風患者康復訓練時的手腕運動角度小于表1所示的極限運動角度，避免患者手腕受到損傷，將CSPM手腕康復裝置的實際運動空間限定在以下范圍：φ3轉(zhuǎn)動角度為，φ2轉(zhuǎn)動角度為-15°,30°,φ1轉(zhuǎn)動角度為.如圖8所示.

圖8 CSPM實際工作空間Fig.8 Actual workspace of CSPM

4 逆解新方法在路徑規(guī)劃中的應用

根據(jù)CSPM手腕康復裝置的設計目標，使用者只能在固定軌跡下進行康復訓練.根據(jù)歐拉角和單位四元數(shù)之間存在的轉(zhuǎn)換關系，可以將手腕實際工作的空間姿態(tài)點轉(zhuǎn)化為單位四元數(shù)，結合所提的逆運動學新方法與單位四元數(shù)插補，實時得到平滑的曲線.

4.1 單位四元數(shù)插補

q可以表示為

單位四元數(shù)q可以轉(zhuǎn)化為歐拉角：

設定機構起始姿態(tài)為初始位姿，經(jīng)過旋轉(zhuǎn)變換q1，機構姿態(tài)為“姿態(tài)1”；在初始位形下經(jīng)過旋轉(zhuǎn)變換q2，q1≠q2機構姿態(tài)為 “姿態(tài)2”，如圖9所示.

圖9 單位四元數(shù)表示旋轉(zhuǎn)Fig.9 Unit quaternion representation rotation

從圖9可知，“姿態(tài)1”與“姿態(tài)2”可以通過單位四元數(shù)Δq表示：

根據(jù)單位四元數(shù)性質(zhì)q-1=q*，得到“姿態(tài)1”到“姿態(tài)2”的插補表達式：

式中：q*1為q1的共軛四元數(shù)，t為時間.

將插值點qt使用式(47)轉(zhuǎn)化為歐拉角，利用求解運動學唯一逆運動學解的方法對全部插值點進行求解，可以得到路徑點的關節(jié)轉(zhuǎn)角.

4.2 實驗驗證

搭建如圖10所示的實驗裝置.選擇初始位姿γ1=[0°,0°,0°]為“姿態(tài)1”，在CSPM手腕康復機構實際工作空間中選取為“姿態(tài)2”，代入式(45)轉(zhuǎn)換為單位四元數(shù)：

圖10 手腕康復機構樣機Fig.10 Wrist rehabilitation mechanism prototype

通過LabVIEW編寫上位機程序，arduino編寫下位機程序，下位機接收上位機發(fā)送的初始點和終點歐拉角度，使用所提的方法進行運動學逆求解及驅(qū)動步進電機.通過查詢相關資料，將實驗運動時間設定為4 s，采用等值壓縮方法，將運動時間點映射到[0,1.0]，并代入式(47)獲得路徑點姿態(tài).為了保證實驗過程中速度更加平滑，引入T型曲線規(guī)劃算法規(guī)劃速度.使用無線位姿傳感器測量動平臺姿態(tài)，反饋給上位機.

實驗結果和理論結果如圖11所示.可知，理論計算結果與試驗測試結果均為光滑曲線，φ1、φ2的最大誤差約為1°，φ3的最大誤差約為2.5°，誤差均在合理范圍內(nèi).上述結果表明，所提的逆運動解方案是可行、可信的.該實驗過程中的誤差產(chǎn)生原因如下.1)實驗為開環(huán)控制策略.2)結構安裝誤差.轉(zhuǎn)動副在安裝過程中未能消除間隙，導致了動平臺姿態(tài)誤差的產(chǎn)生.3)零件彈性變形.驅(qū)動桿和遠端連桿發(fā)生了微小的彈性變形，造成了動平臺的姿態(tài)誤差.圖11所示的實驗曲線比較光滑，表明CSPM手腕康復機構在運動過程中姿態(tài)變化平滑，為后續(xù)手腕康復機構的實時控制器設計提供了良好基礎.

圖11 軌跡規(guī)劃中歐拉角實驗值與理論值的對比Fig.11 Comparison of experimental value and theoretical value of Euler angle in trajectory planning

5 結 論

(1) 提出并驗證了共軸3RRR并聯(lián)機構逆運動學解的新方法，解決了球面手腕康復裝置逆運動學解完備性和無解析解的問題.

(2)基于逆運動學解的新方法，以無連桿碰撞和無奇位形為約束條件，得到手腕康復裝置歐拉角姿態(tài)空間.通過分析手腕運動的特點，獲得手腕康復裝置實際工作空間：

(3) 提出并驗證了基于四元數(shù)插補的實時軌跡規(guī)劃方案，φ1、φ2的最大誤差為1°，φ3的最大誤差為2.5°，為手腕康復機構的控制器設計提供了基礎.