本發(fā)明屬于自動(dòng)駕駛，尤其是涉及一種基于磁釘imu融合的井下車輛定位方法。

背景技術(shù)：

1、現(xiàn)有井工礦下定位方案是基于imu/激光slam的融合定位，imu負(fù)責(zé)高頻率地預(yù)測(cè)車輛的位姿，激光雷達(dá)掃描周圍環(huán)境形成激光點(diǎn)云，通過與提前掃描的高精度地圖進(jìn)行點(diǎn)云匹配計(jì)算出低頻的車輛位姿（激光點(diǎn)云定位），通過后端優(yōu)化二者進(jìn)行融合，最終形成的車輛位姿。

2、基于imu/激光slam的融合定位方法使用過程中存在以下問題，對(duì)于特征重復(fù)度高的長巷道場景，縱向軌跡會(huì)產(chǎn)生較大的跳變；定位過程中依賴于高精地圖，當(dāng)行駛路徑上特征發(fā)生較大變化，需要進(jìn)行補(bǔ)圖，不利于常態(tài)化運(yùn)行，降低了運(yùn)行效率。

技術(shù)實(shí)現(xiàn)思路

1、有鑒于此，本發(fā)明旨在提出一種基于磁釘imu融合的井下車輛定位方法，以期解決上述部分技術(shù)問題中的至少之一。

2、為達(dá)到上述目的，本發(fā)明的技術(shù)方案是這樣實(shí)現(xiàn)的：

3、本發(fā)明第一方面提供了一種基于磁釘imu融合的井下車輛定位方法，包括以下步驟：

4、s1、根據(jù)車輛imu傳感器數(shù)據(jù)和行駛距離融合計(jì)算車輛的慣性導(dǎo)航坐標(biāo)；

5、s2、如果行駛過程中采集到磁釘坐標(biāo)，則以磁釘坐標(biāo)修正慣性導(dǎo)航坐標(biāo)得到最終的車輛當(dāng)前坐標(biāo)；

6、如果行駛過程中未采集到磁釘坐標(biāo)，則進(jìn)入s3；

7、s3、如果行駛過程中激光slam采集到激光點(diǎn)云數(shù)據(jù)，則以激光點(diǎn)云數(shù)據(jù)修正慣性導(dǎo)航坐標(biāo)得到最終的車輛當(dāng)前坐標(biāo)；

8、如果行駛過程中激光slam未采集到激光點(diǎn)云數(shù)據(jù)，則進(jìn)入s4；

9、s4、以慣性導(dǎo)航坐標(biāo)作為最終的車輛當(dāng)前坐標(biāo)。

10、進(jìn)一步的，所述s1包括以下步驟：

11、s11、使用imu傳感器采集車輛的三軸加速度和三軸角速度；

12、s12、運(yùn)用運(yùn)動(dòng)學(xué)模型對(duì)三軸角速度進(jìn)行積分計(jì)算得到姿態(tài)矩陣，將姿態(tài)矩陣投影到慣性導(dǎo)航坐標(biāo)系中；

13、s13、三軸加速度通過積分計(jì)算得到車輛在慣性導(dǎo)航坐標(biāo)系下的速度；

14、s14、將s13得到的速度通過積分計(jì)算得到車輛在慣性導(dǎo)航坐標(biāo)系下位移數(shù)據(jù)，根據(jù)位移數(shù)據(jù)計(jì)算車輛坐標(biāo)；

15、s15、通過輪速里程計(jì)計(jì)算車輛的速度和位移；

16、s16、根據(jù)s15計(jì)算得到的速度和位移與s14得到的車輛坐標(biāo)進(jìn)行融合生成慣性導(dǎo)航坐標(biāo)。

17、進(jìn)一步的，所述s2中采集到磁釘坐標(biāo)的步驟如下：

18、a1、垂直車輛軸線設(shè)置一排磁傳感器的檢測(cè)點(diǎn)，一排檢測(cè)點(diǎn)的中間設(shè)置rfid讀寫器；

19、a2、在車道的軸線上每隔指定距離設(shè)置磁釘和rfid標(biāo)簽，磁釘和rfid標(biāo)簽位于同一坐標(biāo)點(diǎn)，rfid標(biāo)簽的編號(hào)代表一個(gè)點(diǎn)坐標(biāo)數(shù)據(jù)，點(diǎn)坐標(biāo)數(shù)據(jù)由全站儀測(cè)量得到；

20、a3、rfid讀寫器采集rfid標(biāo)簽獲得rfid坐標(biāo)數(shù)據(jù)；

21、a4、通過檢測(cè)點(diǎn)采集到磁釘數(shù)據(jù)計(jì)算得到車輛軸線與磁釘之間的橫向距離。

22、a5、通過誤差距離對(duì)rfid坐標(biāo)數(shù)據(jù)進(jìn)行修正得到磁釘坐標(biāo)。

23、進(jìn)一步的，所述a2中指定距離根據(jù)慣性導(dǎo)航的精度結(jié)合車道的寬度及安全距離確定；

24、慣性導(dǎo)航的精度越高則指定距離越大；

25、車道的寬度越寬則指定距離越大；

26、安全距離越大則指定距離越大。

27、進(jìn)一步的，通過檢測(cè)點(diǎn)采集到磁釘數(shù)據(jù)計(jì)算得到車輛軸線與磁釘之間的橫向距離的公式如下：

28、；

29、其中d為車輛軸線與磁釘之間的橫向距離，n為檢測(cè)點(diǎn)的總數(shù)量，n為最左側(cè)磁傳感器的編號(hào)，m為檢測(cè)到磁釘?shù)拇艂鞲衅鞯臋z測(cè)點(diǎn)數(shù)量，l為相鄰檢點(diǎn)之間的間距。

30、進(jìn)一步的，所述a5中根據(jù)車輛軸線與磁釘之間的橫向距離、rfid坐標(biāo)數(shù)據(jù)、航向角度計(jì)算得到磁釘坐標(biāo)。

31、進(jìn)一步的，所述s3中以激光點(diǎn)云數(shù)據(jù)修正慣性導(dǎo)航坐標(biāo)得到最終的車輛當(dāng)前坐標(biāo)的步驟如下：

32、b1、從激光雷達(dá)傳感器獲取激光點(diǎn)云數(shù)據(jù)，這些激光點(diǎn)云數(shù)據(jù)包含了激光雷達(dá)傳感器掃描到的環(huán)境中的點(diǎn)云信息；

33、b2、對(duì)獲取的激光點(diǎn)云數(shù)據(jù)進(jìn)行處理，剔除無效點(diǎn)，并進(jìn)行點(diǎn)云濾波后激光點(diǎn)云定位模塊，作為實(shí)時(shí)掃描定位的輸入項(xiàng)；

34、b3、采集相鄰時(shí)刻內(nèi)imu傳感器的變換信息；

35、b4、采集地圖信息，地圖信息用于激光點(diǎn)云定位模塊中的掃描匹配；

36、b5、激光點(diǎn)云定位模塊將b3采集到的變換信息作為激光點(diǎn)云定位模塊的掃描匹配初始值輸入，將b2濾波后的激光點(diǎn)云數(shù)據(jù)作為實(shí)時(shí)掃描定位的輸入項(xiàng)，b4地圖信息用于激光點(diǎn)云定位模塊中的掃描匹配，得到最終的車輛當(dāng)前坐標(biāo)。

37、本發(fā)明第二方面提供了一種電子設(shè)備，包括處理器以及與處理器通信連接，且用于存儲(chǔ)所述處理器可執(zhí)行指令的存儲(chǔ)器，所述處理器用于執(zhí)行上述第一方面所述的方法。

38、本發(fā)明第三方面提供了一種服務(wù)器，包括至少一個(gè)處理器，以及與所述處理器通信連接的存儲(chǔ)器，所述存儲(chǔ)器存儲(chǔ)有可被所述至少一個(gè)處理器執(zhí)行的指令，所述指令被所述處理器執(zhí)行，以使所述至少一個(gè)處理器執(zhí)行如第一方面所述的方法。

39、本發(fā)明第四方面提供了一種計(jì)算機(jī)可讀取存儲(chǔ)介質(zhì)，存儲(chǔ)有計(jì)算機(jī)程序，所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)第一方面所述的方法。

40、相對(duì)于現(xiàn)有技術(shù)，本發(fā)明所述的一種基于磁釘imu融合的井下車輛定位方法具有以下有益效果：

41、本發(fā)明所述的一種基于磁釘imu融合的井下車輛定位方法，通過增加磁釘定位,解決井工礦下大地圖條件下局部地圖變動(dòng)引起的定位波動(dòng)問題，相較于只依靠激光slam定位方法而言解決井工礦長巷道特征重復(fù)環(huán)境下，激光slam縱向定位偏移問題，定位系統(tǒng)穩(wěn)定性增加，有益于井工礦無人駕駛的常態(tài)化運(yùn)行。

技術(shù)特征：

1.一種基于磁釘imu融合的井下車輛定位方法，其特征在于：包括以下步驟：

2.根據(jù)權(quán)利要求1所述的一種基于磁釘imu融合的井下車輛定位方法，其特征在于，所述s1包括以下步驟：

3.根據(jù)權(quán)利要求1所述的一種基于磁釘imu融合的井下車輛定位方法，其特征在于，所述s2中采集到磁釘坐標(biāo)的步驟如下：

4.根據(jù)權(quán)利要求3所述的一種基于磁釘imu融合的井下車輛定位方法，其特征在于：所述a2中指定距離根據(jù)慣性導(dǎo)航的精度結(jié)合車道的寬度及安全距離確定；

5.根據(jù)權(quán)利要求3所述的一種基于磁釘imu融合的井下車輛定位方法，其特征在于：通過檢測(cè)點(diǎn)采集到磁釘數(shù)據(jù)計(jì)算得到車輛軸線與磁釘之間的橫向距離的公式如下：

6.根據(jù)權(quán)利要求3所述的一種基于磁釘imu融合的井下車輛定位方法，其特征在于：所述a5中根據(jù)車輛軸線與磁釘之間的橫向距離、rfid坐標(biāo)數(shù)據(jù)、航向角度計(jì)算得到磁釘坐標(biāo)。

7.根據(jù)權(quán)利要求1所述的一種基于磁釘imu融合的井下車輛定位方法，其特征在于：所述s3中以激光點(diǎn)云數(shù)據(jù)修正慣性導(dǎo)航坐標(biāo)得到最終的車輛當(dāng)前坐標(biāo)的步驟如下：

8.一種電子設(shè)備，包括處理器以及與處理器通信連接，且用于存儲(chǔ)所述處理器可執(zhí)行指令的存儲(chǔ)器，其特征在于：所述處理器用于執(zhí)行上述權(quán)利要求1-7任一所述的方法。

9.一種服務(wù)器，其特征在于：包括至少一個(gè)處理器，以及與所述處理器通信連接的存儲(chǔ)器，所述存儲(chǔ)器存儲(chǔ)有可被所述至少一個(gè)處理器執(zhí)行的指令，所述指令被所述處理器執(zhí)行，以使所述至少一個(gè)處理器執(zhí)行如權(quán)利要求1-7任一所述的方法。

10.一種計(jì)算機(jī)可讀取存儲(chǔ)介質(zhì)，存儲(chǔ)有計(jì)算機(jī)程序，其特征在于：所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)權(quán)利要求1-7任一項(xiàng)所述的方法。

技術(shù)總結(jié)
本發(fā)明提供了一種基于磁釘IMU融合的井下車輛定位方法，包括以下步驟：S1、根據(jù)車輛IMU傳感器數(shù)據(jù)和行駛距離融合計(jì)算車輛的慣性導(dǎo)航坐標(biāo)；S2、以磁釘坐標(biāo)修正慣性導(dǎo)航坐標(biāo)得到最終的車輛當(dāng)前坐標(biāo)；S3、以激光點(diǎn)云數(shù)據(jù)修正慣性導(dǎo)航坐標(biāo)得到最終的車輛當(dāng)前坐標(biāo)；S4、以慣性導(dǎo)航坐標(biāo)作為最終的車輛當(dāng)前坐標(biāo)。本發(fā)明有益效果：通過增加磁釘定位,解決井工礦下大地圖條件下局部地圖變動(dòng)引起的定位波動(dòng)問題，相較于只依靠激光SLAM定位方法而言解決井工礦長巷道特征重復(fù)環(huán)境下，激光SLAM縱向定位偏移問題，定位系統(tǒng)穩(wěn)定性增加，有益于井工礦無人駕駛的常態(tài)化運(yùn)行。

技術(shù)研發(fā)人員：張亞琛,周佳偉,朱亞琛,李美貴,付友華,潘子宇
受保護(hù)的技術(shù)使用者：青島慧拓智能機(jī)器有限公司
技術(shù)研發(fā)日：
技術(shù)公布日：2024/10/10