本發(fā)明涉及一種定位系統(tǒng)及方法，具體為一種協(xié)同導(dǎo)航定位系統(tǒng)及導(dǎo)航定位方法，屬于導(dǎo)航技術(shù)領(lǐng)域。

背景技術(shù)：

現(xiàn)階段的導(dǎo)航定位方法有很多種，根據(jù)傳感器分類(lèi)可分為，衛(wèi)星導(dǎo)航(gps)、視覺(jué)傳感器、里程計(jì)、慣性導(dǎo)航元件、激光雷達(dá)等等。其中g(shù)ps傳感器只能在開(kāi)闊無(wú)遮擋的室外環(huán)境下工作；視覺(jué)傳感器能夠提供豐富并直觀(guān)的信息，并且成本較低但往往獲得的數(shù)據(jù)噪聲較大，不適用與環(huán)境紋理相似和低光照的環(huán)境；激光雷達(dá)在測(cè)距范圍和方向上都有著較高的精度，但其只適用于結(jié)構(gòu)化室內(nèi)環(huán)境，不適用與空曠的室外環(huán)境；慣性導(dǎo)航元件是一種相對(duì)定位傳感器，通過(guò)積分計(jì)算得到相對(duì)位置，但其存在著較大的累積誤差以及無(wú)法提供絕對(duì)位置，不適用于長(zhǎng)時(shí)間的精確定位。因此，各個(gè)傳感器都有其自己的局限性，以及特定的應(yīng)用環(huán)境。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的就在于為了解決上述問(wèn)題而提供一種協(xié)同導(dǎo)航定位系統(tǒng)及導(dǎo)航定位方法。

本發(fā)明通過(guò)以下技術(shù)方案來(lái)實(shí)現(xiàn)上述目的：一種協(xié)同導(dǎo)航定位系統(tǒng)及導(dǎo)航定位方法，包括

傳感器數(shù)據(jù)采集模塊，負(fù)責(zé)采集傳感器原始數(shù)據(jù)并整合傳輸給其他模塊；

rgb-d圖像生成模塊，融合單目視覺(jué)傳感器以及激光雷達(dá)的數(shù)據(jù)，生成rgb-d圖像；

運(yùn)動(dòng)學(xué)建模模塊，對(duì)搭載所述系統(tǒng)的移動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模；

零偏修正模塊，融合gps，慣性導(dǎo)航元件數(shù)據(jù)，根據(jù)運(yùn)動(dòng)學(xué)模型實(shí)時(shí)修正慣性導(dǎo)航元件零點(diǎn)漂移量；

位姿跟蹤模塊，接收傳感器采集模塊、rgb–d圖像生成模塊、零偏修正模塊以及人工路標(biāo)模塊的信息實(shí)時(shí)估計(jì)系統(tǒng)自身位姿；

全局優(yōu)化模塊，負(fù)責(zé)閉環(huán)檢測(cè)并優(yōu)化系統(tǒng)全局位姿；

地圖存儲(chǔ)模塊，存儲(chǔ)地圖信息以供重復(fù)使用。

優(yōu)選的，所述傳感器數(shù)據(jù)采集模塊負(fù)責(zé)采集傳感器原始數(shù)據(jù)并整合傳輸給其他模塊，包括：采集gps、單目視覺(jué)傳感器、慣性導(dǎo)航元件、激光雷達(dá)的數(shù)據(jù)并整合傳輸給上述rgb-d圖像生成模塊、人工路標(biāo)模塊、零偏修正模塊、位姿跟蹤模塊。

優(yōu)選的，所述人工路標(biāo)模塊在室內(nèi)環(huán)境下設(shè)立人工路標(biāo)，并根據(jù)單目視覺(jué)傳感器的觀(guān)測(cè)計(jì)算出系統(tǒng)自身距離路標(biāo)的相對(duì)位置，包括：采用apriltags二維碼作為人工路標(biāo)，采用pnp的方法計(jì)算單目視覺(jué)傳感器距離路標(biāo)的相對(duì)位置

優(yōu)選的，所述運(yùn)動(dòng)學(xué)建模模塊對(duì)搭載所述系統(tǒng)的移動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模，包括：根據(jù)ackerman模型對(duì)輪式機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模，ωv＝v/r，vx＝r(1-cos(ωv))，vy＝rsin(ωv)，其中l(wèi)為輪式機(jī)器人前后軸距，δ為前輪轉(zhuǎn)角，ωv為機(jī)器人旋轉(zhuǎn)角速度，vx為機(jī)器人橫向速度，vy為機(jī)器人縱向速度。

優(yōu)選的，所述零偏修正模塊融合gps，慣性導(dǎo)航元件數(shù)據(jù)，根據(jù)運(yùn)動(dòng)學(xué)模型實(shí)時(shí)修正慣性導(dǎo)航元件零點(diǎn)漂移量，包括：根據(jù)gps和運(yùn)動(dòng)學(xué)模型vx，vy和ωv的約束，利用卡爾曼濾波修正慣性導(dǎo)航元件的零點(diǎn)漂移量

優(yōu)選的，所述位姿跟蹤模塊接收傳感器采集模塊、rgb–d圖像生成模塊、零偏修正模塊以及人工路標(biāo)模塊的信息實(shí)時(shí)估計(jì)系統(tǒng)自身位姿，包括：位姿跟蹤模塊根據(jù)rgb-d圖像提取3d特征點(diǎn)，用icp法算出單目視覺(jué)傳感器的位姿并采用levenberg-marquardt最小二乘法優(yōu)化誤差函數(shù)argmin(egps+eimu+eat+ecamera)得到最優(yōu)位姿估計(jì)；其中，

其中，egps為gps與單目視覺(jué)傳感器的誤差函數(shù)，eimu為慣性導(dǎo)航元件積分誤差函數(shù)，eat為人工路標(biāo)誤差函數(shù)，ecamera為3d特征點(diǎn)重投影誤差函數(shù)；為i時(shí)刻單目視覺(jué)傳感器的位姿，為i-1時(shí)刻單目視覺(jué)傳感器的位姿，為單目視覺(jué)傳感器在i-1時(shí)刻到i時(shí)刻位姿的增量；為i時(shí)刻gps傳感器的位姿；為慣性導(dǎo)航元件在i時(shí)刻到i+1時(shí)刻位姿的增量，為慣性導(dǎo)航元件在i時(shí)刻到i+1時(shí)刻位姿的零點(diǎn)偏移量；為在i時(shí)刻到i+1時(shí)刻，單目視覺(jué)傳感器距離人工路標(biāo)的位姿的增量；為3d特征點(diǎn)j在相機(jī)圖像坐標(biāo)系下的坐標(biāo)，為3d特征點(diǎn)j在世界坐標(biāo)系下的坐標(biāo)，函數(shù)π(*)為重投影函數(shù)，將世界坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換為圖像坐標(biāo)系；∑gps為gps信息矩陣，∑imu為慣性導(dǎo)航元件信息矩陣，∑at為人工路標(biāo)信息矩陣，∑camera為3d特征點(diǎn)重投影信息矩陣。上述信息矩陣均服從正態(tài)分布概率模型，當(dāng)在某種特殊環(huán)境下，部分傳感器失效誤差增大時(shí)，例如室內(nèi)丟失gps衛(wèi)星信號(hào)以及室外缺少人工路標(biāo)點(diǎn)，其對(duì)應(yīng)的信息矩陣會(huì)減小，以始終保持目標(biāo)函數(shù)最小，提升系統(tǒng)的通用性以及魯棒性。

優(yōu)選的，所述全局優(yōu)化模塊負(fù)責(zé)閉環(huán)檢測(cè)并優(yōu)化系統(tǒng)全局位姿，包括：采用bag-of-words模型完成閉環(huán)檢測(cè)的任務(wù)，并levenberg-marquardt最小二乘法優(yōu)化閉環(huán)內(nèi)自身位姿以及3d特征點(diǎn)。

優(yōu)選的，地圖存儲(chǔ)模塊存儲(chǔ)地圖信息以供重復(fù)使用，包括：存儲(chǔ)軌跡信息，人工路標(biāo)點(diǎn)位姿信息，3d特征點(diǎn)坐標(biāo)信息。

本發(fā)明的有益效果是：該協(xié)同導(dǎo)航定位系統(tǒng)及導(dǎo)航定位方法設(shè)計(jì)合理，結(jié)合了各個(gè)傳感器的優(yōu)點(diǎn)，提升其精度以及適用范圍，能夠在各種復(fù)雜的環(huán)境下正常工作，具有定位速度快，定位精度高，魯棒性強(qiáng)，適用范圍廣等優(yōu)點(diǎn)。

附圖說(shuō)明

圖1為本發(fā)明結(jié)構(gòu)流程示意圖。

具體實(shí)施方式

下面將結(jié)合本發(fā)明實(shí)施例中的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒(méi)有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

請(qǐng)參閱圖1，一種協(xié)同導(dǎo)航定位系統(tǒng)及導(dǎo)航定位方法，包括

傳感器數(shù)據(jù)采集模塊，負(fù)責(zé)采集傳感器原始數(shù)據(jù)并整合傳輸給其他模塊；

rgb-d圖像生成模塊，融合單目視覺(jué)傳感器以及激光雷達(dá)的數(shù)據(jù)，生成rgb-d圖像；

運(yùn)動(dòng)學(xué)建模模塊，對(duì)搭載所述系統(tǒng)的移動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模；

零偏修正模塊，融合gps，慣性導(dǎo)航元件數(shù)據(jù)，根據(jù)運(yùn)動(dòng)學(xué)模型實(shí)時(shí)修正慣性導(dǎo)航元件零點(diǎn)漂移量；

位姿跟蹤模塊，接收傳感器采集模塊、rgb–d圖像生成模塊、零偏修正模塊以及人工路標(biāo)模塊的信息實(shí)時(shí)估計(jì)系統(tǒng)自身位姿；

全局優(yōu)化模塊，負(fù)責(zé)閉環(huán)檢測(cè)并優(yōu)化系統(tǒng)全局位姿；

地圖存儲(chǔ)模塊，存儲(chǔ)地圖信息以供重復(fù)使用。

進(jìn)一步，所述傳感器數(shù)據(jù)采集模塊負(fù)責(zé)采集傳感器原始數(shù)據(jù)并整合傳輸給其他模塊，包括：采集gps、單目視覺(jué)傳感器、慣性導(dǎo)航元件、激光雷達(dá)的數(shù)據(jù)并整合傳輸給上述rgb-d圖像生成模塊、人工路標(biāo)模塊、零偏修正模塊、位姿跟蹤模塊。

進(jìn)一步，所述人工路標(biāo)模塊在室內(nèi)環(huán)境下設(shè)立人工路標(biāo)，并根據(jù)單目視覺(jué)傳感器的觀(guān)測(cè)計(jì)算出系統(tǒng)自身距離路標(biāo)的相對(duì)位置，包括：采用apriltags二維碼作為人工路標(biāo)，采用pnp的方法計(jì)算單目視覺(jué)傳感器距離路標(biāo)的相對(duì)位置

進(jìn)一步，所述運(yùn)動(dòng)學(xué)建模模塊對(duì)搭載所述系統(tǒng)的移動(dòng)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模，包括：根據(jù)ackerman模型對(duì)輪式機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模，ωv＝v/r，vx＝r(1-cos(ωv))，vy＝rsin(ωv)，其中l(wèi)為輪式機(jī)器人前后軸距，δ為前輪轉(zhuǎn)角，ωv為機(jī)器人旋轉(zhuǎn)角速度，vx為機(jī)器人橫向速度，vy為機(jī)器人縱向速度。

進(jìn)一步，所述零偏修正模塊融合gps，慣性導(dǎo)航元件數(shù)據(jù)，根據(jù)運(yùn)動(dòng)學(xué)模型實(shí)時(shí)修正慣性導(dǎo)航元件零點(diǎn)漂移量，包括：根據(jù)gps和運(yùn)動(dòng)學(xué)模型vx，vy和ωv的約束，利用卡爾曼濾波修正慣性導(dǎo)航元件的零點(diǎn)漂移量

進(jìn)一步，所述位姿跟蹤模塊接收傳感器采集模塊、rgb–d圖像生成模塊、零偏修正模塊以及人工路標(biāo)模塊的信息實(shí)時(shí)估計(jì)系統(tǒng)自身位姿，包括：位姿跟蹤模塊根據(jù)rgb-d圖像提取3d特征點(diǎn)，用icp法算出單目視覺(jué)傳感器的位姿并采用levenberg-marquardt最小二乘法優(yōu)化誤差函數(shù)argmin(egps+eimu+eat+ecamera)得到最優(yōu)位姿估計(jì)；其中，

其中，egps為gps與單目視覺(jué)傳感器的誤差函數(shù)，eimu為慣性導(dǎo)航元件積分誤差函數(shù)，eat為人工路標(biāo)誤差函數(shù)，ecamera為3d特征點(diǎn)重投影誤差函數(shù)；為i時(shí)刻單目視覺(jué)傳感器的位姿，為i-1時(shí)刻單目視覺(jué)傳感器的位姿，為單目視覺(jué)傳感器在i-1時(shí)刻到i時(shí)刻位姿的增量；為i時(shí)刻gps傳感器的位姿；為慣性導(dǎo)航元件在i時(shí)刻到i+1時(shí)刻位姿的增量，為慣性導(dǎo)航元件在i時(shí)刻到i+1時(shí)刻位姿的零點(diǎn)偏移量；為在i時(shí)刻到i+1時(shí)刻，單目視覺(jué)傳感器距離人工路標(biāo)的位姿的增量；為3d特征點(diǎn)j在相機(jī)圖像坐標(biāo)系下的坐標(biāo)，為3d特征點(diǎn)j在世界坐標(biāo)系下的坐標(biāo)，函數(shù)π(*)為重投影函數(shù)，將世界坐標(biāo)系下的坐標(biāo)轉(zhuǎn)換為圖像坐標(biāo)系；∑gps為gps信息矩陣，∑imu為慣性導(dǎo)航元件信息矩陣，∑at為人工路標(biāo)信息矩陣，∑camera為3d特征點(diǎn)重投影信息矩陣。上述信息矩陣均服從正態(tài)分布概率模型，當(dāng)在某種特殊環(huán)境下，部分傳感器失效誤差增大時(shí)，例如室內(nèi)丟失gps衛(wèi)星信號(hào)以及室外缺少人工路標(biāo)點(diǎn)，其對(duì)應(yīng)的信息矩陣會(huì)減小，以始終保持目標(biāo)函數(shù)最小，提升系統(tǒng)的通用性以及魯棒性。

進(jìn)一步，所述全局優(yōu)化模塊負(fù)責(zé)閉環(huán)檢測(cè)并優(yōu)化系統(tǒng)全局位姿，包括：采用bag-of-words模型完成閉環(huán)檢測(cè)的任務(wù)，并levenberg-marquardt最小二乘法優(yōu)化閉環(huán)內(nèi)自身位姿以及3d特征點(diǎn)。

進(jìn)一步，地圖存儲(chǔ)模塊存儲(chǔ)地圖信息以供重復(fù)使用，包括：存儲(chǔ)軌跡信息，人工路標(biāo)點(diǎn)位姿信息，3d特征點(diǎn)坐標(biāo)信息。

案例一，公交車(chē)遠(yuǎn)程監(jiān)控

為了完成車(chē)輛在多工況下的實(shí)時(shí)高精度定位，我們?cè)趥鹘y(tǒng)的gps定位結(jié)合lte共享位置的單一模式下，增加攝像頭或者激光傳感器。在gps覆蓋較好的的線(xiàn)路上，使用松耦合的解決方案，即gps定位和視覺(jué)定位兩套系統(tǒng)并行機(jī)制，在gps定位失鎖后自動(dòng)切換為視覺(jué)定位模式，這種做法算法簡(jiǎn)單，運(yùn)算量小，易于降低成本。適合線(xiàn)路中定位條件相對(duì)可控的情況，考慮到系統(tǒng)在切換過(guò)程中的定位精度已經(jīng)下降，因此就需要將路線(xiàn)上的gps定位情況作為先驗(yàn)知識(shí)輸入系統(tǒng)，使得系統(tǒng)在不損失定位精度的情況下及時(shí)完成切換。在gps覆蓋不穩(wěn)定的或者環(huán)境變化相對(duì)較大線(xiàn)路中，使用松耦合的方式將無(wú)法穩(wěn)定輸出高精度定位結(jié)果，因此我們將使用緊耦合的算法，時(shí)時(shí)刻刻將gps、imu(inertialmeasurementunit，慣性測(cè)量單元)、攝像頭和激光等傳感器加入定位優(yōu)化模型，融合多傳感器模型最小化定位誤差損失函數(shù)，來(lái)保證穩(wěn)定可靠的實(shí)時(shí)高精度定位輸出。通過(guò)對(duì)車(chē)輛的準(zhǔn)確定位跟蹤，云端的系統(tǒng)就可以建立出司機(jī)和車(chē)輛運(yùn)行的動(dòng)作庫(kù)，評(píng)價(jià)某位司機(jī)和某個(gè)車(chē)型的駕駛模型，得出某位司機(jī)的駕駛風(fēng)格在某個(gè)車(chē)型上的表現(xiàn)，是否存在違規(guī)駕駛、是否會(huì)對(duì)車(chē)輛造成損害，并及時(shí)作出調(diào)整。借助對(duì)抗生成學(xué)習(xí)網(wǎng)絡(luò)，正確的駕駛模型將被應(yīng)用于訓(xùn)練生成對(duì)應(yīng)的無(wú)人駕駛控制模型，以輔助車(chē)輛在完成靠邊停車(chē)、車(chē)道保持、自適應(yīng)巡航和緊急停車(chē)等情況下的車(chē)輛自動(dòng)接管。

案例二，“最后一公里”短程自主泊車(chē)

隨著世界汽車(chē)保有量的迅猛增加，各類(lèi)交通事故也越來(lái)越多的出現(xiàn)在不同媒體報(bào)道中。交通事故不僅給人們帶來(lái)嚴(yán)重的經(jīng)濟(jì)損失，更直接威脅到車(chē)輛駕駛員及乘客的人身安全。其中，在泊車(chē)過(guò)程中產(chǎn)生的交通事故也是不容忽視的。由于視線(xiàn)不好或者駕駛技術(shù)不熟練都可能導(dǎo)致一些悲劇的發(fā)生。另一方面，隨著車(chē)輛的急劇增加，人們?cè)诔鲂兄型＼?chē)難問(wèn)題也變的日益明顯尤其在一、二線(xiàn)城市中，該問(wèn)題顯得更為突出。停車(chē)難問(wèn)題已經(jīng)成為城市亟待解決的難題之一，其直接導(dǎo)致了大量的亂停車(chē)行為，對(duì)市容有很大的影響，同時(shí)也產(chǎn)生了極大的安全隱患。如果有一種技術(shù)可以讓泊車(chē)的過(guò)程達(dá)到高精度和自動(dòng)化，從尋找車(chē)位、確認(rèn)停車(chē)位、泊車(chē)到自動(dòng)泊出全程不需要駕駛員參與，且保證零事故率，一方面也保證了車(chē)輛、駕駛員及周?chē)腥说陌踩?，另一方面提高人們的出行效率，同時(shí)還可以最大化利用停車(chē)場(chǎng)有限土地資源，上述幾個(gè)問(wèn)題可以迎刃而解。定位問(wèn)題作為此項(xiàng)功能的核心技術(shù)之一，考慮到車(chē)庫(kù)包括地面和地下，光照和工況彼此差別巨大，通過(guò)傳統(tǒng)的slam定位算法無(wú)法統(tǒng)一解決這個(gè)問(wèn)題，使用本專(zhuān)利則能很好的解決。

對(duì)于本領(lǐng)域技術(shù)人員而言，顯然本發(fā)明不限于上述示范性實(shí)施例的細(xì)節(jié)，而且在不背離本發(fā)明的精神或基本特征的情況下，能夠以其他的具體形式實(shí)現(xiàn)本發(fā)明。因此，無(wú)論從哪一點(diǎn)來(lái)看，均應(yīng)將實(shí)施例看作是示范性的，而且是非限制性的，本發(fā)明的范圍由所附權(quán)利要求而不是上述說(shuō)明限定，因此旨在將落在權(quán)利要求的等同要件的含義和范圍內(nèi)的所有變化囊括在本發(fā)明內(nèi)。不應(yīng)將權(quán)利要求中的任何附圖標(biāo)記視為限制所涉及的權(quán)利要求。

此外，應(yīng)當(dāng)理解，雖然本說(shuō)明書(shū)按照實(shí)施方式加以描述，但并非每個(gè)實(shí)施方式僅包含一個(gè)獨(dú)立的技術(shù)方案，說(shuō)明書(shū)的這種敘述方式僅僅是為清楚起見(jiàn)，本領(lǐng)域技術(shù)人員應(yīng)當(dāng)將說(shuō)明書(shū)作為一個(gè)整體，各實(shí)施例中的技術(shù)方案也可以經(jīng)適當(dāng)組合，形成本領(lǐng)域技術(shù)人員可以理解的其他實(shí)施方式。