陳自強，張永順

（大連理工大學精密與特種加工教育部重點實驗室，遼寧大連 116024）

0 引言

膠囊內窺鏡相較傳統(tǒng)胃鏡具有診斷無痛、安全的優(yōu)點，被廣泛應用在胃腸道疾病檢查中。雖然目前以Given ImagingLtd的M2A和金山科技初代的OMOM為代表的國內外膠囊機器人已技術成熟且投入商用，但它們大多數(shù)依靠胃腸道蠕動行進，位姿隨意性強，漏檢率高[1]。因此開發(fā)可靈活行走且姿態(tài)可控的主動式膠囊內窺鏡便尤為重要。

目前主動式膠囊機器人驅動方式主要分為執(zhí)行機構驅動和永磁體驅動，前者結構復雜，不利于膠囊微型化的同時也存在電源模塊工作時長受限的問題，后者則由于受制梯度磁場而較難實現(xiàn)膠囊位置與姿態(tài)的分離控制。本課題組利用已有的三軸亥姆霍茲線圈裝置產生的空間均勻萬向旋轉磁場與膠囊內嵌永磁體耦合生成的磁力矩驅動整體實現(xiàn)自旋與前進后退[2-4]。隨動效應[5]使機器人軸線可以時刻跟隨磁矢量法線方向，借此實現(xiàn)膠囊被動模態(tài)的懸停調姿和主動模態(tài)的滾動行走，控制更加靈活。

常規(guī)球形機器人依靠擺錘或配重塊等改變重心來驅使整球滾動，其平面滾動速度和爬坡能力受制于偏心裝置的質量占比，爬坡效果不理想[6]。將耦合磁力矩作為驅動源，本文中的機器人具備更好的爬坡效果。

為使采集的圖像穩(wěn)定可用，膠囊機器人爬坡時需有良好的抗干擾能力。描述機器人位姿的狀態(tài)變量互不獨立，是典型的非完整性約束系統(tǒng)，動力學建模工作更加復雜。

本文采用拉格朗日乘子法建立描述機器人爬坡的完整動力學模型，分析與姿態(tài)角相關的夾角變化，說明了機器人的抗干擾能力。建立簡化爬坡動力學模型分析爬坡角度。理論分析和試驗結果為改善和提高機器人爬坡性能提供了理論支持。

1 機器人結構

如圖1所示，內置軸承實現(xiàn)主動半球殼體和被動半球殼體的懸浮連接，保證兩半球間的相對轉動。NeFeB永磁體與主動半球殼固連，當外部磁場旋轉時，受到磁力矩驅動的永磁體帶動主動半球一同旋轉，與攝像頭定位器件等固連的被動半球因無動力而不旋轉。為實現(xiàn)圖像采集等功能，零件還包括發(fā)射天線、LED電路、攝像頭、電池、電路板等。當機器人軸線因隨動效應而隨磁場軸線變?yōu)樗椒较驎r，與腸道接觸的主動半球將借助摩擦力帶動整球一同滾動[7]。

圖1 雙半球膠囊機器人結構

2 爬坡完整動力學建模與動態(tài)特性分析

2.1 建立坐標系

機器人涉及姿態(tài)變化的爬坡過程如圖2所示。

圖2 爬坡運動示意圖

各坐標系情況如圖3所示。

（1）如圖3（a）所示，OXYZ為慣性坐標系，OX軸與水平面重合。旋轉磁矢量B繞原點O在XOZ面內旋轉，磁矢量法線nB方向與OY軸平行。

（2）OX1Y1Z1為固定坐標系，OX1軸與水平面夾角為θ。（x,y）為機器人球心o在OX1Y1Z1內的坐標。

（3）ox0y0z0為平動坐標系，坐標系原點o與球心重合。

（4）oxyz為描述機器人位姿的賴柴坐標系。如圖3（b），先繞oz0軸旋轉α角得到中間坐標系oxy1z0，再繞ox軸旋轉β角得到賴柴坐標系。機器人的軸線n最終與oy軸重合。

圖3 各坐標系示意圖

（5）ox1yz1為機器人本體坐標系。由坐標系oxyz繞y軸旋轉γ角得到。

為描述機器人爬坡時包含姿態(tài)變化的完整動力學方程，有廣義坐標：坐標x、y，姿態(tài)角α、β及γ。設機器人半徑為R，質量為m，機器人和坡面的接觸點為P，內嵌永磁體磁矩為m0，旋轉磁矢量強度幅值為B0。

2.2 非完整性約束方程

假設機器人爬坡全過程為純滾動[8]。角速度經一系列坐標變換后，在坐標系ox0y0z0中可表示為：

在坐標系ox0y0z0中，矢量oP表示為：

純滾動時，速度瞬心點P的速度表達式為：

于是點o速度為：

將式（1）代入式（4），得非完整約束方程：

2.3 磁力矩與阻力矩計算

如圖3（a）所示，磁矢量B的旋轉速度為ω，在慣性坐標系OXYZ中的表達式為：則經過坐標轉換，B在賴柴系oxyz中為：

其中：

A=sin α cos β cos θ+sin θ sin β

B=sin β cos θ-sin α cos β sin θ

C=sin α sin β cos θ-sin θ cos β

D=sin α sin β sin θ+cos β cos θ

和機器人固連的永磁體磁矩m在oxyz中表示為：

式中：ε為轉差角，即機器人旋轉時磁矩矢量滯后磁矢量的角度。

參考磁力矩公式T1=m1×B1，得到T1在賴柴系oxyz中的分量表達式：

其 中：E=B0cos α cos θ sin ωt+B0cos α cos ωt sin θ,F=B0 cos ωt(cos β cos θ+sin α sin β sin θ)。

爬坡時，阻力矩包括機器人殼體與腸道相對滑動受到的粘性摩擦力矩，以及靜摩擦力所產生的摩擦阻力矩。設粘性阻尼系數(shù)為k，靜摩擦力為mgsinθ，則阻力矩Mf在賴柴系oxyz中可表示為：

式中：k0、i、j分別為z0、x、y軸的單位向量。

2.4 爬坡完整動力學方程

非完整約束使得廣義坐標之間失去獨立性，拉格朗日乘子法可以很好解決此問題，其表達式為：

式中：L為拉格朗日函數(shù)；qα為廣義坐標；Qα為廣義力；λr為拉格朗日乘子；fr為非完整約束方程。

經變換，oxyz中機器人角速度ω1表示為：

設T為系統(tǒng)動能，其包括平動動能和轉動動能，聯(lián)立式（5）（12）有：

式中：I1為極轉動慣量；I2為赤道轉動慣量。

取水平面為零勢能面，則勢能V為：

式中：s為球心位移在OX1方向的投影，即x。

拉格朗日函數(shù)L=T-V，其表達式為：

則機器人主動模態(tài)下的爬坡完整動力學方程為：

為了方便分析機器人的姿態(tài)角響應特性，取狀態(tài)變量x=（α，β，）T。當機器人爬坡滾動不丟步時，可將自轉角γ替換為ωt-ε，于是可得到˙=f(x,t)·x形式的方程組：

其中：

式（17）即為可以描述姿態(tài)角變化的微分方程組。

2.5 爬坡動態(tài)特性分析

將旋轉磁矢量所屬平面的法線nB（磁場軸線）調整為水平時，借助隨動效應，機器人的軸線n可以近乎時刻與nB重合，進而以主動模態(tài)完成爬坡運動。然而胃腸道空間并不寬裕，機器人在行進時難免受到胃腸壁刮蹭等環(huán)境干擾。將n與nB間的夾角σ作為反映機器人軸線偏離目標方位的目標函數(shù)，其與姿態(tài)角α、β之間的數(shù)學關系為：

給定初值不同的σ來模擬實際作業(yè)時受到的環(huán)境干擾，結合式（17）線性化后的微分方程組分析機器人軸線姿態(tài)回歸的能力。具體仿真參數(shù)如表1。

表1 仿真參數(shù)取值

從圖4中可以看到，在90°極限范圍內，無論初值σ選取多大，經一段時間后都能收斂于0°，且σ的初值越小，其回歸的也越快。這表明受到干擾時，機器人具備回歸穩(wěn)態(tài)的能力，這為機器人的多場景穩(wěn)定行走提供了理論支持。

圖4 不同初值時σ的時間響應曲線

3 爬坡運動平面動力學建模與爬坡能力分析

3.1 前向爬坡動力學建模

為了方便探究機器人的爬坡能力，可以建立不含姿態(tài)角的坡面運動動力學方程。如圖5所示，建立坐標系OX2Y2。

圖5 球形機器人爬坡運動簡化模型

取爬坡位移s為廣義坐標，滾動摩阻為M0，機器人繞轉軸的轉動慣量為I1，滾過角度為φ。故有關系：

考慮到軸承間粘性阻尼不可忽略，采用基于能量耗散的拉格朗日方程，形式如下：

系統(tǒng)動能Ta為：

取水平面為零勢能面，則任意位置的勢能Va為：

耗散函數(shù)ψ：

式中：k1為軸承間的粘性摩擦因數(shù)。

機器人受到的主動力包括：耦合磁力矩T2、重力mg以及滾動摩阻M0。根據(jù)虛功原理，廣義力可表示為：

式中：σ0為滾動摩阻系數(shù)；引入符號函數(shù)sgn，旨在滾動摩阻方向時刻與角速度方向相反。

最終可得到平面動力學方程：

3.2 純滾動條件分析

機器人在攀爬大角度時往往容易打破純滾動狀態(tài)而發(fā)生滑動甚至原地空轉，這影響了機器人跟隨磁場的能力，造成和理論設定速度的偏差，產生速度損失，因此分析純滾動時的最大爬坡角度更具意義。機器人純滾動時，受力如圖6所示。

圖6 膠囊機器人斜面純滾動時的受力模型

假設機器人向右滾動，角加速度為α，質心C的加速度為aC，靜摩擦力為f，靜摩擦因數(shù)為fs。根據(jù)動量矩定理可得[9]：

式中：滾動摩阻M0=σ0mgcosθ；因為aCy=0，故aCx=aC；點P為速度瞬心，故有aC=αR。

純滾動時滿足：f≤fsFN，也即f≤fsmgcosθ。結合上述條件，可得純滾動條件為：

對式（27）進行整理，可得：

3.3 爬坡能力仿真

固定參數(shù)ω=12π rad/s，I1=1.78×10-7kg·m2，取磁場強度B0的仿真上限為12 mT[10-11]，利用Matlab對式（25）進行仿真，得到圖7所示關系曲線。

圖7 爬坡角度和磁場強度關系曲線

可以看到，隨著磁場強度B0的增大，機器人的最大爬坡角度也變大，爬坡能力增強。

保持磁場強度B0=8 mT，結合式（28），仿真得到各個爬坡角度下純滾動時所需的最小靜摩擦因數(shù)曲線。由圖8可知，爬坡角度越大，純滾動時所需的靜摩擦因數(shù)越大。當腸道環(huán)境達不到對應爬坡角度所需的最小靜摩擦因數(shù)時，機器人便發(fā)生打滑甚至空轉。

圖8 最小靜摩擦因數(shù)與角度關系曲線

為更好地說明純滾動轉變?yōu)榛瑒拥臈l件，保持B0分別為8 mT、9 mT、11 mT，仿真得到圖9。人體腸道的靜摩擦因數(shù)fs為0.6左右[12]，故以0.6作為對照值。當B0=8 mT時，由圖7可知，此時機器人的爬坡角度為17°，在圖9中找到17°時保證純滾動的最小靜摩擦因數(shù)fsmin=0.47，小于0.6，因此機器人可以按設定速度爬坡。同理，B0=9 mT對應的坡角為21.5°，參照圖9，此時滿足純滾動所需的fsmin=0.54，同樣小于腸道可以提供的最大靜摩擦因數(shù)。當B0=11 mT時，爬坡角度為30°，此時純滾動所需的fsmin=0.725，大于腸道所能提供的最大靜摩擦因數(shù)，因此機器人將發(fā)生打滑甚至空轉。

圖9 不同B0對應的爬坡角度與摩擦因數(shù)的關系曲線

結合純滾動要素，繪制不同靜摩擦因數(shù)下機器人純滾動的最大爬坡角度與磁場強度關系曲線。從圖10可知，在幾種典型腸道靜摩擦因數(shù)中，靜摩擦因數(shù)越大，機器人的爬坡極限值越大，爬坡表現(xiàn)越好。

圖10 典型靜摩擦因數(shù)下最大爬坡角度曲線

4 結束語

本文的創(chuàng)新點在于根據(jù)拉格朗日乘子法建立了磁驅動膠囊機器人主動模態(tài)爬坡時的完整動力學方程，通過仿真機器人軸線回歸預設方位情況說明了機器人的抗干擾能力；根據(jù)基于能量耗散的拉格朗日動力學原理建立二維爬坡動力學模型，結合純滾動條件仿真磁場強度與腸道靜摩擦系數(shù)對于爬坡能力的影響規(guī)律。

仿真結果表明：機器人爬坡時具備良好的姿態(tài)回歸能力，其抗干擾特性為保證機器人爬坡的順利行走及穩(wěn)定圖像采集功能奠定了基礎；在一定范圍內，磁場強度越大，機器人的爬坡表現(xiàn)越好，大的腸道靜摩擦因數(shù)將增大機器人的最大不打滑爬坡角度，這一結論對于改善機器人在大坡度時的打滑問題提供了解決思路，對于未來越障方面的研究也具有參考價值。