基于STLS的姿態信息空間配準方法

徐旋孜,朱建良,薄煜明

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

0 引言

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

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

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

1 STLS最優估計的基本原理

1.1 問題描述

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

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

r=B-(A+E)x

(1)

式中:r稱為殘量;A與E的結構相同。

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

(2)

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

1.2 STLS方法及迭代求解

1.2.1X的構造

定義X∈Rm×q,其組成元素為x的元素,令Xα=Ex。構造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,對應的α的微小增量記為Δα,則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)

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

(6)

完整的STLS方法迭代過程總結如下:

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

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

3)根據1.2.1節所述,構造X,計算E;

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

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

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

2 基于STLS的相對姿態誤差角估計與補償

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

2.1 問題分析

2.1.1 空間基準選取

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

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

2.1.2 姿態角與方向余弦陣

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

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

(7)

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

(8)

2.2 相對姿態誤差角的估計與補償

2.2.1 基于姿態信息的模型建立

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

(9)

(10)

2.2.2 相對姿態誤差角的STLS求解

根據1.2.2節,迭代過程的步驟如下:

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

3)構造與xi對應的Xi和Ei;

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

(11)

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

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

2.2.3 相對姿態誤差角的補償

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

使用基于姿態信息的結構總體最小二乘法補償相對姿態誤差角的完整過程如圖1所示。

圖1 姿態配準過程示意圖

3 實驗結果與分析

開展MATLAB仿真實驗和半實物仿真實驗,對上述相對姿態誤差角補償的空間配準方法性能進行驗證。

3.1 MATLAB仿真實驗

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

圖2 STLS迭代估計相對姿態誤差角流程圖

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

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

3.2 半實物仿真實驗

3.2.1 相對姿態誤差角估計實驗

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

圖3 半實物仿真無人車

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

使用MATLAB軟件進行姿態信息的配準工作。對上述標定數據使用STLS方法,過程同仿真實驗的流程圖(圖2),估計結果如表2所示。

表2 半實物仿真相對姿態角估計結果 (°)

3.2.2 相對姿態誤差角補償實驗

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

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

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

(c)CAM航向角配準前后誤差圖4 配準前、后跑車數據CAM姿態角誤差圖

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

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

表3 配準前后跑車數據姿態角誤差 (°)

4 結論

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