一種六自由度手術機器人的運動學計算和仿真

王慶升， 周惠興,2， 王衍學

(1.北京建筑大學 機電與車輛工程學院， 北京 100044; 2.北京建筑大學 建筑機器人與智能設備智能實驗室， 北京 100044)

微創外科手術機器人是集眾多前沿科技于一體的綜合體，在微創外科領域是革命性的手術工具。手術機器人技術是當前醫療機器人領域的研究熱點，受到國內外機構的高度關注[1]。與傳統手術方式相比，微創手術技術大幅提高了外科手術的操作精度，微創手術創傷面積小，進一步減少了術中出血量和術后不良反應的發生，加快了術后的恢復速度。現代醫療機器人技術發展迅速，已在世界各地醫院中得到廣泛應用，微創外科機器人技術還在不斷向前發展，具有廣闊的市場前景。

將手術機器人應用到微創外科手術中，必須嚴格保證機器人在身體介入點處不會對人體造成額外傷害，因此需要機器人提供一個空間位置不變的固定點，即遠程運動中心(Remote Center-of-Motion, RCM)，簡稱為遠心點，其概念由Taylor教授提出[2]。

手術機器人約束RCM的方式主要有2種，一是通過特殊設計的機構約束實現，二是通過算法控制多關節聯動實現[3]。機構約束方式因為控制簡單、機構安全等特點，在手術機器人中應用較為廣泛。目前手術機器人中的RCM機構主要有平行四邊形機構、球面關節機構、圓弧導軌機構等[4]。美國直覺外科公司的da Vinci手術機器人系統[5]，采用的是雙平行四邊形的機構約束方式，哈爾濱工業大學“華鵲”、上海交通大學“神刀華佗”機器人均采用的是雙平行四邊形機構[6]。平行四邊形約束由于冗余自由度過多[7]、體積龐大，導致機械臂之間易發生干涉[8]。

多關節聯動方式的機器人本身不具有RCM機構，手術過程中的RCM通過算法控制的關節聯動實現。多關節聯動方式可以降低對機構的依賴，通用性更高，但對算法穩定性和可靠性的要求較高，機器人整體的控制難度較高。德國宇航中心的DLR MIRO機器人采用的是多關節聯動方式[9]，該機器人外部的7個關節均為主動驅動關節，機器人系統通過自由度的冗余實現手術切口處的定點運動。但是該機器人自由度冗余，需要算法解決冗余自由度，且容易發生奇異。

1 研究分析

手術機器人模型設計的關鍵在于RCM機構設計，一個合理的RCM機構可以以最少的機構數量滿足自由度要求。通過研究分析國內外多種手術機器人的機構設計，本文提出了一種采用多關節聯動方案的串聯六自由度的手術機器人模型，6個關節均為主動驅動關節，模型具有結構緊湊、體積小巧、無額外自由度冗余、不易發生自身干涉等特點。此模型為多關節聯動方案提供了一個思路和方法。

2 建模與計算

2.1 D-H法建模

本文的模型創建采用標準D-H法(Standard Denavit-Hartenber)，用齊次變換矩陣來描述連桿間的位姿變換，從而建立運動學方程(圖1)。

圖1 手術機器人模型Fig.1 Surgical robot model

模型包含5個旋轉關節和1個移動關節，確定機器人模型的D-H參數見表1。為了方便后期擴展關節數量，關節參數編號從4開始計。

表1 D-H參數

θi為關節旋轉變量，di為平移關節的移動變量，ai為連桿間坐標系變換的距離偏置常量，αi為連桿間坐標系變換的偏置角常量。忽略末端執行器夾手開合動作的額外自由度，按照D-H法建立的六自由度機器人各部件坐標系如圖2所示。

圖2 機器人D-H模型Fig.2 Robot D-H model

2.2 正運動學

采用D-H法得到連桿間的齊次變換矩陣，將變換矩陣連續右乘得到模型的正解方程。正運動學是運動學逆解的基礎，將逆解代入正運動學可以驗證逆解的正確性。

由標準D-H變換法，相鄰2個連桿i對i-1之間的齊次變換矩陣i-lTi為：

(1)

式中：Rotx、Rotz分別表示繞x、z軸的旋轉，Transx、Transz表示沿x、z軸的平移，變量和常量由表1確定。

根據式(1)可以分別計算出每2個連桿之間的變換，連續右乘得到從基座到末端的位姿變換，模型末端位姿矩陣Tend為：

(2)

式中：nx、ny、nz分別表示末端坐標系的x軸在基坐標系x、y、z軸的方向余弦，ox、oy、oz分別表示末端坐標系的y軸在基坐標系x、y、z軸的方向余弦，ax、ay、az分別表示末端坐標系的z軸在基坐標系x、y、z軸的方向余弦，px、py、pz表示末端坐標系原點在基坐標系的位置坐標。

2.3 逆運動學

機器人的逆運動學是求解機器人的關鍵問題，是運動規劃和運動控制的基礎。運動學逆解是給定末端位置和姿態，求出各關節變量。

求運動學逆解的常見方法有解析法和數值法，解析法又可以分為幾何解法與代數解法，本文基于矩陣逆變換的方法，即Paul等提出的反變換方法求解關節變量，采用解析法中的分離變量法進行數值計算[10]。

機器人前3軸的3個坐標系原點重合，形式上與zyz歐拉角相同。后3軸分別為移動、旋轉、旋轉關節。該模型3個相鄰軸相交于一點，滿足Pieper準則，存在封閉解。

機器人位姿變換矩陣如式(2)所示，對式(2)矩陣逆乘反變換，聯立等式各項參數可得：

-s4s6+c4c5c6=-axs8+c8(nxc9-oxs9)

(3)

c4s6+s4c5c6=-ays8+c8(nyc9-oys9)

(4)

s5c6=-azs8+c8(nzc9-ozs9)

(5)

-c4s5=axc8+s8(nxc9-oxs9)

(6)

-s4s5=ayc8+s8(nyc9-oys9)

(7)

c5=azc8+s8(nzc9-ozs9)

(8)

s4c6+c4c5s6=-oxc9-nxs9

(9)

-c4c6+s4c5s6=-oyc9-nys9

(10)

s5s6=-ozc9-nzs9

(11)

-d7c4s5=px-a9nx-a8(nxc9-oxs9)

(12)

-d7s4s5=py-a9ny-a8(nyc9-oys9)

(13)

d7c5=pz-a9nz-a8(nzc9-ozs9)

(14)

式中：si表示sinθi，ci表示cosθi。本文機器人關節角中θi的取值為(-π,π]。

2.3.1 求解θ9

將式(6)～(8)分別帶入式(12)～(14)得：

(15)

令：

(16)

式中：Px、Py、Pz是公式化簡的臨時變量。

等式組(15)消去變量d7，得：

(17)

當Pz-a8(nzc9-ozs9)=0時，等式組(17)仍成立，但對于求解θ8、θ9沒有意義。

(18)

將式(18)代入式(17)并化簡，可分離變量θ8：

(19)

將式(19)交叉相乘化簡可得式(20)：

0=(Pz-Rza8)(PxRyaz-PxRzay+PyRzax+…
-PyRxaz+PzRxay-PzRyax)

(20)

令：

(21)

將式(18)代入式(20)可得只含θ9的方程：

(22)

由式(14)可知，當且僅當d7c5=0時，表達式Pz-a8(nzc9-ozs9)的值為零，否則此項不為零。由式(15)可知，此項為零對求解θ9無意義。

由式(22)可得：

(23)

式中：(1±1)π/2為0或π，θ9可求得2個值。若Pm=Pn=0，將式(2)中的各個參數nx、ox、ax、px、ny、oy、ay、py、nz、oz、az、pz的表達式代入可得a8+d7s8=0，此時位置奇異，θ9無唯一解。本文所述唯一解是指在反三角函數指定值域內唯一，而不是只有一個解，實際上在(-π,π]內可能有2組解。

2.3.2 求解θ8

令：

(24)

由式(19)、(24)可得：

(25)

此處θ8將會求得4個值。式(24)中Pxz=Rxz=Pyz=Ryz=0，將式(2)中的各個參數nx、ox、ax、px、ny、oy、ay、py、nz、oz、az、pz的表達式代入式(24)可得：

(26)

即d7=0或c4c6=s4s6c5=0，此時位置奇異，θ8無唯一解。因參數設定d7>0，只考慮c4c6=s4s6c5=0，此狀態可具體分為4種情況：c4=s6=0、c4=c5=0、c6=s4=0或c6=c5=0。此4種狀態虛擬六軸的4、5、6軸將會形成萬向節鎖。

2.3.3 求解d7

由式(15)可得：

(27)

w可取x、y或z。此處d7將會求得4個值。由式(6)～(8)可知，式(27)中分母分別為-c4s5,-s4s5,c5,顯然不會全部為零，故d7有唯一解。

2.3.4 求解θ5

由式(7)可得：

θ5=±arccos[azc8+s8(nzc9-ozs9)]

(28)

此處θ5將會求得8個值，由向量內積可知|azc8+s8(nzc9-ozs9)|≤1。

2.3.5 求解θ4θ6

觀察式(5)～(7)、(11)可知，θ5的取值將影響θ4、θ6的求解。

1)當s5=0時，即θ5為0或π，此時θ4、θ6沒有唯一解，只有θ4+θ6的公共解。此狀態下，由式(9)～(10)可得：

(29)

式中：s46表示sin(θ4+θ6)，c46表示cos(θ4+θ6)。

由式(29)可得：

θ4+θ6=atan2(-oxc9-nxs9,oyc9+nys9)

(30)

若-oxc9-nxs9=oyc9+nys9=0，代入ox、oy、nx、ny得：

(31)

顯然等式組不成立，故θ4+θ6有唯一解。

2)當s5≠0時，由等式(6) (7)、(5) (11)可得：

(32)

(33)

此處θ4、θ6將分別求得4個值。若ayc8+s8Ry=axc8+s8Rx=-azs8+c8Rz=-ozc9-nzs9=0，代入nx、ox、ax、ny、oy、ay、nz、oz、az表達式得：

(34)

顯然，等式組不成立，故假設不成立，θ4、θ6有唯一解。

2.4 逆解驗證

將上述算法腳本編程，設定機器人的關節連桿偏置參數值a8=a9=50 mm，d7取值為(0,1 000]，設定θ4、θ5、θ6、θ8、θ9取值均為(-π,π]。使用rand()隨機函數生成位姿T=(-3.05,-2.72,1.37,233.25,-90.86,-19.82)，求得8組逆解，見表2。

表2 隨機位姿的8組逆解

3 基于ROS的仿真

ROS(Robot Operating System)機器人操作系統，采用分布式處理，提供硬件抽象、設備控制等標準的操作系統服務，支持對Linux硬實時擴展[11]，集成RViz、Gazebo、rqt等強大的工具包，兼容多種編程語言，可實現跨平臺跨語言的源代碼共享。

基于ROS對機器人進行運動仿真，首先在三維建模軟件中創建機器人的模型。3個坐標系重合的結構只在理論上存在，實際上物理結構件都會存在尺寸，如此復雜的結構模型很難直接做成足夠緊湊的機械結構，所以本文仿真時額外設計3個自由度，目的是在RCM處提供4、5、6軸所提供的3個姿態。由于額外3個自由度的存在，原本重合的4、5、6坐標系可以分開，形成更方便求解的通用模型。

模型建立完畢后使用插件生成指定的URDF文件，使用運動控制工具MoveIt!配置參數生成功能包，使用Python接口編寫逆運動學控制程序，利用OMPL/SBL進行運動規劃，并使用可視化工具RViz進行可視化驗證，展示了RCM運動仿真和圓弧插補運動仿真。

3.1 URDF模型文件

統一機器人描述格式(Unified Robot Description Format，URDF)是ROS用來描述機器人模型的一種xml語言，文件包含模型的形狀、大小、慣量、質心、顏色等基本屬性，還包含機器人的動力學參數。采用URDF格式來描述機器人模型，對于復雜的模型可使用sw2urdf插件將Solidworks模型轉換為URDF文件(圖3)，在模型中設置完整的坐標系和轉動移動軸，該插件可以輔助生成并導出對應的URDF文件和ROS功能包。

3.2 運動仿真

Moveit!是ROS中用于運動規劃和運動控制的強大的工具集，包括用于運動規劃的運動學分析、運動學算法、動力學、運動規劃、路徑規劃、軌跡規劃等。MoveIt!是實現運動仿真功能的一個主程序，它集成于ROS通訊框架，并兼容ROS的工具，如可視化RViz、物理仿真Gazebo、機器人描述文件URDF等。本文使用Moveit Setup Assistant來輔助創建整套的功能包，MoveIt!核心組件move_group提供3種用戶自定義算法接口，分別為C++接口、Python接口、GUI接口，為了方便測試和調試算法，選擇Python接口，使用其moveit_commander功能包API、KDLKinematicPlugin運動規劃庫。使用MoveIt!進行關節空間點到點運動仿真，繞固定點運動的運動軌跡如圖4所示。

圖3 使用sw2urdf Setup創建URDF文件Fig.3 Using sw2urdf Setup to create a URDF file

圖4 機器人運動仿真Fig.4 Robot motion simulation

點對點往復運動時，使用rqt_plot獲取關節角度變化曲線，如圖5所示。關節位置/joint_states/position[0-8]縱坐標分別表示1～9關節弧度位置。

為了更好地檢驗算法和測試機器人的性能，研究編寫了一個簡單的圓弧插補程序，可使機器人在繞定點運動的同時，執行器末端畫圓，軌跡仿真如圖6所示。

圖5 各關節角度變化曲線Fig.5 Angle change curve of each joint

圖6 圓弧插補運動仿真Fig.6 Circular interpolation motion simulation

4 結論

本文提出一種串聯六自由度手術機器人模型，推導了機器人模型在基座固定的情況下，繞工作空間內任意位置RCM運動的算法。運用D-H法分別進行了運動學建模和運動學分析，基于矩陣求解的思路求得了逆解，并通過編程驗證了逆解的正確性。通過對方程變量的分析，列舉了發生奇異的位置和末端位姿參數的有效取值范圍。提出了一種基于ROS的機器人運動控制及仿真的快速實現方法，充分利用了三維軟件生成的模型，結合MoveIt!和RViz工具，較為便捷地模擬了自定義算法的機器人控制，形象地展示了機器人仿真運動的過程。此運動學計算和仿真可為此機器人后續研發提供計算研究基礎。