機(jī)器人定位中的航跡推算

1、航跡推算是一種使用最廣泛的定位手段，特別適于短時(shí)短距離定位，精度很高。對于長時(shí)間 運(yùn)動的，可以應(yīng)用其他的傳感器配合相關(guān)的定位算法進(jìn)行校正。利用陀螺儀和加速度計(jì)分別測量出旋轉(zhuǎn)率和加速度，再進(jìn)行積分，從而可求出走過的距離和 航向的變化，進(jìn)而分析出機(jī)器人的位置和姿態(tài)。超聲波傳感器可用于測距，從而探測路標(biāo)（設(shè)置為室內(nèi)墻壁或天花板），計(jì)算位置，來糾正 陀螺儀和編碼器的定位誤差。(X(0)>Y(0)圖1 航跡推算算法（DR）的原理其推導(dǎo)的一般方程為:2、（/）=2、（萬一1 > +5 人(1 1 ) + 1)2s,一y = A3 sin(0 + A6 )COS(9( & - 1 )Z

2、J y ,7,5及(4一 1 )+ S/.(一)sin(9a 1)z > A. SR(k > SL(k1 )(pg=4(k ) HT)其中柒），到上）為車輛在k時(shí)刻的位置以及方向，s“g1）,5：晨一1）分別為車輛右輪和左輪在旌1時(shí)刻到k時(shí)刻時(shí)間間隔所走過的距離“為車輛 的輪距。因?yàn)槲覀?用了陀螺儀可以測出轉(zhuǎn)過的角度，所以沒有必要用上面的公式，但上式可用于修正陀螺儀測 出的角度值。A0注：必然要求陀螺安裝在機(jī)器人不活動的部件上，并且陀螺的安裝只能與車體固 連在一起。微機(jī)械陀螺作為重要的傳感器，它的輸出信號是一個(gè)與轉(zhuǎn)動角速率基本成線 性關(guān)系的模擬電壓值，通過采集其輸出的模擬電壓值，經(jīng)

3、過AD轉(zhuǎn)換為數(shù)字信號, 對轉(zhuǎn)換完的信號進(jìn)行標(biāo)度變換得到其轉(zhuǎn)動的角速率，再積分即可得到角度值。根據(jù)以上假設(shè)，車體被簡化成了一個(gè)具有兩個(gè)平移自由度（縱向和側(cè)向）和一 個(gè)轉(zhuǎn)動自由度（橫擺）的單質(zhì)量剛體。機(jī)器人在全局坐標(biāo)系中的姿態(tài)如圖所示。其中，坐標(biāo)系。TY為全局坐標(biāo)系， 夕點(diǎn)為機(jī)器人上的一個(gè)參考點(diǎn)，坐標(biāo)系O'XP為以P點(diǎn)為原點(diǎn)的車體固連坐標(biāo)系, X，軸與1軸的夾角為8。機(jī)器人的姿態(tài)（Posture）可以用點(diǎn)在全局坐標(biāo)系中 的坐標(biāo)（x, y）和6表示，即可用三維矢量4 = （x,y,6）7'表示。同時(shí)，還可以得 到由全局坐標(biāo)系到車體固連坐標(biāo)系的坐標(biāo)旋轉(zhuǎn)矩陣如下：'cos

4、4; 一 sin 6 O'R(6)= sing cos 6 0< 001，本系統(tǒng)在設(shè)計(jì)時(shí)主要參考DR航位推算m，DR的圖解如圖。其原理是以地球表面某點(diǎn)作為當(dāng)?shù)刈鴺?biāo)系的原點(diǎn)，利用里程計(jì)輸出的距離信 息和特定傳感器輸出的角度信息，計(jì)算確定自主車當(dāng)前的位置。其推導(dǎo)的一般方程為：X （） = X （0） + 1。 cos（9（i）k-ly a）=y（o）+gjo（i）sin（*）"1伏攵） = 6（0）+.（，）其中：x（o）,y（o）為初始時(shí)刻自主車所在位置，。與。為從i-i時(shí) 刻到i時(shí)刻自主車行駛距離和方位角。現(xiàn)在航位推算方法很多，但是其本質(zhì)都是根據(jù)傳感器得到的數(shù)據(jù)，或者直

5、接給出 方位角的信息，或者推算出方位角的值，然后在代入、式，求出其相對位置。碼盤的距離計(jì)算：將兩個(gè)光電編碼器分別安裝在車的左右主動輪上，光電編碼器同步轉(zhuǎn)動。設(shè) 車輪直徑D,光電編碼器線數(shù)（即每轉(zhuǎn)一圈輸出的脈沖數(shù)）是P, 時(shí)間內(nèi)光電 編碼器輸出脈沖數(shù)是、，車輪運(yùn)行距離是A5 （左輪運(yùn)行距離$一右輪運(yùn)行距離AS&）,車輪運(yùn)行速度是口（左輪運(yùn)行速度匕，右輪運(yùn)行速度A。），且兩輪中心距離為D=2L。那么，Ai3 = AS Z /Ar = （N/ / P）mD / Ar< A喙=ASk / Ar = （Nk / P）乃.D /Av = $ / Ar = （> + A。）/ 2 = （

6、N/ + NQ / 2 / P）wD / Z通過碼盤得到的只是機(jī)器小車左右輪的轉(zhuǎn)速，還必須知道兩個(gè)輪子的周長 （或直徑），才能得到機(jī)器小車的行進(jìn)速度。實(shí)際操作中發(fā)現(xiàn)，直接測量輪子的 半徑或直徑，誤差會很大。有兩種方法可以測量輪子的周長：（1）直接法，可以 讓機(jī)器人小車在地上行走，車輪轉(zhuǎn)動一周（或更多），再測量機(jī)器小車行走的距 離；（2）間接法，通過DSP工作在仿真模式，讓機(jī)器小車在地上行走一段距離， 測量這段距離，同時(shí)在CCS中讀取碼盤的反饋值，（或?qū)⒊绦驘浀紻SP后通過 LED顯示得到碼盤的反饋值）。以上兩種方法均可以相對減小測量誤差，但推薦 采用第二種方法，因?yàn)榈诙N方法可以同時(shí)減小或消

7、除碼盤測量的誤差。不管采 用哪一種方法，都應(yīng)該加大測量距離，同時(shí)通過多次測量取平均，才能達(dá)到較高 的精度。運(yùn)用上述方法可以得到機(jī)器小車左輪前進(jìn)1m的碼盤反饋值和右輪前進(jìn)1m的碼盤反饋值%刖（或者左輪的周長Q和右輪的周長CQ。在機(jī)器小車行走的過程中，實(shí)時(shí)采集碼盤信息，可以分別得到左右輪的行進(jìn) 速度。而實(shí)際系統(tǒng)設(shè)計(jì)中需要車體的速度，可以將左右輪的速度取平均，得到兩 輪連線中心的速度即車體的速度V,這樣可以提高精度，更準(zhǔn)確的得到車體行進(jìn) 的速度。則（）式變?yōu)辄c(diǎn)= NJNl。=Nr/Nr0（）n =（4 +。）/2 = （N, / Nlo + NJ NQI2到此，已經(jīng)實(shí)現(xiàn)距離信息的獲取。陀螺的角度計(jì)算

8、：陀螺的輸出信號不是直接與當(dāng)前的角速率對應(yīng)，而是輸出電壓的變化AV與 角速率成比例關(guān)系。因此需要一個(gè)靜態(tài)基準(zhǔn)作為參比，實(shí)際操作中發(fā)現(xiàn)用實(shí)驗(yàn)測 定的靜態(tài)基準(zhǔn)作為參比電壓，短期內(nèi)精度還可以滿足要求。但過一段時(shí)間，陀螺 的靜態(tài)基點(diǎn)會漂移，這樣會造成很大的誤差。所以設(shè)計(jì)時(shí)，實(shí)時(shí)采集陀螺工作的 靜態(tài)基點(diǎn)，作為參比。由于比賽前機(jī)器小車擺放到位到比賽開始還有1分鐘的時(shí) 間，可以利用這段時(shí)間采集陀螺的靜態(tài)信號，取平均后作為陀螺當(dāng)時(shí)的靜態(tài)基點(diǎn)。 并且20分鐘內(nèi)陀螺的靜態(tài)基點(diǎn)基本無漂移，所以賽前一分鐘內(nèi)采集的數(shù)據(jù)可以 作為整場比賽的靜態(tài)基點(diǎn)匕。機(jī)器小車行進(jìn)過程中不斷采集陀螺的輸出信號V,則對應(yīng)當(dāng)前 的角速率值/（

9、x） = k*AV,其中k是陀螺的比例因子。對角速率的積分就可以得到角度值，常用積分公式為梯形公式。設(shè)已知。時(shí)刻的初始角度為q, x時(shí)刻對應(yīng)的速率值為/（X）,則可求出/，時(shí)刻的角度名(bfa)。構(gòu)造以。，為結(jié)點(diǎn)的線性插值多項(xiàng)式于(x) =f(a) +f(b)a-bb-a則從。時(shí)刻到匕時(shí)刻角度的變化量為：rb戈一Z?X - CI8= "皿=-/(«) + -f(b) dxJab-a+ (x-a)dxa-bJab-aJa= /m)xl(-b)2 + f(b) a-b 2b-a 2= L(b-a)f(a) + f(b)可得：& = q + 夕=a+J(-，)/()+/例

10、 乙()()()這樣，在初始角度q的基礎(chǔ)上，每個(gè)采樣周期都計(jì)算一次更新角度為,這 個(gè)名相當(dāng)于下一個(gè)周期的初始坐標(biāo)a，如此循環(huán)，就可以知道任意時(shí)刻機(jī)器人 的角度變化及姿態(tài)。但是機(jī)器人在運(yùn)動的過程中，角速率不斷積分，得到的角度會大于360°或者 小于0° ,這樣會給后續(xù)的計(jì)算和路徑規(guī)劃帶來很大麻煩。因此，在實(shí)際操作中, 將得到的角度規(guī)范到0, 360°）的區(qū)間內(nèi)，即6日0,360）。要知道機(jī)器人的位置坐標(biāo)，首先需要建立x-y二維坐標(biāo)系。由于根據(jù)陀螺和 碼盤計(jì)算的位置坐標(biāo)和姿態(tài)都是相對于車體的起始位置和姿態(tài)，即是在車體坐標(biāo) 系（以車體的參考點(diǎn)為坐標(biāo)原點(diǎn)，x軸方向與全場坐

11、標(biāo)系的x軸方向大概一致， 誤差是由擺放的角度誤差決定的）中，這樣如果比賽策略需求車體的起始擺放位 置變化，那么整個(gè)坐標(biāo)系都會有變化，場上關(guān)鍵點(diǎn)的坐標(biāo)也會變化。這樣通用性 很差，而且不利于操作。所以建立一個(gè)全場坐標(biāo)系很有必要，它不依賴車體的起 始位置和姿態(tài)，而是通過改變車體的起始坐標(biāo)和姿態(tài)角來實(shí)現(xiàn)不同的擺車需求。圖4. 8機(jī)器人運(yùn)動原理圖建立圖所示的直角坐標(biāo)系。機(jī)器人從4匕），,8）點(diǎn)出發(fā)，經(jīng)時(shí)間/沿圓弧走 到即4（x + Ax,y + Ay,e + A8）,其中x,y力分別表示機(jī)器人在起始 位置的橫、縱坐標(biāo)值以及前進(jìn)方向與橫坐標(biāo)的夾角，逆時(shí)針方向?yàn)檎姆较蚪牵?，乂小。表示在7時(shí)間內(nèi)機(jī)器人的橫

12、、縱坐標(biāo)和角度的增加量，AS表示A點(diǎn) 到/V點(diǎn)的瓠長。于是以,），可由如下的公式計(jì)算得到:Ax = AA e cos(6 +a n=2R sin() cos(6 + ) 22e=A5 sin()/ cos(6 + ) 222Ay = AA '>sin( +=2R sin(-) sin( 0 +e22AC r ./£、 /.，C=AS sin()/ sin( 0 + 22其中，AA1表示A點(diǎn)到/T點(diǎn)的距離。當(dāng)機(jī)器人走直線時(shí)si吟吟=。，則式，可寫成:eAx = A5 cos(6 + ；-)Ay = $ sin(6 + 7-)由于時(shí)間間隔&很短，線段A4'的長度近似等于圓弧長度，設(shè)，時(shí)間內(nèi)編碼盤走過的距離為/,貝設(shè)q為上次采樣的角度，4為當(dāng)前的角度，則 3=小8。通過計(jì)算加時(shí)間內(nèi)機(jī)器人的位置變化量人、,），進(jìn)行累加就可以求 出機(jī)器人在整個(gè)場地上的位置。0x' = x +Ar = x + 44'cos(6 + )z/。+ (。+ 八。)、()()=X + / c