張洪，于源卓，邱曉天

(1. 江南大學 機械工程學院，無錫 214122； 2. 江蘇省食品先進制造裝備技術(shù)重點實驗室，無錫 214122)

近年來，移動機器人逐漸走進社會為人們的生產(chǎn)生活提供便利[1-2]。在這種情況下，機器人的控制與路徑規(guī)劃顯得尤為重要[3-4]?？刂婆c路徑規(guī)劃需要定位與建圖作為前提，因此，將機器視覺與同時定位與建圖相結(jié)合(V-SLAM)[5-7]成為了機器人領(lǐng)域研究的熱點[8]。相比于采用雷達的二維SLAM，V-SLAM 通過機器視覺獲取三維信息，對物體空間的幾何外形進行描述，對于機器人在未知領(lǐng)域的導航有著重要意義[9]。隨著國內(nèi)外學者的逐漸深入研究，V-SLAM 先后出現(xiàn)了LSD-SLAM、RGB-D SLAM、ORB-SLAM 及語義SLAM。

TUM[10-11]計算機視覺組采用了直接法的方式，通過灰度圖的強度對相鄰的2 幀進行匹配成功，實現(xiàn)大規(guī)模環(huán)境下的定位與建圖。Endres 等[12]使用深度相機獲取視覺信息，完成了RGB-D SLAM 算法，以提高建圖魯棒性。Mur-Artal 等[13]提出了經(jīng)典的ORB-SLAM 算法，在跟蹤、建圖、重定位及閉環(huán)檢測的過程中，全程采用ORB 特征完成，使其具有較好的實時性；隨后，為解決ORB-SLAM 所用相機單一且精度低的問題，又提出了ORB-SLAM2[14]，該算法采用光速平差法進行優(yōu)化使定位更加精確，并提出了一種輕量級的定位模式，使其能夠更加有效地重用地圖。Liu 等[15]對物體語義信息進行了總結(jié)，語義SLAM[16-17]也逐漸出現(xiàn)在人們的視野中，借用物體的語義信息，成功提高了SLAM 的人機交互能力，提高了算法精度。

相比于其他SLAM 算法，ORB-SLAM2 算法具有高效的實時性、相機使用的多樣性及良好的精度,深受國內(nèi)外研究學者的喜愛。關(guān)鍵幀作為決定ORB-SLAM2 算法精度的重要因素，廣大學者針對其進行了大量創(chuàng)新優(yōu)化，以提升算法性能。Yang 等[18]提出基于地圖元素和關(guān)鍵幀數(shù)據(jù)庫的離線地圖構(gòu)建算法，通過對關(guān)鍵幀與地圖元素在線儲存離線構(gòu)圖的方式提高建圖精度。Xie 等[19]提出重復運動檢測方法改進關(guān)鍵幀選擇算法，剔除冗余關(guān)鍵幀以提高整個系統(tǒng)的實時性能及定位精度。Cui 等[20]在ORB-SLAM2 算法的基礎(chǔ)上使用圖像強度而非特征點匹配關(guān)鍵幀，提出了Direct-ORB-SLAM，該方法避免了計算冗余，使系統(tǒng)在效率與準確度上均有提高。綜上所述，國內(nèi)外針對ORB-SLAM2 關(guān)鍵幀的研究已取得大量成果，但不難發(fā)現(xiàn)：①在跟蹤定位的過程中，跟蹤失敗情況經(jīng)常發(fā)生，嚴重影響定位效果；②雖然對算法進行了優(yōu)化，但精度不高，誤差較大的問題依然存在。

基于此，本文首先采用ORB-SLAM2 算法對幀間相對位姿進行估計；其次在原有算法模型的基礎(chǔ)上，增加旋轉(zhuǎn)與平移量作為判定依據(jù)，決定是否插入新關(guān)鍵幀，解決了跟蹤定位易失敗的問題，并提高了定位精度；然后設計了劣質(zhì)關(guān)鍵幀剔除算法，避免了因移動機器人所安裝的相機與機器人機身產(chǎn)生相對運動而導致劣質(zhì)關(guān)鍵幀生成的問題；最后通過RGB-D 數(shù)據(jù)集與移動機器人開展了實驗驗證。

1 ORB-SLAM2 算法

經(jīng)典的V-SLAM 算法框架主要通過視覺傳感器讀取數(shù)據(jù)，完成前端視覺里程計，通過后端進行非線性優(yōu)化，回環(huán)檢測再次對整體地圖進行優(yōu)化更新，以完成地圖的構(gòu)建。而ORB-SLAM2 算法將經(jīng)典的V-SLAM 算法劃分為3 個線程：Tracking、Local Mapping、Loop Closing，并通過System 總線程完成對移動機器人的定位與建圖。

Tracking 線程主要包含相機定位、關(guān)鍵幀選取與插入、ORB 特征點提取及相機位姿估計。ORBSLAM2 算法采用了ORB 特征點：由FAST 角點與角點位置的BRIEF 描述子組成，這種特征點有著較快的計算速率及較高的精度，其采用opencv 庫中的BruteForceMatcher 結(jié)合漢明距離進行匹配，采用隨機采樣一致性算法(RANSAC)減少誤匹配對，抑制噪聲，剔除外點。

同時，ORB-SLAM2 之所以能夠擁有較好的實時性，是因為在跟蹤線程運行的過程中，不對相機傳來的所有幀進行處理，而是如圖1 所示根據(jù)關(guān)鍵幀生成規(guī)則生成關(guān)鍵幀，僅對選取的關(guān)鍵幀進行處理，減少處理的幀數(shù)，節(jié)約運算資源，保證系統(tǒng)的實時性。

圖1 關(guān)鍵幀選取示意圖Fig. 1 Schematic of key frame selection

2 關(guān)鍵幀選擇算法

2.1 常用關(guān)鍵幀選擇算法

常用的關(guān)鍵幀選擇算法首先要保證當前幀的內(nèi)點數(shù)超過設定的最小閾值，并且重疊度不高，既保證關(guān)鍵幀跟蹤的指令，同時又避免引入過多的信息冗余。上述條件符合時，線程開始對下述條件進行判定，當有一條判定成功時，插入關(guān)鍵幀。

1) 為避免通過幀數(shù)過多造成跟蹤丟失，在距離上次關(guān)鍵幀插入過去MAX 幀(預設的最大值)時，插入關(guān)鍵幀。

2) 為防止圖像重疊度過高，在距離上次插入關(guān)鍵幀至少過去MIN 幀(預設的最小值)且局部建圖線程處于空閑狀態(tài)時，插入關(guān)鍵幀。

3) 局部建圖線程處理能力有限，在該線程中需要保持關(guān)鍵幀隊列的關(guān)鍵幀不超過3 個。

但是由于此關(guān)鍵幀篩選條件只與線程中通過幀的數(shù)量有關(guān)，當機器人在轉(zhuǎn)彎或者移動速度過快的情況下，就會導致關(guān)鍵幀信息采集不足，漏掉重要信息，導致軌跡造成較大偏差，甚至跟蹤中斷。針對這一問題，本文新增幀間相對運動量作為關(guān)鍵幀選擇條件，并設計劣質(zhì)關(guān)鍵幀剔除算法，以提高跟蹤定位精度。

2.2 改進關(guān)鍵幀選擇算法

2.2.1 關(guān)鍵幀的選擇

為提高定位精度，本文增加幀間相對運動量作為關(guān)鍵幀選擇條件之一。幀間相對運動量表示幀與幀之間的相對運動程度，主要由2 部分組成，分別是旋轉(zhuǎn)和平移，其中將旋轉(zhuǎn)作為主要變化量，平移作為次要變化量，計算方式如下：

通過視覺里程計得到相匹配的特征點，計算2 組點的質(zhì)心位置p'和p，計算每個點的去質(zhì)心坐標：

對式（3）進行化簡，去除與R無關(guān)項，實際優(yōu)化函數(shù)化簡為

式中：E為奇異值組成的對角矩陣，對角線元素從大到小排列；U、V為對角矩陣。

當W為滿秩時，即可根據(jù)式（6）計算得出R：

通過計算出的R計算平移向量t：

式中：R表示當前幀與最新關(guān)鍵幀之間的幀間旋轉(zhuǎn)矩陣；t表示二者之間的平移向量。

通過R和t求出D，并在ORB-SLAM2 系統(tǒng)中為其設定閾值Dk：

式中： α為位移與轉(zhuǎn)角的平衡權(quán)值系數(shù)，取值大小隨轉(zhuǎn)角增大呈指數(shù)形式增加。由于本文算法在原有以幀數(shù)為判定依據(jù)的關(guān)鍵幀選擇算法基礎(chǔ)上增加了轉(zhuǎn)角作為主要變化量，轉(zhuǎn)角所采用的平衡權(quán)值系數(shù)，在存在角度轉(zhuǎn)變時遠大于位置移動的平衡權(quán)值系數(shù)，采用式(9)的計算公式進行計算，設ω=min(2π-‖R‖,‖R‖)，取值范圍為0°～90°（當旋轉(zhuǎn)角度大于90°時，將徹底失去視角，無法特征點匹配）：

對計算所得相對運動量與其閾值進行比較：當D≥Dk時， Framekey=Framecur， FrameRef=Framecur； 當D

當判定通過后，將當前幀設為關(guān)鍵幀，同時更新參考關(guān)鍵幀，再次計算當前幀與新關(guān)鍵幀之間的相對運動量，算法流程如圖2 所示。圖中：Tracking線程中存在成員變量指針mCurrentFrame，指向當前幀成員變量，其中包括圖像、深度信息、相對位姿等信息。

圖2 關(guān)鍵幀選擇算法流程Fig. 2 Flowchart of key frame selection algorithm

步驟 1 判斷插入關(guān)鍵幀的前置條件是否滿足：系統(tǒng)不處于定位模式且不在全局閉環(huán)的條件下，同時距離上一次重定位的距離較遠，內(nèi)點數(shù)必須超過設定的最小閾值，保證關(guān)鍵幀跟蹤質(zhì)量。

步驟 2 在原有關(guān)鍵幀篩選條件的基礎(chǔ)上，增加了幀間相對運動量作為篩選條件之一，System 主線程中將Tracking 線程中的第1 幀作為初始關(guān)鍵幀，根據(jù)式(8)計算出當前幀與關(guān)鍵幀的幀間相對運動量D。

步驟 3 對幀間相對運動量進行比較。如果當前幀的相對運動大于或等于一定的閾值，說明相機相對于之前跟蹤定位的位置有了較大的相對運動，相機的視角發(fā)生了較大改變，此時不生成新的關(guān)鍵幀進行定位，極有可能在跟蹤定位時產(chǎn)生誤差，甚至因檢測不到地圖點造成跟蹤中斷。

因此，當相對運動較大時，添加關(guān)鍵幀可以有效改善這種情況。當關(guān)鍵幀生成后，用新產(chǎn)生的關(guān)鍵幀作為參考關(guān)鍵幀繼續(xù)計算下一幀的相對位姿，直到整個系統(tǒng)循環(huán)結(jié)束。

2.2.2 劣質(zhì)關(guān)鍵幀剔除

本文自主設計研發(fā)的移動機器人，在運行過程中與機械臂共用相機，因此地面輪胎的打滑及機器人移動過程中的震動，會導致相機與機器人本身發(fā)生相對運動從而改變相機的視角，產(chǎn)生誤拍攝，此時所得關(guān)鍵幀會對跟蹤定位產(chǎn)生誤導，從而使定位精度降低，本文將其定義為劣質(zhì)關(guān)鍵幀。針對這一問題，本文設計了劣質(zhì)關(guān)鍵幀剔除算法，剔除由于誤拍攝造成的劣質(zhì)關(guān)鍵幀，提高跟蹤定位精度。

在ORB-SLAM2 跟蹤定位線程中，存在冗余關(guān)鍵幀剔除模塊，該模塊只對冗余關(guān)鍵幀進行剔除，并不對精度產(chǎn)生影響，其目的是為了節(jié)約資源，減輕工作線程負擔。本文基于冗余關(guān)鍵幀剔除模塊，設計了劣質(zhì)關(guān)鍵幀剔除算法，剔除過程如下：

步驟 1 在關(guān)鍵幀生成時，臨時存儲彩色圖像及時間戳。

步驟 2 選取相鄰3 關(guān)鍵幀進行ORB 特征匹配，分別保存了2 次相鄰關(guān)鍵幀特征點匹配數(shù)量NUMAdj1、NUMAdj2及一次相隔關(guān)鍵幀匹配數(shù)NUMSep。

步驟 3若相鄰2 幀的特征點匹配數(shù)量小于相隔2 幀，說明3 幀中的中間幀大概率是由于誤拍攝而產(chǎn)生的關(guān)鍵幀。

步驟 4激活關(guān)鍵幀剔除函數(shù)對劣質(zhì)關(guān)鍵幀進行剔除，繼續(xù)線程的運行，直到整個線程結(jié)束，釋放內(nèi)存。

偽代碼如下：

這種方法區(qū)別于傳統(tǒng)SLAM 算法中只對相鄰2幀的關(guān)鍵幀進行比較，而是采用了比較相鄰3 幀的方法。由于本文所設計的移動機器人平臺不存在視角突變，算法運行過程中，如果第1 幀和第3 幀差異較小，而第2 幀與兩者之間圖像差異較大，說明第2 幀在拍攝的過程中，因震動或其他原因?qū)е孪鄼C本身與移動機器人之間發(fā)生了相對運動，從而使視角發(fā)生了改變，產(chǎn)生了劣質(zhì)關(guān)鍵幀。

3 實 驗

3.1 公開數(shù)據(jù)集實驗

在Ubuntul 16.04 環(huán)境下進行了公開數(shù)據(jù)集實驗，計算機所用的CPU 為2.0 GHz i7，運行內(nèi)存為4 GB。實驗采用TUM RGB-D Benchmark 數(shù)據(jù)集，選取fr1中的desk、xyz 和room 進行。依次對比了常用關(guān)鍵幀選擇算法、改進關(guān)鍵幀選擇算法(包含劣質(zhì)關(guān)鍵幀)、改進關(guān)鍵幀選擇算法(剔除劣質(zhì)關(guān)鍵幀)的實驗效果。

3.1.1 ORB-SLAM2 實 時 性 分 析

SLAM 的實時性是評價其優(yōu)劣程度重要的性能指標，表1 對各個數(shù)據(jù)集平均跟蹤時間進行了記錄。

從表1 可知，由于對關(guān)鍵幀選擇算法進行了改進，增加了關(guān)鍵幀選擇的計算量，導致運行時間稍有增加，但由于相對整個系統(tǒng)而言計算并不復雜，增加時間極少，對系統(tǒng)的實時性并沒有造成影響。

表1 平均跟蹤時間對比結(jié)果Table 1 Comparison of mean tracking time

TUM 計算機視覺組數(shù)據(jù)集中包含利用運動視覺捕捉系統(tǒng)記錄的攝像頭在該場景下真實且連續(xù)的軌跡數(shù)據(jù)，圖3(a)～圖3(c)分別為3 種算法所得到的估計軌跡與真實軌跡的對比軌跡曲線。可以明顯看出：①在圖3(a)的軌跡交叉處有較大視角突變，原有關(guān)鍵幀選擇算法未能檢測到此信息，導致跟蹤失敗，采用線段直接相連的方式進行軌跡估計；而改進后的算法如圖3(b)、圖3(c)所示，跟蹤失敗并未發(fā)生，說明改進算法可以將轉(zhuǎn)彎信息較好地采集，并用于關(guān)鍵幀選擇，有效解決了跟蹤失敗的問題。②圖3(a)中軌跡最右側(cè)轉(zhuǎn)彎后估計軌跡明顯偏離真實值，是由于原有關(guān)鍵幀選擇算法檢測轉(zhuǎn)彎信息出現(xiàn)了滯后，造成較大偏差；改進后的算法增加了相對變化量，縮減了該處的定位誤差。③由于劣質(zhì)關(guān)鍵幀僅在移動機器人實驗中產(chǎn)生，圖3(b)與圖3(c)相差不大。

圖3 三種關(guān)鍵幀選擇算法運動軌跡對比Fig. 3 Comparison of motion trajectories of three key frame selection algorithms

對于定位精度，采用絕對軌跡誤差（absolute trajectory error，ATE）進行比較。ATE 越小，精度越高。第i幀的ATE 定義如下：

3.1.2 ORB-SLAM2 跟蹤定位精度分析

式中： Δ為真實位置與估計位置之間的誤差；Fi為第i幀的ATE；Qi為真實位姿；Pi為算法估計位姿；S為從估計位姿到真實位姿的相似旋轉(zhuǎn)矩陣。

將估計軌跡與真實軌跡進行對比，繪制出絕對軌跡誤差曲線，如圖4 所示。圖4(a)為常用關(guān)鍵幀選擇算法所得到的估計軌跡與真實曲線的誤差，圖4(b)為2 種改進關(guān)鍵幀選擇算法后所得到估計軌跡與真實軌跡的誤差，是否添加劣質(zhì)關(guān)鍵幀剔除算法對誤差結(jié)果并沒有太大影響，說明劣質(zhì)關(guān)鍵幀剔除算法對數(shù)據(jù)集的定位精度并沒有太大影響。由圖4可以看出，相較于常用關(guān)鍵幀選擇算法，改進后的算法有明顯提高。

圖4 定位軌跡誤差曲線Fig. 4 Curve of track positioning error

選取最大絕對軌跡誤差作為評判標準，表2 為3 種算法下的最大絕對軌跡誤差。

由表2 可知：①改進關(guān)鍵幀選擇算法相比于常用關(guān)鍵幀選擇算法，在3 個數(shù)據(jù)集中誤差均明顯降低，最優(yōu)結(jié)果改進后的算法誤差僅為改進前誤差的51.9%；②剔除了劣質(zhì)關(guān)鍵幀的改進算法與包含劣質(zhì)關(guān)鍵幀的選擇算法軌跡誤差相差較小，僅存在合理波動，這是由于以數(shù)據(jù)集作為實驗對象時，數(shù)據(jù)集內(nèi)部并不包含劣質(zhì)關(guān)鍵幀。

表2 最大絕對軌跡誤差對比Table 2 Maximum absolute trajectory error

綜上，改進后的關(guān)鍵幀選擇算法在保證實時性的情況下，能夠有效地提高算法估計軌跡精度，同時還能有效避免跟蹤失敗的情況發(fā)生。

3.1.3 ORB-SLAM2 關(guān)鍵幀數(shù)量分析

圖5 比較了不同數(shù)據(jù)集下3 種關(guān)鍵幀選擇算法計算的關(guān)鍵幀數(shù)量。顯然，改進后的關(guān)鍵幀選擇算法的關(guān)鍵幀數(shù)量明顯大于改進前，說明改進后的算法選擇了更多的關(guān)鍵幀以提升估計定位精度。

圖5 數(shù)據(jù)集實驗關(guān)鍵幀數(shù)量比較Fig. 5 Comparison of key frame numbers in dataset experiments

3.2 移動機器人實驗

本文自主設計的移動機器人如圖6 所示，采用大唐工控機新創(chuàng)云i7mini 主機作為平臺的嵌入式主控機，搭配一款15.6 寸觸控屏作為人機交互硬件，并選用奧比中光深度相機作為視覺傳感器為機器人傳送視覺信息數(shù)據(jù)。實驗比較了3 種算法在實時運行過程中的定位精度。

圖6 移動機器人Fig. 6 Mobile robot

實驗內(nèi)容主要包括機器人直線運動、轉(zhuǎn)彎運動及回環(huán)運動3 種運動方式。對機器人設計搭建、相機標定等操作后，通過對3 種運動的實際軌跡和ORB-SLAM2 跟蹤估計的定位軌跡進行測量，分別進行了6 次實驗，并取誤差平均值進行比較。圖7為3 種關(guān)鍵幀選擇算法對應的回環(huán)運動軌跡對比。由于數(shù)據(jù)為機器人實時輸出，真實軌跡存在差異，并不能在同一坐標系內(nèi)進行繪制。

由圖7 可知：①圖7(b)相比于圖7(a)有一部分路程未能檢出，發(fā)生了跟蹤失敗的情況，在轉(zhuǎn)彎處，明顯因檢測信息滯后而導致估計轉(zhuǎn)角過小增大誤差；②由圖7(c)、(d)軌跡曲線可知，算法未發(fā)生大規(guī)模跟蹤失敗，且轉(zhuǎn)角處定位精度明顯提高；③圖7(e)、(f)為剔除了劣質(zhì)關(guān)鍵幀的改進算法，由于剔除了劣質(zhì)關(guān)鍵幀，圖7(f)的軌跡更接近真實軌跡曲線，抖動明顯減少，在一定程度上提高了定位精度。

圖7 回環(huán)運動軌跡對比Fig. 7 Comparison of loop trajectories

表3 為不同關(guān)鍵幀選擇算法所得到的軌跡與真實軌跡平均位置誤差?？芍孩侔淤|(zhì)關(guān)鍵幀的改進算法相較于改進前算法，成功提高了定位精度，由于算法改進主要針對轉(zhuǎn)向，直線時誤差減小并不明顯，但轉(zhuǎn)彎與回環(huán)運動時定位精度顯著提高，誤差僅為原誤差的58.1%；②剔除劣質(zhì)關(guān)鍵幀的改進算法軌跡精度與未剔除相比明顯提高，尤其在直行時，誤差為包含劣質(zhì)關(guān)鍵幀算法誤差的82.1%。

表3 平均位置誤差Table 3 Average position error

圖8 比較了機器人不同運行方式下的3 種關(guān)鍵幀選擇算法計算的關(guān)鍵幀數(shù)量。顯然，機器人實時運行的過程中，改進后的算法除直線外同樣選擇更多的關(guān)鍵幀以保證算法精度，而直線過程未發(fā)生角度轉(zhuǎn)變，因此關(guān)鍵幀數(shù)量在誤差范圍內(nèi)合理波動；剔除劣質(zhì)關(guān)鍵幀后，關(guān)鍵幀數(shù)量相對減少，在直線過程中最為明顯，所剔除的劣質(zhì)關(guān)鍵幀數(shù)量約達到關(guān)鍵幀總數(shù)的25%；回環(huán)運動的直線行駛路程為直線運動的2 倍，但多次實驗所剔除的劣質(zhì)關(guān)鍵幀數(shù)量均未達到直線運動的2 倍。造成這種現(xiàn)象的原因有3 點：①角度的轉(zhuǎn)變會對劣質(zhì)關(guān)鍵幀的判斷造成干擾；②劣質(zhì)關(guān)鍵幀是否生成與機器人所處環(huán)境的優(yōu)劣特別是路面的平滑度有著極大的關(guān)聯(lián)；③實時運行的過程不能嚴格保證機器人運行軌跡完全相同。

圖8 移動機器人實驗關(guān)鍵幀數(shù)量比較Fig. 8 Comparison of key frame numbers in mobile robot experiments

3.3 實驗參數(shù)分析

實驗過程中應對某些參數(shù)進行調(diào)整，參數(shù)調(diào)整不當會造成定位錯誤，系統(tǒng)運行資源浪費，甚至使系統(tǒng)崩潰。對于不同的數(shù)據(jù)集，通過計算及多次實驗調(diào)整的方式確定算法所用參數(shù)。

最大幀間相對位移量Dk是通過計算并進行多次實驗后，依據(jù)算法最小絕對估計誤差確定。Dk越大，表明選取關(guān)鍵幀時通過的相對位移越大，可能導致重要關(guān)鍵幀被忽略而導致精度降低；Dk越小，表明選取時通過的相對位移越小，選取的關(guān)鍵幀就會隨之增多，產(chǎn)生冗余，導致系統(tǒng)實時性降低，對后續(xù)的建圖工作產(chǎn)生較大影響。本文中Dk取轉(zhuǎn)角為45°、位移為0 時計算值，并通過實驗進行微調(diào)。此轉(zhuǎn)角下相鄰2 關(guān)鍵幀在匹配到足夠特征點進行計算的同時，有效防止過多關(guān)鍵幀的產(chǎn)生浪費系統(tǒng)內(nèi)存。在不同的實驗條件下，相機的采樣頻率、機器人的運行速度及環(huán)境的規(guī)模大小均對Dk的最佳值產(chǎn)生影響。一般情況，Dk取30～50 為最佳。

4 結(jié) 論

1) 提出了一種改變關(guān)鍵幀選擇判定條件的ORBSLAM2 算法，在原有算法的基礎(chǔ)增加了相對運動量作為選擇依據(jù)，有效避免了跟蹤失敗，提高了定位精度。

2) 提出了一種劣質(zhì)關(guān)鍵幀剔除算法，解決了相機與移動機器人機身之間的相對運動產(chǎn)生的劣質(zhì)關(guān)鍵幀問題。

3) 通過數(shù)據(jù)集與移動機器人實驗驗證了算法的優(yōu)異性。實驗結(jié)果表明，數(shù)據(jù)集估計定位誤差的最優(yōu)結(jié)果約為改進前算法的51.9%；移動機器人實驗約為改進前的62.9%。剔除劣質(zhì)關(guān)鍵幀后，直線誤差僅為原來的82.1%。