基于MATLAB的膠囊內窺鏡驅動機器人的運動學分析及仿真

陳 曦，俞經虎，錢善華，曹 澍，胡雨農

(1.江南大學機械工程學院，江蘇 無錫 214122)(2.江蘇省食品先進制造裝備技術重點實驗室，江蘇 無錫 214122)

傳統的消化道類疾病的診療方法多為機械侵入式，例如腸鏡、胃鏡等，這種方式會給患者帶來不適感，使患者產生抵觸情緒逃避檢查，并且有可能造成新的損傷。近年來，隨著科技水平的不斷提高，特別是MEMS(微機電系統)技術的發展，使得無創手術與傳統手術相比有著更明顯的優勢。膠囊內窺鏡作為胃腸道疾病的新型檢查方式，逐漸取代了傳統的插入式胃腸鏡診療方式，這種無創檢查方式大大減輕了患者的不適感。

2010年，意大利Gastone Ciuti等[1]針對優化結構、精確和可靠性等目標完成了一個更緊湊的設計，使用6自由度串聯機械手來支持永磁體實時控制膠囊內窺鏡；2015年，意大利Gioia Lucarini等[2]提出用一種新型的環形電磁鐵作為控制和驅動系統，該系統結合了電磁鐵的優點及機器人操縱的靈活性。2010年，華中科技大學的高鳴源[3]提出了一種新型磁控導航系統，該系統共有5個聯動軸，可以實現膠囊在腸道內的快速運動、緩慢運動和局部定位等；2016年，上海安翰醫療技術有限公司[4]研發出NavicamTM膠囊內窺鏡機器人系統。

醫療器械的快速發展對空間技術的要求進一步提高。本文提出了一種結構為三移兩轉的串聯機器人來實現膠囊內窺鏡對人體的診療。診療過程中，串聯機器人執行末端帶動磁球移動，實現膠囊內窺鏡的轉動、前進與轉動前進復合運動，醫生通過控制驅動機器人，可以實時觀察膠囊內窺鏡傳回的圖像，對病變部位進行靶向定位施藥，甚至可以完成手術操作等等。驅動機器人可以在一定程度上讓膠囊內窺鏡的驅動方式更加高效、精準。

1 驅動機器人的總體設計

1.1 驅動機器人結構

在診療過程中，該機器人通過3個移動自由度和2個轉動自由度實現對磁球的控制，進而達到對人體內具有磁性的膠囊的定位和姿態控制，完成膠囊轉動、前進與轉動前進復合運動等一系列運動。膠囊驅動機器人的結構如圖1所示。

圖2為驅動機器人的結構示意圖，由圖可見驅動機器人固定在底座上，由移動副和轉動副共同組成。該驅動機器人為實現控制磁球運動的串聯驅動結構，其中移動運動分別由各方向的伺服電機帶動絲杠沿各自導軌滑動，實現沿X,Y,Z方向的線性傳動，轉動運動分別通過伺服電機控制實現繞Z軸方向的旋轉(θ4)和繞磁球圍架軸線的旋轉(θ5)。

圖1 驅動機器人三維模型

1.2 機械手位姿描述

描述驅動機器人的位置和姿態是對其進行運動學分析的基礎，在傳統的笛卡爾坐標系中D-H矩陣可以精準快速地建立串聯機構的坐標模型，因此本文采用改進后的D-H法[5]確定各部分的位姿和相對運動關系。圖3中移動副互相垂直，運動范圍分別是d1,d2,d3。轉動副的軸線互相垂直，旋轉范圍分別為θ4和θ5，點O5為末端運動點。簡化后各連桿坐標系如圖3所示，相應結構參數見表1。

圖3 驅動機器人D-H坐標系

表1 驅動機器人的D-H參數表

表中，θi為Xi-1到Xi繞Zi-1旋轉的角度；di為沿Zi-1方向Xi-1到Xi的距離；ai-1為沿Xi-1方向Zi-1到Zi的距離；αi-1為Zi-1到Zi繞Xi-1旋轉的角度。為了使計算結果清晰明了，建立基坐標系O-XYZ和機架坐標系O0-X0Y0Z0，以導出相關運動學方程。

2 驅動機器人的運動學分析

運動學分析在機器人的研究過程中有著舉足輕重的作用，可以確定機器人手臂位置、姿態以及各相鄰關節位置的對應關系[6]。運動學分析包括兩類：一是正運動學分析，給定各個連桿關節變量，求解相鄰連桿之間以及機械手末端位姿；二是逆運動學分析，給定機械手末端位姿，求解各個連桿關節的變量值。

2.1 正運動學分析

(1)

(2)

式中：n，o，a均為旋轉矢量；p為平移矢量。

將表1中的D-H參數代入式(1)，可得

(3)

將式(3)代入式(2)得：

(4)

式(4)中，c4表示cosθ4，s5表示sinθ5，以此類推。將各關節已知變量代入驅動機器人的運動學方程式(2)中，便可得到驅動機器人末端坐標系O5-X5Y5Z5相對于基坐標系O-XYZ唯一的準確位姿矩陣。

2.2 逆運動學分析

(5)

由式(5)可以求得：

(6)

(7)

(8)

令矩陣方程(8)兩端的元素(2，3)對應相等，可得axc4-ays4=0，從而可求出

(9)

由式(5)得：

(10)

從而可求出

(11)

3 驅動機器人的工作空間分析及仿真驗證

工作空間指的是末端執行機構上參考點在空間中可以達到的所有點的集合[7]，本文驅動機器人的參考點定為磁球的質心。分析驅動機器人的工作空間可以避免逆運動學計算過程中機器人末端運動軌跡不在最大工作空間內，從而確保逆解的存在。

3.1 建立運動學仿真模型

利用MATLAB軟件高效的數值計算、數據分析、數據可視化等特點，運用機器人工具箱(robotics toolbox)建立驅動機器人的仿真模型，并編寫相應的簡單算法，不但可以完成驅動機器人的運動學仿真，還能驗證其正、逆解運動學的正確性。因此，根據D-H參數的范圍，利用MATLAB工具箱中的link等函數輸入以下代碼并運行，便可建立驅動機器人的運動學模型，如圖4所示。

L0=Link(′theta′,pi,′a′,0,′alpha′,pi/2);

L1=Link(′ theta′,0,′a′,0,′alpha′,-pi/2);

L2=Link(′ theta′,pi/2,′a′,0,′alpha′,pi/2);

L3=Link(′ theta′,0,′a′,0,′alpha′,-pi/2);

L4=Link(′d′,-150,′a′,0,′alpha′,-pi/2);

L5=Link(′d′,0,′a′,70,′alpha′,0);

bot=SerialLink([L0,L1 L2 L3 L4 L5],′name′,′Magnetic Drive Robot′)

teach (Robot)

圖4 運動學仿真模型

圖4顯示的位置姿態信息與圖3相同，因此驗證了驅動機器人的運動學仿真模型的正確性。

3.2 工作空間的計算求解

本文機器人末端工作空間的求解采用蒙特卡洛法(Monte Carlo method)[8]，該方法是一種通過隨機概率來解決問題的數值方法，可以實現運動空間的可視化，能夠快速計算得到可行點，適用于多關節型機械手臂的工作空間求解。

驅動機器人的工作空間求解方法如下：

1) 根據驅動機器人的結構參數大概算出其工作空間范圍。

2)通過MATLAB軟件中的Rand函數，隨機產生大量0～1間的數值，將這些隨機數值定為隨機步長變量，從而得到各關節的偽隨機值θi：

θi=θimin+(θimax-θimin)×Rand(N,1)

(12)

式中：θimin為關節變量的最小值；θimax為關節變量的最大值；N為 0，1 之間隨機數值的個數；i為關節變量的數目。

3)將2)產生的隨機值代入驅動機器人的正運動學方程，計算出驅動機器人末端的位置。求出的末端坐標點數目越多，得到的驅動機器人實際工作空間就越精確。

4)利用MATLAB軟件畫出驅動機器人的工作空間，便能獲得驅動機器人末端的工作空間點云圖，也就是蒙特卡洛工作空間。

運用蒙特卡洛法在MATLAB軟件上進行簡單編程，計算得出驅動機器人的工作空間。設置隨機坐標點的數量為N=15 000，得到驅動機器人的工作空間如圖5所示。

圖5 驅動機器人的工作空間

從圖5可以得到驅動機器人的工作空間為：x∈[-548，50]mm,y∈[0，1 280] mm,z∈[-150，368] mm，驅動機器人的工作空間接近于一個長方體。MATLAB軟件模擬出的工作空間與各個關節的真實工作空間相符，空間尺寸與驅動機器人結構設計的尺寸一致，從三維圖及各方向投影圖上可以看出,坐標點分布均勻并且無明顯空洞，工作空間非常緊湊，因此驗證了蒙特卡洛法求解驅動機器人工作空間的正確性。

4 結束語

本文建立了膠囊內窺鏡驅動機器人的三維模型，對驅動機器人進行了正、逆運動學求解，獲得驅動機器人的運動學方程和執行末端位姿矩陣，通過MATLAB仿真驗證了驅動機器人運動學模型的正確性；蒙特卡洛法計算得到的驅動機器人工作空間與各個關節的真實工作空間相符，空間尺寸與驅動機器人結構設計的尺寸一致。本文的分析為后續的動力學、多物理場仿真分析、聯合控制仿真奠定了理論基礎，也為類似的驅動機器人動力學仿真分析提供了有益的借鑒。