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

田會方 譚樹棟 吳迎峰

摘要：針對室內(nèi)AGV的自主導航中位姿獲取問題，為了得到精度較高、穩(wěn)定可靠的位姿信息，提出了一種基于EKF算法融合多傳感器數(shù)據(jù)的方法。通過建模分析，討論各種位姿估計方法的優(yōu)缺點，針對里程計和IMU的累計誤差問題，借助UWB來消除，針對UWB非視距誤差的問題，提出動態(tài)加權的思想，針對特殊的路段，借助里程計和IMU來改善。實驗結果表明，多傳感器融合能有效地克服各種傳感器的局限性，優(yōu)勢互補，得到精確可靠的位姿信息。

關鍵詞：里程計;IMU;UWB;擴展卡爾曼濾波;融合

中圖分類號? TP391 ? 文獻標識碼：A

文章編號：1009-3044（2022）20-0111-04

1 引言

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

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

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

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

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

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

[ηk，l=MlN*2π，ηk，r=MrN*2π]? ? ? ? ? ? ? ? ? ?（1）

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

[Sk=Sk，l+Sk，r2=R*ηk，l+R*ηk，r2]? ? ? ? ? ? ? ? （2）

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

[Δθk=Sk，l-Sk，rd，rk=SkΔθk]? ? ? ? ? ? ? ? ?（3）

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

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

[xk+1=xk+Δx=xk+Skcos（θk+Δθk2）yk+1=yk+Δy=yk+Sksin（θk+Δθk2）θk+1=θk+Δθk]? ? ? ? ? ?（4）

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

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

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

1） 速度、位移解算

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

[vk=vk-1+ak+ak-12*Δtsk=sk-1+vk-1*Δt+14（ak-1+ak）*Δt2]? ? （5）

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

2） 角度估計

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

[θk=θk-1+ωk+ωk-12*Δt]? ? ? ? ? ? ? ? （6）

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

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

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

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

2.3 基于UWB的測距定位分析

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

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

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

[（x-x1）2+（y-y1）2=d12（x-x2）2+（y-y2）2=d22（x-x3）2+（y-y3）2=d32]? ? ? ? ? ?（7）

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

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

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

3.1 EKF濾波介紹

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

[Xk+1=f（k，Xk）+WkZk=h（k，Xk）+VkWk～（0，Qk），Vk～（0，Rk）]? ? ? ? ? ? ? ? ? （8）

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

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

預測部分：? ? [Xk|k-1=f（k，Xk-1|k-1）Pk|k-1=Fk-1Pk-1|k-1FTk-1+Qk-1]? ?（9）

更新部分：

[Kk=Pk|k-1HTk（HkPk|k-1HTk+Rk-1）-1Xk|k=Xk|k-1+Kk（Zk-h（k，Xk|k-1））Pk|k=（I-KkHk）Pk|k-1] （10）

3.2 數(shù)據(jù)融合濾波

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

[Xk+1=f（k，Xk）+Wk=xk+Skcos（θk+Δθk2）yk+Sksin（θk+Δθk2）θk+ΔθkΔθk+Wk]? （11）

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

[Zk=h（k，Xk）+Vk=xkykθkΔθk+Vk]? ? ? ? ? ? ? （12）

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

1） 預測過程：

①狀態(tài)先驗方程：

[Xk+1|k=f（k，Xk|k）=xk+Skcos（θk+Δθk2）yk+Sksin（θk+Δθk2）θk+ΔθkΔθk] （13）

②先驗方程協(xié)方差矩陣：[Pk+1|k=FkPk|kFTk+Qk] （14）

其中狀態(tài)轉移矩陣Fk由f函數(shù)一階泰勒展開線性化得到。

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

[Qk=0.0100000.010000100000.01]? ? ? ?（15）

2） 更新過程：

①卡爾曼增益Kk：

[Kk=Pk+1|kHTk（HkPk+1|kHTk+Rk）-1]? ? ?（16）

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

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

②狀態(tài)后驗方程：

[Xk+1|k+1=Xk+1|k+Kk（Zk-h（Xk+1|k））]? ? ? （17）

③將先驗協(xié)方差矩陣更新為狀態(tài)后驗估計值的協(xié)方差矩陣：

[Pk+1|k+1=（I-KkHk）Pk+1|k]? ? ? ? （18）

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

4 實驗與分析

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

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

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

[Rk1=100000100000100000.1，Rk2=1000000010000000100000.1]? ?（19）

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

5 結束語

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

參考文獻：

[1] 羅堅銘.移動機器人機床上下料系統(tǒng)的精確定位技術研究[D].廣州：廣東工業(yè)大學，2018.

[2] 李晶.基于ROS的AGV自動導航控制系統(tǒng)開發(fā)[D].武漢：華中科技大學，2017.

[3] 吳鵬，李東京，贠超.一種慣性傳感器與編碼器相結合的AGV航跡推算系統(tǒng)[J].機電工程，2018，35（3）：310-316.

[4] Xia X，Xiong L，Huang Y J，et al.Estimation on IMU yaw misalignment by fusing information of automotive onboard sensors[J].Mechanical Systems and Signal Processing，2022，162：107993.

[5] 王澤華，梁冬泰，梁丹，等.基于慣性/磁力傳感器與單目視覺融合的SLAM方法[J].機器人，2018，40（6）：933-941.

[6] 董興波.基于UWB的高精度室內(nèi)定位系統(tǒng)研究與實現(xiàn)[D].桂林：桂林電子科技大學，2021.

[7] 徐慶坤，王天皓，宋中越.UWB與里程計融合的室內(nèi)移動機器人定位設計[J].機械設計與制造，2021（8）：295-299，304.

[8] 王文博，黃璞，楊章靜.基于超寬帶、里程計、RGB-D融合的室內(nèi)定位方法[J].計算機科學，2020，47（S2）：334-338.

[9] 李義.基于UWB與IMU組合的室內(nèi)移動機器人定位方法研究[D].成都：電子科技大學，2021.

[10] 吳和龍，Pei Xinbiao，Li Jihui，Gao Huibin，Bai Yue.Attitude estimation method based on extended Kalman filter algorithm with 22 dimensional state vector for low-cost agricultural UAV[J].High Technology Letters，2020，26（2）：125-135.

【通聯(lián)編輯：梁書】