本發(fā)明涉及融合定位，尤其涉及一種基于鳥瞰圖的融合定位方法、系統(tǒng)、設(shè)備及介質(zhì)。

背景技術(shù)：

1、融合定位是為消除單一或少數(shù)種類傳感器獲取信息不足與失效的缺點(diǎn)，對(duì)多種傳感器采集的數(shù)據(jù)信息采用合理有效地融合算法進(jìn)行數(shù)學(xué)解算，獲得更加接近真實(shí)有用的對(duì)象信息。目前主流的融合定位方案為gnss/ins（gnss，global?navigation?satellitesystem，全球衛(wèi)星導(dǎo)航系統(tǒng)/?ins?，inertial?navigation?system，慣性導(dǎo)航系統(tǒng)）組合定位方案，盡管?gnss/ins組合導(dǎo)航可以解決大部分開闊的室外定位問(wèn)題，但是其自身也存在一些缺陷仍舊無(wú)法滿足自動(dòng)駕駛復(fù)雜多變的應(yīng)用場(chǎng)景。主要原因有以下幾點(diǎn)：

2、（1）、在城市峽谷或者信號(hào)遮擋路段，衛(wèi)星數(shù)量較少、分布不均勻，接收機(jī)觀測(cè)衛(wèi)星信號(hào)有限，同時(shí)受到衛(wèi)星信號(hào)多路徑影響，造成?gnss信號(hào)的定位精度降低或者無(wú)法定位。

3、（2）、gnss和ins組合定位方式，在衛(wèi)星遮擋后誤差迅速累積，導(dǎo)致融合位置信息不準(zhǔn)確或者出現(xiàn)跳躍等問(wèn)題。

4、（3）、自動(dòng)駕駛應(yīng)用場(chǎng)景復(fù)雜多變，道路上存在大量動(dòng)態(tài)目標(biāo)的干擾等，常見(jiàn)激光雷達(dá)定位技術(shù)采用深度學(xué)的方法進(jìn)行處理，計(jì)算量大，影響定位精度和可靠性。

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

1、本發(fā)明提供了一種基于鳥瞰圖的融合定位方法、系統(tǒng)、設(shè)備及介質(zhì)，用以解決上述問(wèn)題。

2、一方面，本發(fā)明提供了一種基于鳥瞰圖的融合定位方法，所述方法包括以下步驟：步驟s1：獲取車輛的衛(wèi)星位置信息；步驟s2：基于捷聯(lián)慣導(dǎo)系統(tǒng)解算位置信息及姿態(tài)，并修正慣導(dǎo)誤差；步驟s3：獲取點(diǎn)云數(shù)據(jù)生成鳥瞰圖像，并與高精度地圖關(guān)聯(lián)匹配；步驟s4：在gnss信號(hào)不佳時(shí)，基于里程計(jì)提供短期位置信息，以支持慣導(dǎo)定位；步驟s5：通過(guò)多傳感器數(shù)據(jù)進(jìn)行誤差估計(jì)和修正，輸出修正后的車輛實(shí)時(shí)位置信息和姿態(tài)。

3、在本發(fā)明的一種實(shí)現(xiàn)方式中，所述步驟s2中，將慣性測(cè)量單元測(cè)量的原始數(shù)據(jù)作為輸入，經(jīng)過(guò)導(dǎo)航計(jì)算單元完成數(shù)據(jù)姿態(tài)、位置、速度的坐標(biāo)變換和解算工作；其中，慣導(dǎo)分為姿態(tài)更新、速度更新和位置更新；

4、姿態(tài)更新的四元數(shù)微分方程，如下式所示：

5、

6、其中，

7、

8、

9、

10、

11、式中，為描述載體姿態(tài)的姿態(tài)四元數(shù)，是陀螺測(cè)量的載體角速度，為地球自轉(zhuǎn)角速度，為載體運(yùn)動(dòng)引起的角速度，是導(dǎo)航系相對(duì)于慣性空間的轉(zhuǎn)動(dòng)角速度，為海拔高度，是捷聯(lián)慣導(dǎo)速度，、分別為載體所在地點(diǎn)地球子午圈和卯酉圈曲率半徑，為緯度，其計(jì)算公式近似為：

12、

13、

14、其中，為地球參考橢球的長(zhǎng)半軸，為參考橢球的扁率；

15、速度更新微分方程，如下式所示：

16、

17、其中，

18、

19、

20、捷聯(lián)慣性導(dǎo)航中，在b系下加速度計(jì)觀測(cè)的比力加速度記為，??系下觀測(cè)的比力加速度記為，表示為b系與n系的旋轉(zhuǎn)矩陣，為重力加速度，為由載體運(yùn)動(dòng)引起的對(duì)地心加速度；

21、位置更新微分方程，如下式所示：

22、

23、

24、。

25、在本發(fā)明的一種實(shí)現(xiàn)方式中，所述步驟s2中，通過(guò)gnss/rtk模塊修正慣導(dǎo)誤差，具體為：

26、選取捷聯(lián)慣導(dǎo)系統(tǒng)誤差，作如下表示：

27、

28、其中，是姿態(tài)誤差、是速度誤差、是位置誤差、是陀螺儀漂移、是加速度計(jì)常值偏置、，是里程計(jì)安裝誤差；

29、系統(tǒng)上電啟動(dòng)后認(rèn)為陀螺儀漂移、加速度計(jì)偏置、里程計(jì)安裝以及標(biāo)度因數(shù)誤差保持為一固定常數(shù)不變，即：，，，，可得自主導(dǎo)航系統(tǒng)狀態(tài)方程：

30、

31、其中：f為狀態(tài)轉(zhuǎn)移矩陣，g為噪聲分配矩陣，w為系統(tǒng)狀態(tài)噪聲陣；

32、由gnss接收機(jī)構(gòu)成的觀測(cè)方程如式所示：

33、

34、

35、其中：為gnss接收機(jī)提供的位置信息，為gnss接收機(jī)提供的速度信息。

36、在本發(fā)明的一種實(shí)現(xiàn)方式中，所述步驟s4，具體包括：

37、在gnss信號(hào)質(zhì)量不佳時(shí)，基于卡爾曼組合濾波器，根據(jù)里程計(jì)提供的速度信息作為外部觀測(cè)量對(duì)組合濾波器進(jìn)行量測(cè)更新，將更新后的位置誤差估計(jì)值反饋回航位推算模塊中，對(duì)航位推算模塊進(jìn)行位置修正，同時(shí)組合導(dǎo)航濾波器輸出最優(yōu)的組合導(dǎo)航信息；

38、里程計(jì)安裝誤差角記為俯仰、滾動(dòng)和方位偏角，記偏差角矢量，則可得變換矩陣如下：

39、

40、另外，在實(shí)際里程儀的測(cè)量中還可能存在刻度系數(shù)誤差，實(shí)際中在導(dǎo)航坐標(biāo)系上里程儀的速度輸出應(yīng)為：

41、

42、其中記：

43、，

44、針對(duì)里程計(jì)航位推算模塊，將捷聯(lián)慣導(dǎo)解算的位置與里程計(jì)航位推算的位置之差作為系統(tǒng)觀測(cè)量，則由里程計(jì)航位推算模塊構(gòu)成的觀測(cè)方程如式所示：

45、=

46、=。

47、在本發(fā)明的一種實(shí)現(xiàn)方式中，所述步驟s3中，關(guān)聯(lián)匹配的過(guò)程，具體為：基于特征提取算法提取鳥瞰圖中的特征點(diǎn)，并使用特征描述子進(jìn)行匹配，通過(guò)鳥瞰圖道路-地圖要素匹配，將圖像中提取的車道線線段與車道線地圖相匹配；通過(guò)慣性導(dǎo)航計(jì)算出的初始位置，在高精度的地圖中進(jìn)行分塊搜索，先確定大致的位置，再進(jìn)行精細(xì)搜索；第一次搜索時(shí)，對(duì)分塊的車道線地圖中的每個(gè)地圖點(diǎn)進(jìn)行遍歷，計(jì)算每個(gè)地圖點(diǎn)與車輛的最小距離，從而確定車輛在車道線地圖上的位置；在非第一次搜索時(shí)，從上一次估計(jì)結(jié)果的位置沿著移動(dòng)方向進(jìn)行搜索即可其輸出的匹配結(jié)果作為位置觀測(cè)，進(jìn)行狀態(tài)估計(jì)和觀測(cè)誤差的求解。

48、在本發(fā)明的一種實(shí)現(xiàn)方式中，生成鳥瞰圖的過(guò)程，具體為：從激光雷達(dá)傳感器獲取點(diǎn)云數(shù)據(jù)；對(duì)點(diǎn)云進(jìn)行預(yù)處理，如濾波和裁剪，以減少噪聲和無(wú)關(guān)數(shù)據(jù)；將點(diǎn)云數(shù)據(jù)從三維坐標(biāo)系投影到二維平面，生成鳥瞰圖像；將投影后的數(shù)據(jù)轉(zhuǎn)換為便于可視化和處理的圖像格式。

49、在本發(fā)明的一種實(shí)現(xiàn)方式中，所述步驟s5，具體為：輸入多傳感器數(shù)據(jù)，利用卡爾曼濾波進(jìn)行誤差估計(jì)并更新位置信息；卡爾曼濾波系統(tǒng)狀態(tài)方程和觀測(cè)方程進(jìn)行離散化為式：

50、

51、

52、其中，為系統(tǒng)離散化后的狀態(tài)轉(zhuǎn)移矩陣，為離散化后的噪聲分配矩陣。與依次代表系統(tǒng)在k時(shí)刻和k-1時(shí)刻的狀態(tài)量，代表系統(tǒng)的狀態(tài)噪聲，代表系統(tǒng)的觀測(cè)噪聲。具體實(shí)現(xiàn)過(guò)程可使用經(jīng)典kalman濾波的更新過(guò)程表示，?狀態(tài)一步預(yù)測(cè)方程為：

53、

54、狀態(tài)估計(jì)如下式：

55、

56、一步預(yù)測(cè)誤差方差陣如下式：

57、

58、濾波增益矩陣如下式：

59、

60、估計(jì)誤差方差陣如下式：

61、

62、濾波增益矩陣又可以進(jìn)一步寫成：

63、

64、估計(jì)誤差方差陣又可以進(jìn)一步寫成：

65、。

66、本發(fā)明還提供了一種基于鳥瞰圖的融合定位系統(tǒng)，所述系統(tǒng)包括：gnss/rtk模塊，用于獲取車輛的衛(wèi)星位置信息；慣導(dǎo)模塊，用于解算位置信息及姿態(tài)，并修正慣導(dǎo)誤差；激光雷達(dá)處理模塊，用于獲取點(diǎn)云數(shù)據(jù)生成鳥瞰圖像，并與高精度地圖關(guān)聯(lián)匹配；里程計(jì)更新模塊，用于在gnss信號(hào)不佳時(shí)，基于里程計(jì)提供短期位置信息，以支持慣導(dǎo)定位；誤差狀態(tài)估計(jì)與修正模塊，用于通過(guò)多傳感器數(shù)據(jù)進(jìn)行誤差估計(jì)和修正，輸出修正后的車輛實(shí)時(shí)位置信息和姿態(tài)。

67、本發(fā)明還提供了一種基于鳥瞰圖的融合定位設(shè)備，所述設(shè)備包括：至少一個(gè)處理器；以及，與所述至少一個(gè)處理器通信連接的存儲(chǔ)器；其中，所述存儲(chǔ)器存儲(chǔ)有可被所述至少一個(gè)處理器執(zhí)行的指令，所述指令被所述至少一個(gè)處理器執(zhí)行，以使所述至少一個(gè)處理器能夠完成：步驟s1：獲取車輛的衛(wèi)星位置信息；步驟s2：基于捷聯(lián)慣導(dǎo)系統(tǒng)解算位置信息及姿態(tài)，并修正慣導(dǎo)誤差；步驟s3：獲取點(diǎn)云數(shù)據(jù)生成鳥瞰圖像，并與高精度地圖關(guān)聯(lián)匹配；步驟s4：在gnss信號(hào)不佳時(shí)，基于里程計(jì)提供短期位置信息，以支持慣導(dǎo)定位；步驟s5：通過(guò)多傳感器數(shù)據(jù)進(jìn)行誤差估計(jì)和修正，輸出修正后的車輛實(shí)時(shí)位置信息和姿態(tài)。

68、本發(fā)明還提供了一種基于鳥瞰圖的融合定位的非易失性計(jì)算機(jī)存儲(chǔ)介質(zhì)，存儲(chǔ)有計(jì)算機(jī)可執(zhí)行指令，所述計(jì)算機(jī)可執(zhí)行指令被處理器執(zhí)行以用于實(shí)現(xiàn)：步驟s1：獲取車輛的衛(wèi)星位置信息；步驟s2：基于捷聯(lián)慣導(dǎo)系統(tǒng)解算位置信息及姿態(tài)，并修正慣導(dǎo)誤差；步驟s3：獲取點(diǎn)云數(shù)據(jù)生成鳥瞰圖像，并與高精度地圖關(guān)聯(lián)匹配；步驟s4：在gnss信號(hào)不佳時(shí)，基于里程計(jì)提供短期位置信息，以支持慣導(dǎo)定位；步驟s5：通過(guò)多傳感器數(shù)據(jù)進(jìn)行誤差估計(jì)和修正，輸出修正后的車輛實(shí)時(shí)位置信息和姿態(tài)。

69、本發(fā)明提供的一種基于鳥瞰圖的融合定位方法、系統(tǒng)、設(shè)備及介質(zhì)，具有以下有益效果：

70、（1）、整個(gè)多傳感器融合定位系統(tǒng)通過(guò)gnss/rtk、慣性導(dǎo)航、激光雷達(dá)和里程計(jì)的協(xié)同工作，提升了系統(tǒng)的定位精度和魯棒性；

71、（2）、gnss信號(hào)較差的環(huán)境，系統(tǒng)依然能夠通過(guò)多傳感器的融合實(shí)現(xiàn)高精度定位，并將修正后的位置信息實(shí)時(shí)輸出，保障車輛的精確導(dǎo)航和定位。