本發(fā)明涉及自動(dòng)駕駛，具體涉及用于車輛同步定位和建圖的方法、裝置和設(shè)備。

背景技術(shù)：

1、同步定位和建圖是車輛自動(dòng)駕駛中的一個(gè)關(guān)鍵步驟，可以使得車輛在運(yùn)行的過程中，感知車輛的位置和周圍的環(huán)境，從而讓車輛的決策模塊可以基于車輛的位置信息和環(huán)境信息指導(dǎo)車輛的下一步動(dòng)作，進(jìn)而完成設(shè)定的自動(dòng)駕駛?cè)蝿?wù)。進(jìn)行準(zhǔn)確的同步定位和建圖是車輛自動(dòng)駕駛?cè)蝿?wù)中必不可少的一部分，尤其是車輛運(yùn)行在高速路段場景中，面對(duì)高低起伏路況的情況下。

2、為了應(yīng)對(duì)長距離坡度變化的路況，相關(guān)技術(shù)中采用載波相位差分技術(shù)、相機(jī)、激光雷達(dá)和高精度地圖組合的技術(shù)方案，來創(chuàng)建三維的障礙物地圖，成本過高。

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

1、有鑒于此，本發(fā)明提供了一種用于車輛同步定位和建圖的方法、裝置和設(shè)備，以解決自動(dòng)駕駛車輛在高速路段場景中，長距離坡度變化的路況下，相關(guān)技術(shù)中采用載波相位差分技術(shù)、相機(jī)、激光雷達(dá)和高精度地圖組合的技術(shù)方案，來創(chuàng)建三維的障礙物地圖，成本過高的問題。

2、第一方面，本發(fā)明提供了一種用于車輛同步定位和建圖的方法，所述方法包括：獲取車輛的慣性測量單元數(shù)據(jù)、輪速數(shù)據(jù)和全球定位系統(tǒng)數(shù)據(jù)，基于所述慣性測量單元數(shù)據(jù)、所述輪速數(shù)據(jù)和所述全球定位系統(tǒng)數(shù)據(jù)采用誤差狀態(tài)卡爾曼濾波算法，得到所述車輛的三維位姿信息；基于所述慣性測量單元數(shù)據(jù)所表征的角度信息，確定所述三維位姿信息所表征的車輛位姿所在的重力平面，基于所述重力平面建立二維地圖，在所述二維地圖中確定所述車輛的位置；獲取所述車輛感知的車道線信息、障礙物位置信息和障礙物速度信息，基于所述障礙物位置信息在所述二維地圖中確定障礙物的位置，基于所述障礙物的速度信息在所述二維地圖中確定障礙物的速度；基于所述車輛行進(jìn)過程中所述車道線信息的變換更新所述三維位姿信息，通過最小二乘法在所述二維地圖中擬合車道線，所述二維地圖中包括：擬合得到的車道線、所述車輛的位置、所述障礙物的位置和所述障礙物的速度。

3、在一種可選的實(shí)施方式中，基于所述慣性測量單元數(shù)據(jù)、所述輪速數(shù)據(jù)和所述全球定位系統(tǒng)數(shù)據(jù)采用誤差狀態(tài)卡爾曼濾波算法，得到所述車輛的三維位姿信息包括：獲取所述慣性測量單元數(shù)據(jù)所表征的所述車輛的角度增量，基于所述角度增量的累加得到所述三維位姿信息；其中，所述慣性測量單元數(shù)據(jù)包括：所述車輛的加速度信號(hào)和所述車輛的角速度信號(hào)，對(duì)所述加速度信號(hào)進(jìn)行積分運(yùn)算，得到所述車輛的速度，對(duì)所述速度進(jìn)行積分運(yùn)算，得到所述車輛的位移；對(duì)所述角速度信號(hào)進(jìn)行積分運(yùn)算，得到所述車輛的橫滾角、俯仰角和偏航角。

4、在一種可選的實(shí)施方式中，基于所述慣性測量單元數(shù)據(jù)、所述輪速數(shù)據(jù)和所述全球定位系統(tǒng)數(shù)據(jù)采用誤差狀態(tài)卡爾曼濾波算法，得到所述車輛的三維位姿信息還包括：獲取所述全球定位系統(tǒng)數(shù)據(jù)表征的所述車輛的位置增量，基于所述位置增量的累加得到所述三維位姿信息；基于所述全球定位系統(tǒng)數(shù)據(jù)更新的觀測方程如下所示：

5、pgps＝p+δpgps

6、其中，pgps為所述全球定位系統(tǒng)數(shù)據(jù)表征的所述車輛的位置，p為當(dāng)前時(shí)刻的車輛位置，δpgps為pgps與p之間的車輛位置差異。

7、在一種可選的實(shí)施方式中，基于所述慣性測量單元數(shù)據(jù)、所述輪速數(shù)據(jù)和所述全球定位系統(tǒng)數(shù)據(jù)采用誤差狀態(tài)卡爾曼濾波算法，得到所述車輛的三維位姿信息還包括：獲取所述輪速數(shù)據(jù)表征的所述車輛的速度增量，基于所述速度增量的累加得到所述三維位姿信息；基于所述輪速數(shù)據(jù)更新的觀測方程如下所示：

8、vwheel＝rigvg

9、其中，hwpeel為基于所述車輛的輪速脈沖得到的輪速，pig為所述輪速脈沖由車體坐標(biāo)系到東北天坐標(biāo)系下的轉(zhuǎn)換關(guān)系，vg為所述東北天坐標(biāo)系下的車體速度。

10、在一種可選的實(shí)施方式中，基于所述障礙物位置信息在所述二維地圖中確定障礙物的位置，包括：通過如下公式在所述二維地圖中確定所述障礙物的位置：

11、pen＝ttransrgipi

12、其中，pen為在所述二維地圖中的二維障礙物位置，pi為所述障礙物位置信息所表征的二維空間中的二維障礙物位置，rgi為二維障礙物位置由車體坐標(biāo)系到當(dāng)前車體所在三維東北天坐標(biāo)系下的轉(zhuǎn)換關(guān)系，θ1為所述俯仰角，θ0為所述橫滾角，ttrans表示障礙物位置由三維東北天坐標(biāo)系轉(zhuǎn)換到二維東北坐標(biāo)系下的映射關(guān)系。

13、在一種可選的實(shí)施方式中，基于所述障礙物的速度信息在所述二維地圖中確定障礙物的速度，包括：通過如下公式在所述二維地圖中確定所述障礙物的速度：

14、ven＝ttransrgivi

15、其中，ven為在所述二維地圖中的障礙物速度，vi為所述障礙物的速度信息所表征的三維空間中的障礙物速度。

16、在一種可選的實(shí)施方式中，通過最小二乘法在所述二維地圖中擬合車道線包括：構(gòu)建車道線擬合函數(shù)，所述車道線擬合函數(shù)為三階多項(xiàng)式；基于所述車道線擬合函數(shù)的擬合值與觀測值之間的誤差構(gòu)建誤差函數(shù)，求解所述誤差函數(shù)的最小值，得到所述車道線擬合函數(shù)中各項(xiàng)的系數(shù)；基于所述各項(xiàng)的系數(shù)得到在所述二維地圖中的擬合車道線。

17、第二方面，本發(fā)明提供了一種用于車輛同步定位和建圖的裝置，所述裝置包括：數(shù)據(jù)獲取模塊，用于獲取車輛的慣性測量單元數(shù)據(jù)、輪速數(shù)據(jù)和全球定位系統(tǒng)數(shù)據(jù)，基于所述慣性測量單元數(shù)據(jù)、所述輪速數(shù)據(jù)和所述全球定位系統(tǒng)數(shù)據(jù)采用誤差狀態(tài)卡爾曼濾波算法，得到所述車輛的三維位姿信息；第一定位模塊，用于基于所述慣性測量單元數(shù)據(jù)所表征的角度信息，確定所述三維位姿信息所表征的車輛位姿所在的重力平面，基于所述重力平面建立二維地圖，在所述二維地圖中確定所述車輛的位置；第二定位模塊，用于獲取所述車輛感知的車道線信息、障礙物位置信息和障礙物速度信息，基于所述障礙物位置信息在所述二維地圖中確定障礙物的位置，基于所述障礙物的速度信息在所述二維地圖中確定障礙物的速度；建圖模塊，用于基于所述車輛行進(jìn)過程中所述車道線信息的變換更新所述三維位姿信息，通過最小二乘法在所述二維地圖中擬合車道線，所述二維地圖中包括：擬合得到的車道線、所述車輛的位置、所述障礙物的位置和所述障礙物的速度。

18、第三方面，本發(fā)明提供了一種計(jì)算機(jī)設(shè)備，包括：存儲(chǔ)器和處理器，存儲(chǔ)器和處理器之間互相通信連接，存儲(chǔ)器中存儲(chǔ)有計(jì)算機(jī)指令，處理器通過執(zhí)行計(jì)算機(jī)指令，從而執(zhí)行上述第一方面或其對(duì)應(yīng)的任一實(shí)施方式的用于車輛同步定位和建圖的方法。

19、第四方面，本發(fā)明提供了一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，該計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)上存儲(chǔ)有計(jì)算機(jī)指令，計(jì)算機(jī)指令用于使計(jì)算機(jī)執(zhí)行上述第一方面或其對(duì)應(yīng)的任一實(shí)施方式的用于車輛同步定位和建圖的方法。

20、本實(shí)施例提供的用于車輛同步定位和建圖的方法，是車輛運(yùn)行在高速路段場景中，面對(duì)高低起伏路況的情況下，通過imu、gps和輪速傳感器得到滿足自動(dòng)駕駛車輛定位需求的定位結(jié)果，可以有效節(jié)省定位成本；基于車輛行進(jìn)過程中車道線信息的變換更新三維位姿信息，可以提高創(chuàng)建的二維地圖的局部客觀性；得到的包括車輛位置的二維地圖，可以降低車輛的控制決策規(guī)劃模塊使用數(shù)據(jù)的成本。