本發(fā)明屬于無(wú)人駕駛，具體涉及一種無(wú)人駕駛車輛定位方法、系統(tǒng)、終端及存儲(chǔ)介質(zhì)。

背景技術(shù)：

1、隨著自動(dòng)駕駛技術(shù)的迅速發(fā)展，車輛定位技術(shù)已成為實(shí)現(xiàn)無(wú)人駕駛的重要基礎(chǔ)。精確的車輛定位是確保無(wú)人駕駛車輛安全行駛和正確決策的前提。目前，常用的車輛定位技術(shù)主要依賴于全球?qū)Ш叫l(wèi)星系統(tǒng)（gnss）、慣性導(dǎo)航系統(tǒng)（imu）以及其他傳感器（如激光雷達(dá)、攝像頭）。然而，城市環(huán)境中的復(fù)雜性，如高樓的遮擋、衛(wèi)星信號(hào)的失效、以及突發(fā)的交通情況，給車輛定位帶來(lái)了極大的挑戰(zhàn)。

2、現(xiàn)有的車輛定位技術(shù)雖然已經(jīng)有了一定的成熟度，但在復(fù)雜場(chǎng)景下仍存在以下主要弊端：第一，單一傳感器依賴性強(qiáng)：大多數(shù)無(wú)人駕駛車輛的定位系統(tǒng)依賴于單一的gnss（如gps、北斗）作為主要的定位來(lái)源。然而，gnss容易受到城市高樓、大氣條件等因素的干擾，特別是在城市峽谷、隧道等區(qū)域，衛(wèi)星信號(hào)可能完全失效，從而導(dǎo)致車輛定位精度下降甚至定位失敗。第二，imu獨(dú)立工作時(shí)漂移誤差大：慣性導(dǎo)航系統(tǒng)（imu）可以在短時(shí)間內(nèi)提供車輛的運(yùn)動(dòng)信息，但其獨(dú)立工作時(shí)的誤差會(huì)隨時(shí)間累積（漂移誤差），尤其在gnss信號(hào)缺失的情況下，imu的精度會(huì)迅速下降。第三，環(huán)境感知能力有限：?jiǎn)我皇褂胓nss或imu無(wú)法準(zhǔn)確感知車輛周圍的動(dòng)態(tài)環(huán)境，如行人、車輛、交通信號(hào)等。而激光雷達(dá)和相機(jī)雖然可以提供豐富的環(huán)境信息，但這些傳感器本身在某些場(chǎng)景下也可能受到光線、天氣、遮擋物等因素的干擾。第四，定位精度不高，魯棒性不足：現(xiàn)有的單一傳感器定位方案在復(fù)雜環(huán)境中，往往定位精度不足且系統(tǒng)魯棒性差，無(wú)法應(yīng)對(duì)多種環(huán)境條件的變化。

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

1、針對(duì)現(xiàn)有技術(shù)存在的單一傳感器依賴性強(qiáng)、imu獨(dú)立工作時(shí)漂移誤差大的問題，本發(fā)明提供一種無(wú)人駕駛車輛定位方法、系統(tǒng)、終端及存儲(chǔ)介質(zhì)，以解決上述技術(shù)問題。

2、第一方面，本發(fā)明提供一種無(wú)人駕駛車輛定位方法，包括：

3、獲取多個(gè)傳感器采集的車輛定位數(shù)據(jù)和系統(tǒng)噪聲數(shù)據(jù)；

4、基于車輛定位數(shù)據(jù)和系統(tǒng)噪聲數(shù)據(jù)，構(gòu)建車輛定位的預(yù)測(cè)方程與觀測(cè)方程；

5、基于系統(tǒng)噪聲數(shù)據(jù)，構(gòu)建協(xié)方差矩陣；

6、基于車輛定位的預(yù)測(cè)方程、觀測(cè)方程和協(xié)方差矩陣，使用卡爾曼算法計(jì)算卡爾曼增益，得到當(dāng)前時(shí)刻的最優(yōu)估計(jì)值。

7、進(jìn)一步地，所述多個(gè)傳感器采集的車輛定位數(shù)據(jù)包括第一定位數(shù)據(jù)與第二定位數(shù)據(jù)；

8、所述第一定位數(shù)據(jù)包括imu采集的車輛位置數(shù)據(jù)，所述第二定位數(shù)據(jù)包括激光雷達(dá)采集的點(diǎn)云數(shù)據(jù)，所述系統(tǒng)噪聲數(shù)據(jù)包括系統(tǒng)狀態(tài)噪聲和系統(tǒng)觀測(cè)噪聲。

9、進(jìn)一步地，所述基于車輛定位數(shù)據(jù)和系統(tǒng)噪聲數(shù)據(jù)，構(gòu)建車輛定位的預(yù)測(cè)方程與觀測(cè)方程，包括：

10、基于第一定位數(shù)據(jù)和系統(tǒng)狀態(tài)噪聲，構(gòu)建車輛定位的預(yù)測(cè)方程，所述車輛定位的預(yù)測(cè)方程為，其中，為k時(shí)刻的車輛位置，為k-1時(shí)刻的車輛位置的最優(yōu)估計(jì)，為系統(tǒng)狀態(tài)噪聲，為已知的系統(tǒng)輸入量；

11、基于第二定位數(shù)據(jù)和系統(tǒng)觀測(cè)噪聲，構(gòu)建觀測(cè)方程，所述觀測(cè)方程為，其中，為激光雷達(dá)在k時(shí)刻的點(diǎn)云數(shù)據(jù)，為系統(tǒng)觀測(cè)噪聲。

12、進(jìn)一步地，基于系統(tǒng)噪聲數(shù)據(jù)，構(gòu)建協(xié)方差矩陣，包括：

13、基于系統(tǒng)狀態(tài)噪聲，構(gòu)建系統(tǒng)狀態(tài)噪聲的協(xié)方差矩陣，系統(tǒng)狀態(tài)噪聲服從均值為0且協(xié)方差矩陣的高斯分布，記為~n（0，）；

14、基于系統(tǒng)觀測(cè)噪聲，構(gòu)建系統(tǒng)觀測(cè)噪聲的協(xié)方差矩陣，系統(tǒng)觀測(cè)噪聲服從均值為0且協(xié)方差矩陣的高斯分布，記為~n（0，）。

15、進(jìn)一步地，基于車輛定位的預(yù)測(cè)方程、觀測(cè)方程和協(xié)方差矩陣，使用卡爾曼算法計(jì)算卡爾曼增益，得到當(dāng)前時(shí)刻的最優(yōu)估計(jì)值，包括：

16、將所述預(yù)測(cè)方程使用一階泰勒展開，得到，其中，為k時(shí)刻車輛位置的先驗(yàn)估計(jì)值，為k-1時(shí)刻車輛位置的最優(yōu)估計(jì)值；

17、將所述觀測(cè)方程使用一階泰勒展開，得到，其中為觀測(cè)估計(jì)值；

18、對(duì)進(jìn)行一階偏導(dǎo)數(shù)計(jì)算，得到狀態(tài)轉(zhuǎn)移矩陣；

19、對(duì)進(jìn)行一階偏導(dǎo)數(shù)計(jì)算，得到觀測(cè)轉(zhuǎn)移矩陣；

20、基于狀態(tài)轉(zhuǎn)移矩陣、和k-1時(shí)刻對(duì)應(yīng)的協(xié)方差矩陣，得到k時(shí)刻協(xié)方差矩陣的先驗(yàn)估計(jì)值，所述=+；

21、基于k時(shí)刻協(xié)方差矩陣的先驗(yàn)估計(jì)值、觀測(cè)轉(zhuǎn)移矩陣、，計(jì)算卡爾曼增益，所述=，其中，為k時(shí)刻的卡爾曼增益，為的轉(zhuǎn)置矩陣；

22、基于卡爾曼增益、車輛位置的先驗(yàn)估計(jì)值得到當(dāng)前時(shí)刻的最優(yōu)估計(jì)值，所述=。

23、第二方面，本發(fā)明提供一種無(wú)人駕駛車輛定位系統(tǒng)，包括：

24、數(shù)據(jù)獲取模塊，用于獲取多個(gè)傳感器采集的車輛定位數(shù)據(jù)和系統(tǒng)噪聲數(shù)據(jù)；

25、方程構(gòu)建模塊，用于基于車輛定位數(shù)據(jù)和系統(tǒng)噪聲數(shù)據(jù)，構(gòu)建車輛定位的預(yù)測(cè)方程與觀測(cè)方程；

26、協(xié)方差矩陣構(gòu)建模塊，用于基于系統(tǒng)噪聲數(shù)據(jù)，構(gòu)建協(xié)方差矩陣；

27、最優(yōu)估計(jì)值計(jì)算模塊，用于基于車輛定位的預(yù)測(cè)方程、觀測(cè)方程和協(xié)方差矩陣，使用卡爾曼算法計(jì)算卡爾曼增益，得到當(dāng)前時(shí)刻的最優(yōu)估計(jì)值。

28、進(jìn)一步地，方程構(gòu)建模塊包括：

29、預(yù)測(cè)方程構(gòu)建單元，用于基于第一定位數(shù)據(jù)和系統(tǒng)狀態(tài)噪聲，構(gòu)建車輛定位的預(yù)測(cè)方程，所述車輛定位的預(yù)測(cè)方程為，其中，為k時(shí)刻的車輛位置，為k-1時(shí)刻的車輛位置的最優(yōu)估計(jì)，為系統(tǒng)狀態(tài)噪聲，為已知的系統(tǒng)輸入量；

30、觀測(cè)方程構(gòu)建單元，用于基于第二定位數(shù)據(jù)和系統(tǒng)觀測(cè)噪聲，構(gòu)建觀測(cè)方程，所述觀測(cè)方程為，其中，為激光雷達(dá)在k時(shí)刻的點(diǎn)云數(shù)據(jù)，為系統(tǒng)觀測(cè)噪聲。

31、進(jìn)一步地，協(xié)方差矩陣構(gòu)建模塊包括：

32、第一協(xié)方差矩陣構(gòu)建單元，用于基于系統(tǒng)狀態(tài)噪聲，構(gòu)建系統(tǒng)狀態(tài)噪聲的協(xié)方差矩陣，系統(tǒng)狀態(tài)噪聲服從均值為0且協(xié)方差矩陣的高斯分布，記為~n（0，）；

33、第二協(xié)方差矩陣構(gòu)建單元，用于基于系統(tǒng)觀測(cè)噪聲，構(gòu)建系統(tǒng)觀測(cè)噪聲的協(xié)方差矩陣，系統(tǒng)觀測(cè)噪聲服從均值為0且協(xié)方差矩陣的高斯分布，記為~n（0，）。

34、第三方面，提供一種終端，包括：

35、處理器、存儲(chǔ)器，其中，

36、該存儲(chǔ)器用于存儲(chǔ)計(jì)算機(jī)程序，

37、該處理器用于從存儲(chǔ)器中調(diào)用并運(yùn)行該計(jì)算機(jī)程序，使得終端執(zhí)行上述的終端的方法。

38、第四方面，提供了一種計(jì)算機(jī)存儲(chǔ)介質(zhì)，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)中存儲(chǔ)有指令，當(dāng)其在計(jì)算機(jī)上運(yùn)行時(shí)，使得計(jì)算機(jī)執(zhí)行上述各方面所述的方法。

39、本發(fā)明的有益效果在于，本發(fā)明提供的無(wú)人駕駛車輛定位方法、系統(tǒng)、終端及存儲(chǔ)介質(zhì)，通過融合多個(gè)傳感器的數(shù)據(jù)，系統(tǒng)能夠結(jié)合慣性測(cè)量單元和激光雷達(dá)點(diǎn)云數(shù)據(jù)的優(yōu)勢(shì)進(jìn)行車輛定位。imu能夠提供高頻率的運(yùn)動(dòng)信息，激光雷達(dá)則提供環(huán)境的精確空間數(shù)據(jù)。通過將這兩者結(jié)合使用，可以在gnss信號(hào)弱或受干擾的情況下，有效提升車輛在復(fù)雜環(huán)境下的定位精度。

40、本發(fā)明提供的無(wú)人駕駛車輛定位方法、系統(tǒng)、終端及存儲(chǔ)介質(zhì)，利用卡爾曼濾波算法對(duì)車輛定位的預(yù)測(cè)方程和觀測(cè)方程進(jìn)行融合，并基于噪聲數(shù)據(jù)構(gòu)建協(xié)方差矩陣，能夠更好地處理傳感器數(shù)據(jù)中的不確定性。即便傳感器采集的數(shù)據(jù)存在噪聲，卡爾曼濾波可以根據(jù)噪聲特性動(dòng)態(tài)調(diào)整定位結(jié)果，使得車輛即使在噪聲干擾較大的環(huán)境中也能維持高精度的定位，從而增強(qiáng)系統(tǒng)的魯棒性。

41、本發(fā)明提供的無(wú)人駕駛車輛定位方法、系統(tǒng)、終端及存儲(chǔ)介質(zhì)，通過卡爾曼濾波算法具有實(shí)時(shí)性強(qiáng)的優(yōu)勢(shì)。在車輛行駛過程中，系統(tǒng)能夠通過不斷更新車輛位置的預(yù)測(cè)值和觀測(cè)值，動(dòng)態(tài)調(diào)整車輛的定位結(jié)果。通過每一時(shí)刻對(duì)狀態(tài)方程和觀測(cè)方程的快速迭代計(jì)算，系統(tǒng)能夠快速得到當(dāng)前時(shí)刻的最優(yōu)估計(jì)值，從而保證無(wú)人駕駛車輛的實(shí)時(shí)定位需求，確保安全與高效運(yùn)行。

42、本發(fā)明提供的無(wú)人駕駛車輛定位方法、系統(tǒng)、終端及存儲(chǔ)介質(zhì)，通過一階泰勒展開將預(yù)測(cè)方程和觀測(cè)方程線性化，系統(tǒng)能夠適應(yīng)在動(dòng)態(tài)復(fù)雜的行駛環(huán)境中，車輛的運(yùn)動(dòng)狀態(tài)為非線性這一弊端。線性化的過程使得卡爾曼濾波能夠有效應(yīng)用于復(fù)雜的車輛運(yùn)動(dòng)模型中，確保系統(tǒng)能夠精確地追蹤車輛的位置和姿態(tài)變化。

43、本發(fā)明提供的無(wú)人駕駛車輛定位方法、系統(tǒng)、終端及存儲(chǔ)介質(zhì)，通過結(jié)合激光雷達(dá)的觀測(cè)數(shù)據(jù)，并利用卡爾曼濾波器進(jìn)行狀態(tài)更新，系統(tǒng)能夠有效修正由imu帶來(lái)的累積誤差。這種觀測(cè)校準(zhǔn)機(jī)制減少了定位誤差的積累，進(jìn)一步提高了長(zhǎng)期定位的穩(wěn)定性。此外，本發(fā)明設(shè)計(jì)原理可靠，結(jié)構(gòu)簡(jiǎn)單，具有非常廣泛的應(yīng)用前景。