專利名稱：一種適用于自動駕駛車導(dǎo)航的環(huán)境建模方法
技術(shù)領(lǐng)域：
本發(fā)明涉及到車輛導(dǎo)航領(lǐng)域，具體地說，特別涉及到一種適用于自動駕駛車導(dǎo)航的環(huán)境建模方法。
背景技術(shù)：
自動駕駛車導(dǎo)航技術(shù)廣泛應(yīng)用于安全輔助駕駛、外星球探索、國防自動化等領(lǐng)域。 自動駕駛車的導(dǎo)航需要解決的關(guān)鍵問題之一是如何建模車輛行駛的環(huán)境，辨識其中的景物。將環(huán)境信息轉(zhuǎn)化為自動駕駛車智能避障、路徑規(guī)劃任務(wù)中可利用的參數(shù)化信息。該類參數(shù)化信息中以環(huán)境景物的輪廓方位的利用效率最高。為了得到這些信息，同類方法使用視覺、紅外、超聲波傳感器。但這些傳感器較容易受光照、浮塵等環(huán)境因素的影響，同時還需解決觀測配準(zhǔn)、校正方面的難題。相比之下，激光傳感器具有全天候、精度高、裝配簡單的特點。但同類應(yīng)用中大多從激光束中提取出所描述的景物中的點、線、面等幾何基元作為建模環(huán)境的顯著特征，這在真實的應(yīng)用中缺乏普適性。經(jīng)對現(xiàn)有技術(shù)文獻(xiàn)的檢索發(fā)現(xiàn)，Dissanayake等人在《IEEE Transactions on Robotics and Automation》2001，17 卷，2四_241 頁上發(fā)表了 “A solution to the simultaneous localization and map building(SLAM)problem，，(〈〈iiL:||人禾口自動學(xué) 艮〉〉， 一種同時定位與地圖構(gòu)建方案)。該文介紹了一種基于毫米波雷達(dá)用于車輛導(dǎo)航的定位和環(huán)境建模方法。該方法存在的主要問題是使用環(huán)境中的少量點特征作為環(huán)境建模的主要依據(jù)，這不適用于特別復(fù)雜的環(huán)境，例如，很難將包含大量點、線、面特征的都市環(huán)境表征為有限個幾何基元的集合；同時，該方法所構(gòu)建的過于稀疏的環(huán)境模型對于車輛自主導(dǎo)航和智能避障的利用價值有限，很難在實際中應(yīng)用。綜上所述，針對現(xiàn)有技術(shù)的缺陷，特別需要一種適用于自動駕駛車導(dǎo)航的環(huán)境建模方法，以解決現(xiàn)有技術(shù)的不足。

發(fā)明內(nèi)容
本發(fā)明針對上述現(xiàn)有技術(shù)的不足和缺陷，提供了一種適用于自動駕駛車導(dǎo)航的環(huán)境建模方法，解決了背景技術(shù)中存在的不足。本發(fā)明所解決的技術(shù)問題可以采用一種自動駕駛車導(dǎo)航的環(huán)境建模方法來實現(xiàn)， 包括如下步驟1)在自動駕駛車的前端裝配激光傳感器，當(dāng)車輛行駛時激光掃描傳感器開始按照固定頻率采集激光束觀測；2)測量激光傳感器中心與車輛中心的空間距離；3)確定車輛當(dāng)前采樣時刻在地球中心坐標(biāo)系下的位置。首次運行系統(tǒng)時，需借助其它傳感器觀測車輛在地球中心坐標(biāo)系下的初始位置；4)預(yù)處理車輛行駛過程中在相鄰兩采樣時刻所采集的兩束激光束中返回的距離讀數(shù)序列，具體步驟為
a.剔除兩組激光束的讀數(shù)序列中大于傳感器極限觀測距離的距離讀數(shù)；b.基于激光傳感器視角分辨率分別將每束激光束轉(zhuǎn)換為一組以傳感器中心坐標(biāo)系下極坐標(biāo)表征的點序列；c.將每組激光點序列的極坐標(biāo)轉(zhuǎn)換為傳感器中心坐標(biāo)系下的笛卡爾坐標(biāo)；5)通過尋找步驟4)中兩組相鄰激光束中激光點的一一對應(yīng)的關(guān)系，求取兩個相鄰時刻間車輛行駛的位移和航向角，具體步驟為a.構(gòu)建一個鏈?zhǔn)綏l件隨機場圖模型，并將激光點序列及其關(guān)聯(lián)關(guān)系分別映射為該鏈?zhǔn)綏l件隨機場的節(jié)點和狀態(tài)；b.提取兩激光點序列間的幾何特征作為條件隨機場圖模型的局部特征和配對特征；c.基于從標(biāo)定數(shù)據(jù)學(xué)習(xí)得到的特征權(quán)重，通過圖模型的概率推理求取兩個相鄰激光點序列中激光點之間一一對應(yīng)的關(guān)系；d.通過優(yōu)化所有關(guān)聯(lián)激光點的距離累加和最小求取相鄰時刻間車輛行駛的位移和航向角。6)將步驟幻中求得的車輛行駛的位移和航向角與前一采樣時刻車輛的絕對位置合成出當(dāng)前采樣時刻車輛在地球中心坐標(biāo)系下的絕對位置；7)將當(dāng)前采樣時刻傳感器中心坐標(biāo)系下的以笛卡爾座標(biāo)表征的激光點序列與步驟5)中求取的車輛在地球中心坐標(biāo)系下的絕對位置合成，求取激光點序列在地球中心坐標(biāo)系下的坐標(biāo)；8)根據(jù)步驟2)中測得的激光傳感器中心與車輛中心的空間距離作為傳感器中心坐標(biāo)系原點與車輛中心坐標(biāo)系原點的空間偏移，進(jìn)一步修正步驟6)中的激光點序列在地球中心坐標(biāo)系下的坐標(biāo)，存儲激光點序列的坐標(biāo)，這些激光點坐標(biāo)描繪了當(dāng)前環(huán)境景物的距地面一定高度的橫截面輪廓；9)運行步驟幻至步驟8)直到車輛導(dǎo)航結(jié)束；車輛行駛?cè)讨械募す恻c序列建模了整個環(huán)境。有益效果與現(xiàn)有技術(shù)相比，本發(fā)明具有如下有益效果(1)本發(fā)明所提供方法無需像同類方法(見引證文獻(xiàn))那樣提取環(huán)境中的幾何基元作為特征，因此適用于城市街道等復(fù)雜環(huán)境；(2)通過配準(zhǔn)激光傳感器在相鄰采樣時刻的觀測求取車輛的位移和航向角，是一種自包含的方案，可有效避免同類方法中所使用的基于星座系統(tǒng)(例如GPQ的方案在極端環(huán)境中信號丟失的問題；(3)通過概率圖模型推理的方法處理激光束中的激光點序列，智能地利用并管理了景物輪廓的幾何特征，因此對環(huán)境建模的精確度更高。


下面結(jié)合附圖與實施案例進(jìn)一步說明本發(fā)明。圖1本發(fā)明的流程示意圖；圖2本發(fā)明所使用的三種坐標(biāo)系的示意圖3本發(fā)明相鄰兩時刻測量的兩束激光束中激光點關(guān)聯(lián)關(guān)系的示意圖；圖4本發(fā)明實施例中測試車輛行駛的軌跡與衛(wèi)星圖像匹配的效果圖；圖5本發(fā)明實施例中所建模的環(huán)境輪廓的效果圖；圖6本發(fā)明實施例中所建模的環(huán)境輪廓與衛(wèi)星圖像匹配的效果圖。
具體實施例方式為了使本發(fā)明實現(xiàn)的技術(shù)手段、創(chuàng)作特征、達(dá)成目的與功效易于明白了解，下面結(jié)合具體圖示，進(jìn)一步闡述本發(fā)明。本實施例的測試環(huán)境為城市街道，測試中的車輛的整體車程為1410m，平均車速為 40km/ho如圖1所示，本發(fā)明的實施例包括如下步驟第一步，在自動駕駛車的前端裝配激光傳感器。本實施例的實驗中使用民用轎車作為測試車輛，使用SICK (施克)LMS 221為測試激光傳感器，并安裝于測試車輛的前端距地面1. 2m處，面向車輛向前行駛的方向。本實施例配置其視角分辨率為0. 5度，即每束激光束包含361個激光點，其有效量程為80m。LMS221使用直流24V供電，最大輸入電流為 I-SA0數(shù)據(jù)的輸出接口為RS-232或RS-422。本實施例中使用RS 232協(xié)議通過車載電腦接收并存儲激光傳感器數(shù)據(jù)。當(dāng)車輛行駛時激光掃描傳感器開始按照固定頻率采集激光束觀測；第二步，測量激光傳感器中心與車輛中心的空間距離，圖2所示，本發(fā)明所提供方法中共需要三種坐標(biāo)系地球中心坐標(biāo)系G，車輛中心坐標(biāo)系V和傳感器中心坐標(biāo)系S。本實施例中，激光傳感器安裝于車輛前端，其與車輛中心的空間距離作為傳感器中心坐標(biāo)系S 在車輛中心坐標(biāo)系V中的空間偏移，為xsv = 1. 62m, ysv = 0. 45m, rsv = 5° ；第三步，確定車輛當(dāng)前采樣時刻在地球中心坐標(biāo)系下的位置。首次運行系統(tǒng)時，需借助其它傳感器觀測車輛在地球中心坐標(biāo)系下的初始位置。為了獲得車輛在初始狀態(tài)的絕對位置，本實施例中測試車輛還裝配了 GPS傳感器，且GPS傳感器的天線基本與車輛中心重合，因此，GPS測量值可認(rèn)為是車輛中心坐標(biāo)系原點在地球中心坐標(biāo)系下的坐標(biāo)，即車輛的絕對位置；第四步，預(yù)處理車輛行駛過程中在相鄰兩采樣時刻所采集的兩束激光束中返回的距離讀數(shù)序列r，具體步驟為a.剔除兩組激光束的讀數(shù)序列中大于傳感器極限距離的距離讀數(shù)，本實施例中激光傳感器測距的極限距離為80m，故剔除掉返回距離大于80m的讀數(shù)；b.基于激光傳感器視角分辨率分別將每束激光束轉(zhuǎn)換為一組以傳感器中心坐標(biāo)系下極坐標(biāo)表征的點序列。本實施例中，激光傳感器視角分辨率為0. 5度，每束激光束包含 361個激光點，則初始激光點序列對應(yīng)的極坐標(biāo)中的偏角為φ = {φ” φ2, φ3，…，φ 360} = {0° ，0.5° ，1.0° ，...，180° }基于a中剔除激光點的序號，可得到對應(yīng)不超限距離序列的偏角；c.將每組激光點序列的極坐標(biāo)轉(zhuǎn)換為傳感器中心坐標(biāo)系下的笛卡爾坐標(biāo)。對于一個極坐標(biāo)(Y，Φ)的激光點，它的笛卡爾坐標(biāo)(X，y)可依據(jù)如下求得χ = rX cos Φ , y = rX sin Φ
第五步，通過尋找第四步求得的兩組相鄰激光束中激光點的一一對應(yīng)的關(guān)系，求取兩個相鄰時刻間車輛行駛的位移和航向角，具體步驟為a.構(gòu)建一個鏈?zhǔn)綏l件隨機場圖模型，并將激光點序列及其關(guān)聯(lián)關(guān)系分別映射為該鏈?zhǔn)綏l件隨機場的節(jié)點和狀態(tài)；該步驟僅需運行一次即可。本實施例中所構(gòu)建的鏈?zhǔn)綏l件隨機場圖模型只包含兩種勢函數(shù)局部勢和配對勢。其中將參考激光束和新激光束所包含激光點作為觀測量ζ。參考激光束包含N個激光點{/丄f. I· ·■■■⑶， 它們各自的可能的關(guān)聯(lián)狀態(tài)映射為條件隨機場圖中的節(jié)點。新激光束包含M個激光點 {Llw. I:i..w. .... I^U。條件隨機場圖中的任一節(jié)點Xi都對應(yīng)了 Μ+1種狀態(tài)的似然。例如，Xi的狀態(tài)1表示Xi所對應(yīng)的參考激光束中的激光點 4ι與新激光束的激光點關(guān)聯(lián)的似然。Xi狀態(tài)2表示￡“與新激光束的激光點/.iL關(guān)聯(lián)的似然，依次類推，狀態(tài)M+1表示
在新激光束中找不到相關(guān)聯(lián)的激光點的似然，即，化r是孤立點的似然。b.提取兩激光點序列間的幾何特征作為條件隨機場圖模型的局部特征和配對特征。本實施例中提取兩束激光束中激光點之間的距離、角、測地線和輻射等幾何特征作為局部特征，提取配對序列、配對孤立點和配對距離為配對特征；c.基于從標(biāo)定數(shù)據(jù)學(xué)習(xí)得到的特征權(quán)重，通過圖模型的概率推理求取兩個相鄰激光點序列中激光點之間一一對應(yīng)的關(guān)系。本實施例中的特征最佳權(quán)重依據(jù)20組標(biāo)定數(shù)據(jù)并使用最大偽似然(Maximum pseudo-likelihood, MPL)學(xué)習(xí)方法得到，激光點間的關(guān)聯(lián)關(guān)系通過最大和(Max-sum)概率推理得到。如圖3所示，相鄰兩束激光束建立了關(guān)聯(lián)關(guān)系；d.通過優(yōu)化所有關(guān)聯(lián)激光點的距離累加和最小求取相鄰時刻間車輛行駛的位移 T = [Tx, Ty]和航向角R。本實施例中，車輛的位移和航向角組成車輛在相鄰兩時刻間的相對運動RT = [R，T]，它可通過優(yōu)化下式來求得閉合的解
權(quán)利要求
1.一種適用于自動駕駛車導(dǎo)航的環(huán)境建模方法，包括如下步驟1)在自動駕駛車的前端裝配激光傳感器，當(dāng)車輛行駛時激光掃描傳感器開始按照固定頻率采集激光束觀測；2)測量激光傳感器中心與車輛中心的空間距離；3)確定車輛當(dāng)前采樣時刻在地球中心坐標(biāo)系下的位置，首次運行系統(tǒng)時，需借助其它傳感器觀測車輛在地球中心坐標(biāo)系下的初始位置；4)預(yù)處理車輛行駛過程中在相鄰兩采樣時刻所采集的兩束激光束中返回的距離讀數(shù)序列；5)通過尋找步驟4)中兩組相鄰激光束中激光點的一一對應(yīng)的關(guān)系，求取兩個相鄰時刻間車輛行駛的位移和航向角；6)將步驟幻中求得的車輛行駛的位移和航向角與前一采樣時刻車輛的絕對位置合成出當(dāng)前采樣時刻車輛在地球中心坐標(biāo)系下的絕對位置；7)將當(dāng)前采樣時刻傳感器中心坐標(biāo)系下的以笛卡爾座標(biāo)表征的激光點序列與步驟5) 中求取的車輛在地球中心坐標(biāo)系下的絕對位置合成，求取激光點序列在地球中心坐標(biāo)系下的坐標(biāo)；8)根據(jù)步驟2)中測得的激光傳感器中心與車輛中心的空間距離作為傳感器中心坐標(biāo)系原點與車輛中心坐標(biāo)系原點的空間偏移，進(jìn)一步修正步驟6)中的激光點序列在地球中心坐標(biāo)系下的坐標(biāo)，存儲激光點序列的坐標(biāo)，這些激光點坐標(biāo)描繪了當(dāng)前環(huán)境景物的距地面一定高度的橫截面輪廓；9)運行步驟幻至步驟8)直到車輛導(dǎo)航結(jié)束，車輛行駛?cè)讨械募す恻c序列建模了整個環(huán)境。
2.根據(jù)權(quán)利要求1所述的環(huán)境建模方法，其特征在于所述步驟4)具體步驟為a.剔除兩組激光束的讀數(shù)序列中大于傳感器極限觀測距離的距離讀數(shù)；b.基于激光傳感器視角分辨率分別將每束激光束轉(zhuǎn)換為一組以傳感器中心坐標(biāo)系下極坐標(biāo)表征的點序列；c.將每組激光點序列的極坐標(biāo)轉(zhuǎn)換為傳感器中心坐標(biāo)系下的笛卡爾坐標(biāo)。
3.根據(jù)權(quán)利要求1所述的環(huán)境建模方法，其特征在于所述步驟幻具體步驟為a.構(gòu)建一個鏈?zhǔn)綏l件隨機場圖模型，并將激光點序列及其關(guān)聯(lián)關(guān)系分別映射為該鏈?zhǔn)綏l件隨機場的節(jié)點和狀態(tài)；b.提取兩激光點序列間的幾何特征作為條件隨機場圖模型的局部特征和配對特征；c.基于從標(biāo)定數(shù)據(jù)學(xué)習(xí)得到的特征權(quán)重，通過圖模型的概率推理求取兩個相鄰激光點序列中激光點之間一一對應(yīng)的關(guān)系；d.通過優(yōu)化所有關(guān)聯(lián)激光點的距離累加和最小求取相鄰時刻間車輛行駛的位移和航向角。
全文摘要
本發(fā)明為一種適用于自動駕駛車導(dǎo)航的環(huán)境建模方法。自動駕駛車的導(dǎo)航需解決的關(guān)鍵問題之一是建模車輛行駛的環(huán)境，辨識其中的景物；將環(huán)境信息轉(zhuǎn)化為自動駕駛車智能避障、路徑規(guī)劃可用的參數(shù)化信息。本發(fā)明采取在自動駕駛車的前端裝配激光傳感器；測量激光傳感器中心與車輛中心的空間距離等一系列步驟；利用車輛行駛過程中采集的激光點序列建模整個環(huán)境。通過配準(zhǔn)激光傳感器在相鄰采樣時刻的觀測求取車輛的位移和航向角，是一種自包含的方案，可有效避免同類方法中所使用的基于星座系統(tǒng)的方案在極端環(huán)境中信號丟失的問題；基于概率圖模型推理的方法處理激光束中的激光點序列，智能地利用并管理了景物輪廓的幾何特征，因此對環(huán)境建模的精確度更高。
文檔編號G01C21/26GK102353379SQ20111018818
公開日2012年2月15日 申請日期2011年7月6日 優(yōu)先權(quán)日2011年7月6日
發(fā)明者孫作雷, 曾連蓀, 楊寧 申請人:上海海事大學(xué)