淺談關於Robocon中使用的機器人定位技術

關於機器人定位，應該是一個老生常談的問題，我們隨便在網路上搜索一下，就會有很多相關的文章來解答。

這裡我會介紹和分析一下，在我參加Robocon比賽中常用的機器人定位技術。將這個技術首先用於Robocon比賽中的是西安交通大學，將導彈導航的技術用在這裡，預示著比賽將從手動變成自動的方式，從而進入風馳電掣的陀螺時代。

常用定位技術及其優缺點：

主流的定位技術，目前常用的定位方式有：GPS定位、基站定位、wifi定位、IP定位、RFID/二維碼等標籤識別定位、藍牙定位、聲波定位、場景識別定位，慣性導航，SLAM，技術上可以採取以下一種或多種混合。

慣性導航誤原理：

兩個相互垂直的全向輪碼盤+陀螺儀組成的慣性導航，單獨的兩個全向輪碼盤用來對沿輪子兩個方向進行測速，在僅僅平動過程可以測量出機器人左標系下的Vx，Vy。由於存在著機器人的轉動，為保證定位準確，使用陀螺儀用於獲得機器人運動中的角位移。通過乘以旋轉矩陣（用於坐標系轉換這裡使用過兩次，在慣性導航定位的地方使用過一次，在控制機器人實現無頭模式運動的時候，使用一次。將向量（x，y）逆時針轉α，將向量前面乘旋轉矩陣，

{X} {cos -sin 0} {x}

|Y| = |sin cos 0| *|y|

{Z} { 0 0 1 } {z}

其實導航與定位的關鍵就是在於坐標系的精確轉換。）完成編碼器速度值從機器人自身坐標繫到場地全局坐標系的轉換，獲得機器人在全場坐標系下任意運動都可以得到沿x軸與y軸的速度。通過對速度的積分獲得基於場地坐標系的機器人位置信息（X，Y），Z由陀螺儀獲得。坐標更新頻率一般為10ms/0.01S新一次。

慣性導航誤差分析及精度提高：

編碼器有誤差我採用滑動均值濾波處理取連續的N個採樣值看成一個循環隊列，隊列的長度固定為N，每次採集一個新數據放入隊尾，並扔掉原來隊首的一個數據（的數據結構是隊列，先進先出），濾波器對於這N個值進行算數平均值。優點：對於高頻震蕩的誤差有很好的抑制作用，平滑度高。這裡的速度信號，本來就是低頻的連續信號，所以非常適用。缺點：靈敏度低，對於偶然產生的干擾抑制作用差。陀螺儀這裡使用的是串口陀螺儀，進行了三軸加速度與三軸陀螺儀+電子羅盤的的數據融合，並且有卡爾曼濾波。Pitch 和 roll ，電子羅盤解決yaw陀螺儀的漂移，由溫度變化引起，所以需要對於溫度進行控制。

的漂移誤差使用卡爾曼濾波處理，優化：根據安裝角度與位置的不同，會有安裝誤差導致定位精度受很大影響。採用根據數學模型反算的方法，1：就是沿全局坐標系，X軸無旋轉情況下拖動機器人，10M。將位置帶回到方程中，就可以得到L1，L2,這個是定位模型中兩個全向輪距離機器人旋轉中心的距離。

如果在機器人不旋轉的情況下，兩個垂直的全向輪碼盤可以通過速度的積分得到位移從而得到位置，但是安裝時不可能保證一定是平行於全局坐標的X軸Y軸，這裡會有一個微小的角度α，消除誤差的方法就是將X與Y軸速度，使機器人自身坐標系，實現角度為α的轉轉。機器人在移動過程中可能也伴隨著旋轉，陀螺儀可以記錄這個旋轉過的角度，同樣需要進行機器人坐標系的旋轉。而全向輪碼盤也不可能安裝在機器人的運動軸心上，需要距離運動中心L1與L2的距離，這裡的距離也需要補償掉。所以這種慣性導航的安裝位置，直接決定了，α，L1,L2，這三個量對慣性導航的定位精度影響特別大。所以除了解決碼盤測速誤差，陀螺儀漂移誤差之外，對這三個量的測量也至關重要，測量+補償的方式，既不準確又耗費時間。採用模型反算方式，既簡單高效又準確。2：在運動軸心，打一個孔，讓機器人，繞運動軸心，旋轉5周回到初始姿態，同樣就可以反算測量編碼器與運動軸心的夾角，夾角用於旋轉矩陣映射坐標系。這三個值的精度，是使用量角器與直尺遠遠無法達到的。其實之前在比賽中，就遇到了定位的問題，機器人跑的又快又準是一個無止境的問題，而我在專業書上了解到了UWB這種通信領域的可以應用於精確定位新技術，通過查閱很多資料，我決定使用它來定位。

UWB定位原理：

使用的是DecaWave公司生產的，基於UWB技術，的DW1000定位晶元。

UWB通過極短脈衝通信，無需載波，通信持續時間為納秒級別，使用UWB定位需要獲得與位置相關的變數，建立定位數學模型。這裡測距使用的是TWR雙向測距，首先標籤記錄自己的發送時刻並發送一個測距指令，基站收到後，記錄收到測距指令的時刻，然後再記錄發送時刻，發送包含自己接收與發送時間差T2還有自己地址信息的響應消息。標籤接收記錄接受消息的時刻，計算自己發送接收時間差T1

TOF=(T1-T2)/2,Distance=TOF*C。

對於官方提供的晶元常式，我們需要deca_mutex.c，裡面主要是DW1000 設備驅動程序實質上是提供了一系列底層的功能介面，應用層可以調用實現更豐富的功能，有了這些介面，開發人員就不用再關注DW1000 收發方面詳細信息。我們只是需要編寫SPI 硬體介面程序，然後應用層調用設備驅動程序輕鬆實現測距功能。單片機通過SPI對晶元進行訪問輪詢狀態寄存器，處理任何發生的事件先寫地址，數據長度，數據值。官方提供了常式與API編程介面是一個半雙工的設備，時鐘到達高低電平變換的時間不一致，這樣會引起誤差。

具體操作：使用了SS-TWR（雙向測距）沒有使用SDS-TWR(雙邊雙向測距)

原因如下：

1：如果是標籤發起測距，那麼最終距離值在基站內，需要匯聚到標籤，頻率低。

2：如果是基站發起，基站需要不停地檢索標籤，功耗增加。通過中斷先向STM32反饋發送和接收回應。

定位幀遵循IEEE802.15.4，低速無線個人區域網物理層和媒體層接入控制協議

2位元組 frame control（框架控制）

1位元組 序列號

2位元組PAN ID固定

2位元組 對方ID

2位元組 自己ID

可變位元組的 測距信息 根據不同階段改變設置

（1）測距請求信息 1位元組 功能碼+1位元組 範圍號

（2）測距回應信息 1位元組 功能碼+2位元組睡眠修正+4位元組上一階段飛行時間+1位元組範圍號

（3）測距結束信息1位元組 功能碼+1位元組範圍號+5位元組請求發送時間

幀結尾由校驗位組成，由晶元自行完成

*以上截取至DW1000官方手冊

UWB誤差及精度提高：

它的誤差主要在於時鐘頻偏，40ppm，信息延時1ns相當於30cm，總體來說原始數據，誤差在±5-8cm左右。對原始數據進行滑動均值濾波（取連續的N個採樣值看成一個循環隊列，隊列的長度固定為N，每次採集一個新數據放入隊尾，並扔掉原來隊首的一個數據（的數據結構是隊列，先進先出），濾波器對於這N個值進行算數平均值。優點：對於高頻震蕩的誤差有很好的抑制作用，平滑度高。這裡的速度信號，本來就是低頻的連續信號，所以非常適用。缺點：靈敏度低，對於偶然產生的干擾抑制作用差。）和卡爾曼濾波主要是五個方程：

建模比較關鍵

使用上一次的最優估計與狀態控制量進行預測

更新預測誤差的協方差

計算卡爾曼增益

使用測量量進行校正更新獲得最優估計

更新協方差

在演算法的計算處理過程中每一次都在不停的修正K和測量誤差，相當於動態自適應演算法了，主要就是調好Q、R。Q是預測值誤差的協方差R是測量值誤差的協方差，距離數據波形平滑很多。平面定位至少需要不在同一條直線上的三個基站，定位標籤通過依次與三個基站進行輪詢，通過三邊定位演算法也就是三個基站圓的交點定位三邊定位原理：根據信號時間的到達距離畫個圓，三個以上圓的交匯處，就是標籤的大概位置

設標籤坐標（X,Y）,三基站坐標已知（X1，Y1），（X2，Y2），（X3，Y3），

則A與標籤的距離為d1 B與標籤的距離為d2 C與標籤的距離為d3。由簡單的勾股定理建立方程從而聯立方程組：

(X-X1) *(X-X1) +(Y-Y1) *(Y-Y1)=d1*d1

(X-X2)*(X-X2) +(Y-Y2) *(Y-Y2)=d2*d2

(X-X3) *(X-X3) +(Y-Y3) *(Y-Y3)=d3*d3

解方程獲得位置坐標

獲得位置信息，三個方程兩個位置數可以輕易解出標籤位置（X，Y）。只使用參考了官方測距歷程

（因為使用電池供電，有一次，有一個標籤，沒電了，但是還是有數據，而且還在變化，所以加了）有一個數據沒得到，就放棄之前的數據，重新開始。這樣保證三個距離值是同一時刻的。

安裝矯正：（）還有三基站與標籤是否在與地面平行的同一水平面（安裝矯正），（這個問題很類似於慣性導航的安裝誤差）位置放置的是否精準(直尺測量)。坐標更新頻率為5ms更新一次（1s200次）。

使用卡爾曼進行定位數據融合：

慣性導航的位置信息是靠速度積分而來的，會有累積誤差。（如果上一個點的誤差如果是1cm，那下一個點的誤差就會絕對大於1cm，而且越來越大，因為積分就是求和，將誤差也求了和）而UWB定位雖然沒有慣性導航精確，但是誤差穩定。使用UWB的數據對於慣性導航的誤差進行修正。對於定位數據有置信區間（0~10,10~無窮），在一定範圍內各自的置信區間不同，由於UWB和GPS定位類似，但是相比而言優點更多，關於GPS和慣性導航的數據融合，四旋翼無人機上使用的比較多，用到了卡爾曼進行數據融合。

根據卡爾曼5個公式：

第一步, 前一時刻位置最優估計+慣性導航的位移值ds（狀態的控制量，這個誤差比較小）得到此時的預測值。

第二步，計算（預測值與預測值均值之差）誤差的協方差。

第三步，計算卡爾曼增益。

第四步，使用UWB數據作為觀測值來修正估計，得到最優估計。

第五步，更新誤差協方差，完成遞歸（使用棧）。

這裡的Q （測量雜訊）的協方差（需要調試），R是UWB靜態時輸出數據計算的測量誤差（測量雜訊）的協方差（可以測量出大概值）通過串口示波器觀察慣性導航數據與融合之後的數據，長時間不會有非常大的漂移，在UWB和慣性導航之間。

參考文獻：

【1】機械設計與製造 2014年12月 12期《全向輪機器人路徑規劃與導航系統設計》李文學

【2】中國高新技術企業 2014年 16期 《淺談全向輪機器人三位一體定位方法》 唐松

【3】《物聯網與無線感測器網路》 電子工業出版社 劉偉榮 何雲

推薦閱讀：