磁驅(qū)動雙半球膠囊機器人爬坡運動分析*

陳自強，張永順

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

0 引言

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

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

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

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

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

1 機器人結(jié)構(gòu)

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

圖1 雙半球膠囊機器人結(jié)構(gòu)

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

2.1 建立坐標系

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

圖2 爬坡運動示意圖

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

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

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

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

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

圖3 各坐標系示意圖

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

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

2.2 非完整性約束方程

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

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

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

于是點o速度為：

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

2.3 磁力矩與阻力矩計算

如圖3（a）所示，磁矢量B的旋轉(zhuǎn)速度為ω，在慣性坐標系OXYZ中的表達式為：則經(jīng)過坐標轉(zhuǎn)換，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中表示為：

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

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

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

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

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

2.4 爬坡完整動力學方程

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

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

經(jīng)變換，oxyz中機器人角速度ω1表示為：

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

式中：I1為極轉(zhuǎn)動慣量；I2為赤道轉(zhuǎn)動慣量。

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

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

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

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

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

其中：

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

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

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

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

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

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

圖4 不同初值時σ的時間響應(yīng)曲線

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

3.1 前向爬坡動力學建模

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

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

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

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

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

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

耗散函數(shù)ψ：

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

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

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

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

3.2 純滾動條件分析

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

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

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

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

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

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

3.3 爬坡能力仿真

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

圖7 爬坡角度和磁場強度關(guān)系曲線

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

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

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

為更好地說明純滾動轉(zhuǎn)變?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，因此機器人可以按設(shè)定速度爬坡。同理，B0=9 mT對應(yīng)的坡角為21.5°，參照圖9，此時滿足純滾動所需的fsmin=0.54，同樣小于腸道可以提供的最大靜摩擦因數(shù)。當B0=11 mT時，爬坡角度為30°，此時純滾動所需的fsmin=0.725，大于腸道所能提供的最大靜摩擦因數(shù)，因此機器人將發(fā)生打滑甚至空轉(zhuǎn)。

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

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

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

4 結(jié)束語

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

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