一種改進(jìn)點(diǎn)線特征融合的雙目視覺慣性定位算法*

丁 曉，晏玉祥，張永建，蘭衛(wèi)旗，白曉亮

（1. 航空工業(yè)成都飛機(jī)工業(yè)（集團(tuán)）有限責(zé)任公司，成都 610092；2. 西北工業(yè)大學(xué)，西安 710072）

在航空航天等大型復(fù)雜產(chǎn)品裝配過程中，裝配工藝復(fù)雜[1]，由于時間和空間上的限制，目前裝配工作大部分依賴操作人員對照工藝文件和現(xiàn)場環(huán)境展開裝配，信息不集中且可視化程度不高，裝配工作量約占整個產(chǎn)品研制和生產(chǎn)時間的50%[2]。利用增強(qiáng)現(xiàn)實(shí) （Augmented reality，AR）輔助裝配[3–4]可以在一定程度上解決采用二維紙質(zhì)工藝文件帶來的劣勢問題，給操作工人提供一種真實(shí)與虛擬引導(dǎo)信息無縫銜接的混合現(xiàn)實(shí)操作場景，能提高操作的代入感和真實(shí)感，滿足產(chǎn)品裝配質(zhì)量的同時也可以節(jié)省時間和成本。

增強(qiáng)現(xiàn)實(shí)輔助裝配依賴虛擬對象和真實(shí)場景的注冊一致性，定位技術(shù)是實(shí)現(xiàn)虛擬引導(dǎo)信息與裝配現(xiàn)場實(shí)時融合的核心。增強(qiáng)現(xiàn)實(shí)輔助裝配通常通過識別場景標(biāo)簽來對增強(qiáng)現(xiàn)實(shí)設(shè)備進(jìn)行定位，但要求保證標(biāo)簽實(shí)時出現(xiàn)在相機(jī)視野中，才能實(shí)現(xiàn)裝配引導(dǎo)的效果。這種傳統(tǒng)的視覺定位方式，在紋理少的區(qū)域無法工作，當(dāng)運(yùn)動過快或者劇烈晃動的時候，相機(jī)拍攝的圖像由于運(yùn)動而導(dǎo)致模糊；慣性測量單元 （Inertial measurement unit，IMU）可以測量角速度和加速度，對于短時間內(nèi)的快速運(yùn)動具有較好的估計(jì)，但是在靜止情況下測量值明顯存在漂移。相比IMU，相機(jī)拍攝的照片不會存在漂移，使得相機(jī)在慢速運(yùn)動之后的位姿估計(jì)依然有效[5]，但當(dāng)圖像發(fā)生變化之后，相機(jī)自身是難以感知運(yùn)動的，而IMU 能夠感受到本體的運(yùn)動信息，可減少動態(tài)物體的影響。由于IMU 和相機(jī)具有明顯的互補(bǔ)性，基于多傳感器融合的位姿估計(jì)技術(shù)在增強(qiáng)現(xiàn)實(shí)方面得到了越來越多的應(yīng)用[6]。

山東大學(xué)李民[7]采用IMU 和視覺標(biāo)簽Apriltag 融合的辦法組合測量頭盔的位姿，這種定位方式缺點(diǎn)在于光照對相機(jī)產(chǎn)生干擾使其檢測不到標(biāo)簽。西安交通大學(xué)王崴等[8]在面向增強(qiáng)現(xiàn)實(shí)的算法中，提出一種基于互補(bǔ)濾波和無跡卡爾曼濾波的定位方案，該定位方法要求AR 頭盔實(shí)時處在攝像機(jī)視野中，且頭盔標(biāo)記點(diǎn)易受光照遮擋影響。融合了全球定位系統(tǒng) （Global positioning system，GPS）、IMU 以及方向傳感器的融合方法可有效降低累積誤差。在AR 應(yīng)用中，為了與場景交互，即時定位與地圖構(gòu)建 （Simultaneous localization and mapping，SLAM）算法必須提供魯棒的帶正確尺度信息的相機(jī)位姿估計(jì)，視覺慣性定位技術(shù)成為主流的發(fā)展趨勢。傳統(tǒng)的SLAM 框架主要包括前端模塊和后端模塊。前端部分主要通過傳感器來感知場景信息、估算位姿和速度等初始信息。根據(jù)處理方法的不同，后端接收前端的初始數(shù)據(jù)，可以分為基于濾波和基于優(yōu)化兩種方法[9]。

基于優(yōu)化的方法使用了更多信息，隨著計(jì)算機(jī)應(yīng)用技術(shù)的發(fā)展，采用圖優(yōu)化處理SLAM 的方法逐漸替代濾波的方法成為主流，這其中需要采用第三方庫g2o[10]、Ceres-Solver[11]、GTSAM[12]等來進(jìn)行優(yōu)化。圖優(yōu)化方法核心是將所有的點(diǎn)和邊聯(lián)合優(yōu)化，對自身位姿進(jìn)行全局最優(yōu)處理。

Leutenegger 等[13]提出了一種基于關(guān)鍵幀的非線性優(yōu)化的視覺慣性融合SLAM 系統(tǒng)，能夠得到比較好的精度和魯棒性。香港科技大學(xué)提出的VINS-Mono[6]系統(tǒng)在經(jīng)典的視覺SLAM 框架之上融合了相機(jī)和IMU，視覺前端是利用KLT[14]光流進(jìn)行角點(diǎn)跟蹤，后端是采用了滑動窗口來進(jìn)行非線性優(yōu)化。此后，為了避免VINS-Mono 初始化不穩(wěn)定的情況，提出了其雙目版VINS-Fusion，根據(jù)雙目立體獲得深度信息的原理，可以實(shí)時提供穩(wěn)定可靠的場景尺度信息。在視覺信息中，點(diǎn)特征是最常用的特征類型，但在弱紋理的情形下，采用點(diǎn)特征跟蹤的方法可能會追蹤不到足夠的特征點(diǎn)而出現(xiàn)跟蹤失敗或者無法跟蹤的情況[15]。沈冉[16]提出了一種點(diǎn)特征為主，線特征為輔的點(diǎn)–線融合的單目視覺慣性里程計(jì)方法，確保了系統(tǒng)在昏暗環(huán)境中提取跟蹤的點(diǎn)線數(shù)據(jù)的穩(wěn)定性，獲得了較為魯棒的定位效果，但其單目無法提供穩(wěn)定可靠的深度場景信息。而針對結(jié)構(gòu)化重復(fù)紋理場景，He等[17]提出一種使用點(diǎn)線特征融合的視覺前端、視覺和IMU 緊耦合后端優(yōu)化的PL-VIO 算法，優(yōu)化最小化誤差函數(shù)，實(shí)現(xiàn)了較好的定位效果。但該算法不支持在線使用，且計(jì)算量大，不具備場景深度恢復(fù)功能。趙良玉等[18]提出了一種采用Shi-Tomasi 角點(diǎn)提取點(diǎn)特征信息，同時增加快速直線檢測 （Fast line detector，F(xiàn)LD）提取線特征的點(diǎn)線特征融合的雙目慣性SLAM 算法，在保證定位精度的同時能夠獲得更為豐富的環(huán)境地圖，具備較好的魯棒性。其采用的Shi-Tomasi 角點(diǎn)提取方法，可以獲得較快的提取速度，但在使用KLT 光流法實(shí)現(xiàn)特征點(diǎn)的跟蹤時，可能會因?yàn)橄鄼C(jī)的劇烈運(yùn)動而導(dǎo)致幀間匹配失敗，追蹤不夠穩(wěn)定。

針對復(fù)雜裝配任務(wù)、弱紋理下強(qiáng)旋轉(zhuǎn)導(dǎo)致特征容易丟失，增強(qiáng)現(xiàn)實(shí)設(shè)備持續(xù)穩(wěn)定的跟蹤定位非常困難的問題，本文提出了一種改進(jìn)的PL-VIO[17]（Improved PLVIO，IPL-VIO）算法，使用多傳感器融合的位姿估計(jì)技術(shù)，通過視覺和IMU 的定位方式，對相機(jī)進(jìn)行內(nèi)參標(biāo)定并進(jìn)行誤差分析，提高系統(tǒng)采集數(shù)據(jù)的精確度，對圖像進(jìn)行特征分析，提取特征點(diǎn)和線，并采用PL-VIO 算法為主體框架，針對點(diǎn)特征，采用ORB（Oriented fast and rotated brief）算法提取特征點(diǎn)，并改進(jìn)光度不變假設(shè)，采用多層光流進(jìn)行跟蹤。與文獻(xiàn)[18]中的Shi-Tomasi 角點(diǎn)提取相比，ORB 可以用來對圖像中的關(guān)鍵點(diǎn)快速創(chuàng)建特征向量，并用這些特征向量來識別圖像中的對象，因此幀間匹配時追蹤更為穩(wěn)定；針對線特征，通過改進(jìn)的線段檢測器 （Line segment detector，LSD）提取算法，建立合適的空間表征描述方法，同時進(jìn)行雙目左右相機(jī)之間的匹配計(jì)算位姿。采取視覺和IMU 緊耦合的方式，建立基于點(diǎn)線特征的視覺和IMU 融合的位姿誤差融合模型，通過對比PL-VIO 算法試驗(yàn)，本文提出的IPL-VIO算法在結(jié)構(gòu)化場景下的絕對位移和旋轉(zhuǎn)誤差比原算法誤差更小，同時結(jié)構(gòu)化場景信息更加豐富，能夠應(yīng)用在弱紋理的AR 裝配現(xiàn)場中，為增強(qiáng)現(xiàn)實(shí)輔助裝配平臺提供穩(wěn)定可靠的位姿數(shù)據(jù)，以提升多傳感器融合的位姿估計(jì)精度，實(shí)現(xiàn)AR 設(shè)備的實(shí)時定位。

1 多傳感器標(biāo)定

相機(jī)與IMU 的標(biāo)定包括在線標(biāo)定和離線標(biāo)定兩種方式，在線標(biāo)定是指在定位系統(tǒng)初始化過程中估計(jì)相機(jī)與IMU 的外參，方便可靠，但對初始化過程要求比較高?？紤]到裝配場景及裝配任務(wù)的復(fù)雜性，難以保證完成在線標(biāo)定。因此，本文主要采取離線標(biāo)定的方式，采用Rehder 等[19]提出的集成多相機(jī)標(biāo)定、相機(jī)和IMU標(biāo)定的離線標(biāo)定工具Kalibr，在估計(jì)出ZED Mini 相機(jī)內(nèi)參之后，可進(jìn)行優(yōu)化迭代操作。ZED Mini 相機(jī)是立體相機(jī)公司Stereolabs 研發(fā)的一款迷你版的立體聲深度測繪攝像機(jī)，可以作為虛擬現(xiàn)實(shí) （Virtual reality，VR）頭顯HTC VIVE 的增幅設(shè)備，為頭顯提供實(shí)時的拍攝視頻傳輸、深度感知和環(huán)境映射，讓VR 頭顯轉(zhuǎn)變?yōu)锳R 頭顯。在連續(xù)運(yùn)動場景下標(biāo)定相機(jī)系統(tǒng)的內(nèi)外參，同時還可以標(biāo)定相機(jī)和IMU 的相對位姿，以及相機(jī)的重投影誤差信息。

雙目標(biāo)定試驗(yàn)結(jié)果包含相機(jī)的左目和IMU 單元的相對位置信息TIC和TCI，即

2 圖像特征提取與跟蹤

2.1 圖像特征提取

特征是圖像信息的另一種數(shù)字表達(dá)形式。在視覺SLAM 中，常用的特征點(diǎn)提取算法有SIFT[20]、SURF[21]、FAST[22]、ORB[23]和BRIEF[24]等。SIFT 特征點(diǎn)具有信息量豐富、高速性、可擴(kuò)展性強(qiáng)的特點(diǎn)，具有較強(qiáng)的圖像特征提取魯棒性，對旋轉(zhuǎn)、尺度縮放、亮度變化保持不變。SURF 改善了SIFT 計(jì)算復(fù)雜度高的缺點(diǎn)，對特征向量描述進(jìn)行了改進(jìn)。ORB 是一種快速特征點(diǎn)提取和描述的算法，其結(jié)合了FAST 特征點(diǎn)的檢測方法和BRIEF 的特征描述子的優(yōu)點(diǎn)進(jìn)行改進(jìn)和優(yōu)化。BRIEF特征點(diǎn)提取算法與SIFT 的提取方法一致，不同之處在于其在特征點(diǎn)附近隨機(jī)選取若干點(diǎn)，根據(jù)灰度值的大小，描述為二進(jìn)制字符串，大大加快了描述子建立速度，但旋轉(zhuǎn)不變性差。

檢測匹配主要流程是首先對采集的照片進(jìn)行特征點(diǎn)的提取，分別提取SIFT、SURF 和BRIEF、ORB 特征，并兩兩進(jìn)行特征點(diǎn)的比較和匹配，同時對匹配的結(jié)果進(jìn)行RANSAC[25]提純計(jì)算，計(jì)算出其中的內(nèi)點(diǎn)。統(tǒng)計(jì)匹配的特征點(diǎn)（即內(nèi)點(diǎn)）所占總特征點(diǎn)數(shù)目的比例來計(jì)算內(nèi)點(diǎn)率，再根據(jù)內(nèi)點(diǎn)率及匹配耗時計(jì)算獲得每個內(nèi)點(diǎn)所需要的平均耗時 （即時效比），為保證較高的內(nèi)點(diǎn)率，調(diào)整合適的提取特征點(diǎn)數(shù)量。建立模型，即

內(nèi)點(diǎn)率=匹配內(nèi)點(diǎn)數(shù)/特征點(diǎn)總數(shù) （1）

時效比=內(nèi)點(diǎn)率/耗時 （2）

傳統(tǒng)SLAM 系統(tǒng)中均采用點(diǎn)特征來進(jìn)行跟蹤，而在裝配環(huán)境中，由于工人頭部轉(zhuǎn)動導(dǎo)致圖像模糊，無法完成定位。而裝配場景存在結(jié)構(gòu)化的線特征，因此采取LSD[26]直線檢測方法提取裝配場景中的點(diǎn)特征進(jìn)行匹配。

5 種檢測匹配算法的時效比分析如圖1 所示。分析可知，SIFT 和SURF 檢測的準(zhǔn)確率高；ORB 的檢測匹配速度最快，時效比最高；LSD 的匹配效果更好，但是檢測比較耗時。單獨(dú)從特征提取的角度來看，SIFT、ORB、BRIEF 耗時較少?？紤]到結(jié)構(gòu)化的裝配場景，為了保證在復(fù)雜狹小空間內(nèi)工人頭部轉(zhuǎn)動和相機(jī)劇烈抖動的情況下追蹤穩(wěn)定，選用匹配效果較好的LSD 算法進(jìn)行直線特征提取，下文主要以O(shè)RB 點(diǎn)特征和LSD 線特征為例進(jìn)行研究。

LSD 算法用來表征場景的結(jié)構(gòu)特征，但無參數(shù)調(diào)整的LSD 算法在圖像模糊或者有噪聲時，易檢測出過多的短線段特征，另外，檢測時一條長線段容易分割成多條線段，破壞了線特征的完整性，因此通過改進(jìn)LSD 算法進(jìn)行線特征提取。首先通過圖片大小設(shè)置最小長度閾值進(jìn)行線段長度的初步篩選，LSD 檢測線段的兩個端點(diǎn)分為起點(diǎn)和終點(diǎn)，根據(jù)線段支持域的最小外接矩形的主方向、線段之間的距離、線段的最近兩個端點(diǎn)之間的距離和線段的描述子之間的距離來判斷兩條線段是否在同一條直線上，如果滿足閾值條件則合并兩條線段，減少短線段的干擾。

2.2 圖像特征跟蹤

結(jié)構(gòu)化裝配場景下特征提取點(diǎn)的提取和描述子的計(jì)算非常耗時，為了克服這個缺陷，本研究采用直接法來進(jìn)行特征點(diǎn)的跟蹤，即根據(jù)像素灰度差異值來計(jì)算，而且裝配場景存在明暗變化的情況，直接法可以直接工作[5]。本研究以文獻(xiàn)[27]為基礎(chǔ)，使用雙目相機(jī)和IMU 進(jìn)行耦合。在裝配場景中，工人頭部在轉(zhuǎn)動時，利用IMU 計(jì)算頭盔空間位姿，從而預(yù)測圖片特征點(diǎn)的位姿，再利用非線性優(yōu)化 （如滑動窗口優(yōu)化）來估計(jì)位姿。在特征點(diǎn)匹配完成之后，利用隨機(jī)采樣一致性 （Random sample consensus，RANSAC）的方法進(jìn)行異常匹配對的移除。

本研究使用Lucas-Kanade 光流法進(jìn)行特征跟蹤，由于場景中的亮度是改變的，因此使用參數(shù)α和β來校正兩個圖像間的光度變化[28]，圖像Ik和Ik+1之間的光度殘差為

式中，ΩL是內(nèi)點(diǎn)的集合；ξ表示從Ik到Ik+1變換矩陣；u為像素坐標(biāo)；u'為圖像中u對應(yīng)點(diǎn)像素坐標(biāo)。

3 基于視覺和慣性融合的位姿誤差融合模型

為了在AR 裝配過程中完成真實(shí)與虛擬裝配場景的融合，需要計(jì)算AR 設(shè)備的位姿估計(jì)，位姿主要包含相機(jī)的旋轉(zhuǎn)和平移變換，其6 維度的狀態(tài)變量x可以表示為

式中，R和p代表相機(jī)的旋轉(zhuǎn)和位移。當(dāng)相機(jī)運(yùn)動起來后，場景中特征點(diǎn)比較多，特征點(diǎn)的管理較為復(fù)雜，為了更好實(shí)現(xiàn)位姿的估計(jì)，需要參考當(dāng)前時刻周圍的位姿，由于地圖特征可以通過邊緣化的方法轉(zhuǎn)換為位姿間的約束，簡化為對位姿序列的估計(jì)[29]。可以用位姿圖的方式來直接描述。純視覺里程計(jì)位姿估計(jì)因子圖中的節(jié)點(diǎn)對應(yīng)相機(jī)在不同時刻的位置和姿態(tài)，而邊則描述了位姿與位姿間的空間約束關(guān)系，這種約束關(guān)系通過里程計(jì)或觀測信息的配準(zhǔn)得到。

在基于點(diǎn)線融合的視覺里程計(jì)局部地圖中，相機(jī)觀測到視野中的點(diǎn)特征和線特征，根據(jù)投影方程建立點(diǎn)和線特征的誤差模型，對優(yōu)化的R和p進(jìn)行狀態(tài)約束。在對IMU 的參數(shù)標(biāo)定試驗(yàn)中，通過對IMU 的零偏變換曲線分析可以得出零偏是一個動態(tài)量，其滿足一階馬爾科夫的性質(zhì)。本文采取一種緊耦合的方式使用相機(jī)的位姿、速度和IMU 的零偏構(gòu)建目標(biāo)方程進(jìn)行狀態(tài)估計(jì)。系統(tǒng)的狀態(tài)變量x共15 維，可以表示為

式中，R、p和v分別是相機(jī)的旋轉(zhuǎn)、平移和速度；ba和bg分別是IMU 的加速度計(jì)和陀螺儀的零偏。為了同時對狀態(tài)變量進(jìn)行優(yōu)化，采用捆綁調(diào)整[30]（Bundle adjustment，BA）的方法，同時本研究根據(jù)固有的核心BA 算法，將原有的BA 問題分解成若干因子圖，如圖2 所示，在因子圖中，各個子圖具有各自的局部坐標(biāo)系并且可以進(jìn)行優(yōu)化。

圖2 點(diǎn)線融合的視覺慣性里程計(jì)位姿估計(jì)因子圖Fig.2 Position and pose estimation factor diagram of visual inertial odometer based on point and line fusion

在圖2 所示的視覺慣性里程計(jì)的位姿估計(jì)圖中，首先計(jì)算出每一幀的位姿、速度和零偏。然后將位姿估計(jì)問題轉(zhuǎn)化為聯(lián)合優(yōu)化問題，優(yōu)化狀態(tài)變量x主要包括相機(jī)的位姿、旋轉(zhuǎn)速度以及IMU 的零偏，通過最小化特征點(diǎn)和IMU 的誤差來優(yōu)化當(dāng)前幀的位姿，誤差方程可以表示為

而針對線特征重投影誤差[31]，圖優(yōu)化問題的本質(zhì)是找到一個最優(yōu)的狀態(tài)值p，如果利用測量協(xié)方差矩陣Σz的多元正態(tài)分布對似然分布進(jìn)行建模，則狀態(tài)值p*可以表示為

式中，Z為圖像中線特征測量值；εZ為線特征的投影預(yù)測誤差。

綜上所述，最小化特征點(diǎn)、線特征和IMU 的誤差方程可以表示為

4 定位精度試驗(yàn)與分析

為了進(jìn)一步研究IPL-VIO 算法在復(fù)雜場景下的定位精度，選取公開數(shù)據(jù)集，通過計(jì)算公開的數(shù)據(jù)集數(shù)據(jù)，對現(xiàn)有的算法和IPL-VIO 算法進(jìn)行評測，公開數(shù)據(jù)集的選取要反映出線束裝配場景的復(fù)雜性，存在結(jié)構(gòu)化特征、重復(fù)紋理、弱紋理的特點(diǎn)，同時也要體現(xiàn)出裝配任務(wù)的復(fù)雜性。本研究選取EuRoc 數(shù)據(jù)集，它是著名的應(yīng)用于視覺慣性導(dǎo)航的數(shù)據(jù)集之一。用數(shù)據(jù)集的好處在于數(shù)據(jù)集的采集使用高精度的設(shè)備，可以提供精準(zhǔn)的真值進(jìn)行對比，減少了因設(shè)備和客觀因素導(dǎo)致的誤差。

EuRoc 提供20 Hz 的雙目相機(jī)圖像、200 Hz 的IMU數(shù)據(jù)和相機(jī)真實(shí)的運(yùn)動位姿，選取和裝配場景類似的Machine Hall 離線測試集，圖像分辨率為752×480，頻率為20 Hz，IMU 頻率為200 Hz，從均方根誤差、均方根旋轉(zhuǎn)誤差、運(yùn)行時間、GPU 占用率和內(nèi)存占用率這5個方面對VINS-Fusion[6]、PL-VIO 和IPL-VIO 算法進(jìn)行對比。VINS-Fusion 是香港科技大學(xué)Qin 等[6]提出的一種基于雙目視覺慣性的狀態(tài)估計(jì)算法，采用緊耦合、非線性優(yōu)化的方法，融合預(yù)積分后的IMU 測量值和特征觀測值，獲得高精度的視覺里程計(jì)。而PL-VIO 在VINS-Mono 的基礎(chǔ)上加入了線特征約束。

試驗(yàn)采用的計(jì)算機(jī)配置：CPU 為i7–10750 的12 核英特爾處理器、16 GB運(yùn)行內(nèi)存、Ubuntu18.04操作系統(tǒng)。為充分測試算法處理效果，數(shù)據(jù)集回放速率為正常速率的1/2，以此減少由于顯示和加載導(dǎo)致的數(shù)據(jù)延遲問題。

VINS-Fusion 運(yùn)行測試數(shù)據(jù)集MH_05_difficult 的運(yùn)行效果雙目特征檢測與跟蹤基本一致，特征點(diǎn)分布均勻。表1 所示為VINS-Fusion 運(yùn)行數(shù)據(jù)集時的參數(shù)，可以看出，運(yùn)行數(shù)據(jù)集MH_03_medium 時位移絕對誤差最小，運(yùn)行MH_05_difficult 時旋轉(zhuǎn)絕對誤差、內(nèi)存占用率最小。

表1 VINS-Fusion 運(yùn)行數(shù)據(jù)集的參數(shù)Table 1 Parameters of the VINS-Fusion run data set

使用PL-VIO 運(yùn)行測試數(shù)據(jù)集MH_05_difficult 的運(yùn)行效果點(diǎn)線特征檢測與跟蹤效果基本一致。表2 所示為PL-VIO 運(yùn)行數(shù)據(jù)集時的參數(shù)，可以看出，運(yùn)行數(shù)據(jù)集MH_03_medium 時位移絕對誤差最小，運(yùn)行MH_05_difficult 時旋轉(zhuǎn)絕對誤差、運(yùn)行時間和CPU 占用率最小。和VINS-Fusion 相比，PL-VIO 在絕對位移和旋轉(zhuǎn)上有所提高，由于線特征的提取和計(jì)算耗費(fèi)更多的資源，使運(yùn)行時間和CPU 占用率顯著提升。

表2 PL-VIO 運(yùn)行數(shù)據(jù)集的參數(shù)Table 2 Parameters of the PL-VIO run data set

使用IPL-VIO 算法運(yùn)行測試數(shù)據(jù)集MH_05_difficult，運(yùn)行效果如圖3 所示，點(diǎn)特征提取分布均勻，跟蹤效果良好。雙目基本可以檢測出場景的線特征，且雙目相機(jī)的左右目匹配一致，跟蹤效果良好。

圖3 IPL-VIO 算法測試數(shù)據(jù)集的點(diǎn)線特征跟蹤Fig.3 Point and line feature tracking of IPL-VIO algorithm test data set

使用IPL-VIO 算法運(yùn)行測試數(shù)據(jù)集MH_05_difficult，位姿估計(jì)效果如圖4 所示，可以發(fā)現(xiàn)基于雙目的線特征檢測數(shù)量較多，且IPL-VIO 算法可以較好地反映出局部線段縱橫交匯處的結(jié)構(gòu)信息，豐富的場景信息有利于后期場景深度恢復(fù)和地圖構(gòu)建。

圖4 IPL-VIO 算法測試數(shù)據(jù)集點(diǎn)線特征位姿估計(jì)效果Fig.4 Pose estimation effect of point and line feature in test data set by IPL-VIO algorithm

表3 所示為IPL-VIO 算法運(yùn)行數(shù)據(jù)集時的參數(shù)，可以看出，運(yùn)行數(shù)據(jù)集MH_03_medium 時位移絕對誤差最小，運(yùn)行MH_05_difficult 時旋轉(zhuǎn)絕對誤差、運(yùn)行時間和CPU 占用率最小。

表3 IPL-VIO 算法運(yùn)行數(shù)據(jù)集的參數(shù)Table 3 Parameters of run data set by the IPL-VIO algorithm

VINS-Fusion、PL-VIO 的平均位移絕對誤差分別為0.19550 m 和0.16736 m，IPL-VIO 算法的平均位移絕對誤差為0.14035 m，相比于VINS-Fusion、PL-VIO，IPLVIO 算法平均位移絕對誤差最小。VINS-Fusion、PL-VIO的平均旋轉(zhuǎn)絕對誤差分別為0.02755°和0.02331°，IPLVIO 算法的平均旋轉(zhuǎn)絕對誤差為0.01949°，相比于VINSFusion、PL-VIO，IPL-VIO 算法平均旋轉(zhuǎn)絕對誤差最小。而在運(yùn)行時間、CPU 占有率上，IPL-VIO 算法相對于VINS-Fusion、PL-VIO 都有一定的增加?？傊蚔INSFusion、PL-VIO 相比，IPL-VIO 算法在絕對位移和旋轉(zhuǎn)上有明顯提高，但同時在PL-VIO 的基礎(chǔ)之上，由于雙目線特征余數(shù)的存在以及優(yōu)化變量的增多，使運(yùn)行時間和CPU 占用率顯著提升。

綜上所述，通過對比VINS-Fusion 和PL-VIO 算法試驗(yàn)，雙目定位相比單目定位精度更高，且場景恢復(fù)信息豐富；IPL-VIO 算法在結(jié)構(gòu)化場景下的絕對位移和旋轉(zhuǎn)誤差比原算法誤差更小，同時結(jié)構(gòu)化場景信息更加豐富，但耗費(fèi)計(jì)算資源較原來的算法多。

將IPL-VIO 算法應(yīng)用到座艙線束引導(dǎo)裝配定位中，試驗(yàn)采用的頭戴式設(shè)備HTC VIVE Pro 為一種虛擬現(xiàn)實(shí)頭戴式設(shè)備，可以通過基站進(jìn)行定位，也可用連接的攝像頭觀察外部真實(shí)世界實(shí)現(xiàn)增強(qiáng)現(xiàn)實(shí)的效果。試驗(yàn)采取遠(yuǎn)程協(xié)同的方式，通過專家端和工人端來實(shí)現(xiàn)復(fù)雜產(chǎn)品裝配。使用本系統(tǒng)裝配試驗(yàn)的若干關(guān)鍵環(huán)節(jié)包括：（1）裝配工人啟動裝配系統(tǒng)，裝配系統(tǒng)通過相機(jī)與HTC坐標(biāo)原點(diǎn)對齊等完成初始化任務(wù)； （2）裝配工人通過裝配中心查看裝配任務(wù)，以座艙下某管線敷設(shè)為例，系統(tǒng)提示工人佩戴掛載ZED Mini 相機(jī)的頭盔并在裝配現(xiàn)場通過AR 場景演示裝配零件狀態(tài)信息，便于學(xué)習(xí)； （3）當(dāng)在零件區(qū)拿取零件之后，AR 相機(jī)渲染裝配模型信息于座艙上，信息中心提示裝配人員進(jìn)行裝配操作； （4）完成后信息中心提示進(jìn)行安裝檢查操作，結(jié)束后信息中心下達(dá)結(jié)束該工序命令。

試驗(yàn)以線束敷設(shè)安裝為例，為便于展示虛實(shí)融合效果，本文以第一和第三視角進(jìn)行混合敘述。開始后，試驗(yàn)人員根據(jù)系統(tǒng)提示佩戴AR 輔助終端，在裝配現(xiàn)場通過AR 效果學(xué)習(xí)線束系統(tǒng)布局的大體空間位置信息。開始敷線操作時，試驗(yàn)人員拿取束線管之后，前往座艙裝配區(qū)，此時系統(tǒng)將束線管和座艙模型融合，試驗(yàn)人員在現(xiàn)場觀察虛實(shí)融合情況，圖5 所示為試驗(yàn)人員第三視角和信息管理中心的第一視角下的虛擬束線管與座艙融合情況，可以看出，系統(tǒng)對試驗(yàn)人員頭部的虛實(shí)遮擋情況處理得比較好。然后試驗(yàn)人員根據(jù)系統(tǒng)提示，按照束線管的走向和布局進(jìn)行束線操作。

圖5 座艙線束引導(dǎo)裝配定位效果圖Fig.5 Guiding assembly and positioning rendering of cabin wiring harness

裝配人員可以隨意走動觀察零件空間位置信息，線束模型定位穩(wěn)定，與真實(shí)場景融合良好，且裝配人員在走動時，AR 頭戴式設(shè)備以及與其綁定的相機(jī)也隨之運(yùn)動的情況下，虛擬線束模型在真實(shí)的座艙中虛實(shí)注冊穩(wěn)定，沒有出現(xiàn)模型劇烈移動現(xiàn)象。

5 結(jié)論

提出了一種點(diǎn)線特征融合的雙目視覺慣性定位算法，并對該方法進(jìn)行了全面的研究，試驗(yàn)結(jié)果表明，本文提出的IPL-VIO 算法誤差更小，定位精度較高，能夠應(yīng)用在弱紋理的AR 裝配現(xiàn)場中，為增強(qiáng)現(xiàn)實(shí)輔助裝配平臺提供穩(wěn)定可靠的位姿數(shù)據(jù)。近年來隨著AR 技術(shù)的發(fā)展，特別是在大場景弱紋理的AR 裝配現(xiàn)場，本方法有著較大的應(yīng)用前景。下一步將考慮采取結(jié)構(gòu)化的直線代替裝配場景中雜亂無章的線段，這將在線段的提取、匹配上節(jié)省一定的時間和算力，從而減少定位的時間并提高定位的精度，更好地實(shí)現(xiàn)增強(qiáng)現(xiàn)實(shí)設(shè)備的實(shí)時定位。