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

（1. 湖南大學機械與運載工程學院，長沙 410082；2. 湖南大學無錫智能控制研究院，無錫 214072）

前言

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

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

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

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

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

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

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

1 系統(tǒng)概述

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

圖1 系統(tǒng)架構

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

2 內外角點約束

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

圖2 AprilTag檢測與位姿估計流程

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

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

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

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

圖3 內部角點的提取與匹配

計算角點對應的三維空間點時，通過射影變換獲得規(guī)則化標簽，如圖3 （b）所示。若標簽某一頂點像素坐標為[x1，x2]T，射影變換后的像素坐標為[x′1，x′2]T，則該過程可表示為

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

AprilTag 由8×8 的方格組成，如圖3 （e）所示，建立與像素坐標系對應的方格坐標系，角點的像素坐標、方格坐標與圖3 （f） 中的標簽平面坐標存在對應關系。但標簽邊緣一般與像素坐標軸不平行，如圖4 所示，沿標簽的邊緣和邊緣法線構建新的坐標系，用于方格坐標計算。

圖4 標簽邊緣坐標系

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

將坐標[l1，l2]除以單位方格邊長Δl取整得到該點的方格坐標，其對應的三維空間點坐標為方格坐標乘以實際方格邊長。

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

式中：s為尺度；H矩陣為8個自由度的3×3矩陣。使用4對點構建8個方程對H矩陣進行求解，計算角點Pi的投影誤差：

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

式中：p為置信度，取0.995；w為內點所占的比例；m為最少所需點對，取4。

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

3 標簽位姿地圖

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

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

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

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

如圖5所示，在相機和標簽外的一點P，與相機、標簽位于同一空間坐標系內，定義該點在世界坐標系下的坐標為Pw，在相機坐標系內的坐標為Pc，在標簽坐標系下的坐標為Pt。則Pw與Pc、Pc與Pt之間的位姿變換關系為

圖5 坐標轉換關系

式中：Tcw為相機坐標系到世界坐標系的位姿轉換矩陣；Ttc為標簽坐標系到相機坐標系的位姿轉換矩陣。所以Pw與Pt之間的關系為

因此標簽坐標系到世界坐標系的位姿轉換矩陣Ttw表示為

其中，相機的全局位姿Tcw由ORB 提供初步估計，標簽相對于相機的位姿Ttc通過內外角點估計，最終由式 （14） 計算得到當前標簽的全局位姿Ttw，作為標簽地圖的初值。

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

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

標簽的位姿包括旋轉與平移：

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

將該中間插值轉換為旋轉矩陣，則該標簽最終的位姿矩陣為

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

4 基于標簽的定位方法

4.1 相機位姿的初步估計

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

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

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

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

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

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

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

設u為地圖點觀測的二維坐標，uˉ為地圖點的重投影位置的二維坐標，則重投影誤差為

構建誤差函數(shù)：

對相機的位姿Twc和標簽位姿Ttw進行優(yōu)化，并通過卡方檢驗剔除地圖外點。

4.2.2 卡方檢驗

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

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

r服從自由度為2的卡方分布，在0.05的顯著性水平（95%的準確率）下，卡方檢驗的拒絕域為

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

5 試驗測試

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

5.1 內外角點約束效果與視角魯棒性測試

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

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

5.1.1 基于內外角點約束策略的定位效果測試

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

圖8 相機位置與標簽場景

圖9 無內外角點約束方法與本文方法的軌跡對比

圖10 無內外角點約束方法與本文方法的軌跡在位移與角度上的對比

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

表1 標簽間距相對誤差

圖11 標簽位置示意圖

5.1.2 視角魯棒性測試

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

圖12 相機位置與標簽場景

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

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

圖14 相機視角改變后軌跡對比

5.2 基于標簽的視覺SLAM 算法性能測試

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

圖15 試驗車輛與室內停車場

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

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

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

5.2.2 定位精度測試

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

圖16 室內停車場運行軌跡對比

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

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

表3 重復定位誤差對比

5.2.3 效率與實時性測試

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

表4 算法各模塊平均耗時 ms

6 結論

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

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