一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法
【專利摘要】本發(fā)明公開了一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法。在由節(jié)點(diǎn)和邊構(gòu)成的路網(wǎng)地圖中，從已知起點(diǎn)出發(fā)，通過航位推算方法實(shí)時(shí)獲得車輛初步位置，將每個(gè)車輛初步位置與起點(diǎn)連接形成車輛行駛初步軌跡，實(shí)時(shí)檢測拐點(diǎn)獲得拐點(diǎn)并標(biāo)記，在各拐點(diǎn)將車輛行駛初步軌跡分段，分為各段車輛軌跡分段，獲得每個(gè)拐點(diǎn)后對拐點(diǎn)進(jìn)行實(shí)時(shí)更新，將各個(gè)實(shí)時(shí)更新后的拐點(diǎn)和起點(diǎn)按行駛時(shí)序依次連接作為最終車輛行駛軌跡。本發(fā)明能有效修正航位推算方法產(chǎn)生的累積定位誤差，具備良好的定位精度，計(jì)算代價(jià)很小，實(shí)時(shí)性強(qiáng)。
【專利說明】
一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及機(jī)器人的導(dǎo)航定位方法，特別是涉及一種面向無人車導(dǎo)航的基于軌跡 拐點(diǎn)濾波的地圖定位方法。
【背景技術(shù)】
[0002] 傳統(tǒng)的定位方法包括使用GPS、慣性導(dǎo)航系統(tǒng)以及里程計(jì)等。GPS的優(yōu)點(diǎn)是其良好 的通用性。但在高樓林立的城市環(huán)境下或者橋梁、隧道中，衛(wèi)星信號容易受到遮擋而無法獲 得良好的定位精度。慣導(dǎo)系統(tǒng)通過對系統(tǒng)加速度和角速度的積分來獲得機(jī)器人所在方位， 受到累積誤差的影響較大，一般都結(jié)合GPS-起使用，很少單獨(dú)應(yīng)用。里程計(jì)方法分為傳統(tǒng) 輪式里程計(jì)和目前受到廣泛關(guān)注的視覺里程計(jì)。輪式里程計(jì)一般只能提供相對起點(diǎn)的距離 信息，需要結(jié)合移動方向信息才能實(shí)現(xiàn)定位。視覺里程計(jì)將單目或者雙目圖像序列作為輸 入，通過測量幀間對應(yīng)像素的運(yùn)動，來計(jì)算車輛相對與起點(diǎn)的方位，并獲得運(yùn)動軌跡，是一 種成本較低的但定位精度較好的方法。但視覺里程計(jì)同樣存在累積誤差的問題，并不適合 長距離定位。
[0003] 為了解決累積誤差的問題，一類方法是采用同時(shí)定位與地圖構(gòu)建(SLAM)的方法， 將運(yùn)動軌跡形成閉環(huán)，通過約束軌跡閉合誤差來減少定位累計(jì)誤差。該方法對運(yùn)動軌跡有 閉環(huán)要求，限制了使用環(huán)境，計(jì)算復(fù)雜度也較高。另一類方法是將視覺里程計(jì)或者慣導(dǎo)（以 下統(tǒng)稱航位推算方法)與GPS相結(jié)合，通過GPS對累計(jì)誤差的定期修正，提高定位可靠度。
[0004] 近年來隨著互聯(lián)網(wǎng)各類地圖服務(wù)的不斷普及，各類城市路網(wǎng)地圖變得十分容易獲 得，現(xiàn)有技術(shù)中缺少了一種能累計(jì)誤差用于實(shí)現(xiàn)無人車導(dǎo)航的精準(zhǔn)實(shí)時(shí)定位方法。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明的目的在于提供了一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位 方法，利用易得的道路地圖，通過拐點(diǎn)濾波方法來實(shí)現(xiàn)對里程計(jì)或者慣導(dǎo)累積誤差定位的 有效修正，從而實(shí)現(xiàn)車輛的可靠定位。具體是通過先驗(yàn)地圖信息，將其與航位推算方法相結(jié) 合，來有效應(yīng)對定位的累計(jì)誤差。
[0006] 本發(fā)明首先采用圖模型建立導(dǎo)航區(qū)域的路網(wǎng)地圖，通過視覺里程計(jì)或者慣導(dǎo)等航 位推算方法得到車輛的初始運(yùn)動軌跡，將該軌跡按照拐點(diǎn)進(jìn)行分段后，通過對每個(gè)拐點(diǎn)實(shí) 施粒子濾波方法來修正每個(gè)拐點(diǎn)在路網(wǎng)地圖上的位置，從而消除航位推算方法產(chǎn)生的累積 誤差，提尚定位的精度。
[0007] 本發(fā)明采用的具體技術(shù)方案如下：
[0008] 1)在主要由節(jié)點(diǎn)和邊構(gòu)成的路網(wǎng)地圖中，車輛從已知位置的起點(diǎn)出發(fā)，在車輛行 駛過程中，通過航位推算方法實(shí)時(shí)獲得車輛初步位置，將每個(gè)時(shí)刻的車輛初步位置與起點(diǎn) 依次連接形成車輛行駛初步軌跡，對車輛行駛初步軌跡實(shí)時(shí)檢測拐點(diǎn)獲得拐點(diǎn)并標(biāo)記，并 將起點(diǎn)作為拐點(diǎn)，在各拐點(diǎn)將車輛行駛初步軌跡用直線段進(jìn)行分段，分為各段車輛軌跡分 段；
[0009] 2)獲得每個(gè)拐點(diǎn)后對拐點(diǎn)進(jìn)行實(shí)時(shí)更新，將各個(gè)實(shí)時(shí)更新后的拐點(diǎn)和起點(diǎn)按行駛 時(shí)序依次連接作為最終車輛行駛軌跡。
[0010] 所述路網(wǎng)地圖用節(jié)點(diǎn)和邊來構(gòu)建:邊作為道路，以節(jié)點(diǎn)作為道路與道路之間的交 叉路口，節(jié)點(diǎn)的位置由經(jīng)煒度表示，表示為n=(lat，Ion)，兩個(gè)節(jié)點(diǎn)一段道路表示為e = (ni，n2)，ηι，η2分別表示為兩個(gè)相鄰的節(jié)點(diǎn)。
[0011] 所述對車輛行駛初步軌跡實(shí)時(shí)檢測拐點(diǎn)獲得拐點(diǎn)具體是：
[0012] 1.1)在車輛行駛過程中，將通過航位推算方法實(shí)時(shí)獲得當(dāng)前時(shí)刻下的車輛初步位 置與行駛經(jīng)過最近的一個(gè)拐點(diǎn)用直線進(jìn)行連接，計(jì)算在當(dāng)前時(shí)刻下的車輛初步位置與行駛 過最近的一個(gè)拐點(diǎn)之間的每一個(gè)中間時(shí)刻下的車輛初步位置到該直線的垂直距離；
[0013] 1.2)將垂直距離與拐點(diǎn)閾值進(jìn)行比較：
[0014] 若所有中間時(shí)刻下的車輛初步位置到該直線的垂直距離小于等于拐點(diǎn)閾值，則當(dāng) 前時(shí)刻下的車輛初步位置與行駛經(jīng)過最近的一個(gè)拐點(diǎn)之間的車輛行駛初步軌跡不存在拐 占 .
[0015] 若所有中間時(shí)刻下的車輛初步位置到該直線的垂直距離大于拐點(diǎn)閾值，則車輛初 步位置到該直線的垂直距離最大所在的中間時(shí)刻的車輛初步位置作為新的拐點(diǎn)，通過新的 拐點(diǎn)將當(dāng)前時(shí)刻下的車輛初步位置與行駛經(jīng)過最近的一個(gè)拐點(diǎn)之間的車輛行駛初步軌跡 分為兩段車輛軌跡分段，
[0016] 1.3)重復(fù)迭代上述兩個(gè)步驟，直到行駛經(jīng)過最近的一段車輛軌跡分段中所有中間 時(shí)刻下的車輛初步位置到該直線的垂直距離小于等于拐點(diǎn)閾值。
[0017] 所述的對拐點(diǎn)進(jìn)行實(shí)時(shí)更新具體是通過構(gòu)建粒子并通過粒子濾波方法計(jì)算獲得 每一個(gè)粒子的權(quán)重，通過比較獲得權(quán)重最大的粒子，并用該粒子對其所在拐點(diǎn)進(jìn)行替換，從 而實(shí)現(xiàn)對拐點(diǎn)的實(shí)時(shí)更新。
[0018] 所述步驟2)實(shí)時(shí)更新中每次計(jì)算的具體過程如下：
[0019] 2.1)對于每個(gè)當(dāng)前拐點(diǎn)，在與上一個(gè)拐點(diǎn)最近的節(jié)點(diǎn)的各個(gè)相關(guān)節(jié)點(diǎn)周圍附近隨 機(jī)生成一堆新粒子，每個(gè)新粒子歸屬于其所生成對應(yīng)的節(jié)點(diǎn)，相關(guān)節(jié)點(diǎn)指的是該節(jié)點(diǎn)所在 邊上的節(jié)點(diǎn)，節(jié)點(diǎn)的所有相關(guān)節(jié)點(diǎn)包括了該節(jié)點(diǎn)所在邊上的所有節(jié)點(diǎn)。用一堆粒子來表示 其在路網(wǎng)地圖上的位置，粒子不一定要分布在該節(jié)點(diǎn)上以此來彌補(bǔ)路網(wǎng)的不精確性；
[0020] 當(dāng)需要初始化第i + Ι組粒子的時(shí)候，首先找到第i組粒子所有屬于的節(jié)點(diǎn)，利用路 網(wǎng)地圖的拓?fù)潢P(guān)系，找到所有其相鄰的節(jié)點(diǎn)作為第i+Ι組粒子可能的初始位置，并將每個(gè)粒 子的歸屬屬性設(shè)為與該粒子位置最接近的節(jié)點(diǎn)。
[0021] 2.2)每個(gè)當(dāng)前拐點(diǎn)下生成的所有新粒子形成了該當(dāng)前拐點(diǎn)下的一組粒子，對于生 成的每一組粒子采用以下方式處理：
[0022] 將每一個(gè)新生成的粒子分別與上一個(gè)拐點(diǎn)周圍附近的粒子中在同一條邊上對應(yīng) 的各個(gè)粒子進(jìn)行兩兩組合用直線段進(jìn)行連接，每兩個(gè)粒子之間的直線段作為備選線段，計(jì) 算每一備選線段的長度和角度，然后將每一條備選線段的長度和朝向角度與對應(yīng)的車輛軌 跡分段的長度和角度進(jìn)行比較，獲得每一條備選線段的長度誤差和角度誤差：
[0025] 式中，ei為長度誤差，1。為備選線段的長度，ls為對應(yīng)的車輛軌跡分段的長度，ea為 角度誤差，h。為備選線段的角度，h s為對應(yīng)的車輛軌跡分段的角度；
[0026] 對于起點(diǎn)后的第一個(gè)拐點(diǎn)，將該拐點(diǎn)生成的每一個(gè)粒子分別與起點(diǎn)進(jìn)行組合用直 線段進(jìn)行連接，獲得各條備選線段。
[0027] 判斷備選線段的長度誤差和角度誤差是否在閾值范圍內(nèi)，若在閾值范圍內(nèi)則保留 該備選線段作為車輛可能的軌跡，否則舍棄該備選線段；
[0028] 2.3)再通過以下公式計(jì)算每一條備選線段與其對應(yīng)的車輛軌跡分段之間的相似 度Ss :
[0029] Ss = a*sl+(l-a)*S￡i
[0030] 其中，si為備選線段長度的相似度，sa為備選線段角度的相似度，ss為備選線段與 車輛軌跡分段之間的相似度，a表示權(quán)重參數(shù)；
[0031] 2.4)將步驟2.2)處理后保留的備選線段與上一次拐點(diǎn)迭代計(jì)算后剩余的備選線 段通過樹結(jié)構(gòu)相連形成了所有可能的各條備選路徑;對于當(dāng)前拐點(diǎn)的每一個(gè)粒子，然后采 用以下公式計(jì)算各條包含有該粒子的備選路徑與車輛行駛初步軌跡之間的相似度 Sr: M-l
[0032] 4 =(Σ·ν>[/^/Μ
[0033] 式中，Ss[i]表示一條備選路徑中第i段備選線段與其對應(yīng)的車輛軌跡分段之間的 相似度，i表示備選路徑的序號，Μ表示車輛行駛初步軌跡中車輛軌跡分段的總數(shù)，i表示車 輛軌跡分段的段序數(shù)；
[0034] 2.5)通過以下公式計(jì)算所有包含該粒子的備選路徑與車輛行駛初步軌跡相似度 的平均值，作為粒子的權(quán)重：
[0036] 式中，wu代表第i組第j個(gè)粒子的權(quán)重，義表示第k條備選路徑與車輛行駛初步軌 跡之間的相似度，η代表該粒子下所有備選路徑的總數(shù)，k表示該粒子下備選路徑的序數(shù)；
[0037] 2.6)對當(dāng)前拐點(diǎn)下該組所有粒子是否歸屬于同一個(gè)節(jié)點(diǎn)進(jìn)行判斷：
[0038] 若當(dāng)前拐點(diǎn)下該組所有粒子是均歸屬于同一個(gè)節(jié)點(diǎn)，則認(rèn)為這組粒子對應(yīng)的拐點(diǎn) 在路網(wǎng)地圖上的位置已確定，這組粒子的迭代計(jì)算過程停止，取其中權(quán)重最大的粒子替換 當(dāng)前拐點(diǎn)作為當(dāng)前在路網(wǎng)地圖上的位置，將當(dāng)前拐點(diǎn)周圍附近的其余所有粒子舍棄；
[0039] 若當(dāng)前拐點(diǎn)下該組所有粒子不是均歸屬于同一個(gè)節(jié)點(diǎn)，則該組所有粒子全部保留 并作為下一次計(jì)算的剩余粒子，返回到步驟2.1)繼續(xù)進(jìn)行迭代濾波，對所有粒子進(jìn)行按照 其權(quán)重重采樣，進(jìn)行實(shí)時(shí)更新。
[0040] 所述步驟2.3)中的備選線段長度的相似度S1和備選線段角度的相似度&采用以下 公式計(jì)算：
[0043]式中，e表示自然常數(shù)，ei為長度誤差，ea為角度誤差。
[0044] 所述步驟2.1)中，若上一個(gè)拐點(diǎn)計(jì)算后存在剩余剩余粒子，所述當(dāng)前拐點(diǎn)生成粒 子時(shí)還要在上一個(gè)拐點(diǎn)的剩余粒子周圍附近隨機(jī)再生成一堆粒子。
[0045] 如果通過航位推算得到車輛的行駛軌跡的終點(diǎn)離地圖上的節(jié)點(diǎn)太遠(yuǎn)，則不通過濾 波得到終點(diǎn)的位置，而是通過上一個(gè)拐點(diǎn)更新后的位置和航位推算直接得到終點(diǎn)的位置。
[0046] 本發(fā)明是在視覺里程計(jì)或者慣導(dǎo)系統(tǒng)的支持下，可以獲得機(jī)器人當(dāng)前的初步定 位。將獲得的車輛運(yùn)動軌跡和已知的路網(wǎng)地圖結(jié)合，將運(yùn)動軌跡分段后通過對每段之間的 連接拐點(diǎn)進(jìn)行濾波，逐個(gè)修正每個(gè)拐點(diǎn)的位置，從而消除航位推算方法產(chǎn)生的累積誤差，使 定位更加精確。
[0047] 與【背景技術(shù)】相比，本發(fā)明具有的有益效果是：
[0048] 1)本發(fā)明能在無GPS等絕對定位信息的情況下，依賴視覺或者慣導(dǎo)等航位推算方 法，結(jié)合地圖實(shí)現(xiàn)可靠定位；
[0049] 2)本發(fā)明能有效修正航位推算算法的累積誤差，提高長距離定位的精度，即使在 車輛長距離行駛的情況下仍然具備良好的定位精度；
[0050] 3)本發(fā)明使用拐點(diǎn)濾波的方法框架，與傳統(tǒng)的每個(gè)傳感器數(shù)據(jù)到來時(shí)刻進(jìn)行粒子 濾波的方法不同，只在軌跡拐點(diǎn)處進(jìn)行粒子濾波，大大減小了運(yùn)算量，實(shí)時(shí)性強(qiáng)，并提高了 定位效果。
【附圖說明】
[0051] 圖1是本發(fā)明方法的流程圖。
[0052] 圖2是實(shí)施例的粒子濾波實(shí)施步驟示意圖。
[0053]圖3是實(shí)施例KITTY08序列航位推算方法結(jié)果。
[0054]圖4是實(shí)施例KITTY08序列本方法結(jié)果。
[0055]圖5是實(shí)施例KITTY08序列真值軌跡。
[0056]圖6是實(shí)施例KITTY08序列航位推算方法和本方法定位誤差比較。
【具體實(shí)施方式】
[0057]下面結(jié)合附圖和實(shí)施例對本發(fā)明做進(jìn)一步說明。
[0058]如圖1所示，本發(fā)明方法的實(shí)施例及其實(shí)施過程如下：
[0059] 下面以一個(gè)簡單的例子（由圖2所示)來更清楚地表述本發(fā)明濾波的過程。圖2. (a) 為通過航位推算方法和軌跡分段得到車輛分段后的軌跡，軌跡中有五個(gè)拐點(diǎn)（把終點(diǎn)也當(dāng) 作拐點(diǎn)處理），因此用五組粒子來表示這五個(gè)拐點(diǎn)在地圖中對應(yīng)的位置。以下為具體濾波的 步驟：
[0060] 步驟一:假設(shè)車輛起點(diǎn)已知（圖2. (b)標(biāo)號為0的節(jié)點(diǎn)），當(dāng)初始化第一組粒子的時(shí) 候，找到所有與節(jié)點(diǎn)〇相關(guān)的節(jié)點(diǎn)（圖2. (b)灰色區(qū)域），作為第一組粒子初始化分布的位置。 根據(jù)上述的步驟更新粒子的權(quán)重，從圖2. (c)中可見，所有粒子都?xì)w屬于同一個(gè)節(jié)點(diǎn)（圖2. (c)標(biāo)號為1的節(jié)點(diǎn)），因此不需要再次更新這堆粒子，找出權(quán)重最大的那個(gè)粒子作為第一個(gè) 拐點(diǎn)對應(yīng)的位置。
[0061] 步驟二:下一步為搜索所有第一組粒子歸屬的節(jié)點(diǎn)（圖2. (c)標(biāo)號為1的節(jié)點(diǎn)）的相 關(guān)節(jié)點(diǎn)，作為第二組粒子初始化的位置(圖2. (c)灰色區(qū)域）。從圖2. (d)可看出，經(jīng)過粒子更 新，第二組的粒子歸屬于三個(gè)節(jié)點(diǎn)（圖2. (d)標(biāo)號為2的三個(gè)節(jié)點(diǎn)），因此需要保留所有粒子 繼續(xù)迭代。
[0062]步驟三:下一步為搜索所有第二組粒子歸屬的節(jié)點(diǎn)（圖2. (d)標(biāo)號為2的節(jié)點(diǎn)）的相 關(guān)節(jié)點(diǎn)，作為第三組粒子初始化的位置(圖2. (d)灰色區(qū)域）。從圖2. (e)可看出，經(jīng)過粒子更 新，第二組粒子和第三組粒子分別有三個(gè)歸屬的節(jié)點(diǎn)（圖2. (e)標(biāo)號為2和3的節(jié)點(diǎn)），因此第 二組粒子和第三組粒子都需要保留所有粒子繼續(xù)迭代更新。
[0063] 步驟四：下一步為搜索所有第三組粒子歸屬的節(jié)點(diǎn)（圖2. (e)標(biāo)號為3的節(jié)點(diǎn)）的相 關(guān)節(jié)點(diǎn)，作為第四組粒子初始化的位置(圖2. (e)灰色區(qū)域）。從圖2. (f)可看出，經(jīng)過粒子更 新，第二組粒子和第三組粒子都已經(jīng)分別歸屬于一個(gè)節(jié)點(diǎn)（圖2. (f)標(biāo)號為2和3的節(jié)點(diǎn)），而 第四組粒子有兩個(gè)歸屬的節(jié)點(diǎn)（圖2. (f)標(biāo)號為4的兩個(gè)節(jié)點(diǎn)），因此第二組粒子和第三組粒 子都不需要再次更新，分別找出第二組粒子和第三組粒子中權(quán)重最大的粒子作為第二個(gè)拐 點(diǎn)和第三個(gè)拐點(diǎn)對應(yīng)的位置，而第四組粒子需要保留所有粒子繼續(xù)迭代。
[0064] 步驟五:下一步為搜索所有第四組粒子歸屬的節(jié)點(diǎn)（圖2. (f)標(biāo)號為4的節(jié)點(diǎn)）的相 關(guān)節(jié)點(diǎn)，作為第五組粒子初始化的位置(圖2. (f)灰色區(qū)域）。從圖2. (g)可看出，經(jīng)過粒子更 新，第四組粒子和第五組粒子都已經(jīng)分別歸屬于一個(gè)節(jié)點(diǎn)（圖2. (g)標(biāo)號為4和5的節(jié)點(diǎn)），此 時(shí)所有的拐點(diǎn)的位置都已經(jīng)可以確定，找出每一組粒子中權(quán)重最大的粒子，其位置作為對 應(yīng)拐點(diǎn)更新后的位置。將各個(gè)拐點(diǎn)更新后的位置和起點(diǎn)按行駛時(shí)序依次用直線連接作為濾 波后得到的車輛行駛軌跡(圖2. (h))。
[0065] 如果通過航位推算得到車輛的行駛軌跡的終點(diǎn)離地圖上的節(jié)點(diǎn)太遠(yuǎn)，則不通過濾 波得到終點(diǎn)的位置，而是通過上一個(gè)拐點(diǎn)更新后的位置和航位推算直接得到終點(diǎn)的位置。
[0066] 另外測試了KITTY數(shù)據(jù)集來評估本方法的效果，以雙目圖像序列以及對應(yīng)的路網(wǎng) 地圖作為輸入，根據(jù)KITTY提供的真值，比較采用航位推算定位方法和本方法與真值的平均 誤差。
[0067]實(shí)驗(yàn)主要測試了 KITTY圖像序列08。序列08航位推算定位的結(jié)果如圖3所示，本方 法的結(jié)果如圖4所示，真值如圖5所示，可以看出航位推算定位在拐彎處累積了較大的角度 誤差，導(dǎo)致定位的累積誤差大大增加，而本方法可以有效地消除累積誤差，使定位誤差一直 維持在一個(gè)比較小的范圍內(nèi)。誤差對比曲線如圖6所示，平均定位誤差從45.61m減小到 3.25m，見下表1。
[0068]表1航位推算方法與本方法定位誤差比較
[0070] 實(shí)驗(yàn)采用一臺配備主頻為2.8GHz的inter i5處理器的電腦，實(shí)驗(yàn)中每次迭代平均 耗時(shí)70.62ms。由于拐點(diǎn)濾波不需要每一幀都進(jìn)行迭代，所以相對于航位推算定位，增加的 計(jì)算代價(jià)很小。
[0071] 可以看出，航位推算定位誤差隨著距離的增加而增大，而采用本方法能有效地修 正航位推算方法產(chǎn)生的累積定位誤差，使定位誤差一直處于一個(gè)比較小的范圍內(nèi)，而且計(jì) 算代價(jià)小，可實(shí)時(shí)運(yùn)行。
【主權(quán)項(xiàng)】
1. 一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法，其特征在于包括如下步 驟： 1) 在主要由節(jié)點(diǎn)和邊構(gòu)成的路網(wǎng)地圖中，車輛從已知位置的起點(diǎn)出發(fā)，在車輛行駛過 程中，通過航位推算方法實(shí)時(shí)獲得車輛初步位置，將每個(gè)時(shí)刻的車輛初步位置與起點(diǎn)依次 連接形成車輛行駛初步軌跡，對車輛行駛初步軌跡實(shí)時(shí)檢測拐點(diǎn)獲得拐點(diǎn)并標(biāo)記，并將起 點(diǎn)作為拐點(diǎn)，在各拐點(diǎn)將車輛行駛初步軌跡用直線段進(jìn)行分段，分為各段車輛軌跡分段； 2) 獲得每個(gè)拐點(diǎn)后對拐點(diǎn)進(jìn)行實(shí)時(shí)更新，將各個(gè)實(shí)時(shí)更新后的拐點(diǎn)和起點(diǎn)按行駛時(shí)序 依次連接作為最終車輛行駛軌跡。2. 根據(jù)權(quán)利要求1所述的一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法， 其特征在于:所述路網(wǎng)地圖用節(jié)點(diǎn)和邊來構(gòu)建:邊作為道路，以節(jié)點(diǎn)作為道路與道路之間的 交叉路口。3. 根據(jù)權(quán)利要求1所述的一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法， 其特征在于:所述對車輛行駛初步軌跡實(shí)時(shí)檢測拐點(diǎn)獲得拐點(diǎn)具體是： 1.1) 在車輛行駛過程中，將通過航位推算方法實(shí)時(shí)獲得當(dāng)前時(shí)刻下的車輛初步位置與 行駛經(jīng)過最近的一個(gè)拐點(diǎn)用直線進(jìn)行連接，計(jì)算在當(dāng)前時(shí)刻下的車輛初步位置與行駛過最 近的一個(gè)拐點(diǎn)之間的每一個(gè)中間時(shí)刻下的車輛初步位置到該直線的垂直距離； 1.2) 將垂直距離與拐點(diǎn)閾值進(jìn)行比較： 若所有中間時(shí)刻下的車輛初步位置到該直線的垂直距離小于等于拐點(diǎn)閾值，則當(dāng)前時(shí) 刻下的車輛初步位置與行駛經(jīng)過最近的一個(gè)拐點(diǎn)之間的車輛行駛初步軌跡不存在拐點(diǎn)； 若所有中間時(shí)刻下的車輛初步位置到該直線的垂直距離大于拐點(diǎn)閾值，則車輛初步位 置到該直線的垂直距離最大所在的中間時(shí)刻的車輛初步位置作為新的拐點(diǎn)，通過新的拐點(diǎn) 將當(dāng)前時(shí)刻下的車輛初步位置與行駛經(jīng)過最近的一個(gè)拐點(diǎn)之間的車輛行駛初步軌跡分為 兩段車輛軌跡分段， 1.3) 重復(fù)迭代上述兩個(gè)步驟，直到行駛經(jīng)過最近的一段車輛軌跡分段中所有中間時(shí)刻 下的車輛初步位置到該直線的垂直距離小于等于拐點(diǎn)閾值。4. 根據(jù)權(quán)利要求1所述的一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法， 其特征在于:所述的對拐點(diǎn)進(jìn)行實(shí)時(shí)更新具體是通過構(gòu)建粒子并通過粒子濾波方法計(jì)算 獲得每一個(gè)粒子的權(quán)重，通過比較獲得權(quán)重最大的粒子，并用該粒子對其所在拐點(diǎn)進(jìn)行替 換，從而實(shí)現(xiàn)對拐點(diǎn)的實(shí)時(shí)更新。5. 根據(jù)權(quán)利要求1或4所述的一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方 法，其特征在于:所述步驟2)實(shí)時(shí)更新中每次計(jì)算的具體過程如下： 2.1) 對于每個(gè)當(dāng)前拐點(diǎn)，在與上一個(gè)拐點(diǎn)最近的節(jié)點(diǎn)的各個(gè)相關(guān)節(jié)點(diǎn)周圍附近隨機(jī)生 成一堆新粒子，每個(gè)新粒子歸屬于其所生成對應(yīng)的節(jié)點(diǎn)，相關(guān)節(jié)點(diǎn)指的是該節(jié)點(diǎn)所在邊上 的節(jié)點(diǎn)； 2.2) 每個(gè)當(dāng)前拐點(diǎn)下生成的所有新粒子形成了該當(dāng)前拐點(diǎn)下的一組粒子，對于生成的 每一組粒子采用以下方式處理： 將每一個(gè)新生成的粒子分別與上一個(gè)拐點(diǎn)周圍附近的粒子中在同一條邊上對應(yīng)的各 個(gè)粒子進(jìn)行兩兩組合用直線段進(jìn)行連接，每兩個(gè)粒子之間的直線段作為備選線段，然后將 每一條備選線段的長度和角度與對應(yīng)的車輛軌跡分段的長度和角度進(jìn)行比較，獲得每一條 備選線段的長度誤差和角度誤差：式中，ei為長度誤差，1。為備選線段的長度，Is為對應(yīng)的車輛軌跡分段的長度，ea為角度 誤差，h。為備選線段的角度，hs為對應(yīng)的車輛軌跡分段的角度； 從屬： 判斷備選線段的長度誤差和角度誤差是否在閾值范圍內(nèi)，若在閾值范圍內(nèi)則保留該備 選線段作為車輛可能的軌跡，否則舍棄該備選線段； 2.3) ?誦忖以下公式計(jì)算每一條備選線段與其對應(yīng)的車輛軌跡分段之間的相似度Ss:其中，S1為備選線段長度的相似度，sa為備選線段角度的相似度，Ss為備選線段與車輛 軌跡分段之間的相似度，α表示權(quán)重參數(shù)； 2.4) 將步驟2.2)處理后保留的備選線段與上一次拐點(diǎn)迭代計(jì)算后剩余的備選線段通 過樹結(jié)構(gòu)相連形成了所有可能的各條備選路徑;對于當(dāng)前拐點(diǎn)的每一個(gè)粒子，然后采用以 下公式計(jì)算各條包含有該粒子的備選路徑與車輛行駛初步軌跡之間的相似度Sr:式中，Ss[i]表示一條備選路徑中第i段備選線段與其對應(yīng)的車輛軌跡分段之間的相似 度，i表示備選路徑的序號，M表示車輛行駛初步軌跡中車輛軌跡分段的總數(shù)，i表示車輛軌 跡分段的段序數(shù)； 2.5) 通過以下公式計(jì)算所有包含該粒子的備選路徑與車輛行駛初步軌跡相似度的平 均值，作為粒子的權(quán)重：式中，代表第i組第j個(gè)粒子的權(quán)重，< 表示第k條備選路徑與車輛行駛初步軌跡之間 的相似度，η代表該粒子下所有備選路徑的總數(shù)，k表示該粒子下備選路徑的序數(shù)； 2.6) 對當(dāng)前拐點(diǎn)下該組所有粒子是否歸屬于同一個(gè)節(jié)點(diǎn)進(jìn)行判斷： 若當(dāng)前拐點(diǎn)下該組所有粒子是均歸屬于同一個(gè)節(jié)點(diǎn)，則取其中權(quán)重最大的粒子替換當(dāng) 前拐點(diǎn)作為當(dāng)前在路網(wǎng)地圖上的位置，將當(dāng)前拐點(diǎn)周圍附近的其余所有粒子舍棄； 若當(dāng)前拐點(diǎn)下該組所有粒子不是均歸屬于同一個(gè)節(jié)點(diǎn)，則該組所有粒子全部保留并作 為下一次計(jì)算的剩余粒子，返回到步驟2.1)繼續(xù)進(jìn)行迭代濾波，對所有粒子進(jìn)行按照其權(quán) 重重采樣，進(jìn)行實(shí)時(shí)更新。6. 根據(jù)權(quán)利要求5所述的一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法， 其特征在于:所述步驟2.2)中對于起點(diǎn)后的第一個(gè)拐點(diǎn)，將該拐點(diǎn)生成的每一個(gè)粒子分別 與起點(diǎn)進(jìn)行組合用直線段進(jìn)行連接，獲得各條備選線段。7. 根據(jù)權(quán)利要求5所述的一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法， 其特征在于:所述步驟2.3)中的備選線段長度的相似度S 1和備選線段角度的相似度sa采用 以下公式計(jì)筧：式中，e表示自然常數(shù)，ei為長度誤差，ea為角度誤差。8.根據(jù)權(quán)利要求5所述的一種面向無人車導(dǎo)航的基于軌跡拐點(diǎn)濾波的地圖定位方法， 其特征在于:所述步驟2.1)中，若上一個(gè)拐點(diǎn)計(jì)算后存在剩余剩余粒子，所述當(dāng)前拐點(diǎn)生成 粒子時(shí)還要在上一個(gè)拐點(diǎn)的剩余粒子周圍附近隨機(jī)再生成一堆粒子。
【文檔編號】G01C21/28GK106017486SQ201610326309
【公開日】2016年10月12日
【申請日】2016年5月16日
【發(fā)明人】項(xiàng)志宇, 金亦東
【申請人】浙江大學(xué)