(上海工程技術(shù)大學(xué) 城市軌道交通學(xué)院，上海 201620)

0 引言

由于軌道線路直接承受列車的載荷，容易造成軌道線路的不平順影響到列車運(yùn)行的安全性[1]，因此，實(shí)現(xiàn)對(duì)軌道線路的高精度檢測(cè)具有重要意義。隨著慣性傳感器技術(shù)和視覺(jué)導(dǎo)航技術(shù)的不斷發(fā)展，多傳感器組合測(cè)量成為我國(guó)軌道狀態(tài)參數(shù)檢測(cè)的主要方式[2-6]。由于慣性測(cè)量單元中的加速度計(jì)和陀螺儀在長(zhǎng)時(shí)間檢測(cè)過(guò)程中會(huì)產(chǎn)生累積誤差，使得檢測(cè)精度較低[7]；雙目視覺(jué)在運(yùn)動(dòng)過(guò)程中易受晃動(dòng)或環(huán)境因素的影響引入誤差，測(cè)量精度難以得到保障[8]。因此，如何提高軌道線路的檢測(cè)精度已成為軌道檢測(cè)領(lǐng)域的主要研究方向[9-10]。

針對(duì)加速度計(jì)和陀螺儀組合測(cè)量方法，文獻(xiàn)[11]首先利用陀螺儀數(shù)據(jù)更新姿態(tài)矩陣，將加速度信息轉(zhuǎn)換到地理坐標(biāo)系中，最后利用小波變換對(duì)加速度信號(hào)進(jìn)行去噪處理并利用積分濾波器對(duì)加速度信號(hào)進(jìn)行二次積分來(lái)獲取物體的運(yùn)動(dòng)軌跡。考慮到雙目視覺(jué)拍攝圖像測(cè)量可以提供軌道線形的絕對(duì)坐標(biāo)，文獻(xiàn)[12]對(duì)車輛運(yùn)動(dòng)時(shí)拍攝的鋼軌圖像利用拼接技術(shù)恢復(fù)鋼軌表面的紋路，利用雙目視覺(jué)定標(biāo)原理計(jì)算鋼軌輪廓上的空間點(diǎn)三維坐標(biāo)，得到鋼軌的線形信息。

為了提高軌道線形檢測(cè)精度，本文提出一種基于多傳感信息融合的軌道線形檢測(cè)方法?；诮萋?lián)慣性系統(tǒng)和雙目視覺(jué)的測(cè)量原理建立了雙目視覺(jué)與慣性測(cè)量結(jié)合的多傳感數(shù)據(jù)融合模型，并利用擴(kuò)展卡爾曼濾波來(lái)有效融合雙目視覺(jué)、陀螺儀和加速度計(jì)的測(cè)量信息，提高測(cè)量數(shù)據(jù)的精確度，從而達(dá)到提高軌道線形檢測(cè)精度的目的。實(shí)驗(yàn)表明，該方法的測(cè)量精度與慣性測(cè)量方法相比提高了近9倍，且測(cè)量所得坐標(biāo)在三個(gè)方向上的最大位移絕對(duì)誤差不超過(guò)0.536 mm，具有較高的實(shí)用性。

1 多傳感信息融合檢測(cè)原理

多傳感信息融合檢測(cè)系統(tǒng)主要由兩個(gè)攝像機(jī)構(gòu)成的雙目視覺(jué)系統(tǒng)和陀螺儀、加速度計(jì)組成的慣性測(cè)量單元組成，所有傳感器被固定安裝在同一運(yùn)動(dòng)載體上。檢測(cè)系統(tǒng)結(jié)構(gòu)如圖1所示。為了實(shí)現(xiàn)高精度的軌道線形檢測(cè)，利用手眼定標(biāo)算法將各傳感器直接測(cè)量數(shù)據(jù)轉(zhuǎn)換到世界坐標(biāo)系下[13]，采用雙目視覺(jué)與慣性測(cè)量結(jié)合的多傳感數(shù)據(jù)融合算法，建立雙目視覺(jué)、陀螺儀和加速度計(jì)之間位移及姿態(tài)角的融合關(guān)系，并利用擴(kuò)展卡爾曼濾波實(shí)現(xiàn)雙目視覺(jué)、陀螺儀和加速度計(jì)的測(cè)量信息的融合，得到運(yùn)動(dòng)載體準(zhǔn)確的位移、姿態(tài)角信息。利用融合的姿態(tài)角實(shí)時(shí)更新姿態(tài)矩陣為位移提供運(yùn)動(dòng)方向，從而獲取運(yùn)動(dòng)控制平臺(tái)的空間運(yùn)動(dòng)曲線及軌道線形，實(shí)現(xiàn)軌道線形的高精度檢測(cè)。

圖1 多傳感器融合檢測(cè)系統(tǒng)

1.1 捷聯(lián)慣性系統(tǒng)測(cè)量原理

捷聯(lián)式慣性系統(tǒng)檢測(cè)原理是：將測(cè)量器件加速度計(jì)和陀螺儀直接安裝在同一運(yùn)載體平臺(tái)上，當(dāng)運(yùn)載體平臺(tái)運(yùn)動(dòng)時(shí)，加速度計(jì)獲取沿運(yùn)載體X、Y、Z三個(gè)軸方向的加速度信號(hào)，陀螺儀獲取沿運(yùn)載體X、Y、Z三個(gè)軸方向的角速度信號(hào)。利用陀螺儀獲取到的角速度信息分別更新三軸上的姿態(tài)矩陣，從而將加速度三軸測(cè)量數(shù)據(jù)轉(zhuǎn)換到地理坐標(biāo)系中，通過(guò)對(duì)地理坐標(biāo)系中的加速度信息計(jì)算得到運(yùn)載體在三軸中的運(yùn)動(dòng)速度和位置信息[14]。

在進(jìn)行坐標(biāo)系轉(zhuǎn)換時(shí)，采用旋轉(zhuǎn)矩陣來(lái)表示坐標(biāo)間的轉(zhuǎn)換關(guān)系。陀螺儀繞X、Y、Z三軸的轉(zhuǎn)動(dòng)角度分別為α、β、γ可通過(guò)對(duì)陀螺儀測(cè)量的三軸角速度積分獲得。歐拉角與旋轉(zhuǎn)矩陣的轉(zhuǎn)換關(guān)系為：

R(α,β,γ)=RX(α)RY(β)RZ(γ)=

(1)

式中，歐拉角α、β、γ對(duì)應(yīng)的旋轉(zhuǎn)矩陣分別為RX(α)，RY(β)，RZ(γ)。采用計(jì)算量相對(duì)較小的四元素法對(duì)姿態(tài)矩陣實(shí)時(shí)更新，從而將加速度信號(hào)從運(yùn)載體坐標(biāo)系轉(zhuǎn)換到地理坐標(biāo)系下，轉(zhuǎn)換關(guān)系如下：

(2)

通過(guò)坐標(biāo)轉(zhuǎn)換得到地理坐標(biāo)系中的加速度信號(hào)后，分別對(duì)三個(gè)軸向上的加速度信號(hào)a積分可獲得采樣周期內(nèi)運(yùn)載體在各軸向上的運(yùn)動(dòng)速度v和位移s，計(jì)算公式如下：

(3)

(4)

式中，Ts為加速度計(jì)測(cè)量采樣周期。當(dāng)運(yùn)動(dòng)控制平臺(tái)按照預(yù)定軌跡在空間運(yùn)動(dòng)時(shí)，慣性測(cè)量單元連續(xù)采集陀螺儀數(shù)據(jù)以及加速度信息。若測(cè)量的陀螺儀、加速度信息存在誤差，積分得到的運(yùn)動(dòng)速度以及位移曲線會(huì)出現(xiàn)嚴(yán)重偏差問(wèn)題，因此在軌道檢測(cè)中需提高測(cè)量數(shù)據(jù)的精確度。

1.2 雙目視覺(jué)測(cè)量原理

雙目立體視覺(jué)是利用兩個(gè)攝像機(jī)從不同的位置拍攝被測(cè)物體，通過(guò)計(jì)算同一空間物理點(diǎn)在不同圖像中的映像點(diǎn)之間的位置偏差從而獲取物理點(diǎn)的空間三維坐標(biāo)的方法[15]。圖2為雙目視覺(jué)系統(tǒng)示意圖，使用兩個(gè)不同位置的攝像機(jī)同時(shí)拍攝同一物體的圖像，已知任意一點(diǎn)P在兩個(gè)攝像機(jī)中的投影點(diǎn)分別是p和p′，根據(jù)三角幾何關(guān)系計(jì)算P點(diǎn)在兩幅圖像中的雙目視差便能獲得該點(diǎn)的三維空間坐標(biāo)值。同理，可得到其他空間點(diǎn)的三維空間坐標(biāo)值，實(shí)現(xiàn)圖像到真實(shí)世界的三維場(chǎng)景重建。

圖2 雙目視覺(jué)系統(tǒng)示意圖

(5)

(6)

(7)

式中，f為相機(jī)焦距。

選取標(biāo)定板原點(diǎn)作為坐標(biāo)系原點(diǎn)建立世界坐標(biāo)系，根據(jù)雙目相機(jī)定標(biāo)原理計(jì)算出相機(jī)坐標(biāo)系與世界坐標(biāo)系轉(zhuǎn)換的旋轉(zhuǎn)和平移關(guān)系，實(shí)現(xiàn)相機(jī)坐標(biāo)系中空間點(diǎn)P(XC,YC,ZC)到世界坐標(biāo)系的轉(zhuǎn)換。兩坐標(biāo)系之間的關(guān)系為：

(8)

當(dāng)運(yùn)動(dòng)控制平臺(tái)按照預(yù)先設(shè)置的軌跡連續(xù)運(yùn)動(dòng)時(shí)，左右兩側(cè)攝像機(jī)以固定頻率連續(xù)拍攝標(biāo)定板圖像，確保拍攝的標(biāo)定板圖像信息完整。通過(guò)對(duì)連續(xù)拍攝的圖像進(jìn)行標(biāo)定、特征點(diǎn)匹配等計(jì)算，得到攝像機(jī)在三維空間內(nèi)的運(yùn)動(dòng)軌跡[16]。在圖像拍攝過(guò)程中，攝像機(jī)容易受到振動(dòng)和光線等環(huán)境因素的影響，導(dǎo)致圖像缺失或者圖像清晰度不夠，對(duì)后續(xù)的標(biāo)定、計(jì)算產(chǎn)生影響導(dǎo)致檢測(cè)結(jié)果不準(zhǔn)確，因此在圖像采集時(shí)需保證攝像機(jī)的高分辨率和拍攝圖像的高清晰度，存在一定的難度。

2 基于擴(kuò)展卡爾曼濾波算法的多傳感器數(shù)據(jù)融合

擴(kuò)展卡爾曼濾波是通過(guò)預(yù)測(cè)值結(jié)合測(cè)量值對(duì)當(dāng)前系統(tǒng)狀態(tài)的最優(yōu)估計(jì)值進(jìn)行估計(jì)的，利用這一特性對(duì)雙目視覺(jué)、慣性單元測(cè)量的加速度信號(hào)和陀螺儀數(shù)據(jù)進(jìn)行融合。將慣性測(cè)量單元的加速度計(jì)和陀螺儀的測(cè)量值作為預(yù)測(cè)值，雙目視覺(jué)測(cè)量值作為觀測(cè)值，利用擴(kuò)展卡爾曼濾波實(shí)現(xiàn)多傳感信息的融合，得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)值。雙目視覺(jué)與慣性單元的加速度計(jì)和陀螺儀測(cè)量數(shù)據(jù)融合模型如圖3所示。

圖3 視覺(jué)與慣性信息融合模型

具體融合過(guò)程如下：首先，通過(guò)坐標(biāo)系轉(zhuǎn)換將各傳感器的直接測(cè)量數(shù)據(jù)轉(zhuǎn)換到世界坐標(biāo)系下；其次，利用雙目視覺(jué)系統(tǒng)解算得到的運(yùn)載體位置、姿態(tài)信息與慣性單元測(cè)量計(jì)算得到的運(yùn)載體位置、姿態(tài)信息分別求差，將差值帶入擴(kuò)展卡爾曼濾波器進(jìn)行優(yōu)化，估計(jì)出慣性單元測(cè)量的位置誤差、姿態(tài)誤差；最后，把估計(jì)出的位置誤差、姿態(tài)誤差反饋回慣性單元測(cè)量下一個(gè)周期的檢測(cè)計(jì)算中進(jìn)行誤差補(bǔ)償和反饋校正。直至循環(huán)結(jié)束，得到測(cè)量系統(tǒng)的所有參數(shù)的最優(yōu)估計(jì)值。將修正后慣性單元測(cè)量輸出的位置信息結(jié)合融合后的姿態(tài)角實(shí)現(xiàn)運(yùn)載體空間定位，即可得到測(cè)量系統(tǒng)在世界坐標(biāo)系中的運(yùn)行軌跡。

雙目視覺(jué)與慣性單元組合測(cè)量系統(tǒng)的狀態(tài)向量由慣性單元測(cè)量的加速度信號(hào)計(jì)算得到的世界坐標(biāo)系中的速度信息、位置信息以及慣性單元的測(cè)量誤差構(gòu)成。設(shè)雙目視覺(jué)與慣性測(cè)量單元組合測(cè)量系統(tǒng)的狀態(tài)向量為[17-18]：

(9)

利用擴(kuò)展卡爾曼濾波算法得到任意時(shí)刻測(cè)量系統(tǒng)的狀態(tài)方程為：

(10)

其中:aG為世界坐標(biāo)系下的加速度數(shù)據(jù)，可通過(guò)對(duì)加速度計(jì)直接測(cè)量值進(jìn)行坐標(biāo)轉(zhuǎn)換得到。

測(cè)量系統(tǒng)的量測(cè)方程由攝像機(jī)拍攝圖片計(jì)算得到的空間特征點(diǎn)組成。任意空間點(diǎn)在攝像機(jī)坐標(biāo)系和像素坐標(biāo)系中的坐標(biāo)分別為(xi,yi,zi)和(ui,vi)，根據(jù)兩者之間的轉(zhuǎn)換關(guān)系得出n個(gè)特征點(diǎn)構(gòu)成的測(cè)量方程為：

(11)

利用擴(kuò)展卡爾曼濾波算法得出t時(shí)刻系統(tǒng)狀態(tài)協(xié)方差為：

P(t|t)=(I-Kg(t)HP(t|t-1))

(12)

式中，P(t|t-1)為系統(tǒng)上一時(shí)刻的協(xié)方差，Kg(t)為t時(shí)刻系統(tǒng)的卡爾曼增益，計(jì)算公式為：

(13)

式中，H為測(cè)量系統(tǒng)的輸出矩陣，R(t)表示測(cè)量噪聲協(xié)方差。由此計(jì)算出t時(shí)刻的信息融合結(jié)果：

p(t)=pI(t)+Kg(t)(pI(t)-Zp(t))

(14)

θ(t)=θI(t)+Kg(t)(θI(t)-Zθ(t))

(15)

式中，pI(t)、θI(t)為t時(shí)刻慣性測(cè)量輸出的位置和姿態(tài)，Zp(t)、Zθ(t)為t時(shí)刻慣性測(cè)量輸出的修正量，p(t)、θ(t)為經(jīng)多傳感信息融合模型輸出的t時(shí)刻融合位置和姿態(tài)角信息，即擴(kuò)展卡爾曼濾波輸出的最優(yōu)估計(jì)值。完成當(dāng)前時(shí)刻的濾波計(jì)算后，系統(tǒng)依據(jù)收到的下一時(shí)刻慣性測(cè)量參數(shù)解算輸出進(jìn)入下一周期的濾波運(yùn)算。直至結(jié)束即可遞推求解出最優(yōu)軌道空間線形參數(shù)[19]。

3 實(shí)驗(yàn)驗(yàn)證

3.1 實(shí)驗(yàn)條件

采用如圖4所示的控制平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證。系統(tǒng)所用到的實(shí)驗(yàn)設(shè)備包括，陀螺儀和加速度計(jì)組成的慣性測(cè)量單元、標(biāo)定板、兩臺(tái)數(shù)字?jǐn)z像機(jī)。

圖4 軌道線形檢測(cè)實(shí)驗(yàn)平臺(tái)

3.2 實(shí)驗(yàn)過(guò)程

實(shí)驗(yàn)開(kāi)始前，設(shè)置好運(yùn)動(dòng)控制平臺(tái)在三個(gè)軸向上運(yùn)動(dòng)路徑。運(yùn)動(dòng)路徑由12段直線構(gòu)成，運(yùn)動(dòng)路線的真實(shí)節(jié)點(diǎn)如表1所示?？刂茩z測(cè)平臺(tái)多軸運(yùn)動(dòng)，保證左右攝像機(jī)均可拍攝到完整的定標(biāo)板圖像。

實(shí)驗(yàn)開(kāi)始后，按照預(yù)定的路線移動(dòng)控制平臺(tái)，左右兩側(cè)攝像機(jī)連續(xù)拍攝標(biāo)定板圖像，并同步采集運(yùn)動(dòng)狀態(tài)下的慣性測(cè)量單元陀螺儀數(shù)據(jù)以及加速度信息。

3.3 實(shí)驗(yàn)結(jié)果

在采集到的所有數(shù)據(jù)中等間隔選取12組圖像及每組圖像對(duì)應(yīng)幀慣性單元的加速度計(jì)和陀螺儀測(cè)量數(shù)據(jù)，利用Matlab軟件編程進(jìn)行仿真得到運(yùn)動(dòng)控制平臺(tái)在世界坐標(biāo)系下的運(yùn)動(dòng)坐標(biāo)及姿態(tài)。表1為運(yùn)動(dòng)曲線節(jié)點(diǎn)真實(shí)坐標(biāo)，表2為單獨(dú)使用慣性單元測(cè)量計(jì)算得到的世界坐標(biāo)系中的運(yùn)動(dòng)坐標(biāo)；表3為雙目視覺(jué)與慣性信息融合模型計(jì)算得到的世界坐標(biāo)系中的運(yùn)動(dòng)坐標(biāo)。

表1 運(yùn)動(dòng)曲線節(jié)點(diǎn)真實(shí)坐標(biāo) mm

表2和表3給出了采用常規(guī)慣性測(cè)量方法與多傳感信息融合方法計(jì)算得到的運(yùn)動(dòng)控制平臺(tái)在世界坐標(biāo)系下的運(yùn)動(dòng)坐標(biāo)。對(duì)比表1、表2和表3的數(shù)據(jù)，可以看出采用多傳感信息融合方法計(jì)算得到的運(yùn)動(dòng)坐標(biāo)更接近通過(guò)運(yùn)動(dòng)控制平臺(tái)設(shè)計(jì)的運(yùn)動(dòng)路徑的真實(shí)節(jié)點(diǎn)坐標(biāo)。為更直觀地看出兩種測(cè)量方法的區(qū)別，圖5中的(a)，(b)，(c)為采用常規(guī)慣性測(cè)量方法與多傳感信息融合方法輸出結(jié)果在X、Y、Z三軸位移誤差對(duì)比圖。通過(guò)分析可知，采用多傳感信息融合方法計(jì)算結(jié)果在三個(gè)軸上的絕對(duì)偏差明顯小于常規(guī)慣性檢測(cè)方法。相對(duì)于慣性單元測(cè)量方法的誤差大小，多傳感信息融合方法在X、Y、Z三個(gè)軸向最大絕對(duì)偏差分別為0.536 mm、0.455 mm、0.460 mm，檢測(cè)精度提高近9倍。說(shuō)明多傳感信息融合檢測(cè)方法得出的結(jié)果具有較高的準(zhǔn)確性。

表2 慣性測(cè)量在世界坐標(biāo)系下的運(yùn)動(dòng)坐標(biāo) mm

表3 融合模型在世界坐標(biāo)系下的運(yùn)動(dòng)坐標(biāo) mm

圖5 不同測(cè)量方法三軸位移誤差對(duì)比圖

4 結(jié)論

利用擴(kuò)展卡爾曼濾波算法將雙目視覺(jué)與慣性測(cè)量單元加速度計(jì)、陀螺儀測(cè)量數(shù)據(jù)進(jìn)行融合能夠消除傳感器在測(cè)量過(guò)程中存在的誤差，增強(qiáng)檢測(cè)數(shù)據(jù)的精確度，達(dá)到提高檢測(cè)精度的目的。通過(guò)搭建測(cè)量平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證，慣性測(cè)量單元測(cè)量計(jì)算的結(jié)果在X、Y、Z三個(gè)軸向最大絕對(duì)偏差分別為4.501 mm、4.192 mm、4.220 mm，多傳感信息融合模型輸出結(jié)果在X、Y、Z三個(gè)軸向最大絕對(duì)偏差分別為0.536 mm、0.455 mm、0.460 mm。與常規(guī)的慣性檢測(cè)方法相比較，多傳感信息融合測(cè)量方法精度提高了近9倍。證明多傳感信息融合方法能夠有效地提高軌道空間線形檢測(cè)精度，具有較高的實(shí)用價(jià)值。