張 利, 潘承毅, 劉征宇, 徐 娟, 黃業(yè)偉

(1.合肥工業(yè)大學(xué) 機(jī)械與汽車(chē)工程學(xué)院,安徽 合肥 230009;2.合肥工業(yè)大學(xué) 計(jì)算機(jī)與信息學(xué)院,安徽 合肥 230009)

0 引 言

移動(dòng)機(jī)器人導(dǎo)航的關(guān)鍵問(wèn)題是定位,在解決定位問(wèn)題中遇到的不確定性問(wèn)題的方法各有優(yōu)缺點(diǎn)[1]?；诮邮招盘?hào)強(qiáng)度、紅外、藍(lán)牙、超寬帶等定位技術(shù)都是基于到達(dá)時(shí)間 TOA、到達(dá)時(shí)間差TDOA和到達(dá)方向AOA這類信息,這些技術(shù)的定位結(jié)果只有在通視信號(hào)占支配地位的情況下才是可靠的[2]。輔助定位就是為了彌補(bǔ)這些技術(shù)不足而提出來(lái)的,移動(dòng)機(jī)器人的輔助定位技術(shù)主要有慣性定位、航跡推算定位。輔助定位具有短時(shí)間內(nèi)局部高精度定位特點(diǎn),結(jié)合常規(guī)定位[3],就能比較好地解決文獻(xiàn)[2]中的室內(nèi)非視距定位誤差大的問(wèn)題。從成本和實(shí)現(xiàn)的角度看,服務(wù)機(jī)器人的輔助定位一般采用航跡推算實(shí)現(xiàn)。

基于雙輪差動(dòng)驅(qū)動(dòng)的航跡推算方法是建立其運(yùn)動(dòng)學(xué)微分方程[4],然后對(duì)方程進(jìn)行數(shù)值積分迭代推算[4-9](簡(jiǎn)稱為積分推算),從而實(shí)現(xiàn)對(duì)機(jī)器人的跟蹤和定位。

積分推算的實(shí)質(zhì)是用一小段直線代替機(jī)器人的一小段軌跡,在采樣頻率和采樣精度有限下積分推算誤差較大。為了進(jìn)一步提高推算精度,本文從幾何和運(yùn)動(dòng)學(xué)的角度提出用小段的圓弧來(lái)逼近輪椅實(shí)際行走的軌跡,在采樣數(shù)據(jù)和采樣精度不變下能有效地提高推算定位的精度。為了方便敘述,以下稱該方法為幾何推算方法。

1 幾何推算方法原理

雙輪差動(dòng)驅(qū)動(dòng)機(jī)器人轉(zhuǎn)向的原理是：同軸的左、右2個(gè)主動(dòng)輪轉(zhuǎn)動(dòng)角速度不同,當(dāng)左輪轉(zhuǎn)速nL大于右輪轉(zhuǎn)速nR時(shí),機(jī)器人就開(kāi)始向右轉(zhuǎn);當(dāng)nL＜nR時(shí),機(jī)器人就開(kāi)始向左轉(zhuǎn);當(dāng)左右轉(zhuǎn)速相同時(shí)則往正前方運(yùn)動(dòng),2個(gè)驅(qū)動(dòng)輪的運(yùn)動(dòng)軌跡滿足同心圓的規(guī)律[4,7]。幾何推算和積分推算都建立在這個(gè)原理上,積分推算用直線代替實(shí)際的曲線,如圖1所示,Pi為 A、B輪軸線中點(diǎn),o′為A 、B輪轉(zhuǎn)彎時(shí)的轉(zhuǎn)向圓心,積分推算方法用直線來(lái)逼近實(shí)際路線PiPi+1。

圖1 微分和幾何推算原理對(duì)比

幾何推算法采用圓弧PiP′i+1來(lái)代替直線實(shí)際機(jī)器人運(yùn)動(dòng)的時(shí)候,其軌跡是由很多不同半徑不同圓心的小段圓弧組成。顯然,圓弧線PiP′i+1更加接近曲線 PiPi+1。因此,采用圓弧段代替行走的曲線段來(lái)推算機(jī)器人軌跡更加符合實(shí)際。圖2所示為在速度測(cè)量無(wú)誤差下行走圓弧路線積分推算和幾何推算的定位結(jié)果對(duì)比。

圖2 測(cè)量無(wú)誤差沿圓弧行走定位對(duì)比

2 幾何推算法推算過(guò)程

幾何推算法推算下一個(gè)時(shí)刻的位置,需要知道當(dāng)前左、右輪的位置及其對(duì)應(yīng)的角速度。通過(guò)角速度ωL和ωR求出機(jī)器人的轉(zhuǎn)向半徑rm[4-7]和轉(zhuǎn)向圓心o′點(diǎn),采樣時(shí)間 Ti內(nèi)可以近似 ωL和 ωR的值不變,利用幾何關(guān)系就可以求出下一個(gè)時(shí)刻左、右輪的位置。記左輪為 A,右輪為B,確定基于圓弧代替曲線的幾何推算定位過(guò)程。

由圓周運(yùn)動(dòng)公式可知：

其中,rn是Tn時(shí)刻對(duì)應(yīng)轉(zhuǎn)向半徑,即轉(zhuǎn)向圓心o′到A輪的距離;H為輪子A、B間距的1/2。設(shè)Tn時(shí)刻轉(zhuǎn)向圓心 o′坐標(biāo)為o′(xo,yo),由于o′點(diǎn)在A、B輪的軸線上,o′到 A輪的距離為rn,即可以解出轉(zhuǎn)向圓心o′(xo,yo)點(diǎn)的坐標(biāo)值。

平移坐標(biāo)系原點(diǎn)o(0,0)到轉(zhuǎn)向圓心o′(xo,yo),此時(shí)的坐標(biāo)系記為 o′-x′y′,并求出在 o′-x′y′坐標(biāo)系下的An(xAn,yAn)、Bn(xBn,yBn)的坐標(biāo)A′n(x′An,y′An)、B′n(x′Bn,y′Bn)。

將o′-x′y′坐標(biāo)系旋轉(zhuǎn) θ角度,使得 o′-x′y′坐標(biāo)系的x′軸與A、B輪子的軸線重合;旋轉(zhuǎn)后的坐標(biāo)系統(tǒng)記為o″-x″y″,其上點(diǎn)坐標(biāo)記為(x″,y″)。其中θ值由(2)式確定：

求出 A′n(x′An,y′An)、B′n(x′Bn,y′Bn)在 o″-x″y″下的坐標(biāo) A″n(x″An,y″An)和 B″n(x″Bn,y″Bn)。其中坐標(biāo)變換矩陣C為：

于是有：

在采樣時(shí)間 T內(nèi)視ωn不變,則在這段時(shí)間內(nèi) A、B點(diǎn)相對(duì)于o′轉(zhuǎn)的角度ω=ωnT,于是求出Tn+1時(shí)刻 A 和 B 在 o″-x″y″坐標(biāo)系下的坐標(biāo)(x″An+1,y″An+1)和 (x″Bn+1,y″Bn+1)。

通過(guò)坐標(biāo)線性變換,可得到A在Tn+1時(shí)刻的 坐標(biāo)An+1(xAn+1,yAn+1)為：

同理,求出點(diǎn)B在Tn+1時(shí)刻坐標(biāo)Bn+1(xBn+1,yBn+1)為：

在已知A、B輪初始位置情況下,通過(guò)不斷采樣測(cè)量A、B的角速度ωA和ωB,將角速度乘以輪子半徑R即可得兩輪的線速度vAn、vBn,通過(guò)(1)式和圓周運(yùn)動(dòng)幾何關(guān)系,可以求出xo和yo,代入(3)式和(4)式迭代計(jì)算,就可以逐步地推算出 A、B輪運(yùn)動(dòng)軌跡,從而實(shí)現(xiàn)對(duì)機(jī)器人定位。

3 仿真實(shí)驗(yàn)

3.1 計(jì)算機(jī)仿真驗(yàn)證原理

仿真實(shí)驗(yàn)首先要正確地獲取2種數(shù)據(jù),即機(jī)器人真實(shí)的行走軌跡、采樣時(shí)刻A、B輪子的線速度。然后把A、B的各個(gè)采樣點(diǎn)的角速度值ωA、ωB和轉(zhuǎn)向圓心坐標(biāo)(xo,yo)代人(3)式、(4)式,通過(guò)迭代計(jì)算即可推算出A、B輪的軌跡,再將推算的軌跡與理論擬定的軌跡進(jìn)行比較。

3.2 仿真方法和數(shù)據(jù)vAn、vBn的獲得

機(jī)器人實(shí)際行走軌跡是一個(gè)很復(fù)雜的模型,為了簡(jiǎn)化計(jì)算,同時(shí)又能驗(yàn)證本文闡述方法的可行性,本文設(shè)定機(jī)器人的中心D沿著特定的幾種曲線運(yùn)動(dòng),方法如下：

(1)擬定行走曲線y=f(x),其中f(x)為光滑曲線。

(2)將y=f(x)化為參數(shù)方程 x=x(t)和y=y(t)。

(3)求出行走路線 x、y軸方向上的速度分量,其中 vx=x′(t),vy=y′(t)。

(4)通過(guò)曲線y=f(x)求出其曲率半徑,即相對(duì)于轉(zhuǎn)向圓心 o′的轉(zhuǎn)彎半徑r,于是 ωn=,從而求出機(jī)器人 A、B輪在Tn時(shí)刻采樣的精確線速度值vAn、vBn。

(5)將輪子線速度值vAn、vBn化為A、B輪的角速度值ωA、ωB,然后加上一個(gè)隨機(jī)誤差值error作為仿真測(cè)量值。

(6)將仿真測(cè)量值代入(3)式和(4)式,分別推算出A、B輪的軌跡,然后將擬定的軌跡與仿真計(jì)算出的軌跡進(jìn)行對(duì)比和分析。

3.3 仿真實(shí)驗(yàn)的計(jì)算機(jī)環(huán)境

本文的仿真實(shí)驗(yàn)是在Windows操作系統(tǒng)上的VC++和Matlab上進(jìn)行的,將3.2的幾何推算步驟和積分推算方法轉(zhuǎn)化為C++代碼并在VC++上進(jìn)行編譯。

仿真時(shí)用同一份模擬測(cè)量數(shù)據(jù)(準(zhǔn)確值+隨機(jī)誤差值)分別輸入幾何推算仿真程序和積分推算仿真程序,并將2種推算的數(shù)據(jù)保存在text文件中,然后把text文件的數(shù)據(jù)導(dǎo)入Matlab進(jìn)行繪圖和比較分析。

3.4 仿真推算軌跡與理論擬定軌跡對(duì)比

為了讓仿真實(shí)驗(yàn)行走的路線有代表性,本文選擇了圓弧、橢圓弧和sin x曲線段進(jìn)行軌跡跟蹤仿真。按照3.2的方法,分別對(duì)3種曲線仿真。

(1)沿著圓弧曲線行走的仿真。圓弧是差動(dòng)驅(qū)動(dòng)機(jī)器人最常見(jiàn)的行走軌跡,對(duì)沿圓弧行走定位的精度直接影響該推算方法是否有應(yīng)用價(jià)值。本文闡述的幾何推算法是基于機(jī)器人行走軌跡的圓弧段組成的理論,因此,理論上在沒(méi)有速度測(cè)量誤差的情況下沿著圓弧行走,幾何推算的航跡與實(shí)際行走的軌跡應(yīng)保持一致。圖2為采樣無(wú)誤差、采樣步數(shù)n=20時(shí),輪椅沿著半徑為40的半圓行走時(shí),幾何推算和積分推算軌跡對(duì)比圖。顯然,幾何推算與理論擬定路線基本吻合,而積分推算產(chǎn)生了5%～12.5%的誤差。

將真值加上一個(gè)5%隨機(jī)誤差,則可以視為測(cè)量值,仿真結(jié)果如圖3a所示。

在采樣精度不變的情況下,增加采樣頻率,能有效地降低定位誤差。圖3b所示是測(cè)量隨機(jī)誤差為5%,采樣步數(shù)分別為200和2000時(shí)的幾何推算仿真結(jié)果。

(2)沿著半橢圓曲線行走仿真。機(jī)器人沿著圓弧運(yùn)動(dòng)比較常見(jiàn),圖3證明幾何推算法能跟蹤機(jī)器人行走圓心固定的圓弧,但實(shí)際運(yùn)動(dòng)中機(jī)器人是行走不同圓心不同曲率的小段圓弧組成的曲線。橢圓曲線各點(diǎn)的曲率和曲心都不同,正好能驗(yàn)證這種情況。圖4a所示是采樣步數(shù)n=20,測(cè)量隨機(jī)誤差為5%時(shí)幾何推算和積分推算2種方法的對(duì)比,圖4b所示是采樣步數(shù)n=400,測(cè)量隨機(jī)誤差為5%時(shí)的幾何推算軌跡。

圖3 測(cè)量誤差為5%,沿著圓弧行走仿真

圖4 測(cè)量誤差為5%,沿著橢圓弧行走仿真

(3)沿著正弦曲線sin x行走的仿真。針對(duì)很復(fù)雜的運(yùn)動(dòng)模式,如速度、加速度、轉(zhuǎn)向角速度都不斷變化(實(shí)際很少甚至不可能出現(xiàn)),研究幾何推算法是否有效。針對(duì)這種情況,本文擬定機(jī)器人沿著sin x曲線運(yùn)動(dòng),機(jī)器人中心D點(diǎn)的速度在x軸上的分量恒為1個(gè)速度單位。由幾何關(guān)系可知D點(diǎn)的速度、加速度和轉(zhuǎn)向角速度都在不斷變化。在采樣誤差為5%內(nèi),n=20時(shí)幾何推算和積分推算仿真結(jié)果如圖5a所示,n=50時(shí)幾何推算的結(jié)果如圖5b所示。

圖5 采樣誤差為5%沿著正弦曲線行走仿真

3.5 仿真分析

圓弧、橢圓弧和正弦曲線段能組合機(jī)器人實(shí)際運(yùn)動(dòng)的所有可能的曲線,幾何推算法能對(duì)這3種情況推算定位,所以幾何推算法能對(duì)機(jī)器人的任何運(yùn)動(dòng)情況進(jìn)行跟蹤和定位。

本文幾何推算法利用圓弧代替機(jī)器人的運(yùn)動(dòng)曲線,比積分推算定位用直線代替曲線的精度高。當(dāng)然對(duì)于隨機(jī)誤差比較大的情況,幾何推算法并不一定比積分推算的結(jié)果更好。由圖4a可知,2種方法的結(jié)果差強(qiáng)人意,對(duì)于更大的隨機(jī)誤差,哪一種定位效果更好取決于隨機(jī)誤差的分布。從仿真實(shí)驗(yàn)可知,本文闡述的推算定位方法,速度采樣最大誤差不能偏離真值的5%,如果超過(guò)5%定位的精度情況不確定。在采樣精度不變且誤差小于5%的情況下,提高采樣頻率能有效地提高定位精度。

4 結(jié)束語(yǔ)

本文從運(yùn)動(dòng)和幾何的角度,推導(dǎo)出一種新的基于雙輪差動(dòng)驅(qū)動(dòng)的航跡推算方法。在采樣精度和采樣步數(shù)固定的情況下,該方法的推算定位精度比積分推算定位方法有較大地提高。在測(cè)量精度有限的情況下,通過(guò)提高采樣頻率可以有效地提高定位精度。和其它航跡推算定位一樣,本文方法的誤差積累無(wú)法通過(guò)其自身算法的改進(jìn)來(lái)消除。同時(shí)對(duì)于其它意外的情況,如輪子打滑、轉(zhuǎn)彎離心力所產(chǎn)生的偏移誤差,也無(wú)法克服。當(dāng)誤差累積到一定量的時(shí)候,必須通過(guò)其它方法將累積誤差消除,否則以后的推算定位將無(wú)效或失去意義。

一般情況下,本文論述的方法具有局部短期內(nèi)高精度定位能力。將此法結(jié)合其它定位,如室內(nèi)標(biāo)簽定位、超聲波定位、UWB定位等,將會(huì)達(dá)到很好的定位效果。

[1]徐則中,莊燕濱.移動(dòng)機(jī)器人定位方法對(duì)比研究[J].系統(tǒng)仿真學(xué)報(bào),2009,21(7)：1891-1893.

[2]陳永光,李修和.基于信號(hào)強(qiáng)度的室內(nèi)定位技術(shù)[J].電子學(xué)報(bào),2004,32(9)：1456-1457.

[3]鄭利龍,曹志剛.GPS組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合[J].電子學(xué)報(bào),2002,30(9)：1384-1385.

[4]徐 德,鄒 偉.室內(nèi)移動(dòng)式服務(wù)機(jī)器人的感知、定位與控制[M].北京：科學(xué)出版社,2008：237-238.

[5]柳長(zhǎng)安,李國(guó)棟,劉春陽(yáng).差動(dòng)驅(qū)動(dòng)式移動(dòng)機(jī)器人的運(yùn)動(dòng)規(guī)劃[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2003,35(9)：1095-1096.

[6]吳克河,李 為,柳長(zhǎng)安,等.雙輪驅(qū)動(dòng)式移動(dòng)機(jī)器人動(dòng)力學(xué)控制[J].宇航學(xué)報(bào),2006,27(2)：272-274.

[7]Han S M,Lee K W.M obile robot navigation using circular path planning algorithm[C]//International Conference on Control,Automation and Sy stems,Seoul,Korea,2008：2082-2084.

[8]Chitsaz H,La Valle S M.Minmum wheel-rotation path for differential-drive mobile robots[C]//International Conference on Robotics and Automation,Roma,Itly,2007：2718-2719.

[9]Bobyr S,Lumelsky V.Control of dynamics and sensor based motion planning for a differential drive robot[C]//Proceedings of the IEEE International Conference on Robotics&Automation,Detroit,Michigan.1999：159-161.