王培云

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

0 引言

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

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

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

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

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

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

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

1.2 姿態(tài)更新

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

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

1.3 慣性測(cè)量單元

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

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

(1)

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

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

(2)

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

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

2.1 圖像處理流程

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

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

2.2 視覺殘差計(jì)算

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

(3)

2.3 雙目視覺方向檢測(cè)

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

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

圖2 高斯球模型示意

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

(4)

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

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

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

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

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

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

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

(5)

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

(6)

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

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

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

由定理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)系,就可以對(duì)邏輯推理系統(tǒng)的擾動(dòng)參數(shù)的選取和魯棒性分析起到一定的指導(dǎo)作用。

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

3.3 邊緣化信息

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

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

(15)

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

(16)

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

4 實(shí)驗(yàn)測(cè)試及分析

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

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

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

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

4.2 測(cè)試結(jié)果分析

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

表1 運(yùn)動(dòng)軌跡長(zhǎng)度及誤差評(píng)價(jià)指標(biāo) 單位：m

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

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

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

5 結(jié)束語

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