使用到了gps傳感器、視覺里程計以及慣性導(dǎo)航，本發(fā)明只需要用到gps傳感器以及激光雷達，并且視覺里程計容易受到光照等因素的影響，魯棒性不及激光雷達。

上述專利為了實現(xiàn)建圖效果，根據(jù)不同路段gps的信號狀態(tài)，選擇gps的位姿還是慣導(dǎo)與視覺里程計的融合位姿作為Zui終的位姿，并沒有考慮到在gps信號極差的路段，由于慣性導(dǎo)航誤差的不斷積累會導(dǎo)致無人車的定位不夠jingque。而本發(fā)明考慮了gps信號的全部狀態(tài)，針對園區(qū)低速場景，采用多傳感器融合的方式，實現(xiàn)了無人車在園區(qū)內(nèi)任意場景下的jingque定位。

本發(fā)明依據(jù)低速園區(qū)無人車運行環(huán)境的特點(半開放、路程短、樓宇樹木遮擋嚴(yán)重等)，提出了激光雷達slam(simultaneouslocalizationandmapping)與gps(globalpositioningsystem)傳感器融合定位方法，解決了無人車在園區(qū)任意環(huán)境下slam的持續(xù)定位問題和激光雷達掃描匹配失敗后的快速重新匹配問題。

激光雷達可以生成三維的位置信息，快速確定物體的位置、大小，比攝像頭等傳感器，探測距離更遠(yuǎn)、jingque度更高，還有更靈敏的響應(yīng)速度，還不受環(huán)境光照的影響，但是激光雷達Zui大的缺陷，就是不能在雪雨等極端天氣下工作，在這種天氣下會導(dǎo)致無人車的“位置暈眩”。gps傳感器導(dǎo)航靈活性高，但需要接受衛(wèi)星信號，會有信號失真及丟失的情況，影響定位精度。單一傳感器在無人車的定位過程中都存在不足。

本發(fā)明采用gps傳感器和激光雷達傳感器融合定位的思路，獲得gps傳感器和激光雷達傳感器融合的車輛位姿信息，當(dāng)無人車在使用激光雷達slam定位算法ndt點云匹配失敗時，根據(jù)當(dāng)前的gps信號狀態(tài)替代點云匹配算法ndt算法的定位預(yù)測矩陣，實現(xiàn)無人車slam算法的持續(xù)定位，提高無人車在低速園區(qū)環(huán)境下的定位精度。本發(fā)明整體系統(tǒng)框架圖如圖4所示：

有益效果

本發(fā)明提出的多傳感器融合的定位方法，在激光雷達slam點云失配時，根據(jù)gps當(dāng)前的狀態(tài)情況，使用gps傳感器與激光雷達的融合位姿來替換激光雷達slam算法中的預(yù)測矩陣，可將重新匹配的搜索范圍大大縮小，實現(xiàn)重新的快速匹配，實現(xiàn)了園區(qū)無人車激光雷達slam算法的持續(xù)定位。本方法在園區(qū)低速無人駕駛車中得到了實驗驗證，在匹配精度以及匹配速度上可以取得很好的效果。

本發(fā)明基于激光雷達與gps傳感器融合實現(xiàn)激光雷達快速重新匹配的方法，為了保證激光雷達與gps傳感器坐標(biāo)一致性。由于激光雷達以及gps傳感器與車輛是剛性連接，二者的安裝位置已固定，對激光雷達以及gps傳感器的安裝位置進行標(biāo)定，使激光雷達坐標(biāo)系、gps坐標(biāo)系以及車輛坐標(biāo)系三者統(tǒng)一。本發(fā)明以車輛坐標(biāo)系為基準(zhǔn)，其中車輛坐標(biāo)系坐標(biāo)原點為車頂gps傳感器前天線位置，x軸方向指向車頭正前方，y軸正方向為x軸正方向逆時針旋轉(zhuǎn)90°，z軸正方向指向天空。采用旋轉(zhuǎn)平移矩陣的方法將gps傳感器以及激光雷達傳感器的坐標(biāo)原點轉(zhuǎn)換到車輛坐標(biāo)原點。本發(fā)明采用的gps導(dǎo)航裝置為雙天線導(dǎo)航，一般的雙天線導(dǎo)航后天線為定位天線，前天線為定向天線，本發(fā)明為了將雷達坐標(biāo)原點與導(dǎo)航定位點統(tǒng)一故將前天線連接到導(dǎo)航接收機的后天線位置，后天線連接到導(dǎo)航接收機的前天線位置，并且本發(fā)明只用到前天線。前天線位置即為車輛坐標(biāo)系原點。

激光雷達與gps傳感器的安裝位置關(guān)系如圖1所示。其中，激光雷達坐標(biāo)系，x軸正方向為雷達數(shù)據(jù)輸出線纜的相反方向，y軸正方向為x軸正方向逆時針旋轉(zhuǎn)90°，z軸正方向指向天空；假設(shè)雷達的安裝位置距地面高度為0.5米，距車輛坐標(biāo)系原點的前向位移為1.5米，水平方向無偏差，那么激光雷達坐標(biāo)系以及gps坐標(biāo)系向車輛坐標(biāo)系的轉(zhuǎn)化過程，以激光雷達為例，設(shè)定激光雷達三維坐標(biāo)系中的一個向量其中點p在xy平面、xz平面、yz平面的投影分別為點m、點p、點n，繞z轉(zhuǎn)旋轉(zhuǎn)θ角，旋轉(zhuǎn)矩陣為繞x轉(zhuǎn)旋轉(zhuǎn)θ角，旋轉(zhuǎn)矩陣為：繞y轉(zhuǎn)旋轉(zhuǎn)θ角，旋轉(zhuǎn)矩陣為：在z、x、y軸的平移矩陣為：

其中，激光雷達、gps傳感器與車輛坐標(biāo)系聯(lián)合標(biāo)定算法流程圖如圖2所示：

ndt(normaldistributionstransform)匹配算法介紹

在slam導(dǎo)航過程中，傳統(tǒng)的點云配準(zhǔn)算法一般為icp迭代的方法，這種方法一般需要提供一個較好的初值，也就是需要粗配準(zhǔn)，同時由于算法本身缺陷，Zui終迭代結(jié)果可能會陷入局部Zui優(yōu)，導(dǎo)致配準(zhǔn)失敗，效果往往不夠理想。本發(fā)明介紹的是另一種比較好的配準(zhǔn)算法，ndt配準(zhǔn)算法。所謂ndt就是正態(tài)分布變換，作用與icp一樣用來估計兩個點云之間的剛體變換。用標(biāo)準(zhǔn)Zui優(yōu)化技術(shù)來確定兩個點云間的Zui優(yōu)的匹配，因為其在配準(zhǔn)過程中不利用對應(yīng)點的特征計算和匹配，所以時間比其他方法快。這個配準(zhǔn)算法耗時穩(wěn)定，跟初值相關(guān)不大，初值誤差大時，也能很好的糾正過來。

瑞士KELLER系列產(chǎn)品及部分型號如下：

一，瑞士KELLER壓力傳感器

① 壓敏電阻OEM壓力傳感器： 7S / 9S系列、7L1系列、8系列、9系列

② 高科技微型壓力傳感器：2 Mi系列、M5系列、傳感器探頭

③ 壓敏電阻標(biāo)準(zhǔn)壓力傳感器：11系列、13系列、15系列

④ 經(jīng)濟型壓力傳感器：1 TAB系列、6 M / S系列、2系列、4系列

二，瑞士KELLER壓力變送器

① 工業(yè)用壓力變送器：PD-39 X系列、41 X系列、22 DT系列、PRD-33 X 系列、33 X / 35 X系列、21 G EOL系列、23 / 25系列、22 M/S系列

② 危險場所用壓力變送器：23 Ed/25 Ed, 33 X Ed/35 X Ed、33 X Ei / 35 X Ei / 36 XW Ei、23 SY Ei / 25 Y Ei / 26 Y Ei、PD-39 X Ei系列、41 X Ei系列、46 X Ei系列

③ 被動RFID界面壓力變送器：21 D RFID系列

ndt配準(zhǔn)算法步驟如下：

將參考點云(referencescan)所占的空間劃分成指定大小(cellsize)的網(wǎng)格或體素(voxel)，并計算每個網(wǎng)格的多維正態(tài)分布參數(shù)，激光雷達的原始點云以及點云在ndt算法下的空間劃分效果如圖三所示。

(2)初始化變換參數(shù)初始化變換參數(shù)(賦予零值或者使用里程計數(shù)據(jù)賦值)。

(3)對于要配準(zhǔn)的點云(secondscan)，通過變換t將其轉(zhuǎn)換到參考點云的網(wǎng)格中xi′＝t(xi,p)

根據(jù)正態(tài)分布參數(shù)計算每個轉(zhuǎn)換點的概率密度

ndt配準(zhǔn)得分(score)通過對每個網(wǎng)格計算出的概率密度相加得到

根據(jù)牛頓優(yōu)化算法對目標(biāo)函數(shù)-score-行優(yōu)化，即尋找變換參數(shù)p使得score的值Zui大。

跳轉(zhuǎn)到第3步繼續(xù)執(zhí)行，直到達到收斂條件為止。

gps、激光雷達傳感器融合位姿

本發(fā)明在實驗過程中，首先要對gps、激光雷達接收到的數(shù)據(jù)進行數(shù)據(jù)預(yù)處理，濾波處理以及剔除噪點，無人車的狀態(tài)可以表示為

其中px,py代表無人車當(dāng)前的位置，代表無人車的航向角信息。gps傳感器可以檢測無人車的當(dāng)前位置，沒有速度信息。其測量值z＝(gx,gy)，激光雷達傳感器可以檢測到距離、航向角、速度信息，其測量值表示為gps與激光雷達位姿融合流程圖如圖5所示：

首先，對數(shù)據(jù)進行初始化，在收到傳感器返回的測量值時，對無人車的狀態(tài)進行初始化，初始化默認(rèn)車輛當(dāng)前處于原點而且速度值為零，同時加上對時間的更新。

gps傳感器處理過程如公式4所示：

在gps的測量值z＝(gx,gy)左乘一個系數(shù)矩陣，gps傳感器處理之后的結(jié)果為無人車此時的位置坐標(biāo)。

激光雷達傳感器處理過程如公式5所示，通過將激光雷達的測量值解算，可以得到當(dāng)前無人車的位置坐標(biāo)，以及無人車在x、y軸的速度大小。

將兩種傳感器處理后的結(jié)果進行融合，無人車的位姿表示為

經(jīng)過gps以及激光雷達傳感器數(shù)據(jù)解算后，得到來自激光雷達和gps傳感器解算的無人車位置坐標(biāo)，以及來自激光雷達表示的無人車航向角信息，當(dāng)激光雷達slam定位算法失配時，若gps傳感器接受信號狀態(tài)良好，則將gps傳感器解析的位置坐標(biāo)以及激光雷達解析的航向角作為Zui終的融合位姿對ndt算法的預(yù)測矩陣進行替換。若gps傳感器接受信號狀態(tài)差，則將激光雷達傳感器解析出來的位置坐標(biāo)以及航向角作為Zui終的融合位姿對ndt算法的預(yù)測矩陣進行替換。

由于無人車處于持續(xù)運動中，故還可以使用卡爾曼濾波算法預(yù)測下一位置，利用預(yù)測后的信息計算位姿矩陣，達到矯正位姿的作用。兩種傳感器在預(yù)測無人車下一時刻的狀態(tài)時，使用卡爾曼濾波算法預(yù)測下一位置，具體如下

位姿預(yù)測狀態(tài)方程：

其中，x為無人車的狀態(tài)，f是系統(tǒng)的控制函數(shù)，表示系統(tǒng)的改變，在這里，只考慮簡單的線性問題，p代表無人車當(dāng)前的位姿，q代表系統(tǒng)誤差。