差動驅動機器人的一種航跡推算定位方法

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

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

0 引 言

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

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

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

1 幾何推算方法原理

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

圖1 微分和幾何推算原理對比

幾何推算法采用圓弧PiP′i+1來代替直線實際機器人運動的時候,其軌跡是由很多不同半徑不同圓心的小段圓弧組成。顯然,圓弧線PiP′i+1更加接近曲線 PiPi+1。因此,采用圓弧段代替行走的曲線段來推算機器人軌跡更加符合實際。圖2所示為在速度測量無誤差下行走圓弧路線積分推算和幾何推算的定位結果對比。

圖2 測量無誤差沿圓弧行走定位對比

2 幾何推算法推算過程

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

由圓周運動公式可知：

其中,rn是Tn時刻對應轉向半徑,即轉向圓心o′到A輪的距離;H為輪子A、B間距的1/2。設Tn時刻轉向圓心 o′坐標為o′(xo,yo),由于o′點在A、B輪的軸線上,o′到 A輪的距離為rn,即可以解出轉向圓心o′(xo,yo)點的坐標值。

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

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

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

于是有：

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

通過坐標線性變換,可得到A在Tn+1時刻的 坐標An+1(xAn+1,yAn+1)為：

同理,求出點B在Tn+1時刻坐標Bn+1(xBn+1,yBn+1)為：

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

3 仿真實驗

3.1 計算機仿真驗證原理

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

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

機器人實際行走軌跡是一個很復雜的模型,為了簡化計算,同時又能驗證本文闡述方法的可行性,本文設定機器人的中心D沿著特定的幾種曲線運動,方法如下：

(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)通過曲線y=f(x)求出其曲率半徑,即相對于轉向圓心 o′的轉彎半徑r,于是 ωn=,從而求出機器人 A、B輪在Tn時刻采樣的精確線速度值vAn、vBn。

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

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

3.3 仿真實驗的計算機環(huán)境

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

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

3.4 仿真推算軌跡與理論擬定軌跡對比

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

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

將真值加上一個5%隨機誤差,則可以視為測量值,仿真結果如圖3a所示。

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

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

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

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

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

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

3.5 仿真分析

圓弧、橢圓弧和正弦曲線段能組合機器人實際運動的所有可能的曲線,幾何推算法能對這3種情況推算定位,所以幾何推算法能對機器人的任何運動情況進行跟蹤和定位。

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

4 結束語

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

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

[1]徐則中,莊燕濱.移動機器人定位方法對比研究[J].系統(tǒng)仿真學報,2009,21(7)：1891-1893.

[2]陳永光,李修和.基于信號強度的室內(nèi)定位技術[J].電子學報,2004,32(9)：1456-1457.

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

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

[5]柳長安,李國棟,劉春陽.差動驅動式移動機器人的運動規(guī)劃[J].哈爾濱工業(yè)大學學報,2003,35(9)：1095-1096.

[6]吳克河,李 為,柳長安,等.雙輪驅動式移動機器人動力學控制[J].宇航學報,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.