一種基于視覺及慣性信息融合的軌道空間線形測量方法
【專利摘要】本發(fā)明涉及一種基于視覺及慣性信息融合的軌道空間線形測量方法，用以獲取軌道結(jié)構(gòu)的沉降變化量，包括以下步驟：1)分別對(duì)運(yùn)載體地理坐標(biāo)系和運(yùn)載體坐標(biāo)系進(jìn)行定標(biāo)；2)通過安裝在低速運(yùn)動(dòng)的運(yùn)載體上的慣性測量單元和視覺單元實(shí)時(shí)檢測到的運(yùn)動(dòng)數(shù)據(jù)并據(jù)此計(jì)算獲取軌道空間初步線形；3)根據(jù)視覺單元實(shí)時(shí)檢測到的圖像進(jìn)行特征提取、特征匹配和投影變換，獲取投影變換矩陣中的平移向量，并據(jù)此獲得軌道空間圖像線形；4)對(duì)軌道空間初步線形和軌道空間圖像線形進(jìn)行融合，獲取運(yùn)載體最終空間線形，最終獲得軌道結(jié)構(gòu)的沉降變化量。與現(xiàn)有技術(shù)相比，本發(fā)明具有測量準(zhǔn)確、適用于低速檢測等優(yōu)點(diǎn)。
【專利說明】
一種基于視覺及慣性信息融合的軌道空間線形測量方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及軌道安全領(lǐng)域，尤其是涉及一種基于視覺及慣性信息融合的軌道空間 線形測量方法。
【背景技術(shù)】
[0002] 當(dāng)前隨著列車速度的提升，對(duì)軌道結(jié)構(gòu)的穩(wěn)定性要求越來越高，而軌道沉降嚴(yán)重 影響鐵路行車平順性、舒適度、安全性。軌道沉降技術(shù)上主要表現(xiàn)為對(duì)軌道空間線形的檢 測，空間上主要表現(xiàn)為軌道的長波不平順。軌道結(jié)構(gòu)的不平順變形是列車產(chǎn)生振動(dòng)的主要 根源，是車輛-軌道的激擾函數(shù)，直接影響輪軌間相互作用及列車運(yùn)行的安全性，平穩(wěn)性及 舒適性。
[0003] 目前，對(duì)于軌道狀態(tài)檢測主要分為靜態(tài)檢測與軌檢車動(dòng)態(tài)檢測。軌道沉降靜態(tài)檢 測采用人工測量，或借助全站儀、GPS精測網(wǎng)，其主要的缺點(diǎn)是效率低，無法實(shí)現(xiàn)長距離連續(xù) 運(yùn)動(dòng)狀態(tài)下的測量，且靜態(tài)測量的數(shù)據(jù)是建立在絕對(duì)靜止的坐標(biāo)系下的沉降值。高速運(yùn)動(dòng) 狀態(tài)下軌道沉降檢測方法除了靜態(tài)檢測方法所能檢測的項(xiàng)目之外，還能夠?qū)壍儡囕v在動(dòng) 力作用下的軌道產(chǎn)生的彈性形變進(jìn)行檢測，同時(shí)可以測定車輛垂向和橫向加速度，該方法 依靠的檢測設(shè)備依然為軌檢車，但是其測量的軌道沉降數(shù)據(jù)是建立在自身隨車體運(yùn)動(dòng)的相 對(duì)坐標(biāo)系下。而且，其檢測效果受速度影響較大，不宜低速檢測，引入的其他輔助設(shè)備，也未 能完全將建立在軌檢車上的動(dòng)態(tài)坐標(biāo)系與建立在空間中的靜態(tài)基準(zhǔn)坐標(biāo)系取得聯(lián)系。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明的目的就是為了克服上述現(xiàn)有技術(shù)存在的缺陷而提供一種測量準(zhǔn)確、適用 于低速檢測的基于視覺及慣性信息融合的軌道空間線形測量方法。
[0005] 本發(fā)明的目的可以通過以下技術(shù)方案來實(shí)現(xiàn)：
[0006] 1.-種基于視覺及慣性信息融合的軌道空間線形測量方法，用以獲取軌道結(jié)構(gòu)的 沉降變化量，包括以下步驟：
[0007] 1)分別對(duì)運(yùn)載體地理坐標(biāo)系和運(yùn)載體坐標(biāo)系進(jìn)行定標(biāo)；
[0008] 2)通過安裝在低速運(yùn)動(dòng)的運(yùn)載體上的慣性測量單元和視覺單元實(shí)時(shí)檢測到的運(yùn) 動(dòng)數(shù)據(jù)并據(jù)此計(jì)算獲取軌道空間初步線形；
[0009] 3)根據(jù)視覺單元實(shí)時(shí)檢測到的圖像進(jìn)行特征提取、特征匹配和投影變換，獲取投 影變換矩陣中的平移向量，并據(jù)此獲得軌道空間圖像線形；
[0010] 4)對(duì)軌道空間初步線形和軌道空間圖像線形進(jìn)行融合，獲取運(yùn)載體最終空間線 形，最終獲得軌道結(jié)構(gòu)的沉降變化量。
[0011] 所述的步驟2)具體包括以下步驟：
[0012] 21)根據(jù)視覺單元拍攝的圖像設(shè)定地理坐標(biāo)系的基準(zhǔn)點(diǎn)；
[0013] 22)通過慣性測量單元中的加速度計(jì)獲取運(yùn)載體坐標(biāo)系下運(yùn)載體的三軸加速度， 通過慣性測量單元中的陀螺儀獲取運(yùn)載體坐標(biāo)系下運(yùn)載體的角速度和方向角；
[0014] 23)根據(jù)角速度和方向角對(duì)運(yùn)載體坐標(biāo)系下的三軸加速度進(jìn)行坐標(biāo)系轉(zhuǎn)換，獲得 地理坐標(biāo)系下的三軸加速度；
[0015] 24)根據(jù)視覺單元中的雙目攝像機(jī)實(shí)時(shí)獲取運(yùn)載體的單目視覺連續(xù)圖像信息以及 方向角，對(duì)慣性測量單元獲取的三軸加速度中的低頻成分進(jìn)行補(bǔ)償；
[0016] 25)對(duì)補(bǔ)償后的三軸加速度進(jìn)行雙重積分，獲取運(yùn)載體的初步位移，并根據(jù)初步位 移獲取軌道空間初步線形。
[0017] 所述的步驟23)中的坐標(biāo)系轉(zhuǎn)換采用四元數(shù)法進(jìn)行坐標(biāo)系轉(zhuǎn)換，則有：
[0021] 其中，Rq為坐標(biāo)轉(zhuǎn)換矩陣，atx、aty、atz分別為地理坐標(biāo)系下的三軸加速度，a bx、aby、 abz分別為運(yùn)載體坐標(biāo)系下的三軸加速度，qi、q2、q3、q4分別為中間變量，a、0、y分別為繞x， y，z軸旋轉(zhuǎn)的角度。
[0022]所述的步驟24)中，三軸加速度中的低頻成分進(jìn)行補(bǔ)償包括采用方向角對(duì)三軸加 速度中的低頻成分進(jìn)行位移方向補(bǔ)償，以及采用單目視覺連續(xù)圖像信息對(duì)三軸加速度中的 低頻成分進(jìn)行位移大小補(bǔ)償。
[0023] 所述的步驟4)中，采用擴(kuò)展卡爾曼濾波對(duì)軌道空間初步線形和軌道空間圖像線形 進(jìn)行融合。
[0024] 與現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點(diǎn)：
[0025] -、測量準(zhǔn)確:本發(fā)明首先根據(jù)低速補(bǔ)償后的三軸加速度數(shù)據(jù)進(jìn)行雙重積分獲取 軌道空間初步線形，提高了低速運(yùn)動(dòng)下產(chǎn)生線形的精度，并通過視覺單元的圖像進(jìn)行特征 提取、特征匹配和投影變換獲取了高精度的圖像線形，采用擴(kuò)展卡爾曼濾波進(jìn)行融合，獲取 了更高精度的線形，從而獲得了更高精度的沉降量。
[0026] 二、適用于低速檢測：由于慣性測量單元在運(yùn)載體速度較低檢測時(shí)誤差比較大，主 要是低速狀態(tài)下，慣性測量單元所測加速度信號(hào)比較微弱，信噪比比較低，且對(duì)加速度進(jìn)行 雙重積分獲得位移信息過程中，低頻信號(hào)容易使積分達(dá)到飽和，本發(fā)明采用分別對(duì)位移方 向和大小進(jìn)行雙重補(bǔ)償?shù)姆绞?，解決了低速檢測下信息丟失的問題。
【附圖說明】
[0027]圖1為本方法的流程原理圖。
[0028]圖2為軌檢系統(tǒng)測量圖。
[0029]圖3為軌檢系統(tǒng)坐標(biāo)轉(zhuǎn)換圖。
[0030]圖4為平臺(tái)移動(dòng)前后坐標(biāo)相對(duì)位置示意圖。
[0031]圖5為軌道沉降計(jì)算原理圖。
[0032]圖6為軌道空間線形檢測系統(tǒng)示意圖。
[0033]圖7為機(jī)器人手眼定標(biāo)示意圖。
[0034]圖8為測量獲得的空間位移曲線。
【具體實(shí)施方式】
[0035]下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明進(jìn)行詳細(xì)說明。
[0036] 實(shí)施例：
[0037]如圖1所示，圖1為本方法的流程原理圖。
[0038]本發(fā)明的主要測量方法如下：
[0039] 1、測量原理：
[0040] 連續(xù)運(yùn)動(dòng)方式下的沉降測量，表現(xiàn)為在同一基準(zhǔn)坐標(biāo)系下，不同時(shí)間段，同一軌道 線路內(nèi)各個(gè)測點(diǎn)空間坐標(biāo)的差值，如圖5所示。反應(yīng)在一次測量過程中，即是軌道空間現(xiàn)行 的檢測，如圖6所示，而軌道空間線形的高精度檢測決定了軌道沉降的準(zhǔn)確計(jì)算。
[0041] 本發(fā)明的檢測系統(tǒng)主要由慣性測量單元(MU)，傾角傳感器，雙目攝像機(jī)以及用于 計(jì)算的工控機(jī)和相應(yīng)的軟件組成。其中慣性測量單元中的加速度計(jì)用來輸出當(dāng)前時(shí)刻的三 軸加速度信息，陀螺儀用來輸出當(dāng)前時(shí)刻的角速度以及角度信息，傾角儀輸出的兩軸歐拉 角，可用于低速狀態(tài)下與陀螺儀角度加權(quán)融合，雙目攝像機(jī)拍攝的圖像信息，用于對(duì)慣性單 元低頻信息的補(bǔ)償，以及與慣性測量單元信息融合，實(shí)現(xiàn)長距離累積誤差的校正。
[0042] 提出了基于慣性測量單元及視覺傳感器信息融合的方式測量軌道空間線形，通過 對(duì)慣性測量單元輸出的加速度信息進(jìn)行二次積分，獲得軌道空間位移，并針對(duì)慣性測量單 元輸出的空間域低頻成分不準(zhǔn)確問題，提出了多傳感器信息融合互補(bǔ)的方法，最后通過卡 爾曼濾波對(duì)視覺傳感器與補(bǔ)償后的慣性測量單元信息進(jìn)行融合，從而達(dá)到軌道空間線形高 精度測量的目的，并在初始時(shí)刻利用視覺傳感器拍攝靜態(tài)基準(zhǔn)點(diǎn)的方式來建立絕對(duì)坐標(biāo)系 與相對(duì)坐標(biāo)系之間的聯(lián)系。
[0043] 2、慣性信息處理方法
[0044]檢測軌道空間線形。利用慣性測量單元中陀螺儀輸出的角度及角速度信息對(duì)加速 度計(jì)輸出的加速度信息姿態(tài)進(jìn)行轉(zhuǎn)換，轉(zhuǎn)換后的加速度信息雙重積分即可計(jì)算出位移信 息，從而得出軌道空間線形。
[0045]常用的慣性導(dǎo)航坐標(biāo)系為東_北-地坐標(biāo)系，構(gòu)建其坐標(biāo)系的三個(gè)分為：指向東方 的E分量，指向北方的N分量，指向本地重力加速度方向的D分量，在本地導(dǎo)航過程中，一般情 況下，運(yùn)載體運(yùn)動(dòng)的范圍相對(duì)地球半徑較小，可直接認(rèn)為運(yùn)載體前進(jìn)方向即為N方向，運(yùn)載 體靜止時(shí)，當(dāng)?shù)刂亓铀俣确较驗(yàn)镈方向，則此時(shí)的東-北-地坐標(biāo)系也就是常說的地理坐標(biāo) 系，亦即依托運(yùn)載體初始化時(shí)刻所處的地理位置建立的坐標(biāo)系，記為t系，運(yùn)載體坐標(biāo)系，即 建立在運(yùn)載體上，沿著運(yùn)動(dòng)軌跡時(shí)刻變化的坐標(biāo)系，記為b系。
[0046] 用四元素法對(duì)旋轉(zhuǎn)矩陣進(jìn)行表述：
[0047] 對(duì)于平面上單位圓上任意一點(diǎn)可以代表一個(gè)旋轉(zhuǎn)角，則空間中單位球上任意一點(diǎn) 代表了繞兩個(gè)軸旋轉(zhuǎn)的角度。因此，假設(shè)存在四維空間中的單位球，則在該單位球上任取一 點(diǎn)，可看做繞三個(gè)坐標(biāo)軸旋轉(zhuǎn)角度。假設(shè)在單位球上存在一點(diǎn)Q=(qo qi q2 q3)，則運(yùn)載體 坐標(biāo)系到地理坐標(biāo)系的旋轉(zhuǎn)可用四元數(shù)法來表示。其具體過程如下所示：
[0048] Q = q〇+qi i +q2 j+q3k (2-1)
[0049]其中9〇，如，屯，陽為實(shí)數(shù)，1，」少為四元數(shù)的正交基。四元數(shù)的微分方程形式為：
[0051]其中+ + / + <「7t為運(yùn)載體坐標(biāo)系到地理坐標(biāo)系角速度的四元數(shù)表達(dá) 形式，將式(2-2)寫成矩陣形式：
[0053]其中<、<、<分別表示載體坐標(biāo)系下陀螺儀輸出的三軸角速度。
[0054]對(duì)于式（2-2)的求解可以采用四階龍格庫塔法，假設(shè)時(shí)間間隔為T，可得四元數(shù)的 遞推公式：
[0061] 其中M為的反對(duì)稱陣。
[0062] 由此便可求得單位四元素Q的各個(gè)分量大小。由四元素表示的旋轉(zhuǎn)矩陣為：
[0064]且四元素與歐拉角之間的關(guān)系為：
[0066] 其中a，0, y分別為繞x，y，z軸旋轉(zhuǎn)的角度。
[0067] 利用安裝于運(yùn)動(dòng)載體上的加速度計(jì)實(shí)時(shí)輸出運(yùn)載體的加速度，陀螺儀輸出角速率 對(duì)其姿態(tài)更新，從而實(shí)現(xiàn)動(dòng)態(tài)的載體坐標(biāo)系下的加速度信號(hào)轉(zhuǎn)換到初始的地理坐標(biāo)系下， 最后對(duì)轉(zhuǎn)換到地理坐標(biāo)系下的加速度進(jìn)行雙重積分，獲取位移信息。則
[0069]其中，at表示地理坐標(biāo)系下的加速度，ab表示運(yùn)載體坐標(biāo)系下的加速度。
[0070] 對(duì)轉(zhuǎn)換后的加速度信號(hào)進(jìn)行雙重積分，即可獲得運(yùn)載體的位移信息，從而反應(yīng)出 軌道空間線形。
[0071] 由于慣性測量單元在運(yùn)載體速度較低檢測時(shí)誤差比較大，主要是低速狀態(tài)下，慣 性測量單元所測加速度信號(hào)比較微弱，信噪比比較低，且對(duì)加速度進(jìn)行雙重積分獲得位移 信息過程中，低頻信號(hào)容易使積分達(dá)到飽和。因此為保持位移信息的連續(xù)準(zhǔn)確輸出，測量過 程中需要去除低頻成分，但采集到的低頻成分對(duì)應(yīng)著軌道線形中的平緩部分，去掉之后會(huì) 造成有用信息丟失，所以利用單目視覺連續(xù)拍攝的圖像信息對(duì)位移大小進(jìn)行補(bǔ)償，陀螺儀 輸出角度信息更新姿態(tài)矩陣對(duì)位移方向進(jìn)行補(bǔ)償，從而實(shí)現(xiàn)對(duì)低頻成分有效的補(bǔ)償。
[0072] 軌道沉降檢測示意圖如圖2所示，運(yùn)動(dòng)狀態(tài)下，不同時(shí)刻傳感器所測的數(shù)據(jù)處于該 時(shí)刻該傳感器坐標(biāo)系下，則整個(gè)測量過程可簡化為坐標(biāo)轉(zhuǎn)換關(guān)系。任意相鄰位置上的低頻 信息補(bǔ)償算法及坐標(biāo)系示意圖如圖3所示。
[0073]正常情況下，在運(yùn)動(dòng)過程中，由于運(yùn)載體車輪始終與軌道相互接觸，方向?yàn)檠刂?道的方向，因此，低頻狀態(tài)下位移的方向可近似的認(rèn)為沿著X'軸（即沿軌道方向），大小待 求，則問題轉(zhuǎn)化為求解位移t在地理坐標(biāo)系下的坐標(biāo)(xt，yt，z t)
[0075]由于位移t的方向沿著x'軸，且旋轉(zhuǎn)角度可通過陀螺儀直接測得，因此位移t可以 看做由地理坐標(biāo)系的x軸反向旋轉(zhuǎn)得到，則
[0077]對(duì)式（2-7)左乘 RQi-、則
[0079]其中位移大小，| |t| | =S(待求），將式(2-8)代入(2-6)得
[0081]若要求解在大地坐標(biāo)系下的坐標(biāo)(x，y，z)，可利用相機(jī)拍攝基準(zhǔn)點(diǎn)的方式來獲取。 即
[0083]將（2-9)帶入（2-10)得
[0085] 其中，Re，te分別為定義在慣性導(dǎo)航系統(tǒng)下的地理坐標(biāo)系向定義在軌道空間線形下 的絕對(duì)大地坐標(biāo)系轉(zhuǎn)換的旋轉(zhuǎn)矩陣和平移向量。
[0086] 3、視覺信息處理方法
[0087]用圖像數(shù)據(jù)設(shè)計(jì)相應(yīng)算法，將二維的圖像信號(hào)，轉(zhuǎn)化為一維的位移信號(hào)，進(jìn)行位移 大小的計(jì)算。該算法需要對(duì)不同時(shí)刻采集到的圖像進(jìn)行特征提取，特征匹配以及計(jì)算透視 投影變換矩陣，投影矩陣中的平移向量的模即為位移的大小。
[0088] (1)特征提取
[0089] 利用SURF算法，通過計(jì)算Hessian矩陣的行列式來近似圖像。在構(gòu)造Hessian矩陣 之前，需要先對(duì)圖像高斯濾波。
[0090] (2)特征匹配
[0091] 對(duì)提取到的同一場景不同圖像的特征點(diǎn)，采用快速最近鄰逼近搜索函數(shù)庫 (FLANN)進(jìn)行初匹配，獲取初始的特征點(diǎn)信息。
[0092] (3)特征點(diǎn)提純
[0093] 初始匹配獲得的數(shù)據(jù)難免存在著大量誤匹配點(diǎn)對(duì)，若要剔除初始匹配點(diǎn)對(duì)中的誤 匹配點(diǎn)對(duì)，則可采用隨機(jī)采樣一致算法(RANSAC)，求得較好的匹配結(jié)果。
[0094] (4)得到最優(yōu)的匹配點(diǎn)對(duì)后，利用最小二乘法即可確定這些內(nèi)點(diǎn)之間的最優(yōu)投影 矩陣。最小二乘法求解投影矩陣及求解位移大小過程如下：
[0095] 記(幻1，711)4 = 1，2...11為目標(biāo)圖像匹配特征點(diǎn)的坐標(biāo)，（妨，721)為原圖像匹配特 征點(diǎn)的坐標(biāo)，貝1J:
[0098] 將(3-1)式改寫為：
[0101] 則原圖像與目標(biāo)圖像匹配特征點(diǎn)之間的投影矩陣參數(shù)的最小二乘解為：
[0102] X=(ATA)_1ATb (3-3)
[0103] 由此便可獲得原圖像與目標(biāo)圖像匹配特征點(diǎn)之間的投影矩陣參數(shù)。
[0104] (5)位移大小求解
[0105]因?yàn)橄鄼C(jī)與慣性單元之間為剛性連接，相同時(shí)間內(nèi)，慣性測量單元移動(dòng)的距離等 于相機(jī)移動(dòng)的距離，而相機(jī)移動(dòng)的距離又等于圖像坐標(biāo)系原點(diǎn)偏移的距離。因此，位移大小 S為：
[0107] 慣性單元與視覺傳感器之間的采樣頻率不同，則位移大小S需要進(jìn)行插值計(jì)算，以 達(dá)到時(shí)間上的匹配。本文采用的是均分法，對(duì)位移大小進(jìn)行分配，從而達(dá)到時(shí)間匹配的目 的。
[0108] 4、定標(biāo)方法
[0109] 本課題所涉及的傳感器定標(biāo)為慣性測量單元與視覺傳感器之間相對(duì)位置關(guān)系的 確定。慣性單元與視覺傳感器之間相對(duì)位置，可通過基于機(jī)器人手眼定標(biāo)方程的分步法求 取，并在求解過程中，引入傾角傳感器信息，對(duì)所求得的旋轉(zhuǎn)矩陣進(jìn)行優(yōu)化。求得旋轉(zhuǎn)矩陣 之后，將其帶入機(jī)器人手眼方程即可求得平移向量。
[0110] 機(jī)器人手眼定標(biāo)基本思想是利用攝像機(jī)拍攝目標(biāo)物體來跟蹤機(jī)器人手爪的位置， 從而測量目標(biāo)物體與手爪之間的相對(duì)空間位置。該問題的數(shù)學(xué)描述如圖7所示。
[0111] 空間中目標(biāo)物體的坐標(biāo)系記為Cobj，攝像機(jī)坐標(biāo)系記為C。，機(jī)器人手爪平臺(tái)坐標(biāo)系 記為Ce，攝像機(jī)坐標(biāo)系C。與目標(biāo)物體的坐標(biāo)系Cc^之間的旋轉(zhuǎn)矩陣及平移向量，記為R a，ta。 機(jī)器人手爪坐標(biāo)系Ce與攝像機(jī)坐標(biāo)系C。之間的旋轉(zhuǎn)矩陣及平移向量，記為R與t。確定旋轉(zhuǎn)矩 陣R與平移向量t的過程稱為機(jī)器人手眼定標(biāo)。
[0112]記
[0114]矩陣A與B分別表示平臺(tái)移動(dòng)前后攝像機(jī)與定標(biāo)參考坐標(biāo)系Cc^的相對(duì)位置，C表示 攝像機(jī)由Ccl移動(dòng)到Cc2時(shí)的旋轉(zhuǎn)矩陣與平移向量，則由雙目視覺定標(biāo)關(guān)系可得C = AB'求出 A與B后，C為已知矩陣。
[0115] 不同位置上的平臺(tái)坐標(biāo)系之間的旋轉(zhuǎn)矩陣和平移向量，可由機(jī)器人控制器讀出， 記為矩陣D，則由坐標(biāo)系轉(zhuǎn)換關(guān)系可得：
[0116] CX=XD (4-2)
[0117] 式(4-2)即為機(jī)器人手眼定標(biāo)的基本方程，X為待求參數(shù)。
[0118]將式(4-2)展開，則可寫為：
[0120] 由式(4-3)可得
[0121] RcR=RRd (4-4)
[0122] Rct+tc = Rtd+t (4-5)
[0123] 式(4-4)與(4-5)中Rc，Rd，tc，td均為已知參數(shù)，R與t為待求解的矩陣。
[0124] 本課題的定標(biāo)系統(tǒng)由慣性測量單元、視覺傳感器、傾角傳感器以及標(biāo)定板組成，空 間相對(duì)位置關(guān)系如圖4所示。
[0125] 在標(biāo)定板上建立世界坐標(biāo)系記為{w}，用來描述慣性及視覺傳感器在世界坐標(biāo)系 下的絕對(duì)位置。視覺傳感器坐標(biāo)系記為C。慣性測量單元坐標(biāo)系即載運(yùn)坐標(biāo)系，記為MU，用 來測量物體的線性加速度以及旋轉(zhuǎn)角速度。傾角儀坐標(biāo)系記為in，輸出其繞x，y軸轉(zhuǎn)動(dòng)的兩 個(gè)角度。慣性傳感器歐拉角坐標(biāo)系{e}，即初始測量時(shí)刻的地理坐標(biāo)系。
[0126] (1)慣性及視覺傳感器之間旋轉(zhuǎn)關(guān)系的求解
[0127] 利用慣性傳感器輸出的角速度以及加速度數(shù)據(jù)求解三軸歐拉角，同時(shí)利用攝像機(jī) 標(biāo)定算法獲取攝像機(jī)坐標(biāo)系與世界坐標(biāo)系之間的空間位置關(guān)系，從而獲得攝像機(jī)與慣性傳 感器之間的空間位置關(guān)系。傾角儀輸出的歐拉角的穩(wěn)定性比利用慣性傳感器計(jì)算得到的三 軸歐拉角穩(wěn)定性高，因此利用傾角儀的輸出信息優(yōu)化慣性及視覺傳感器之間旋轉(zhuǎn)矩陣的精 度。
[0128] (2)視覺與慣性傳感器之間的空間變換
[0129] 從不同視角拍攝m張同一標(biāo)定板的圖像，記為1^12，...Im，對(duì)于每張圖像h記錄該 時(shí)刻慣性傳感器的Roll角ai、Pitch角&以及Yaw角Y，同時(shí)利用攝像機(jī)標(biāo)定獲得該時(shí)刻世界 坐標(biāo)系到攝像機(jī)坐標(biāo)系的旋轉(zhuǎn)矩陣Rw-q。
[0130] 對(duì)于不同時(shí)刻的圖像UPL以及慣性傳感器相關(guān)數(shù)據(jù)，根據(jù)機(jī)器人手眼定標(biāo)基本 方程可得：
[0131] Rw-c,ij * Rm-c = Rm-c * Re-m,ij (4-6)
[0132] 其中，尺表示在i，j兩時(shí)刻攝像機(jī)坐標(biāo)系之間的旋轉(zhuǎn)矩陣， 足_/=尺^_見^表示在1，」兩時(shí)刻慣性傳感器坐標(biāo)系之間的旋轉(zhuǎn)矩陣。1^1,1表示1時(shí)刻 慣性傳感器歐拉角坐標(biāo)系到慣性傳感器坐標(biāo)系的旋轉(zhuǎn)矩陣，其中
[0134] 將式(4-7)帶入(4-6)可得：
[0136] 將Rw-c,ij和慣性傳感器的歐拉角€4、隊(duì)、<1」、的帶入到等式(4-8)中，并令￥"-。為旋轉(zhuǎn) 矩陣R m_。的向量化形式，可得如下線性方程：
[0137] AijVm-c = 0 (4-9)
[0138] 其中為式(4-8)經(jīng)過化簡后的系數(shù)矩陣。
[0139] 由于在不同時(shí)刻不同位置上拍攝的標(biāo)定板圖像為空間靜止不動(dòng)的，理論上講，其 中任意兩個(gè)不同位置的兩幅圖像均可形成一個(gè)約束條件，因此，對(duì)拍攝的m幅圖像進(jìn)行兩兩 組合，總共可得種不同的圖像對(duì)，即可得個(gè)形如式(4-9)的關(guān)于慣性傳感器坐標(biāo)系到 攝像機(jī)坐標(biāo)系下旋轉(zhuǎn)矩陣R m-。的方程組，即
[0141]由于Rm-C為旋轉(zhuǎn)矩陣，且因此對(duì)旋轉(zhuǎn)矩陣Rm-C的求解問題可轉(zhuǎn)化為如 下有約束的非線性優(yōu)化問題：
[0143] s.t.M = Rm-c,MTM=E3 (4-12)
[0144] 對(duì)于慣性傳感器坐標(biāo)系與攝像機(jī)坐標(biāo)系之間的旋轉(zhuǎn)矩陣Rm-。，可通過增廣的拉格 朗日乘子法進(jìn)行優(yōu)化求解。
[0145] (3)慣性及視覺傳感器之間平移關(guān)系的求解
[0146] 獲得慣性及視覺傳感器之間的旋轉(zhuǎn)矩陣后，根據(jù)手眼定標(biāo)基本方程有：
[0148] 其中，為攝像機(jī)坐標(biāo)系在i，j時(shí)刻的平移向量，R表示慣性傳感器坐標(biāo)系在i， j時(shí)刻的旋轉(zhuǎn)矩陣山,^為慣性傳感器在i，j時(shí)刻的平移向量，通過對(duì)慣性傳感器輸出的加 速度信息進(jìn)行雙重積分獲得。
[0149] 根據(jù)式(4-13)可得：
[0150] (Rw-c, ij-E3) tm-c = Rm-ctm, ij-tw-c, ij (4-14)
[0151] 由于的秩為2,等式(4-14)僅可提供兩個(gè)關(guān)于的獨(dú)立約束方程，因此， 需要控制平臺(tái)進(jìn)行至少兩次運(yùn)動(dòng)，獲得兩組(或更多組)形如式(4-14)的約束方程組，進(jìn)而 得到如下約束方程：
[0152] Ctm-c = D (4-15)
[0154] tm-c = C* ? D (4-16)
[0155] 其中C表示矩陣C的偽逆，求解得到tm-c。
[0156] 5、信息融合
[0157] 擴(kuò)展卡爾曼濾波是解決這類非線性問題的有效途徑，擴(kuò)展卡爾曼濾波將系統(tǒng)的非 線性函數(shù)做一階泰勒展開，對(duì)系統(tǒng)進(jìn)行線性化，再利用線性卡爾曼濾波完成對(duì)目標(biāo)的濾波 估計(jì)等處理。
[0158] 對(duì)于非線性離散系統(tǒng)的狀態(tài)方程以及量測方程如下：
[0159] X(k+l)=f[k,X(k)]+G(k)ff(k)
[0160] Z(k)=h[k,X(k)]+V(k)
[0161] 為了便于處理，假定系統(tǒng)中沒有控制量輸入，并且過程噪聲均值為零的高斯白噪 聲，噪聲分布矩陣G(k)是已知的。觀測噪聲V(k)也是均值為零的加性高斯白噪聲。過程噪聲 與觀測噪聲是彼此獨(dú)立的，初始狀態(tài)估計(jì)2(01〇)和協(xié)方差矩陣P(〇|〇)。與線性系統(tǒng)一樣， 擴(kuò)展的卡爾曼濾波依然具有如下五個(gè)核心公式：
[0162] 狀態(tài)預(yù)測：
[0164] 協(xié)方差預(yù)測：
[0165] P(k+1 |k) = 〇(k+l |k)P(k|k)〇T(k+l|k)+Q(k+l) (5-2)
[0166] 卡爾曼濾波增益：
[0167] K(k+l)=P(k+l |k)HT(k+l)[H(k+l)P(k+l|k)HT(k+l)+R(k+l)]- 1 (5-3)
[0168] 狀態(tài)更新：
[0169] Z(灸+ 11 々 +1) = (左 +11 /c)+ /〈（A: + l)「Z(/t +1) - AZ(灸+ 1 丨人^ (5-4)
[0170] 協(xié)方差更新：
[0171] P(k+l) = [I-K(k+l)H(k+l)]P(k+l |k) (5-5)
[0172] (1)系統(tǒng)狀態(tài)方程
[0173] 系統(tǒng)的狀態(tài)方程義=[(火f，(v/: )7',(/U",]為9行1列的列向量。其中％為慣性 測量單元在世界坐標(biāo)系下的位置；4為慣性測量單元在世界坐標(biāo)系下的線速度;1^和1^分 別為慣性測量單元測量得到的加速度偏差以及角速度偏差。慣性測量單元的加速度偏差及 角速度偏差是由白噪聲nb4Pn b4g動(dòng)的高斯隨機(jī)噪聲。
[0174] 由于前面已經(jīng)獲得了慣性及視覺傳感器之間的空間位置關(guān)系，則軌道沉降檢測系 統(tǒng)的狀態(tài)方程可表示如式(5-6)所示，其中T為慣性測量單元的采樣時(shí)間間隔。
[0176] 其中aC是慣性測量單元測量到的加速度在世界坐標(biāo)系下的表示，其與慣性測量單 元得到的線性加速度ai之間滿足如下關(guān)系：
[0177] u(i (/) = C{q[, {!)){(!,U)- /).(/) - ?,(/))
[0178] 其中C(')為由旋轉(zhuǎn)四元數(shù)表示的旋轉(zhuǎn)矩陣，它們之間轉(zhuǎn)化關(guān)系如下所示：
[0180] (2)量測方程
[0181] 設(shè)空間中任意一點(diǎn)PFC在攝像機(jī)坐標(biāo)系下的坐標(biāo)為(^，71，21)，在圖像中的像素坐 標(biāo)為(m， Vl)，將特征點(diǎn)的圖像坐標(biāo)作為量測值，量測方程由空間中的n個(gè)特征點(diǎn)構(gòu)成，記為 mi, (i = l，2,. . .，n)，則量測方程為：
[0185] 上式中pFG表示特征點(diǎn)在世界坐標(biāo)系下的坐標(biāo)，K為一個(gè)3行3列矩陣如下所示，其中 f?表示相機(jī)焦距。
[0187] 構(gòu)建好系統(tǒng)狀態(tài)方程及量測方程之后，由擴(kuò)展卡爾曼濾波的五個(gè)核心公式即可遞 推求解出最優(yōu)軌道空間線形，如圖8。實(shí)際測量過程中該特征點(diǎn)的空間三維坐標(biāo)可利用三維 重建算法獲得。
[0188] 本發(fā)明的硬件系統(tǒng)包括運(yùn)動(dòng)控制平臺(tái)以及簡易軌檢小車。其中運(yùn)動(dòng)控制平臺(tái)主要 是由慣性測量單元(F100A5)、C⑶工業(yè)相機(jī)、圖像采集卡、傾角傳感器、標(biāo)定板組成。
[0189] 而軌檢小車則由電力驅(qū)動(dòng)的一節(jié)拖車，與安裝于平板車上的慣性測量單元 (頂1]520)及易于實(shí)驗(yàn)所用的USB接口的工業(yè)攝像機(jī)組成。
[0190] 頂U(kuò)520是可靠的固態(tài)慣性測量單元。內(nèi)置三軸陀螺儀、三軸加速度計(jì)，并經(jīng)過精密 校準(zhǔn)。數(shù)據(jù)融合通過內(nèi)置DSP進(jìn)行擴(kuò)展卡爾慢濾波器實(shí)現(xiàn)。其具有如下主要特性:震動(dòng)環(huán)境 下良好的性能。精確校準(zhǔn)陀螺儀和加速度的零點(diǎn)、零點(diǎn)溫度系數(shù)、靈敏度、靈敏度溫度系數(shù)、 正交性誤差。500Hz快速計(jì)算，128kSPS高速同步采樣保證運(yùn)動(dòng)細(xì)節(jié)的計(jì)算，抑制混疊，提高 信噪比。數(shù)據(jù)包速率最高250Hz，RS232接口。低功耗:典型0.25A(12V)。
[0191] 數(shù)字?jǐn)z像機(jī)MER-500-7UC是由大恒圖像研發(fā)，該攝像機(jī)外形極其小巧緊湊，便于實(shí) 際操作且提供線纜鎖緊裝置，能在各種惡劣環(huán)境下長時(shí)間穩(wěn)定工作，是具有高可靠性以及 高性價(jià)比的工業(yè)攝像機(jī)產(chǎn)品，能夠適用于多種桌面應(yīng)用及機(jī)器視覺，該系列產(chǎn)品具有支持 GENICAM接口，500萬像素，可直接與第三方軟件HALC0N連接等主要特色。
[0192] 軟件系統(tǒng)包括用于圖像采集的Halcon軟件，基于C#語言編寫的用于對(duì)頂U(kuò)數(shù)據(jù)進(jìn) 行采集的串口通訊程序，用于不同幀間圖像特征匹配的Opencv以及用于模型算法仿真的 Mat lab軟件。
[0193] 步驟：
[0194] 打開軌道車輛控制系統(tǒng)軟件
[0195] 同時(shí)打開串口通信程序等待一段時(shí)間，待數(shù)據(jù)穩(wěn)定輸出后，打開Halcon軟件利用 提前編寫的程序進(jìn)行圖像數(shù)據(jù)的采集，并通過控制軟件調(diào)整車速，使攝像機(jī)能夠連拍攝到 較為清晰的軌道線路，且相鄰兩幀圖像之間的重疊區(qū)域較大。
[0196] 采集完數(shù)據(jù)之后，即開始進(jìn)行數(shù)據(jù)處理，利用Opencv中特征匹配算法對(duì)相鄰兩幀 圖像間進(jìn)行匹配，并求解投影變換矩陣。
【主權(quán)項(xiàng)】
1. 一種基于視覺及慣性信息融合的軌道空間線形測量方法，用以獲取軌道結(jié)構(gòu)的沉降 變化量，其特征在于，包括以下步驟： 1) 分別對(duì)運(yùn)載體地理坐標(biāo)系和運(yùn)載體坐標(biāo)系進(jìn)行定標(biāo)； 2) 通過安裝在低速運(yùn)動(dòng)的運(yùn)載體上的慣性測量單元和視覺單元實(shí)時(shí)檢測到的運(yùn)動(dòng)數(shù) 據(jù)并據(jù)此計(jì)算獲取軌道空間初步線形； 3) 根據(jù)視覺單元實(shí)時(shí)檢測到的圖像進(jìn)行特征提取、特征匹配和投影變換，獲取投影變 換矩陣中的平移向量，并據(jù)此獲得軌道空間圖像線形； 4) 對(duì)軌道空間初步線形和軌道空間圖像線形進(jìn)行融合，獲取運(yùn)載體最終空間線形，最 終獲得軌道結(jié)構(gòu)的沉降變化量。2. 根據(jù)權(quán)利要求1所述的一種基于視覺及慣性信息融合的軌道空間線形測量方法，其 特征在于，所述的步驟2)具體包括以下步驟： 21) 根據(jù)視覺單元拍攝的圖像設(shè)定地理坐標(biāo)系的基準(zhǔn)點(diǎn)； 22) 通過慣性測量單元中的加速度計(jì)獲取運(yùn)載體坐標(biāo)系下運(yùn)載體的三軸加速度，通過 慣性測量單元中的陀螺儀獲取運(yùn)載體坐標(biāo)系下運(yùn)載體的角速度和方向角； 23) 根據(jù)角速度和方向角對(duì)運(yùn)載體坐標(biāo)系下的三軸加速度進(jìn)行坐標(biāo)系轉(zhuǎn)換，獲得地理 坐標(biāo)系下的三軸加速度； 24) 根據(jù)視覺單元中的雙目攝像機(jī)實(shí)時(shí)獲取運(yùn)載體的單目視覺連續(xù)圖像信息以及方向 角，對(duì)慣性測量單元獲取的三軸加速度中的低頻成分進(jìn)行補(bǔ)償； 25) 對(duì)補(bǔ)償后的三軸加速度進(jìn)行雙重積分，獲取運(yùn)載體的初步位移，并根據(jù)初步位移獲 取軌道空間初步線形。3. 根據(jù)權(quán)利要求2所述的一種基于視覺及慣性信息融合的軌道空間線形測量方法，其 特征在于，所述的步驟23)中的坐標(biāo)系轉(zhuǎn)換采用四元數(shù)法進(jìn)行坐標(biāo)系轉(zhuǎn)換，則有：其中，Rq為坐標(biāo)轉(zhuǎn)換矩陣，atx、aty、atz分別為地理坐標(biāo)系下的三軸加速度，abx、ab y、abz分 別為運(yùn)載體坐標(biāo)系下的三軸加速度，qi、q2、q3、q4分別為中間變量，a、0、y分別為繞x，y，z軸 旋轉(zhuǎn)的角度。4. 根據(jù)權(quán)利要求2所述的一種基于視覺及慣性信息融合的軌道空間線形測量方法，其 特征在于，所述的步驟24)中，三軸加速度中的低頻成分進(jìn)行補(bǔ)償包括采用方向角對(duì)三軸加 速度中的低頻成分進(jìn)行位移方向補(bǔ)償，以及采用單目視覺連續(xù)圖像信息對(duì)三軸加速度中的 低頻成分進(jìn)行位移大小補(bǔ)償。5. 根據(jù)權(quán)利要求1所述的一種基于視覺及慣性信息融合的軌道空間線形測量方法，其 特征在于，所述的步驟4)中，采用擴(kuò)展卡爾曼濾波對(duì)軌道空間初步線形和軌道空間圖像線 形進(jìn)行融合。
【文檔編號(hào)】G01B11/24GK106052584SQ201610349090
【公開日】2016年10月26日
【申請(qǐng)日】2016年5月24日
【發(fā)明人】鄭樹彬, 李立明, 李鵬程, 柴曉冬, 張磊
【申請(qǐng)人】上海工程技術(shù)大學(xué)