面向倉庫內(nèi)貨運車的低成本航跡修正定位算法

王 海， 吳 楚， 白 鑫， 陳宗哲， 侯芊荷

(江蘇大學(xué) 汽車與交通工程學(xué)院， 江蘇 鎮(zhèn)江 212013)

近年來，隨著智能汽車的迅猛發(fā)展，尤其是自動駕駛、車聯(lián)網(wǎng)等技術(shù)的不斷突破，汽車的高精度定位問題受到了學(xué)術(shù)界和工業(yè)界的廣泛關(guān)注.圍繞這一問題，學(xué)者們展開了廣泛的研究，常見的定位方法有全球?qū)Ш叫l(wèi)星系統(tǒng)(global navigation satellite system，GNSS)定位、無線網(wǎng)絡(luò)(wireless fidelity，Wi-Fi)定位、超帶寬(ultra wide band，UWB)定位等.然而，這些定位方法容易受到外界信息的干擾，依賴于外界信號[1]，價格高昂，不適合工作在室內(nèi)、工作場景固定的車輛定位，如倉庫內(nèi)的貨車定位等.因此，如何對室內(nèi)車輛進行低成本、高精度定位仍然是個難題.

作為一種不依賴外界信號、不需要在室內(nèi)預(yù)先搭建網(wǎng)絡(luò)、通過傳感器采集信息的室內(nèi)定位手段，捷聯(lián)式慣性導(dǎo)航(strapdown inertial navigation system，SINS)定位得到了越來越多的關(guān)注.但慣性導(dǎo)航的精度會隨著時間的推進而降低，其系統(tǒng)誤差會隨著時間累積無限增長，導(dǎo)致長期的定位精度差，從而限制了其應(yīng)用.針對慣性導(dǎo)航系統(tǒng)的誤差累計問題，學(xué)界有諸多方案.學(xué)者普遍認(rèn)為采用單一定位導(dǎo)航手段很難滿足長時間穩(wěn)定的高精度定位需求.單一傳感器無法獲得多級別、多方位和多層次的觀測信息，單一的定位導(dǎo)航技術(shù)存在更新頻率低、會受到不同環(huán)境影響、存在累計誤差等不足，因此融合多種定位技術(shù)的組合導(dǎo)航定位方式正成為自動駕駛定位的發(fā)展趨勢[2].HE K.等[3]采用慣性測量與UWB相結(jié)合的方式，通過線性回歸矩陣模型來修正慣性導(dǎo)航路徑，并通過最小二乘法來確定UWB軌跡與慣性導(dǎo)航軌跡相關(guān)的方程式，從而修正軌跡.李榮冰等[4]提出了以 SLAM(simultaneous localization and mapping)定位結(jié)果為觀測量的激光雷達、MEMS(micro-electro-mechanical system)慣性的組合導(dǎo)航方案，建立了面向室內(nèi)平面運動環(huán)境的組合濾波模型，搭建試驗驗證系統(tǒng)，并對結(jié)果進行驗證，證明了激光雷達與微慣性系統(tǒng)的信息融合架構(gòu)與算法是有效的室內(nèi)導(dǎo)航方案.

盡管針對消除慣性定位累計誤差的研究有了許多進展，但是，這些方法用到的設(shè)備成本較高，并存在累計誤差.筆者針對倉庫內(nèi)的貨運車，使用低成本、低精度的慣性傳感器，通過提取途徑路線的拐彎起始點作為修正點，通過轉(zhuǎn)向盤角度傳感器判斷車輛運動狀態(tài)，與修正點進行匹配，對位置、速度以及姿態(tài)誤差進行修正，從而減少慣性定位的累計誤差.

1 系統(tǒng)設(shè)計

該系統(tǒng)主要由數(shù)據(jù)采集部分和數(shù)據(jù)處理部分組成，定位系統(tǒng)組成如圖1所示.數(shù)據(jù)采集部分主要包括：布置在車輛上的慣性傳感器，采集車輛運動時的三軸加速度與三軸角速度；安裝在車輪處的輪速傳感器采集車輪轉(zhuǎn)速；布置在轉(zhuǎn)向盤轉(zhuǎn)向柱內(nèi)的角度傳感器采集轉(zhuǎn)向盤轉(zhuǎn)角.數(shù)據(jù)處理部分主要包括：傳感器數(shù)據(jù)、地圖信息的存儲；位置解算以及修正定位誤差等部分.傳感器采集到的數(shù)據(jù)文件通過網(wǎng)絡(luò)傳到云端，云端再根據(jù)內(nèi)置的算法實時解算車輛的位置，并根據(jù)傳感器信息對位置、速度和姿態(tài)進行修正.

圖1 定位系統(tǒng)組成

2 傳統(tǒng)航位推算技術(shù)

航位推算(dead-reckoning, DR)技術(shù)是一種僅需慣性傳感器就能進行定位的室內(nèi)定位技術(shù)[5]，主要利用三軸加速度和角速度數(shù)據(jù)進行姿態(tài)更新、坐標(biāo)變換以及位置解算等工作.

2.1 姿態(tài)計算

慣性傳感器自身的載體系和當(dāng)?shù)貙?dǎo)航坐標(biāo)系之間的轉(zhuǎn)換關(guān)系就是姿態(tài)矩陣，此姿態(tài)矩陣由兩坐標(biāo)系之間的方向余弦矩陣組成.姿態(tài)矩陣[6]為

(1)

式中：θ為橫滾角;ψ為偏航角;φ為俯仰角.

2.2 坐標(biāo)變換

慣性傳感器輸出載體坐標(biāo)系下的加速度信息左乘姿態(tài)矩陣轉(zhuǎn)換到導(dǎo)航坐標(biāo)系下的加速度信息，并減去重力加速度g得到導(dǎo)航坐標(biāo)系下的純線性運動加速度[7],即

(2)

式中：下標(biāo)n為導(dǎo)航坐標(biāo)系;下標(biāo)b為載體坐標(biāo)系；anx、any、anz為導(dǎo)航坐標(biāo)系下的三軸加速度；abx、aby、abz為載體坐標(biāo)系下的三軸加速度.

2.3 位置解算

將得到的純直線運動加速度積分1次，可得縱向速度vny和橫向速度vnx分別為

(3)

(4)

設(shè)汽車在t0時刻的起始坐標(biāo)為(x0,y0)，在t1時刻的位置坐標(biāo)為 (x1,y1)，汽車位移為(Δx, Δy)，縱向速度為vny，橫向速度為vnx.則單位時間內(nèi)汽車?yán)锍滩頪8]為Δs，計算公式為

(5)

(6)

(7)

在初始位置已知時，根據(jù)Δs、ψ，在一定的采樣時間內(nèi)通過式(8)可推算出后面每個時刻的位置(Xi,Yi)，即

(8)

3 基于修正點的改進軌跡修正算法

由于累計誤差，DR定位的精度將隨時間降低，無法提供準(zhǔn)確的車輛位置信息.真實位置與DR推算結(jié)果對比如圖2所示.圖2a為車輛實際狀況，此時車輛正處于拐彎的起點并正在拐彎，由于累計誤差，系統(tǒng)估計的車輛位置已偏離車道(見圖2b).此時可借助修正點對定位結(jié)果進行修正，修正過程主要包括判斷車輛運動位置以及速度、位置信息修正.

提出的改進軌跡修正算法主要包括航位推算和軌跡修正2部分，系統(tǒng)流程如圖3所示.

3.1 修正點選取

駕駛員在轉(zhuǎn)彎時幾乎都是在道路直線與轉(zhuǎn)彎相切點處開始轉(zhuǎn)動轉(zhuǎn)向盤，因此，選擇該切點作為理論轉(zhuǎn)彎點，即修正點.將該點坐標(biāo)、該點地面與起點地面之間的夾角存入云端，為后續(xù)修正做準(zhǔn)備.

3.2 運動位置判斷

車輛運動狀態(tài)的判斷依據(jù)是轉(zhuǎn)向盤轉(zhuǎn)角以及根據(jù)DR算法解算的車輛當(dāng)前坐標(biāo)與修正點的距離，當(dāng)車輛在修正點附近轉(zhuǎn)向時，則判定汽車位于修正點處.當(dāng)轉(zhuǎn)向盤轉(zhuǎn)角αf大于轉(zhuǎn)向盤間隙δf時，判定汽車正在轉(zhuǎn)彎.利用最短距離法[9]，根據(jù)DR算法，計算車輛當(dāng)前位置與最近的修正點距離L，計算公式為

(9)

當(dāng)L小于距離閾值μ時，判定汽車位于修正點處，μ取決于慣性傳感器的精度.

3.3 車輛運動狀態(tài)修正

3.3.1位置修正

當(dāng)判定汽車位于修正點時，將最近的修正點坐標(biāo)作為汽車的當(dāng)前坐標(biāo)，進行位置修正，令

(10)

式中：x′、y′、z′分別為距離汽車最近的修正點坐標(biāo).

修正后的位置誤差為

(11)

式中：x″、y″、z″分別為距離汽車實際位置的坐標(biāo).

εp取決于汽車的實際位置與修正點的距離，顯然，εp小于先前設(shè)定的μ，因此位置誤差在一定范圍內(nèi).汽車的速度與姿態(tài)也有誤差，若不修正，車輛位置誤差的增長速度將不斷加快，導(dǎo)致車輛即使位于修正點上做出轉(zhuǎn)彎動作，由于DR解算的位置與修正點位置距離過大，無法啟用修正程序，因此，需要借助其余信息修正速度與姿態(tài)的誤差.

3.3.2速度修正

利用輪速傳感器測量的車輪轉(zhuǎn)動角速度ω，計算得到汽車速度v，計算公式為

v=ωr，

(12)

式中：r為輪胎的靜力半徑.

將式(12)的汽車速度作為汽車的當(dāng)前速度，則可以修正部分速度誤差.修正后的車速誤差為

εv=u-v=u-ωr，

(13)

式中：u為汽車的真實速度.

根據(jù)滑移率計算公式可得

(14)

式中：rb為輪胎的滾動半徑；sb為輪胎的滑移率.

將式(14)代入式(13)得到修正后的速度誤差為

(15)

由于靜力半徑與滾動半徑差值一般在5 mm內(nèi)[10]，而汽車的滑移率在正常行駛時一般較小[11-12]，因此，速度誤差很小.

3.3.3姿態(tài)修正

對于車輛姿態(tài)角，使用預(yù)知修正點所在地平面與起點所在地平面的夾角作為汽車橫滾角與俯仰角的修正結(jié)果.對于偏航角，考慮到汽車轉(zhuǎn)彎時刻的朝向一般平行于所在車道走向，故將直行車道與車輛起始朝向的夾角作為此時的偏航角.

修正后的橫滾角誤差與俯仰角誤差取決于地面的不平度，一般處于較小范圍內(nèi)，而引起偏航角誤差的主要因素為此刻汽車實際朝向與直行車道的夾角，此誤差由駕駛員的駕駛習(xí)慣決定，但也在可控范圍內(nèi).

4 試驗與分析

基于嚴(yán)恭敏[13]團隊發(fā)布的開源代碼PSINS進行仿真試驗.

4.1 軌跡生成與參數(shù)設(shè)定

使用MATLAB進行仿真，生成車輛的運動軌跡，并根據(jù)軌跡反推車輛運動過程中產(chǎn)生的慣性數(shù)據(jù)，繪制的車輛運動軌跡如圖4所示.

圖4 繪制的車輛運動軌跡

設(shè)定慣性傳感器的相關(guān)參數(shù)，對生成的慣性數(shù)據(jù)添加誤差.采用低精度的慣性傳感器，陀螺儀偏置為0.08 (°)·h-1，隨機游走為0.78 (°)·h-1/2.

4.2 軌跡追蹤

利用DR算法解算當(dāng)前位置，根據(jù)轉(zhuǎn)向盤角度傳感器的數(shù)據(jù)和車輛位置判斷車輛是否處于修正點，并基于修正點與輪速傳感器的信息修正軌跡.車輛的真實軌跡、使用DR算法解算的原始軌跡以及修正軌跡的對比曲線如圖5所示.

圖5 真實軌跡、原始軌跡與修正軌跡對比曲線

從圖5可以看出：修正軌跡與真實軌跡重合度始終較高，說明修正后的定位精度較高；原始軌跡與修正軌跡在第1個修正點前重合，當(dāng)車輛在修正點附近轉(zhuǎn)向時，修正軌跡會發(fā)生突變，造成修正軌跡不光滑，這是因為，此時DR算法解算的坐標(biāo)被強制地糾正到了真實軌跡上，遠離了上一刻的車輛位置；修正后，軌跡的偏離速度也遠小于原始軌跡，這得益于速度和姿態(tài)的修正；當(dāng)車輛在離修正點較遠位置處由于需要變道而轉(zhuǎn)向時，并不會錯誤觸發(fā)修正程序而造成更大的誤差；使用修正算法得到的軌跡精度較原始軌跡有較大的提升.

4.3 誤差對比與分析

分別采用DR算法和修正算法計算得到的速度誤差和位置誤差曲線如圖6所示.

圖6 2種算法的速度誤差和位置誤差曲線

從圖6可以看出:采用修正算法后，慣性定位的位置誤差得到了修正，速度誤差減??；車輛每經(jīng)過一個修正點，相關(guān)誤差就會被修正，解決了誤差無限增加的問題.

分別采用DR算法和修正算法計算得到的位置誤差和速度誤差如表1所示，修正算法提升了定位精度，滿足倉庫內(nèi)貨運車的使用需求.

表1 位置誤差和速度誤差

重復(fù)進行10次試驗，得到修正后與原始軌跡的累計誤差分布函數(shù)，累計誤差分布如圖7所示.

圖7 10次試驗的累計誤差分布

從圖7可以看出:修正算法得到的位置誤差小于8.76 m，DR算法解算位置誤差小于8.76 m的數(shù)據(jù)占總樣本的38.9%.結(jié)果表明：使用修正算法對定位結(jié)果進行修正有效，減小了慣性定位的誤差，與DR算法相比，有更好的定位精度和穩(wěn)定性.

分別采用DR算法和修正算法計算得到的姿態(tài)角誤差曲線如圖8所示.

圖8 姿態(tài)角誤差曲線

從圖8a可以看出：由于DR解算的橫滾角誤差本身較小，被修正后的誤差反而有時大于DR解算的誤差，不過修正后的誤差仍然在一定范圍內(nèi)，對定位精度影響較小.從圖8b、c可以看出：修正方法能夠有效修正定位誤差，修正后的誤差大部分小于DR算法解算的誤差.

5 結(jié) 論

在DR算法的基礎(chǔ)上融合了運動狀態(tài)識別與運動信息修正等技術(shù).利用DR算法推算車輛的大致位置，結(jié)合運動狀態(tài)識別實現(xiàn)車輛位置與修正點坐標(biāo)的匹配，借助運動信息修正技術(shù)修正車輛的姿態(tài)、速度與位置信息，從而減小了慣性定位的累計誤差.通過仿真試驗進行驗證，結(jié)果表明：該方法能夠有效提高DR算法定位的精度，即使是用低精度的慣性傳感器也能滿足倉庫內(nèi)貨車的使用要求.由于是仿真試驗，對一些條件進行了簡化，在車輛實際運動中，有很多條件達不到理想狀態(tài)，可能會對結(jié)果造成影響，因此，下一步的研究將使用從實車采集的數(shù)據(jù)，并在更復(fù)雜的道路條件下進行.