一種六自由度手術(shù)機(jī)器人的運(yùn)動學(xué)計算和仿真

王慶升， 周惠興,2， 王衍學(xué)

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

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

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

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

多關(guān)節(jié)聯(lián)動方式的機(jī)器人本身不具有RCM機(jī)構(gòu)，手術(shù)過程中的RCM通過算法控制的關(guān)節(jié)聯(lián)動實現(xiàn)。多關(guān)節(jié)聯(lián)動方式可以降低對機(jī)構(gòu)的依賴，通用性更高，但對算法穩(wěn)定性和可靠性的要求較高，機(jī)器人整體的控制難度較高。德國宇航中心的DLR MIRO機(jī)器人采用的是多關(guān)節(jié)聯(lián)動方式[9]，該機(jī)器人外部的7個關(guān)節(jié)均為主動驅(qū)動關(guān)節(jié)，機(jī)器人系統(tǒng)通過自由度的冗余實現(xiàn)手術(shù)切口處的定點運(yùn)動。但是該機(jī)器人自由度冗余，需要算法解決冗余自由度，且容易發(fā)生奇異。

1 研究分析

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

2 建模與計算

2.1 D-H法建模

本文的模型創(chuàng)建采用標(biāo)準(zhǔn)D-H法(Standard Denavit-Hartenber)，用齊次變換矩陣來描述連桿間的位姿變換，從而建立運(yùn)動學(xué)方程(圖1)。

圖1 手術(shù)機(jī)器人模型Fig.1 Surgical robot model

模型包含5個旋轉(zhuǎn)關(guān)節(jié)和1個移動關(guān)節(jié)，確定機(jī)器人模型的D-H參數(shù)見表1。為了方便后期擴(kuò)展關(guān)節(jié)數(shù)量，關(guān)節(jié)參數(shù)編號從4開始計。

表1 D-H參數(shù)

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

圖2 機(jī)器人D-H模型Fig.2 Robot D-H model

2.2 正運(yùn)動學(xué)

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

由標(biāo)準(zhǔn)D-H變換法，相鄰2個連桿i對i-1之間的齊次變換矩陣i-lTi為：

(1)

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

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

(2)

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

2.3 逆運(yùn)動學(xué)

機(jī)器人的逆運(yùn)動學(xué)是求解機(jī)器人的關(guān)鍵問題，是運(yùn)動規(guī)劃和運(yùn)動控制的基礎(chǔ)。運(yùn)動學(xué)逆解是給定末端位置和姿態(tài)，求出各關(guān)節(jié)變量。

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

機(jī)器人前3軸的3個坐標(biāo)系原點重合，形式上與zyz歐拉角相同。后3軸分別為移動、旋轉(zhuǎn)、旋轉(zhuǎn)關(guān)節(jié)。該模型3個相鄰軸相交于一點，滿足Pieper準(zhǔn)則，存在封閉解。

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

-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。本文機(jī)器人關(guān)節(jié)角中θi的取值為(-π,π]。

2.3.1 求解θ9

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

(15)

令：

(16)

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

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

(17)

當(dāng)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)可知，當(dāng)且僅當(dāng)d7c5=0時，表達(dá)式Pz-a8(nzc9-ozs9)的值為零，否則此項不為零。由式(15)可知，此項為零對求解θ9無意義。

由式(22)可得：

(23)

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

2.3.2 求解θ8

令：

(24)

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

(25)

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

(26)

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

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個值，由向量內(nèi)積可知|azc8+s8(nzc9-ozs9)|≤1。

2.3.5 求解θ4θ6

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

1)當(dāng)s5=0時，即θ5為0或π，此時θ4、θ6沒有唯一解，只有θ4+θ6的公共解。此狀態(tài)下，由式(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)當(dāng)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表達(dá)式得：

(34)

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

2.4 逆解驗證

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

表2 隨機(jī)位姿的8組逆解

3 基于ROS的仿真

ROS(Robot Operating System)機(jī)器人操作系統(tǒng)，采用分布式處理，提供硬件抽象、設(shè)備控制等標(biāo)準(zhǔn)的操作系統(tǒng)服務(wù)，支持對Linux硬實時擴(kuò)展[11]，集成RViz、Gazebo、rqt等強(qiáng)大的工具包，兼容多種編程語言，可實現(xiàn)跨平臺跨語言的源代碼共享。

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

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

3.1 URDF模型文件

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

3.2 運(yùn)動仿真

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

圖3 使用sw2urdf Setup創(chuàng)建URDF文件Fig.3 Using sw2urdf Setup to create a URDF file

圖4 機(jī)器人運(yùn)動仿真Fig.4 Robot motion simulation

點對點往復(fù)運(yùn)動時，使用rqt_plot獲取關(guān)節(jié)角度變化曲線，如圖5所示。關(guān)節(jié)位置/joint_states/position[0-8]縱坐標(biāo)分別表示1～9關(guān)節(jié)弧度位置。

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

圖5 各關(guān)節(jié)角度變化曲線Fig.5 Angle change curve of each joint

圖6 圓弧插補(bǔ)運(yùn)動仿真Fig.6 Circular interpolation motion simulation

4 結(jié)論

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