特征稀疏場景下基于標(biāo)簽的車輛視覺SLAM *

秦洪懋，沈國利，周云水，黃圣杰，秦曉輝，謝國濤，丁榮軍

（1. 湖南大學(xué)機(jī)械與運(yùn)載工程學(xué)院，長沙 410082；2. 湖南大學(xué)無錫智能控制研究院，無錫 214072）

前言

智能駕駛相關(guān)技術(shù)中，同時(shí)定位與建圖（simultaneous localization and mapping， SLAM）指車輛在未知環(huán)境中運(yùn)動(dòng)時(shí)，借助特定傳感器獲取環(huán)境信息、建立地圖模型并估計(jì)自身運(yùn)動(dòng)的過程。在現(xiàn)有的SLAM 技術(shù)中，視覺特征點(diǎn)法通過提取圖像特征點(diǎn)、計(jì)算描述子與特征匹配等過程估計(jì)相機(jī)的位姿，典 型 的 特 征 點(diǎn) 法SLAM 如PTAM［1］、ORBSLAM［2-4］等。

自然特征穩(wěn)定性差，易受外界因素干擾，在定位過程中，若場景的紋理特征稀疏，定位系統(tǒng)難以提取足夠特征點(diǎn)，使得幀間數(shù)據(jù)不能有效關(guān)聯(lián)，導(dǎo)致系統(tǒng)因約束不足而無法精確估計(jì)位姿，嚴(yán)重限制了算法在實(shí)際場景的應(yīng)用。針對場景特征稀疏問題，已有諸多學(xué)者進(jìn)行研究，其解決方法根據(jù)原理可分為：基于圖像增強(qiáng)的方法、挖掘圖像額外特征的方法與引入人工標(biāo)簽的方法。

在基于圖像增強(qiáng)的方法中，Banik 等［5］使用直方圖均衡法調(diào)整圖像灰度值，增強(qiáng)特征稀疏圖像的對比度，提高昏暗環(huán)境下紋理的清晰度，改善圖像質(zhì)量以保證特征的穩(wěn)定識(shí)別；Wang 等［6］提出一種多層圖像表示法來解決環(huán)境特征稀疏問題，基于信息論計(jì)算對比度增強(qiáng)的最佳參數(shù)，通過不同程度的對比度增強(qiáng)進(jìn)行圖像優(yōu)化，改進(jìn)特征點(diǎn)的檢測與匹配。但是基于圖像增強(qiáng)的方法自適應(yīng)性差，依賴人工調(diào)整參數(shù)，圖像增強(qiáng)也會(huì)導(dǎo)致噪點(diǎn)增加，反而影響特征的匹配，無法完美地解決場景特征稀疏問題。

在挖掘圖像額外特征的方法中，Pumarola 等［7］提出了一種基于點(diǎn)線特征的PL-SLAM，針對低紋理的特征稀疏環(huán)境，將場景結(jié)構(gòu)化邊緣作為線特征并借助端點(diǎn)參數(shù)化，用于幀間的特征匹配與位姿估計(jì)；He 等［8］提出基于點(diǎn)和線特征的視覺慣性里程計(jì)PLVIO，借助圖像點(diǎn)線特征的提取和匹配進(jìn)行位姿計(jì)算，充分利用環(huán)境的幾何結(jié)構(gòu)信息，并通過與慣性測量結(jié)果的緊耦合獲得最優(yōu)的位姿估計(jì)；Sun等［9］通過融合線面特征估計(jì)相機(jī)位姿，在特征稀疏場景，利用邊緣和平面提取線面特征，構(gòu)建運(yùn)動(dòng)估計(jì)約束，并根據(jù)面特征的置信度調(diào)整相關(guān)線特征的權(quán)重。通過挖掘圖像紋理外的線面特征，可解決場景特征稀疏時(shí)的位姿估計(jì)問題，但是線面特征易被遮擋，優(yōu)化效果不夠穩(wěn)定，且處理特征時(shí)的計(jì)算量巨大，嚴(yán)重增加系統(tǒng)的資源占用。

在引入人工標(biāo)簽的方法中，Chen 等［10］在室內(nèi)場景布置標(biāo)簽，通過標(biāo)定獲得標(biāo)簽的全局位姿，并建立標(biāo)簽地圖，在定位時(shí)使用慣性測量數(shù)據(jù)初步估計(jì)車輛位姿，同時(shí)進(jìn)行標(biāo)簽檢測，基于標(biāo)簽位姿優(yōu)化定位結(jié)果；Chakraborty 等［11］在走廊、隧道等具有相似特征的場景中，部署視覺標(biāo)簽并預(yù)先標(biāo)定，借助標(biāo)簽區(qū)分相似環(huán)境，在檢測到重復(fù)標(biāo)簽時(shí)進(jìn)行回環(huán)檢測，并借助標(biāo)簽絕對位姿優(yōu)化定位結(jié)果；Pfrommer 等［12］提出了一個(gè)用于構(gòu)建標(biāo)簽地圖的TagSLAM 系統(tǒng)，在場景中部署標(biāo)簽，使用相機(jī)同時(shí)觀測多個(gè)標(biāo)簽，計(jì)算同一幀圖像中各標(biāo)簽的相對位姿，并在運(yùn)動(dòng)過程中持續(xù)更新優(yōu)化各標(biāo)簽的位置關(guān)系，最終構(gòu)建出所有標(biāo)簽的相對位姿地圖；Roos-Hoefgeest等［13］通過在車輛上布置8 個(gè)相機(jī)構(gòu)建相機(jī)位姿約束，檢測在同一時(shí)刻車輛周圍的所有標(biāo)簽，并獲取相對位姿估計(jì)，在車輛運(yùn)動(dòng)時(shí)通過相對位姿變換更新標(biāo)簽地圖，然后基于該地圖對車輛進(jìn)行定位。

相比于圖像增強(qiáng)和挖掘額外特征，引入人工標(biāo)簽受到環(huán)境的限制少，穩(wěn)定性更高，更適用于解決特征稀疏問題。但當(dāng)前基于人工標(biāo)簽的方法仍存在以下不足。（1）標(biāo)簽標(biāo)定成本過高：標(biāo)簽常常以較短間距無規(guī)律地分布在場景中，初次布置與后續(xù)維護(hù)都須對標(biāo)簽位姿進(jìn)行人工標(biāo)定，工作量巨大。（2）對標(biāo)簽形狀變化敏感：在運(yùn)動(dòng)中相機(jī)難以時(shí)刻觀測到清晰的標(biāo)簽，相機(jī)的偏轉(zhuǎn)常常導(dǎo)致標(biāo)簽發(fā)生變形，當(dāng)前通過頂點(diǎn)像素坐標(biāo)獲取標(biāo)簽位姿估計(jì)的方法不夠穩(wěn)定，易出現(xiàn)位姿抖動(dòng)。（3）標(biāo)簽信息利用率低：當(dāng)前基于標(biāo)簽的定位方法對標(biāo)簽內(nèi)部角點(diǎn)的利用率不足，位姿估計(jì)時(shí)引入的約束條件較少，難以獲得穩(wěn)定的標(biāo)簽位姿，無法保證地圖的一致性，影響系統(tǒng)的精度和穩(wěn)定性。

為解決上述問題，本文提出一種融合標(biāo)簽的視覺SLAM 系統(tǒng)，其主要?jiǎng)?chuàng)新點(diǎn)包括：（1）充分利用標(biāo)簽信息，通過提取標(biāo)簽內(nèi)部角點(diǎn)引入額外約束獲得更穩(wěn)定、更精準(zhǔn)的標(biāo)簽位姿估計(jì)，降低當(dāng)標(biāo)簽存在變形時(shí)的位姿抖動(dòng)；（2）基于內(nèi)外角點(diǎn)約束、標(biāo)簽位姿優(yōu)化模型和視覺里程計(jì)構(gòu)建低漂移、全局一致的地圖，提高標(biāo)簽地圖的精度并保證建圖過程的穩(wěn)定性；（3）設(shè)計(jì)融合標(biāo)簽檢測的視覺SLAM 算法框架，基于標(biāo)簽地圖估計(jì)車輛位姿，并對標(biāo)簽地圖和車輛位姿進(jìn)行聯(lián)合優(yōu)化，顯著提升建圖精度與系統(tǒng)魯棒性。

1 系統(tǒng)概述

本文提出的基于標(biāo)簽的視覺SLAM 方法，以AprilTag［15］作為視覺標(biāo)簽，借鑒ORB-SLAM3 設(shè)計(jì)基于標(biāo)簽定位的系統(tǒng)框架，如圖1 所示。本方法集成為系統(tǒng)的標(biāo)簽定位線程，主要包括檢測標(biāo)簽、內(nèi)外角點(diǎn)提取、估計(jì)標(biāo)簽位姿、構(gòu)建標(biāo)簽位姿地圖和恢復(fù)相機(jī)位姿5 個(gè)模塊，并在后續(xù)線程中優(yōu)化標(biāo)簽與相機(jī)的位姿。

圖1 系統(tǒng)架構(gòu)

系統(tǒng)獲取圖像并完成特征提取與匹配后，進(jìn)入標(biāo)簽處理線程，檢測圖像中是否存在標(biāo)簽。若存在，則基于內(nèi)外角點(diǎn)約束，篩選出最優(yōu)的標(biāo)簽并進(jìn)行內(nèi)外角點(diǎn)提取，估計(jì)標(biāo)簽相對于相機(jī)的位姿。若系統(tǒng)中未發(fā)現(xiàn)標(biāo)簽地圖文件，則基于特征點(diǎn)對相機(jī)位姿進(jìn)行初步估計(jì)，計(jì)算標(biāo)簽的全局位姿，進(jìn)而建立標(biāo)簽位姿地圖，然后返回ORB 跟蹤線程，對相機(jī)位姿和標(biāo)簽地圖進(jìn)行優(yōu)化；若系統(tǒng)中存在標(biāo)簽地圖文件，則使用標(biāo)簽位姿地圖獲取相機(jī)位姿的初步估計(jì)，結(jié)合ORB中的地圖點(diǎn)進(jìn)行優(yōu)化獲得精確的定位結(jié)果。

2 內(nèi)外角點(diǎn)約束

AprilTag 的開發(fā)者提供了檢測標(biāo)簽和估計(jì)位姿的算法［16］，流程如圖2 所示。將輸入的標(biāo)簽圖像二值化后，通過邊緣檢測獲得圖像中所有線段；然后搜索閉合的凸多邊形，并篩選出四邊形；獲取各四邊形內(nèi)的像素排列，解碼得到標(biāo)簽的編號(hào)；最后計(jì)算4 個(gè)頂點(diǎn)的像素坐標(biāo)，并通過直接線性變換估計(jì)標(biāo)簽位姿。

圖2 AprilTag檢測與位姿估計(jì)流程

但是該方法對標(biāo)簽的利用率有限，僅依賴4 個(gè)頂點(diǎn)計(jì)算位姿，穩(wěn)定性不足，常出現(xiàn)位姿跳動(dòng)。因此，本文設(shè)計(jì)了標(biāo)簽內(nèi)外角點(diǎn)約束策略，使用標(biāo)簽外邊緣頂點(diǎn)和內(nèi)部角點(diǎn)共同進(jìn)行位姿估計(jì)，提高標(biāo)簽信息的利用率，通過增加約束降低位姿抖動(dòng)，保證地圖的全局一致性。

在進(jìn)行標(biāo)簽檢測時(shí)，若圖像中存在多個(gè)標(biāo)簽，需要根據(jù)標(biāo)簽的大小、變形程度等標(biāo)準(zhǔn)選擇距離最近、變形最小的標(biāo)簽用于位姿估計(jì)。本文方法制定的標(biāo)簽評價(jià)策略為

式中：Simg為圖像像素面積；Stag為標(biāo)簽的像素面積，由對角線乘積近似；k1～k4為標(biāo)簽4 條邊的斜率。選擇W最小的標(biāo)簽，用于后續(xù)的位姿估計(jì)。

在標(biāo)簽內(nèi)部提取的若干角點(diǎn)中，通過迭代匹配篩選出最優(yōu)的4 個(gè)角點(diǎn)用于位姿估計(jì)，詳細(xì)步驟如圖3所示。篩選獲得最優(yōu)標(biāo)簽后，如圖3 （a）所示，使用頂點(diǎn)坐標(biāo)劃分角點(diǎn)提取范圍，以減少外部噪點(diǎn)的影響。通過Shi-Tomasi 算法［18］提取標(biāo)簽的內(nèi)部角點(diǎn)，并借助亞像素細(xì)化算法提高角點(diǎn)坐標(biāo)的精度。

圖3 內(nèi)部角點(diǎn)的提取與匹配

計(jì)算角點(diǎn)對應(yīng)的三維空間點(diǎn)時(shí)，通過射影變換獲得規(guī)則化標(biāo)簽，如圖3 （b）所示。若標(biāo)簽?zāi)骋豁旤c(diǎn)像素坐標(biāo)為[x1，x2]T，射影變換后的像素坐標(biāo)為[x′1，x′2]T，則該過程可表示為

式中Hp為3×3 的射影變換矩陣。通過4 個(gè)頂點(diǎn)求解出Hp后，對內(nèi)部角點(diǎn)進(jìn)行射影變換，然后進(jìn)行二位像素點(diǎn)到三維空間點(diǎn)的匹配。

AprilTag 由8×8 的方格組成，如圖3 （e）所示，建立與像素坐標(biāo)系對應(yīng)的方格坐標(biāo)系，角點(diǎn)的像素坐標(biāo)、方格坐標(biāo)與圖3 （f） 中的標(biāo)簽平面坐標(biāo)存在對應(yīng)關(guān)系。但標(biāo)簽邊緣一般與像素坐標(biāo)軸不平行，如圖4 所示，沿標(biāo)簽的邊緣和邊緣法線構(gòu)建新的坐標(biāo)系，用于方格坐標(biāo)計(jì)算。

圖4 標(biāo)簽邊緣坐標(biāo)系

圖4 中角點(diǎn)的像素坐標(biāo)為 [u，v]，P1、P2為標(biāo)簽的2個(gè)頂點(diǎn)，沿P1P2及其法線方向建立坐標(biāo)系，角點(diǎn)在該坐標(biāo)系下的坐標(biāo)[l1，l2]和單位方格邊長Δl為

將坐標(biāo)[l1，l2]除以單位方格邊長Δl取整得到該點(diǎn)的方格坐標(biāo)，其對應(yīng)的三維空間點(diǎn)坐標(biāo)為方格坐標(biāo)乘以實(shí)際方格邊長。

由于噪聲點(diǎn)和射影變換的影響，可能存在角點(diǎn)不是方格頂點(diǎn)或方格坐標(biāo)錯(cuò)誤的情況，影響標(biāo)簽的位姿估計(jì)。因此借鑒RANSAC 算法，以4 個(gè)角點(diǎn)一組，通過選取不同的角點(diǎn)進(jìn)行迭代，求解最優(yōu)的單應(yīng)矩陣。已知標(biāo)簽所在二維平面內(nèi)一點(diǎn)的坐標(biāo)P=[x，y]T，和對應(yīng)的相機(jī)投影平面的投影點(diǎn)的坐標(biāo)p=[u，v]T，由投影關(guān)系得：

式中：s為尺度；H矩陣為8個(gè)自由度的3×3矩陣。使用4對點(diǎn)構(gòu)建8個(gè)方程對H矩陣進(jìn)行求解，計(jì)算角點(diǎn)Pi的投影誤差：

若誤差小于1 像素則將該點(diǎn)標(biāo)記為內(nèi)點(diǎn)，然后對所有內(nèi)點(diǎn)重復(fù)此過程，并更新迭代次數(shù)k：

式中：p為置信度，取0.995；w為內(nèi)點(diǎn)所占的比例；m為最少所需點(diǎn)對，取4。

迭代完成后，得到最優(yōu)的H矩陣，在最終的內(nèi)點(diǎn)集合中選擇投影誤差最小的4 個(gè)內(nèi)部角點(diǎn)作為新約束，與外部4個(gè)頂點(diǎn)共同用于標(biāo)簽位姿的估計(jì)。

3 標(biāo)簽位姿地圖

3.1 標(biāo)簽位姿優(yōu)化模型

基于內(nèi)外角點(diǎn)約束策略獲得當(dāng)前場景中最優(yōu)標(biāo)簽的4個(gè)頂點(diǎn)與4個(gè)內(nèi)點(diǎn)，進(jìn)而構(gòu)建標(biāo)簽位姿優(yōu)化模型，通過最小化重投影誤差迭代求解。若已知某三維空間點(diǎn)坐標(biāo)P=[X，Y，Z]T及其投影的像素坐標(biāo)p=[u，v]T，重投影誤差為e為

式中：K為相機(jī)內(nèi)參矩陣；ξ為位姿變換矩陣T對應(yīng)的李代數(shù)；ξ∧為ξ的反對稱矩陣。假設(shè)誤差函數(shù)為f(x)，進(jìn)行1階泰勒展開得到增量方程：

式中JT為雅可比矩陣，位姿增量為Δξ，不斷迭代并更新T= exp(Δξ∧)T，直到Δξ足夠小，此時(shí)便得到最優(yōu)的T，即標(biāo)簽相對于相機(jī)的位姿。

如圖5所示，在相機(jī)和標(biāo)簽外的一點(diǎn)P，與相機(jī)、標(biāo)簽位于同一空間坐標(biāo)系內(nèi)，定義該點(diǎn)在世界坐標(biāo)系下的坐標(biāo)為Pw，在相機(jī)坐標(biāo)系內(nèi)的坐標(biāo)為Pc，在標(biāo)簽坐標(biāo)系下的坐標(biāo)為Pt。則Pw與Pc、Pc與Pt之間的位姿變換關(guān)系為

圖5 坐標(biāo)轉(zhuǎn)換關(guān)系

式中：Tcw為相機(jī)坐標(biāo)系到世界坐標(biāo)系的位姿轉(zhuǎn)換矩陣；Ttc為標(biāo)簽坐標(biāo)系到相機(jī)坐標(biāo)系的位姿轉(zhuǎn)換矩陣。所以Pw與Pt之間的關(guān)系為

因此標(biāo)簽坐標(biāo)系到世界坐標(biāo)系的位姿轉(zhuǎn)換矩陣Ttw表示為

其中，相機(jī)的全局位姿Tcw由ORB 提供初步估計(jì)，標(biāo)簽相對于相機(jī)的位姿Ttc通過內(nèi)外角點(diǎn)估計(jì)，最終由式 （14） 計(jì)算得到當(dāng)前標(biāo)簽的全局位姿Ttw，作為標(biāo)簽地圖的初值。

3.2 地圖構(gòu)建與優(yōu)化策略

在估計(jì)標(biāo)簽位姿時(shí)，相機(jī)會(huì)多次觀測到同一個(gè)標(biāo)簽，所以通過n次觀測可估計(jì)出n個(gè)位姿，由于存在誤差，這些位姿在真實(shí)值附近浮動(dòng)。為提高標(biāo)簽位姿地圖的精度，當(dāng)n足夠大時(shí)，標(biāo)簽位姿估計(jì)數(shù)據(jù)滿足正態(tài)分布，通過3σ原則剔除誤差過大的數(shù)據(jù)，然后計(jì)算剩余位姿的均值作為標(biāo)簽的全局位姿。

標(biāo)簽的位姿包括旋轉(zhuǎn)與平移：

式中：R為3 × 3 的旋轉(zhuǎn)矩陣；t為三維平移向量。平移的均值μt和方差σ2t可直接計(jì)算，旋轉(zhuǎn)則將矩陣轉(zhuǎn)換為歐拉角，計(jì)算歐拉角的均值μθ和方差σ2θ。剔除[μ- 3σ，μ+ 3σ]外的數(shù)據(jù)，計(jì)算剩余位姿矩陣的平移向量均值tˉ和旋轉(zhuǎn)矩陣的四元數(shù)中間插值，若剩余m個(gè)位姿，平移向量均值為，旋轉(zhuǎn)矩陣R對應(yīng)的四元數(shù)為Q=[q0，q1，q2，q3]T，進(jìn)行插值：

將該中間插值轉(zhuǎn)換為旋轉(zhuǎn)矩陣，則該標(biāo)簽最終的位姿矩陣為

若n不夠大，例如n＜10，則3σ原則失效，直接對位姿排序，選擇中間值作為標(biāo)簽最終位姿。最后，以標(biāo)簽的編號(hào)為索引，存儲(chǔ)所有標(biāo)簽優(yōu)化后的全局位姿，得到標(biāo)簽位姿地圖。

4 基于標(biāo)簽的定位方法

4.1 相機(jī)位姿的初步估計(jì)

完成標(biāo)簽地圖構(gòu)建后，借助該地圖對相機(jī)進(jìn)行定位。當(dāng)圖像中檢測到標(biāo)簽后，系統(tǒng)加載地圖文件獲取標(biāo)簽位姿，并由該位姿獲取相機(jī)位姿的初步估計(jì)。

基于內(nèi)外角點(diǎn)約束策略和標(biāo)簽位姿優(yōu)化模型估計(jì)標(biāo)簽相對于相機(jī)的位姿Ttc；通過讀取標(biāo)簽地圖文件，以編號(hào)為索引查找得到標(biāo)簽在世界坐標(biāo)系下的全局位姿Ttw，則相機(jī)的全局位姿表示為

獲取相機(jī)位姿的初步估計(jì)后，為了提高定位結(jié)果的精度和穩(wěn)定性，借助地圖點(diǎn)進(jìn)行優(yōu)化。

4.2 相機(jī)位姿的精確估計(jì)與優(yōu)化

4.2.1 位姿優(yōu)化方案

完成對相機(jī)全局位姿的初步估計(jì)后，加入ORB-SLAM3 中的地圖點(diǎn)構(gòu)建g2o 問題來優(yōu)化相機(jī)的位姿，以獲得精確的定位結(jié)果。如圖6 所示，以地圖點(diǎn)位姿Pm、相機(jī)位姿Twc、標(biāo)簽位姿Ttw作為圖的頂點(diǎn)，優(yōu)化相機(jī)位姿與標(biāo)簽地圖。

圖6 位姿圖優(yōu)化

設(shè)u為地圖點(diǎn)觀測的二維坐標(biāo)，uˉ為地圖點(diǎn)的重投影位置的二維坐標(biāo)，則重投影誤差為

構(gòu)建誤差函數(shù)：

對相機(jī)的位姿Twc和標(biāo)簽位姿Ttw進(jìn)行優(yōu)化，并通過卡方檢驗(yàn)剔除地圖外點(diǎn)。

4.2.2 卡方檢驗(yàn)

每次優(yōu)化完成后，均通過卡方檢驗(yàn)篩選出地圖點(diǎn)中的外點(diǎn)并剔除。計(jì)算各個(gè)地圖點(diǎn)的重投影誤差e，該誤差服從高斯分布e～N(0，Σ)，在ORB-SLAM3中圖像金字塔的縮放比為1.2，協(xié)方差Σ由圖像金字塔的層級(jí)n決定：

使用協(xié)方差對重投影誤差進(jìn)行加權(quán)，計(jì)算向量的內(nèi)積作為閾值：

r服從自由度為2的卡方分布，在0.05的顯著性水平（95%的準(zhǔn)確率）下，卡方檢驗(yàn)的拒絕域?yàn)?/p>

位于拒絕域內(nèi)的點(diǎn)被判定為外點(diǎn)，不再參與后續(xù)對相機(jī)位姿的優(yōu)化，經(jīng)過四次優(yōu)化后最終獲得精確的相機(jī)位姿。

5 試驗(yàn)測試

本文通過實(shí)車試驗(yàn)對系統(tǒng)的綜合性能進(jìn)行測試，為驗(yàn)證不同場景中算法的通用性，分別在室內(nèi)回型走廊與地下停車場進(jìn)行試驗(yàn)。視覺標(biāo)簽選用AprilTag 的tag36h11 序列，標(biāo)簽的尺寸為0.16 m×0.16 m，使用realsense-D455 相機(jī)，系統(tǒng)在配備intel i7-12700H CPU、2.3Hz8 核、16GB RAM、Linux 系統(tǒng)的計(jì)算機(jī)上運(yùn)行。

5.1 內(nèi)外角點(diǎn)約束效果與視角魯棒性測試

在室內(nèi)回型走廊環(huán)境進(jìn)行試驗(yàn)，走廊墻壁和地板的表面缺乏紋理，如圖7 （a）所示，屬于特征稀疏場景；使用的試驗(yàn)小車如圖7 （b）所示，通過手動(dòng)控制。在該場景中，通過比較有無內(nèi)外角點(diǎn)約束時(shí)的運(yùn)行軌跡和建圖精度，驗(yàn)證該策略的有效性；改變相機(jī)觀測標(biāo)簽的視角，測試當(dāng)標(biāo)簽發(fā)生變形時(shí)系統(tǒng)的定位效果。

圖7 試驗(yàn)小車與測試環(huán)境

5.1.1 基于內(nèi)外角點(diǎn)約束策略的定位效果測試

如圖8 （a），相機(jī)安裝在小車側(cè)面，走廊內(nèi)標(biāo)簽的布置如圖8 （b）所示，分別在有無內(nèi)外角點(diǎn)約束時(shí)進(jìn)行建圖與定位。運(yùn)動(dòng)軌跡如圖9 所示，未加入內(nèi)外角點(diǎn)約束的方法存在位姿抖動(dòng)；圖10 為二者在x、y、z3 軸的位移和橫滾角（roll）、俯仰角（pitch）、偏航角（yaw）上的對比，位移最大跳動(dòng)約0.2 m，角度最大跳動(dòng)約10°；本文方法得到的結(jié)果在位移與角度上均過渡平滑，通過引入內(nèi)外角點(diǎn)約束，顯著提高了特征稀疏時(shí)系統(tǒng)的魯棒性。

圖8 相機(jī)位置與標(biāo)簽場景

圖9 無內(nèi)外角點(diǎn)約束方法與本文方法的軌跡對比

圖10 無內(nèi)外角點(diǎn)約束方法與本文方法的軌跡在位移與角度上的對比

標(biāo)簽的位置示意圖如圖11 所示，完成標(biāo)簽位姿地圖的構(gòu)建后，計(jì)算編號(hào)0～5、6～15、16～21、22～31標(biāo)簽的相對距離，記錄5 次試驗(yàn)結(jié)果，并計(jì)算最大相對誤差。結(jié)果如表1 所示，使用本文方法得到的相對誤差范圍保持在0.05 m 以內(nèi)，相比于無內(nèi)外角點(diǎn)約束策略的方法誤差縮小了60%以上，顯著提高了系統(tǒng)對特征稀疏場景適應(yīng)性，有效地保證了地圖的一致性。

表1 標(biāo)簽間距相對誤差

圖11 標(biāo)簽位置示意圖

5.1.2 視角魯棒性測試

相機(jī)安裝位置如圖12 （a）所示，位于小車前端，走廊地面的標(biāo)簽布置如圖12 （b）所示，從相機(jī)視角觀察標(biāo)簽時(shí)，標(biāo)簽存在變形，測試當(dāng)前場景下系統(tǒng)建圖與定位的效果，驗(yàn)證本文所提方法對標(biāo)簽存在變形時(shí)的魯棒性。系統(tǒng)的運(yùn)行效果如圖13 所示，相機(jī)視角偏轉(zhuǎn)導(dǎo)致標(biāo)簽變形時(shí)，本文方法依舊能正常進(jìn)行標(biāo)簽檢測。

圖12 相機(jī)位置與標(biāo)簽場景

圖13 系統(tǒng)運(yùn)行效果

改變相機(jī)視角后，未加入內(nèi)外角點(diǎn)約束的方法與本文方法的相機(jī)運(yùn)動(dòng)軌跡對比結(jié)果如圖14 所示，二者均得到了平滑的軌跡，本文方法軌跡的偏移量更小。試驗(yàn)結(jié)果說明，本文提出的方案有效降低了標(biāo)簽存在變形時(shí)的位姿抖動(dòng)，對相機(jī)視角和標(biāo)簽位置的要求更加寬松，提高了定位系統(tǒng)的魯棒性。

圖14 相機(jī)視角改變后軌跡對比

5.2 基于標(biāo)簽的視覺SLAM 算法性能測試

如圖15 （a）所示，將本文算法部署至智能車輛平臺(tái)，在室內(nèi)停車場中，通過人工駕駛車輛驗(yàn)證算法在實(shí)際特征稀疏場景下的應(yīng)用效果。相機(jī)安裝于車輛右側(cè)靠前位置，標(biāo)簽布置如圖15 （b）所示，在該場景下進(jìn)行建圖與定位，通過對比幀間特征關(guān)聯(lián)成功率、重復(fù)定位誤差、各模塊耗時(shí)與內(nèi)存占用程度，綜合評估ORB-SLAM3與本文算法在各維度上的優(yōu)劣。

圖15 試驗(yàn)車輛與室內(nèi)停車場

5.2.1 幀間特征關(guān)聯(lián)成功率對比

成功關(guān)聯(lián)幀間特征是構(gòu)建優(yōu)化函數(shù)、解算車輛位姿的前提條件，其成功率能從側(cè)面反映算法提取場景特征、構(gòu)建有效約束的能力。在室內(nèi)停車場進(jìn)行5 次試驗(yàn)，并計(jì)算成功關(guān)聯(lián)的特征點(diǎn)比例，如表2所示，相比于ORB-SLAM3，本文所提方法的特征關(guān)聯(lián)成功率提高了10%以上，在特征稀疏場景下依舊能夠建立有效約束。

表2 特征點(diǎn)關(guān)聯(lián)成功率 %

5.2.2 定位精度測試

室內(nèi)停車場環(huán)境中，駕駛車輛繞行一周，運(yùn)動(dòng)路線總長約60 m，圖16 為ORB-SLAM3 與本文方法生成的相機(jī)運(yùn)動(dòng)軌跡對比，二者均獲得了平滑的軌跡，位移與角度對比結(jié)果如圖17 所示，由本文所提方法生成的軌跡在平移與角度上的漂移略小。

圖16 室內(nèi)停車場運(yùn)行軌跡對比

圖17 ORB-SLAM3與本文方法的軌跡在位移與角度上的對比

記錄車輛在室內(nèi)停車場中運(yùn)動(dòng)的起點(diǎn)位置和終點(diǎn)位置，在ORB-SLAM3 與本文所提方法完成定位后，計(jì)算基于視覺里程計(jì)和基于地圖兩種方式的重復(fù)定位誤差，共進(jìn)行5 組試驗(yàn)，結(jié)果如表3 所示。本文所提方法的定位精度與ORB-SLAM3 相比平均提升了30%以上。本文提出的融合標(biāo)簽的視覺SLAM算法顯著提高了車輛在稀疏場景下的定位精度，同時(shí)保證了地圖的一致性。

表3 重復(fù)定位誤差對比

5.2.3 效率與實(shí)時(shí)性測試

系統(tǒng)運(yùn)行時(shí)，ORB-SLAM3 占用內(nèi)存0.9 GB，約6.2%，本文所提方法占用內(nèi)存1.0 GB，約6.6%，二者的效率相差不大。記錄10 次試驗(yàn)中定位算法運(yùn)行的時(shí)間，由于本文所提方法的特征提取模塊加入了標(biāo)簽檢測，使耗時(shí)略微增加，計(jì)算平均耗時(shí)如表4所示，ORB-SLAM3 與本文所提方法的平均總耗時(shí)分別為37.969 和63.057 ms，本文所提出的算法可達(dá)到15 幀/s 的處理速度，滿足車輛在特征稀疏場景中的定位要求，后續(xù)將針對標(biāo)簽檢測的效率進(jìn)行優(yōu)化。

表4 算法各模塊平均耗時(shí) ms

6 結(jié)論

針對車輛在特征稀疏場景下定位時(shí)，特征數(shù)量不足和穩(wěn)定性差等問題，本文提出了一種基于標(biāo)簽的視覺SLAM 方法，借助內(nèi)外角點(diǎn)約束策略和標(biāo)簽位姿優(yōu)化模型，融合標(biāo)簽與視覺里程計(jì)構(gòu)建全局一致的地圖，從而基于該地圖與視覺特征點(diǎn)進(jìn)行定位，并聯(lián)合優(yōu)化標(biāo)簽地圖與車輛位姿。本文提出的構(gòu)建標(biāo)簽位姿地圖的方法兼顧一致性與穩(wěn)定性，通過與ORB-SLAM3 的算法融合，并搭建試驗(yàn)平臺(tái)測試系統(tǒng)的定位性能，結(jié)果表明，相比于ORB-SLAM3 和傳統(tǒng)標(biāo)簽定位方案，本文提出的方法顯著降低了相機(jī)的位姿漂移，提高了基于標(biāo)簽定位方法的精度與魯棒性，有利于保障智能車輛的行駛安全。

后續(xù)工作將優(yōu)化標(biāo)簽檢測的效率，進(jìn)一步提高算法的實(shí)時(shí)性。