田小靜

（西安航空職業(yè)技術(shù)學院自動化工程學院，陜西西安710089）

機器人的位姿估計是移動機器人要實現(xiàn)自主導航的一個重要環(huán)節(jié)。移動機器人的位姿是指移動機器人的位置和姿態(tài)，需要解決移動機器人定位和姿態(tài)獲取問題。

根據(jù)移動機器人定位技術(shù)和傳感器的不同，機器人的位姿可通過慣性導航定位、地圖匹配定位和基于信標的定位方法獲得[1]。慣性導航定位利用移動機器人自帶的傳感器獲得的信息解算出機器人的位置和姿態(tài)，這種定位需要給定移動機器人的初始位姿，定位精度高，但隨著時間的推移會產(chǎn)生累積誤差[2]，為了彌補慣性導航定位的缺陷，出現(xiàn)了大量的組合導航的研究[3-5]；基于地圖的匹配定位是指機器人利用其自身攜帶的傳感器建立局部環(huán)境地圖，通過與已經(jīng)建立的地圖對比估計載體的位姿，目前對地圖匹配定位的研究主要在機器人同時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）的問題上[6-8]，對于動態(tài)環(huán)境，SLAM 方法可能會因系統(tǒng)的誤差增大而失效；基于信標的定位依賴環(huán)境中已知位置的信標，需在機器人上安裝傳感器與信標建立聯(lián)系從而獲得載體的位置信息，基于信標的定位有紅外線、超聲波、WIFI、超寬帶定位等方法[9]，基于信標的定位可以獲得載體的絕對位置信息，但是無法獲得移動機器人可靠的偏航角，且大多數(shù)基于信標的定位方法的定位精度低，抗多徑能力和穿透性差。

超寬帶技術(shù)是近幾年發(fā)展起來的一種新型的通信技術(shù)，其帶寬到中心頻率比大于0.2且信號的總帶寬大于500MHz[10]。由于其非常大的帶寬，其脈沖具有非常短的持續(xù)時間，所以UWB具有非常高的時間分辨率，使得UWB定位相比于其他基于信標的定位法擁有更高的室內(nèi)定位精度。UWB定位系統(tǒng)理論上可以達到厘米級甚至更高的定位精度[11]，但是實際的室內(nèi)環(huán)境往往存在遮擋，且脈沖信號易發(fā)生反射和折射，影響UWB的測距精度，從而降低了其定位精度。

針對以上單一定位方法存在的不足，提出一種用擴展卡爾曼濾波融合UWB、陀螺儀、編碼器信息的定位方法，該方法既能利用UWB抑制機器人自帶傳感器誤差的累計，同時能夠提高UWB的定位精度。

1 UWB定位

1.1 飛行時間測距

飛行時間測距是通過測定UWB脈沖信號從UWB標簽到UWB基站的信號往返時間從而確定其距離的，這種方法解決常規(guī)時間到達（Time of Arrival，TOA）以及時間到達差（Time Difference of Arrival，TDOA）方法中需要UWB標簽與基站保持時間同步的問題。該測距原理是標簽先向基站發(fā)送測距序列，其中可包含要求應答等信息，基站經(jīng)過一個固定延時之后轉(zhuǎn)發(fā)信號[12]，標簽接收后計算一次來回程時間，標簽到基站i距離di可用式（1）表示：

1.2 三邊定位法

三邊定位法[13]是一種幾何法，通過測量UWB標簽到多個已知位置的UWB基站的距離來估計標簽的位置，該方法在目前室內(nèi)定位系統(tǒng)中應用最廣，對于二維空間定位，至少需要3個基站才能估計標簽的位置。假設(shè)標簽到各基站的距離分別為d1，d2，…，dn，各基站的位置分別為（x1，y1），…，（xn，yn），標簽的位置為（x，y）為待求位置，可得方程組如式（2）所示。

從第一個方程開始分別減去最后一個方程，得到式（3）。

該方程組可以用式（4）的線性方程組表示：

式中，

通過式（5）計算得到標簽的位置。

2 移動機器人模型

移動機器人在室內(nèi)環(huán)境下的模型如圖1所示，在圖中建立兩個坐標系，XYZ坐標系為導航坐標系n系，XrYrZr為機器人坐標系b系。移動機器人通過UWB標簽輸出的信息獲取其位置，通過陀螺儀獲得其姿態(tài)，里程計可為機器人提供線速度。

圖1 室內(nèi)環(huán)境下的移動機器人模型

機器人坐標系與導航坐標系的角位移關(guān)系即為機器人的姿態(tài)，可由航向角ψ、俯仰角θ和橫滾角γ三個角度描述。

根據(jù)歐拉角微分方程式[14]，可以得到姿態(tài)角與角速度的關(guān)系為：

式中，

輪式機器人的線速度可通過編碼器獲得，設(shè)兩輪輪距為L，編碼器的線數(shù)為P（輪子轉(zhuǎn)一圈編碼器輸出的脈沖數(shù)），輪徑為D。通過左右編碼器的脈沖頻率fL和fR可以算得左右輪子的線速度為：

式中，vL和vR分別代表左邊輪子的線速度和右邊輪子的線速度，γ為噪聲。

則機器人的線速度v可通過式（9）求得。

3 多傳感器信息融合算法

3.1 系統(tǒng)狀態(tài)方程

在二維平面中，UWB定位可以獲得機器人在導航坐標系中的位置坐標（x，y），編碼器可獲得機器人的線速度v，陀螺儀獲得機器人的偏航角速度ψ。用向量Xk描述移動機器人k時刻系統(tǒng)的狀態(tài)：

系統(tǒng)的狀態(tài)方程為：

機器人在室內(nèi)運動模型可以如圖2所示的，采樣周期為T，機器人在k-1時刻所在的估計位置在點pk-1處，后驗估計狀態(tài)為假設(shè)機器人沿著弧s在k時刻運動到位置為pk處，在pk處的先驗估計狀態(tài)為則弧長弧s對應的圓心為o，圓心角角度 Δψ=Tdψk-1，圓弧的半徑為r=s/Δψ，直線長度 l=2rsin（Δψ/2），該直線與x軸夾角為可推導得到系統(tǒng)的狀態(tài)方程為：

式中，wk～ N（0，Qk），Qk是k時刻過程激勵噪聲協(xié)方差矩陣。

圖2 移動機器人運動模型

3.2 測量方程

上面得到的狀態(tài)方程是非線性的方程，需要依照擴展Kalman濾波器[15]的方式對其進行線性化和離散化處理，得到k時刻的過程雅可比矩陣如下：

擴展卡爾曼濾波時間更新過程為：

測量更新過程為：

式中，Kk是k時刻的卡爾曼增益。

4 實驗與分析

4.1 實驗準備

實驗場所為3個UWB基站如圖1所示分布在室內(nèi)三個角落構(gòu)成正三角形形狀，UWB標簽放置于移動機器人且與基站等高，實驗場景如圖3所示。

圖3 實驗場景

實驗中的機器人平臺為美國Willow Garage公司設(shè)計的TurtleBot2移動機器人平臺，機器人里程計采用高精度增量編碼器（11.7ticks/mm，25718.16ticks/revolution），采樣頻率為50 Hz；陀螺儀型號為STMicroelectronics生產(chǎn)的L3G4200D，測量范圍為±250/s，采樣頻率為50Hz；UWB采用英國DecaWave公司的DWM1000測距模塊，官方的測距精度為cm級，采樣頻率為33 Hz。

4.2 軌跡分析

推動機器人沿著四塊地板邊緣行進，四塊地板的邊緣的形狀為長方形，該長方形長為1.72 m，寬為1.61 m。如圖4到圖6所示是實驗過程中的軌跡圖。

圖4藍色的線為單一的UWB測量得到的位置軌跡點，紅色的線為機器人行進的實際軌跡，UWB測量值與實際值最大誤差為0.294 m，平均誤差為0.035 m，平均定位精度達到厘米級，但是在部分位置數(shù)據(jù)比較發(fā)散，單單依靠UWB進行移動機器人的定位導航還存在不足；

圖4 UWB測量軌跡

圖5藍色的線為編碼器測得的速度與陀螺儀獲得的角速度進行積分獲得的軌跡，從圖中可以看出，該定位方法在短時間內(nèi)獲得比較高的定位精度，但是隨著時間的增加，累計誤差越來越大，軌跡越來越偏離實際的軌跡；

圖5 編碼器和陀螺儀估計軌跡

圖6為運用擴展卡爾曼濾波融合UWB、編碼器和陀螺儀后獲得的軌跡圖，可以看出，融合后的軌跡與實際的軌跡比較接近，用擴展卡爾曼濾波估計位置與實際位置的最大誤差為0.058 m，平均誤差為0.022 m，相比單一UWB定位，多傳感器融合算法最大誤差定位精度提高0.236 m，平均誤差定位精度提高0.013 m。

圖6 擴展卡爾曼估計軌跡

4.3 偏航角分析

機器人的偏航角是機器人位姿的重要參數(shù)，圖7藍色的線為陀螺儀測量值經(jīng)過積分得到的偏航角，紅色曲線為實際偏航角曲線，從圖中結(jié)合表1可以看出隨著實際偏航角的增大，經(jīng)陀螺儀積分得到的偏航角誤差越來越大；從圖8可以看出，利用多傳感器融合得到的偏航角沒有經(jīng)陀螺儀獲得的偏航角穩(wěn)定，但是從表1可以看出多傳感器融合得到的偏航角沒有隨著時間的累計而增大，另外在移動機器人不知道其初始偏航角時，利用多傳感信息融合可快速得到機器人的偏航角，非常適合移動機器人進行室內(nèi)導航。

圖7 陀螺儀測量偏航角

圖8 多傳感器融合測量偏航角

5 結(jié)論

多傳感器信息融合算法可以提供移動機器人較高精度的位置和偏航角信息，針對單一定位算法存在的問題，提出一種擴展卡爾曼濾波融合UWB、陀螺儀和編碼器的多傳感器定位方法，該方法首先推導出融合算法的狀態(tài)方程，利用UWB解算的位置和陀螺儀解算得到的機器人偏航角角速度以及編碼器采集得到的機器人的線速度作為測量信息，利用擴展卡爾曼濾波獲得移動機器人的位姿，并利用實驗對該算法進行了驗證，實驗表明多傳感信息融合算法能夠讓移動機器人獲得更高的定位精度以及可靠的偏航角，為移動機器人實現(xiàn)自主導航做了重要的準備。