張 敏 徐 鑒
(同濟(jì)大學(xué)航空航天與力學(xué)學(xué)院,上海200092)
振動驅(qū)動移動系統(tǒng)平面避障運(yùn)動分析1)
張 敏 徐 鑒2)
(同濟(jì)大學(xué)航空航天與力學(xué)學(xué)院,上海200092)
近年來,工業(yè)機(jī)器人的應(yīng)用領(lǐng)域日益廣泛,可移動機(jī)器人的發(fā)展備受關(guān)注,為了在一些復(fù)雜環(huán)境中準(zhǔn)確地完成作業(yè),學(xué)者們提出并研究了振動驅(qū)動移動系統(tǒng).本文研究了在各向異性黏性摩擦環(huán)境中一類有兩個(gè)在平行軌道內(nèi)做正弦運(yùn)動的內(nèi)部質(zhì)量塊的振動驅(qū)動移動系統(tǒng)的運(yùn)動規(guī)律,提出了使系統(tǒng)完成包括避障等規(guī)定作業(yè)的驅(qū)動設(shè)計(jì)方法.首先利用第二類拉格朗日方程,建立了系統(tǒng)的動力學(xué)方程;然后,利用速度Verlet積分法分析了系統(tǒng)的運(yùn)動規(guī)律,得到了內(nèi)部驅(qū)動參數(shù)與系統(tǒng)運(yùn)動軌跡、運(yùn)動速度的關(guān)系;最后,結(jié)合振動驅(qū)動移動系統(tǒng)的運(yùn)動規(guī)律,提出了使系統(tǒng)沿預(yù)設(shè)路徑運(yùn)動和實(shí)現(xiàn)避障運(yùn)動的驅(qū)動設(shè)計(jì)方法.通過曲線離散得到了系統(tǒng)沿預(yù)設(shè)路徑運(yùn)動的移動軌跡,進(jìn)而通過改變內(nèi)部質(zhì)量塊的驅(qū)動參數(shù),使系統(tǒng)沿預(yù)設(shè)路徑運(yùn)動.為了使移動系統(tǒng)在障礙物環(huán)境中達(dá)到目標(biāo)位置,提出了結(jié)合柵格法,F(xiàn)loyd算法及最小頂點(diǎn)圓法的優(yōu)化的路徑規(guī)劃計(jì)算方法,得到了振動驅(qū)動移動系統(tǒng)在障礙物環(huán)境中運(yùn)動的最優(yōu)路徑,并通過改變內(nèi)部質(zhì)量塊的驅(qū)動參數(shù)實(shí)現(xiàn)了移動系統(tǒng)的避障運(yùn)動.
振動驅(qū)動移動系統(tǒng),平面運(yùn)動,正弦驅(qū)動,各向異性黏性摩擦,驅(qū)動設(shè)計(jì),避障運(yùn)動
工業(yè)機(jī)器人的應(yīng)用領(lǐng)域日益廣泛,可移動機(jī)器人的發(fā)展越來越得到人們的關(guān)注,輪式和腿式機(jī)器人可以代替人類在危險(xiǎn)、惡劣(如輻射、有毒等)和人所不及的(如宇宙空間)環(huán)境下作業(yè),但在一些狹小(如管道、腸道等)和特殊介質(zhì)(如碎石、水下等)環(huán)境中,輪式和腿式機(jī)器人卻很難準(zhǔn)確地移動并完成作業(yè).為了解決這一難題,學(xué)者們提出并研究了一些仿生學(xué)模型[1-2].受蚯蚓在一些復(fù)雜環(huán)境下運(yùn)動的啟發(fā),Chernousko[3]提出了振動驅(qū)動系統(tǒng),它是指在內(nèi)部質(zhì)量塊的周期性驅(qū)動下,能夠在有阻力的環(huán)境中實(shí)現(xiàn)剛體位移的可移動系統(tǒng),具有結(jié)構(gòu)簡單、易于微型化和密封性好等優(yōu)點(diǎn).這種可移動系統(tǒng)可以是單單元系統(tǒng),也可以是由彈性元件連接而成的多單元系統(tǒng),可用來設(shè)計(jì)模仿蚯蚓運(yùn)動的蠕動型移動機(jī)器人,因此,對其動力學(xué)行為的研究具有結(jié)構(gòu)仿生和工業(yè)應(yīng)用雙重意義.本文主要關(guān)注振動驅(qū)動系統(tǒng)的單單元模型.在該類模型中,內(nèi)部質(zhì)量塊相對外部剛體做周期運(yùn)動,外部剛體在內(nèi)部質(zhì)量塊的慣性力和支撐面的摩擦力共同作用下實(shí)現(xiàn)平面運(yùn)動.內(nèi)部質(zhì)量塊的運(yùn)動形式主要包含兩相運(yùn)動[3-4]、三相運(yùn)動[3-6[7-8].系統(tǒng)與支撐面間的摩擦力主要分為庫倫干摩擦[5,9-10]和各向異性黏性摩擦[6,11-12].
學(xué)者們對振動驅(qū)動系統(tǒng)的直線運(yùn)動的研究比較豐富.Chernousko[3-4,13-14]、Bolotnik等[7-8]、Sergey等[15]和方虹斌等[5-6]主要研究了在庫倫摩擦和各向異性黏性摩擦下,含單個(gè)和兩個(gè)內(nèi)部質(zhì)量塊的振動驅(qū)動系統(tǒng)在不同驅(qū)動下的穩(wěn)態(tài)運(yùn)動,并以系統(tǒng)的最大穩(wěn)態(tài)平均速度或最小能量消耗為目標(biāo),對內(nèi)部驅(qū)動進(jìn)行優(yōu)化.方虹斌和徐鑒[6]利用平均法,在理論上得到了單單元系統(tǒng)在各向異性黏性摩擦環(huán)境下,三相驅(qū)動時(shí)系統(tǒng)做直線運(yùn)動的穩(wěn)態(tài)運(yùn)動速度的近似解析解.Sobolev等[11]和Li等[16]分別利用含有多個(gè)轉(zhuǎn)子的振動驅(qū)動系統(tǒng)和倒擺車實(shí)驗(yàn)?zāi)P万?yàn)證了單單元系統(tǒng)的直線運(yùn)動理論.陳祺等[17]研究了振動驅(qū)動系統(tǒng)直線運(yùn)動的滑移分岔.
對振動驅(qū)動系統(tǒng)平面運(yùn)動的研究則相對較少. Volkova等[9]研究了由兩個(gè)在平行軌道內(nèi)做正弦運(yùn)動的內(nèi)部質(zhì)量塊驅(qū)動的振動驅(qū)動系統(tǒng),系統(tǒng)通過四個(gè)傾角可變的支撐與運(yùn)動面接觸來實(shí)現(xiàn)各向異性摩擦,用數(shù)值方法分析了內(nèi)部質(zhì)量塊的運(yùn)動頻率和相位差對系統(tǒng)運(yùn)動的影響;并通過改變四個(gè)支撐角的傾角,實(shí)現(xiàn)了S形的折線運(yùn)動.占雄和徐鑒[10,12]研究了由兩個(gè)在相互垂直軌道內(nèi)做正弦運(yùn)動的內(nèi)部質(zhì)量塊驅(qū)動和有三個(gè)工字形排布的周期運(yùn)動的內(nèi)部質(zhì)量塊的振動驅(qū)動模型,在庫倫摩擦各向異性時(shí),考慮stick-slip黏滑效應(yīng),理論上得到了系統(tǒng)轉(zhuǎn)動和平動時(shí)的速度解析式,并分析了內(nèi)部驅(qū)動頻率和相位差對系統(tǒng)運(yùn)動的影響,通過組合平動和轉(zhuǎn)動,得到了近似S形的折線運(yùn)動.殷蓬勃等[18]研究了由兩個(gè)在相互垂直軌道內(nèi)做三相運(yùn)動的內(nèi)部質(zhì)量塊的振動驅(qū)動模型在各向異性黏性摩擦環(huán)境中的平面運(yùn)動規(guī)律,并通過調(diào)節(jié)內(nèi)部驅(qū)動的周期比和相位,得到了系統(tǒng)軌跡在直線和圓弧間相互切換的六種平面運(yùn)動形式.
已有的振動驅(qū)動移動系統(tǒng)的研究,均為給定系統(tǒng)內(nèi)部驅(qū)動形式來研究系統(tǒng)的運(yùn)動軌跡.而預(yù)設(shè)振動驅(qū)動移動系統(tǒng)運(yùn)動軌跡或系統(tǒng)完成避障運(yùn)動的研究目前還沒有,因?yàn)樵趯?shí)際工況中,機(jī)器人的工作形式往往是接收命令之后,自主完成作業(yè).所以針對振動驅(qū)動移動系統(tǒng)的運(yùn)動目標(biāo),設(shè)計(jì)驅(qū)動參數(shù)的問題顯得尤為重要.針對這一問題,本文首先分析了各向異性黏性摩擦環(huán)境下,振動驅(qū)動系統(tǒng)在兩個(gè)平行的正弦驅(qū)動激勵(lì)下的系統(tǒng)運(yùn)動規(guī)律,然后根據(jù)系統(tǒng)的運(yùn)動規(guī)律,設(shè)計(jì)了通過調(diào)節(jié)系統(tǒng)內(nèi)部驅(qū)動參數(shù),使系統(tǒng)完成包括避障在內(nèi)的平面運(yùn)動的方法.
在使振動驅(qū)動系統(tǒng)沿預(yù)設(shè)路徑運(yùn)動的問題研究中,針對系統(tǒng)的運(yùn)動規(guī)律,需要對軌跡進(jìn)行曲線離散.對曲線離散方法的研究有很多,目前常用的曲線離散化處理方法主要有等間隔法、等弦長法、等誤差法和特征點(diǎn)識別法[19-20].等間隔法的特點(diǎn)是便于計(jì)算機(jī)硬件實(shí)現(xiàn),運(yùn)行速度較快,但逼近弦長不均勻且在曲率變化較大處會產(chǎn)生較大的逼近誤差;等弦長法的特點(diǎn)是弦長均勻,有利于提高三角化質(zhì)量,但在曲率變化較大處會產(chǎn)生較大的逼近誤差;等誤差法可以使直線較為均勻地逼近理想曲線,產(chǎn)生較少的冗余點(diǎn),但是當(dāng)曲線形式較為復(fù)雜時(shí)算法實(shí)現(xiàn)較為困難,運(yùn)算速度慢,同時(shí)在曲率較高處,會使逼近線段過短,從而導(dǎo)致在三角化過程中,產(chǎn)生狹長三角形而影響三角面片的質(zhì)量.由劉丹丹等[19]提出的基于特征點(diǎn)識別法的離散方法,可根據(jù)誤差限調(diào)節(jié)曲線逼近的程度,既保證了曲線的逼近精度,避免了在曲線曲率較高處和曲線端點(diǎn)處產(chǎn)生過短的逼近弦長,同時(shí),通過局部特征點(diǎn)的優(yōu)化,過濾掉不重要的局部特征點(diǎn),使得曲線的離散點(diǎn)數(shù)減少,有利于加快后續(xù)三角化速度.為了在保證精度的前提下,減小振動驅(qū)動系統(tǒng)瞬態(tài)運(yùn)動的誤差,減少驅(qū)動參數(shù)的切換次數(shù),本文選用基于特征點(diǎn)識別法的曲線離散方法,結(jié)合系統(tǒng)的運(yùn)動特點(diǎn),設(shè)計(jì)了振動驅(qū)動系統(tǒng)沿預(yù)設(shè)路徑運(yùn)動的驅(qū)動設(shè)計(jì)方法.
振動驅(qū)動移動系統(tǒng)在障礙物環(huán)境中自主選擇路徑到達(dá)目標(biāo)位置的研究中,首先需要完成路徑規(guī)劃工作.路徑規(guī)劃技術(shù)在很多領(lǐng)域都具有廣泛的應(yīng)用.凡是可拓?fù)錇辄c(diǎn)線網(wǎng)絡(luò)的規(guī)劃問題基本上都可以采用路徑規(guī)劃的方法解決.路徑規(guī)劃分為全局規(guī)劃和局部規(guī)劃,從獲取的障礙物信息來看,全局規(guī)劃屬于靜態(tài)規(guī)劃,局部規(guī)劃屬于動態(tài)規(guī)劃,其核心是算法的設(shè)計(jì).常用的路徑規(guī)劃方法有傳統(tǒng)算法、圖形學(xué)法、智能算法和搜索式算法[21-23].其中傳統(tǒng)算法有模擬退火算法、人工勢場法、模糊邏輯算法等,圖形學(xué)法主要有柵格法、可視圖空間法等,常用于全局規(guī)劃;智能算法包括蟻群算法、遺傳算法、粒子群算法等,常用于局部規(guī)劃.本文主要研究了已知環(huán)境中障礙物信息的情況下,振動驅(qū)動系統(tǒng)內(nèi)部驅(qū)動的設(shè)計(jì),即在路徑規(guī)劃中屬于全局規(guī)劃.根據(jù)振動驅(qū)動移動系統(tǒng)的運(yùn)動規(guī)律,結(jié)合柵格法[23-24]、Floyd算法[25]和最小頂點(diǎn)圓法[26]提出了優(yōu)化的路徑規(guī)劃算法,然后設(shè)計(jì)出完成規(guī)劃路徑所需要的內(nèi)部驅(qū)動.
1.1 力學(xué)模型
本文的研究對象為有兩個(gè)內(nèi)部可動質(zhì)量塊的振動驅(qū)動系統(tǒng),內(nèi)部質(zhì)量塊分別處于兩個(gè)平行軌道中,用M,m1和m2分別表示剛性物體和兩個(gè)內(nèi)部質(zhì)量塊的重量.系統(tǒng)通過4個(gè)傾斜小角Ci(i=1,2,3,4)與地面接觸,作用有各向異性的黏性摩擦.系統(tǒng)的力學(xué)模型如圖1所示.剛體的長和寬分別為2k和2d,高度不計(jì).剛體內(nèi)部的兩個(gè)平行軌道關(guān)于剛體的對稱軸對稱,長度均為L,即內(nèi)部質(zhì)量允許運(yùn)動的最大距離為L;兩軌道間的距離為2b.兩個(gè)內(nèi)部質(zhì)量塊于軌道內(nèi)做相對剛體的正弦運(yùn)動,即
圖1 振動驅(qū)動系統(tǒng)的平面運(yùn)動模型Fig.1 The planar motion model of vibration-driven system
1.2 系統(tǒng)動力學(xué)方程
引入慣性坐標(biāo)系Oxy和相對系統(tǒng)的動坐標(biāo)系O1ξη,O1位于剛體M的形心,軸O1ξ和O1η分別為剛體的兩個(gè)對稱軸,轉(zhuǎn)角?表示軸O1ξ與軸Ox之間的夾角.
下面利用矢量法計(jì)算系統(tǒng)的動能.在動坐標(biāo)系O1ξη中,質(zhì)量塊m1和m2的相對位置矢量分別為
其中ξ1和ξ2分別表示質(zhì)量塊m1和m2的相對位移,則慣性坐標(biāo)系Oxy下,剛體M、質(zhì)量塊m1和m2的位置矢量分別為
其中Tr表示由動坐標(biāo)系O1ξη到慣性坐標(biāo)系Oxy轉(zhuǎn)換矩陣,即
由方程(3)~方程(6)可得剛體M和內(nèi)部質(zhì)量塊m1和m2的速度矢量,即
由此可得系統(tǒng)平面運(yùn)動的動能,即
其中
下面利用矢量法計(jì)算系統(tǒng)平面運(yùn)動時(shí)4個(gè)支撐角的速度.在慣性坐標(biāo)系Oxy下,4個(gè)支撐ci(i=1,2,3,4)的位置矢量分別為
結(jié)合方程(12)和方程(14)得4個(gè)支撐ci(i=1,2,3,4)的速度矢量在O1ξ和O1η方向上的投影,
即
除了支撐處的速度,黏性摩擦系數(shù)也影響?zhàn)ば阅Σ亮Φ拇笮?為了使該模型的摩擦各向異性,將系統(tǒng)的4個(gè)支撐ci(i=1,2,3,4)按圖2所示方式設(shè)計(jì).圖2(a)中,在O1ξ方向上,支撐與O1ξ正方向的夾角小于90°,使得系統(tǒng)在向前(O1ξ正向)和向后(O1ξ負(fù)向)運(yùn)動時(shí)黏性摩擦系數(shù)的不同;圖2(b)中,在O1η方向上,支撐與平面的夾角等于90°[13].
圖2 支撐ci(i=1,2,3,4)處的黏性摩擦力模型Fig.2 Model of the viscous frictional force at thei-th support
本文系統(tǒng)運(yùn)動過程中4個(gè)支撐ci(i=1,2,3,4)在O1ξ方向上的傾角保持不變,O1η方向保持豎直.給定4個(gè)支撐的傾角,則系統(tǒng)在O1ξ和O1η方向上的黏性摩擦系數(shù)滿足如下關(guān)系
其中,λ為與支撐傾角相關(guān)的系數(shù)[13].
利用方程(15)和方程(16),可以得到系統(tǒng)的4個(gè)支撐在O1ξ和O1η方向上的黏性摩擦力如圖3所示,即
圖3 振動驅(qū)動系統(tǒng)與平面間的相互作用力和力矩Fig.3 The interaction force and moment of force between vibration-driven system and ground
利用轉(zhuǎn)換矩陣得系統(tǒng)的4個(gè)支撐在慣性坐標(biāo)系Oxy下的黏性摩擦力,即
則系統(tǒng)所受的摩擦合力和摩擦合力矩分別為
利用虛功原理計(jì)算廣義力,系統(tǒng)平面運(yùn)動的廣義坐標(biāo)分別為剛體形心的位移(xM,yM)和系統(tǒng)相對形心的轉(zhuǎn)角?,當(dāng)δx=0,δy=0,δ?=0時(shí),由虛功原理得
則系統(tǒng)在x方向上的廣義力為
同理,得到系統(tǒng)在y方向上的廣義力和繞形心轉(zhuǎn)動的廣義力矩,即
將方程(11)、方程(17)和方程(18)分別代入第二類拉格朗日方程,得到系統(tǒng)平面運(yùn)動的動力學(xué)方程(19),即
且滿足初始條件x(0)=0,(0)=0,y(0)=0,(0)=0,?(0)=0,(0)=0.
在系統(tǒng)的動力學(xué)方程(19)中,由于廣義坐標(biāo)間的相互耦合,方程求解具有很大的難度,下文將采用數(shù)值算法[13,16]來分析系統(tǒng)的平面運(yùn)動規(guī)律.
2.1 速度Verlet積分法
設(shè)x(t)為質(zhì)點(diǎn)在t時(shí)刻的位置向量,則在t0時(shí)刻的泰勒展開式為
將方程(20)和方程(21)相加,得
將方程(20)和方程(22)相減,得
由式(23)可得
并代入方程(24)中,得到
方程(23)和方程(25)即為速度Verlet積分法,通過這種數(shù)值算法可以同時(shí)得到速度和位移.速度Verlet積分法具有較好的穩(wěn)定性和計(jì)算速度.由于振動驅(qū)動移動系統(tǒng)的每個(gè)周期的凈位移很微小,所以對其運(yùn)動規(guī)律的研究需要考慮較長的運(yùn)動時(shí)間,速度Verlet積分法能夠很好地滿足這樣的要求.
在本文中設(shè)定以下基本參數(shù)
2.2 直線運(yùn)動
當(dāng)ω1=ω2=ω時(shí),即ξ1=ξ2=asinωt,此時(shí)兩個(gè)內(nèi)部質(zhì)量塊同步運(yùn)動,方程(19)簡化為
因?yàn)橄到y(tǒng)的對稱性,在y方向上不存在驅(qū)動力,只有x方向上的驅(qū)動力,所以系統(tǒng)將沿x方向做直線運(yùn)動.ω不同時(shí),直線運(yùn)動的軌跡、x方向速度M和位移xM與時(shí)間t圖像如下圖.
由圖4~圖6可見,當(dāng)兩個(gè)內(nèi)部質(zhì)量塊同步運(yùn)動時(shí),系統(tǒng)做直線運(yùn)動,且運(yùn)動具有周期性.隨著驅(qū)動頻率的增大,系統(tǒng)運(yùn)動的穩(wěn)態(tài)平均速度增大.
圖4 系統(tǒng)直線運(yùn)動軌跡Fig.4 Trajectories of vibration-driven system
圖5 系統(tǒng)x方向位移xM與時(shí)間t的關(guān)系Fig.5 Relations between displacementxMand timet
圖6 系統(tǒng)x方向速度M與時(shí)間t的關(guān)系Fig.6 Relations between velocityMand timet
2.3 平面運(yùn)動
當(dāng)ω1≠ω2時(shí),令n=ω2/ω1,系統(tǒng)做圓周運(yùn)動.令ω1=100rad/s,改變參數(shù)n,可得到如圖7所示系統(tǒng)的運(yùn)動軌跡[16].
由圖7可知,當(dāng)n<1時(shí),系統(tǒng)做逆時(shí)針的圓周運(yùn)動,隨著n的增大,系統(tǒng)運(yùn)動的曲率半徑越來越大;當(dāng)n>1時(shí),系統(tǒng)做順時(shí)針的圓周運(yùn)動,隨著n的增大,系統(tǒng)運(yùn)動的曲率半徑越來越小.且當(dāng)n1,n2滿足倒數(shù)關(guān)系時(shí),系統(tǒng)運(yùn)動的曲率半徑相同,方向相反.
圖7 ω1=100rad/s,n取不同值時(shí)系統(tǒng)軌跡Fig.7 Trajectories of vibration-driven system under di ff erentnwhen ω1=100rad/s
在圖8中,當(dāng)n<1時(shí),隨著n的增大,系統(tǒng)轉(zhuǎn)角?的變化越來越慢,這是由于隨著n的增大,系統(tǒng)的曲率半徑越來越大.在圖9中,隨著n的增大,系統(tǒng)路程s的變化越來越快.因?yàn)殡S著n的增大,系統(tǒng)運(yùn)動的穩(wěn)態(tài)平均線速度越來越大.
圖8 ω1=100rad/s,n<1時(shí)系統(tǒng)的轉(zhuǎn)角?與時(shí)間t的關(guān)系Fig.8 Relations between angular displacement ? and timetwhenn<1 and ω1=100rad/s
圖9 ω1=100rad/s,n<1時(shí)系統(tǒng)路程s與時(shí)間t的關(guān)系Fig.9 Relations between displacementsand timetwhenn<1 and ω1=100rad/s
當(dāng)n<1時(shí),系統(tǒng)運(yùn)動的曲率半徑R與參數(shù)n的關(guān)系(ω1=100rad/s)如圖10.隨著n從0增大到1,系統(tǒng)的曲率半徑由最小值0.1076m逐漸增大,直至n=1時(shí),系統(tǒng)做直線運(yùn)動,即系統(tǒng)運(yùn)動的曲率半徑為無窮大.
通過對系統(tǒng)平面運(yùn)動規(guī)律的分析,可見當(dāng)兩個(gè)內(nèi)部質(zhì)量塊的驅(qū)動頻率不同時(shí),系統(tǒng)可以實(shí)現(xiàn)不同半徑的圓周運(yùn)動和直線運(yùn)動,可以通過改變內(nèi)部質(zhì)量塊的驅(qū)動頻率比n(ω2/ω1)來實(shí)現(xiàn)系統(tǒng)的平面運(yùn)動.
圖10 ω1=100rad/s,系統(tǒng)曲率半徑R與驅(qū)動頻率比n的關(guān)系Fig.10 Relations between radiusRandnwhen ω1=100rad/s
在實(shí)際工況中,有些情況需要機(jī)器人按照既定的路徑運(yùn)動,本節(jié)將介紹振動驅(qū)動系統(tǒng)按給定軌跡運(yùn)動的驅(qū)動設(shè)計(jì)方法.首先,對給定軌跡進(jìn)行曲線離散,將曲線離散為滿足誤差條件的折線;然后用速度Verlet積分法得到數(shù)值模擬的系統(tǒng)驅(qū)動參數(shù).
3.1 對系統(tǒng)運(yùn)動軌跡進(jìn)行曲線離散
曲線離散流程見圖11.預(yù)設(shè)軌跡為函數(shù) γ=f(x),x∈[a,b],誤差限e.
(1)求出曲線的所有駐點(diǎn)和不可導(dǎo)點(diǎn)Pi(i=1,2,···,n),與曲線的端點(diǎn)一起組成特征點(diǎn)集C;
(2)將特征點(diǎn)集C中的點(diǎn)按照x坐標(biāo)的順序排列,將函數(shù)曲線分為n+1段.選取每段的起始點(diǎn)Ps與終止點(diǎn)Pe;
(3)對每段函數(shù)曲線γ=f(x),x∈[ai,bi],以Ps,Pe為端點(diǎn)做連線,求取局部特征點(diǎn)Pk,局部特征點(diǎn)處的導(dǎo)數(shù)值等于直線PsPe的斜率.即
由此得到點(diǎn)Pk,并求出其到直線PsPe的距離dk
(4)若dk>e,將Pk加入特征點(diǎn)集C.若dk≤e,則舍棄;
(5)對新的特征點(diǎn)集C重復(fù)步驟(2)~(4),直到所有的dk≤e.此時(shí)的C就是函數(shù)曲線的特征點(diǎn)集;
(6)按照一定的順序依次連接C內(nèi)的點(diǎn),即為所求的曲線的離散.
圖11 曲線離散流程圖Fig.11 Flow chart of curve discrete
對軌跡f(x)=2cos(x+π/2)-cos(3x)作曲線離散的圖像如圖12,分別選取誤差限e1=0.2,e2=0.1,可見隨著誤差限e的減小,離散的結(jié)果越來越接近原曲線.通過對誤差限e的調(diào)整,可以得到不同精度的離散曲線,e越小,離散后的曲線與原曲線越接近.在工程實(shí)踐中可根據(jù)實(shí)際要求控制誤差限e來進(jìn)行調(diào)整.
曲線離散過程中的誤差由誤差限e來控制,在特征點(diǎn)集的確定過程中,隨著迭代次數(shù)的增大,離散后的折線與原曲線之間的最大誤差越來越小,即max(dk)越來越趨近于誤差限e.示例中的迭代次數(shù)與最大誤差之間的關(guān)系如圖13所示.
圖12 曲線離散示例Fig.12 Sample of curve discrete
圖13 誤差分析Fig.13 Error analysis
3.2 驅(qū)動設(shè)計(jì)
得到曲線離散后的折線后,利用系統(tǒng)內(nèi)部質(zhì)量塊的驅(qū)動頻率比n(ω2/ω1)與系統(tǒng)運(yùn)動的曲率半徑R的關(guān)系,來設(shè)計(jì)系統(tǒng)的驅(qū)動參數(shù).
(1)針對方程(19)利用速度Verlet積分法算出系統(tǒng)驅(qū)動頻率比n(ω2/ω1)與曲率半徑R的關(guān)系;
(2)利用切線圓法計(jì)算每個(gè)拐點(diǎn)處所允許的最大轉(zhuǎn)彎半徑R0如圖14;
圖中x0表示完成轉(zhuǎn)彎需要預(yù)留的距離,a表示軌道寬度.由幾何關(guān)系,可得
(3)找出R0對應(yīng)頻率比n;
圖14 轉(zhuǎn)彎半徑Fig.14 Turning radius
(4)令n=1,即ω1=ω2,振動驅(qū)動系統(tǒng)作直線運(yùn)動至拐點(diǎn);
(5)切換頻率比n,轉(zhuǎn)彎至所需角度;
(6)重復(fù)步驟(4)和(5)直至系統(tǒng)完成最后一次轉(zhuǎn)彎;
(7)令n=1,振動驅(qū)動系統(tǒng)運(yùn)動至終點(diǎn).
驅(qū)動設(shè)計(jì)的示例圖和流程圖如圖15和圖16所示.由圖中可見,通過我們設(shè)計(jì)的算法,系統(tǒng)的運(yùn)動軌跡與預(yù)設(shè)軌跡之間的誤差極小,從程序中可以讀出系統(tǒng)完成每個(gè)步驟所需要的驅(qū)動參數(shù).
圖15 設(shè)計(jì)預(yù)設(shè)軌跡所需驅(qū)動的示例Fig.15 Sample of drive design for preset trajectory
圖16 驅(qū)動設(shè)計(jì)流程圖Fig.16 Flow chart of drive design
振動驅(qū)動系統(tǒng)在移動過程中躲避障礙物的運(yùn)動是一種平面運(yùn)動,關(guān)鍵在于內(nèi)部質(zhì)量塊驅(qū)動參數(shù)的設(shè)計(jì),即驅(qū)動力的設(shè)計(jì).另外,有些工況不會給出系統(tǒng)的運(yùn)動軌跡,而需要其在有障礙物的環(huán)境中自主選取軌跡,只知道障礙物和系統(tǒng)起始點(diǎn)、目標(biāo)點(diǎn)的信息.因此,需要提出設(shè)計(jì)振動驅(qū)動系統(tǒng)驅(qū)動參數(shù)的方法,本節(jié)將提出振動驅(qū)動移動系統(tǒng)躲避障礙物的方法.
4.1 對系統(tǒng)運(yùn)動環(huán)境進(jìn)行區(qū)分,標(biāo)識出障礙物區(qū)域及可運(yùn)動區(qū)域
在知道運(yùn)動環(huán)境中障礙物信息的情況下,為了能夠?qū)⒖傻竭_(dá)區(qū)域和障礙物區(qū)域區(qū)分開來,我們選用柵格法來進(jìn)行處理.
假設(shè)已知一個(gè)長為L,寬為H的矩形區(qū)域,其中有障礙物Oi(i=1,2,3,···),并已知各障礙物位置信息,如圖17所示.
(1)選取柵格邊長為a,對矩形區(qū)域進(jìn)行柵格劃分,則區(qū)域內(nèi)共有L/a×H/a個(gè)柵格.按照柵格所處的行列,將每個(gè)柵格中心表示為(x,y),其中x表示列數(shù),y表示行數(shù),方向總是從起始點(diǎn)至目標(biāo)點(diǎn).
(2)對柵格進(jìn)行區(qū)分,引入狀態(tài)函數(shù)z=f(x,y).令
(3)將起始點(diǎn)所在柵格表示為(xs,ys),目標(biāo)點(diǎn)所在柵格表示為(xe,ye).
4.2 利用垂線法和Floyd算法選出模型運(yùn)動過程中躲避的關(guān)鍵位置
在完成柵格劃分,對運(yùn)動環(huán)境進(jìn)行區(qū)分之后,我們首先需要初步規(guī)劃出系統(tǒng)從起始點(diǎn)到目標(biāo)點(diǎn)運(yùn)動的軌跡,在用垂線法選出可選擇的中間節(jié)點(diǎn)之后,利用Floyd算法[25]選出完成最短路徑時(shí)經(jīng)過的中間節(jié)點(diǎn).
(1)連結(jié)起始點(diǎn)柵格(xs,ys)與目標(biāo)點(diǎn)柵格(xe,ye).得到線段l.可表示為
圖17 柵格地圖模型(紅點(diǎn):起始點(diǎn),綠點(diǎn):目標(biāo)點(diǎn))Fig.17 Model of grid map(red:initial point,green:target point)
(2)若線段l通過的所有柵格的狀態(tài)函數(shù)f(x,y)均為零,即起始點(diǎn)到目標(biāo)點(diǎn)的連線不經(jīng)過障礙物,振動驅(qū)動系統(tǒng)可作直線運(yùn)動,線段l即為運(yùn)動軌跡;
若線段l通過障礙物柵格,則需要計(jì)算系統(tǒng)躲避障礙物的拐點(diǎn).
(3)根據(jù)垂線法選擇節(jié)點(diǎn)柵格.計(jì)算線段l穿過的障礙物Oi相鄰的所有可到達(dá)柵格(x,y)到線段l的距離d.并分別選取線段l兩側(cè)max(d)對應(yīng)的柵格為節(jié)點(diǎn)柵格
(4)將步驟(3)所得節(jié)點(diǎn)柵格與起始點(diǎn)、目標(biāo)點(diǎn)柵格一起編號,起始點(diǎn)柵格標(biāo)記為1,步驟(3)求得的柵格按x從小到大的順序依次排序,終點(diǎn)柵格標(biāo)記為m.
(5)寫出無中間節(jié)點(diǎn)的權(quán)值矩陣A=(aij)m×m,其中aij代表從節(jié)點(diǎn)(xi,yi)到節(jié)點(diǎn)(xj,yj)的距離.
(6)根據(jù)節(jié)點(diǎn)所在位置計(jì)算節(jié)點(diǎn)間權(quán)值大小.
(a)aii=0;(b)若節(jié)點(diǎn)(xi,yi)與節(jié)點(diǎn)(xj,yj)連線通過障礙物柵格,則aij=aji=∞;(c)若節(jié)點(diǎn)(xi,yi)與節(jié)點(diǎn)(xj,yj)連線不通過障礙物柵格,則
(d)若j=0,則aij=∞;若i=m,則aij=∞.
(7)計(jì)算節(jié)點(diǎn)(xi,yi)到節(jié)點(diǎn)(xj,yj)間有1個(gè)中間節(jié)點(diǎn)情況下的權(quán)值矩陣.設(shè)節(jié)點(diǎn)(xi,yi)經(jīng)過一個(gè)中間節(jié)點(diǎn)(xr,yr)到達(dá)節(jié)點(diǎn)(xj,yj),則節(jié)點(diǎn)(xi,yi)到節(jié)點(diǎn)(xj,yj)的最短距離為權(quán)值矩陣為
4.3 利用最小頂點(diǎn)圓法確定系統(tǒng)的最終運(yùn)動軌跡
因?yàn)檎駝域?qū)動系統(tǒng)在內(nèi)部質(zhì)量塊的驅(qū)動頻率不同,即驅(qū)動頻率比n≠1時(shí),系統(tǒng)將做圓周運(yùn)動,且圓周運(yùn)動的最小半徑即為n=0時(shí)得到的曲率半徑,所以在得到初步的系統(tǒng)運(yùn)動軌跡后,結(jié)合模型的運(yùn)動特點(diǎn),采用最小頂點(diǎn)圓法對選出的路徑進(jìn)行優(yōu)化,得到更適合振動驅(qū)動系統(tǒng)的運(yùn)動路徑.具體方法如下.
(1)已知起始點(diǎn)柵格(xs,ys)、中間節(jié)點(diǎn)柵格(xi,yi) (i=1,2,···)、目標(biāo)點(diǎn)柵格(xe,ye).確定安全距離r,以中間節(jié)點(diǎn)為圓心,r′=r/a為半徑做圓,作第i個(gè)點(diǎn)與第i+1個(gè)點(diǎn)之間的切線.
(a)當(dāng)i=1或m-1時(shí),為第1種情況,即求點(diǎn)--圓切線;(b)當(dāng)i≠1或m-1時(shí),為第2種情況,即求圓--圓切線.
(2)選取遠(yuǎn)離障礙物的切線及切點(diǎn)間的圓弧為系統(tǒng)的軌跡,即優(yōu)化后的路徑.
當(dāng)取r=0.1076時(shí),圖17所示的示例得到的優(yōu)化路徑如圖18所示.
4.4 利用速度Verlet積分法設(shè)計(jì)系統(tǒng)驅(qū)動
確定出振動驅(qū)動系統(tǒng)的最終運(yùn)動路徑后,利用系統(tǒng)內(nèi)部質(zhì)量塊的驅(qū)動頻率比n(ω2/ω1)與系統(tǒng)運(yùn)動的曲率半徑R的關(guān)系,可設(shè)計(jì)系統(tǒng)的驅(qū)動參數(shù).
圖18 優(yōu)化的運(yùn)動路徑Fig.18 Optimized motion path
(1)針對方程(19)用速度Verlet積分法計(jì)算出系統(tǒng)驅(qū)動頻率比n(ω2/ω1)與曲率半徑R的關(guān)系;
(2)找出安全距離r對應(yīng)的驅(qū)動頻率比n,并將各柵格坐標(biāo)變換為真實(shí)坐標(biāo)
(3)令n=1,即ω1=ω2,系統(tǒng)從起始點(diǎn)出發(fā)作直線運(yùn)動,到達(dá)切點(diǎn);
(4)切換頻率比n,系統(tǒng)作圓周運(yùn)動,到達(dá)下一個(gè)切點(diǎn),完成轉(zhuǎn)向;
(5)重復(fù)步驟(3)和(4)直至系統(tǒng)完成最后一次轉(zhuǎn)向;
(6)令n=1,振動驅(qū)動系統(tǒng)運(yùn)動至目標(biāo)點(diǎn).
針對圖18中優(yōu)化的運(yùn)動路徑,安全距離r=0.1076對應(yīng)的驅(qū)動頻率比n=0,首先系統(tǒng)從起始點(diǎn)出發(fā),令n=1,即ω1=ω2=100rad/s,運(yùn)動至第一個(gè)切點(diǎn)處,然后令n=0,即ω1=100rad/s,ω2=0,系統(tǒng)運(yùn)動至第二個(gè)切點(diǎn),完成第一次方向的改變;依次重復(fù)令n=1,n=0直到系統(tǒng)完成最后一次方向改變,最后令n=1,系統(tǒng)到達(dá)目標(biāo)點(diǎn).
設(shè)計(jì)驅(qū)動到達(dá)目標(biāo)位置示例見圖19,從圖19中可見,振動驅(qū)動移動系統(tǒng)通過給出的算法可以成功實(shí)現(xiàn)避障平面運(yùn)動.值得指出的是,本算法可以實(shí)現(xiàn)任意避障路徑的移動.
圖19 設(shè)計(jì)驅(qū)動到達(dá)目標(biāo)位置示例Fig.19 Sample of reaching the target position
4.5 MATLAB仿真實(shí)驗(yàn)
對于更加一般的避障移動,應(yīng)用上述提出的計(jì)算方法和避障策略,并利用MATLAB平臺,通過編程實(shí)現(xiàn)了所提出算法的仿真實(shí)驗(yàn),可以得到系統(tǒng)在障礙物環(huán)境中避障運(yùn)動的仿真實(shí)驗(yàn)動態(tài)圖.圖20為系統(tǒng)運(yùn)動過程的各個(gè)不同時(shí)間段的截圖.仿真虛擬實(shí)驗(yàn)表明,提出的避障計(jì)算方法是可行和有效的.
圖20 仿真實(shí)驗(yàn)截圖Fig.20 Screenshots of simulation experiment
本文研究了一類有兩個(gè)在平行軌道內(nèi)做正弦運(yùn)動的內(nèi)部質(zhì)量塊的振動驅(qū)動移動系統(tǒng)在各向異性黏性摩擦環(huán)境中的運(yùn)動規(guī)律,并提出了使系統(tǒng)完成預(yù)設(shè)軌跡和避障作業(yè)的驅(qū)動設(shè)計(jì)方法.通過對預(yù)設(shè)路徑離散化處理得到振動驅(qū)動移動系統(tǒng)的運(yùn)動軌跡,進(jìn)而提出了切線圓法,通過改變內(nèi)部質(zhì)量塊的驅(qū)動參數(shù),使振動驅(qū)動移動系統(tǒng)沿預(yù)設(shè)路徑運(yùn)動.針對振動驅(qū)動移動系統(tǒng)在障礙物環(huán)境中到達(dá)目標(biāo)位置的問題,提出了結(jié)合柵格法、Floyd算法和最小頂點(diǎn)圓法對運(yùn)動路徑規(guī)劃進(jìn)一步優(yōu)化的計(jì)算方法,通過設(shè)計(jì)內(nèi)部質(zhì)量塊的驅(qū)動參數(shù)實(shí)現(xiàn)了移動系統(tǒng)的避障移動.
本文針對振動驅(qū)動移動系統(tǒng)的研究完成了使移動系統(tǒng)沿預(yù)設(shè)路徑運(yùn)動和在障礙物環(huán)境中工作的可行方法的分析,對振動驅(qū)動機(jī)器人的研究具有重要的意義.后續(xù)將進(jìn)一步進(jìn)行針對文中的避障算法進(jìn)行物理實(shí)驗(yàn).
1 李鐵風(fēng),李國瑞,梁藝鳴等.軟體機(jī)器人結(jié)構(gòu)機(jī)理與驅(qū)動材料研究綜述.力學(xué)學(xué)報(bào),2016,48(4):756-766(Li Tiefeng,Li Guorui, Liang Yiming,et al.Review of materials and structures in soft robotics.Chinese Journal of Theoretical&Applied Mechanics, 2016,48(4):756-766(in Chinese))
2 范紀(jì)華,章定國.基于變形場不同離散方法的柔性機(jī)器人動力學(xué)建模與仿真.力學(xué)學(xué)報(bào),2016,48(4):843-856(Fan Jihua,Zhang Dingguo.Mechanical analysis and calm control of dual-arm space robot for capturing a satellite.Chinese Journal of Theoretical&Applied Mechanics,2016,48(4):843-856(in Chinese))
3 Chernousko FL.Analysis and optimization of the motion of a body controlled by means of a movable internal mass.Journal of Applied Mathematics and Mechanics,2006,70(6):819-842
4 Chernousko FL.Dynamics of a body controlled by internal motions//Proceedings of the IUTAM Symposium on Dynamics and Control of Nonlinear Systems with Uncertainty.Nanjing,China: Springer Netherlands,2007:227-236
5 Fang HB,Xu J.Dynamic analysis and optimization of a three-phase control mode of a mobile system with an internal mass.Journal of Vibration and Control,2011,17(1):19-26
6 Fang HB,Xu J.Dynamics of a mobile system with an internal acceleration-controlled mass in a resistive medium.Journal of Sound and Vibration,2011,330(16):4002-4018
7 Bolotnik NN,Zeidis IM,Zimmermann K,et al.Dynamics of controlled motion of vibration-driven systems.Journal of Computer and Systems Sciences International,2006,45(5):831-840
8 Bolotnik NN,Figurina TY.Optimal control of the rectilinear motion of a rigid body on a rough plane by means of the motion of two internal masses.Journal of Applied Mathematics and Mechanics,2008, 72(2):126-135
9 Volkova LY,Yatsun SF.Simulation of the plane controlled motion of a three-mass vibration system.Journal of Computer and Systems Sciences International,2012,51(6):859-878
10 Zhan X.Xu J,Fang HB.Planar locomotion of a vibration-driven system with two internal masses.Applied Mathematical Modeling, 2015,40(2016):871-885
11 Sobolev NA,Sorokin KS.Experimental investigation of a model of a vibration-driven robot with rotating masses.Journal of Computer and Systems Sciences International,2007,46(5):826-835
12 Zhan X,Xu J.Locomotion analysis of a vibration-driven system with three acceleration-controlled internal masses.Advances in Mechanical Engineering,2015,7(3):1-12
13 Chernousko FL.On the optimal motion of a body with an internal mass in a resistive medium.Journal of Vibration and Control,2008, 14(1-2):197-208
14 Chernousko FL.The optimal periodic motions of a two-mass system in a resistant medium.Journal of Applied Mathematics and Mechanics,2008,72(2):116-125
15 Sergey J,Vyacheslav D,Andrey Y,et al.Modelling of robot’s motion by use of vibration of internal masses//Ceccarelli M,eds.Proceedings of EUCOMES 08.Dordrecht:Springer Netherlands,2008. 263-270
16 Li HY,Katsuhisa F.A pendulum-driven cart via internal force and static friction//Proceedings of the 2005 International Conference on Physics and Control,IEEE,2005:15-17
17 陳祺,占雄,徐鑒.振動驅(qū)動移動機(jī)器人直線運(yùn)動的滑移分岔.力學(xué)學(xué)報(bào),2016,48(4):792-803(Chen Qi,Zhan Xiong,Xu Jian. Sliding bifurcations of rectilinear motion of a three-phase vibrationdriven system subject to coulomb dry friction.Chinese Journal of Theoretical&Applied Mechanics,2016,48(4):792-803(in Chinese))
18 殷蓬勃,占雄,徐鑒.三相驅(qū)動下含兩質(zhì)量塊剛體的平面運(yùn)動.固體力學(xué)學(xué)報(bào),2016,37(5):421-437(Yin Pengbo,Zhan Xiong,Xu Jian.Planar locomotion of a rigid body driven by three-phase motion of two internal masses.Chinese Journal of Solid Mechanics, 2016,37(5):421-437(in Chinese))
19 劉丹丹,張樹有,劉元開等.一種基于特征點(diǎn)識別的曲線離散化方法.中國圖象圖形學(xué)報(bào),2004,9(6):755-759(Liu Dandan,Zhang Shuyou,Liu Yuankai,et al.A curve discretization method based on recognization of feature point.Journal of Image and Graphics, 2004,9(6):755-759(in Chinese))
20 樊宏斌,耿國華,周明全.一種求曲線極小特征點(diǎn)集的算法.西北大學(xué)學(xué)報(bào)自然科學(xué)版,2002,32(2):166-168(Fan Hongbin,Geng Guohua,Zhou Mingquan.An algorithm on searching minim um characteristic pointsofcurve.Journal of Northwest University(Natural Science Edition),2002,32(2):166-168(in Chinese))
21 朱大奇,顏明重.移動機(jī)器人路徑規(guī)劃技術(shù)綜述.控制與決策, 2010,25(7):961-967(Zhu Daqi,Yan Mingzhong.Survey on technology of mobile robot path planning.Control&Decision,2010, 25(7):961-967(in Chinese))
22 張廣林,胡小梅,柴劍飛等.路徑規(guī)劃算法及其應(yīng)用綜述.現(xiàn)代機(jī)械,2011(5):85-90(Zhang Guanglin,Hu Xiaomei,Chai Jianfei,et al.Summary path planning algorithm and its application.Modern Machinery,2011(5):85-90(in Chinese))
23 于紅斌,李孝安.基于柵格法的機(jī)器人快速路徑規(guī)劃.微電子學(xué)與計(jì)算機(jī),2005,22(6):98-100(Yu Hongbin,Li Xiaoan.Fast path planning based on grid model of robot.Microelectronics&Computer,2005,22(6):98-100(in Chinese))
24 朱磊,樊繼壯,趙杰等.基于柵格法的礦難搜索機(jī)器人全局路徑規(guī)劃與局部避障.中南大學(xué)學(xué)報(bào)(自然科學(xué)版),2011,42(11):3421-3428(Zhu Lei,Fan Jizhuang,Zhao Jie,et al.Global path planning and local obstacle avoidance of searching robot in mine disasters based on grid method.Journal of Central South University(Science and Technology),2011,42(11):3421-3428(in Chinese))
25 石為人,王楷.基于Floyd算法的移動機(jī)器人最短路徑規(guī)劃研究.儀器儀表學(xué)報(bào),2009,30(10):2088-2092(Shi Weiren,Wang Kai. Floyd algorithm for the shortest path planning of mobile robot.Chinese Journal of Scientifi Instrument,2009,30(10):2088-2092(in Chinese))
26 李彥剛.機(jī)器人躲避多靜態(tài)障礙物的路徑規(guī)劃模型.自動化與儀器儀表,2013(2):160-163(Li Yangang.Path planning model for robot to avoid multi static obstacles.Automation and Instrumentation,2013(2):160-163(in Chinese))
ANALYSIS ON PLANAR OBSTACLE AVOIDANCE LOCOMOTION OF VIBRATION-DRIVEN SYSTEM1)
Zhang Min Xu Jian2)
(School of Aerospace Engineering and Applied Mechanics,Tongji University,Shanghai200092,China)
In recent years,with the wide application of the industrial robot,the development of the mobile robot has attracted more and more attention.In order to finisthe work accurately in some complex environments,vibrationdriven system has been proposed and researched by scholars.At the presence of anisotropic viscous friction,this paper investigates the motion law of a vibration-driven locomotion system in which two internal masses vibrate sinusoidally in two parallel guides and puts forward a design method to conduct the tasks like obstacle avoiding.Firstly,by using the second-kind Lagrange’s equation,dynamical equations of the system are established;then,the motion law is numerically analyzed,the relationship between the internal drive parameters and the system trajectory and the system velocity are obtained by using the velocity-verlet algorithm;finall,based on the motion law of the vibration-driven locomotion system,the drive design method is proposed to make the system move along a prescribed path and realize the obstacle avoidance.To make the mobile system move along a prescribed path,the motion trajectory of the system could be obtained by curve discretization.Then,by changing the driving parameters of the internal mass block,the system could move along the preset path.In order to make the mobile system reach the goal position in the obstacle environment,anoptimized path planning method based on the grid method,Floyd algorithm and the minimum vertex circle method is proposed,and the optimal motion path of the vibration-driven mobile system is obtained.Finally,obstacle avoiding can be realized through changing the driving parameters of the internal mass block.
vibration-driven locomotion system,planar locomotion,sinusoidal driving,anisotropic viscous friction, driven design,obstacle avoidance
O313
A
10.6052/0459-1879-16-367
2016–12–07收稿,2017–02–22錄用,2017–02–22網(wǎng)絡(luò)版發(fā)表.
1)國家自然科學(xué)基金資助項(xiàng)目(11572224).
2)徐鑒,教授,主要研究方向:非線性動力學(xué).E-mail:xujian@#edu.cn
張敏,徐鑒.振動驅(qū)動移動系統(tǒng)平面避障運(yùn)動分析.力學(xué)學(xué)報(bào),2017,49(2):397-409
Zhang Min,Xu Jian.Analysis on planar obstacle avoidance locomotion of vibration-driven system.Chinese Journal of Theoretical and Applied Mechanics,2017,49(2):397-409