【技術實現(xiàn)步驟摘要】
本專利技術涉及自動駕駛，具體涉及一種用于礦卡在卸料口rtk失效場景下的融合定位方法。
技術介紹
1、在礦山無人駕駛領域，礦卡的精確定位是實現(xiàn)自動化裝載和卸載作業(yè)的關鍵。
2、傳統(tǒng)的定位系統(tǒng)通常依賴于全球定位系統(tǒng)gps及其增強形式實時動態(tài)定位rtk來提供高精度的位置數(shù)據(jù)。然而，在特殊的卸料口區(qū)域，由于地形遮擋、多路徑效應等因素，rtk信號可能會受到干擾，導致定位結果出現(xiàn)跳變，影響定位的連續(xù)性和穩(wěn)定性。
3、為了降低對rtk技術的依賴性，現(xiàn)有技術提出將多傳感器數(shù)據(jù)融合以實現(xiàn)礦卡自動駕駛的準確定位。雖然該技術方案提高了系統(tǒng)的魯棒性，但是不同傳感器的數(shù)據(jù)可能存在時間同步問題，同時，計算力較大。
4、由此可見，確保多傳感器數(shù)據(jù)融合過程中各傳感器時間同步是值得研究的問題。
技術實現(xiàn)思路
1、為了解決上述技術問題，本專利技術提供了一種用于礦卡在卸料口rtk失效場景下的融合定位方法，所述方法利用lidar、輪速和imu傳感器進行融合定位礦卡于卸料口區(qū)域的狀態(tài)，所述方法包括以下步驟：
2、s1、時間同步操作：獲取當前的任務狀態(tài)，檢測到所述任務狀態(tài)符合卸料區(qū)倒車任務時，融合定位系統(tǒng)開始接管，并采用初始時刻的rtk狀態(tài)信息進行外參校驗，計算位姿變換值用于對點云地圖重定位里程計信息、imu-輪速里程計信息和rtk狀態(tài)信息進行時間同步，且將所述imu-輪速里程計信息和所述點云地圖重定位里程計信息同步到rtk信息時刻；
3、s2、因子圖建立與優(yōu)化：將
4、進一步的，所述采用初始時刻的rtk狀態(tài)信息進行外參校驗，還包括：
5、所述融合定位系統(tǒng)的初始時刻利用當前的rtk狀態(tài)信息作為先驗狀態(tài)，并得到初始時刻的位姿信息trtk，點云地圖重定位里程計通過配準得到優(yōu)化后的位姿結果tlio；由所述trtk和tlio計算估計出所述lidar的在線外參值tonline，其滿足公式：
6、tonline＝tlio-1*trtk
7、比較所述在線外參值tonline與離線標定的外參值toffline進的差值δtcalib，其滿足公式：
8、δtcalib＝tonline-1*toffline
9、比較所述差值δtcalib與閾值：
10、若所述差值δtcalib大于所述閾值，則直接報故障，進行停車處理；
11、若所述差值δtcalib滿足所述閾值，則對所述點云地圖重定位里程計信息、imu-輪速里程計信息和rtk狀態(tài)信息進行時間同步，并將所述imu-輪速里程計信息和點云地圖重定位里程計信息同步到所述rtk信息時刻。
12、進一步的，所述時間同步操作中所述點云地圖重定位里程計信息的時間同步包括：
13、已知當前的rtk信息時刻trtk為當前參考時間，提取距離當前參考時間最近的上一時刻的所述點云地圖重定位里程計信息，并記所述點云地圖重定位里程計信息的時間為tlio；提取并累乘所述tlio至所述trtk時間段的所有所述imu-輪速里程計信息timu-wheel，計算得到所述tlio至所述trtk時間段的位姿變換值其滿足公式：
14、
15、利用所述位姿變換值將所述tlio時刻的所述點云地圖重定位里程計信息進行遞推，同步到當前的所述rtk信息時刻trtk，其滿足公式：
16、
17、完成所述點云地圖重定位里程計信息的時間同步。
18、進一步的，所述時間同步操作中所述imu-輪速里程計信息的時間同步包括：
19、已知當前的rtk信息時刻trtk為當前參考時間，提取距離當前參考時間最近的上一時刻的rtk信息時刻t-1rtk，提取并累乘所述t-1rtk至所述trtk時間段的所有所述imu-輪速里程計信息timu-wheel，計算得到所述t-1rtk至所述trtk時間段的位姿變換值其滿足公式：
20、
21、利用所述位姿變換值將上一時刻rtk信息進行遞推，同步到當前的rtk信息時刻trtk，則此時的rtk信息為同步后的所述imu-輪速里程計信息，其滿足公式：
22、
23、完成所述imu-輪速里程計信息的時間同步。
24、進一步的，所述s2中因子圖建立還包括：
25、所述rtk狀態(tài)信息作為因子圖的先驗因子，只作用于初始狀態(tài)節(jié)點；
26、所述imu-輪速里程計信息作為中間約束因子，作用相鄰的狀態(tài)節(jié)點之間；
27、所述點云地圖重定位里程計信息作為一元邊因子，只作用于對應時刻下的狀態(tài)節(jié)點。
28、進一步的，所述s2中因子圖優(yōu)化還包括：
29、據(jù)貝葉斯定律推導得到所述最大后驗分布，作為當前最優(yōu)的系統(tǒng)狀態(tài)信息，求解在zi觀測下，最有可能的系統(tǒng)狀態(tài)xi，其滿足公式：
30、
31、其中，xi表示系統(tǒng)狀態(tài)x集合里的元素。
32、進一步的，所述監(jiān)測因子圖優(yōu)化后的系統(tǒng)狀態(tài)信息，還包括：
33、若監(jiān)測結果正常，則直接作為最終輸出；
34、若監(jiān)測結果異常，則通過同步后的相鄰兩時刻之間的imu-輪速里程計信息和點云地圖重定位里程計信息計算各自的位姿變換值δtimu-wheel和δtlio，其滿足公式：
35、
36、計算所述位姿變換值δtimu-wheel和δtlio之間的差值δtlio_imu-wheel，其滿足公式：
37、δtlio_imu-wheel＝δtimu-wheel-1*δtlio
38、比較所述差值δtlio_imu-wheel與閾值：
39、若所述差值δtlio_imu-wheel滿足所述閾值，則校驗通過，將所述imu-輪速里程計信息作為最終輸出；
40、若所述差值δtlio_imu-wheel大于所述閾值，則校驗失敗，直接報故障，進行停車處理。
41、進一步的，當卸料口接管距離大于設定的閾值時，同時當前任務狀態(tài)為裝料任務，則上山循跡時，所述融合定位系統(tǒng)結束接管。
42、與現(xiàn)有技術相比，本專利技術具有如下有益效果：
43、本專利技術利用lidar、輪速和imu多個傳感器信息進行融合定位，減少了對rtk的依賴性；并在因子圖優(yōu)化多位姿源之前，進行了時間同步操作，得到更準確優(yōu)化結果；同時，采用對比校驗的操作，實時監(jiān)測各個位姿源的狀態(tài)，滿足礦山無人駕駛的精確定位要求。
本文檔來自技高網(wǎng)...【技術保護點】
1.一種用于礦卡在卸料口RTK失效場景下的融合定位方法，其特征在于，所述方法利用LiDAR、輪速和IMU傳感器進行融合定位礦卡于卸料口區(qū)域的狀態(tài)，所述方法包括以下步驟：
2.根據(jù)權利要求1所述的用于礦卡在卸料口RTK失效場景下的融合定位方法，其特征在于，所述采用初始時刻的RTK狀態(tài)信息進行外參校驗，還包括：
3.根據(jù)權利要求1所述的用于礦卡在卸料口RTK失效場景下的融合定位方法，其特征在于，所述時間同步操作中所述點云地圖重定位里程計信息的時間同步包括：
4.根據(jù)權利要求1所述的用于礦卡在卸料口RTK失效場景下的融合定位方法，其特征在于，所述時間同步操作中所述IMU-輪速里程計信息的時間同步包括：
5.根據(jù)權利要求1所述的用于礦卡在卸料口RTK失效場景下的融合定位方法，其特征在于，所述S2中因子圖建立還包括：
6.根據(jù)權利要求5所述的用于礦卡在卸料口RTK失效場景下的融合定位方法，其特征在于，所述S2中因子圖優(yōu)化還包括：
7.根據(jù)權利要求5所述的用于礦卡在卸料口RTK失效場景下的融合定位方法，其特征在于，所述實
8.根據(jù)權利要求7所述的用于礦卡在卸料口RTK失效場景下的融合定位方法，其特征在于，當卸料口接管距離大于設定的閾值時，同時當前任務狀態(tài)為裝料任務，則上山循跡時，所述融合定位系統(tǒng)結束接管。
...【技術特征摘要】
1.一種用于礦卡在卸料口rtk失效場景下的融合定位方法，其特征在于，所述方法利用lidar、輪速和imu傳感器進行融合定位礦卡于卸料口區(qū)域的狀態(tài)，所述方法包括以下步驟：
2.根據(jù)權利要求1所述的用于礦卡在卸料口rtk失效場景下的融合定位方法，其特征在于，所述采用初始時刻的rtk狀態(tài)信息進行外參校驗，還包括：
3.根據(jù)權利要求1所述的用于礦卡在卸料口rtk失效場景下的融合定位方法，其特征在于，所述時間同步操作中所述點云地圖重定位里程計信息的時間同步包括：
4.根據(jù)權利要求1所述的用于礦卡在卸料口rtk失效場景下的融合定位方法，其特征在于，所述時間同步操作中所述imu-輪速里程計...
【專利技術屬性】
技術研發(fā)人員：何申中，吳志國，王煒杰，汪貴冬，張帥乾，
申請(專利權)人：安徽海博智能科技有限責任公司，
類型：發(fā)明
國別省市：
還沒有人留言評論。發(fā)表了對其他瀏覽者有用的留言會獲得科技券。