球面手腕康復(fù)機(jī)構(gòu)逆運(yùn)動(dòng)學(xué)解新方法及應(yīng)用

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

(太原理工大學(xué) 機(jī)械與運(yùn)載工程學(xué)院，山西 太原 030024)

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

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

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

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

1 球面手腕康復(fù)機(jī)構(gòu)的運(yùn)動(dòng)學(xué)模型

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

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

表1 手腕活動(dòng)范圍Tab.1 Wrist range of motion

圖1 人體腕關(guān)節(jié)運(yùn)動(dòng)模型Fig.1 Human wrist joint movement model

1.2 坐標(biāo)系設(shè)定

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

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

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

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

1.3 運(yùn)動(dòng)學(xué)模型

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

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

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

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

式中：β為機(jī)構(gòu)參數(shù).

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

動(dòng)平臺(tái)姿態(tài)Vi為

式中：

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

2 基于歐拉角逆運(yùn)動(dòng)學(xué)求解的新方法

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

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

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

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

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

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

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

4)結(jié)合式(9)，可得

5)利用式(9)、(10)，計(jì)算Ti對應(yīng)的關(guān)節(jié)轉(zhuǎn)角.

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

7)當(dāng)動(dòng)平臺(tái)繞Z軸旋轉(zhuǎn)0,0,φ3時(shí)，近端連桿所需的輸入角度為

將θ*與進(jìn)行加法運(yùn)算后，可得唯一的逆運(yùn)動(dòng)學(xué)解，即當(dāng)CSPM動(dòng)平臺(tái)旋轉(zhuǎn)角度φ1,φ2,φ3時(shí)，近端連桿輸入角度為

對上述過程歸納整理后，得到如圖4所示的唯一逆運(yùn)動(dòng)學(xué)解流程圖.

圖4 求解唯一逆運(yùn)動(dòng)學(xué)解的流程圖Fig.4 Flow chart for solving unique inverse kinematics

2.2 結(jié)果驗(yàn)證與討論

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

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

利用文獻(xiàn)[14]的正運(yùn)動(dòng)學(xué)方法對逆運(yùn)動(dòng)學(xué)新解法進(jìn)行驗(yàn)證，將得到的逆運(yùn)動(dòng)學(xué)解θ代入式(4)，可得

根據(jù)CSPM動(dòng)平臺(tái)任意2個(gè)姿態(tài)向量之間的幾何約束關(guān)系，存在

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

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

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

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

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

將式（31）的正運(yùn)動(dòng)學(xué)解與式（25）所示的初始值進(jìn)行對比可知，兩者的誤差非常小，驗(yàn)證了所提方法的正確性.

3 基于逆解新方法的手腕康復(fù)機(jī)構(gòu)工作空間

3.1 奇異位形分析

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

對式(7)求導(dǎo)，得到

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

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

條件系數(shù)定義為

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

3.2 連桿碰撞檢測

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

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

CSPM在運(yùn)動(dòng)過程中只能發(fā)生以下2種類型的碰撞[18]：1)在相鄰2個(gè)驅(qū)動(dòng)桿與遠(yuǎn)端連桿的連接點(diǎn)處發(fā)生碰撞；2)動(dòng)平臺(tái)與底座之間發(fā)生碰撞.

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

圖6 不同碰撞的碰撞點(diǎn)位置Fig.6 Collision point position of different collisions

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

3.3 手腕康復(fù)機(jī)構(gòu)的姿態(tài)角空間與實(shí)際工作空間

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

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

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

圖8 CSPM實(shí)際工作空間Fig.8 Actual workspace of CSPM

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

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

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

q可以表示為

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

設(shè)定機(jī)構(gòu)起始姿態(tài)為初始位姿，經(jīng)過旋轉(zhuǎn)變換q1，機(jī)構(gòu)姿態(tài)為“姿態(tài)1”；在初始位形下經(jīng)過旋轉(zhuǎn)變換q2，q1≠q2機(jī)構(gòu)姿態(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”的插補(bǔ)表達(dá)式：

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

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

4.2 實(shí)驗(yàn)驗(yàn)證

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

圖10 手腕康復(fù)機(jī)構(gòu)樣機(jī)Fig.10 Wrist rehabilitation mechanism prototype

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

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

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

5 結(jié) 論

(1) 提出并驗(yàn)證了共軸3RRR并聯(lián)機(jī)構(gòu)逆運(yùn)動(dòng)學(xué)解的新方法，解決了球面手腕康復(fù)裝置逆運(yùn)動(dòng)學(xué)解完備性和無解析解的問題.

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

(3) 提出并驗(yàn)證了基于四元數(shù)插補(bǔ)的實(shí)時(shí)軌跡規(guī)劃方案，φ1、φ2的最大誤差為1°，φ3的最大誤差為2.5°，為手腕康復(fù)機(jī)構(gòu)的控制器設(shè)計(jì)提供了基礎(chǔ).