本發(fā)明涉及一種無人車自主定位方法。

背景技術(shù)：

現(xiàn)有技術(shù)狀況及其存在缺陷

隨著計算機(jī)技術(shù)和人工智能的發(fā)展，無人駕駛汽車(以下簡稱無人車)在軍事、交通、工業(yè)生產(chǎn)、物流倉儲、日常生活等方面展現(xiàn)出巨大的應(yīng)用前景。在國防軍事方面，無人車多用于執(zhí)行危險場景下的軍事任務(wù)，如軍事救援和物資輸送等。在交通安全方面，無人駕駛技術(shù)是促進(jìn)智能交通系統(tǒng)發(fā)展的有效手段，基于人工智能的無人駕駛技術(shù)可以提高車輛行駛的主動安全性，能夠有效減少駕駛員由于誤操作導(dǎo)致的交通事故，從而提高交通行駛效率和安全性。在工業(yè)生產(chǎn)、物流倉儲方面，無人車可以配合自動化生產(chǎn)線實(shí)現(xiàn)全自主無人生產(chǎn)，進(jìn)一步推進(jìn)工業(yè)生產(chǎn)的自動化和智能化，進(jìn)而提高生產(chǎn)效率。另外，無人車的出現(xiàn)也將極大地方便人們的工作、旅游等日常生活。

無人車定位系統(tǒng)是自動駕駛系統(tǒng)中的一個重要組成部分，許多無人車算法(例如：行為決策、路徑規(guī)劃和車輛運(yùn)動控制)都依賴于自身車輛的位姿信息。無人車定位系統(tǒng)根據(jù)不同的定位需求，分成三個層次：路網(wǎng)級別、介于路網(wǎng)和車道級別之間、車道級別。無人駕駛系統(tǒng)要求的定位系統(tǒng)是車道級別的，即要求的定位精度是厘米級。

可靠和精確的實(shí)時定位與導(dǎo)航是無人車安全行駛在城市環(huán)境下的基礎(chǔ)。在早期的無人車中，高精度gps融合航位推算算法通常被用作無人車定位系統(tǒng)，然而，在城市環(huán)境中，gps信號會被高聳的建筑物遮擋，缺少足夠的可視衛(wèi)星，或存在信號多徑反射，而航位推測傳感器(加速度計和陀螺儀等)存在測量噪聲，只通過航位推測算法來估計無人車的位姿，會導(dǎo)致定位系統(tǒng)累計誤差越來越大，因此，基于gps融合航位推測算法的定位系統(tǒng)無法保證無人車的長時間和長距離穩(wěn)定運(yùn)行。為了克服這個問題，基于高精度地圖結(jié)合傳感器檢測的定位算法被提出并應(yīng)用于無人駕駛系統(tǒng)中。

地圖-感知相結(jié)合的定位算法主要為基于卡爾曼濾波器的特征層面定位。

基于卡爾曼濾波器(或用于非線性系統(tǒng)狀態(tài)估計的擴(kuò)展卡爾曼濾波器)的定位算法是地圖-感知相結(jié)合定位算法中的一種經(jīng)典解決方案，從車載傳感器(相機(jī)、毫米波雷達(dá)或激光)提取的特征與高精度地圖中的特征進(jìn)行匹配，根據(jù)匹配結(jié)果，卡爾曼濾波器更新車輛位姿的均值和協(xié)方差。基于卡爾曼濾波器的定位方法具有結(jié)構(gòu)簡單，計算復(fù)雜度較低等優(yōu)點(diǎn)。

例如，ibeo激光雷達(dá)被應(yīng)用于高精度地圖的構(gòu)建和車輛定位，根據(jù)激光雷達(dá)返回的點(diǎn)云構(gòu)建柵格地圖，提取出靜態(tài)物體作為地標(biāo)與傳感器數(shù)據(jù)實(shí)時匹配，從而計算出無人車的航向角誤差和側(cè)向誤差，結(jié)合擴(kuò)展卡爾曼濾波器估計無人車位姿。

盡管基于特征層面的定位算法具有結(jié)構(gòu)簡單，計算復(fù)雜度低等優(yōu)勢，但被應(yīng)用于復(fù)雜環(huán)境下的無人車定位和估計非線性系統(tǒng)狀態(tài)時，存在一些局限性。當(dāng)無人車行駛在復(fù)雜環(huán)境下，車載傳感器檢測的特征點(diǎn)和地圖數(shù)據(jù)關(guān)聯(lián)是一個難點(diǎn)，其原因在于，特征點(diǎn)檢測會存在誤檢和障礙物遮擋等情況，將錯誤的特征點(diǎn)與地圖數(shù)據(jù)匹配，會導(dǎo)致定位算法失效。

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

本發(fā)明需要保護(hù)的方法技術(shù)方案，表征為：

一種基于三維激光雷達(dá)的無人車定位算法，其特征在于，

步驟1，從單幀的激光點(diǎn)云數(shù)據(jù)中提取出路沿特征點(diǎn)，根據(jù)車輛運(yùn)動學(xué)模型，將多幀檢測的路沿特征點(diǎn)坐標(biāo)轉(zhuǎn)換到當(dāng)前車輛坐標(biāo)系下；

步驟2，利用beammodel方法提取出多幀的路沿特征點(diǎn)的輪廓(創(chuàng)新點(diǎn))；

步驟3，將步驟2提取的路沿輪廓與高精度地圖數(shù)據(jù)進(jìn)行匹配，計算出當(dāng)前車輛的側(cè)向、縱向和航向角偏差作為觀測值，輸入到卡爾曼濾波器(該卡爾曼濾波器本身已為現(xiàn)有技術(shù))進(jìn)行車輛位姿估計。

步驟1，從單幀的激光點(diǎn)云數(shù)據(jù)中提取出路沿特征點(diǎn)，根據(jù)車輛運(yùn)動學(xué)模型，將多幀檢測的路沿特征點(diǎn)坐標(biāo)轉(zhuǎn)換到當(dāng)前車輛坐標(biāo)系下。

步驟(1.1)路沿特征點(diǎn)檢測

具體為：

所述步驟1，利用車載的慣性導(dǎo)航系統(tǒng)獲取相鄰兩幀車輛的位移和航向角變化量dx，dy和dθ，單幀的路沿特征點(diǎn)表示為qk，其中qk＝(xk,yk)為第k幀的路沿坐標(biāo)，在當(dāng)前車輛坐標(biāo)系下的多幀路沿為：

r＝[qkf(qk-1)f²(qk-2)...fⁿ(qk-n)]

步驟2、多幀的路沿特征點(diǎn)的輪廓提取

首先通過高斯投影(現(xiàn)有技術(shù))，得到局部坐標(biāo)系下的路徑點(diǎn)坐標(biāo)(xini,j,yini,j)，j表示第j個路徑點(diǎn)。

然后在無人車的每個路徑點(diǎn)上設(shè)置多個beammodel，每個beammodel用beam區(qū)域來表示(創(chuàng)新點(diǎn))，beam區(qū)域用下式表示：

其中zp為第p個beam區(qū)域。

在每個beam區(qū)域中，計算最近的路沿坐標(biāo)：

其中ri＝(xi,yi)其是第i個路沿坐標(biāo)。

將每束beam遇到的最近的路沿點(diǎn)提取出來，最終，提取出的路沿輪廓為c＝{dp,p＝1,2,…,n}

步驟3、地圖匹配

步驟3.1、將地圖數(shù)據(jù)點(diǎn)集由m表示，采用最近點(diǎn)迭代算法(該算法本身已屬于現(xiàn)有技術(shù))的目的是最小化如下代價函數(shù)：

j＝∑dist(tc,m)

其中dist為歐式距離函數(shù)，

通過以下迭代方法解得：

步驟(3.1.1)在m中找到每個ci的關(guān)聯(lián)點(diǎn)，通過kd樹尋找最近點(diǎn)實(shí)現(xiàn)。

步驟(3.1.2)利用奇異值分解方法計算變換矩陣t。

步驟(3.1.3)應(yīng)用變換：c＝tc，然后計算代價j。

步驟(3.1.4)當(dāng)代價的改變小于閾值τ時終止迭代。

在迭代過程結(jié)束之后，獲得了c和m的變換矩陣t：

其中θ、mx、my分別為地圖匹配得到的航向角、x方向的偏移量和y方向的偏移量。

步驟3.2、定位

經(jīng)過上述迭代算法后，獲得了變換矩陣t，將t時刻的車輛位置表示為根據(jù)車輛運(yùn)動學(xué)模型(現(xiàn)有技術(shù))預(yù)測t+1時刻的車輛

位姿：

預(yù)測t+1時刻的的車輛位姿表示為：式中δt為時間間隔。

第一個卡爾曼濾波器用于融合gps測量值和預(yù)測值輸出值，提供給第二個卡爾曼濾波器。

第二個卡爾曼濾波器用于融合變換矩陣t和第一個卡爾曼濾波器的輸出值，得到最終的無人車位姿。

與現(xiàn)有技術(shù)相比，本發(fā)明的優(yōu)點(diǎn)

1.利用三維激光雷達(dá)傳感器進(jìn)行環(huán)境感知，由于激光雷達(dá)是主動探測式傳感器，因此，在任何光照條件下都能有效感知環(huán)境信息，進(jìn)行車輛定位。

2.采用高精度地圖作為先驗(yàn)知識，使得定位算法沒有累計誤差，能夠保證車輛長時間，長距離的穩(wěn)定定位。

3.本發(fā)明方法采用多幀點(diǎn)云數(shù)據(jù)技術(shù)進(jìn)行環(huán)境檢測，可適用于多種激光傳感器，并且也能夠獲得較高的定位精度。

4.使用了beammodel算法，能夠有效去除異常點(diǎn)，使得地圖匹配更精確，定位精度更高。

附圖說明

圖1激光原始點(diǎn)云

圖2單幀路沿

圖3多幀路沿

圖4beammodel

圖5路沿輪廓

圖6地圖匹配

圖7本發(fā)明流程圖。

圖8是第2步驟流程圖。

具體實(shí)施方式

基于本發(fā)明技術(shù)方案，給出實(shí)施例和仿真實(shí)驗(yàn)。

實(shí)施例1

具體步驟如下：

步驟1，從單幀的激光點(diǎn)云數(shù)據(jù)中提取出路沿特征點(diǎn)，根據(jù)車輛運(yùn)動學(xué)模型，將多幀檢測的路沿特征點(diǎn)坐標(biāo)轉(zhuǎn)換到當(dāng)前車輛坐標(biāo)系下。

步驟(1.1)路沿特征點(diǎn)檢測

本實(shí)施例采用的是velodynehdl-32e激光雷達(dá)作為環(huán)境感知傳感器，其體積更小，更輕，結(jié)構(gòu)堅(jiān)固，有32個激光發(fā)射和接收裝置(市售產(chǎn)品，該設(shè)備本身已屬于現(xiàn)有技術(shù))。hdl032e具有360度水平視角和41.3度的垂直視角，最大檢測距離為70m，精度為2cm。在本實(shí)施例中，velodynehdl-32e激光雷達(dá)安裝于車輛頂部。如圖1所示，其原始點(diǎn)云坐標(biāo)為三維極坐標(biāo)。

本發(fā)明采用了基于點(diǎn)云數(shù)據(jù)空間特征的路沿檢測算法，首先，在大部分城市環(huán)境中，路沿的高度是有統(tǒng)一標(biāo)準(zhǔn)的，一般高出路面10-15cm，其次，在笛卡爾坐標(biāo)系中，路沿的z軸高度變化劇烈。根據(jù)這些空間特征，對單幀點(diǎn)云數(shù)據(jù)進(jìn)行路沿提取，其檢測結(jié)果如圖2所示。

步驟(1.2)、由于單幀檢測的路沿特征點(diǎn)的密度隨著檢測距離的增加而減小，為了獲得一個對路沿的完整描述，需要將多幀檢測的路沿轉(zhuǎn)換到同一坐標(biāo)系下。由于傳感器坐標(biāo)系是隨著車輛的運(yùn)動而改變的，因此，利用車載的慣性導(dǎo)航系統(tǒng)獲取相鄰兩幀車輛的位移和航向角變化量dx，dy和dθ，單幀的路沿特征點(diǎn)表示為qk，其中qk＝(xk,yk)為第k幀的路沿坐標(biāo)，因此，在當(dāng)前車輛坐標(biāo)系下的多幀路沿為：

r＝[qkf(qk-1)f²(qk-2)...fⁿ(qk-n)]

其結(jié)果如圖3所示。

步驟2、多幀的路沿特征點(diǎn)的輪廓提取

從圖3中可以看出，由于路沿檢測算法的局限性，在多幀路沿中存在一些異常點(diǎn)，這些異常點(diǎn)通常位于路沿輪廓外側(cè)。beammodel是由同一個起始點(diǎn)發(fā)射一系列虛擬beams的方法。在這里，beammodel被用于提取路沿輪廓。

由于無人車的軌跡點(diǎn)是由gps坐標(biāo)表示的，因此，首先通過高斯投影(現(xiàn)有技術(shù))，得到局部坐標(biāo)系下的路徑點(diǎn)坐標(biāo)(xini,j,yini,j)，j表示第j個路徑點(diǎn)。

然后在無人車的每個路徑點(diǎn)上設(shè)置多個beammodel，每個beammodel用beam區(qū)域來表示(創(chuàng)新點(diǎn))，beam區(qū)域用下式表示：

其中zp為第p個beam區(qū)域。

在每個beam區(qū)域中，計算最近的路沿坐標(biāo)：

其中ri＝(xi,yi)其是第i個路沿坐標(biāo)。

將每束beam遇到的最近的路沿點(diǎn)提取出來，最終，提取出的路沿輪廓為c＝{dp,p＝1,2,…,n}，得到圖5所示的結(jié)果。圖4藍(lán)色的線即為發(fā)射的beam(已有文字指示)。

具體的算法流程如圖8所示。

第2步驟的原理是，本發(fā)明采用beammodel是用來提取路沿輪廓的，這個算法有一個參數(shù)，就是發(fā)射虛擬beam的原點(diǎn)，這個原點(diǎn)在本發(fā)明算法里就是車子行駛的軌跡點(diǎn)，由于gps返回的車子的軌跡點(diǎn)是gps全局坐標(biāo)，所以用“高斯投影”的方法轉(zhuǎn)換到以當(dāng)前車子為原點(diǎn)的笛卡爾坐標(biāo)，就是(xini,j,yini,j)所示，后面zp表示每個虛擬beam的區(qū)域，是一個關(guān)于坐標(biāo)x,y的區(qū)域，x,y的關(guān)系滿足zp右邊的不等式。最后就是計算出在每個beam區(qū)域中，距離原點(diǎn)最近的路沿點(diǎn)，把它提取出來，得到路沿輪廓c。

步驟3、地圖匹配

步驟3.1、地圖匹配的目的是估計出檢測到的路沿與高精度地圖數(shù)據(jù)的偏差。最近點(diǎn)迭代是一種用于估計兩個點(diǎn)云集變換關(guān)系的算法。這里將地圖數(shù)據(jù)點(diǎn)集由m表示，本發(fā)明采用最近點(diǎn)迭代算法(該算法本身已屬于現(xiàn)有技術(shù))的目的是最小化如下代價函數(shù)：

j＝∑dist(tc,m)

其中dist為歐式距離函數(shù)，

優(yōu)化上述式子，通過以下迭代方法解得：

步驟(3.1.1)在m中找到每個ci的關(guān)聯(lián)點(diǎn)，通過kd樹尋找最近點(diǎn)實(shí)現(xiàn)。

步驟(3.1.2)利用奇異值分解方法計算變換矩陣t。

步驟(3.1.3)應(yīng)用變換：c＝tc，然后計算代價j。

步驟(3.1.4)當(dāng)代價的改變小于閾值τ時終止迭代。

在迭代過程結(jié)束之后，獲得了c和m的變換矩陣t：

其中θ、mx、my分別為地圖匹配得到的航向角、x方向的偏移量和y方向的偏移量。

步驟3.2、定位

經(jīng)過上述迭代算法后，獲得了變換矩陣t，采用了兩個卡爾曼濾波器對噪聲濾波并估計出一個相對精確的位置信息。將t時刻的車輛位置表示為

根據(jù)車輛運(yùn)動學(xué)(現(xiàn)有技術(shù))預(yù)測t+1時刻的車輛位姿：

預(yù)測t+1時刻的的車輛位姿表示為：式中δt為時間間隔。

第一個卡爾曼濾波器用于融合gps測量值和預(yù)測值輸出值，提供給第二個卡爾曼濾波器。

第二個卡爾曼濾波器用于融合變換矩陣t和第一個卡爾曼濾波器的輸出值，得到最終的無人車位姿。