本發(fā)明涉及移動(dòng)機(jī)器人導(dǎo)航定位，尤其是涉及到一種基于2d激光雷達(dá)的無軌導(dǎo)航agv機(jī)器人的slam(同時(shí)定位與地圖構(gòu)建)方法。

背景技術(shù)：

同時(shí)定位與地圖構(gòu)建(或并發(fā)建圖與定位)(simultaneouslocalizationandmapping，slam)，是一種實(shí)現(xiàn)真正全自主移動(dòng)機(jī)器人的關(guān)鍵技術(shù)。傳統(tǒng)的agv(automatedguidedvehicle)導(dǎo)航主要有磁條導(dǎo)引、磁釘導(dǎo)引、色帶或二維碼導(dǎo)引等，雖然簡單易行、路徑跟蹤可靠性好，但均屬于固定路徑引導(dǎo)方式、靈活性和柔性差。新的激光導(dǎo)航方式無需固定路線導(dǎo)引，在應(yīng)用時(shí)具有更高的柔性，但目前激光導(dǎo)航方法大多數(shù)是采用反射板和三角定位原理進(jìn)行的，依然存在前期施工及標(biāo)定問題，其靈活性和柔性受到限制，基于slam的激光導(dǎo)航是agv的重要發(fā)展趨勢之一。

目前基于激光傳感器的slam多數(shù)使用濾波器方法、概率方法、最小二乘法、以及圖優(yōu)化等，如常用的gmapping、hectorslam、kartoslam等算法，擁有較小的誤差和較低的計(jì)算量，在室內(nèi)移動(dòng)機(jī)器人中得到了一定應(yīng)用，但濾波、估計(jì)、優(yōu)化的穩(wěn)定性以及定位精度無法絕對(duì)保證(這些方法多數(shù)針對(duì)的是低成本低精度的激光測距傳感器)，在工業(yè)agv機(jī)器人中的適用性還有待驗(yàn)證。使用輪廓外形匹配的方法相對(duì)于其他方法而言，物理意義更明確，符合人的思維方式，在保證agv導(dǎo)航定位的穩(wěn)定性和精度上具有很好的可行性。

參考文獻(xiàn)：

[1]羅元,傅有力,程鐵鳳.基于改進(jìn)rao-blackwellized粒子濾波器的同時(shí)定位與地圖構(gòu)建[j].控制理論與應(yīng)用,2015,32(2):267-272.

[2]蘇州艾吉威機(jī)器人有限公司.無反射板激光導(dǎo)航agv在汽車零部件整廠物流調(diào)度中的應(yīng)用[j].物流技術(shù)與應(yīng)用,2016,21(10):175-176.

[3]吳文軍,張巖,吳為民,等.一種運(yùn)輸自動(dòng)導(dǎo)引車導(dǎo)航方法研究[j].物聯(lián)網(wǎng)技術(shù),2016,6(9):58-62.

[4]常皓,楊巍.基于全向移動(dòng)模型的gmapping算法[j].計(jì)量與測試技術(shù),2016,43(10):1-4.

[5]葛艷茹,張國偉,沈宏雙,等.基于激光測距儀全局匹配掃描的slam算法研究[j].計(jì)算機(jī)測量與控制,2016,24(12):198-199.

[6]汪威,吳耀華,陳云霞.自然導(dǎo)航在agv中的應(yīng)用[j].物流技術(shù),2016,35(12):33-36.

[7]孫海,任翠平,盧軍,等.激光測距在倉儲(chǔ)搬運(yùn)機(jī)器人運(yùn)動(dòng)中的應(yīng)用[j].電子技術(shù)與軟件工程,2017(1):103-104.

[8]林偉民,頓向明,林子洋,等.基于激光雷達(dá)的重型自動(dòng)導(dǎo)引運(yùn)載車定位研究[j].機(jī)械與電子,2017(3):64-68.

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

本發(fā)明的目的在于針對(duì)現(xiàn)有激光slam算法多數(shù)針對(duì)低精度傳感器，其濾波、估計(jì)、優(yōu)化的穩(wěn)定性以及定位精度無法絕對(duì)保證，難以達(dá)到工業(yè)agv機(jī)器人應(yīng)用要求的問題，提供基于虛擬掃描與測距匹配的agv激光slam方法。

本發(fā)明包括以下步驟：

1)柵格地圖表示與創(chuàng)建方法；

在步驟1)中，所述柵格地圖表示與創(chuàng)建方法的具體方法可為：采用柵格表示地圖m，每個(gè)柵格為正方形，其長寬為δ(δ≤定位精度指標(biāo))，對(duì)于某個(gè)柵格mi為障礙時(shí)，定義其標(biāo)志位si＝1(為非障礙時(shí)si＝-1、不確定時(shí)si＝0)，其列行編號(hào)為(xi,yi)，柵格地圖m表示為：

m＝{m1(x1,y1,s1),m2(x2,y2,s2),...,mi(xi,yi,si),...,mn(xn,yn,sn)}(1)

使用2d激光雷達(dá)(lidar)進(jìn)行掃描測距(最大測距范圍為dmax)，得到一幀測距數(shù)據(jù)為{d1,d2,...,di,...,dn}，對(duì)于其中某一方向的測距di，其相對(duì)于激光雷達(dá)的角度為θi，定義其標(biāo)志位為ai(若di<dmax，則令ai＝1；否則ai＝-1)；激光雷達(dá)總的一幀測距數(shù)據(jù)l表示為：

l＝{l1(d1,θ1,a1),l2(d2,θ2,a2),...,li(di,θi,ai),...,ln(dn,θn,an)}(2)

agv機(jī)器人采用2d激光雷達(dá)進(jìn)行導(dǎo)航，假設(shè)激光雷達(dá)的位姿即為機(jī)器人的位姿，xr、yr表示位置坐標(biāo)，θr表示方向角，在已知激光雷達(dá)位姿情況下，則可以把當(dāng)前的測距數(shù)據(jù)l映射到柵格地圖m當(dāng)中，其中m為增量式構(gòu)建地圖，對(duì)于l中的li，其對(duì)應(yīng)的柵格mi為：

若ai＝1，取柵格標(biāo)志位si(xi,yi)＝1；以δ為單位長度(或更小)把線段di等分，采用同樣的方法計(jì)算各等分點(diǎn)對(duì)應(yīng)的柵格并令其標(biāo)志位為-1(即線段di穿過的柵格為非障礙點(diǎn)，標(biāo)志位為-1)。

2)虛擬掃描與匹配定位方法；

在步驟2)中，所述虛擬掃描與匹配定位方法的具體方法可為：agv機(jī)器人每運(yùn)行一步后，通過激光雷達(dá)獲得當(dāng)前的測距數(shù)據(jù)l，再結(jié)合之前建立的柵格地圖m，進(jìn)行當(dāng)前位姿的估算(即定位)：

(1)虛擬激光雷達(dá)(vlidar)掃描：

采用局部遍歷的方式，先設(shè)置一定的位置和角度遍歷范圍ω，在所有的遍歷位姿(假設(shè)為可能的激光雷達(dá)位姿)，模擬激光雷達(dá)，逐一對(duì)地圖m進(jìn)行虛擬掃描；在某一遍歷位姿(vxi,vyi,vθi)，獲得虛擬掃描數(shù)據(jù)li如下：

li＝{vli1(vdi1,vθi1,vai1),...,vlij(vdij,vθij,vaij),...,vlin(vdin,vθin,vain)}(4)

在虛擬掃描的vθi1方向獲得測距vdi1(若vdi1<dmax，則令vai1＝1；否則vai1＝-1)；在vθi2方向獲得測距vdi2(若vdi2<dmax，則令vai2＝1；否則vai2＝-1)；...；在vθin方向獲得測距vdin(若vdin<dmax，則令vain＝1；否則vain＝-1)；

以方向vθij為例，采用逐步推進(jìn)的方式獲得測距vdij，其過程如下：從起始點(diǎn)(vxi,vyi)開始，沿相對(duì)于vθi角度為vθij的方向，以長度δ(或更小)為增量向前推進(jìn)，每推進(jìn)一次計(jì)算一次到達(dá)點(diǎn)對(duì)應(yīng)的柵格(計(jì)算方法同式(3))，直到所到達(dá)點(diǎn)柵格的標(biāo)志位為1則停止推進(jìn)(若推進(jìn)距離>dmax則同樣停止推進(jìn))，此時(shí)起始點(diǎn)(vxi,vyi)到該柵格的推進(jìn)距離即為vdij(若vdij<dmax，則令vaij＝1；否則vaij＝-1)；

遍歷方式：逐一遍歷ω中的每個(gè)柵格點(diǎn)，遍歷到每個(gè)柵格點(diǎn)時(shí)再逐一遍歷每個(gè)角度值。例如，若ω的位置范圍為10×10＝100個(gè)柵格，角度范圍為5°/0.1°＝50個(gè)角度值，則總的遍歷次數(shù)為100×50＝5000＝k，遍歷完后獲得k個(gè)虛擬掃描數(shù)據(jù)vss(ω)＝{l1,l2,...,li,...,lk}；

(2)基于測距的輪廓匹配：

采用基于測距的輪廓匹配方式進(jìn)行定位，將vss(ω)中所有的虛擬掃描數(shù)據(jù)與當(dāng)前的真實(shí)測距數(shù)據(jù)l進(jìn)行比較，找出與l的測距數(shù)據(jù)最接近的虛擬掃描數(shù)據(jù)l*，即：

其中dmin≥0，根據(jù)具體應(yīng)用環(huán)境來設(shè)定；l*所在的遍歷位姿(vx*,vy*,vθ*)即為激光雷達(dá)當(dāng)前位姿的最優(yōu)估計(jì)值，即：

xr＝vx*,yr＝vy*,θr＝vθ*(6)

3)提高算法實(shí)時(shí)性的方法。

在步驟3)中，所述提高算法實(shí)時(shí)性的方法的具體方法可為：基于虛擬掃描與測距匹配的slam方法可先用于離線建圖，然后用于agv機(jī)器人工作時(shí)的即時(shí)定位，其中，虛擬激光雷達(dá)掃描的計(jì)算量最大，每次遍歷時(shí)都需要模擬激光雷達(dá)在其n個(gè)掃描方向上進(jìn)行推進(jìn)測距，采取以下措施：

(1)采用多gpu并行處理方式，每個(gè)gpu運(yùn)行一個(gè)掃描方向上模擬測距的推進(jìn)測距算法；

(2)對(duì)于第i個(gè)掃描方向，改變虛擬測距方向vθi上的初始推進(jìn)位置，從小于真實(shí)測距離di的某個(gè)位置開始(即不從遍歷位置開始)，推進(jìn)時(shí)遇到障礙柵格或到距離大于di的某個(gè)位置則停止推進(jìn)。

所述虛擬掃描方向vθi對(duì)應(yīng)的gpu推進(jìn)測距算法如下：

算法輸入：遍歷位姿(vx,vy,vθ)，地圖m，激光測距l(xiāng)

算法輸出：虛擬掃描方向上的測距vdi，標(biāo)志位vai

本發(fā)明采用輪廓外形遍歷匹配的原理，在每一遍歷位姿采用虛擬激光雷達(dá)對(duì)地圖進(jìn)行掃描，然后虛擬掃描的數(shù)據(jù)與當(dāng)前激光雷達(dá)的數(shù)據(jù)直接進(jìn)行比較，找出agv機(jī)器人的最優(yōu)位姿信息，再增量式構(gòu)建地圖。本發(fā)明基于虛擬掃描與測距匹配的agv激光slam方法，涉及一種應(yīng)用于agv機(jī)器人導(dǎo)航的同時(shí)定位與地圖構(gòu)建方法。主要包括一個(gè)柵格地圖表示與創(chuàng)建步驟；一個(gè)虛擬掃描與匹配定位步驟；一種提高虛擬掃描實(shí)時(shí)性的措施和算法實(shí)現(xiàn)。針對(duì)現(xiàn)有激光slam算法多數(shù)針對(duì)低精度傳感器，其濾波、估計(jì)、優(yōu)化的穩(wěn)定性以及定位精度無法絕對(duì)保證，難以達(dá)到工業(yè)agv機(jī)器人應(yīng)用要求的問題，提供了一種適合高精度激光雷達(dá)的slam方法。該方法采用輪廓外形遍歷匹配的原理，在每一遍歷位姿采用虛擬激光雷達(dá)對(duì)地圖進(jìn)行掃描，然后虛擬測距數(shù)據(jù)與當(dāng)前激光雷達(dá)的測距數(shù)據(jù)直接進(jìn)行比較，找出agv機(jī)器人的最優(yōu)位姿信息，再增量式構(gòu)建地圖；其中可通過采用多gpu并行處理和改變虛擬測距的初始推進(jìn)位置，提高該slam方法的實(shí)時(shí)性。因此，解決了采用反射板和三角定位原理進(jìn)行導(dǎo)航存在的前期施工及標(biāo)定問題，為提高agv與其他移動(dòng)機(jī)器人的靈活性和柔性，提供了一種簡單可靠和有精度保證的可行slam方法。

附圖說明

圖1為柵格地圖表示與創(chuàng)建方法示意圖。

圖2為虛擬激光雷達(dá)(vlidar)掃描方法示意圖。

圖3為基于虛擬掃描與測距匹配的slam方法組成框圖。

具體實(shí)施方式

下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明的具體實(shí)施方式進(jìn)行說明。

實(shí)施例1：本發(fā)明基于虛擬掃描與測距匹配的slam方法用于離線建圖，具體實(shí)施的操作過程如下。

step1：采用遙控或其他人工操作方式，控制agv機(jī)器人在工作環(huán)境中行走一遍，激光雷達(dá)采集并保存所有時(shí)刻(t0至tend)的測距信息{d1,d2,...,dn}|t0、{d1,d2,...,dn}|t1、...、{d1,d2,...,dn}|tend。

step2：采用合適的數(shù)據(jù)格式和文件，定義如式(1)所示的柵格地圖m，并進(jìn)行初始化。

step3：從t0開始時(shí)刻到tend結(jié)束時(shí)刻，利用測距信息構(gòu)建地圖，步驟如下。

step3.1：對(duì)于初始時(shí)刻t0，激光雷達(dá)的初始位姿已知，根據(jù)式(2)把{d1,d2,...,dn}|t0轉(zhuǎn)換為lt0，然后根據(jù)式(3)將lt0映射到柵格地圖m當(dāng)中；

step3.2：對(duì)于時(shí)刻t1，①激光雷達(dá)的當(dāng)前位姿未知，先根據(jù)式(2)把{d1,d2,...,dn}|t1轉(zhuǎn)換為lt1，在遍歷范圍ω中逐一模擬激光雷達(dá)進(jìn)行掃描，得到虛擬掃描數(shù)據(jù)vss(ω)＝{l1,l2,...,li,...,lk}(其li的格式如式(4)所示)，然后與lt1進(jìn)行比較，根據(jù)式(5)找出與lt1最接近的虛擬掃描數(shù)據(jù)l*；②根據(jù)l*和式(6)獲得激光雷達(dá)t1時(shí)刻的位姿，進(jìn)而根據(jù)式(3)將lt1映射到柵格地圖m當(dāng)中；

step3.3：對(duì)于時(shí)刻t2，定位和建圖過程與step3.2類似；

...(依此一直往下進(jìn)行，直到時(shí)刻tend，則建圖結(jié)束)

實(shí)施例2：在離線建圖后，本發(fā)明用于有地圖的agv機(jī)器人實(shí)時(shí)定位，具體實(shí)施的操作過程如下。

step1：在當(dāng)前時(shí)刻，激光雷達(dá)獲得測距信息{d1,d2,...,dn}，根據(jù)式(2)把{d1,d2,...,dn}轉(zhuǎn)換為l。

step2：在遍歷范圍ω中的每一位姿，模擬激光雷達(dá)對(duì)地圖m進(jìn)行掃描(采用多gpu并行處理和推進(jìn)測距算法，每個(gè)gpu對(duì)應(yīng)一個(gè)激光掃描方向)，得到虛擬掃描數(shù)據(jù)vss(ω)＝{l1,l2,...,li,...,lk}。

step3：將數(shù)據(jù)vss(ω)＝{l1,l2,...,li,...,lk}與l進(jìn)行比較，根據(jù)式(5)找出與l最接近的虛擬掃描數(shù)據(jù)l*；

step4：根據(jù)l*和式(6)獲得激光雷達(dá)當(dāng)前時(shí)刻的位姿估計(jì)值(即定位)。

實(shí)施例3：本發(fā)明基于虛擬掃描與測距匹配的slam方法直接用于實(shí)時(shí)并發(fā)建圖與定位，具體實(shí)施的操作過程如下。

step1：采用合適的數(shù)據(jù)格式和文件，定義如式(1)所示的柵格地圖m，并進(jìn)行初始化。

step2：對(duì)于初始時(shí)刻t0，激光雷達(dá)的初始位姿已知，根據(jù)式(2)把測距{d1,d2,...,dn}|t0轉(zhuǎn)換為lt0，然后根據(jù)式(3)將lt0映射到柵格地圖m當(dāng)中；

step3：在下一個(gè)當(dāng)前時(shí)刻，激光雷達(dá)獲得測距信息{d1,d2,...,dn}，根據(jù)式(2)把{d1,d2,...,dn}轉(zhuǎn)換為l。

step4：在遍歷范圍ω中的每一位姿，模擬激光雷達(dá)對(duì)地圖m進(jìn)行掃描(采用多gpu并行處理和推進(jìn)測距算法，每個(gè)gpu對(duì)應(yīng)一個(gè)激光掃描方向)，得到所有虛擬掃描數(shù)據(jù)vss(ω)＝{l1,l2,...,li,...,lk}。

step5：將數(shù)據(jù)vss(ω)＝{l1,l2,...,li,...,lk}與l進(jìn)行比較，根據(jù)式(5)找出與l最接近的虛擬掃描數(shù)據(jù)l*；

step6：根據(jù)l*和式(6)獲得激光雷達(dá)當(dāng)前時(shí)刻的位姿估計(jì)值(即定位)；

step7：激光雷達(dá)的位姿估計(jì)值已知，根據(jù)式(3)將l映射到柵格地圖m當(dāng)中；返回到step3。

以上實(shí)施例中，柵格地圖表示與創(chuàng)建方法的如圖1所示，虛擬激光雷達(dá)掃描方法的原理如圖2所示，步驟之間的邏輯關(guān)系如圖3所示。