1、航跡推算是一種使用最廣泛的定位手段，特別適于短時短距離定位，精度很高。對于長時間 運動的，可以應用其他的傳感器配合相關的定位算法進行校正。利用陀螺儀和加速度計分別測量出旋轉率和加速度，再進行積分，從而可求出走過的距離和 航向的變化，進而分析出機器人的位置和姿態。超聲波傳感器可用于測距，從而探測路標（設置為室內墻壁或天花板），計算位置，來糾正 陀螺儀和編碼器的定位誤差。(X(0)>Y(0)圖1 航跡推算算法（DR）的原理其推導的一般方程為: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時刻的位置以及方向，s“g1）,5：晨一1）分別為車輛右輪和左輪在旌1時刻到k時刻時間間隔所走過的距離“為車輛 的輪距。因為我們 用了陀螺儀可以測出轉過的角度，所以沒有必要用上面的公式，但上式可用于修正陀螺儀測 出的角度值。A0注：必然要求陀螺安裝在機器人不活動的部件上，并且陀螺的安裝只能與車體固 連在一起。微機械陀螺作為重要的傳感器，它的輸出信號是一個與轉動角速率基本成線 性關系的模擬電壓值，通過采集其輸出的模擬電壓值，經

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

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

5、接給出 方位角的信息，或者推算出方位角的值，然后在代入、式，求出其相對位置。碼盤的距離計算：將兩個光電編碼器分別安裝在車的左右主動輪上，光電編碼器同步轉動。設 車輪直徑D,光電編碼器線數（即每轉一圈輸出的脈沖數）是P, 時間內光電 編碼器輸出脈沖數是、，車輪運行距離是A5 （左輪運行距離$一右輪運行距離AS&）,車輪運行速度是口（左輪運行速度匕，右輪運行速度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通過碼盤得到的只是機器小車左右輪的轉速，還必須知道兩個輪子的周長 （或直徑），才能得到機器小車的行進速度。實際操作中發現，直接測量輪子的 半徑或直徑，誤差會很大。有兩種方法可以測量輪子的周長：（1）直接法，可以 讓機器人小車在地上行走，車輪轉動一周（或更多），再測量機器小車行走的距 離；（2）間接法，通過DSP工作在仿真模式，讓機器小車在地上行走一段距離， 測量這段距離，同時在CCS中讀取碼盤的反饋值，（或將程序燒錄到DSP后通過 LED顯示得到碼盤的反饋值）。以上兩種方法均可以相對減小測量誤差，但推薦 采用第二種方法，因為第二種方法可以同時減小或消

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

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

9、x） = k*AV,其中k是陀螺的比例因子。對角速率的積分就可以得到角度值，常用積分公式為梯形公式。設已知。時刻的初始角度為q, x時刻對應的速率值為/（X）,則可求出/，時刻的角度名(bfa)。構造以。，為結點的線性插值多項式于(x) =f(a) +f(b)a-bb-a則從。時刻到匕時刻角度的變化量為：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的基礎上，每個采樣周期都計算一次更新角度為,這 個名相當于下一個周期的初始坐標a，如此循環，就可以知道任意時刻機器人 的角度變化及姿態。但是機器人在運動的過程中，角速率不斷積分，得到的角度會大于360°或者 小于0° ,這樣會給后續的計算和路徑規劃帶來很大麻煩。因此，在實際操作中, 將得到的角度規范到0, 360°）的區間內，即6日0,360）。要知道機器人的位置坐標，首先需要建立x-y二維坐標系。由于根據陀螺和 碼盤計算的位置坐標和姿態都是相對于車體的起始位置和姿態，即是在車體坐標 系（以車體的參考點為坐標原點，x軸方向與全場坐

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

12、、縱坐標和角度的增加量，AS表示A點 到/V點的瓠長。于是以,），可由如下的公式計算得到: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點到/T點的距離。當機器人走直線時si吟吟=。，則式，可寫成:eAx = A5 cos(6 + ；-)Ay = $ sin(6 + 7-)由于時間間隔&很短，線段A4'的長度近似等于圓弧長度，設，時間內編碼盤走過的距離為/,貝設q為上次采樣的角度，4為當前的角度，則 3=小8。通過計算加時間內機器人的位置變化量人、,），進行累加就可以求 出機器人在整個場地上的位置。0x' = x +Ar = x + 44'cos(6 + )z/。+ (。+ 八。)、()()=X + / c