田會(huì)方 譚樹(shù)棟 吳迎峰

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

關(guān)鍵詞：里程計(jì);IMU;UWB;擴(kuò)展卡爾曼濾波;融合

中圖分類號(hào)? TP391 ? 文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2022）20-0111-04

1 引言

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

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

2 里程計(jì)、IMU、UWB的模型分析

2.1 基于輪式里程計(jì)的位姿估計(jì)

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

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

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

小車的采樣時(shí)間內(nèi)的行駛距離Sk可以由轉(zhuǎn)角以及驅(qū)動(dòng)輪半徑R來(lái)表示：

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

因此，小車的偏航角變化量Δθk，可以通過(guò)兩輪的間距d和每個(gè)輪子的行駛距離來(lái)計(jì)算得到，進(jìn)而可以計(jì)算得到小車的轉(zhuǎn)動(dòng)半徑rk：

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

這里對(duì)模型進(jìn)行一種假設(shè)，小車沿著圓弧的割線運(yùn)動(dòng)，即先轉(zhuǎn)過(guò)一半的角度Δθk/2，然后沿此方向直線運(yùn)動(dòng)Sk，最后再轉(zhuǎn)一半的角度Δθk/2，這種方法叫作割線模型。

因此，k+1時(shí)刻的小車的位姿信息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）

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

2.2 基于慣性單元的位姿估計(jì)

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

1） 速度、位移解算

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

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

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

2） 角度估計(jì)

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

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

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

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

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

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

2.3 基于UWB的測(cè)距定位分析

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

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

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

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

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

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

3 基于擴(kuò)展卡爾曼濾波的信息融合

3.1 EKF濾波介紹

擴(kuò)展卡爾曼濾波是在卡爾曼濾波基礎(chǔ)上對(duì)于非線性問(wèn)題的擴(kuò)展，利用線性化技巧將非線性系統(tǒng)轉(zhuǎn)化為近似的線性化模型，然后應(yīng)用卡爾曼濾波實(shí)現(xiàn)對(duì)目標(biāo)的濾波估計(jì)[9]。非線性系統(tǒng)的狀態(tài)方程和觀測(cè)方程可以表示為：

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

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

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

預(yù)測(cè)部分：? ? [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時(shí)刻里程計(jì)運(yùn)動(dòng)模型的x、y坐標(biāo)以及航向角和航向角變化量，建立的系統(tǒng)狀態(tài)方程為：

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

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

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

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

1） 預(yù)測(cè)過(guò)程：

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

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

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

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

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

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

2） 更新過(guò)程：

①卡爾曼增益Kk：

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

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

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

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

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

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

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

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

4 實(shí)驗(yàn)與分析

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

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

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

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

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

5 結(jié)束語(yǔ)

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

參考文獻(xiàn)：

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

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

[3] 吳鵬，李東京，贠超.一種慣性傳感器與編碼器相結(jié)合的AGV航跡推算系統(tǒng)[J].機(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] 王澤華，梁冬泰，梁丹，等.基于慣性/磁力傳感器與單目視覺(jué)融合的SLAM方法[J].機(jī)器人，2018，40（6）：933-941.

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

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

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

[9] 李義.基于UWB與IMU組合的室內(nèi)移動(dòng)機(jī)器人定位方法研究[D].成都：電子科技大學(xué)，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)編輯：梁書(shū)】