基于概率分析的環(huán)境構(gòu)建方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)器人或無人車技術(shù)領(lǐng)域，具體涉及一種基于概率分析的環(huán)境構(gòu)建方 法
【背景技術(shù)】
[0002] 環(huán)境構(gòu)建主要是指機(jī)器人對外部未知環(huán)境進(jìn)行自主識(shí)別與理解，構(gòu)建或恢復(fù)真實(shí) 的環(huán)境信息，是機(jī)器人進(jìn)行自主導(dǎo)航所必需的關(guān)鍵技術(shù)，具有重要的研究意義與應(yīng)用價(jià)值。 由于激光雷達(dá)測距與定位精度高、性價(jià)比高等特點(diǎn)，利用激光雷達(dá)構(gòu)建未知的環(huán)境成為重 要的研究內(nèi)容。在獲取激光雷達(dá)點(diǎn)云數(shù)據(jù)后，傳統(tǒng)的環(huán)境建模方法是通過帶高斯噪聲概率 誤差模型補(bǔ)償位置誤差，從不精確的數(shù)據(jù)中恢復(fù)精確的環(huán)境模型，并假定下一時(shí)刻，同一目 標(biāo)會(huì)被激光雷達(dá)重復(fù)掃描。
[0003] 而實(shí)際中，由于點(diǎn)云數(shù)據(jù)的有限性，恢復(fù)的環(huán)境模型是不精確的也不會(huì)出現(xiàn)點(diǎn)云 數(shù)據(jù)重疊。因此，如何依據(jù)有限的點(diǎn)云數(shù)據(jù)，準(zhǔn)確地構(gòu)建環(huán)境成為亟待解決的技術(shù)問題。

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

[0004] (一)要解決的技術(shù)問題
[0005] 本發(fā)明要解決的技術(shù)問題是:如何依據(jù)有限的點(diǎn)云數(shù)據(jù)，準(zhǔn)確地構(gòu)建環(huán)境。
[0006] (二)技術(shù)方案
[0007] 為了解決上述技術(shù)問題，本發(fā)明提供了一種基于概率分析的環(huán)境構(gòu)建方法，包括 以下步驟：
[0008] S1、建立機(jī)器人的位姿誤差模型，獲取激光雷達(dá)的點(diǎn)云數(shù)據(jù)；
[0009] S2、利用步驟S1建立的位姿誤差模型，確定障礙物滿足的概率統(tǒng)計(jì)測試；
[0010] S3、通過二維柵格單元及其鄰域進(jìn)行障礙物測試，生成地形分類地圖，從而實(shí)現(xiàn)環(huán) 境構(gòu)建。
[0011](三)有益效果
[0012] 本發(fā)明通過一種基于概率分析的環(huán)境構(gòu)建方法利用激光雷達(dá)提供的點(diǎn)云數(shù)據(jù)，對 機(jī)器人當(dāng)前的未知外部環(huán)境進(jìn)行統(tǒng)計(jì)分析，將激光雷達(dá)采集的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換成可通行、未 知以及障礙物等三類狀態(tài)的地形分類地圖，從而實(shí)現(xiàn)準(zhǔn)確地構(gòu)建環(huán)境，提高了環(huán)境構(gòu)建的 精度，為機(jī)器人自主導(dǎo)航時(shí)的障礙規(guī)避提供了可靠保障。
【具體實(shí)施方式】
[0013] 為使本發(fā)明的目的、內(nèi)容、和優(yōu)點(diǎn)更加清楚，下面結(jié)合實(shí)施例，對本發(fā)明的具體實(shí) 施方式作進(jìn)一步詳細(xì)描述。
[0014] 本發(fā)明實(shí)施例提供的一種基于概率分析的環(huán)境地圖構(gòu)建方法，包括以下步驟：
[0015] 步驟S1:建立機(jī)器人的位姿誤差模型，獲取激光雷達(dá)點(diǎn)云數(shù)據(jù)；
[0016] 為了補(bǔ)償因機(jī)器人橫滾角、俯仰角導(dǎo)致的分類誤差，以一階馬爾科夫模型作為機(jī) 器人的位姿誤差模型，該模型如式(1)所示：
[0018] 其中，（Xt，yt，zt)表示t時(shí)刻機(jī)器人的位置；（(^，0(，(^)表示七時(shí)刻機(jī)器人的姿態(tài) 角;wt表示隨時(shí)間變化、服從正態(tài)分布:Ν(^，R)的高斯噪聲;vt表示瞬間的、服從正態(tài)分布： N(0，Q)的高斯噪聲;R和Q表示時(shí)不變的噪聲協(xié)方差矩陣，分別用式(2)與式(3)表示：
[0022]定義激光雷達(dá)在t時(shí)刻第i個(gè)元素的觀測數(shù)據(jù)為〇*/，觀測距離為rf'，觀測角為 #/,則激光雷達(dá)的點(diǎn)云數(shù)據(jù)(X/, 17, Z/):通過式(4)計(jì)算得到：
[0024] 其中，Tt表示t時(shí)刻由機(jī)器人姿態(tài)角構(gòu)成的旋轉(zhuǎn)矩陣;^與1表示激光雷達(dá)相對于 機(jī)器人局部坐標(biāo)系的偏移量和方向角。
[0025] 步驟S2:確定基于概率統(tǒng)計(jì)的障礙物測試；
[0026] 利用步驟S1建立的一階馬爾科夫模型，將障礙物測試轉(zhuǎn)化為基于概率統(tǒng)計(jì)的問 題。給定兩點(diǎn)（XuSYuSZu1)和0丨 2;_，1/, Z;/)，t2>tl，高度差分的概率分布如式（5)所 示：
[0028] 漢（|筆-私| (^ + ? -^)母％(幻）表示正態(tài)分布，-Z引 值，w))7 (2ρ+(/: 表示方差。
[0029] 其中，表示值ZtklW的雅克比矩陣(k=l，2)，上標(biāo)i(j)表示i或j。假定觀 測角?^ = ?^并忽略激光雷達(dá)的偏移量Xs，則V/^Z)用式(6)近似表示。
[0031]合并式(5)和式(6)，得式(7):
[0033]因此，基于概率統(tǒng)計(jì)的障礙物測試表示為：
[0035] 其中，Η表示障礙物高度閾值;λ表示誤差概率閾值，并令λ = 〇 . 03。則上式(8)可表 示為如式(9)所示：
[0037]由上式(9)可知，障礙物測試的概率隨著時(shí)間差分的增加逐漸降低。因此，障礙物 測試的判斷依據(jù)主要集中在激光雷達(dá)點(diǎn)云數(shù)據(jù)中鄰近的兩點(diǎn)。
[0038]步驟S3:通過二維柵格單元及其鄰域進(jìn)行障礙物測試，生成地形分類地圖，從而實(shí) 現(xiàn)環(huán)境構(gòu)建。
[0039]二維柵格地圖包含了環(huán)境的有用信息。當(dāng)分析一組新的點(diǎn)云數(shù)據(jù)時(shí)，基于概率分 析的方法需要相對應(yīng)的2維柵格單元及其領(lǐng)域，其中，每個(gè)柵格單元存儲(chǔ)一個(gè)相對較大的值 Zmax和一個(gè)相對較小的值Zmin。在柵格單元中當(dāng)觀測到一個(gè)新的數(shù)據(jù)時(shí)，分別與其領(lǐng)域中的 Zmax和Zmin比較，如果一個(gè)滿足步驟2障礙物測試，則標(biāo)記該柵格單元狀態(tài)為障礙物；同時(shí)，在 柵格單元中，當(dāng)觀測值Z小于該Z_或大于Z max時(shí)，則分別用觀測值Z替代。假定柵格單元初始 狀態(tài)為未知狀態(tài)，當(dāng)有新的觀測數(shù)據(jù)時(shí)，改變柵格單元及其鄰域的狀態(tài)為可通行狀態(tài)，如果 柵格單元滿足障礙物測試，則改變柵格單元及其鄰域狀態(tài)為障礙物，若不滿足，仍為可通行 狀態(tài)，沒有觀測數(shù)據(jù)的柵格單元為初始的未知狀態(tài)，進(jìn)而生成包含未知、可通行、障礙物三 種狀態(tài)的地形分類地圖。
[0040] 通過以上步驟，基于概率分析的環(huán)境構(gòu)建方法將激光雷達(dá)觀測到的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換 為包含可通行、未知以及障礙物等三種狀態(tài)的柵格地圖，為機(jī)器人自主導(dǎo)航時(shí)的障礙規(guī)避 提供可靠保障。
[0041] 以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人 員來說，在不脫離本發(fā)明技術(shù)原理的前提下，還可以做出若干改進(jìn)和變形，這些改進(jìn)和變形 也應(yīng)視為本發(fā)明的保護(hù)范圍。
【主權(quán)項(xiàng)】
1. 一種基于概率分析的環(huán)境構(gòu)建方法，其特征在于，包括W下步驟： 51、 建立機(jī)器人的位姿誤差模型，獲取激光雷達(dá)的點(diǎn)云數(shù)據(jù)； 52、 利用步驟S1建立的位姿誤差模型，確定障礙物滿足的概率統(tǒng)計(jì)測試； 53、 通過二維柵格單元及其鄰域進(jìn)行障礙物測試，生成地形分類地圖，從而實(shí)現(xiàn)環(huán)境構(gòu) 建。2. 如權(quán)利要求1所述的方法，其特征在于，步驟S1中，W-階馬爾科夫模型作為機(jī)器人 的位姿誤差模型，即式(1):(1) 其中，（xt，yt，zt)表示t時(shí)刻機(jī)器人的位置；（Φ t，0t，Φ t)表示t時(shí)刻機(jī)器人的姿態(tài)角;wt 表示隨時(shí)間變化、服從正態(tài)分布:N(wt-i，R)的高斯噪聲;V康示瞬間的、服從正態(tài)分布:N(0， Q)的高斯噪聲;R和Q表示時(shí)不變的噪聲協(xié)方差矩陣，分別用式(2)與式(3)表示：根據(jù)觀測數(shù)據(jù)，得到準(zhǔn)確的機(jī)器人位姿信息。 定義激光雷達(dá)在t時(shí)刻第i個(gè)元素的觀測數(shù)據(jù)為〇/，/的)，觀測距離為r/，觀測角為#/'， 則激光雷達(dá)的點(diǎn)云數(shù)據(jù)07，Γ/，通過式(4)計(jì)算得到：(句 其中，Tt表示t時(shí)刻由機(jī)器人姿態(tài)角構(gòu)成的旋轉(zhuǎn)矩陣；Xs與Ts表示激光雷達(dá)相對于機(jī)器 人局部坐標(biāo)系的偏移量和方向角。3. 如權(quán)利要求2所述的方法，其特征在于，步驟S2具體為： 給定兩點(diǎn)(Xtii，Ytii，2*11)和<又乂片/，名/)，t2〉tl，高度差分的概率分布如式(5)所 示：(5)良不正態(tài)分布：表不均值， (▽:;i(z))'口 ρ+的-/.I)巧)7/:(2)表示方差； 其中，▽tkiW(Z)表示值ZtkiW的雅克比矩陣化=1，2)，上標(biāo)i(j)表示i或j。假定觀測角 0tii = i3t2^并忽略激光雷達(dá)的偏移量xs，則▽tkiW(Z)用式(6)近似表示；其中，Η表示障礙物高度闊值;λ表示誤差概率闊值，并令λ = 〇.03。則上式(8)可表示為 式(9):(9)4. 如權(quán)利要求3所述的方法，其特征在于，步驟S3具體為:假定二維柵格單元初始狀態(tài) 為未知狀態(tài)，當(dāng)有新的觀測數(shù)據(jù)時(shí)，改變二維柵格單元及其鄰域的狀態(tài)為可通行狀態(tài)，如果 二維柵格單元滿足步驟S2的障礙物測試，則改變二維柵格單元及其鄰域狀態(tài)為障礙物，若 不滿足，仍為可通行狀態(tài)，沒有觀測數(shù)據(jù)的二維柵格單元為初始的未知狀態(tài)，進(jìn)而生成包含 未知、可通行、障礙物Ξ種狀態(tài)的地形分類地圖。5. 如權(quán)利要求4所述的方法，其特征在于，判斷是否滿足步驟S2的障礙物測試具體為： 二維柵格單元的每個(gè)柵格單元存儲(chǔ)一個(gè)相對較大的值Zmax和一個(gè)相對較小的值Zmin，在柵格 單元中當(dāng)觀測到一個(gè)新的數(shù)據(jù)時(shí)，分別與其領(lǐng)域中的Zmax和Zmin比較，如果一個(gè)滿足步驟S2 的障礙物測試，則標(biāo)記該柵格單元狀態(tài)為障礙物；同時(shí)，在柵格單元中，當(dāng)觀測值Z小于Zmin 或大于Zmax時(shí)，則用觀測值Z替代Zmin或Zmax。
【專利摘要】本發(fā)明涉及一種基于概率分析的環(huán)境構(gòu)建方法，屬于機(jī)器人或無人車技術(shù)領(lǐng)域。本發(fā)明通過一種基于概率分析的環(huán)境構(gòu)建方法利用激光雷達(dá)提供的點(diǎn)云數(shù)據(jù)，對機(jī)器人當(dāng)前的未知外部環(huán)境進(jìn)行統(tǒng)計(jì)分析，將激光雷達(dá)采集的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換成可通行、未知以及障礙物等三類狀態(tài)的地形分類地圖，從而實(shí)現(xiàn)準(zhǔn)確地構(gòu)建環(huán)境，提高了環(huán)境構(gòu)建的精度，為機(jī)器人自主導(dǎo)航時(shí)的障礙規(guī)避提供了可靠保障。
【IPC分類】G05D1/02
【公開號】CN105573318
【申請?zhí)枴緾N201510940000
【發(fā)明人】靳璐, 蘇波, 康曉, 吳越
【申請人】中國北方車輛研究所
【公開日】2016年5月11日
【申請日】2015年12月15日