本發(fā)明屬于slam領域，是一種適用于視覺slam的線特征后端位姿估計方法。

背景技術：

1、在機器人領域，同步定位與地圖構建(simultaneous?localization?andmapping，slam)技術一直備受學者關注，基于點特征的視覺slam技術也日益成熟。但僅基于點特征，在弱紋理場景下，特征點稀疏導致的定位精度下降問題被暴露出來。為解決這一問題，學者們引入其他的結構特征，如經(jīng)典的線特征。對于線特征，目前的主流方法與點特征的處理類似，視覺前端提取線特征，對線特征三角化后進行重投影，利用重投影誤差構建約束。但這樣不可避免引入了線特征的三角化誤差，無論是后端基于何種框架均無法有效修正三角化誤差帶來的影響。

技術實現(xiàn)思路

1、傳統(tǒng)slam線特征在構建重投影誤差時需要對線特征三角化，會引入三角化誤差使得定位精度下降，并且伴隨著線特征的增加，狀態(tài)變量隨之增多。針對這些問題，本發(fā)明基于點特征的僅位姿方法提出了一種適用于視覺slam的線特征僅位姿測量模型構建方法，將其融入到點特征僅位姿的vins(visual-inertial?navigation?system，視覺慣性導航系統(tǒng))中，直接通過三幀觀測構建線重投影，使得重投影誤差變量中只有系統(tǒng)的狀態(tài)變量，無須對線特征三角化來計算得到線特征在局部世界系下的坐標，從而能有效提升定位精度。

2、本發(fā)明的一種適用于視覺slam的線特征僅位姿量測模型構建方法，包括以下步驟：

3、步驟一，利用視覺前端所提取的線特征進行僅位姿重投影誤差構建；對提取的線特征l使用三幀共視幀構建該線特征在當前幀的重投影誤差；其中，三幀共視幀包括首次觀測到線特征l的幀ci、當前幀ck以及在ci和ck之間的某一共視l的觀測幀cj；線特征l在當前幀的重投影誤差的構建方式包括：

4、首先通過幀ci和cj獲取線特征l在ci相機系下到相機光心的距離參數(shù)再利用幀ci和ck之間的位姿關系得到線特征l在當前幀ck下的重投影直線的預測結果，無需進行線特征三角化，設獲得直線的齊次坐標進一步計算線特征l在當前幀的重投影誤差el；

5、其中，

6、中間變量其中，獲取線特征l在幀ci、cj相機下的相機光心和線特征l歸一化平面的兩個端點構成的反投影平面的單位法向量為cj相機系到ci相機系的旋轉(zhuǎn)矩陣，表示幀cj到幀ci的平移向量；sgn為符號函數(shù)；變量和為幀ci中去畸像素平面的線特征l的兩端點坐標，上角標t表示轉(zhuǎn)置。

7、步驟二，計算線特征重投影誤差關于狀態(tài)變量的誤差狀態(tài)雅可比，以此對狀態(tài)變量進行修正；采用鏈式求導方法，計算重投影誤差關于相機位姿的誤差狀態(tài)雅可比矩陣，以及重投影誤差對相機外參和內(nèi)參的雅可比矩陣，用于修正慣性傳感器imu的位姿，相機外參和相機內(nèi)參。

8、步驟三，在線特征重投影構建基礎上，設置線特征篩選與僅位姿基幀選取方式。對視覺前端提取的線特征和對線特征的共視幀cj進行篩選；其中，根據(jù)視差角進行共視幀的選取，對于線特征的中間共視幀cj，以獲取最大視差的目標來選取；對于線特征，(1)若出現(xiàn)中間共視幀與首次觀測幀ci間的視差角小于閾值，則剔除該線特征，(2)計算每一中間共視幀的線特征在ci相機系下到相機光心的距離參數(shù)，剔除其中距離參數(shù)小于0的不合理中間幀，根據(jù)合理中間幀的距離參數(shù)的均值和方差計算變異系數(shù)，將變異系數(shù)不小于設定閾值的線特征剔除。

9、相比現(xiàn)有技術，本發(fā)明的優(yōu)點和積極效果在于：

10、(1)本發(fā)明方法在構建重投影誤差時無需對線特征三角化，這避免了線特征三角化誤差引入的問題，同時也無需構建線特征的4自由度表征，使得線特征的引入方式更直觀簡潔。

11、(2)本發(fā)明方法在線特征引入時不會使得狀態(tài)變量增加，無需將線特征參數(shù)作為狀態(tài)變量。通過實驗可以看出，使用本發(fā)明方法能有效修正視覺slam的定位誤差。

12、(3)本發(fā)明方法應用于msckf框架下時，可以實現(xiàn)對系統(tǒng)當前幀狀態(tài)的實時修正。

技術特征：

1.一種適用于視覺slam的線特征僅位姿量測模型構建方法，其特征在于，包括如下步驟：

2.根據(jù)權利要求1所述的方法，其特征在于，所述的步驟1中，通過三幀共視幀構建僅位姿重投影誤差包括：

3.根據(jù)權利要求1或2所述的方法，其特征在于，所述的步驟1中，在應用于濾波方法時，根據(jù)重投影誤差el對觀測噪聲協(xié)方差進行自適應調(diào)整，包括：

4.根據(jù)權利要求1或2所述的方法，其特征在于，所述的步驟2中，獲取重投影誤差對相機位姿的誤差狀態(tài)雅可比矩陣的方法包括：

5.根據(jù)權利要求1或2所述的方法，其特征在于，所述的步驟2中，計算誤差el對相機內(nèi)參的雅可比矩陣，包括：

6.根據(jù)權利要求1或2所述的方法，其特征在于，所述的步驟2中，計算誤差el對相機外參的雅可比矩陣，包括：

7.根據(jù)權利要求1或2所述的方法，其特征在于，所述的方法還包括步驟3，對視覺前端提取的線特征和對線特征的共視幀cj進行篩選，篩選方式如下：

技術總結
本發(fā)明公開了一種適用于視覺SLAM的線特征僅位姿量測模型構建方法，屬于SLAM領域。傳統(tǒng)SLAM線特征在構建重投影誤差時需要對線特征三角化，引入的三角化誤差使得定位精度下降，并隨著線特征的增加，狀態(tài)變量隨之增多，針對這些問題，本發(fā)明方法利用視覺前端所提取的線特征進行僅位姿重投影誤差構建時，利用三幀約束構建線特征在當前幀的重投影誤差，并給出了對應的線特征重投影誤差關于狀態(tài)變量的誤差狀態(tài)雅可比求解方法，還提供了一種線特征篩選與僅位姿基幀選取方式以提高定位精度。將本發(fā)明方法融入點僅位姿系統(tǒng)中使用，可以有效提升定位精度，并減少狀態(tài)變量數(shù)量，應用于MSCKF框架下時，可以實現(xiàn)對系統(tǒng)當前幀狀態(tài)的實時修正。

技術研發(fā)人員：楊釗龍,張海,陳樹文,王愛萍,渠海榕
受保護的技術使用者：北京航空航天大學
技術研發(fā)日：
技術公布日：2025/1/2