程傳奇，郝向陽，李建勝，胡 鵬，張 旭

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

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

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

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

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

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

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

1.1 視覺動態(tài)定位的最大后驗概率問題建模

移動機(jī)器人視覺動態(tài)定位可建模為一個最大后驗概率估計(maximum-a-posteriori，MAP)問題[24]。

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

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

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

(1)

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

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

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

(2)

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

測量模型可寫為

zk=hk(xik,ljk)+εk

(3)

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

1.2 視覺動態(tài)定位的非線性優(yōu)化問題建模

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

(4)

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

(5)

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

2.1 模型建立

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

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

(6)

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

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

(7)

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

(8)

(9)

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

2.2 模型求解

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

2.2.1 估計運(yùn)動指數(shù)(M-step)

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

(10)

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

(11)

對每一個wl，有

(12)

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

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

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

(13)

(14)

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

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

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

(15)

對目標(biāo)函數(shù)中測量約束項一階泰勒展開，得

(16)

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

(17)

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

(18)

故，式(17)可寫為

(19)

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

(20)

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

3 試驗與分析

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

3.1 仿真試驗

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

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

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

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

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

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

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

3.2 真實數(shù)據(jù)試驗

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

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

3.2.1 精度評價指標(biāo)

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

(21)

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

(Pi-1Pj))

(22)

(Pi-1Pj))

(23)

(24)

(25)

3.2.2 結(jié)果與分析

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

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

序號傳統(tǒng)優(yōu)化算法本文算法絕對定位誤差/m相對誤差絕對定位誤差/m相對誤差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ù)試驗結(jié)果Fig.6 Experimental results of KITTI01 dataset

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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

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

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