具重力補償功能三自由度力覺主手動力學研究

曹燕燕，潘 博，付宜利，李 坤

(哈爾濱工業(yè)大學機器人研究所，哈爾濱150001)

腹腔微創(chuàng)手術機器人系統(tǒng)是通過主從操作來完成腹腔手術的系統(tǒng)，與傳統(tǒng)開放式手術相比，創(chuàng)傷小、恢復周期短、可以最大限度的減少病人的痛苦.主手是腹腔微創(chuàng)手術系統(tǒng)的重要組成部分，為了給醫(yī)生提供逼真的力覺臨場感，研制高性能的主手設備已經受到越來越多研究者和制造商們的重視.目前，Da Vinci手術機器人系統(tǒng)是醫(yī)療上應用最廣、商品化最成功的機器人系統(tǒng)之一，但是Da Vinci手術機器人系統(tǒng)的主手沒有力覺反饋.在國內，天津大學研制出了一種7－DOF力反饋型主操作手[1]，具有三自由度的力反饋，已經成功的應用到“Micro Hand A”上，初步完成了力感覺的實現(xiàn).但是該主操作手力反饋的實現(xiàn)僅僅是靜力反饋，考慮動力學模型的力反饋控制方法還有待進一步的驗證和實際應用.北京航空航天大學對自主研發(fā)的力覺交互設備提出了重力補償?shù)膶嶒炈惴╗2]，并通過實驗驗證了算法的有效性.

本文自主研發(fā)的力覺主手具有三自由度的力反饋，并且能夠實現(xiàn)重力補償.力覺主手的各個關節(jié)安裝有用以實現(xiàn)力反饋的伺服電機.如何有效地對電機進行控制，以實現(xiàn)臨場力感覺，是力覺主手的一個極其重要的設計問題.多種不同形式的控制方法被嘗試過:從簡單的靜力反饋，到傳統(tǒng)的 PID控制，再到阻抗控制，以及更新的現(xiàn)代控制方法和智能控制方法.然而，要想獲得真實準確的反饋力，實現(xiàn)主手的力感覺，建立有效的主操作手的動力學模型是必不可少的.

建立動力學模型的方法一般可分為三類[3]:1)牛頓－歐拉法.2)達朗貝爾原理(包括拉格朗日方程、凱恩方程等).3)極值原理法.其中較常用的為牛頓－歐拉法和拉格朗日方法.牛頓－歐拉法處理方法比較直觀，概念比較清楚，但是考慮了不做工的內力和內力矩，計算量較大;拉格朗日方程的方法是一種基于能量的動力學方程求解方法，具有較強的理論性和邏輯性，直觀簡潔，是目前機器人領域應用最廣的一種方法，因此本文采用拉格朗日方程的方法來建立主手動力學模型.

本文針對自主設計的三自由度力覺主手進行了簡要介紹，采用拉格朗日方程的方法建立了主手的動力學模型，建立虛擬樣機，并在ADAMS中進行了動力學和重力補償?shù)姆抡娣治?

1 機構描述

本文所設計的主手為三自由度串并混聯(lián)結構，能夠實現(xiàn)三維力反饋以及重力補償，其計算機三維造型圖如圖1所示.使用“電機－傳動絲－轉盤－彈簧”這一絲傳動回路實現(xiàn)主手的力反饋，通過設計設計了一個具有兩個自由度平行四邊形機構9和配重塊6實現(xiàn)主手的初始位置的自重平衡.

圖1 主手三維模型圖

2 動力學建模

拉格朗日方法是一種基于能量的動力學方法，方程中不出現(xiàn)約束力，只需分析已知的主動力.拉格朗日方程需要適當?shù)剡x取一組描述系統(tǒng)的廣義坐標，方便而完全地描述一個系統(tǒng)相對于參考坐標系的配置.定義拉格朗日函數(shù)為:其中:L為拉格朗日函數(shù)，K為系統(tǒng)各部分動能之和，P為系統(tǒng)各部分勢能之和.拉格朗日方程的定義為:

其中:qi為系統(tǒng)的廣義坐標，為廣義坐標qi對時間的一階導數(shù)，τi為作用于系統(tǒng)的相對于廣義坐標qi的廣義力或廣義力矩.

2.1 機構劃分

在使用拉格朗日方程方法進行動力學建模之前，需要將主手進行劃分，并計算出各個部分的動能和勢能.本文將三自由度力覺主手劃分為A、B、C、D、E五個部分，并在各部分的轉動關節(jié)上建立各自的體坐標系，得到主手的動力學模型結構簡圖如圖2所示.

圖2 主手動力學模型結構簡圖

2.2 各部分的動能計算

剛體做平面運動時的動能等于剛體隨質心平動的動能與繞之心轉動的動能之和，即

其中:Jc為剛體繞質心的轉動慣量，ω為剛體繞質心的角速度，m為剛體的質量，vc為質心平動的速度.因此，要求各部分剛體的動能，就要求得以上各個參數(shù).這里需要注意的是，我們需要求得的是各部分剛體相對于基座標系的動能，所以以上各量需要轉化到基坐標系中才能進行求解.

使用Pro/ENGINEER三維建模軟件對圖2中模型劃分的五部分剛體的質量屬性進行測量，得到各部分的質量 ma、mb、mc、md、me，質心在體坐標系中的位置矢量的齊次坐標表示和各部分相對于質心坐標系的慣性張量 Ia、Ib、Ic、Id、Ie如下:

通過運用改進的D－H法[4]，計算得到各個部分相對于基座標系o0x0y0z0的變換矩陣如下:

式中，si=sin(θi)，ci=cos(θi)，i=1，2，3 下文同此，不再贅述.則各部分質心在基座標系中的位置矢量為

各部分質心在基座標系中的速度矢量為

各部分相對于質心坐標系的慣性張量在基座標系下的表示為

下面通過求雅克比矩陣的方法來計算各部分剛體的角速度.

對于轉動關節(jié)，機器人雅克比矩陣的的計算公式為:

其中:Jωi為角速度的傳遞函數(shù)，且有 Jωi=Zi，其中Zi為關節(jié)i的單位運動矢量，根據(jù)各部分剛體的旋轉變換矩陣即可求得如下:

由以上計算即可求得各部分剛體的動能為:

主手各部分動能之和為:

2.3 各部分的勢能計算

以圖2中基座標系o0x0y0z0的x0o0y0所在平面為勢能零點，求得主手各部分的勢能分別為:則主手各部分勢能之和為:

其中:

2.4 主手的拉格朗日方程

由式(1)、(2)、(6)和(7)即可求得主手的拉格朗日方程為:

其中:式中Mij為有效慣量和耦合慣量項，Cij為向心力項和哥氏力項系數(shù)，Ni為重力項.各項的具體數(shù)值如下:

以上式中，s23=sin(θ2+ θ3)，c23=cos(θ2+ θ3)，s32=sin(θ3－θ2)，c32=cos(θ3－θ2).

3 ADAMS仿真

在ADAMS中建立主手的虛擬樣機，對主手的動力學方程和重力補償性能進行仿真分析.

3.1 動力學仿真與驗證

在虛擬樣機的三個關節(jié)上添加電機驅動，根據(jù)主手的各關節(jié)的運動范圍設定三個電機的驅動函數(shù)分別為:

運行主手動力學仿真，測得各個關節(jié)電機的驅動力矩，并將仿真結果與式(8)的實際計算結果進行對比，發(fā)現(xiàn)兩種結果基本吻合(如圖3)，有一些差異的原因在于使用ADAMS軟件時，仿真參數(shù)的設定會影響仿真的效果和精度[5].另外，實際計算所用的模型參數(shù)是通過Pro/ENGINEER軟件測得的，兩種軟件所獲得參數(shù)會有一定的差異[6].以上結果表明，主手的動力學模型是正確的.

圖3 各關節(jié)電機輸出力矩仿真結果與計算結果對比

3.2 重力補償功能分析與驗證

本文所設計的三自由度主手的重力補償功能是通過兩個方面來實現(xiàn)的.一方面，通過在主手機械結構上施加配重來實現(xiàn)主手自身在初始位置的自重平衡;另一方面，通過控制電機施加反向驅動力矩來實現(xiàn)主手在操作過程中的重力補償.下面分別進行配重功能和電機重力項補償?shù)尿炞C.

3.2.1 配重功能驗證

在ADAMS中分別對施加配重的虛擬樣機和不加配重的虛擬樣機的進行靜平衡分析，得到各個關節(jié)電機的靜平衡測量結果如圖4所示.

圖4 加配重與不加配重時各關節(jié)電機靜平衡測量結果對比

可以看到，關節(jié)電機1和3的測量結果基本相同，但對關節(jié)電機2，加配重塊之后電機的輸出力矩僅是不加配重塊時的1/6左右.由此可知施加配重是十分有必要的，能夠大大的減小電機的輸出力矩，使主手的結構緊湊，并且改善主手的性能.

3.2.2 電機重力項補償驗證

式(8)中各關節(jié)電機輸出力矩中包含的重力項Ni是主手工作過程中電機為平衡主手各部分重力變化而輸出的力矩.為了驗證計算結果的正確性，將虛擬樣機的仿真結果與計算結果進行對比，如圖5所示.可以看出，仿真結果與計算結果完全吻合.

圖5 各關節(jié)電機輸出力矩重力項仿真與計算結果對比

4 結語

對具有重力補償功能的三自由度力覺主手進行了動力學建模和拉格朗日方程的求解，得到了主手各個關節(jié)電機的動力學輸出力矩表達式.基于虛擬樣機技術，在ADAMS中進行了主手的動力學仿真，驗證了計算結果的正確性.同時對主手的重力補償功能進行了仿真分析，驗證了重力補償策略的正確性.本文的研究為主手電機的選取和力反饋控制提供了理論基礎.

[1] 張林安.力反饋性主操作手設計及其控制策略研究[D].天津:天津大學，2010.

[2] 李斯橋，張玉茹，曹永剛，等.力覺交互設備的重力補償實驗研究[J].機械設計與研究，2007，23(1):96－97.

[3] MACLEAN K E.Designing with Haptic Feedback[C]//Symposium on Haptic Feedback in the Proceedings of IEEE Robotics and Automation(ICRA’2000).San Francisco.CA.，2000:22－28.

[4] [美]CRAIG J J.機器人學導論[M].3版.贠 超，譯.北京:機械工業(yè)出版社，2006.

[5] 于殿勇，錢玉進.基于ADAMS動力學仿真參數(shù)設置的研究[J].計算機仿真，2006，23(9):103 －107.

[6] 劉海清，劉玉斌，張 赫，等.仿生六足機器人基于足力分布的位姿調整策略[J].哈爾濱商業(yè)大學學報:自然科學版，2012，28(5):541 －545.