基于EKF多傳感器融合的自動導航車(AGV)位姿估計

田會方，譚樹棟，吳迎峰

(武漢理工大學機電工程學院，湖北武漢430070)

1 引言

在傳統生產制造業中，產品的生產過程，真正加工和制造所占用的時間很少，大部分時間消耗在搬運、裝卸和倉儲等物流環節。隨著社會的發展，勞動力成本不斷提高，企業想要提高自身的競爭力，就需要對生產的物流環節進行改進優化[1]。AGV(自動導航車)是連接物流與生產的重要橋梁，是工業自動化中不可或缺的重要設備和技術，因其具有較強的機動靈活性、高度的作業重復性和安全可靠性等優勢，可以有效地提高生產效率并降低成本[2]。

AGV 在物流搬運與工業生產等領域有著越來越廣泛的應用，在生產車間環境中，傳統的導軌式AGV 有著諸多的限制，因此AGV 的自主導引能力越來越關鍵，AGV 的定位是實現自主導航功能的前提。因此獲取AGV 的位置和姿態信息，并通過算法提高位姿的精度是本文研究的重點。

2 里程計、IMU、UWB的模型分析

2.1 基于輪式里程計的位姿估計

里程計算法建立的運動模型主要依賴于小車輪上的編碼器，通過編碼器的脈沖計數來估算得到小車的位姿。本文的小車底盤采用的是兩輪差速驅動，底盤后方兩個同構驅動輪為其提供動力，前方的萬向輪起支撐作用，把小車完全視為剛體，不考慮任何力的影響，建立運動模型圖如圖1[3]。

圖1 里程計航跡推算模型

小車的速度、位置以及偏航角都可以基于兩輪的編碼器測得的脈沖數來計算得到。編碼器一周產生的脈沖數記作N，k時刻測得的脈沖數記作M，則有左右兩輪的轉角公式為：

小車的采樣時間內的行駛距離Sk可以由轉角以及驅動輪半徑R來表示：

因此，小車的偏航角變化量Δθk，可以通過兩輪的間距d和每個輪子的行駛距離來計算得到，進而可以計算得到小車的轉動半徑rk：

這里對模型進行一種假設，小車沿著圓弧的割線運動，即先轉過一半的角度Δθk/2，然后沿此方向直線運動Sk，最后再轉一半的角度Δθk/2，這種方法叫作割線模型。

因此，k+1 時刻的小車的位姿信息Pk+1=(xk+1,yk+1,θk+1)可以表示為：

此方法在建模過程中會對模型進行一定的假設優化，因此存在不可避免的誤差；編碼器位姿估計需要車輪直徑和回轉中心到驅動輪中心線的距離，在測量過程中也會存在誤差；同時運動過程中小車可能會出現輪子打滑、側移等現象，也會引起偶然誤差。

2.2 基于慣性單元的位姿估計

慣性導航系統基于慣性測量單元(IMU) 來實現的，利用IMU數據結合目標物體的初始位置和方向來確定其運動姿態。IMU 主要由加速度計和陀螺儀組成，其中，加速度計用于輸出小車在載體坐標系下的三個坐標軸方向上的線性加速度信息，而陀螺儀用于輸出載體小車相對于世界坐標系的三個坐標軸方向的角速度信息[4]。

圖2 IMU慣導模型

1)速度、位移解算

積分運算就是將采集到的加速度計數據進行積分處理，能夠得到運動目標的速度信息，進而得到位移信息。加速度計推算的離散模型為：

式中，vk表示k 時刻的瞬時速度，ak表示k 時刻加速度計測得的加速度數據，Sk表示從計時開始到k時刻的總位移。

2)角度估計

陀螺儀是IMU 中最為關鍵的模塊，陀螺儀的精度決定了IMU單元的上限和價格，陀螺儀可以測得運動物體相對于自身的三個軸向的角速度數據，那么陀螺儀推算的姿態角變化的離散模型為：

式中θk表示k 時刻的姿態角，ωk表示k 時刻陀螺儀測得的角速度數據。二維平面行駛的AGV，這里只考慮偏航角yaw。

對實驗用到的消費級加速度計采集到的原始數據分析時，由于精度較低的原因以及AGV小車在運輸過程中加速度比較小，因此加速度的誤差對于積分計算得到的速度和位移信息有較大的誤差，且不可避免，對導航影響較大，因此本文對位置信息的獲取不采用加速度計的方式。

對于陀螺儀采集到的角速度數據，可以一次積分得到角增量，結合上一時刻的航向角可以得到下一時刻的航向角信息，由于航向角的誤差對速度的二次的，對于位置的影響是三次的，因此減小航向角誤差能夠極大地提高導航的精度以及穩定性。然而陀螺儀本身存在零偏誤差，就是當車體靜止且水平時陀螺儀的讀數不為零[5]。因此需要與其他的傳感器配合使用來獲得相對精準的航向角信息。

與陀螺儀配合使用的傳感器通常有磁力計，磁力計是測量磁場強度的傳感器，將磁力計作為觀測量引入導航系統可以抑制陀螺儀數據對航向角信息的誤差累計，但是，磁力計的使用對環境有一定的要求，環境中存在一些干擾磁場，因此也會產生行進過程中的誤差，而且生產車間中對磁力計的干擾影響比較大，因此通過與陀螺儀等傳感器的配合使用可以有效降低誤差。

2.3 基于UWB的測距定位分析

超寬帶技術(UWB)是一種新興的非正弦窄脈沖通信方式，其特點是帶寬極寬，穿透能力強，傳輸速率高，有較強的分辨能力和抗干擾能力，同時功耗很低，抗多徑效果好，定位精度高，可達到厘米級別，因此在室內定位方面獲得了較高的關注[6]。

本文采用的是基于TOA的測距定位法，基本思想是通過測量目標標簽與基站之間的信號飛行時間，從而得到標簽與基站之間的距離，然后通過三邊定位算法得到標簽在世界坐標系下的坐標位置[7]。下面介紹三邊定位算法的原理。

圖3 TOA三邊定位法

已知三個基站點A(x1,y1)，B(x2,y2)，C(x3,y3)，目標節點為D(x,y)，三個基站到D 的距離分別為d1，d2，d3，根據幾何關系列出方程組：

在實際的工廠環境中，影響UWB 系統的定位精度是多方面的，包括硬件本身的性能、定位算法的精度以及環境因素等。這里主要考慮工廠環境中的非視距誤差以及多徑效應誤差等。

非視距誤差(NLOS)的產生主要是工廠環境中存在障礙物遮擋，比如設備、貨架、墻壁等，使得信號傳播并不是理想狀態下的直線傳播方式，而是信號傳播發生反射、折射等形式，在這種情況下障礙物材質以及厚度都會帶來不同的影響，對UWB信號的削弱導致最終定位受到影響[8]。對于多路徑效應誤差，UWB技術對其有一定的抑制能力，并且相比非視距誤差，其影響要小得多，因此本文忽略多路徑的影響。

3 基于擴展卡爾曼濾波的信息融合

3.1 EKF濾波介紹

擴展卡爾曼濾波是在卡爾曼濾波基礎上對于非線性問題的擴展，利用線性化技巧將非線性系統轉化為近似的線性化模型，然后應用卡爾曼濾波實現對目標的濾波估計[9]。非線性系統的狀態方程和觀測方程可以表示為：

其中，Xk表示k時刻的狀態變量，Zk表示k時刻對應狀態的觀測值，f(?)和h(?)為非線性函數，過程噪聲Wk和觀測噪聲Vk為高斯白噪聲，服從均值為0，協方差分別為Qk和Rk的正態分布，而且Wk和Vk兩兩互不相關。

EKF以卡爾曼濾波為基礎，核心思想是將非線性函數進行泰勒級數展開，忽略其二階及以上的高階項，進而得到近似的線性模型[10]。算法實現過程如下：

3.2 數據融合濾波

選取Xk=[xk,yk,θk,Δθk]T作為AGV 系統的狀態量，狀態量為k時刻里程計運動模型的x、y坐標以及航向角和航向角變化量，建立的系統狀態方程為：

觀測量為UWB 定位系統測得的坐標信息(xk,yk)，以及陀螺儀測得的角增量信息△θk和磁力計測得的航向角信息θk，因此系統的觀測方程為：

對于上述狀態方程的過程噪聲Wk，主要是由于基于里程計的運動模型中建模過程產生的高斯白噪聲；對于觀測方程的過程噪聲Vk主要來自傳感器的測量過程，有陀螺儀的隨機游走噪聲和磁力計受到其他磁場干擾產生的噪聲，以及UWB 定位模塊受到環境因素干擾產生的噪聲，均滿足正態分布，且彼此之間互不相關。對于這個典型的非線性系統，可以通過擴展卡爾曼濾波來處理噪聲。

1)預測過程：

①狀態先驗方程：

其中狀態轉移矩陣Fk由f函數一階泰勒展開線性化得到。

過程噪聲協方差矩陣Qk由里程計系統模型中的誤差得到，通常里程計模型的過程噪聲設置為wx=wy=w△θ=0.1，wθ=1，所以：

2)更新過程：

①卡爾曼增益Kk：

其中觀測矩陣Hk由f函數一階泰勒展開線性化得到。

觀測噪聲協方差矩陣Rk是傳感器測量過程中產生的噪聲。對于UWB 模塊會受到環境因素的干擾，因此可以采取動態更新權重的方式來設置對應的Rk分量，對于特殊路段，可能受到遮擋而產生的非視距誤差的程度不同，設置不同的值，而這些路段的坐標信息會存儲在導航系統中，當AGV 行進到該范圍時，更改對應的值。對于磁力計和陀螺儀模塊可以設定一個固定的值，通過對數據融合過程的效果來確定Rk矩陣的各個值。

②狀態后驗方程：

③將先驗協方差矩陣更新為狀態后驗估計值的協方差矩陣：

以上為AGV 定位系統的EKF 算法的一個計算周期，各個時刻EKF 對非線性系統的處理過程就是不斷循環這個計算周期。通過EKF 濾波算法，對傳感器信息進行數據融合，最終得到AGV在世界坐標系下的位姿信息。

4 實驗與分析

本實驗采用搭載JY901B慣導模塊、DWM1000超寬帶模塊以及電機自帶的霍爾編碼器等傳感器，以及STM32F103單片機來進行傳感器數據的采集，將數據通過串口傳輸到PC電腦，并在電腦上進行數據處理融合。實驗步驟如下：先將三個UWB基站模塊放置在實驗場地的預置的坐標點，然后將AGV 小車放置于起始坐標點，控制小車從起始點出發順時針方向經過一個長為6m，寬為6m的矩形的四條邊并回到起始點。

在工廠環境中，即使將UWB基站放置在比較高的位置，也會存在遮擋的現象，因此會使UWB定位精度下降，因此本文在EKF算法的基礎上，針對UWB影響較為嚴重的路段，位置誤差變大，采用動態加權法來配置Rk的值，其實驗效果圖如下：

圖4 為里程計模型的定位軌跡圖；圖5 為UWB 定位軌跡圖，在A-B路段設置了障礙物遮擋，因此波動明顯變大，與實際位置的誤差也較大；圖6為協方差矩陣Rk1為固定值的情況下，EKF算法得到的定位軌跡圖，會發現在A-B路段與實際運行軌跡有一定的偏差；圖7為協方差矩陣Rk2為動態加權的情況下，EKF 算法得到的定位軌跡圖，可以看出受到遮擋情況下，其融合效果也比較理想。其中Rk1和Rk2在本次實驗中的設定值如下：

圖4 里程計定位軌跡圖

圖5 UWB定位軌跡圖

圖6 固定協方差矩陣的EKF定位軌跡

圖7 動態協方差矩陣的EKF定位軌跡

其中，在Rk1的基礎上，對于A-B受遮擋嚴重路段，UWB的誤差變大，因此增大觀測誤差協方差矩陣的值，方差值越大表示越不準確，代表這段路程更信任里程計得到的位置信息。

5 結束語

本文考慮在室內環境下，能夠實現AGV的定位問題，AGV可以通過自身的慣導系統來進行導航定位，但是里程計、IMU存在累計誤差，長時間運行會有較大誤差，因此需要結合其他傳感器，考慮UWB技術的獨特室內定位優勢，因此提出了利用EKF 算法來融合多傳感器數據，并且考慮了UWB 技術的非視距誤差，采用動態的協方差矩陣來改善。通過實驗結果分析，可以得出該方案能夠提高單一定位系統的精度。