【技術實現步驟摘要】
本專利技術屬于slam(simultaneouslocalization?and?mapping，同步定位與建圖)的，主要涉及了一種雷達輔助視覺線特征的視覺慣性里程計方法及系統。
技術介紹
1、單目視覺slam極其衍生的視覺慣性里程計問題中，誤差大部分來源于點特征的深度估計。針對此問題，傳統的雷達輔助視覺慣性里程計沒有使用多視圖幾何的方法對特征進行深度估計，而是采用雷達掃描深度對特征進行配準，大幅度降低由于點特征深度估計導致的誤差。
2、線特征一般被用于點特征缺失的無紋理環境，在系統中承擔特征補充功能。然而，相比于點特征，高自由度的線特征深度求解將會引入更高維度的誤差，甚至導致系統發散。因而，急需一種更高效更準確的方法來避免線特征三角化所帶來的誤差。
技術實現思路
1、本專利技術正是針對現有技術中線特征估計不準確的問題，提出了一種雷達輔助視覺線特征的視覺慣性里程計方法及系統，使用雷達點云信息擬合視覺線特征，首先對每幀圖像提取線特征，并且追蹤相鄰幀中的線特征；使用慣性測量單元信息做積分運算，去除雷達掃描點云中的運動畸變；對去畸變的點云提取帶有標簽的特征，所述標簽分為角點、面點和地面點；對視覺線特征坐標進行求解，求解的視覺線特征坐標包括視覺地面線特征、視覺垂直視覺線特征和其余線形特征，利用角點求解垂直視覺線特征，利用地面點求解地面視覺線特征，其余線形特征根據面點計算得出；最后將得到的視覺線特征坐標的重投影誤差以及慣性測量單元預積分殘差送入基于圖優化的緊耦合視覺慣性非線性優化
2、為了實現上述目的，本專利技術采取的技術方案是：一種雷達輔助視覺線特征的視覺慣性里程計方法，包括如下步驟：
3、s1、提取并追蹤圖像線特征：對每幀圖像提取線特征，并追蹤相鄰幀中的線特征；
4、s2、去除點云運動畸變：使用慣性測量單元信息做積分運算，去除雷達探測的點云中的運動畸變；具體為：對任意一幀雷達點云，以雷達掃描到的第一個點為起點，對高頻慣性測量單元信息積分，對除第一點之外的后續所有點，計算其到第一點的時間關系，利用對應慣性測量單元信息積分結果推算該點的坐標，最后整合所有點云得到一幀去畸變的新點云；
5、s3、點云特征提?。簩涍^步驟s2去畸變的點云提取帶有標簽的特征，所述標簽至少分為角點、面點和地面點；
6、s4、視覺線特征坐標求解：所述求解的視覺線特征坐標包括視覺地面線特征、視覺垂直視覺線特征和其余線形特征，
7、所述視覺地面線特征的求解：基于步驟s3的地面點，利用地面點云信息計算采樣點的深度，根據三維采樣點坐標擬合地面特征而得到；
8、所述視覺垂直線特征的求解：基于步驟s3的角點，使用直線擬合及線段裁剪的方法對視覺垂直線形特征的坐標進行求解；
9、所述其余線形特征的求解：基于步驟s3的面點，對二維視覺線形特征采樣，分為多個三維采樣點，利用面點信息計算采樣點的深度，根據三維采樣點坐標擬合其余線形特征而得到；
10、s5、緊耦合定位：將步驟s4得到的視覺線特征坐標以及慣性測量單元預積分殘差送入基于圖優化的緊耦合視覺慣性非線性優化估計器中，該估計器通過最小化損失函數以得到運動載體的最優姿態估計。
11、作為本專利技術的一種改進，所述步驟s1中，使用lsd對每一幀圖像進行提取線特征；對每相鄰的兩幀圖像，根據被提取線特征的坐標，使用lbd建立相鄰幀中線特征的關聯，追蹤相鄰圖像上的線特征。
12、作為本專利技術的一種改進，所述步驟s2具體包括如下步驟：
13、s21：對任意一幀點云，假設i時刻載體的位置和姿態分量四元數為掃描到點云中第一點l1為起始點，此時的位置和姿態分量四元數通過中點積分慣性測量單元數據得到：
14、
15、其中：
16、
17、其中為時刻t時載體的速度，和表示在時刻t時慣性測量單元提供的加速度和角速度信息，表示在時刻t+1時慣性測量單元提供的角速度信息，和為相同時刻陀螺儀和加速度計的零偏，gw表示為世界坐標系下的重力加速度，δt對應了i到i+1時刻的時間間隔；
18、s22：計算i到i+1時刻相對平移量和旋轉矩陣
19、
20、s23：根據相對旋轉和平移量，去除第二點中i到i+1時刻的運動畸變：
21、
22、其中f1和f2分別表示去除畸前后點云的三維坐標；
23、s24：循環步驟s21-s23，以第一點為基準，去除點云中每個點的運動畸變，最終得到一幀新的去畸變點云。
24、作為本專利技術的另一種改進，所述步驟s3具體包括如下步驟：
25、s31：將點云轉化成距離圖片，篩選出靠近地面的線束，如果發現相鄰線束的對應點距離變化不大，則視為地面點；否則為非地面點，進入步驟s32：
26、s32：對非地面點集中的每一點，計算其附近曲率，若曲率大，則視為角點；否則視為面點；
27、s33：按激光線束掃描的方向，若發現連續深度變化不大的角點子集或者面點子集，使用雷達點云物體分割方法對其賦予相同標簽，視為相同的簇或類。
28、作為本專利技術的另一種改進，所述步驟s32中非地面點集中點的曲率具體為：
29、
30、其中hn表示點云中第n個點的曲率值，rn表示點云中第n個點到坐標原點的距離值。
31、作為本專利技術的又一種改進，所述步驟s4視覺垂直線特征的求解過程中，
32、當視野中存在兩個相交平面，其相交于一條直線時，將角點點云投影到圖像平面，篩選出離垂直線特征較近的角點；如果一條線束中掃描到超過三個角點，挑選出兩個最左邊的點和兩個最右邊的點，并且擬合成兩條直線，隨后檢測兩條直線的夾角，如果夾角趨近于90度，將交點視為有效點，求出此時的垂直視覺線特征的擬合點坐標；
33、當視野中只存在單一相交平面，將角點投影到圖像平面并篩選出靠近視覺線特征的角點集合，進行線特征裁剪，求出此時的垂直視覺線特征的擬合點坐標；
34、得到兩種情況下擬合點坐標之后，如果擬合點全部來自同一標簽，使用與地面線特征求解類似的損失函數進行優化，得到最優普朗克坐標。
35、作為本專利技術的又一種改進，所述步驟s5中，將得到的每條三維線特征坐標送入基于圖優化的緊耦合視覺慣性非線性優化估計器，將慣性測量單元預積分殘差以及線特征重投影誤差設置為殘差項，通過最小化殘差項得到姿態的最優估計；
36、對滑動窗口所有待估計的狀態變量變量如下：
37、x＝[x0,x1…xs]τ
38、
39、其中xk為慣性測量單元坐標系k時刻的狀態，包括該時刻慣性測量單元坐標系到世界坐標系的平移、旋轉、在世界坐標系下的速度以及零偏；
40、最小化以本文檔來自技高網...
【技術保護點】
1.一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于，包括如下步驟：
2.如權利要求1所述的一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于：所述步驟S1中，使用LSD對每一幀圖像進行提取線特征；對每相鄰的兩幀圖像，根據被提取線特征的坐標，使用LBD建立相鄰幀中線特征的關聯，追蹤相鄰圖像上的線特征。
3.如權利要求2所述的一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于：所述步驟S2具體包括如下步驟：
4.如權利要求1所述的一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于：所述步驟S3具體包括如下步驟：
5.如權利要求4所述的一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于：所述步驟S32中非地面點集中點的曲率具體為：
6.如權利要求1所述的一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于：所述步驟S4視覺垂直線特征的求解過程中，
7.如權利要求1所述的一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于：所述步驟S5中，將得到的每條三維線特征坐標送入基于圖優化的緊耦合非
8.一種雷達輔助視覺線特征的視覺慣性里程計系統，包括計算機程序，其特征在于：所述計算機程序被處理器執行時實現如上述任一種所述方法的步驟。
...【技術特征摘要】
1.一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于，包括如下步驟：
2.如權利要求1所述的一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于：所述步驟s1中，使用lsd對每一幀圖像進行提取線特征；對每相鄰的兩幀圖像，根據被提取線特征的坐標，使用lbd建立相鄰幀中線特征的關聯，追蹤相鄰圖像上的線特征。
3.如權利要求2所述的一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于：所述步驟s2具體包括如下步驟：
4.如權利要求1所述的一種雷達輔助視覺線特征的視覺慣性里程計方法，其特征在于：所述步驟s3具體包括如下步驟：
5.如權利要求4所述的一種雷達輔助視覺線特征的...
還沒有人留言評論。發表了對其他瀏覽者有用的留言會獲得科技券。