一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法及系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法，首先接收與預(yù)處理激光雷達(dá)數(shù)據(jù)；分割聚類環(huán)境數(shù)據(jù)；然后從激光雷達(dá)數(shù)據(jù)中選擇簇群；并對目標(biāo)簇群進(jìn)行跟蹤；獲取簇群中每個(gè)子簇群的頂點(diǎn)在局部坐標(biāo)系下的坐標(biāo)數(shù)據(jù)，獲取簇群的頂點(diǎn)的坐標(biāo)和每個(gè)連接連線的傾角；將起始點(diǎn)和終點(diǎn)組成連接直線并計(jì)算偏轉(zhuǎn)角度；通過滑動濾波計(jì)算機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)；最后初始化起始點(diǎn)的位姿和采樣位姿點(diǎn)。本發(fā)明提供的移動機(jī)器人位姿推算方法是使用精度較高的二維激光雷達(dá)，無需對環(huán)境進(jìn)行任何修改；機(jī)器人的位姿信息相對于具體環(huán)境客觀描述，更便捷將位姿信息集成到其它移動機(jī)器人的業(yè)務(wù)模塊中，提高系統(tǒng)的可靠性、易實(shí)現(xiàn)性并降低成本。
【專利說明】
一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法及系統(tǒng)
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及移動機(jī)器人及智能車輛的局部導(dǎo)航領(lǐng)域，特別涉及一種基于激光雷達(dá) 數(shù)據(jù)的移動機(jī)器人位姿推算方法。
【背景技術(shù)】
[0002] 機(jī)器人作為一種新型的生產(chǎn)工具，表現(xiàn)出極大的優(yōu)越性。在減輕勞動強(qiáng)度，提高生 產(chǎn)效率，改變生產(chǎn)模式等方面可將人們從危險(xiǎn)、惡劣、繁重的工作環(huán)境中解放出來。然而隨 著機(jī)器人的不斷發(fā)展，人們逐漸發(fā)現(xiàn)這些固定于某一位置操作的機(jī)器人對很多應(yīng)用需求均 不能滿足。因此在20世紀(jì)80年代后期，許多國家開始有計(jì)劃地開展了移動機(jī)器人技術(shù)的研 究。
[0003] 對于移動機(jī)器人而言，導(dǎo)航是一個(gè)重要的方向。而移動機(jī)器人的導(dǎo)航的前提就是 對機(jī)器人的定位和姿態(tài)識別。精確的位姿推算對于移動機(jī)器人的定位、自動地圖生成、路徑 規(guī)劃和控制、目標(biāo)檢測和跟蹤等具有重要意義。在二維的平面坐標(biāo)系下，移動機(jī)器人的位姿 就是機(jī)器人在全局坐標(biāo)系中的位置以及機(jī)器人的車體方向，即表示為：（x，y，9)。機(jī)器人的 位置可以用機(jī)器人質(zhì)心坐標(biāo)點(diǎn)表不為，而用車體前方偏向全局坐標(biāo)系X軸的夾角來表不車 體的方向。在本文中，定義方向角為：以X軸0度，順時(shí)針方向?yàn)樨?fù)，逆時(shí)針方向?yàn)檎?如圖1所 示，圖1機(jī)器人的位姿模型；圖中XgOgYg為機(jī)器所在的全局坐標(biāo)系;XrOrYr為機(jī)器人所在的局 部坐標(biāo)系；（x，y)為機(jī)器人質(zhì)心的坐標(biāo);Θ為機(jī)器人相對于起始點(diǎn)的位姿偏轉(zhuǎn)角。應(yīng)用在移動 機(jī)器人的位姿推算方法一般可分為兩種:航位推算(Dead-Reckoning)是一種非常經(jīng)典的相 對位姿推算方法，通過安裝在移動機(jī)器人輪子上的光電編碼器輸出的輪速與移動機(jī)器人的 運(yùn)動學(xué)模型，累積每個(gè)采樣周期下的移動機(jī)器人位移以及角度偏轉(zhuǎn)。航位推算的優(yōu)點(diǎn)是方 法簡單、價(jià)格低廉，但從其本質(zhì)上上可以看出，航位推算方法的精度容易受輪速的標(biāo)定、車 輪與地面的打滑、顛簸等因素造成積分誤差而大大降低，因此誤差較大。慣性導(dǎo)航系統(tǒng)INS (InertialNavigation Systems)常使用陀螺儀得到機(jī)器人在每個(gè)方向上的角加速度，經(jīng)過 對角加速度的積分就可以得到偏轉(zhuǎn)角度，使用加速度傳感器得到機(jī)器人的加速度信息，并 結(jié)合角度信息，對加速度進(jìn)行矢量積分獲得機(jī)器人的位置信息，從而獲得移動機(jī)器人在不 同采樣時(shí)刻的位姿信息。一般情況下，慣性導(dǎo)航系統(tǒng)的精度高于航位推算，但是其精度也要 受陀螺儀漂移、標(biāo)定誤差、敏感度等問題影響。
[0004] 無論是航位推算還是慣性導(dǎo)航系統(tǒng)，都存在一個(gè)共同的缺點(diǎn):存在積分誤差，移動 機(jī)器人的行程越遠(yuǎn)，該積分誤差越大。因此現(xiàn)有的位姿推算方法并不適用于對機(jī)器人位姿 信息要求較高的場合。如何實(shí)現(xiàn)更加精確的且不受地面情況限制的位姿推算方法，是一個(gè) 很重要的問題。
[0005] 因此，需要一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿描述方法。

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

[0006] 本發(fā)明的目的就是提供一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿描述方法。
[0007] 本發(fā)明的目的是通過這樣的技術(shù)方案實(shí)現(xiàn)的：
[0008] 本發(fā)明提供的一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法，包括以下步 驟：
[0009] Stepl :接收與預(yù)處理激光雷達(dá)數(shù)據(jù)；
[0010] Step2:分割聚類環(huán)境數(shù)據(jù)；
[0011] Step3:從激光雷達(dá)數(shù)據(jù)中選擇簇群；
[0012] Step4:目標(biāo)簇群的跟蹤;獲取簇群中每個(gè)子簇群的頂點(diǎn)在局部坐標(biāo)系下的坐標(biāo)數(shù) 據(jù)，獲取簇群的頂點(diǎn)的坐標(biāo)和每個(gè)連接連線的傾角；
[0013] Step5:將起始點(diǎn)和終點(diǎn)組成連接直線并計(jì)算簇群中連接直線的偏轉(zhuǎn)角度；
[0014] Step6:通過滑動濾波計(jì)算機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)；
[0015] Step7:初始化起點(diǎn)的位姿并存儲位置坐標(biāo)。
[0016]進(jìn)一步，所述步驟Stepl中的接收與預(yù)處理激光雷達(dá)數(shù)據(jù)的具體步驟如下：
[0017] 通過與計(jì)算機(jī)連接的二維掃描型激光雷達(dá)傳感器獲取環(huán)境數(shù)據(jù)；
[0018] 將環(huán)境數(shù)據(jù)以數(shù)組形式存儲于計(jì)算機(jī)中，所述環(huán)境數(shù)據(jù)包括距離信息、激光雷達(dá) 掃描范圍和角度分辨率；
[0019] 預(yù)處理距離數(shù)據(jù)，包括去除有效范圍外的數(shù)據(jù)點(diǎn)、濾除孤立的噪聲點(diǎn)及對激光雷 達(dá)測量機(jī)制的缺陷補(bǔ)償；
[0020] 將環(huán)境數(shù)據(jù)從激光雷達(dá)的極坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)化為機(jī)器人或智能車輛的局部直 角坐標(biāo)系坐標(biāo)。
[0021] 進(jìn)一步，所述步驟Step2中的按照以下步驟來分割聚類環(huán)境數(shù)據(jù)：
[0022]循環(huán)比較距離數(shù)據(jù)的每兩個(gè)相鄰數(shù)據(jù)點(diǎn)差值，標(biāo)記差值大于分割閾值的數(shù)據(jù)點(diǎn)為 分割點(diǎn)；
[0023]存儲分割點(diǎn)的角標(biāo)；
[0024]進(jìn)一步，所述步驟Step3中從激光雷達(dá)數(shù)據(jù)中選擇簇群的具體步驟如下：
[0025]計(jì)算簇群中相對應(yīng)起始點(diǎn)和終點(diǎn)連接直線；
[0026]計(jì)算簇群中數(shù)據(jù)點(diǎn)與連接直線的距離；
[0027]獲取距離直線最遠(yuǎn)的數(shù)據(jù)點(diǎn)為頂點(diǎn)；
[0028]計(jì)算頂點(diǎn)坐標(biāo)；
[0029]計(jì)算連接直線的傾角。
[0030]進(jìn)一步，所述步驟Step5中的連接直線及偏轉(zhuǎn)角度按照以下步驟來實(shí)現(xiàn)：
[0031]獲取機(jī)器人相對于起始位置的A'位置坐標(biāo)和角度信息Φ ;
[0032] 對機(jī)器人的角度信息進(jìn)行滑動濾波；
[0033] 對簇群的角度信息進(jìn)行滑動濾波；
[0034] 得到B'位置機(jī)器人相對起始位置的角度信息；
[0035] 通過對角度信息進(jìn)行均值濾波得到機(jī)器人在B'位置的偏轉(zhuǎn)角度：
[0037]其中，ΘΒ'表不機(jī)器人在B'位置的偏轉(zhuǎn)角度;η表不一幀環(huán)境數(shù)據(jù)中經(jīng)過分割算法分 割出來的簇群總數(shù);i表示η個(gè)簇群中第i個(gè)子簇群;&.,表示第i個(gè)子簇群在j*y時(shí)刻的位姿 偏轉(zhuǎn)角度；氣#,表示第i個(gè)子簇群在(j+l)*y時(shí)刻的位姿偏轉(zhuǎn)角度；φ表示機(jī)器人在A'位置 的偏轉(zhuǎn)角度。
[0038] 進(jìn)一步，所述步驟Step6中機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)按照以下步驟來計(jì) 算：
[0039] 獲取機(jī)器人相對應(yīng)起始點(diǎn)的A'位置坐標(biāo)(x，y);
[0040] 通過以下公式來計(jì)算機(jī)器人在B'位置的坐標(biāo)：
[0042] 其中，Lrqbcit為機(jī)器人長度，XB'表不機(jī)器人在B'位置的橫坐標(biāo);yB'表不機(jī)器人在B' 位置的縱坐標(biāo)；| |CF| |表示機(jī)器人質(zhì)心從A'位置到B'位置的縱坐標(biāo)距離；| |BC| |表示機(jī)器 人質(zhì)心從A '位置到B '位置的橫坐標(biāo)距離。
[0043]本發(fā)明還提供了一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算系統(tǒng)，包括預(yù)處理 模塊、分割聚類模塊、簇群選擇模塊、簇群跟蹤模塊、偏轉(zhuǎn)角度計(jì)算模塊、位置坐標(biāo)計(jì)算模塊 和存儲模塊；
[0044]所述預(yù)處理模塊，用于接收與預(yù)處理激光雷達(dá)數(shù)據(jù)；
[0045]所述分割聚類模塊，用于分割聚類環(huán)境數(shù)據(jù)；
[0046]所述簇群選擇模塊，用于從激光雷達(dá)數(shù)據(jù)中選擇簇群；
[0047] 所述簇群跟蹤模塊，用于獲取簇群中每個(gè)子簇群的頂點(diǎn)在局部坐標(biāo)系下的坐標(biāo)數(shù) 據(jù)，獲取簇群的頂點(diǎn)的坐標(biāo)和每個(gè)連接連線的傾角；
[0048] 所述偏轉(zhuǎn)角度計(jì)算模塊，用于將起始點(diǎn)和終點(diǎn)組成連接直線并計(jì)算簇群中連接直 線的偏轉(zhuǎn)角度；
[0049] 所述位置坐標(biāo)計(jì)算模塊，用于通過滑動濾波計(jì)算機(jī)器人位置坐標(biāo)變化后的位置坐 標(biāo)；
[0050] 所述存儲模塊，用于初始化起點(diǎn)的位姿并存儲位置坐標(biāo)。
[0051] 進(jìn)一步，所述預(yù)處理模塊中的接收與預(yù)處理激光雷達(dá)數(shù)據(jù)的具體步驟如下：
[0052] 通過與計(jì)算機(jī)連接的二維掃描型激光雷達(dá)傳感器獲取環(huán)境數(shù)據(jù)；
[0053]將環(huán)境數(shù)據(jù)以數(shù)組形式存儲于計(jì)算機(jī)中，所述環(huán)境數(shù)據(jù)包括距離信息、激光雷達(dá) 掃描范圍和角度分辨率；
[0054]預(yù)處理距離數(shù)據(jù)，包括去除有效范圍外的數(shù)據(jù)點(diǎn)、濾除孤立的噪聲點(diǎn)及對激光雷 達(dá)測量機(jī)制的缺陷補(bǔ)償；
[0055] 將環(huán)境數(shù)據(jù)從激光雷達(dá)的極坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)化為機(jī)器人或智能車輛的局部直 角坐標(biāo)系坐標(biāo)。
[0056] 進(jìn)一步，所述簇群選擇模塊中從激光雷達(dá)數(shù)據(jù)中選擇簇群的具體步驟如下：
[0057] 計(jì)算簇群中相對應(yīng)起始點(diǎn)和終點(diǎn)連接直線；
[0058]計(jì)算簇群中數(shù)據(jù)點(diǎn)與連接直線的距離；
[0059]獲取距離直線最遠(yuǎn)的數(shù)據(jù)點(diǎn)為頂點(diǎn)；
[0060]計(jì)算頂點(diǎn)坐標(biāo)；
[0061] 計(jì)算連接直線的傾角。
[0062] 所述偏轉(zhuǎn)角度計(jì)算模塊中的連接直線及偏轉(zhuǎn)角度按照以下步驟來實(shí)現(xiàn)：
[0063]獲取機(jī)器人相對于起始位置的A'位置坐標(biāo)和角度信息Φ ;
[0064] 對機(jī)器人的角度信息進(jìn)行滑動濾波；
[0065] 對簇群的角度信息進(jìn)行滑動濾波；
[0066] 得到B'位置機(jī)器人相對起始位置的角度信息；
[0067] 通過對角度信息進(jìn)行均值濾波得到機(jī)器人在B'位置的偏轉(zhuǎn)角度：
[0069]其中，ΘΒ'表不機(jī)器人在B'位置的偏轉(zhuǎn)角度;η表不一幀環(huán)境數(shù)據(jù)中經(jīng)過分割算法分 割出來的簇群總數(shù);i表示η個(gè)簇群中第i個(gè)子簇群；義表示第i個(gè)子簇群在j*y時(shí)刻的位姿 偏轉(zhuǎn)角度；6表示第i個(gè)子簇群在(j+1 )* γ時(shí)刻的位姿偏轉(zhuǎn)角度；Φ表示機(jī)器人在A'位置 的偏轉(zhuǎn)角度。
[0070]進(jìn)一步，所述位置坐標(biāo)計(jì)算模塊中機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)按照以下步 驟來計(jì)算：
[0071 ]獲取機(jī)器人相對應(yīng)起始點(diǎn)的A'位置坐標(biāo)(x，y);
[0072]通過以下公式來計(jì)算機(jī)器人在B'位置的坐標(biāo)：
[0074] 其中，Lr_t為機(jī)器人長度，XB'表不機(jī)器人在B '位置的橫坐標(biāo);yB'表不機(jī)器人在B ' 位置的縱坐標(biāo)；I |CF| |表示機(jī)器人質(zhì)心從A'位置到B'位置的縱坐標(biāo)距離；| |BC| |表示機(jī)器 人質(zhì)心從A '位置到B '位置的橫坐標(biāo)距離。
[0075] 由于采用了上述技術(shù)方案，本發(fā)明具有如下的優(yōu)點(diǎn)：
[0076] 本發(fā)明提供的移動機(jī)器人位姿推算方法是基于移動機(jī)器人自身的信息得出的位 姿信息，使用激光雷達(dá)信息來描述移動機(jī)器人的位姿;二維激光雷達(dá)具有較高的精度而無 需對環(huán)境進(jìn)行任何修改;機(jī)器人的位姿信息相對于具體環(huán)境客觀描述，使用基于激光雷達(dá) 的位姿估計(jì)可以更便捷的將位姿信息集成到其它移動機(jī)器人的業(yè)務(wù)模塊中，例如避障路徑 規(guī)劃，而省去任何標(biāo)定過程，從而提高系統(tǒng)的可靠性、易實(shí)現(xiàn)性并降低成本。可廣泛使用于 對環(huán)境描述精度較高的二維激光雷達(dá)控制中。
[0077] 本發(fā)明的其他優(yōu)點(diǎn)、目標(biāo)和特征在某種程度上將在隨后的說明書中進(jìn)行闡述，并 且在某種程度上，基于對下文的考察研究對本領(lǐng)域技術(shù)人員而言將是顯而易見的，或者可 以從本發(fā)明的實(shí)踐中得到教導(dǎo)。本發(fā)明的目標(biāo)和其他優(yōu)點(diǎn)可以通過下面的說明書和權(quán)利要 求書來實(shí)現(xiàn)和獲得。
【附圖說明】
[0078] 本發(fā)明的【附圖說明】如下。
[0079] 圖1為本發(fā)明的機(jī)器人的位姿模型示意圖。
[0080] 圖2為本發(fā)明的ABD算法示意圖。
[0081] 圖3a為本發(fā)明的頂點(diǎn)在連接直線一側(cè)的簇群示意圖。
[0082]圖3b為本發(fā)明的頂點(diǎn)在連接直線另一側(cè)的簇群示意圖。
[0083]圖4a為在第j* γ個(gè)時(shí)刻起點(diǎn)和終點(diǎn)連線示意圖。
[0084]圖4b為在第(j+Ι)* γ個(gè)時(shí)刻起點(diǎn)和終點(diǎn)連線示意圖。
[0085]圖4c為機(jī)器人在A '位置和Β '位置變化示意圖。
[0086]圖5為本發(fā)明的激光雷達(dá)數(shù)據(jù)預(yù)處理流程圖。
[0087]圖6為本發(fā)明的ABD分割算法流程圖。
[0088] 圖7為本發(fā)明的位姿推算方法流程圖。
【具體實(shí)施方式】
[0089] 下面結(jié)合附圖和實(shí)施例對本發(fā)明作進(jìn)一步說明。
[0090] 實(shí)施例1
[0091] 如圖所示，本實(shí)施例提供的一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法， 包括以下步驟：
[0092] Stepl:接收與預(yù)處理激光雷達(dá)數(shù)據(jù)；
[0093] Step2:分割聚類環(huán)境數(shù)據(jù)；
[0094] Step3:從激光雷達(dá)數(shù)據(jù)中選擇簇群；
[0095] Step4:目標(biāo)簇群的跟蹤;獲取簇群中每個(gè)子簇群的頂點(diǎn)在局部坐標(biāo)系下的坐標(biāo)數(shù) 據(jù)，獲取簇群的頂點(diǎn)的坐標(biāo)和每個(gè)連接連線的傾角；
[0096] Step5:將起始點(diǎn)和終點(diǎn)組成連接直線并計(jì)算簇群中連接直線的偏轉(zhuǎn)角度；
[0097] Step6:通過滑動濾波計(jì)算機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)；
[0098] Step7:初始化起點(diǎn)的位姿并存儲位置坐標(biāo)。
[0099]所述步驟Stepl中的接收與預(yù)處理激光雷達(dá)數(shù)據(jù)的具體步驟如下：
[0100] 通過與計(jì)算機(jī)連接的二維掃描型激光雷達(dá)傳感器獲取環(huán)境數(shù)據(jù)；
[0101] 將環(huán)境數(shù)據(jù)以數(shù)組形式存儲于計(jì)算機(jī)中，所述環(huán)境數(shù)據(jù)包括距離信息、激光雷達(dá) 掃描范圍和角度分辨率；
[0102] 預(yù)處理距離數(shù)據(jù)，包括去除有效范圍外的數(shù)據(jù)點(diǎn)、濾除孤立的噪聲點(diǎn)及對激光雷 達(dá)測量機(jī)制的缺陷補(bǔ)償；
[0103] 將環(huán)境數(shù)據(jù)從激光雷達(dá)的極坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)化為機(jī)器人或智能車輛的局部直 角坐標(biāo)系坐標(biāo)。
[0104] 所述步驟Step2中的按照以下步驟來分割聚類環(huán)境數(shù)據(jù)：
[0105]循環(huán)比較距離數(shù)據(jù)的每兩個(gè)相鄰數(shù)據(jù)點(diǎn)差值，標(biāo)記差值大于分割閾值的數(shù)據(jù)點(diǎn)為 分割點(diǎn)；
[0106] 存儲分割點(diǎn)的角標(biāo)；
[0107] 所述步驟Step3中從激光雷達(dá)數(shù)據(jù)中選擇簇群的具體步驟如下：
[0108] 計(jì)算簇群中相對應(yīng)起始點(diǎn)和終點(diǎn)連接直線；
[0109] 計(jì)算簇群中數(shù)據(jù)點(diǎn)與連接直線的距離；
[0110]獲取距離直線最遠(yuǎn)的數(shù)據(jù)點(diǎn)為頂點(diǎn)；
[0111]計(jì)算頂點(diǎn)坐標(biāo)；
[0112] 計(jì)算連接直線的傾角。
[0113] 所述步驟Step5中的連接直線及偏轉(zhuǎn)角度按照以下步驟來實(shí)現(xiàn)：
[0114] 獲取機(jī)器人相對于起始位置的A'位置坐標(biāo)和角度信息Φ ;
[0115] 對機(jī)器人的角度信息進(jìn)行滑動濾波；
[0116]對簇群的角度信息進(jìn)行滑動濾波；
[0117] 得到B'位置機(jī)器人相對起始位置的角度信息；
[0118] 通過對角度信息進(jìn)行均值濾波得到機(jī)器人在B'位置的偏轉(zhuǎn)角度：
[0120]其中，ΘΒ'表不機(jī)器人在B'位置的偏轉(zhuǎn)角度;η表不一幀環(huán)境數(shù)據(jù)中經(jīng)過分割算法分 割出來的簇群總數(shù);i表示η個(gè)簇群中第i個(gè)子簇群；義.表示第i個(gè)子簇群在j*y時(shí)刻的位姿 偏轉(zhuǎn)角度;表示第i個(gè)子簇群在(j+1)* γ時(shí)刻的位姿偏轉(zhuǎn)角度；Φ表示機(jī)器人在A'位置 的偏轉(zhuǎn)角度。
[0121] 所述步驟Step6中機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)按照以下步驟來計(jì)算：
[0122] 獲取機(jī)器人相對應(yīng)起始點(diǎn)的A'位置坐標(biāo)(x，y);
[0123] 通過以下公式來計(jì)算機(jī)器人在B'位置的坐標(biāo)：
[0125] 其中，Lrqbcit為機(jī)器人長度，XB'表不機(jī)器人在B '位置的橫坐標(biāo);yB'表不機(jī)器人在B ' 位置的縱坐標(biāo)；| |CF| |表示機(jī)器人質(zhì)心從A'位置到B'位置的縱坐標(biāo)距離；| |BC| |表示機(jī)器 人質(zhì)心從A '位置到B '位置的橫坐標(biāo)距離。
[0126] 本實(shí)施例提供的一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算系統(tǒng)，包括預(yù)處理 模塊、分割聚類模塊、簇群選擇模塊、簇群跟蹤模塊、偏轉(zhuǎn)角度計(jì)算模塊、位置坐標(biāo)計(jì)算模塊 和存儲模塊；
[0127] 所述預(yù)處理模塊，用于接收與預(yù)處理激光雷達(dá)數(shù)據(jù)；
[0128] 所述分割聚類模塊，用于分割聚類環(huán)境數(shù)據(jù)；
[0129] 所述簇群選擇模塊，用于從激光雷達(dá)數(shù)據(jù)中選擇簇群；
[0130] 所述簇群跟蹤模塊，用于獲取簇群中每個(gè)子簇群的頂點(diǎn)在局部坐標(biāo)系下的坐標(biāo)數(shù) 據(jù)，獲取簇群的頂點(diǎn)的坐標(biāo)和每個(gè)連接連線的傾角；
[0131] 所述偏轉(zhuǎn)角度計(jì)算模塊，用于將起始點(diǎn)和終點(diǎn)組成連接直線并計(jì)算簇群中連接直 線的偏轉(zhuǎn)角度；
[0132] 所述位置坐標(biāo)計(jì)算模塊，用于通過滑動濾波計(jì)算機(jī)器人位置坐標(biāo)變化后的位置坐 標(biāo)；
[0133] 所述存儲模塊，用于初始化起點(diǎn)的位姿并存儲位置坐標(biāo)。
[0134] 所述預(yù)處理模塊中的接收與預(yù)處理激光雷達(dá)數(shù)據(jù)的具體步驟如下：
[0135] 通過與計(jì)算機(jī)連接的二維掃描型激光雷達(dá)傳感器獲取環(huán)境數(shù)據(jù)；
[0136] 將環(huán)境數(shù)據(jù)以數(shù)組形式存儲于計(jì)算機(jī)中，所述環(huán)境數(shù)據(jù)包括距離信息、激光雷達(dá) 掃描范圍和角度分辨率；
[0137] 預(yù)處理距離數(shù)據(jù)，包括去除有效范圍外的數(shù)據(jù)點(diǎn)、濾除孤立的噪聲點(diǎn)及對激光雷 達(dá)測量機(jī)制的缺陷補(bǔ)償；
[0138] 將環(huán)境數(shù)據(jù)從激光雷達(dá)的極坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)化為機(jī)器人或智能車輛的局部直 角坐標(biāo)系坐標(biāo)。
[0139] 所述簇群選擇模塊中從激光雷達(dá)數(shù)據(jù)中選擇簇群的具體步驟如下：
[0140] 計(jì)算簇群中相對應(yīng)起始點(diǎn)和終點(diǎn)連接直線；
[0141 ]計(jì)算簇群中數(shù)據(jù)點(diǎn)與連接直線的距離；
[0142]獲取距離直線最遠(yuǎn)的數(shù)據(jù)點(diǎn)為頂點(diǎn)；
[0143]計(jì)算頂點(diǎn)坐標(biāo)；
[0144] 計(jì)算連接直線的傾角。
[0145] 所述偏轉(zhuǎn)角度計(jì)算模塊中的連接直線及偏轉(zhuǎn)角度按照以下步驟來實(shí)現(xiàn)：
[0146] 獲取機(jī)器人相對于起始位置的A'位置坐標(biāo)和角度信息Φ ;
[0147] 對機(jī)器人的角度信息進(jìn)行滑動濾波；
[0148] 對簇群的角度信息進(jìn)行滑動濾波；
[0149] 得到B'位置機(jī)器人相對起始位置的角度信息；
[0150] 通過對角度信息進(jìn)行均值濾波得到機(jī)器人在B'位置的偏轉(zhuǎn)角度：
[0152]其中，ΘΒ'表不機(jī)器人在B'位置的偏轉(zhuǎn)角度;η表不一幀環(huán)境數(shù)據(jù)中經(jīng)過分割算法分 割出來的簇群總數(shù);i表示η個(gè)簇群中第i個(gè)子簇群;氣"表示第i個(gè)子簇群在j*y時(shí)刻的位姿 偏轉(zhuǎn)角度表示第i個(gè)子簇群在(j+l)*y時(shí)刻的位姿偏轉(zhuǎn)角度；Φ表示機(jī)器人在A'位置 的偏轉(zhuǎn)角度。
[0153] 所述位置坐標(biāo)計(jì)算模塊中機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)按照以下步驟來計(jì) 算：
[0154] 獲取機(jī)器人相對應(yīng)起始點(diǎn)的A'位置坐標(biāo)(x，y);
[0155] 通過以下公式來計(jì)算機(jī)器人在B'位置的坐標(biāo)：
[0157] 其中，Lrqbcit為機(jī)器人長度，XB'表不機(jī)器人在B '位置的橫坐標(biāo);yB'表不機(jī)器人在B ' 位置的縱坐標(biāo)；| |CF| |表示機(jī)器人質(zhì)心從A'位置到B'位置的縱坐標(biāo)距離；| |BC| |表示機(jī)器 人質(zhì)心從A '位置到B '位置的橫坐標(biāo)距離。
[0158] 實(shí)施例2
[0159] 本實(shí)施例提供的基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法，首先，接收和預(yù) 處理激光雷達(dá)數(shù)據(jù)，具體如下：
[0160] 將二維掃描型激光雷達(dá)傳感器與計(jì)算機(jī)進(jìn)行連接獲取環(huán)境數(shù)據(jù)，并以數(shù)組形式存 儲于計(jì)算機(jī)中，環(huán)境數(shù)據(jù)主要表現(xiàn)為距離信息D= {dhc^cb，. . .，土，. . .，dN}。然后對距離數(shù) 據(jù)進(jìn)行預(yù)處理，包括去除有效范圍外的數(shù)據(jù)點(diǎn)、濾除孤立的噪聲點(diǎn)及對激光雷達(dá)測量機(jī)制 的缺陷補(bǔ)償。另外將環(huán)境數(shù)據(jù)從激光雷達(dá)的極坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)化為機(jī)器人或智能車輛的 局部直角坐標(biāo)系坐標(biāo)，即將數(shù)據(jù)集合表示為:bg={Pi，P2，P3,......,Pi,......Pn}，其中Pi 表示為激光雷達(dá)第i線激光束反映的信息，即Pi= (xi，yi)，（xi，yi)為第i線激光束檢測到的 環(huán)境在局部坐標(biāo)系下的坐標(biāo)。
[0161] 對環(huán)境數(shù)據(jù)的分割聚類：
[0162] 平面ABD分割聚類算法在實(shí)時(shí)性和分割效果上均滿足，本發(fā)明的要求，因此采用此 算法對環(huán)境數(shù)據(jù)進(jìn)行分割聚類，方法如下：
[0163] 循環(huán)比較距離數(shù)據(jù)的每兩個(gè)相鄰點(diǎn)之間的差值，當(dāng)大于分割閾值時(shí)，則認(rèn)為找到 了分割點(diǎn)，公式如下：
[0164] dn-dn-l | >Dmax
[01 05]其中：dn為當(dāng)前掃描點(diǎn)的距離，dn-1為前一掃描點(diǎn)的距離，Dmax為分割閾值。此處Dmax 描述如下：
[0167] 其中：γη-ι為dn和dn-i取得最小值，δ為余量。λ和Δ φ如下圖所示，圖中φη和φη-Α ΑΡη和點(diǎn)Ρη相當(dāng)于激光雷達(dá)極坐標(biāo)的角度值。
[0168] 圖2ΑΚ)算法示意圖，圖中：ΑΡη和點(diǎn)Pn-i為掃描點(diǎn)中連續(xù)的兩個(gè)點(diǎn);dn為當(dāng)前掃描點(diǎn) 的距離;dn-l為前一掃描點(diǎn)的距離；γ η-1為dn和dn-l取得最小值;Dmax為分割閾值；Δ Φ為兩個(gè) 點(diǎn)與坐標(biāo)原點(diǎn)形成向量的夾角;λ為分割圓的輔助角。如果APn和點(diǎn)PH根據(jù)上述算法檢測 得出的結(jié)果都是分割點(diǎn)時(shí)，則暫時(shí)標(biāo)記點(diǎn)Pn和點(diǎn)Pn-A分割點(diǎn)，然后利用一個(gè)閾值Kd n)進(jìn)行 二次檢測，e(dn)=k*dn，其中k為常系數(shù)。為了使分割點(diǎn)間的距離不至于太遠(yuǎn)，本發(fā)明對閾值 進(jìn)行限定，規(guī)定e(dn) < μ。若I dn-dn-l I >ε((1η)，則認(rèn)為點(diǎn)Pn和點(diǎn)Pn-ι為分割點(diǎn)，反之，貝lj不是 分割點(diǎn)，取消之前分割的標(biāo)記。
[0169] 對一幀激光雷達(dá)的N個(gè)距離數(shù)據(jù)進(jìn)行優(yōu)化型ABD算法聚類分析后將得p個(gè)分割點(diǎn)， 即將環(huán)境分成P類。并將每個(gè)分割點(diǎn)的角標(biāo)存儲在計(jì)算機(jī)內(nèi)。
[0170] 對不同時(shí)刻進(jìn)行目標(biāo)簇群跟蹤
[0171] ①在采樣時(shí)刻t時(shí)，尋找具有特征點(diǎn)的簇群0={&，&，. . .，Cn}。
[0172] 尋找采樣時(shí)刻t的激光雷達(dá)數(shù)據(jù)中具有頂點(diǎn)的簇群，如下圖所示。規(guī)則為連接該簇 群的起始點(diǎn)和終點(diǎn)形成直線，計(jì)算除了這兩點(diǎn)外的其他所有點(diǎn)距離該直線的距離。距離直 線最遠(yuǎn)的即為頂點(diǎn)。
[0173]圖3具有頂點(diǎn)的簇群示意圖
[0174]基于上述選擇的簇群，求出頂點(diǎn)坐標(biāo)，記為：
[0176]將每個(gè)簇群的起始點(diǎn)與終點(diǎn)連線形成的直線，并計(jì)算每條直線的傾角，記為：
[0178] ②目標(biāo)簇群的跟蹤
[0179] 每經(jīng)過γ個(gè)采樣時(shí)刻時(shí)，獲取上述簇群集合C內(nèi)每個(gè)子簇群的頂點(diǎn)在局部坐標(biāo)系 下的坐標(biāo)數(shù)據(jù)，在經(jīng)過m* γ時(shí)刻后，所有子簇群的頂點(diǎn)的坐標(biāo)和每個(gè)子簇群起點(diǎn)和終點(diǎn)連 線的傾角分別表示為：
[0182] 基于滑動濾波的位姿推算
[0183] 對于第i個(gè)子簇群匕，從第j*y個(gè)時(shí)刻到第(j+l)*y時(shí)刻，該簇群的變化示意圖如 下所示：圖4不同時(shí)刻機(jī)器人位姿變化示意圖，圖a中山 n為某簇群在第j* γ個(gè)時(shí)刻起點(diǎn)和 終點(diǎn)連線；圖b中：L(j+mY為某簇群在第(j+Ι)* γ個(gè)時(shí)刻起點(diǎn)和終點(diǎn)連線；圖c中：ΘΒ，為Β'位 置的偏轉(zhuǎn)角度；Φ為機(jī)器人在A'位置相對于起始位置的角度。
[0184] 由圖4(a)與圖4(b)可知，機(jī)器人從A'運(yùn)動到B'位置時(shí)，機(jī)器人偏轉(zhuǎn)的角度差就是 某一個(gè)簇群偏轉(zhuǎn)的角度差，該角度差等于kn與Lu+υη傾角的變化程度，即（θ ?+υη-θ」η)。
[0185] 在此設(shè)Α'位置相對于起始位置，機(jī)器人的角度為Φ。對機(jī)器人位姿中的角度信息 進(jìn)行滑動濾波。將滑動窗口寬度設(shè)置為k* γ長度(k<m)，經(jīng)過對上述Θ矩陣內(nèi)η個(gè)簇群進(jìn)行 (m-k)次滑動濾波后，得到Β'位置機(jī)器人相對起始位置的角度為：
[0187]對以上η個(gè)(j + l)*y時(shí)刻的位姿偏轉(zhuǎn)角度進(jìn)行均值濾波后，得到在B'位置的偏轉(zhuǎn) 角度為：
[0189]由圖4(a)可以很明顯看出，當(dāng)機(jī)器人從A'運(yùn)動到B'位置時(shí)，視野里的局部環(huán)境也 發(fā)生了變化。設(shè)在A'位置時(shí)，機(jī)器人的位置坐標(biāo)為(x，y)，圖（c)將變化定量后，有幾何知識 可以得到:在B '位置時(shí)，機(jī)器人的坐標(biāo)即為：
[0191] 其中LRQBQT為機(jī)器人長度，其余相關(guān)參數(shù)見圖4 (c)所示。
[0192] 由圖（c)可知，D點(diǎn)即為該簇群的頂點(diǎn)，當(dāng)機(jī)器人到達(dá)B'位置時(shí)，此時(shí)D點(diǎn)對應(yīng)的激 光雷達(dá)線數(shù)也是確定了的，假設(shè)此時(shí)的線數(shù)為P，那么圖（C)上爐=/^6其中ξ為二維激光雷 達(dá)的角度分辨率。
[0193] 由：
[0195] 得到：
[0196] |BC| | = | |BD| |*sin(p*C-9O°+0B·)
[0197] CF的長度也可以根據(jù)幾何關(guān)系得到，即：
[0199] 得到：
[0200] I |CF| I = I |DF| |-| |BD| |*cos(p*|-90°+9B'）
[0201] 因此可以得到機(jī)器人在B'位置處的坐標(biāo)為：
[0203]對機(jī)器人位姿中的位置信息進(jìn)行滑動濾波。將滑動窗口寬度設(shè)置為k*y長度(k< m)，經(jīng)過對上述P矩陣內(nèi)η個(gè)簇群進(jìn)行(m-k)次滑動濾波后，得到B '位置機(jī)器人相對起始位置 的位置為：
[0205]對以上η個(gè)(j+Ι)* γ時(shí)刻的位置進(jìn)行均值濾波后，得到在B'位置的位置坐標(biāo)為：
[0207]根據(jù)上面的公式可以從起始點(diǎn)進(jìn)行疊加，疊加次數(shù)與采樣周期有關(guān)。因此得到機(jī) 器人在任何時(shí)刻的位姿。
[0208] 實(shí)施例3
[0209]本實(shí)施例詳細(xì)描述基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法的實(shí)施方式，具 體包括以下步驟：
[0210] Stepl:激光雷達(dá)數(shù)據(jù)的接收與預(yù)處理
[0211]將二維掃描型激光雷達(dá)傳感器與計(jì)算機(jī)進(jìn)行連接獲取環(huán)境數(shù)據(jù)，并以數(shù)組形式存 儲于計(jì)算機(jī)中，環(huán)境數(shù)據(jù)主要表現(xiàn)為距離信息D= {dhd^cb，...，心，...，dN}，N常取值為 500,對應(yīng)激光雷達(dá)的掃描范圍180°，角度分辨率ξ = 0.36°。然后對距離數(shù)據(jù)進(jìn)行預(yù)處理，包 括去除有效范圍外的數(shù)據(jù)點(diǎn)、濾除孤立的噪聲點(diǎn)及對激光雷達(dá)測量機(jī)制的缺陷補(bǔ)償。另外 將環(huán)境數(shù)據(jù)從激光雷達(dá)的極坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)化為機(jī)器人或智能車輛的局部直角坐標(biāo)系 坐標(biāo)，即將數(shù)據(jù)集合表示為:bg = {Ρ!，Ρ2，Ρ3，......，Pi，......Ρν}，其中Pi表示為激光雷達(dá) 第i線激光束反映的信息，即？1=(11，71)，（11，7 1)為第1線激光束檢測到的環(huán)境在局部坐標(biāo) 系下的坐標(biāo)。
[0212] Step2:對環(huán)境數(shù)據(jù)的分割聚類
[0213] 平面ABD分割聚類算法在實(shí)時(shí)性和分割效果上均滿足，本發(fā)明的要求，因此采用此 算法對環(huán)境數(shù)據(jù)進(jìn)行分割聚類，方法如下：
[0214] 循環(huán)比較距離數(shù)據(jù)的每兩個(gè)相鄰點(diǎn)之間的差值，當(dāng)大于分割閾值時(shí)，則認(rèn)為找到 了分割點(diǎn)，公式如下：
[0215] I dn_dn-1 I〉Dmax
[021 6]其中：dn為當(dāng)前掃描點(diǎn)的距離，dn-1為前一掃描點(diǎn)的距離，Dmax為分害lj閾值。此處Dm ax 描述如下
[0218] 其中：rn-Adn和dn-i取得最小值，δ為余量。λ和雜如下圖所示，圖中^和為點(diǎn) pn和點(diǎn)PH相當(dāng)于激光雷達(dá)極坐標(biāo)的角度值。
[0219]圖2為ABD算法示意圖，如果APn和點(diǎn)Pw根據(jù)上述算法檢測得出的結(jié)果都是分割 點(diǎn)時(shí)，則暫時(shí)標(biāo)記APn和點(diǎn)Pw為分割點(diǎn)，然后利用一個(gè)閾值ε (dn)進(jìn)行二次檢測，ε (dn) =k* dn，其中k為常系數(shù)。為了使分割點(diǎn)間的距離不至于太遠(yuǎn)，本發(fā)明對閾值進(jìn)行限定，規(guī)定e(dn) 《μ。若| dn-dn-1 I >e(dn)，則認(rèn)為點(diǎn)Pn和點(diǎn)Pn-1為分割點(diǎn)，反之，則不是分割點(diǎn)，取消之前分割 的標(biāo)記。其中，在本實(shí)施例中，k = 0.15，μ = 0· lm，5 = 0.03m。
[0220] 對一幀激光雷達(dá)的N = 500個(gè)距離數(shù)據(jù)進(jìn)行優(yōu)化型ABD算法聚類分析后將得p個(gè)分 割點(diǎn)，即將環(huán)境分成P類。并將每個(gè)分割點(diǎn)的角標(biāo)存儲在計(jì)算機(jī)內(nèi)。
[0221] Step3:在采樣時(shí)刻t時(shí)，尋找具有特征點(diǎn)的簇群C= {&，C2，…，Cn}。
[0222] 尋找采樣時(shí)刻t的激光雷達(dá)數(shù)據(jù)中具有頂點(diǎn)的簇群，如下圖所示。規(guī)則為連接該簇 群的起始點(diǎn)和終點(diǎn)形成直線，計(jì)算除了這兩點(diǎn)外的其他所有點(diǎn)距離該直線的距離。距離直 線最遠(yuǎn)的即為頂點(diǎn)。
[0223] 圖3為具有頂點(diǎn)的簇群示意圖，基于上述選擇的簇群，求出頂點(diǎn)坐標(biāo)，記為：
[0225]將每個(gè)簇群的起始點(diǎn)與終點(diǎn)連線形成的直線，并計(jì)算每條直線的傾角，記為：
[0227] Step4:目標(biāo)簇群的跟蹤
[0228] 每經(jīng)過γ =1個(gè)采樣時(shí)刻(采樣周期一般取T= 100ms)時(shí)，獲取上述簇群集合C內(nèi)每 個(gè)子簇群的頂點(diǎn)在局部坐標(biāo)系下的坐標(biāo)數(shù)據(jù)，在經(jīng)過m*y個(gè)采樣周期后，所有子簇群的頂 點(diǎn)的坐標(biāo)和每個(gè)子簇群起點(diǎn)和終點(diǎn)連線的傾角分別表示為：
[0231] 其中，在本實(shí)施例中，m=10。
[0232] Step5:基于滑動濾波的偏轉(zhuǎn)角度推算
[0233] 對于第i個(gè)子簇群匕，從第j*y個(gè)時(shí)刻到第(j+l)*y時(shí)刻，該簇群的變化示意圖如 下所示：
[0234] 圖4為不同時(shí)刻機(jī)器人位姿變化示意圖，由圖4(a)與圖4(b)可知，機(jī)器人從A'運(yùn)動 到B'位置時(shí)，機(jī)器人偏轉(zhuǎn)的角度差就是某一個(gè)簇群偏轉(zhuǎn)的角度差，該角度差等于與 L(j + 1)*Y傾角的變化程度，即（θ (」+ 1)*γ_θ)。
[0235] 在此設(shè)Α'位置相對于起始位置，機(jī)器人的角度為Φ。對機(jī)器人位姿中的角度信息 進(jìn)行滑動濾波。將滑動窗口寬度設(shè)置為k*y長度，其中k = 5,經(jīng)過對上述Θ矩陣內(nèi)η個(gè)簇群進(jìn) 行(m-k) = 5次滑動濾波后，得到Β '位置機(jī)器人相對起始位置的角度為：
[0237]對以上η個(gè)(j + l)*y時(shí)刻的位姿偏轉(zhuǎn)角度進(jìn)行均值濾波后，得到在B'位置的偏轉(zhuǎn) 角度為：
[0239] Step6:基于滑動濾波的位置坐標(biāo)推算
[0240]由圖4a可以很明顯看出，當(dāng)機(jī)器人從A'運(yùn)動到B'位置時(shí)，視野里的局部環(huán)境也發(fā) 生了變化。設(shè)在A'位置時(shí)，機(jī)器人的位置坐標(biāo)為(x，y)，圖4c將變化定量后，有幾何知識可以 得到:在B '位置時(shí)，機(jī)器人的坐標(biāo)即為：
[0242] 其中Lrqbcit為機(jī)器人長度，本實(shí)施例米用的機(jī)器人長度為114cm，即Lrqbqt= 114cm， 其余相關(guān)參數(shù)見圖4c所示。
[0243] 由圖4c可知，D點(diǎn)即為該簇群的頂點(diǎn)，當(dāng)機(jī)器人到達(dá)B'位置時(shí)，此時(shí)D點(diǎn)對應(yīng)的激光 雷達(dá)線數(shù)也是確定了的，假設(shè)此時(shí)的線數(shù)為P，那么圖4c上9 = #=?^，其中ξ為二維激光雷達(dá) 的角度分辨率，本實(shí)施例中取ξ = 0.36°。
[0244] 由：
[0246] 得到：
[0247] | | BC | | = | | BD | | *sin(0 · 36。*ρ-90。+ΘΒ'）
[0248] CF的長度也可以根據(jù)幾何關(guān)系得到，即：
[0250] 得到：
[0251] |CF| | = | |DF| |-| |BD| |*cos(0.36°*ρ-90°+ΘΒ·)
[0252] 因此可以得到機(jī)器人在Β'位置處的坐標(biāo)為：
[0254]對機(jī)器人位姿中的位置信息進(jìn)行滑動濾波。將滑動窗口寬度設(shè)置為k*y長度(k< m)，經(jīng)過對上述P矩陣內(nèi)η個(gè)簇群進(jìn)行(m-k)次滑動濾波后，得到B '位置機(jī)器人相對起始位置 的位置為：
[0256]對以上η個(gè)(j+1)* γ時(shí)刻的位置進(jìn)行均值濾波后，得到在B'位置的位置坐標(biāo)為：
[0258] 3七6口7:初始化起點(diǎn)的位姿，8卩（1，7，0) = (〇,〇,〇)，每經(jīng)過丫=1的采樣周期時(shí)，對 位姿進(jìn)行累積。因此得到機(jī)器人在任何時(shí)刻的位姿。
[0259]最后說明的是，以上實(shí)施例僅用以說明本發(fā)明的技術(shù)方案而非限制，盡管參照較 佳實(shí)施例對本發(fā)明進(jìn)行了詳細(xì)說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解，可以對本發(fā)明的技 術(shù)方案進(jìn)行修改或者等同替換，而不脫離本技術(shù)方案的宗旨和范圍，其均應(yīng)涵蓋在本發(fā)明 的權(quán)利要求范圍當(dāng)中。
【主權(quán)項(xiàng)】
1. 一種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法，其特征在于:包括以下步驟： Stepl:接收與預(yù)處理激光雷達(dá)數(shù)據(jù)； Step2:分割聚類環(huán)境數(shù)據(jù)； Step3:從激光雷達(dá)數(shù)據(jù)中選擇簇群； Step4:目標(biāo)簇群的跟蹤;獲取簇群中每個(gè)子簇群的頂點(diǎn)在局部坐標(biāo)系下的坐標(biāo)數(shù)據(jù)， 獲取簇群的頂點(diǎn)的坐標(biāo)和每個(gè)連接連線的傾角； Step5:將起始點(diǎn)和終點(diǎn)組成連接直線并計(jì)算簇群中連接直線的偏轉(zhuǎn)角度； Step6:通過滑動濾波計(jì)算機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)； Step7:初始化起點(diǎn)的位姿并存儲位置坐標(biāo)。2. 如權(quán)利要求1所述的基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法，其特征在于:所 述步驟Stepl中的接收與預(yù)處理激光雷達(dá)數(shù)據(jù)的具體步驟如下： 通過與計(jì)算機(jī)連接的二維掃描型激光雷達(dá)傳感器獲取環(huán)境數(shù)據(jù)； 將環(huán)境數(shù)據(jù)以數(shù)組形式存儲于計(jì)算機(jī)中，所述環(huán)境數(shù)據(jù)包括距離信息、激光雷達(dá)掃描 范圍和角度分辨率； 預(yù)處理距離數(shù)據(jù)，包括去除有效范圍外的數(shù)據(jù)點(diǎn)、濾除孤立的噪聲點(diǎn)及對激光雷達(dá)測 量機(jī)制的缺陷補(bǔ)償； 將環(huán)境數(shù)據(jù)從激光雷達(dá)的極坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)化為機(jī)器人或智能車輛的局部直角坐 標(biāo)系坐標(biāo)。3. 如權(quán)利要求1所述的基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法，其特征在于:所 述步驟Step2中的按照以下步驟來分割聚類環(huán)境數(shù)據(jù)： 循環(huán)比較距離數(shù)據(jù)的每兩個(gè)相鄰數(shù)據(jù)點(diǎn)差值，標(biāo)記差值大于分割閾值的數(shù)據(jù)點(diǎn)為分割 占 . 存儲分割點(diǎn)的角標(biāo)。4. 如權(quán)利要求1所述的基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法，其特征在于:所 述步驟St ep3中從激光雷達(dá)數(shù)據(jù)中選擇簇群的具體步驟如下： 計(jì)算簇群中相對應(yīng)起始點(diǎn)和終點(diǎn)連接直線； 計(jì)算簇群中數(shù)據(jù)點(diǎn)與連接直線的距離； 獲取距離直線最遠(yuǎn)的數(shù)據(jù)點(diǎn)為頂點(diǎn)； 計(jì)算頂點(diǎn)坐標(biāo)； 計(jì)算連接直線的傾角。5. 如權(quán)利要求1所述的基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法，其特征在于:所 述步驟Step5中的連接直線及偏轉(zhuǎn)角度按照以下步驟來實(shí)現(xiàn)： 獲取機(jī)器人相對于起始位置的A'位置坐標(biāo)和角度信息Φ ; 對機(jī)器人的角度信息進(jìn)行滑動濾波； 對簇群的角度信息進(jìn)行滑動濾波； 得到B '位置機(jī)器人相對起始位置的角度信息； 通過對角度信息進(jìn)行均值濾波得到機(jī)器人在B'位置的偏轉(zhuǎn)角度：其中，Θβ'表不機(jī)器人在B'位置的偏轉(zhuǎn)角度;η表不一幀環(huán)境數(shù)據(jù)中經(jīng)過分割算法分割出 來的簇群總數(shù);i表示η個(gè)簇群中第i個(gè)子簇群;病_表示第i個(gè)子簇群在j* γ時(shí)刻的位姿偏轉(zhuǎn) 角度；表示第i個(gè)子簇群在(j+l)*y時(shí)刻的位姿偏轉(zhuǎn)角度；Φ表示機(jī)器人在Α'位置的偏 轉(zhuǎn)角度。6. 如權(quán)利要求1所述的基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算方法，其特征在于:所 述步驟Step6中機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)按照以下步驟來計(jì)算： 獲取機(jī)器人相對應(yīng)起始點(diǎn)的A '位置坐標(biāo)(X，y); 通過以下公式來計(jì)算機(jī)器人在B '位置的坐標(biāo)：其中，Lrqbcit為機(jī)器人長度，XB'表不機(jī)器人在B '位置的橫坐標(biāo);yB'表不機(jī)器人在B '位置 的縱坐標(biāo)；I |CF| |表示機(jī)器人質(zhì)心從A'位置到B'位置的縱坐標(biāo)距離；| |BC| |表示機(jī)器人質(zhì) 心從A '位置到B '位置的橫坐標(biāo)距離。7. -種基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算系統(tǒng)，其特征在于:包括預(yù)處理模塊、 分割聚類模塊、簇群選擇模塊、簇群跟蹤模塊、偏轉(zhuǎn)角度計(jì)算模塊、位置坐標(biāo)計(jì)算模塊和存 儲豐吳塊； 所述預(yù)處理模塊，用于接收與預(yù)處理激光雷達(dá)數(shù)據(jù)； 所述分割聚類模塊，用于分割聚類環(huán)境數(shù)據(jù)； 所述簇群選擇模塊，用于從激光雷達(dá)數(shù)據(jù)中選擇簇群； 所述簇群跟蹤模塊，用于獲取簇群中每個(gè)子簇群的頂點(diǎn)在局部坐標(biāo)系下的坐標(biāo)數(shù)據(jù)， 獲取簇群的頂點(diǎn)的坐標(biāo)和每個(gè)連接連線的傾角； 所述偏轉(zhuǎn)角度計(jì)算模塊，用于將起始點(diǎn)和終點(diǎn)組成連接直線并計(jì)算簇群中連接直線的 偏轉(zhuǎn)角度； 所述位置坐標(biāo)計(jì)算模塊，用于通過滑動濾波計(jì)算機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)； 所述存儲模塊，用于初始化起點(diǎn)的位姿并存儲位置坐標(biāo)。8. 如權(quán)利要求7所述的基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算系統(tǒng)，其特征在于:所 述預(yù)處理模塊中的接收與預(yù)處理激光雷達(dá)數(shù)據(jù)的具體步驟如下： 通過與計(jì)算機(jī)連接的二維掃描型激光雷達(dá)傳感器獲取環(huán)境數(shù)據(jù)； 將環(huán)境數(shù)據(jù)以數(shù)組形式存儲于計(jì)算機(jī)中，所述環(huán)境數(shù)據(jù)包括距離信息、激光雷達(dá)掃描 范圍和角度分辨率； 預(yù)處理距離數(shù)據(jù)，包括去除有效范圍外的數(shù)據(jù)點(diǎn)、濾除孤立的噪聲點(diǎn)及對激光雷達(dá)測 量機(jī)制的缺陷補(bǔ)償； 將環(huán)境數(shù)據(jù)從激光雷達(dá)的極坐標(biāo)系下的數(shù)據(jù)轉(zhuǎn)化為機(jī)器人或智能車輛的局部直角坐 標(biāo)系坐標(biāo)。9. 如權(quán)利要求7所述的基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算系統(tǒng)，其特征在于:所 述簇群選擇模塊中從激光雷達(dá)數(shù)據(jù)中選擇簇群的具體步驟如下： 計(jì)算簇群中相對應(yīng)起始點(diǎn)和終點(diǎn)連接直線； 計(jì)算簇群中數(shù)據(jù)點(diǎn)與連接直線的距離； 獲取距離直線最遠(yuǎn)的數(shù)據(jù)點(diǎn)為頂點(diǎn)； 計(jì)算頂點(diǎn)坐標(biāo)； 計(jì)算連接直線的傾角。 所述偏轉(zhuǎn)角度計(jì)算模塊中的連接直線及偏轉(zhuǎn)角度按照以下步驟來實(shí)現(xiàn)： 獲取機(jī)器人相對于起始位置的A'位置坐標(biāo)和角度信息Φ ; 對機(jī)器人的角度信息進(jìn)行滑動濾波； 對簇群的角度信息進(jìn)行滑動濾波； 得到B '位置機(jī)器人相對起始位置的角度信息； 通過對角度信息進(jìn)行均值濾波得到機(jī)器人在B'位置的偏轉(zhuǎn)角度：其中，Θβ'表不機(jī)器人在B'位置的偏轉(zhuǎn)角度;η表不一幀環(huán)境數(shù)據(jù)中經(jīng)過分割算法分割出 來的簇群總數(shù);i表示η個(gè)簇群中第i個(gè)子簇群;表示第i個(gè)子簇群在j* γ時(shí)刻的位姿偏轉(zhuǎn) 角度；C ,表示第i個(gè)子簇群在(j+l)*y時(shí)刻的位姿偏轉(zhuǎn)角度；Φ表示機(jī)器人在Α'位置的偏 轉(zhuǎn)角度。10. 如權(quán)利要求7所述的基于激光雷達(dá)數(shù)據(jù)的移動機(jī)器人位姿推算系統(tǒng)，其特征在于： 所述位置坐標(biāo)計(jì)算模塊中機(jī)器人位置坐標(biāo)變化后的位置坐標(biāo)按照以下步驟來計(jì)算： 獲取機(jī)器人相對應(yīng)起始點(diǎn)的A '位置坐標(biāo)(X，y); 通過以下公式來計(jì)算機(jī)器人在B '位置的坐標(biāo)：其中，Lrqbcit為機(jī)器人長度，XB'表不機(jī)器人在B '位置的橫坐標(biāo);yB'表不機(jī)器人在B '位置 的縱坐標(biāo)；I |CF| |表示機(jī)器人質(zhì)心從A'位置到B'位置的縱坐標(biāo)距離；| |BC| |表示機(jī)器人質(zhì) 心從A '位置到B '位置的橫坐標(biāo)距離。
【文檔編號】G05D1/02GK105867373SQ201610213132
【公開日】2016年8月17日
【申請日】2016年4月7日
【發(fā)明人】孫棣華, 趙敏, 廖孝勇, 程森林, 杜道軼
【申請人】重慶大學(xué)