面向微創外科手術機械臂的主動力擺位控制策略

王偉， 杜志江， 閆志遠， 王偉東

(哈爾濱工業大學 機器人技術與系統國家重點實驗室，黑龍江 哈爾濱 150080)

機器人輔助微創手術是將機器人技術與腹腔鏡微創外科手術技術相融合產生的新型治療手段，在繼承傳統微創手術創傷小、感染率低、并發癥少及術后恢復快等優點的同時，從根本上解決了腹腔鏡微創手術帶來的操作弊端[1-2]。目前機器人輔助微創手術技術已成為機器人領域的研究熱點，許多研制成功的醫療手術機器人已取得良好的操作效果，某些微創外科手術系統在實際手術中得以應用[3-6]。

當利用微創外科手術機器人進行手術操作時，醫護人員需要在術前準備階段將機械臂及微創器械拖拽至微創切口位置并手動調整機械臂的主、被動關節，使手術器械能夠獲得良好的工作空間，盡量避免各機械臂之間的術中干涉。上述環節稱為微創外科手術機器人術前初始位姿的建立過程，即醫護人員直接對機械臂施加外力，根據操作者的意圖拖拽機器臂進行相應的位姿調整。問題的理想方式是在關節電機處于受控狀態下進行機械臂的主動擺位操作，根據機械臂的受力情況估計操作者的控制意圖，通過關節電機驅動機械臂輔助醫護人員完成微創外科手術機器人的位姿調整，即所謂的主動順應控制。根據控制對象的不同，此類控制問題可大致分為基于力平衡的力矩控制方式和基于虛擬環境模型的速度或位置控制方式兩類。

對于力平衡的控制方式，通常使伺服電機工作在力矩模式下，一般采用關節電機的反饋電流估測人機之間的接觸力，通過關節電機輸出轉矩主動補償機械臂連桿的重力矩及傳動環節中存在的摩擦力矩。文獻[7]提出一種基于機械臂動力學模型的人機交互控制算法，機械臂與環境的接觸力通過電機電流進行估計。文獻[8]采用自測量的方式獲得關節所受重力矩及摩擦力矩的補償模型并通過電機輸出相應的力矩進行主動平衡，機械臂的慣性力由外力克服，旨在為機械臂與操作者之間的力交互建立無摩擦和重力影響的操作環境。

基于虛擬環境模型的控制方式是指以人為設定的人機操作環境近似替代實際動力學模型的控制方式，此時伺服電機工作在位置或速度模式下，通過已建立的關節位置或速度與接觸力之間的關系模型實現機器人與環境的力交互。阻抗控制[9-11]與虛擬工具法[12-14]在該類控制方式中比較常見，兩種控制算法在原理上比較接近，都是通過抽象出的簡化人機交互模型建立輸入力、力矩與輸出位置或速度之間的對應關系，并根據實際要求和測試反饋調整各關節的阻尼系數、虛擬質量或接觸力增益等虛擬環境參數，以達到理想的人機力交互狀態。文獻[15]還提出了一種基于位置的零力控制算法，該算法根據伺服電機的控制模型將機械臂所受外力轉換成關節電機對應的位置輸出，控制電機轉動以映射外力對機械臂的作用效果。

微創外科手術機器人的主動擺位操作對于控制算法的穩定性要求較高，這是由于在進行術中器械位姿調整時，即使是很小的意外移動都有可能對病患造成嚴重傷害。文獻[7-8]提出的方法是伺服電機在力矩模式下的力平衡控制，其定位效果依賴于力學模型的補償精度。當機械臂構型或連桿間傳動關系變得復雜時，傳動摩擦和自身重力的理論模型與實際情況不可避免的會存在偏差，致使機械臂不斷地進行調整甚至發生震蕩，穩定性較差。文獻[15]提出的算法實際上是將與接觸力大小相當的電機輸出力矩轉換成位置模式下的關節期望位置，伺服電機雖然工作在位置模式，但本質上仍屬于力平衡控制，同樣會存在上述問題。此外，與機械人直接示教過程只關注末端執行器的位置不同，微創外科手術機器人的主動擺位過程為了避免機械臂之間的碰撞，確保微創器械的有效工作空間，往往更注重各連桿的姿態調整。若采用文獻[9-14]的控制方式則會為操作者帶來諸多不便，這是由于此種方式將人機交互的作用位置限定在機械臂的執行末端，不利于各連桿位姿的獨立控制。因此，合理的實現方式是使操作者與機械臂之間的接觸位置不受約束，可根據操作者的控制意圖實現機械臂位姿調整。

1 主動力擺位控制方法

為了滿足機器人輔助微創外科手術對主動擺位的實際需求，本文采用在機械臂各關節處安裝力矩傳感器的方式測量機械臂與操作者之間的作用力，利用力學模型與采樣數據相結合的方式處理重力的補償精度問題。為了保證主動力擺位的穩定性，在伺服電機的速度模式下提出了基于關節力矩的虛擬工具控制模型。本節將分為傳感器的安裝方式，重力矩模型的建立方法以及交互力的控制算法三個部分介紹醫療機器人主動力擺位的實現方法。

1.1 傳感器安裝方式

相對于依靠電機電流反饋進行接觸力估計的方式，采用傳感器進行接觸力測量的方式更加穩定可靠。傳感器的安裝方式不但決定了接觸力的測量精度、機器人與外界環境的力交互位置甚至還會影響控制算法的響應速度。本文在機械臂各轉動關節中集成一維力矩傳感器，由于此種方式的傳感器與關節驅動電機的安裝位置較在機械臂末端安裝六維力傳感器的方式更為接近，與外界環境之間接觸力的傳遞與檢測變得更加簡單直接，可避免由于機構參數不準確而產生關節分力的計算偏差，同時可以有效地減少由機構間隙產生的力反饋延遲與檢測誤差，從而提高控制系統的響應時間。此外，此種安裝方式使機械臂與外界環境的力交互位置不受限于特定的局部區域，可擴展至整條機械臂的任意位置，更有利于機械臂各連桿位姿的獨立調整。本文設計的機械臂關節驅動單元如圖1所示，一維力矩傳感器安裝在諧波減速機的輸出端與機械臂的固定基座之間，只檢測關節轉動方向受到的力矩值，采用增加額外軸承的方式消除摩擦力對關節力矩測量的干擾，并采用低通模擬濾波器對傳感器進行濾波。

圖1 力矩傳感器安裝示意圖Fig.1 Installation diagram of the torque sensor

1.2 重力補償模型

由于采用在機械臂關節處安裝力矩傳感器的測量方式，因此傳感器的測量值會包含由機械臂自重產生的重力矩分量，為了能夠提取機械臂與操作者之間的作用力，需要建立準確的重力矩模型以進行對應關節的重力補償。本文首先根據機械臂的運動學方程推導出每個主動關節的重力矩理論模型。為了降低理論模型與實際情況的偏差，將連桿重量、質心位置等機械臂屬性參數作為未知變量，通過實測采樣的方式記錄各關節在其工作空間內受到的重力矩分布情況并利用優化算法求得最佳的參數值，整個模型的建立過程并不需要確切的機械臂屬性參數和繁瑣的數值計算。

圖2所示為本文設計的微創外科手術機械臂，其中包含2個主動驅動關節以及1個被動調整關節。每個主動關節均集成力矩傳感器，被動調整關節中安裝絕對位置碼盤以記錄該關節位置。在進行手術操作時，復合平行四邊形機構(由圖2中第2、3、4連桿構成)能夠保證手術機械臂在微創器械介入處不會對病人造成額外的非手術傷害，是目前最常用的遠心不動點執行機構，其工作原理如圖3所示。連桿1、2、3、4之間通過轉動副連接，連桿5在實際結構中并不存在，機構依靠平行四邊形A、B、C之間的彼此約束實現繞遠心不動點RCM的平面轉動并利用軸線通過RCM點的轉動關節驅動該平面機構，從而實現微創器械繞切口位置的空間轉動而不發生相對移動，因此不會對病人造成額外傷害。

當進行主動力擺位時，復合平行四邊形機構連桿1、2、3的質心位置由驅動關節2的轉角決定，驅動關節1控制整個遠心不動點機構的轉動，而被動調整關節的位置變化會改變驅動關節處力矩傳感器的測量值。實際上，該結構的重力矩補償模型與4個機械臂連桿重量、質心位置及3個轉動關節的角度相關。為了便于描述機械臂各連桿間的位姿關系以及驅動關節處所受的重力矩，本文采用D-H法建立機械臂運動學模型(如圖4所示)，相應的坐標系齊次變換矩陣為

(1)

式中：si=sinθi,ci=cosθi,i=1,2,3。

圖2 微創外科手術機械臂Fig.2 Minimally invasive surgery manipulator

圖3 復合平行四邊形RCM機構Fig.3 The compound parallelogram mechanism for RCM

圖4 機械臂D-H坐標系Fig.4 D-H coordinates system of the manipulator

設P2m1、P3m2、P4m3、P5m4分別為機械臂連桿1、2、3、4質心在坐標系2、3、4、5中的位置向量：

(2)

則由式(1)、(2)可得到各質心位置向量在坐標系2和基系中表示為

(3)

(4)

若以G1、G2、G3、G4表示機械臂連桿1、2、3、4的重力，則各連桿質心處的重力向量在坐標系2中的表述如式為

(5)

式中：E代表3階單位矩陣，inv(·)表示矩陣的逆運算，i=1,2,3,4。由力矩的定義可得驅動關節1所受到的重力矩在坐標系2中的表述如式為

(6)

驅動關節1在其軸線方向受到的重力矩為向量τg的z軸分量，其方向與z2軸正方向相同，表達式如為

τ1=c1(G1ym1c2+G2zm2c2+G3zm3c2+

G4zm4c2+G1xm1s2+G3ym3s2+

G3a3c3s2+G2xm2c3s2+G4xm4c3s2-

G2ym2s3s2-G4ym4s3s2)

(7)

以同樣的方式亦可求得驅動關節2在其軸線方向上所受重力矩的表達式表示為

τ2=G3ym3s1-G4a3c1c2+G3a3s1c3-G3xm3c1c2+
G2xm2c3s1+G4xm4c3s1-G2ym2a3s1s3-
G4ym4s1s3+G3a3c1c2s3+G2ym2c1c2c3+
G4ym4c1c2c3+G2xm2c1c2s3+G4xm4c1c2s3

(8)

在主動關節各自的運動空間內隨機生成若干機械臂的手術擺位姿態，并在機械臂各連桿靜止時記錄力矩傳感器數值以及相應的關節位置。令式(7)、(8)左端分別等于驅動關節1和驅動關節2的力矩傳感器測量值，將相應的主、被動關節位置代入表達式右側，由此可得到與采樣點數目相同的方程式，聯立方程組并利用Matlab軟件提供的fminunc函數求解機械臂各待定參數的最優值。優化參數如表1所示，其中目標函數終值為式(7)、(8)右側計算值與左側采樣值之差的平方和。

表1 優化參數設置值Table 1 The optimization parameter settings

圖5所示為機械臂被動調整關節θ1分別為-19°、-24°以及-29°時，驅動關節的重力矩補償模型與傳感器實測數據的對比圖，其中平滑曲面由式(7)、(8)計算獲得，圓點分別表示驅動關節1和驅動關節2傳感器的實測數值。兩類數據之間存在一定偏差，這與機械結構及采樣點的密集程度等因素有關，但最主要的原因是來自傳感器自身的測量值波動。由圖5以及表2中的統計數據可知被動調整關節θ1的改變對于重力矩補償模型的精度影響很小，每個驅動關節處的重力矩均可以得到較好的補償，偏差在可接受范圍之內。關節1的重力矩模型要略好于關節2，這是由于關節2的傳動環節更加復雜，力在傳遞過程中會受到更多來自機構自身的干擾。在引入適當的檢測閾值加以限定后，可消除于傳感器測量值波動而引發的接觸力判斷錯誤問題，可增強力控制模型的輸入穩定性。

1.3 基于關節力矩的虛擬工具控制算法

如文獻[12]所述，以虛擬工具的動力學模型近似代替機械臂動力學模型的方式稱為力控制的虛擬工具模型。當以此種方式進行力控制時，并不需要考慮機械臂各連桿的實際質量，以虛擬質量和人為設定的環境阻尼建立動力學模型。在此基礎上，本文提出的控制算法以提取出的關節力矩作為輸入，關節的驅動電機以速度模式進行控制，相應的輸出速度由虛擬環境模型計算得到的加速度確定。對虛擬摩擦參數進行實時調整，以滿足主動擺位過程中的不同階段的需求，其控制原理如圖6所示。

圖5 重力矩補償模型與實測數據的比較Fig.5 A comparison between the actual data and the compensation model of the gravity torque

圖6 基于關節力矩的虛擬工具法控制框圖Fig.6 A diagram block of the virtual-tool controller based on joint torque

θ1/(°)重力矩補償誤差誤差均值/(N·m)標準差/(N·m)誤差最大值/(N·m)-19-24-29τ1～-τ10.00160.09720.2828τ2～-τ2-0.01650.18340.3793τ1～-τ1-0.00140.11270.2350τ2～-τ20.01480.2063-0.4072τ1～-τ10.00350.12060.3651τ2～-τ20.01270.22740.4211

基于關節力矩的主動力擺位控制分為機械臂與外界的作用力檢測和虛擬工具模型算法的實現兩個部分。當操作者與機械臂進行力交互時，重力補償模型根據各關節的實時位置θ計算相應的重力矩補償值，驅動關節在轉動方向上受到的合力矩τ通過力矩傳感器進行檢測，將經過模擬濾波后的合力矩值τ～與由重力矩模型得到的補償值相減并與傳感器檢測閾值h進行比較，以大于檢測閾值的關節力矩提取值τ′作為虛擬工具算法的控制輸入。外界環境施加在關節處的接觸力與機械臂連桿間的運動關系通過虛擬質量M體現，即以M作為關節所受力矩與關節加速度的轉換參數。為了能夠提供柔順且自然的控制方式，需要引入相應的環境反饋參數，以關節轉動速度v和虛擬摩擦參數C作為外界環境對機械臂的阻尼反饋。主動擺位控制算法通過當前關節的輸入力矩以及前一周期的關節速度計算本周期的關節加速度，從而實現速度模式下的力控制，其表達式為

(9)

式中：虛擬質量M,虛擬摩擦參數C1、C2以及檢測閾值h統稱為主動力擺位控制模型的模型參數。每個驅動關節的控制均采用相同的模塊配置，但每個關節對應的模型參數和檢測閾值會有所不同。模型參數值的選取對主動力擺位控制模型的表現性能有顯著影響。由于虛擬質量和檢測閾值會決定關節加速度值，因此其數值的大小會影響關節運動的流暢性，較大的虛擬質量會使關節運動變得敏感而易產生震蕩，而較小的虛擬質量會使機械臂的拖動變得困難。另一方面，虛擬摩擦參數會決定運動的平滑性以及作用力消失后的關節運動時間及位移，較小的虛擬摩擦參數會使運動更加流暢，但同時也會增加機械臂擺位后的停止時間和移動位移。為了滿足微創外科手術機械臂的主動擺位要求，本文提出的主動力擺位控制算法會根據機械臂的受力情況主動修改虛擬工具模型的摩擦參數，當機械臂檢測到與外界環境之間的作用力時，以較小的摩擦參數C1進行虛擬環境的阻尼反饋，使機械臂的拖拽過程更為輕便流暢；當機械臂與外界環境之間的接觸力消失后，采用較大的摩擦參數C2控制機械臂運動，由于環境阻尼的增大會迫使轉動關節快速地完成定位過程，以避免產生較大的位置誤差。

2 算法驗證與分析

為了驗證本文提出的主動力擺位控制算法的有效性，以圖2所示自行研制的微創外科手術機械臂作為驗證平臺。驗證實驗與實際的微創外科手術機器人的主動力擺位過程相似，為了使實驗結果簡潔清晰，便于分析主動力擺位控制模型的實際表現，操作者分別對兩個主動關節進行一次大幅度的拖拽動作(約Δθ≈ 75°，不包含姿態的微調過程)，并記錄這一過程中關節所受到的接觸力矩，轉動速度以及關節位置等信息(如圖7所示)，相應的控制模型參數如表3所示。

圖7 關節力矩與關節運動的對應關系Fig.7 The corresponding relationship between the joint torque and joint motion

關節MC1C2h/(N·m)采樣周期/ms移動均值濾波窗口值12.50.090.350.40.28023.10.120.400.50.280

由圖7可以看出，基于關節力矩的虛擬工具模型僅需要較小的作用力即可實現機械臂的主動力擺位過程，每個驅動關節的轉動速度與關節處所受力矩的變化趨勢基本一致，關節加速度會隨著接觸力增量的變化趨勢進行調整。當操作者試圖拖動機械臂時，隨著接觸力的增大驅動關節會以較大的加速度進行位置跟隨(圖7所示A階段)，能夠快速消除由于期望位置與當前位置偏差而產生的遲滯感；若關節所受力矩變化趨于平穩，主動關節會以相對穩定的速度運動以保持當前連桿運動速度與作用力矩之間的對應關系(圖7所示B階段)；當外界與機械臂之間的作用力消失時，關節會以較大的加速度進行減速(圖7所示C階段)，在保證運動流暢自然的前提下能夠快速停止關節運動以減少位置誤差。值得注意的是，當關節處的作用力矩發生波動時(如圖7所示驅動關節2的B階段)，與之對應的關節速度也能保證穩定，不會產生機械臂的震蕩以及受力不均的操作感受，由此可見主動力擺位控制模型具有較好的穩定性。

3 結論

1)面向微創外科手術機器人的主動擺位引導操作設計了相應的主動關節驅動單元，可實現關節接觸力矩的獨立檢測與運動控制。

2)提出了一種機械臂重力補償模型，可有效識別主動關節驅動單元所檢測力矩中的重力矩分量，在機械臂屬性參數未知的前提下亦可實現較精確的重力矩補償。

3)提出了一種基于關節力矩的虛擬工具控制算法，能夠流暢地完成微創外科手術機器人的主動擺位引導任務，可在機械臂的任意位置對RCM機構連桿進行位姿調整，操作方式更便捷靈活。

由于虛擬工具模型的表現效果會受到模型參數的影響，在后續研究中會考慮根據機械臂的運動狀態實現模型參數的連續調整，以期待進一步改善微創外科手術機器人擺位過程的操作感受。

[1] MURPHY D A, MILLER J S, LANGFORD D A, et al. Endoscopic robotic mitral valve surgery[J]. The journal of thoracic and cardiovascular surgery, 2006, 132(4): 1119-1120.

[2] MEIRELES O, HORGAN S. Applications of surgical robotics in general surgery[M]. New York: Springer, 2011: 791-812.

[3] MARESCAUX J, RUBINO F. The ZEUS robotic system: experimental and clinical applications.[J]. Surgical clinics of North America, 2003, 83(6): 1305-1315.

[4] MIYAMOTO S, SUGIURA M, WATANABE S, et al. Development of minimally invasive surgery systems[J]. Hitachi review, 2003, 52(4): 189-195.

[5] BLAKE H, JACOB R, FRIEDMAN D W, et al. Raven-II: an open platform for surgical robotics research[J]. IEEE transactions on biomedical engineering, 2013, 60(4): 954-959.

[6] MA J, BERKELMAN P. A compact, simple, and robust teleported robotic surgery system[M]. New York: Springer, 2011: 139-158.

[7] KOBAYASHI S, MUIS A, OHNISHI K. Sensorless cooperation between human and mobile manipulator[C]//IEEE International Conference on Industrial Technology. Lyon, France, 2005: 811-816.

[8] 游有鵬, 張宇, 李成剛. 面向直接示教的機器人零力控制[J]. 機械工程學報, 2014, 50(3): 10-17.

YOU Youpeng, ZHANG Yu, LI Chenggang. Force-free control for the direct teaching of robots[J]. Chinese journal of mechanical engineering, 2014, 50(3): 10-14.

[9] DUCHAINE V, GOSSELIN C M. General model of human-robot cooperation using a novel velocity based variable impedance control[C]//Euro Haptics Conference. Tsukuba, Japan, 2007: 446-451.

[11] LECOURS A, MAYER-ST-ONGE B, GOSSELIN C. Variable admittance control of a four-degree-of-freedom intelligent assist device[C]//International Conference on Robotics and Automation. St. Paul, USA, 2012: 3903-3908.

[12] KOSUGE K, FUJISAWA Y, FUKUDA T. Control of robot directly maneuvered by operator[C]//International Conference on Intelligent Robots and Systems. Tokyo, Japan, 1993: 49-54.

[13] INFANTE M L, KYRKI V. Usability of force-based controllers in physical human-robot interaction[C]//International Conference on Human-Robot Interaction. Lausanne, Switzerland, 2012: 355-362.

[14] FERRETTI G, MAGNANI G A, ROCCO P. Assigning virtual tool dynamics to an industrial robot through an admittance controller[C]//International Conference on Advanced Robotics. Munich, Germany, 2009: 1-6.

[15] GOTO S, USUI T, KYURA N, et al. Forcefree control with independent compensation for industrial articulated robot arms[J]. Control engineering practice, 2007, 15(6): 627-638.

本文引用格式：

王偉， 杜志江， 閆志遠， 等. 面向微創外科手術機械臂的主動力擺位控制策略[J]. 哈爾濱工程大學學報， 2017， 38(12): 1915-1921.

WANG Wei, DU Zhijiang, YAN Zhiyuan, et al. Active force positioning control strategy to design a minimally invasive surgical manipulator[J]. Journal of Harbin Engineering University， 2017， 38(12): 1915-1921.