陳自強(qiáng)，張永順

（大連理工大學(xué)精密與特種加工教育部重點(diǎn)實(shí)驗(yàn)室，遼寧大連 116024）

0 引言

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

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

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

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

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

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

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

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

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

2.1 建立坐標(biāo)系

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

圖2 爬坡運(yùn)動(dòng)示意圖

各坐標(biāo)系情況如圖3所示。

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

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

（3）ox0y0z0為平動(dòng)坐標(biāo)系，坐標(biāo)系原點(diǎn)o與球心重合。

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

圖3 各坐標(biāo)系示意圖

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

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

2.2 非完整性約束方程

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

在坐標(biāo)系ox0y0z0中，矢量oP表示為：

純滾動(dòng)時(shí)，速度瞬心點(diǎn)P的速度表達(dá)式為：

于是點(diǎn)o速度為：

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

2.3 磁力矩與阻力矩計(jì)算

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

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

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

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

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

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

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

2.4 爬坡完整動(dòng)力學(xué)方程

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

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

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

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

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

取水平面為零勢(shì)能面，則勢(shì)能V為：

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

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

則機(jī)器人主動(dòng)模態(tài)下的爬坡完整動(dòng)力學(xué)方程為：

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

其中：

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

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

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

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

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

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

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

3 爬坡運(yùn)動(dòng)平面動(dòng)力學(xué)建模與爬坡能力分析

3.1 前向爬坡動(dòng)力學(xué)建模

為了方便探究機(jī)器人的爬坡能力，可以建立不含姿態(tài)角的坡面運(yùn)動(dòng)動(dòng)力學(xué)方程。如圖5所示，建立坐標(biāo)系OX2Y2。

圖5 球形機(jī)器人爬坡運(yùn)動(dòng)簡(jiǎn)化模型

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

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

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

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

耗散函數(shù)ψ：

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

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

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

最終可得到平面動(dòng)力學(xué)方程：

3.2 純滾動(dòng)條件分析

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

圖6 膠囊機(jī)器人斜面純滾動(dòng)時(shí)的受力模型

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

式中：滾動(dòng)摩阻M0=σ0mgcosθ；因?yàn)閍Cy=0，故aCx=aC；點(diǎn)P為速度瞬心，故有aC=αR。

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

對(duì)式（27）進(jìn)行整理，可得：

3.3 爬坡能力仿真

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

圖7 爬坡角度和磁場(chǎng)強(qiáng)度關(guān)系曲線

可以看到，隨著磁場(chǎng)強(qiáng)度B0的增大，機(jī)器人的最大爬坡角度也變大，爬坡能力增強(qiáng)。

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

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

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

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

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

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

4 結(jié)束語

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

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