【技術實現步驟摘要】
本專利技術涉及自動駕駛，具體涉及一種基于因子圖的多位姿源融合定位方法。
技術介紹
1、在礦山無人駕駛領域，礦卡的精準定位是實現自動化運輸和提高作業效率的關鍵技術之一。然而，由于礦山環境的復雜性和多變性，傳統的單一定位技術難以提供全面、精確的礦卡位置信息，例如，傳統使用的里程計模塊和rtk技術分別存在以下缺陷：
2、里程計模塊：在礦山無人駕駛中，常用的里程計模塊(如輪式里程計、慣性測量單元等)可以提供車輛的相對位移信息。但是，這些設備容易受到累積誤差的影響，隨著時間的推移和距離的增加，誤差會逐漸累積，導致定位結果的不準確。
3、rtk技術：雖然實時動態差分全球定位系統(rtk?gps)技術能夠提供高精度的位置信息，但在礦山環境中，由于地形遮擋、多路徑效應等因素，rtk信號可能會受到干擾，導致定位結果出現跳變，影響定位的連續性和穩定性。
4、此外，當前的自主定位算法包括slam(同步定位與建圖)和基于機器學習、深度學習的方法。其中，基于機器學習和深度學習的方法需要大量的標注數據進行訓練，且存在魯棒性弱，對于新環境的適應能力有限。而在礦山這種特殊環境下，收集大量高質量的訓練數據是非常困難的，因此這些方法的應用受到了限制。
5、綜上，目前自動駕駛領域已著力研究多個傳感器信息融合定位的方法，這樣的優勢在于不依賴于某一個特定的傳感器。然而，由于各個傳感器均具有特性，如何融合定位以及利用不同傳感器之間的信息差異如何優化不同位姿源的融合結果仍是項具有挑戰性的研究。
技術
1、為了解決上述技術問題，本專利技術提供了一種基于因子圖的多位姿源融合定位方法，所述方法利用rtk、lidar、imu以及輪速傳感器進行融合定位礦卡于卸料口區域的狀態，所述方法包括以下步驟：
2、s1、因子圖建立：將融合開始時刻的rtk狀態信息作為當前狀態下的先驗因子，輪速-imu里程計信息作為中間約束因子，點云地圖重定位里程計信息作為一元邊因子，建立因子圖；
3、s2、所述點云地圖重定位里程計信息優化：利用imu數據進行前向傳播，建立狀態量誤差模型；利用前向傳播的預測值對lidar原始點云進行后向傳播來進行去畸變操作，利用去畸變后的點云建立觀測方程；再采用iekf進行更新，求解殘差，至滿足迭代截止條件后更新所述點云地圖重定位里程計的最終狀態；
4、s3、所述輪速-imu里程計信息優化：根據imu提供的角速度利用imu?kinematic進行狀態前向傳播，得到旋轉誤差的觀測方程；同時根據imu提供的加速度建立一個關于當前旋轉的量測方程，通過卡爾曼濾波得到優化后的imu旋轉信息，并結合輪速計算得到imu數據時刻下的輪速-imu里程計信息；
5、s4、多位姿源融合：將所述先驗因子、點云地圖重定位里程計因子和輪速-imu里程計因子進行累乘，得到最大概率的系統狀態。
6、進一步的：
7、所述點云地圖重定位里程計信息優化中的狀態量誤差的雅可比矩陣其計算公式為：
8、
9、所述點云地圖重定位里程計信息優化中的噪聲量wi的雅可比矩陣其計算公式為：
10、
11、同時計算得到與預測協方差其計算公式為：
12、
13、其中，xi+1表示存在噪聲的真值，表示噪聲為零的預測狀態，qi表示噪聲量的協方差矩陣。
14、進一步的，所述點云地圖重定位里程計信息優化還包括：
15、s21、利用相對位姿變換將其他時刻的點云從各自的局部坐標系轉換到掃描時刻下的統一坐標系以完成去畸變操作，其滿足公式：
16、
17、其中，itl表示lidar到imu的外參；
18、s22、利用外參itl將去噪后的點云lpj+lnj從lidar坐標系轉換到imu坐標系，再利用先驗狀態量中的位姿將點云從imu坐標系轉換到全局坐標系；
19、s23、統一坐標系后，點云找到對應的若干個對應點gqj，且點云與對應點組成的向量與局部平面的法向量內積的理想狀態應為0，其滿足等式：
20、
21、并計算對應的雅可比矩陣。
22、進一步的，所述s2中采用iekf進行更新還包括：
23、s24、采用iekf進行更新的過程中，通過迭代進行重復觀測，以逐漸逼近最優線性化點；
24、s25、假設系統初始先驗狀態為對應的誤差狀態為δx；則記第k次迭代時，狀態向量先驗值為待求的誤差狀態為δxk；根據最大后驗估計求出δxk后，將所述δxk加回到得到重新觀測求解殘差z與雅可比矩陣h；再次求解其滿足公式：
25、k＝(htr-1h+p-1)-1htr-1
26、
27、其中，i表示單位矩陣，j表示雅可比矩陣，p表示協方差；
28、s26、如此往復所述s25，直到滿足迭代截止條件后，更新所述點云地圖重定位里程計信息的最終狀態。
29、進一步的，將所述s3中的旋轉誤差作為預測方程，其滿足公式：
30、
31、
32、其中，θ表示旋轉角度，bg表示偏移量，t表示時間，ωm表示角速度，i表示單位矩陣，fx表示預測狀態的雅可比矩陣，fi表示噪聲量的雅可比矩陣，k表示時刻。
33、進一步的，所述輪速-imu里程計信息優化還包括：
34、s31、建立量測方程，其滿足公式：
35、
36、其中，at表示重力方向，vg表示理想的重力方向[0?0?1]，
37、計算雅可比矩陣h，其滿足公式：
38、
39、s32、利用協方差矩陣p、雅克比矩陣h和噪聲na計算卡爾曼增益k，其滿足公式：
40、k＝p·ht·(h·pht+na)-1
41、s33、利用所述卡爾曼增益k計算優化后的旋轉矩陣、偏差值以及預測協方差，得到優化后的旋轉信息后，計算當前imu數據時刻前、后楨的輪速數據在imu數據時刻的線性插值結果，對應為imu數據時刻下的輪速數據；
42、s34、將運動化為n等分，n趨近于無窮，得到位姿變換公式，作為輪速-imu里程計因子，所述位姿變換公式為：
43、
44、其中，c表示所述s32得到的優化后的旋轉矩陣，j表示旋轉矩陣對應的雅可比矩陣。
45、進一步的，所述最大概率的系統狀態由貝葉斯定律推導所得，其滿足公式：
46、
47、其中，zi表示觀測，xi表示系統狀態。
48、與現有技術相比，本專利技術具有如下有益效果：
49、本專利技術通過結合rtk、輪速、imu以及lidar多個傳感器數據，利用誤差卡爾曼濾波以及因子圖優化的方法對這些數據進行融合優化，得到短時間內礦卡精確的自主定位信息。
本文檔來自技高網...【技術保護點】
1.一種基于因子圖的多位姿源融合定位方法，其特征在于，所述方法利用RTK、LiDAR、IMU以及輪速傳感器進行融合，獲取礦卡于卸料口區域的狀態，所述方法包括以下步驟：
2.根據權利要求1所述的基于因子圖的多位姿源融合定位方法，其特征在于：
3.根據權利要求2或1所述的基于因子圖的多位姿源融合定位方法，其特征在于，所述點云地圖重定位里程計信息優化還包括：
4.根據權利要求3所述的基于因子圖的多位姿源融合定位方法，其特征在于，所述S2中采用IEKF進行更新還包括：
5.根據權利要求1所述的基于因子圖的多位姿源融合定位方法，其特征在于，將所述S3中的旋轉誤差作為預測方程，其滿足公式：
6.根據權利要求5所述的基于因子圖的多位姿源融合定位方法，其特征在于，所述輪速-IMU里程計信息優化還包括：
7.根據權利要求1所述的基于因子圖的多位姿源融合定位方法，其特征在于，所述最大概率的系統狀態由貝葉斯定律推導所得，其滿足公式：
【技術特征摘要】
1.一種基于因子圖的多位姿源融合定位方法，其特征在于，所述方法利用rtk、lidar、imu以及輪速傳感器進行融合，獲取礦卡于卸料口區域的狀態，所述方法包括以下步驟：
2.根據權利要求1所述的基于因子圖的多位姿源融合定位方法，其特征在于：
3.根據權利要求2或1所述的基于因子圖的多位姿源融合定位方法，其特征在于，所述點云地圖重定位里程計信息優化還包括：
4.根據權利要求3所述的基于因子圖的多位姿源融合定位方法...
【專利技術屬性】
技術研發人員：何申中，潘偉，汪貴冬，王煒杰，張帥乾，
申請(專利權)人：安徽海博智能科技有限責任公司，
類型：發明
國別省市：
還沒有人留言評論。發表了對其他瀏覽者有用的留言會獲得科技券。