本發(fā)明涉及導(dǎo)航,尤其是指一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法和系統(tǒng)。
背景技術(shù):
1、慣性導(dǎo)航系統(tǒng)(ins)是adas以及自動(dòng)駕駛領(lǐng)域環(huán)境感知與導(dǎo)航這一塊重要的、作用無法被替代的核心傳感器模塊,其三維姿態(tài)信息輸出以及相對和絕對位置推算不依賴任何外部設(shè)備,不需要任何外部信號,它可以被安裝在汽車底盤等隱蔽位置,這樣就可以避免電子或機(jī)械的攻擊。ins可以驗(yàn)證gnss結(jié)果的自洽性,并對無法自洽的絕對定位數(shù)據(jù)進(jìn)行濾波和修正。ins還可以在gnss信號消失之后,仍然提供持續(xù)若干秒的亞米級定位精度,為自動(dòng)駕駛汽車爭取寶貴的異常處理的時(shí)間。同樣的道理,ins也可以在相對定位失效時(shí),對相對定位的結(jié)果進(jìn)行航跡推演,在一段時(shí)間內(nèi)保持相對定位的精度;例如,在車道線識別模塊失效時(shí),利用失效之前感知到的道路信息和ins對汽車航跡的推算,仍然能夠讓汽車?yán)^續(xù)在車道內(nèi)行駛。可以說ins是自動(dòng)駕駛中的最后一道安全防線。ins的關(guān)鍵技術(shù)主要在于算法的設(shè)計(jì)實(shí)現(xiàn)。
2、當(dāng)前主流的組合導(dǎo)航算法基本流程,從imu獲取六軸角速度、加速度信息,進(jìn)行捷聯(lián)慣導(dǎo)更新,再從gnss模塊獲取位置、速度、航向信息作為觀測量,進(jìn)行卡爾曼濾波更新。同時(shí)在gnss失鎖時(shí)候,采用車體can信息進(jìn)行融合。該方法在imu性能較好(零偏、隨機(jī)游走)時(shí)候,適用一般的車載慣性導(dǎo)航。但是應(yīng)用方式頗為死板,不能充分發(fā)揮gnss、imu、can的性能。
3、需要說明的是,在上述背景技術(shù)部分公開的信息僅用于加強(qiáng)對本公開的背景的理解,因此可以包括不構(gòu)成對本領(lǐng)域普通技術(shù)人員已知的現(xiàn)有技術(shù)的信息。
技術(shù)實(shí)現(xiàn)思路
1、為此,本發(fā)明所要解決的技術(shù)問題在于當(dāng)前主流的組合導(dǎo)航算法基本流程,從imu獲取六軸角速度、加速度信息,進(jìn)行捷聯(lián)慣導(dǎo)更新,再從gnss模塊獲取位置、速度、航向信息作為觀測量,進(jìn)行卡爾曼濾波更新。同時(shí)在gnss失鎖時(shí)候,采用車體can信息進(jìn)行融合。該方法在imu性能較好(零偏、隨機(jī)游走)時(shí)候,適用一般的車載慣性導(dǎo)航。但是應(yīng)用方式頗為死板,不能充分發(fā)揮gnss、imu、can的性能的問題。
2、為解決上述技術(shù)問題,本發(fā)明的第一方面提供了一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法,所述方法包括:
3、獲取觀測量;
4、根據(jù)所述觀測量對觀測量誤差矩陣進(jìn)行自適應(yīng)調(diào)整得到自適應(yīng)矩陣;
5、判斷所述自適應(yīng)矩陣是否包括速度、位置、航向調(diào)整量測更新矩陣,得到判斷結(jié)果;
6、根據(jù)所述判斷結(jié)果和所述測量數(shù)據(jù)建立卡爾曼濾波過程;
7、根據(jù)所述卡爾曼濾波過程得到零偏補(bǔ)償數(shù)據(jù);
8、所述零偏補(bǔ)償數(shù)據(jù)通過平滑濾波方法得到最優(yōu)零偏補(bǔ)償值。
9、在本發(fā)明的一個(gè)實(shí)施例中,獲取觀測量之前的步驟包括:
10、通過pps信號對arm系統(tǒng)進(jìn)行授時(shí),以使imu信息和can信息的時(shí)間戳一致。
11、在本發(fā)明的一個(gè)實(shí)施例中,判斷所述自適應(yīng)矩陣是否包括速度、位置、航向調(diào)整量測更新矩陣,得到判斷結(jié)果的步驟包括:
12、對所述自適應(yīng)矩陣進(jìn)行標(biāo)識識別;
13、若存在預(yù)設(shè)標(biāo)識,則所述判斷結(jié)果為包括量測數(shù)據(jù)更新;
14、若不存在預(yù)設(shè)標(biāo)識,則所述判斷結(jié)果為不包括量測數(shù)據(jù)更新。
15、在本發(fā)明的一個(gè)實(shí)施例中,根據(jù)所述判斷結(jié)果對所述測量數(shù)據(jù)進(jìn)行卡爾曼濾波處理得到零偏補(bǔ)償數(shù)據(jù)的步驟包括:
16、若所述判斷結(jié)果為包括量測數(shù)據(jù)更新,則根據(jù)所述判斷結(jié)果和所述觀測量數(shù)據(jù)建立卡爾曼濾波過程。
17、在本發(fā)明的一個(gè)實(shí)施例中,根據(jù)所述判斷結(jié)果和所述測量數(shù)據(jù)建立卡爾曼濾波過程的步驟包括:
18、獲取輪速;
19、將所述輪速作為狀態(tài)量,所述測量數(shù)據(jù)作為備用觀測量用于建立所述卡爾曼濾波過程。
20、在本發(fā)明的一個(gè)實(shí)施例中,所述零偏補(bǔ)償數(shù)據(jù)通過平滑濾波方法得到最優(yōu)零偏補(bǔ)償值的步驟還包括:
21、對所述零偏補(bǔ)償數(shù)據(jù)進(jìn)行篩選得到異常數(shù)據(jù);
22、根據(jù)所述平滑濾波方法對所述異常數(shù)據(jù)進(jìn)行處理,得到最優(yōu)零偏補(bǔ)償值。
23、在本發(fā)明的一個(gè)實(shí)施例中,所述觀測量包括位置、速度和航向。
24、本發(fā)明的第二方面提供了一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償系統(tǒng),所述系統(tǒng)包括:gnss觀測量精度評估模塊、can信息精度評估模塊和imu零偏補(bǔ)償模塊;
25、所述gnss觀測量精度評估模塊配置為:獲取觀測量;根據(jù)所述觀測量對觀測量誤差矩陣進(jìn)行自適應(yīng)調(diào)整得到自適應(yīng)矩陣;
26、所述can信息精度評估模塊被配置為:判斷所述自適應(yīng)矩陣是否包括速度、位置、航向調(diào)整量測更新矩陣,得到判斷結(jié)果;根據(jù)所述判斷結(jié)果和所述測量數(shù)據(jù)建立卡爾曼濾波過程;
27、所述imu零偏補(bǔ)償模塊被配置為:根據(jù)所述卡爾曼濾波過程得到零偏補(bǔ)償數(shù)據(jù);所述零偏補(bǔ)償數(shù)據(jù)通過平滑濾波方法得到最優(yōu)零偏補(bǔ)償值。
28、本發(fā)明的第三方面提供了一種電子設(shè)備,包括存儲(chǔ)器、處理器及存儲(chǔ)在存儲(chǔ)器上并可在處理器上運(yùn)行的計(jì)算機(jī)程序,所述處理器執(zhí)行所述程序時(shí)實(shí)現(xiàn)上述第一方面或第一方面中任一種可能的實(shí)施方式中所述的方法。
29、本發(fā)明的第四方面提供了一種非暫態(tài)計(jì)算機(jī)可讀存儲(chǔ)介質(zhì),其上存儲(chǔ)有計(jì)算機(jī)程序,該計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)上述第一方面或第一方面中任一種可能的實(shí)施方式中所述的方法。
30、本發(fā)明的上述技術(shù)方案相比現(xiàn)有技術(shù)具有以下優(yōu)點(diǎn):
31、本發(fā)明所述的一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法及系統(tǒng),通過根據(jù)組合導(dǎo)航系統(tǒng)的gnss模塊、imu信息、can信息的協(xié)方差精度、置信度等,自適應(yīng)匹配融合算法中的卡爾曼濾波參數(shù),同時(shí)對輪速、載體與imu夾角等因素根據(jù)速度協(xié)方差、組合導(dǎo)航速度與輪速對比等進(jìn)行自適應(yīng)匹配。可以保證該算法適用于任何組合導(dǎo)航硬件設(shè)備及各類使用場景。
1.一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法,其特征在于,所述方法包括:
2.根據(jù)權(quán)利要求1所述的一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法,其特征在于,獲取觀測量之前的步驟包括:
3.根據(jù)權(quán)利要求1所述的一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法,其特征在于,判斷所述自適應(yīng)矩陣是否包括速度、位置、航向調(diào)整量測更新矩陣,得到判斷結(jié)果的步驟包括:
4.根據(jù)權(quán)利要求3所述的一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法,其特征在于,根據(jù)所述判斷結(jié)果對所述測量數(shù)據(jù)進(jìn)行卡爾曼濾波處理得到零偏補(bǔ)償數(shù)據(jù)的步驟包括:
5.根據(jù)權(quán)利要求1所述的一種基于基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法,其特征在于,根據(jù)所述判斷結(jié)果和所述測量數(shù)據(jù)建立卡爾曼濾波過程的步驟包括:
6.根據(jù)權(quán)利要求1所述的一種基于基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法,其特征在于,所述零偏補(bǔ)償數(shù)據(jù)通過平滑濾波方法得到最優(yōu)零偏補(bǔ)償值的步驟還包括:
7.根據(jù)權(quán)利要求1所述的一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法,其特征在于:所述觀測量包括位置、速度和航向。
8.一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償系統(tǒng),其特征在于,所述系統(tǒng)包括:gnss觀測量精度評估模塊、can信息精度評估模塊和imu零偏補(bǔ)償模塊;
9.一種電子設(shè)備,包括存儲(chǔ)器、處理器及存儲(chǔ)在存儲(chǔ)器上并可在處理器上運(yùn)行的計(jì)算機(jī)程序,其特征在于,所述處理器執(zhí)行所述程序時(shí)實(shí)現(xiàn)如權(quán)利要求1至7任一項(xiàng)所述一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法的步驟。
10.一種非暫態(tài)計(jì)算機(jī)可讀存儲(chǔ)介質(zhì),其上存儲(chǔ)有計(jì)算機(jī)程序,其特征在于,該計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)如權(quán)利要求1至7任一項(xiàng)所述一種基于組合導(dǎo)航的自適應(yīng)補(bǔ)償方法的步驟。