基于計算力矩法的骨折復位機器人系統仿真

董鑫宇，孫 昊，郭 悅，傅卓鑫

(1.河北工業大學，天津 300132；2.國家康復輔具研究中心附屬康復醫院，北京 100176)

1 引言

長骨骨折是骨折中最常見的骨折種類，其中股骨，肱骨骨干骨折分別占成年人骨折的6%和5%[1]，骨折發生時，閉合的復位并固定是最佳的治療方案。該方法的優勢在于可以減少軟組織解刨，對于骨折愈合，尤其是軟組織挫傷嚴重或有骨膜損傷的骨折愈合的意義重大。外科醫生在治療骨折的過程中，常常需要多名經驗豐富的醫生聯合進行操作，即便是最優秀的醫生團隊在消費大量體力的同時也不能保證每名患者的復位效果滿足預期。骨折復位完成后還需要考慮如何固定以及進一步促進愈合的問題，內固定雖可靠，但需切開骨折端，對血運破壞明顯，容易導致感染和不愈合。而外固定則易面臨復位不徹底、術后固定失效和松動斷裂等問題。上述問題的解決在目前看來必須依賴骨科機器人的引入。通過骨折復位機器人進行骨折復位的治療，骨折復位的精度將得到提高，同時因為不知疲倦的機器手臂可以穩定控制骨斷端，理論上將可以明顯的減少傳統骨折復位手術中的并發癥，如擠壓、血管神經損傷或者二次骨折的發生。

并聯機構具有承載能力強，末端執行器精度高等優點，6-UPU并聯機構是一種六自由度并聯機構，動平臺與連桿之間由虎克鉸相連，末端執行器在空間有平移及旋轉的六個自由度，十分契合骨外科手術的需求。國內外研究者投入了大量精力研究類似的并聯機構治療骨折或骨畸形，并獲得了一定實質性成果。Seide[2]教授基于Taylor骨外固定架，給Stewart平臺的執行連桿加裝了電驅動器，并用計算機對驅動器進行控制和檢測。該機構縮短了復位的周期，軌跡的規劃也更為靈活，但由于成本高且在拍攝骨折處透影像時，機構和電器設備會給成像效果造成很大程度上的干擾，并未廣泛的應用于臨床。Changsheng Li[3]等人將Stewart機構做為執行器應用在閉合性骨干骨折復位手術中，并利用利用串聯誤差傳遞模型和并聯機構誤差差分求解方法建立誤差模型，分析了骨折復位的精度。H.Lin[4]等人將并聯機器人機構引入骨牽引臺并實施了股骨骨干的骨折復位。上述文獻從不同角度介紹了并聯機構在骨折復位及畸形矯正方面的應用，但缺乏對機構本身運動學機構動力學性能的分析。本文基于一種新穎的骨折復位機構，從運動學及動力學角度分析機器人設計的合理性和利用該機器人進行骨折復位手術的可行性。

本文介紹了一種新穎的骨折復位機器人，介紹了利用該機器人進行骨折復位的方法，并針對機器人的主要執行機構進行了逆運動學分析。利用SimMechanic建立了機器人執行機構的動力學仿真模型，并通過拉格朗日法建立了詳細的動力學模型，推導出了該機構的雅各比矩陣，通過分析驅動關節與末端執行器的受力情況，驗證了該動力學模型的正確性。最后，利用建立的機構動力學模型設計了關節空間中基于計算力矩法的機器人控制器，通過非線性反饋補償機器人的非線性部分，使得機構利用線性控制器也可以獲得滿意的控制效果。

2 骨折復位機器人機構設計

在傳統骨折復位手術過程中，醫師通過牽引并緩慢矯正移動骨相對于近端參照骨的畸形來實現復位。骨折發生時，移動骨相對于參照骨有可能會形成復雜的畸形狀態，根據剛體在空間的姿態描述，可以通過6個自由度來確定移動骨相對于參照骨的全部畸形狀態，即3個旋轉自由度和3個平動自由度。為了滿足一定范圍內移動骨復位的需求，采用6-UPU并聯機構作為機器人的主要執行機構。基本的骨折復位手術生物學固定原則指出，外固定減少了植入物與組織的接觸面積，很大程度上減少了骨折復位并發癥的發生。通過考慮傳統骨折復位手術中醫生體力消耗大，復位精度不高等問題，參考了醫生的復位手法相關臨床經驗，利用Soildworks軟件設計骨折復位機器人如圖1所示，圖2為復位機器人的主要執行機構。該機器人由軌跡規劃單元，位姿反解單元，控制系統及執行機構組成，并聯機構的動平臺作為主要的執行器對移動骨進行牽伸和對位。利用骨折復位機器人進行骨折復位手術的流程可以分三個階段：

圖1 骨折復位機器人系統

圖2 骨折復位機器人執行器部分

首先利用骨外固定環分別固定患者遠端及近端，由遠端環充當并聯機構的移動平臺，近端與手術床保持剛性連接。再通過透視影像測算移動骨與參照骨的相對位姿，利用計算機軟件根據醫生的臨床經驗設計移動骨的復位軌跡，機器人根據設定好的復位軌跡進行骨折復位。在復位結束后，通過定制外固定連桿連接近/遠端的骨外固定環構成六自由度外固定器或采用內固定(如髓內釘或鋼板等)的方式固定。最后斷開固定環與手術床與機器人的連接，結束手術。

3 骨折復位機器人逆運動學分析

本文主要以骨折復位機器人的執行機構為研究對象。由于并聯機構的正運動學問題分析較為復雜且本文在對其進行控制時刻意回避了正運動學求解，因此本文僅對骨折復位機器人并聯執行機構的逆運動學進行分析及求解。圖3為該機構的結構簡圖。

圖3 骨折復位機器人機構簡圖

(1)

因此，第i根支鏈的長度ρi可以表示為

至此，給定動平臺相對于定平臺的位姿變化，就能通過(1)式計算出各執行器的長度，這實際上就是骨折復位機器人逆運動學的求解過程。

4 骨折復位機器人的動力學建模

4.1 拉格朗日法求機器人動力學方程

本文利用拉格朗日法建立骨折復位機器人執行機構的動力學數學模型。利用拉格朗日法建立機構的動力學模型，對于骨折復位機器人這一完全約束的并聯機構，支鏈與平臺構成封閉的結構，機構的每個部分存在許多中間坐標和約束條件，通常相互依賴。首先需要采用合適的廣義坐標求解系統的動能和勢能[5]。根據虛功原理的定義，在廣義坐標的基礎上定義拉格朗日函數為

L=K-P

(2)

其中K表示系統的動能，P表示系統的勢能，由于機構動能取決于廣義坐標及其它們時間的導數，勢能取決于廣義坐標本身，則系統的拉格朗日方程為：

其中Q表示廣義力，對于六自由度并聯機構，選取運動平臺質心位置變量和螺旋系下的姿態變量X作為廣義坐標。機械臂的動能可以由質量矩陣M(X)表示

因此拉格朗日函數可以重寫為

由于動能是速度向量的二次型，將上式帶入式(2)可得

定義

(3)

(4)

在拉格朗日方程中，式(3)包含了科氏力與離心力加速度引起的所有慣性力，為了簡化科氏力和離心力向量，利用張量積可以將V表示為如下形式

根據張量積的運算法則，定義C被表示為如下形式

采用這種表示形式，對于任何并聯機構的動力學方程都可以寫成一般的封閉形式：

為了應用機構參數及運動學參數并推導出骨折復位機構具體的動力學方程，將機構分解為運動平臺和6條支鏈分別建立其動力學方程，分別建立廣義坐標并獨立分析，最后通過轉換關系將兩部分合并，求出動力學矩陣的參數表達式。

首先建立各支鏈的運動學方程。假設每個支鏈由動臂和靜臂兩部分組成，動臂和靜臂的質心分別為ci1和ci2，質量分別為mi1，mi2.作用于肢體上的有外部力，驅動力和重力并假定沒有擾動扭力。定義支鏈與上平臺鉸點Bi的位置為廣義坐標。定義為xi，支鏈動力學方程的一般形式為

質量矩陣可以從每個肢體的動能中直接求出

重力項Gi可以根據勢能相對于廣義坐標xi的偏導求得，即

科氏力和離心力項則根據拉格朗日等式的拉格朗日函數的時間導數直接求得。

下面出了它們的具體形式

(5)

(6)

(7)

其中

骨折復位機器人的動平臺是一個幾何對稱質量均勻的剛體，選取運動平臺自身的運動參數X作為廣義坐標，建立運動平臺的動力學方程

其中質量矩陣可以從運動平臺動能推導而來

科氏力與離心力向量可以通過將質量矩陣相對于時間進行微分以及將動能相對于廣義坐標進行微分得到

重力項則是通過將動平臺勢能對廣義坐標求偏導得到：

最終，動平臺的動力學矩陣為

(8)

(9)

(10)

為了獲得機構整體的動力學矩陣，需要將中間廣義坐標系xi通過雅各比矩陣Ji轉化為全局的廣義坐標X，并按如下公式計算骨折復位機構的整體動力學矩陣

此外，作用于機構的廣義力Q可以認為是驅動力與擾動扭轉力Fd共同作用，即

Q=JTτ+Fd

至此，即求得了骨折復位機構的動力學方程的封閉形式，該形式有助于將先進的控制策略運用到控制系統中。

4.2 逆動力學仿真

通過給定末端執行器運動軌跡，觀察各支鏈及平臺所受的力是否符合預期，可以驗證動力學模型建立的正確性和完整性。首先需要為機器人的動平臺設定運動的軌跡。由于6-UPU并聯機構在空間有3各個平移及三個旋轉的自由度因此需要為6個自由度分別設計運動軌跡。處于機器人考慮，因此希望執行器可以柔和的啟動并緩慢的停止，即希望末端執行器在個自由度的起始和終止速度為零，并且在軌跡的中間階段運動盡可能保持勻速，多項式函數可以被用于模擬機構的運動軌跡，本文采用三次多項式表示個自由度的運動軌跡

q(t)=a0+a1t+a2t2+a3t3

軌跡的速度為

通過Simulink模擬了動平臺的運動軌跡，設置仿真時間為6秒并模擬了三條動平臺的運動軌跡如圖4所示，分別使動平臺沿Z軸平移30mm，沿X軸及Z軸分別平移30mm及繞Y軸旋轉30度，其中藍色線段表示機構的起始狀態，紅色線段表示機構的終止狀態。將該軌跡輸入骨折復位機器人的動力學模型，認為作用于機構的外部力只有重力g=[0，0，-9.8]T，其對應的平臺受力曲線如圖所示，利用力雅可比矩陣得到各支鏈的受力情況如圖5所示。

圖4 由左到右分別為動平臺沿z軸上升30mm;動平臺分別沿z軸和x軸移動30mm;動平臺繞X軸旋轉20度

圖5 各支鏈在三種運動軌跡下所受的力

三組軌跡下的動平臺分別沿著Z正方向的力向上移動30mm，沿Z軸及X軸同時各移動30mm和繞Y軸旋轉30度，并經歷先加速再減速的過程，由于設置Z軸沿豎直向下，動平臺沿各軸受力偏離0軸，以克服重力的影響。

5 骨折復位機器人控制系統設計

5.1 關節空間的控制系統架構

基于工作空間的控制系統如圖6a所示，期望軌跡與機構輸出的軌跡產生的偏差被送入控制器，控制器根據機構的動力學模型產生相應的關節驅動力輸入機構的各個關節，構成一個閉環系統。但是，實際的情況是，機構動平臺的運動參數X通常很難直接測量，相比于測量動平臺的運動參數，測量主動關節的位移變化更加經濟方便。一種可行的方法是通過在工作空間中設計控制器，通過傳感器測量機構關節空間中的關節變量，并通過前向運動學求解工作空間中動平臺的位姿X，但由于前向運動學需要求解一系列非線性方程，計算復雜度高，尤其是需要實時的提供控制信號時，該方法的實現較為困難。相比于前向動力學，并聯機構的反向運動學求解就要簡單的多。本文首先利用并聯機構的反向運動學模型根據機器人的期望軌跡計算出相應的期望關節位移量Ld，基于期望關節位移與實際關節位移q(t)在關節空間的誤差eq來設計控制器，控制器輸入是誤差eq，輸出是執行器關節的力τ。通過這種改變，明顯減少了動態系統的耦合性和非線性特性，使得在控制架構中為每個關節設置獨立控制器變得有效。

圖6 控制系統架構

由于上一章所推導的機器人動力學模型是在工作空間中建立的，再利用關節空間的控制架構時有必要將動力學模型轉換到關節空間中。執行器的力矩向量和對應的工作空間的廣義力的關系可以表示為

F=JTτ

其中J為機構的雅各比矩陣。由于機構是完全約束的機構，雅各比矩陣在非奇異狀態下可逆。工作空間中加速度與速度可以表示為

(11)

Mq=J-TM(X)J-1

Gq=J-TG(X)；τd=J-TFd

5.2 逆動力學模型的控制系統設計

計算力矩法是典型的考慮機器人動力學模型的控制方案，通過機器人的動力學模型為PD控制的輸出提供了非線性的補償，使得被控對象變成更容易控制的線性定長系統，再采用PD控制便可以的到滿意的控制效果。式(11)為機器人在關節空間中的動力學方程，計算力矩法的控制架構如圖7所示。

圖7 逆動力學模型控制

各支鏈獲得的控制量為

機器人的動力學方程可以表示為

若動態矩陣的值精確且M(q)可逆，則可以消去上式的非線性項，上式等價于一個解耦的線性定常系統

當期望軌跡qd(t)給定后，可對上述系統引入包含偏置的PD控制

式中e=qd(t)-q(t)，Kd，Kp均是對角矩陣且正定的。閉環系統的方程為

5.3 逆動力學模型的控制系統仿真

圖8 動平臺運動軌跡

圖9 基于力矩法的仿真控制系統

圖10 各桿的長度變化

圖11 各桿的受力情況

圖12 動平臺受力情況

圖13 動平臺實際的運動軌跡及與期望軌跡的誤差

左圖為期望變換曲線，右圖為實際變化曲線

6 結論

本文設計了一種新型的并聯骨折復位機器人用于替代外科醫生來進行長骨骨折復位手術。該機構通過動平臺帶動骨折遠端移動骨進行空間上六個自由度的運動以實現骨折復位的目的。通過MATLAB軟件中的SimMechanic工具箱結合Soildworks軟件設計骨折復位機器人，通過Simulink工具箱搭建了機構的軌跡生成模塊及反向運動學求解模塊，對骨折復位機器人機構進行了運動學仿真。基于運動學分析的結果，運用拉格朗日法建立了該骨折復位機器人的動力學模型，并探究了平臺位姿與關節驅動力及平臺受力的關系；利用計算力矩法在關節空間中設計了骨折復位機構逆動力學控制器。通過該仿真結果可以看出引入機構動力學模型的計算力矩法的控制器使得機器人具有較好的跟蹤性能及魯棒性。該研究為實際控制器設計及控制系統開發提供了理論支持。