滾動(dòng)球式醫(yī)療服務(wù)型機(jī)器人動(dòng)力學(xué)建模與仿真

劉 怡，鐘文敏

（贛南醫(yī)學(xué)院第一附屬醫(yī)院，江西 贛州 341000）

1 引言

由于新技術(shù)的推動(dòng)和應(yīng)用市場(chǎng)的拉動(dòng)，近年來(lái)機(jī)器人在醫(yī)療方面的應(yīng)用越來(lái)越廣泛了，很多醫(yī)院利用機(jī)器人分擔(dān)一些例如消毒、輸液、運(yùn)輸?shù)确?wù)型的工作。滾動(dòng)球式機(jī)器人作為機(jī)器人的一種，因其運(yùn)動(dòng)靈活、通過(guò)性好等優(yōu)點(diǎn)，在醫(yī)療領(lǐng)域有巨大的發(fā)展?jié)摿1]。

滾動(dòng)球式機(jī)器人是在獨(dú)輪自平衡機(jī)器人的研究基礎(chǔ)上發(fā)展起來(lái)的，國(guó)內(nèi)對(duì)滾動(dòng)球式器人的研究起步較晚，目前大部分還處于理論研究階段，國(guó)外在研究滾動(dòng)球式機(jī)器人方面比較早的是文獻(xiàn)[3-8]于2005年前后研制出一種新型的滾動(dòng)球式機(jī)器人“Ballbot”，但是最成功的應(yīng)該算是2010年，由蘇黎世瑞士聯(lián)邦理工學(xué)院與蘇黎世應(yīng)用科學(xué)學(xué)院以及蘇黎世藝術(shù)學(xué)院合作研制的“Rezero”機(jī)器人[10-12]，“Rezero”機(jī)器人不僅能在球體上實(shí)現(xiàn)動(dòng)平衡，而且具有非常強(qiáng)的靈活性和運(yùn)動(dòng)性，但是“Rezero”機(jī)器人在結(jié)構(gòu)上缺乏失電保護(hù)裝置，在機(jī)器人啟停及瞬時(shí)失電的狀態(tài)下整個(gè)機(jī)器人很容易傾倒，而且“Rezero”機(jī)器人動(dòng)力學(xué)建模復(fù)雜且準(zhǔn)確性不足，因此為了提高滾動(dòng)球式機(jī)器人動(dòng)力學(xué)模型的準(zhǔn)確性及滾動(dòng)球式機(jī)器人的安全性和穩(wěn)定性，設(shè)計(jì)一種自帶失電保護(hù)裝置且建模簡(jiǎn)單、方便的滾動(dòng)球式機(jī)器人就變得非常有必要了。

利用Solidworks設(shè)計(jì)出了一種新型的滾動(dòng)球式醫(yī)療服務(wù)型機(jī)器人，該機(jī)器人通過(guò)增加失電保護(hù)裝置有效的提高了機(jī)器人在啟停及制動(dòng)狀態(tài)下系統(tǒng)的穩(wěn)定性。同時(shí)，由于Lagrange-Routh方程同時(shí)具備了第一類(lèi)和第二類(lèi)Lagrange方程的功能[2]，通過(guò)引入了Lagrange乘子，使Lagrange-Routh方程建立的動(dòng)力學(xué)系統(tǒng)模型理想假設(shè)更少、模型誤差更小、求解過(guò)程更加簡(jiǎn)單，且能很好的解決滾動(dòng)球式機(jī)器人系統(tǒng)的非線(xiàn)性、非完整性等問(wèn)題，因此文中采用Lagrange-Routh方程對(duì)滾動(dòng)球式機(jī)器人系統(tǒng)進(jìn)行動(dòng)力學(xué)建模。

2 滾動(dòng)球式移動(dòng)機(jī)器人結(jié)構(gòu)設(shè)計(jì)

滾動(dòng)球式機(jī)器人主要由機(jī)架、驅(qū)動(dòng)機(jī)構(gòu)、失電保護(hù)機(jī)構(gòu)、球體四部分組成，如圖1所示。其中機(jī)架上安裝有各類(lèi)型的傳感器、三維慣性測(cè)量單元、鋰電池、控制器、伺服驅(qū)動(dòng)器等；驅(qū)動(dòng)電機(jī)采用直流無(wú)刷伺服電機(jī)，三個(gè)全向輪對(duì)稱(chēng)安裝在球體上，全向輪軸線(xiàn)與垂直方向成30°，全向輪中心、球體中心以及它們的接觸點(diǎn)共線(xiàn)；失電保護(hù)裝置主要由精密彈簧合頁(yè)、圓形電磁吸盤(pán)、支撐板等組成，當(dāng)通電運(yùn)行狀態(tài)下電磁吸盤(pán)緊吸支撐板，當(dāng)系統(tǒng)瞬時(shí)斷電或啟停瞬時(shí)狀態(tài)時(shí)電磁吸盤(pán)失電，四個(gè)支撐在彈簧的作用下同時(shí)打向地面防止整個(gè)機(jī)器人傾倒。由于驅(qū)動(dòng)機(jī)構(gòu)、失電保護(hù)機(jī)構(gòu)、鋰電池、控制器等都是固接在機(jī)架上，為了簡(jiǎn)化模型的計(jì)算，因此在建立模型前把固接在機(jī)架上的物體和機(jī)架當(dāng)做一個(gè)整體稱(chēng)為主架體，由此，整個(gè)滾動(dòng)球式機(jī)器人就可以看成是由主架體和球體兩部分組成。

圖1 滾動(dòng)球式機(jī)器人本體結(jié)構(gòu)Fig.1 Body Structure of Rolling Ball Robot

3 滾動(dòng)球式機(jī)器人動(dòng)力學(xué)建模

3.1 模型的理想化假設(shè)與參數(shù)設(shè)置

根據(jù)前面所設(shè)計(jì)的單新型球輪移動(dòng)機(jī)器人本體機(jī)構(gòu)建立相應(yīng)的坐標(biāo)關(guān)系，如圖2所示。圖2中坐標(biāo)系OXYZ是全局固定慣性坐標(biāo)系、坐標(biāo)系O1X1Y1Z1是球體的動(dòng)坐標(biāo)系、坐標(biāo)系O2X2Y2Z2主架體的動(dòng)態(tài)坐標(biāo)系，其中O是全局固定慣性坐標(biāo)系的原點(diǎn)、O1是球體的球心也是球體動(dòng)坐標(biāo)系的原點(diǎn)、O2是主架體的質(zhì)心點(diǎn)也是主架體動(dòng)坐標(biāo)系的原點(diǎn)。滾動(dòng)球式機(jī)器人在全局固定慣性坐標(biāo)系OXYZ中的姿態(tài)位置用固定在機(jī)架上的坐標(biāo)系O2X2Y2Z2的位置來(lái)表示。為了表示坐標(biāo)系OXYZ和坐標(biāo)系O2X2Y2Z2之間的位置關(guān)系，引入歐拉角θ、φ、ψ及坐標(biāo)系，如圖3所示。同時(shí)假設(shè)，主架體的質(zhì)心點(diǎn)O2（主架體動(dòng)坐標(biāo)系的原點(diǎn)）在全局固定慣性坐標(biāo)系中的坐標(biāo)為（x，y，z），球體的質(zhì)心O1（球心）在全局慣性坐標(biāo)系中的坐標(biāo)為（x0，y0，z0）。

圖2 滾動(dòng)球式機(jī)器人坐標(biāo)關(guān)系Fig.2 Coordinate Relation of Rolling Ball Robot

為了降低計(jì)算的復(fù)雜程度對(duì)模型做幾點(diǎn)假設(shè)：（1）假設(shè)球體質(zhì)心（球體圓心）與主架體的質(zhì)心共線(xiàn)，且開(kāi)始時(shí)滾動(dòng)球式機(jī)器人處于平衡狀態(tài)位于全局固定慣性坐標(biāo)系的原點(diǎn)O，當(dāng)受到瞬時(shí)的擾動(dòng)運(yùn)動(dòng)到圖2的位置；（2）假設(shè)球體與地面作純滾動(dòng)；（3）假設(shè)全向輪與球體之間的運(yùn)動(dòng)為純滾運(yùn)動(dòng)。

圖3 坐標(biāo)系的旋轉(zhuǎn)關(guān)系Fig.3 Rotation Relation of Coordinate System

3.2 廣義坐標(biāo)的設(shè)定

由于滾動(dòng)球式機(jī)器人系統(tǒng)是非完整系統(tǒng)，在建模的過(guò)程中把該系統(tǒng)分為主架體和球體兩部分。假設(shè)滾動(dòng)球式機(jī)器人從全局固定慣性坐標(biāo)系的原點(diǎn)運(yùn)動(dòng)到圖2所示的位置球體繞X軸轉(zhuǎn)過(guò)的角度為α，繞Y軸轉(zhuǎn)過(guò)的角度為β，繞Z軸轉(zhuǎn)過(guò)的角度為γ轉(zhuǎn)動(dòng)順序X-Y-Z，而主架體質(zhì)心在局部固定慣性坐標(biāo)系中的坐標(biāo)為（x，y，z），其姿態(tài)角可以用θ、φ、ψ來(lái)表示，假設(shè)球體的質(zhì)心（球心）與主架體的質(zhì)心共線(xiàn)，可以得到整個(gè)滾動(dòng)球式機(jī)器人系統(tǒng)在全局固定慣性坐標(biāo)系中的位置可以由參數(shù)x0，y0，z0，α，β，γ，x，y，z，θ、φ、ψ來(lái)表示，其中z0=R，R為球體的半徑，由此，對(duì)整個(gè)系統(tǒng)分析，將整個(gè)系統(tǒng)分為球體和主架體兩個(gè)部分，并分別對(duì)這兩個(gè)部分進(jìn)行建模分析，假設(shè)主架體的廣義坐標(biāo)為qz，則有qz=（x，y，z，θ，φ，ψ），球體的廣義坐標(biāo)的為qq，則有qq=（x0，y0，α，β，γ）。

3.3 球體動(dòng)力學(xué)建模

Lagrange-Routh方程動(dòng)力學(xué)建模的一般形式為[13-15]：

式中：qj—系統(tǒng)的廣義坐標(biāo)；Qj—廣義坐標(biāo)所對(duì)應(yīng)的廣義力；n—廣義坐標(biāo)的個(gè)數(shù)，其中n=9，L=T-V；T—系統(tǒng)動(dòng)能；V—系統(tǒng)的勢(shì)能；A—一個(gè)代表了k個(gè)Pfaff形式的非完整約束的k×n維的雅克比矩陣，且有A（q）q˙=0[1]。

首先對(duì)球體分析假設(shè)其廣義坐標(biāo)為qqj、P點(diǎn)的速度為VP、雅克比矩陣為Aq（qqj）、廣義坐標(biāo)對(duì)應(yīng)的廣義力為Qqj，動(dòng)能為T(mén)q，勢(shì)能為Vq，則qqj可以表示為qqj=（x0，y0，α，β，γ），假設(shè)球體在全局固定慣性坐標(biāo)系中的角速度為，根據(jù)文獻(xiàn)[1、6、8]所述，則可表示為如下公式：

由于球體在地面上作純滾運(yùn)動(dòng)，所以球體與地面的接觸點(diǎn)瞬時(shí)速度為零，假設(shè)球體與地面的接觸點(diǎn)為P，球體的半徑是R，則有VP=0，且[9]：

將式（2）代入到式（3）中可得：

對(duì)于新型滾動(dòng)球式動(dòng)機(jī)器人球體分析可知k=2，n=5，所以有：

球體的動(dòng)能、勢(shì)能分別為T(mén)q、Vq：

將廣義力及球體的動(dòng)能及勢(shì)能帶入到拉格朗日勞斯方程中得出了球體的完整動(dòng)力學(xué)方程，具體形式如下：

再將式（4）代入（8）消去拉格朗日乘子λ1和λ2，得出球體的完整動(dòng)力學(xué)方程，式（8）中Qq3、Qq4、Qq5分別為球體廣義坐標(biāo)α、β、γ所對(duì)應(yīng)的廣義力，根據(jù)球體完整動(dòng)力學(xué)方程可知只要給出球體的運(yùn)動(dòng)狀態(tài)及三個(gè)全向輪的輸出扭矩便可求出球體廣義坐標(biāo)qqj＝（x0，y0，α，β，γ）的值。

3.4 主架體動(dòng)力學(xué)建模

由于主架體不存在非完整約束，因此采用主動(dòng)力部分為有勢(shì)力的拉格朗日法對(duì)其系統(tǒng)進(jìn)行動(dòng)力學(xué)建模，假設(shè)主架體的角速度為w，總質(zhì)量為m，Jzx、Jzy、Jzz分別表示主架體繞x、y、z軸的轉(zhuǎn)動(dòng)慣量，系統(tǒng)的動(dòng)能為T(mén)z，勢(shì)能為Vz，則有：

結(jié)合拉格朗日方程可得主架體的完整動(dòng)力學(xué)方程如下：

式中：q—主架體的廣義坐標(biāo)為qzj=（x，y，z，φ，θ，ψ），Qzj—主架體廣義坐標(biāo)所對(duì)應(yīng)的廣義力，矩陣M、C、G分別表示廣義坐標(biāo)的二階導(dǎo)、一階導(dǎo)、廣義坐標(biāo)的系數(shù)矩陣，矩陣中的元素的值可以根據(jù)拉格朗日方程解出。

4 滾動(dòng)球式機(jī)器人動(dòng)力學(xué)仿真與分析

為了驗(yàn)證滾動(dòng)球式機(jī)器人動(dòng)力學(xué)方程建立的是否準(zhǔn)確運(yùn)用MATLAB/SIMULINK軟件對(duì)滾動(dòng)球式機(jī)器人的動(dòng)力學(xué)方程進(jìn)行仿真分析。對(duì)于滾動(dòng)球式機(jī)器人的運(yùn)動(dòng)主要分為三種情況，分別是原點(diǎn)自轉(zhuǎn)運(yùn)動(dòng)、沿平面直線(xiàn)運(yùn)動(dòng)、沿平面曲線(xiàn)運(yùn)動(dòng)。根據(jù)這三種運(yùn)動(dòng)情況對(duì)所研究的滾動(dòng)球式機(jī)器人進(jìn)行仿真分析。仿真分析的相關(guān)參數(shù)的設(shè)定如下：m=15kg，m0=4.056kg，R=0.15m。

（1）當(dāng)滾動(dòng)球式機(jī)器人繞原點(diǎn)自轉(zhuǎn)運(yùn)動(dòng)時(shí)，SIMULINK仿真結(jié)果，如圖4所示。對(duì)于球體有x0=y0=α=β=0，對(duì)于主架體有x=y=θ=φ＝0，x=y=θ=φ＝0，z=L+R而球體的γ角和主架體的ψ角應(yīng)該由零開(kāi)始逐漸增大，大小相等，方向相反。根據(jù)圖4可知球體和主架體的運(yùn)動(dòng)都符合實(shí)際運(yùn)動(dòng)規(guī)律。

圖4 滾動(dòng)球式機(jī)器人繞原點(diǎn)自轉(zhuǎn)運(yùn)動(dòng)時(shí)仿真結(jié)果Fig.4 Simulation Results of Rolling Ball Robot with Rotation Around the Original Point

（2）當(dāng)滾動(dòng)球式機(jī)器人沿y軸正方向直線(xiàn)運(yùn)動(dòng)時(shí)，MATLAB/SIMULINK仿真結(jié)果，如圖5所示。對(duì)于球體有x0=β=γ=0，對(duì)于主架體有x=ψ=0，z=L+R，根據(jù)圖5可知主架體的仰俯角和傾斜角在運(yùn)動(dòng)開(kāi)始時(shí)出現(xiàn)突然增大后慢慢趨于零，主架體沿y軸運(yùn)動(dòng)的距離起始出現(xiàn)小范圍的偏離原點(diǎn)后逐漸增大，對(duì)于球體其繞x軸的轉(zhuǎn)角及沿y軸運(yùn)動(dòng)的距離都是逐漸增大，實(shí)際滾動(dòng)球式機(jī)器人在剛開(kāi)始運(yùn)動(dòng)時(shí)會(huì)出現(xiàn)短時(shí)的抖動(dòng)，但很快將趨于穩(wěn)定，仿真結(jié)果與機(jī)器人實(shí)際運(yùn)動(dòng)基本相符。

圖5 滾動(dòng)球式機(jī)器人沿y軸正方向運(yùn)動(dòng)時(shí)仿真結(jié)果Fig.5 Simulation Results of Rolling Ball Robot Moving Along the Direction of Y Axis

（3）當(dāng)滾動(dòng)球式機(jī)器人沿曲線(xiàn)運(yùn)動(dòng)時(shí)，假設(shè)沿y=x2（x＞0）運(yùn)動(dòng)，對(duì)于主架體有z=L+R，其MATLAB/SIMULINK仿真結(jié)果，如圖6所示。實(shí)際中滾動(dòng)球式機(jī)器人主架體的仰俯角和傾斜角起始有微小波動(dòng)后逐漸趨于零，主架的航向角與球體繞z軸轉(zhuǎn)動(dòng)的角大小基本相等，方向相反且逐漸增大，球體繞x和y軸轉(zhuǎn)動(dòng)的角都是逐漸的增大，對(duì)于球體和主架體運(yùn)動(dòng)距離在x和y軸上的投影都基本相等且成逐漸增大的趨勢(shì)，從圖6的仿真結(jié)果圖可知球體和主架體的運(yùn)動(dòng)基本符合實(shí)際的運(yùn)動(dòng)趨勢(shì)。

圖6 滾動(dòng)球式機(jī)器人沿曲線(xiàn)運(yùn)動(dòng)時(shí)仿真結(jié)果Fig.6 Simulation Results of Rolling Ball Robot Along the Curve

5 結(jié)論

（1）通過(guò)對(duì)已有的“Rezero”機(jī)器人進(jìn)行系統(tǒng)的分析，設(shè)計(jì)出了一種滾動(dòng)球式醫(yī)療服務(wù)型機(jī)器人，該機(jī)器人通過(guò)優(yōu)化制動(dòng)裝置和增加失電保護(hù)裝置，有效的提高了機(jī)器人的安全性和穩(wěn)定性。（2）運(yùn)用拉格朗日-勞斯方程對(duì)滾動(dòng)球式醫(yī)療服務(wù)型機(jī)器人動(dòng)力學(xué)建模，通過(guò)引入拉格朗日乘子，減少了方程的求解難度，彌補(bǔ)了拉格朗日方程無(wú)法計(jì)算約束反力的缺點(diǎn)，解決了該機(jī)器人系統(tǒng)的非完整性問(wèn)題。（3）通過(guò)運(yùn)用MATLAB/SIMULINK軟件對(duì)建立的數(shù)學(xué)模型進(jìn)行了仿真，驗(yàn)證了所建立的動(dòng)力學(xué)模型的準(zhǔn)確性，同時(shí)也為后續(xù)的結(jié)構(gòu)優(yōu)化和控制系統(tǒng)的設(shè)計(jì)提供了有力依據(jù)，具有較大的實(shí)際意義。