基于STLS的姿態(tài)信息空間配準(zhǔn)方法

徐旋孜,朱建良,薄煜明

(南京理工大學(xué)自動化學(xué)院,江蘇 南京 210094)

0 引言

現(xiàn)代社會的快速發(fā)展使智慧交通成為大勢所趨,無人車的應(yīng)用作為智慧交通發(fā)展的重要體現(xiàn)[1],能有效保障交通控制系統(tǒng)的穩(wěn)定運行[2]。隨著無人駕駛技術(shù)的發(fā)展,單一、同類型的傳感器已無法滿足無人駕駛車輛分析路況信息的需求,利用具有獨特優(yōu)勢的不同傳感器及其相應(yīng)的導(dǎo)航方式(受外界干擾小、短時精度高的慣性導(dǎo)航,全天候高精度、技術(shù)成熟的衛(wèi)星導(dǎo)航,設(shè)備精簡、可構(gòu)建地圖實現(xiàn)避障的視覺導(dǎo)航等)進行多源融合導(dǎo)航成為無人駕駛領(lǐng)域中一項行之有效、必不可少的關(guān)鍵技術(shù)[3]。近年來,基于多源異構(gòu)異質(zhì)數(shù)據(jù)開發(fā)自主導(dǎo)航系統(tǒng)成為無人駕駛領(lǐng)域?qū)W界關(guān)注的熱點[4]。多源信息融合的無人車導(dǎo)航系統(tǒng)能夠有效提高導(dǎo)航性能,切實帶動地圖信息和空間結(jié)構(gòu)的一致性和穩(wěn)定性,然而如果直接對多源信息進行融合,若傳感器的結(jié)構(gòu)不一致、空間基準(zhǔn)不一致,非但不能優(yōu)化系統(tǒng)性能,反而可能造成性能的惡化。為對多傳感器的量測信息進行綜合整理從而更精確地估計出導(dǎo)航信息,在信息融合之前的數(shù)據(jù)預(yù)處理階段須進行空間配準(zhǔn)。多傳感器空間配準(zhǔn)誤差問題主要有:空間基準(zhǔn)不一致,即各傳感器的導(dǎo)航數(shù)據(jù)依據(jù)不同坐標(biāo)系測量而得,彼此存在誤差;傳感器安置存在誤差,即各傳感器安裝完畢后彼此的對應(yīng)坐標(biāo)軸朝向不完全重合,安裝的姿態(tài)存在偏差,造成安裝的相對姿態(tài)誤差;各傳感器安裝在車輛載體的不同位置存在桿臂效應(yīng),造成桿臂效應(yīng)誤差[5]。

相對姿態(tài)誤差的參數(shù)獲取通常也被稱為標(biāo)定,通?？煞譃樵诰€標(biāo)定和離線標(biāo)定兩種方法。在線標(biāo)定一般基于濾波實時估計相對姿態(tài)參數(shù),對算法要求高,當(dāng)估計結(jié)果不收斂時將會影響其他濾波狀態(tài)量,引起導(dǎo)航精度下降。謝宇婷等[6]、程然等[7]分別采用卡爾曼濾波、基于期望最大化的容積卡爾曼濾波法在線估計相對姿態(tài)誤差角,但都是針對目標(biāo)跟蹤的量測數(shù)據(jù)進行分析,在導(dǎo)航領(lǐng)域的配準(zhǔn)效果未知。離線標(biāo)定法一般基于最優(yōu)估計方法,或借助其他輔助的標(biāo)定設(shè)備完成。胡雷[8]通過最小二乘類方法對相對姿態(tài)誤差角進行估計,其中相對姿態(tài)僅針對純方位雷達的方位角。呂志剛[9]指出,當(dāng)更換水下航行器的某導(dǎo)航傳感器時,其與捷聯(lián)慣導(dǎo)的相對姿態(tài)關(guān)系發(fā)生變化造成系統(tǒng)誤差,需要額外的角度測量儀進行標(biāo)定以滿足導(dǎo)航精度要求。何騫等[10]提出一種基于預(yù)積分的IMU/ODO外參估計方法,但是該方法中相對姿態(tài)誤差的航向角不可觀測從而無法估計;周陽林等[11]、葉玨磊等[12]均對激光雷達與GNSS/INS組合導(dǎo)航系統(tǒng)提出標(biāo)定方法,前者提出的方法可快速獲取相對姿態(tài)參數(shù)但需要經(jīng)緯儀,后者提出的方法不需要經(jīng)緯儀但基于點云信息,無法用于其他導(dǎo)航傳感器。由此可見,由于存在空間配準(zhǔn)的相對姿態(tài)誤差問題,在組合導(dǎo)航領(lǐng)域缺乏適用于不同種類導(dǎo)航設(shè)備的空間配準(zhǔn)方法,無法完成姿態(tài)信息的配準(zhǔn)。

考慮到無人車的多種導(dǎo)航傳感器與車體剛性連接,導(dǎo)航傳感器之間的相對位姿關(guān)系參數(shù)固定,相對姿態(tài)誤差是常值,因此,考慮使用離線標(biāo)定方法對相對姿態(tài)誤差進行估計,降低算法難度和計算量。本文基于傳感器測量所得的姿態(tài)信息,利用對應(yīng)的方向余弦矩陣作為中間變量,建立相對姿態(tài)誤差模型,使用STLS(structured total least square,結(jié)構(gòu)總體最小二乘)方法對模型進行迭代求解,獲得三軸相對姿態(tài)誤差對應(yīng)的方向余弦陣。利用該方向余弦陣可反解出相對姿態(tài)誤差對傳感器測量的姿態(tài)信息進行補償,僅依靠傳感器的量測信息而無需借助標(biāo)定輔助設(shè)備,可實現(xiàn)不同種類導(dǎo)航傳感器的姿態(tài)信息空間配準(zhǔn),為信息融合做好準(zhǔn)備工作,利于組合導(dǎo)航精度的提升。

1 STLS最優(yōu)估計的基本原理

1.1 問題描述

超定方程Ax=B的系數(shù)矩陣A和觀測矩陣B同時存在擾動誤差,分別為E和e,采用STLS方法求解該超定方程,但基于奇異值分解的方法會破壞矩陣原有的結(jié)構(gòu)。在此基礎(chǔ)上,Moor和謝景超提出的STLS方法可以保留原數(shù)組的結(jié)構(gòu)特征[13-14]。

使用STLS方法求解的問題可描述為:尋找r∈Rm,E∈Rm×n,使得‖[rE]‖F(xiàn)=min。其中

r=B-(A+E)x

(1)

式中:r稱為殘量;A與E的結(jié)構(gòu)相同。

若A中有q(q≤mn)個元素存在誤差,由E的q個元素構(gòu)成的向量記為α=[α1,…,αq]T∈Rq,則α與E等價,已知α和A的結(jié)構(gòu)特點即可唯一確定E。殘量r可視為α與x的函數(shù),r=r(α,x)。用加權(quán)對角矩陣D表示α各元素在E中的重復(fù)次數(shù)。使用STLS方法求解的問題可改述為:

(2)

其中‖·‖p表示p-范數(shù),p=1,2,…,∞,而當(dāng)p=2時使用STLS方法求解兩種描述等價。

1.2 STLS方法及迭代求解

1.2.1X的構(gòu)造

定義X∈Rm×q,其組成元素為x的元素,令Xα=Ex。構(gòu)造X的方法如下:

定義m個矩陣Pi(i=1,…,m)∈Rq×n,若αk位于E的(i,j)處,則Pi的(k,j)處元素為1,其余元素為0。由定義,E和X可由公式(3)計算:

(3)

1.2.2 迭代求解過程

考慮每輪迭代過程中E的微小增量分別為ΔE,對應(yīng)的α的微小增量記為Δα,則XΔα=ΔEx。對每輪迭代的殘量r=r(α+Δα,x+Δx)采用線性逼近的方法并忽略Δα和Δx的二階項,得到:

r(α+Δα,x+Δx)=

B-(A+E+ΔE)(x+Δx)=

B-(A+E)x-ΔEx-(A+E)Δx-ΔEΔx≈

[B-(A+E)x]-ΔEx-(A+E)Δx=

r(α,x)-XΔα-(A+E)Δx

(4)

對式(2)考慮Δα和Δx后描述為

寫成間接平差的形式為:

(5)

根據(jù)間接平差公式,求得微小增量Δα和Δx為

(6)

完整的STLS方法迭代過程總結(jié)如下:

1)確定參數(shù):系數(shù)矩陣A,加權(quán)對角矩陣D,觀測矩陣B以及允許誤差ε;

2)確定迭代初始值:E=0,α=0,x=xLS;

3)根據(jù)1.2.1節(jié)所述,構(gòu)造X,計算E;

4)計算殘量r(α,x),根據(jù)平差公式(6)計算本輪迭代的微小增量Δα和Δx;

5)更新:α=α+Δα,x=x+Δx;

6)判斷條件:滿足max{‖Δα‖,‖Δx‖}≤ε時,停止迭代;不滿足,則轉(zhuǎn)到步驟3開始新一輪迭代,直至滿足條件停止。

2 基于STLS的相對姿態(tài)誤差角估計與補償

姿態(tài)信息空間配準(zhǔn)可分為兩個過程:事先采集靜態(tài)標(biāo)定數(shù)據(jù),通過參數(shù)估計方法計算出相對姿態(tài)誤差角;使用得到的相對姿態(tài)誤差角估計值,導(dǎo)航過程中,在進行多源融合之前對該相對姿態(tài)誤差角進行補償。在此之前,需要確定相對姿態(tài)誤差角的基準(zhǔn)傳感器。

2.1 問題分析

2.1.1 空間基準(zhǔn)選取

開展空間配準(zhǔn)前的基礎(chǔ)問題是確定空間基準(zhǔn)。一般導(dǎo)航系統(tǒng)選取載體幾何質(zhì)心作為基準(zhǔn)坐標(biāo)原點,選取坐標(biāo)系及正方向為右-前-上方向。該選取方式的問題在于,載體質(zhì)心處的真實參考數(shù)據(jù)無法直接獲取,只能通過安裝在質(zhì)心處的精度可靠的傳感器來獲取,然而這種方式在實際實施過程中難以實現(xiàn)。比如:精度較高的GPS需要安裝天線才能通過RTK定位,而天線需要裸露在載體外部,難以安裝在質(zhì)心處;其余高精傳感器也有類似或其他問題,因此獲取質(zhì)心處的真實參考數(shù)據(jù)較為困難。

在本文研究的無人車系統(tǒng)中,由于考慮拒止條件下將高精IMU作為最主要的導(dǎo)航傳感器,其余各傳感器作為輔助修正傳感器,且IMU的安裝位置靠近無人車的質(zhì)心處,因此,空間基準(zhǔn)原點選取為IMU處,空間基準(zhǔn)坐標(biāo)系選為右-前-上坐標(biāo)系,各傳感器剛體的姿態(tài)數(shù)據(jù)配準(zhǔn)到空間基準(zhǔn)坐標(biāo)系下。

2.1.2 姿態(tài)角與方向余弦陣

首先對姿態(tài)角及對應(yīng)的方向余弦陣等相關(guān)定義進行說明。選取導(dǎo)航坐標(biāo)系n系為東-北-天坐標(biāo)系(E-N-U),姿態(tài)角旋轉(zhuǎn)順序定義為繞固定軸z-y-x的順序,即以導(dǎo)航坐標(biāo)系n為初始姿態(tài),先繞z軸轉(zhuǎn)動一個角度為航向角θY,再繞y軸旋轉(zhuǎn)一個角度為橫滾角θR,最后繞x軸轉(zhuǎn)動一個角度為俯仰角θP,旋轉(zhuǎn)完畢得到載體坐標(biāo)系b。

在此旋轉(zhuǎn)順序定義下,由b系(下角標(biāo))變換到n系(上角標(biāo))的方向余弦陣為

(7)

已知方向余弦矩陣也可以反求出姿態(tài)角,式(8)中arctan表示四象限的反正切。

(8)

2.2 相對姿態(tài)誤差角的估計與補償

2.2.1 基于姿態(tài)信息的模型建立

無人車多源融合導(dǎo)航系統(tǒng)一般裝有多種傳感器。此處使用的無人車裝有IMU(慣性測量單元)、AHRS(航姿參考系統(tǒng))、LiDAR(激光雷達)、CAM(視覺里程計)等傳感器,考慮各種傳感器均與前述選取的IMU基準(zhǔn)坐標(biāo)系存在相對姿態(tài)誤差角,因此,需基于各種傳感器的姿態(tài)數(shù)據(jù)與IMU的姿態(tài)數(shù)據(jù)建立模型。由于不同傳感器的姿態(tài)數(shù)據(jù)不可直接進行數(shù)學(xué)計算,因此,需利用方向余弦陣作為建模對象。以CAM為例,估計CAM與IMU之間的相對姿態(tài)誤差角(其他傳感器同理)。

(9)

(10)

2.2.2 相對姿態(tài)誤差角的STLS求解

根據(jù)1.2.2節(jié),迭代過程的步驟如下:

2)迭代初始值E=0,α=0,x=xLS=(ATA)-1ATb,則xi為xLS的第i列;

3)構(gòu)造與xi對應(yīng)的Xi和Ei;

4)計算殘量r(α,xi),根據(jù)平差公式計算本輪迭代的微小增量Δα和Δxi;

(11)

5)更新:α=α+Δα,xi=xi+Δxi;

6)判斷條件:滿足max{‖Δα‖,‖Δxi‖}≤ε時,停止迭代;不滿足,則轉(zhuǎn)到步驟3開始新一輪迭代,直至滿足條件停止。

2.2.3 相對姿態(tài)誤差角的補償

由上述2.2.1、2.2.2節(jié)可以通過離線的方式完整求解出相對姿態(tài)誤差角(常量誤差),采集的數(shù)據(jù)均為估計誤差所使用的數(shù)據(jù),但還未對實際的數(shù)據(jù)進行誤差補償。

使用基于姿態(tài)信息的結(jié)構(gòu)總體最小二乘法補償相對姿態(tài)誤差角的完整過程如圖1所示。

圖1 姿態(tài)配準(zhǔn)過程示意圖

3 實驗結(jié)果與分析

開展MATLAB仿真實驗和半實物仿真實驗,對上述相對姿態(tài)誤差角補償?shù)目臻g配準(zhǔn)方法性能進行驗證。

3.1 MATLAB仿真實驗

在MATLAB R2020a的環(huán)境下進行仿真實驗,其中使用STLS方法迭代估計的流程如圖2所示。

圖2 STLS迭代估計相對姿態(tài)誤差角流程圖

表1 仿真實驗不同方法相對姿態(tài)角估計結(jié)果 (°)

表1數(shù)據(jù)顯示,估計的相對姿態(tài)誤差值都較為準(zhǔn)確,彼此之間差異較小,就參數(shù)估計而言,幾種方法的區(qū)別并未很好地體現(xiàn)。由于仿真實驗已知準(zhǔn)確真值,可以進一步考察單位權(quán)中誤差。單位權(quán)中誤差是權(quán)值為1的觀測值均方根差,可以反映等精度測量情況下觀測值偏離真值的程度。分析表1最后一行,發(fā)現(xiàn)LS方法的單位權(quán)中誤差大于TLS方法、大于STLS方法,單位權(quán)中誤差值越小說明估計結(jié)果精度越高,因此,STLS方法優(yōu)于TLS方法、TLS方法優(yōu)于LS方法。

3.2 半實物仿真實驗

3.2.1 相對姿態(tài)誤差角估計實驗

半實物仿真無人車如圖3所示。

圖3 半實物仿真無人車

根據(jù)2.2.1節(jié)所述,采集估計相對姿態(tài)誤差角所需要的多組標(biāo)定數(shù)據(jù),采集每組數(shù)據(jù)時,無人車至少運動10 m距離以便LiDAR、CAM有足夠的識別目標(biāo)進行SLAM建圖,隨后靜止一段時間采集STLS方法所使用的靜態(tài)數(shù)據(jù)。采集完畢后選取北、東北、東、西、南、上坡6組靜態(tài)數(shù)據(jù)作為相對姿態(tài)誤差角估計的使用數(shù)據(jù)。

使用MATLAB軟件進行姿態(tài)信息的配準(zhǔn)工作。對上述標(biāo)定數(shù)據(jù)使用STLS方法,過程同仿真實驗的流程圖(圖2),估計結(jié)果如表2所示。

表2 半實物仿真相對姿態(tài)角估計結(jié)果 (°)

3.2.2 相對姿態(tài)誤差角補償實驗

實驗數(shù)據(jù)采集地點為學(xué)校廣場,跑車數(shù)據(jù)采集時間大于60 s。使用3.2.1節(jié)求得的相對姿態(tài)誤差角對跑車數(shù)據(jù)進行補償。以CAM(視覺里程計)為例,配準(zhǔn)前、后跑車數(shù)據(jù)姿態(tài)角誤差情況如圖4所示。

(a)CAM俯仰角配準(zhǔn)前后誤差

(b)CAM橫滾角配準(zhǔn)前后誤差

(c)CAM航向角配準(zhǔn)前后誤差圖4 配準(zhǔn)前、后跑車數(shù)據(jù)CAM姿態(tài)角誤差圖

比較配準(zhǔn)前后,AHRS、LiDAR、CAM的俯仰角、橫滾角、航向角誤差的均值、RMS(均方根)值,由表3配準(zhǔn)前后跑車數(shù)據(jù)姿態(tài)角誤差數(shù)據(jù),可以看出,3種傳感器的俯仰角、橫滾角誤差均減小,但是航向角誤差增大。

分析誤差增減百分比情況,發(fā)現(xiàn)待配準(zhǔn)傳感器與基準(zhǔn)傳感器之間的相對姿態(tài)誤差角越大,配準(zhǔn)效果越好。LiDAR和CAM的相對姿態(tài)俯仰角最大,在2°上下,配準(zhǔn)前后誤差均值、RMS值減小比例可達70%以上;其次是CAM的相對姿態(tài)橫滾角,為0.8°左右,其誤差均值減小比例超過60%。AHRS的俯仰角、橫滾角,LiDAR的橫滾角的誤差均值、RMS值也均有不同程度的減小。但是3種傳感器的航向角的誤差均值和RMS值反而增大,我們認(rèn)為是待配準(zhǔn)傳感器與基準(zhǔn)傳感器之間的相對航向誤差角的數(shù)值過小,估計值不超過0.4°,并且配準(zhǔn)前傳感器測量所得姿態(tài)角與實際值之間的誤差很小,均在0.1°上下,從而配準(zhǔn)的結(jié)果容易受傳感器隨機測量噪聲的影響。進一步,配準(zhǔn)后3種傳感器的航向角誤差最大也僅為0.36°,在誤差允許范圍內(nèi)。因此,本文所提出的姿態(tài)信息配準(zhǔn)方法綜合誤差較小,能夠滿足無人車多源融合導(dǎo)航的空間配準(zhǔn)要求。

表3 配準(zhǔn)前后跑車數(shù)據(jù)姿態(tài)角誤差 (°)

4 結(jié)論

本文針對多源融合導(dǎo)航空間配準(zhǔn)中的相對姿態(tài)誤差問題,提出一種基于姿態(tài)信息的STLS參數(shù)估計方法,先通過迭代方式求解出待配準(zhǔn)傳感器相對于基準(zhǔn)傳感器的相對姿態(tài)誤差角,隨后對所估計出的相對姿態(tài)誤差角進行補償。通過MATLAB無實物仿真實驗,比較LS、TLS、STLS估計結(jié)果和單位權(quán)中誤差,驗證STLS迭代方法求解相對姿態(tài)誤差角的性能。通過無人車實時跑車數(shù)據(jù)進行半實物仿真實驗,發(fā)現(xiàn)相對姿態(tài)誤差角度越大,配準(zhǔn)效果越好;其中的相對姿態(tài)誤差最大,俯仰角的配準(zhǔn)效果最佳,配準(zhǔn)前后誤差減小70%以上。因此,本文所提出的姿態(tài)信息配準(zhǔn)方法具有易于操作、可行性強的優(yōu)點,可為后續(xù)多源融合導(dǎo)航做好準(zhǔn)備工作。