移動(dòng)機(jī)器人視覺(jué)動(dòng)態(tài)定位的穩(wěn)健高斯混合模型

程傳奇，郝向陽(yáng)，李建勝，胡 鵬，張 旭

1. 武警工程大學(xué)，新疆 烏魯木齊 830000； 2. 信息工程大學(xué)地理空間信息學(xué)院，河南 鄭州 450001

視覺(jué)定位是目前移動(dòng)機(jī)器人自主導(dǎo)航研究的一個(gè)熱點(diǎn)[1-5]，其關(guān)鍵技術(shù)是視覺(jué)同時(shí)定位和地圖構(gòu)建(visual simultaneous localization and mapping，V-SLAM)，圖優(yōu)化V-SLAM是其中一個(gè)重要研究方向[6-8]。圖優(yōu)化V-SLAM把自主導(dǎo)航過(guò)程建模為因子圖中的推理問(wèn)題。在因子圖模型中，路標(biāo)點(diǎn)位置和機(jī)器人位姿是變量節(jié)點(diǎn)，而空間測(cè)量值是作為變量節(jié)點(diǎn)之間約束的觀測(cè)節(jié)點(diǎn)。該推理問(wèn)題的目標(biāo)是得到該因子圖聯(lián)合概率的最大似然估計(jì)，也就是機(jī)器人軌跡和地圖點(diǎn)之間的幾何一致性估計(jì)[9]。該最大似然估計(jì)問(wèn)題可轉(zhuǎn)化為非線性最小二乘問(wèn)題，通過(guò)數(shù)值算法求解[10-11]。

經(jīng)典的V-SLAM算法大都假定環(huán)境靜止[12-15]，但最新的移動(dòng)機(jī)器人應(yīng)用場(chǎng)景往往是周圍人群或者設(shè)備不斷移動(dòng)的動(dòng)態(tài)環(huán)境[16-17]。V-SLAM通過(guò)提取場(chǎng)景特征點(diǎn)進(jìn)行數(shù)據(jù)處理，若場(chǎng)景中存在動(dòng)態(tài)物體，提取的特征點(diǎn)不斷移動(dòng)，則選定的路標(biāo)點(diǎn)集合不再屬于剛體，那么計(jì)算機(jī)視覺(jué)多像幾何約束方程不再成立，進(jìn)而導(dǎo)致定位錯(cuò)誤，且會(huì)造成構(gòu)建的地圖扭曲。

關(guān)于動(dòng)態(tài)場(chǎng)景V-SLAM的研究，主要有兩種思路。一種是把目標(biāo)識(shí)別和跟蹤技術(shù)融入視覺(jué)動(dòng)態(tài)定位過(guò)程[18-19]，稱為視覺(jué)前端“預(yù)濾波”。文獻(xiàn)[18]提出了一種解決動(dòng)態(tài)場(chǎng)景SLAM的貝葉斯模型框架，設(shè)計(jì)了一種目標(biāo)識(shí)別、跟蹤及數(shù)據(jù)融合算法對(duì)目標(biāo)進(jìn)行運(yùn)動(dòng)建模。在這種方法中，目標(biāo)識(shí)別和跟蹤技術(shù)主要用于視覺(jué)前端(front-end)，把運(yùn)動(dòng)目標(biāo)作為干擾背景剔除。

另外一種思路是采用抗差技術(shù)處理前端“外點(diǎn)”問(wèn)題[20-22]，主要方法為建立穩(wěn)健目標(biāo)函數(shù)或者抗差觀測(cè)表達(dá)式。動(dòng)態(tài)方差膨脹(dynamic covariance scaling，DCS)方法[20]使用抗差核函數(shù)調(diào)整路標(biāo)點(diǎn)觀測(cè)量高斯分布的馬氏誤差(Mahalanobis error)。文獻(xiàn)[21]使用混合高斯分布替代單峰高斯分布以增強(qiáng)潛在因子的影響?？汕袚Q約束(switchable constraints)方法[22]在優(yōu)化過(guò)程中把切換變量作為與約束相關(guān)聯(lián)的信息矩陣的縮放因子，可自然改變SLAM問(wèn)題的拓?fù)浣Y(jié)構(gòu)。這類方法仍然假定數(shù)據(jù)關(guān)聯(lián)錯(cuò)誤主要源于錯(cuò)誤的“閉環(huán)”，而忽視了環(huán)境動(dòng)態(tài)的影響。除非在潛在因子中進(jìn)行明確建模，這些方法無(wú)法處理動(dòng)態(tài)場(chǎng)景中的運(yùn)動(dòng)路標(biāo)點(diǎn)。也有學(xué)者通過(guò)在后端圖優(yōu)化框架中定義“運(yùn)動(dòng)指數(shù)”來(lái)處理視覺(jué)前端的“外點(diǎn)”和動(dòng)態(tài)因素。文獻(xiàn)[23]在圖優(yōu)化模型中加入了潛在指示變量來(lái)推斷路標(biāo)點(diǎn)是否運(yùn)動(dòng)。采用期望最大化(expectation maximization，EM)算法迭代推斷潛在路標(biāo)點(diǎn)指示變量，并估計(jì)SLAM最優(yōu)解。然而，這類基于EM算法的解決方案缺少抗差函數(shù)方法的穩(wěn)健性，且基于單觀測(cè)量的指示變量?jī)H對(duì)該觀測(cè)量建模，而不能反映與多觀測(cè)量相關(guān)聯(lián)的路標(biāo)點(diǎn)的運(yùn)動(dòng)性。

本文提出了一種適用于移動(dòng)機(jī)器人視覺(jué)動(dòng)態(tài)定位的穩(wěn)健高斯混合模型，致力于解決動(dòng)態(tài)場(chǎng)景中運(yùn)動(dòng)路標(biāo)點(diǎn)帶來(lái)的狀態(tài)估計(jì)不一致問(wèn)題，以提高圖優(yōu)化V-SLAM算法在動(dòng)態(tài)場(chǎng)景中的適用性。

1 傳統(tǒng)圖優(yōu)化視覺(jué)動(dòng)態(tài)定位模型

1.1 視覺(jué)動(dòng)態(tài)定位的最大后驗(yàn)概率問(wèn)題建模

移動(dòng)機(jī)器人視覺(jué)動(dòng)態(tài)定位可建模為一個(gè)最大后驗(yàn)概率估計(jì)(maximum-a-posteriori，MAP)問(wèn)題[24]。

如圖1所示，機(jī)器人狀態(tài)(在地圖坐標(biāo)系中隨時(shí)間變化的位置和姿態(tài))表示為X={xi,i=0,1,…,T}，在地圖坐標(biāo)系中的路標(biāo)點(diǎn)位置表示為L(zhǎng)={lj,j=1,2,…,N}，控制運(yùn)動(dòng)的輸入量表示為U={ui,i=1,2,…,T}，路標(biāo)點(diǎn)在機(jī)器人坐標(biāo)系下的觀測(cè)量表示為Z={zk,k=1,2,…,K}。

圖1 傳統(tǒng)圖優(yōu)化視覺(jué)定位模型示意Fig.1 Traditional graph-based vision-based localization model formulation

假設(shè)每次觀測(cè)相互獨(dú)立，且運(yùn)動(dòng)模型為馬爾科夫過(guò)程，則所有變量和觀測(cè)量的聯(lián)合概率[23]可表示為

(1)

式中，P(x0)表示初始狀態(tài)的先驗(yàn)概率(一般設(shè)為常量)；P(xi|xi-1,ui)表示機(jī)器人運(yùn)動(dòng)模型，由控制輸入ui參數(shù)化；P(zk|xik,ljk)表示路標(biāo)點(diǎn)測(cè)量模型。

運(yùn)動(dòng)模型可寫為

xi=fi(xi-1,ui)+ηi

(2)

式中，fi(·)是已知函數(shù)(運(yùn)動(dòng)模型)；ηi表示運(yùn)動(dòng)過(guò)程噪聲。

測(cè)量模型可寫為

zk=hk(xik,ljk)+εk

(3)

式中，hk(·)為已知函數(shù)(視覺(jué)測(cè)量模型)；εk表示測(cè)量噪聲。

1.2 視覺(jué)動(dòng)態(tài)定位的非線性優(yōu)化問(wèn)題建模

在MAP估計(jì)問(wèn)題中，給定運(yùn)動(dòng)控制輸入U(xiǎn)和路標(biāo)測(cè)量值Z，通過(guò)計(jì)算使得后驗(yàn)概率P(X,L,U,Z)最大的變量(X*,L*)來(lái)估計(jì)機(jī)器人位姿X和路標(biāo)點(diǎn)三維坐標(biāo)L，也就是最小化該對(duì)數(shù)似然函數(shù)

(4)

當(dāng)過(guò)程噪聲及測(cè)量噪聲服從高斯分布時(shí)，即ηi～N(0,Γi)，εk～N(0,Σk)。式(4)轉(zhuǎn)化為如式(5)所示非線性最小二乘優(yōu)化問(wèn)題

(5)

2 視覺(jué)動(dòng)態(tài)定位的穩(wěn)健高斯混合模型

2.1 模型建立

如圖2所示，與傳統(tǒng)圖優(yōu)化視覺(jué)定位模型不同，這里為每個(gè)路標(biāo)點(diǎn)標(biāo)記了運(yùn)動(dòng)指示性參數(shù)，W={wj,j=1,2,…,N}，文中稱為“運(yùn)動(dòng)指數(shù)”。在每個(gè)時(shí)刻k，觀測(cè)量zk與機(jī)器人位姿xik、路標(biāo)點(diǎn)ljk及指示變量wjk相對(duì)應(yīng)。其中ik把機(jī)器人位姿和測(cè)量值關(guān)聯(lián)起來(lái)，jk把路標(biāo)點(diǎn)與測(cè)量值關(guān)聯(lián)起來(lái)。

考慮到動(dòng)態(tài)場(chǎng)景，使用運(yùn)動(dòng)指數(shù)W對(duì)每一個(gè)路標(biāo)點(diǎn)的運(yùn)動(dòng)性進(jìn)行建模，以表示其運(yùn)動(dòng)的可能性，使得觀測(cè)量更穩(wěn)健。通過(guò)W，與動(dòng)態(tài)路標(biāo)點(diǎn)相關(guān)聯(lián)的“不好”的觀測(cè)量會(huì)在視覺(jué)定位過(guò)程中作為“外點(diǎn)”去除掉。這里給出所有變量和觀測(cè)量的聯(lián)合概率

(6)

圖2 視覺(jué)定位的穩(wěn)健高斯混合模型示意Fig.2 Robust Gaussian mixture model for vision-based Kinematical localization

給定觀測(cè)量Z，控制量U和當(dāng)前運(yùn)動(dòng)指數(shù)W，機(jī)器人狀態(tài)X和路標(biāo)點(diǎn)L的最大似然估計(jì)可寫為

(7)

與傳統(tǒng)圖優(yōu)化視覺(jué)定位模型不同，式(6)中第二部分加入了運(yùn)動(dòng)指數(shù)，另外，這里在觀測(cè)量誤差項(xiàng)中加入抗差函數(shù)vk，那么zk的條件概率可表示為增強(qiáng)的高斯分布

(8)

(9)

式中，wjk表示在k時(shí)刻與觀測(cè)量zk相關(guān)聯(lián)的路標(biāo)點(diǎn)ljk為靜態(tài)的可能性；vk表示與每一個(gè)路標(biāo)觀測(cè)量相關(guān)的抗差函數(shù)，用來(lái)約束該觀測(cè)量的“粗差”。當(dāng)wjk或vk趨于0時(shí)，等價(jià)于把高斯分布的方差設(shè)置為無(wú)窮大，使得該分布退化為均勻分布，導(dǎo)致該分布表示的約束對(duì)圖優(yōu)化過(guò)程無(wú)影響。

2.2 模型求解

式(8)中“運(yùn)動(dòng)指數(shù)”不可直接被觀測(cè)到，屬于隱性變量，故所描述模型為高斯混合分布模型。這里使用EM算法求解增強(qiáng)的目標(biāo)函數(shù)，估計(jì)路標(biāo)點(diǎn) “運(yùn)動(dòng)指數(shù)”，并解算機(jī)器人位姿和地圖點(diǎn)。

2.2.1 估計(jì)運(yùn)動(dòng)指數(shù)(M-step)

文獻(xiàn)[23]指出，隱性變量wk必須通過(guò)對(duì)同一路標(biāo)點(diǎn)的多次觀測(cè)才能進(jìn)行估計(jì)。在M-step(最大化步驟)需要選取最優(yōu)的w，使得聯(lián)合似然最大化。借鑒文獻(xiàn)[23]，首先令所有wk=0(假設(shè)所有路標(biāo)點(diǎn)都是運(yùn)動(dòng)的)，可最大化該似然函數(shù)(為1)；然后引入拉格朗日算子來(lái)“懲罰”“設(shè)置過(guò)多運(yùn)動(dòng)路標(biāo)點(diǎn)”這個(gè)假設(shè)，也可把拉格朗日算子理解為潛在路標(biāo)點(diǎn)運(yùn)動(dòng)性的先驗(yàn)知識(shí)。引入拉格朗日算子的對(duì)數(shù)似然目標(biāo)函數(shù)為

(10)

該對(duì)數(shù)似然目標(biāo)函數(shù)對(duì)W求導(dǎo)，令導(dǎo)數(shù)為零，可使得該目標(biāo)函數(shù)最大化

(11)

對(duì)每一個(gè)wl，有

(12)

式中，Kl表示路標(biāo)點(diǎn)l的一系列觀測(cè)量；λ為設(shè)定的常量，來(lái)權(quán)衡“懲罰”目標(biāo)函數(shù)，λ越大，認(rèn)為原假設(shè)“所有路標(biāo)點(diǎn)都是運(yùn)動(dòng)的”越不可靠。

2.2.2 增強(qiáng)的圖優(yōu)化目標(biāo)函數(shù)求解(E-step)

在E-step(期望步驟)中，需要使用已經(jīng)在M-step估計(jì)出的潛在隱性變量“運(yùn)動(dòng)指數(shù)”，求解機(jī)器人位姿以及路標(biāo)點(diǎn)三維坐標(biāo)。直接使用M-step估計(jì)的運(yùn)動(dòng)指數(shù)變量，采用圖優(yōu)化方法進(jìn)行求解。這里直接使用文獻(xiàn)[24]中提出的標(biāo)準(zhǔn)圖優(yōu)化方法，求解加入路標(biāo)點(diǎn)運(yùn)動(dòng)指數(shù)和穩(wěn)健觀測(cè)函數(shù)的增強(qiáng)目標(biāo)函數(shù)。該問(wèn)題可寫為如下優(yōu)化問(wèn)題

(13)

(14)

式中，sk=hk(xik,ljk)-zk，表示k時(shí)刻觀測(cè)量的原始預(yù)測(cè)誤差。DCS核函數(shù)為目標(biāo)函數(shù)(13)限定了一個(gè)由Φ確定的上界。通過(guò)對(duì)所有觀測(cè)確定上界Φ，核函數(shù)vk在圖優(yōu)化過(guò)程每次迭代中動(dòng)態(tài)縮放所有觀測(cè)節(jié)點(diǎn)的信息矩陣(權(quán)重)，并且把“外點(diǎn)”方差替換為較大的值以削弱其對(duì)目標(biāo)函數(shù)的貢獻(xiàn)。本文參考文獻(xiàn)[20]的建議，取Φ=1。

該優(yōu)化問(wèn)題可轉(zhuǎn)化為一個(gè)標(biāo)準(zhǔn)最小二乘問(wèn)題，具體推導(dǎo)如下：

把目標(biāo)函數(shù)中過(guò)程約束項(xiàng)進(jìn)行一階泰勒展開

(15)

對(duì)目標(biāo)函數(shù)中測(cè)量約束項(xiàng)一階泰勒展開，得

(16)

因此，該優(yōu)化問(wèn)題可寫為

(17)

重寫Mahalanobis范數(shù)，考慮到

(18)

故，式(17)可寫為

(19)

把矩陣F、G、H、J連接起來(lái)組成系數(shù)方陣A，δx、δl組成待估計(jì)向量δθ，a、c組成殘差向量b，則轉(zhuǎn)化為標(biāo)準(zhǔn)最小二乘問(wèn)題

(20)

E-step和M-step循環(huán)迭代進(jìn)行，直到達(dá)到收斂條件，輸出機(jī)器人位姿和路標(biāo)點(diǎn)坐標(biāo)。

3 試驗(yàn)與分析

為測(cè)試本文算法，分別用仿真數(shù)據(jù)和真實(shí)數(shù)據(jù)進(jìn)行試驗(yàn)。

3.1 仿真試驗(yàn)

使用的仿真數(shù)據(jù)是由佐治亞理工大學(xué)隨iSAM[10]發(fā)布的Vicotoria_park數(shù)據(jù)集和Citytrees10000數(shù)據(jù)集，該仿真數(shù)據(jù)集被廣泛應(yīng)用于基于路標(biāo)點(diǎn)的SLAM算法對(duì)比。其中，Vicotoria_park數(shù)據(jù)集包含6969個(gè)機(jī)器人位姿，6968個(gè)里程計(jì)測(cè)量值，151個(gè)路標(biāo)點(diǎn)及3640個(gè)路標(biāo)點(diǎn)測(cè)量值；Citytrees10000數(shù)據(jù)集包含10 000個(gè)機(jī)器人位姿，9999個(gè)里程計(jì)測(cè)量值，100個(gè)路標(biāo)點(diǎn)及4443個(gè)路標(biāo)點(diǎn)測(cè)量值。圖3為兩組數(shù)據(jù)集初始化情況，其中“*”表示機(jī)器人在各時(shí)刻的位姿，“+”號(hào)表示場(chǎng)景內(nèi)的路標(biāo)點(diǎn)位置。

圖3 兩組數(shù)據(jù)集初始化機(jī)器人位姿和路標(biāo)點(diǎn)Fig.3 Robot poses and landmarks initialization of 2 datasets

為測(cè)試本文算法對(duì)動(dòng)態(tài)路標(biāo)點(diǎn)的穩(wěn)健性，分別對(duì)兩組數(shù)據(jù)中特定的路標(biāo)點(diǎn)進(jìn)行“破壞”。針對(duì)Vicotoria_park數(shù)據(jù)集，選取“249”號(hào)路標(biāo)點(diǎn)，讓其北向勻速運(yùn)動(dòng)，最終位置偏離初始位置約10 m；針對(duì)Citytrees10000數(shù)據(jù)集，選取“50”號(hào)路標(biāo)點(diǎn)，讓其西向勻速運(yùn)動(dòng)，最終位置偏離初始位置約20 m。之所以選擇這兩個(gè)特殊的路標(biāo)點(diǎn)，是因?yàn)檫@兩個(gè)路標(biāo)點(diǎn)與機(jī)器人位姿之間的關(guān)聯(lián)相對(duì)較多(分別為171個(gè)和84個(gè))。一般認(rèn)為對(duì)同一路標(biāo)點(diǎn)的觀測(cè)越多，那么該路標(biāo)點(diǎn)的運(yùn)動(dòng)對(duì)最終優(yōu)化結(jié)果的影響越大。

本文對(duì)比了傳統(tǒng)優(yōu)化算法、DCS算法[20]和本文算法對(duì)兩組數(shù)據(jù)集的優(yōu)化結(jié)果，如圖4和圖5所示。由解算結(jié)果可看出：

(1) 由于動(dòng)態(tài)路標(biāo)點(diǎn)的存在，破壞了整個(gè)圖優(yōu)化的約束關(guān)系，傳統(tǒng)優(yōu)化算法已經(jīng)失效。

(2) DCS算法能削弱部分影響，但效果不顯著。因?yàn)镈CS算法屬于抗差估計(jì)核函數(shù)模型，不區(qū)分路標(biāo)點(diǎn)是否運(yùn)動(dòng)。動(dòng)態(tài)路標(biāo)點(diǎn)造成的前后觀測(cè)不一致使得全局約束的子集可能收斂至局部最優(yōu)值，但其余觀測(cè)會(huì)扭曲整個(gè)解算結(jié)果。

(3) 本文算法可有效緩解動(dòng)態(tài)路標(biāo)點(diǎn)的影響，解算結(jié)果與真值基本吻合。值得注意的是，Vicotoria_park數(shù)據(jù)集解算結(jié)果劣于Citytrees10000數(shù)據(jù)集。這主要是因?yàn)?，Vicotoria_park中動(dòng)態(tài)路標(biāo)點(diǎn)“249”的觀測(cè)更多(171個(gè)，從位姿節(jié)點(diǎn)“248”至位姿節(jié)點(diǎn)“7091”都有觀測(cè)，甚至存在環(huán)路)，因此對(duì)最終優(yōu)化結(jié)果影響更大。不過(guò)通常移動(dòng)機(jī)器人視覺(jué)動(dòng)態(tài)定位過(guò)程中，對(duì)路標(biāo)點(diǎn)的觀測(cè)往往集中在某一特定短時(shí)間段內(nèi)，因此本文算法可處理實(shí)際移動(dòng)機(jī)器人行進(jìn)中的動(dòng)態(tài)路標(biāo)點(diǎn)問(wèn)題。

3.2 真實(shí)數(shù)據(jù)試驗(yàn)

為進(jìn)一步驗(yàn)證算法有效性，選用真實(shí)數(shù)據(jù)進(jìn)行試驗(yàn)。試驗(yàn)數(shù)據(jù)為德國(guó)卡爾斯魯厄理工學(xué)院提供的KITTI Odometry數(shù)據(jù)集[25]。KITTI Odometry數(shù)據(jù)集包含F(xiàn)lea2 Firewire相機(jī)采集的高質(zhì)量立體圖像序列及OXTS RT 3000 IMU/GPS測(cè)量系統(tǒng)采集的地理數(shù)據(jù)(作為真值)。KITTI給出了22組車載測(cè)量數(shù)據(jù)，內(nèi)容涵蓋城市街道、鄉(xiāng)村、高速路等復(fù)雜場(chǎng)景，包括運(yùn)動(dòng)車輛、行人等各種動(dòng)態(tài)目標(biāo)。由于只有00～10數(shù)據(jù)給出了載體位置姿態(tài)真值，因此為客觀對(duì)比算法優(yōu)劣，這里選取00～10數(shù)據(jù)進(jìn)行試驗(yàn)，以下分別記為KITTI00～KITTI10。

圖4 Vicotoria_Park數(shù)據(jù)集機(jī)器人位姿和路標(biāo)點(diǎn)算法優(yōu)化結(jié)果Fig.4 Optimization results of robot poses and landmarks in Vicotoria_Park dataset

3.2.1 精度評(píng)價(jià)指標(biāo)

文中有兩種精度評(píng)定指標(biāo)： 絕對(duì)誤差與相對(duì)誤差。其中，絕對(duì)誤差為算法解算結(jié)果與真值的絕對(duì)定位差異，定義如下

(21)

相對(duì)誤差采用KITTI數(shù)據(jù)集的精度評(píng)價(jià)指標(biāo)[25]，它計(jì)算固定運(yùn)動(dòng)距離下所有相對(duì)位姿誤差的平均值，定義如下

(Pi-1Pj))

(22)

(Pi-1Pj))

(23)

(24)

(25)

3.2.2 結(jié)果與分析

由于DCS算法跟蹤過(guò)程不夠穩(wěn)健，導(dǎo)致多條序列導(dǎo)航過(guò)程中跟蹤失敗，因此這里只給出了傳統(tǒng)優(yōu)化算法和本文算法的試驗(yàn)結(jié)果對(duì)比。表1為KITTI00～KITTI10共計(jì)11組試驗(yàn)的誤差統(tǒng)計(jì)結(jié)果，加粗字體表示結(jié)果較優(yōu)。篇幅所限，圖6—圖9分別給出了具有代表性的KITTI01、KITTI03、KITTI04和KITTI07的解算結(jié)果：載體軌跡、解算軌跡的X、Y、Z及絕對(duì)點(diǎn)位誤差和相對(duì)平均誤差。

圖5 Citytrees 10000數(shù)據(jù)集機(jī)器人位姿和路標(biāo)點(diǎn)算法優(yōu)化結(jié)果Fig.5 Optimization results of robot poses and landmarks in Citytrees10000 dataset

序號(hào)傳統(tǒng)優(yōu)化算法本文算法絕對(duì)定位誤差/m相對(duì)誤差絕對(duì)定位誤差/m相對(duì)誤差meanmedianRMSEEtrans/(%)Erot/(deg/m)meanmedianRMSEEtrans/(%)Erot/(deg/m)007.4537.6597.7500.9620.00358.8169.2889.3050.89460.00330152.69947.09562.4841.79340.002717.75916.92720.3071.37690.00170215.20412.81816.2370.8370.00309.4916.36712.1760.8200.0031034.0904.1924.2180.6680.00171.5391.1811.7790.7370.0018047.6417.5137.7701.0900.00270.6890.6350.7840.4350.0019053.0683.3003.5640.5550.00243.2343.4013.7600.6020.0024062.7442.6442.9480.7020.00223.0512.6723.3710.7740.0023072.6602.6803.2010.8880.00422.9832.8973.6590.9840.00470811.11011.22411.3721.15740.003710.15310.55210.4271.00480.0030096.4506.7137.3970.9020.00305.5025.1776.5010.8570.0023104.9423.9366.0390.6780.00264.2183.4875.0780.5810.0020

圖6 KITTI01數(shù)據(jù)試驗(yàn)結(jié)果Fig.6 Experimental results of KITTI01 dataset

圖7 KITTI03數(shù)據(jù)試驗(yàn)結(jié)果Fig.7 Experimental results of KITTI03 dataset

圖8 KITTI04數(shù)據(jù)試驗(yàn)結(jié)果Fig.8 Experimental results of KITTI04 dataset

圖9 KITTI07數(shù)據(jù)試驗(yàn)結(jié)果Fig.9 Experimental results of KITTI07 dataset

KITTI01和KITTI04的試驗(yàn)結(jié)果可看出：本文算法精度明顯優(yōu)于傳統(tǒng)優(yōu)化算法。由于強(qiáng)動(dòng)態(tài)場(chǎng)景中多處出現(xiàn)大量動(dòng)態(tài)目標(biāo)(車輛、行人)，傳統(tǒng)優(yōu)化算法不能保證解算過(guò)程中動(dòng)態(tài)路標(biāo)點(diǎn)的正確識(shí)別與過(guò)濾，使得解算結(jié)果與真值出現(xiàn)較大偏差，如圖6和圖8所示。而本文算法可大大削弱動(dòng)態(tài)路標(biāo)點(diǎn)的影響，驗(yàn)證了算法設(shè)計(jì)的正確性及對(duì)動(dòng)態(tài)場(chǎng)景的有效性。

KITTI00、KITTI02和KITTI05—KITTI10的試驗(yàn)結(jié)果可看出：本文算法與傳統(tǒng)優(yōu)化算法精度相差不大，這是由于數(shù)據(jù)場(chǎng)景中動(dòng)態(tài)目標(biāo)較少，且少量動(dòng)態(tài)路標(biāo)點(diǎn)在解算過(guò)程中被當(dāng)作“外點(diǎn)”剔除掉了，因此不會(huì)影響整體優(yōu)化結(jié)果。某些情況下(KITTI00，KITTI05—KITTI07數(shù)據(jù)中)，如圖9所示，傳統(tǒng)優(yōu)化算法精度要優(yōu)于本文算法，這是由于靜態(tài)場(chǎng)景或弱動(dòng)態(tài)場(chǎng)景中動(dòng)態(tài)目標(biāo)較少(或基本沒(méi)有)，而本文算法則把優(yōu)化函數(shù)中靜態(tài)路標(biāo)點(diǎn)的權(quán)重降低了，因此會(huì)影響最終優(yōu)化結(jié)果。

KITTI03的結(jié)果比較特殊，絕對(duì)定位誤差本文算法優(yōu)于傳統(tǒng)優(yōu)化算法(圖7(a)和圖7(b)所示)，而相對(duì)誤差傳統(tǒng)優(yōu)化算法優(yōu)于本文算法，但基本上相差不大(圖7(c)和圖7(d)所示)。這是由于開始解算階段數(shù)據(jù)中存在動(dòng)態(tài)目標(biāo)(車輛)的干擾，導(dǎo)致起始優(yōu)化結(jié)果出現(xiàn)較大偏差，進(jìn)而影響了后續(xù)所有的定位結(jié)果，因此使得本文算法絕對(duì)定位誤差優(yōu)于傳統(tǒng)優(yōu)化算法。而后續(xù)解算過(guò)程中動(dòng)態(tài)目標(biāo)干擾較小，因此統(tǒng)計(jì)相對(duì)誤差時(shí)，兩種算法精度相當(dāng)。

綜上，在強(qiáng)動(dòng)態(tài)場(chǎng)景中，本文算法優(yōu)于傳統(tǒng)優(yōu)化算法；在靜態(tài)或弱動(dòng)態(tài)場(chǎng)景中，本文算法與傳統(tǒng)優(yōu)化算法精度相當(dāng)，說(shuō)明了本文算法對(duì)動(dòng)態(tài)場(chǎng)景中動(dòng)態(tài)路標(biāo)點(diǎn)的穩(wěn)健性，驗(yàn)證了算法設(shè)計(jì)的有效性和正確性。

表2為算法運(yùn)算時(shí)間統(tǒng)計(jì)，可看出單幀運(yùn)算時(shí)間約為70 ms，可滿足移動(dòng)機(jī)器人實(shí)時(shí)自主定位的需求。

表2 算法運(yùn)行時(shí)間統(tǒng)計(jì)

4 結(jié) 論

在移動(dòng)機(jī)器人視覺(jué)動(dòng)態(tài)定位研究中，針對(duì)動(dòng)態(tài)環(huán)境中傳統(tǒng)圖優(yōu)化視覺(jué)定位算法精度低甚至失效的問(wèn)題，提出了一種顧及動(dòng)態(tài)路標(biāo)點(diǎn)的穩(wěn)健高斯混合模型。通過(guò)EM算法估計(jì)該高斯混合模型中的路標(biāo)點(diǎn)“運(yùn)動(dòng)指數(shù)”和相機(jī)位姿及路標(biāo)點(diǎn)坐標(biāo)。仿真試驗(yàn)和真實(shí)數(shù)據(jù)試驗(yàn)驗(yàn)證了算法在強(qiáng)動(dòng)態(tài)場(chǎng)景中的有效性和正確性，且該方法具有良好的可擴(kuò)展性，可應(yīng)用于目前任一圖優(yōu)化SLAM框架內(nèi)，以提高算法對(duì)動(dòng)態(tài)環(huán)境的適應(yīng)性。

值得注意的是，模型求解時(shí)，參數(shù)λ的取值為預(yù)先設(shè)置的經(jīng)驗(yàn)值，具有主觀性；且懲罰函數(shù)模型的選取具有多樣性。關(guān)于如何選取合適的參數(shù)值和懲罰函數(shù)模型及參數(shù)值對(duì)算法的敏感性研究是下一步工作的主要內(nèi)容。

另外，在實(shí)際應(yīng)用中，視覺(jué)場(chǎng)景復(fù)雜多變，光照明暗、快速旋轉(zhuǎn)運(yùn)動(dòng)及遮擋情況極易造成視覺(jué)動(dòng)態(tài)定位失敗，而慣性傳感器具有其特殊優(yōu)勢(shì)(位姿輸出穩(wěn)定，工作頻率高，短時(shí)精度可靠)，因此下一步也將開展慣性傳感器與視覺(jué)傳感器的融合方法研究，以進(jìn)一步提高移動(dòng)機(jī)器人自主導(dǎo)航應(yīng)用的穩(wěn)健性。