圖像分析與傳感器融合的室內(nèi)導(dǎo)航定位系統(tǒng)設(shè)計及實踐

王培云

(華芯拓遠(yuǎn)(天津)科技有限公司 天津 300457)

0 引言

步入21世紀(jì)，傳感器技術(shù)和相應(yīng)計算軟件的開發(fā)正處于迅猛發(fā)展的態(tài)勢，促使基于位置的服務(wù)(location based service,LBS)愈發(fā)進(jìn)步[1]。該服務(wù)存在的最終目標(biāo)是能夠在任何時間和地點為用戶提供精準(zhǔn)的定位服務(wù)和導(dǎo)航功能，以解決在陌生地區(qū)方向辨認(rèn)和路徑規(guī)劃困難的問題。

LBS由室外定位系統(tǒng)和室內(nèi)定位系統(tǒng)兩類組成。室外定位系統(tǒng)研究較為成熟，常用的包括全球定位系統(tǒng)(global positioning system,GPS)和我國自主研發(fā)的北斗衛(wèi)星導(dǎo)航系統(tǒng)，這類系統(tǒng)在戶外精度高、信號穩(wěn)定性強，配合相應(yīng)設(shè)備通?？蛇_(dá)到亞米級。然而當(dāng)處于室內(nèi)、隧道等衛(wèi)星信號衰弱甚至屏蔽的地點，定位將產(chǎn)生劇烈漂移，因此室內(nèi)導(dǎo)航定位已經(jīng)成為當(dāng)前的一大研究熱點。

在室內(nèi)導(dǎo)航定位系統(tǒng)的研究中，目前主流研究思想是搭建無線傳感器網(wǎng)絡(luò)的實現(xiàn)方案，常用的方法包括無線網(wǎng)絡(luò)[2-3]、藍(lán)牙[4-6]和超寬帶[7]等，但是這些方法通常受制于設(shè)備網(wǎng)絡(luò)系統(tǒng)的搭建。為提高對于陌生環(huán)境的適應(yīng)性，慣性定位導(dǎo)航逐步進(jìn)入人們的研究視野。本文主要研究將圖像分析應(yīng)用于室內(nèi)慣性導(dǎo)航系統(tǒng)中，采用多傳感融合的思想提高室內(nèi)導(dǎo)航定位的精度，實現(xiàn)其關(guān)鍵技術(shù)和應(yīng)用。

1 慣性導(dǎo)航技術(shù)相關(guān)理論

慣性導(dǎo)航最初應(yīng)用于戰(zhàn)機(jī)的作戰(zhàn)姿態(tài)計算，隨著科技不斷發(fā)展，目前該技術(shù)已經(jīng)成為室內(nèi)導(dǎo)航定位主流方法之一。其基本原理是使用多個不同種類的慣性測量單元(inertial measurement unit,IMU)收集被測目標(biāo)的加速度、角速度等運動學(xué)信息，通過積分等計算求解目標(biāo)的速度和位置，從而實現(xiàn)對目標(biāo)進(jìn)行定位的功能。IMU安裝在被測目標(biāo)之上，在工作時既不需要依賴于外界數(shù)據(jù)，也不需要向外界傳遞信息。其關(guān)鍵技術(shù)包括坐標(biāo)系統(tǒng)換算、姿態(tài)更新方法、慣性測量單元和數(shù)據(jù)解算等。

1.1 坐標(biāo)系統(tǒng)

在任何一種導(dǎo)航定位系統(tǒng)中，坐標(biāo)系的建立都是實現(xiàn)定位的基礎(chǔ)。導(dǎo)航坐標(biāo)系即在測繪學(xué)中常用的N系，是導(dǎo)航解算的坐標(biāo)系[8]。N系以運動物體本身為坐標(biāo)原點，X、Y、Z三坐標(biāo)軸分別指向東、北、天三個方向，因此也被稱為東北天坐標(biāo)系。載體坐標(biāo)系，即B系，是運動物體結(jié)構(gòu)形成的坐標(biāo)系，其原點在物體的重心，由XOY組成一水平面，以右手螺旋法則確定Z軸的方向垂直于XOY面指向天。

1.2 姿態(tài)更新

針對被研究運動物體，其運動姿態(tài)是采用上述坐標(biāo)系的轉(zhuǎn)換和解算求得的，在慣性導(dǎo)航中，通常根據(jù)求解從B系相對于N系的改變。

由于B系和N系均滿足右手螺旋法則，在坐標(biāo)系中坐標(biāo)軸兩兩互相垂直，因此坐標(biāo)系的變換可以分為三種不同的類型。載體只有原點距離發(fā)生變化但未進(jìn)行旋轉(zhuǎn)，此時只需進(jìn)行坐標(biāo)系平移使兩坐標(biāo)系原點重合；載體只有坐標(biāo)系間角度發(fā)生變化但原點沒有改變，此時需進(jìn)行某一坐標(biāo)系旋轉(zhuǎn)即可求得相對于另一坐標(biāo)系變化；同時發(fā)生角度旋轉(zhuǎn)和距離改變必須同時調(diào)整原點的位置和坐標(biāo)系的角度，使用一個可表達(dá)目標(biāo)運動姿態(tài)的矩陣進(jìn)行姿態(tài)更新。

1.3 慣性測量單元

從上述基本原理可以看出，慣導(dǎo)測量的數(shù)據(jù)是目標(biāo)加速度和角速度，由慣性測量單元實現(xiàn)。常用的IMU必須包含用于測量角速度的三軸陀螺儀和用于測量加速度的加速度儀，再通過運算求得運動目標(biāo)的速度、空間位置等信息。

三軸陀螺儀是測定角速度的裝置，其由兩相同質(zhì)量的塊構(gòu)成，兩質(zhì)量塊速度相反，其基本工作原理是科里奧利力，角速度公式[9]為

(1)

式中，wm(t)為角速度真值；w′(t)為測量值；bg(t)為零點漂移；ng(t)為測量噪聲。

三軸加速度計是一種慣性傳感器，其是測定目標(biāo)相對于載體坐標(biāo)系的重力加速度和絕對加速度之和的作用，其測量值基本公式如下：

(2)

2 雙目視覺圖像處理設(shè)計

在傳統(tǒng)的圖像處理領(lǐng)域，一般是單目視覺，即采用單個圖片采集設(shè)備采集到的圖片，這種圖片不具有縱深，一般只能獲得一個相對于圖片邊緣的二維坐標(biāo)，因此采用雙目視覺圖像處理。

2.1 圖像處理流程

雙目視覺圖像處理的具體流程如圖1所示，首先將每個相機(jī)的固有參數(shù)和兩個相機(jī)之間的非固有變換進(jìn)行離線校準(zhǔn)，采用兩個圖像采集設(shè)備獲取的圖片后對每一張圖片提取角點[10]，對相應(yīng)角點進(jìn)行匹配和光流法跟蹤，最后對結(jié)果進(jìn)行緊耦合優(yōu)化。

圖1 雙目視覺圖像處理流程

2.2 視覺殘差計算

在緊耦合優(yōu)化滑動窗口時，需要進(jìn)行視覺殘差計算，其是指在當(dāng)前幀上某一特征點的投影誤差。對于特征點i，在第b幀進(jìn)行觀察時視覺殘差為

(3)

2.3 雙目視覺方向檢測

在室內(nèi)環(huán)境中，視覺消失點與坐標(biāo)系中坐標(biāo)軸正交方向相同，通過這種正交約束，即可判斷圖像采集設(shè)備相對于室內(nèi)環(huán)境的方向。為進(jìn)行方向檢測，本文采用Jana等人提出的預(yù)期最大化(expectation maximization,EM)算法進(jìn)行計算[11]，并將上述正交約束引入以提高魯棒性。

在EM算法中，通常采用梯度下降求目標(biāo)函數(shù)，結(jié)果相對粗糙。本文使用高斯球體進(jìn)行基于EM的方向檢測估計。如圖2所示，高斯球球心位于成像設(shè)備焦點，圖像平面上每一條線段及其在真實世界中對應(yīng)直線都是由該直線和投影中心定義的平面法線表示。平面和高斯球角點構(gòu)成一大圓，在真實世界中平行線大圓相交于球體上一點。從中心到這個交點方向為消失方向，平行線投影相交點即為消失點。

圖2 高斯球模型示意

從圖2可以看出，消失點、圖片采集設(shè)備方向、消失方向是具有幾何關(guān)系的，設(shè)三個主消失方向分別為Vdi,i=1,2,3，通過EM對目標(biāo)檢索獲取的線段lj聚類，被檢索線段對應(yīng)的法線為Nl，設(shè)Z是圖像潛在變量先的簇，那么有相應(yīng)的高斯分布[12]為

(4)

3 緊耦合的視覺-慣導(dǎo)數(shù)據(jù)融合設(shè)計

基于以上慣導(dǎo)原理和雙目圖像分析設(shè)計進(jìn)行視覺慣導(dǎo)數(shù)據(jù)融合設(shè)計，實現(xiàn)步行者的室內(nèi)導(dǎo)航。設(shè)計采用基于滑動窗口的緊耦合優(yōu)化方法進(jìn)行目標(biāo)估計，將滑動窗口內(nèi)的各變量整合在統(tǒng)一優(yōu)化框架內(nèi)，變量包括IMU零偏、速度、三維位置坐標(biāo)、深度等。

3.1 緊耦合融合設(shè)計

在視覺慣導(dǎo)數(shù)據(jù)融合室內(nèi)導(dǎo)航定位系統(tǒng)中，本文采用緊耦合設(shè)計方法，充分使用通過雙目視覺單元所獲取信息與慣性測量單元所獲取信息進(jìn)行對比，使用兩者原始數(shù)據(jù)共同實現(xiàn)對于同組變量估算，從而追求更高定位精度，基本流程設(shè)計見圖3。

圖3 緊耦合系統(tǒng)設(shè)計

3.2 視覺慣導(dǎo)數(shù)據(jù)融合

在本系統(tǒng)設(shè)計中，采用緊耦合數(shù)據(jù)融合，滑動窗口內(nèi)的狀態(tài)向量為

(5)

把滑動窗口中視覺殘差和慣導(dǎo)單元中預(yù)積分?jǐn)?shù)據(jù)約束放在同一框架之下進(jìn)行優(yōu)化處理，最終就構(gòu)成了一個非線性最小二乘問題見式(6)。

(6)

對于實時方向的估計更新，本文將從視頻幀獲得的方向信息與來自IMU的角度測量相結(jié)合，估計模型如式(7)～式(9)所示。

式中,errort+1為誤差觀測；H為測量矩陣。

在對于偏航角和俯仰角的估計中，可以采用從加速計提取的重力分量表示，再用上述方法融合。

由定理2.1—2.2可以看到,F(X)上的度量H與[0，1]中的度量ρ的形式聯(lián)系非常緊密。那么H和ρ具體是怎么聯(lián)系的?[0,1]剩余格上的邏輯度量ρ的性質(zhì)是怎樣影響度量空間(F(X),H)的性質(zhì)?文獻(xiàn)[6,8]詳細(xì)的論證了(F(X),H)的拓?fù)湫再|(zhì)與邏輯推理的魯棒性分析之間的關(guān)系。如果我們研究清楚[0,1]剩余格上的邏輯度量空間的拓?fù)湫再|(zhì)及其與拓?fù)淇臻g(F(X),H)的關(guān)系,就可以對邏輯推理系統(tǒng)的擾動參數(shù)的選取和魯棒性分析起到一定的指導(dǎo)作用。

式中,βg、αg分別為偏航角和俯仰角；xg、yg、zg分別為重力在每個方向上的分量。

3.3 邊緣化信息

系統(tǒng)采集的數(shù)據(jù)隨著時間不斷增加，狀態(tài)數(shù)也隨之增長，計算復(fù)雜程度不斷提高。為了提高計算效率，約束計算復(fù)雜性的同時盡量減少可用信息損失，本文采用邊緣化先驗法解決，對于無關(guān)狀態(tài)標(biāo)量進(jìn)行消元整合。

高斯牛頓法求解非線性最小二乘問題公式為[13]

(15)

式中，Δxa、Δxb為狀態(tài)變量，通過高斯消元消去CT，求得

(16)

從而舍棄狀態(tài)Δxa，僅保留系統(tǒng)中的Δxb以提高計算效率。

4 實驗測試及分析

為驗證本文提出的基于雙目視覺圖像處理與傳感器融合的室內(nèi)導(dǎo)航系統(tǒng)技術(shù)性能，筆者使用了相同設(shè)備在相同環(huán)境下分別對視覺導(dǎo)航、慣性導(dǎo)航和本文提出方法進(jìn)行了多組測試，并同時收集各個方法的信息。

4.1 測試儀器及環(huán)境

在測試中，圖片采集設(shè)備使用機(jī)器人搭載奧比中光雙目相機(jī)，幀率為30 fps；使用的慣性導(dǎo)航設(shè)備是NGIMU，幀率為60 fps，安裝在足部。

圖4 測試環(huán)境平面圖及實景圖分析估算

4.2 測試結(jié)果分析

在七次試驗中，僅使用慣導(dǎo)系統(tǒng)和使用本文系統(tǒng)所計算得到的運動軌跡長度及相應(yīng)誤差評價指標(biāo)見表1。

表1 運動軌跡長度及誤差評價指標(biāo) 單位：m

從表1可以看出，使用本文系統(tǒng)時平均值誤差僅有0.22 m，精度可以達(dá)到分米級。將該系統(tǒng)的結(jié)果與單獨慣導(dǎo)系統(tǒng)進(jìn)行比較，可以發(fā)現(xiàn)單獨慣導(dǎo)系統(tǒng)的整體距離數(shù)值偏低，這是由于在測試的室內(nèi)行進(jìn)路線中存在一直角轉(zhuǎn)彎，一些研究表明[14-15]慣導(dǎo)系統(tǒng)在面向旋轉(zhuǎn)運動載體進(jìn)行目標(biāo)姿態(tài)更新時存在轉(zhuǎn)彎半徑較實際距離計算變小的情況，而圖像分析系統(tǒng)矯正了該問題的存在。兩者絕對偏差平均值相近，但均方根誤差本文系統(tǒng)遠(yuǎn)小于單獨慣導(dǎo)系統(tǒng)，說明本文系統(tǒng)測試出的距離分布更接近真實數(shù)據(jù)。該實例證明，本文圖像分析與傳感器融合的室內(nèi)定位精度基本可滿足日常在室內(nèi)的定位需求。使用視覺信息繪制運動圖形的效果見圖5。

圖5 測試結(jié)果制圖

圖5中，黑色線段為雙目視覺系統(tǒng)建立起的廊道二維平面，黑色曲線為人的實際運動軌跡，該軌跡采用北醒CE30-C三維激光點云系統(tǒng)精準(zhǔn)構(gòu)建，準(zhǔn)確度可達(dá)厘米級，淺色曲線為本文方法所建立的測試者運動軌跡，該軌跡為7次測試結(jié)果采用Matlab擬合獲得，制圖軟件為EVO軌跡評估工具。從圖中可以看出，所建立起的廊道二維平面與實際平面圖較為符合，同時測試者的運動軌跡也較為準(zhǔn)確，接近實際的運動曲線，具有良好的導(dǎo)航作用。

5 結(jié)束語

本文提出了一種采用緊耦合的圖像分析與傳感器融合的室內(nèi)導(dǎo)航定位系統(tǒng)，并在工作室樓層進(jìn)行了測試。測試結(jié)果表明，通過圖像分析與慣導(dǎo)融合系統(tǒng)能夠在一陌生環(huán)境中有效建立起虛擬地圖并提供相對準(zhǔn)確的導(dǎo)航和定位服務(wù)，一方面彌補了慣導(dǎo)系統(tǒng)中需要先驗環(huán)境的特點，另一方面解決了室內(nèi)衛(wèi)星定位信號弱的弊端，是一種有效的室內(nèi)定位方法。