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

(江蘇大學(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]，價(jià)格高昂，不適合工作在室內(nèi)、工作場景固定的車輛定位，如倉庫內(nèi)的貨車定位等.因此，如何對室內(nèi)車輛進(jìn)行低成本、高精度定位仍然是個難題.

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

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

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

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

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

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

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

2.1 姿態(tài)計(jì)算

慣性傳感器自身的載體系和當(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)系下的純線性運(yùn)動加速度[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 位置解算

將得到的純直線運(yùn)動加速度積分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，計(jì)算公式為

(5)

(6)

(7)

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

(8)

3 基于修正點(diǎn)的改進(jìn)軌跡修正算法

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

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

3.1 修正點(diǎn)選取

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

3.2 運(yùn)動位置判斷

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

(9)

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

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

3.3.1位置修正

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

(10)

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

修正后的位置誤差為

(11)

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

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

3.3.2速度修正

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

v=ωr，

(12)

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

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

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

(13)

式中：u為汽車的真實(shí)速度.

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

(14)

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

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

(15)

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

3.3.3姿態(tài)修正

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

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

4 試驗(yàn)與分析

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

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

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

圖4 繪制的車輛運(yùn)動軌跡

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

4.2 軌跡追蹤

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

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

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

4.3 誤差對比與分析

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

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

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

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

表1 位置誤差和速度誤差

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

圖7 10次試驗(yàn)的累計(jì)誤差分布

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

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

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

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

5 結(jié) 論

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