本發(fā)明屬于智能倉儲(chǔ)物流領(lǐng)域，具體涉及一種基于雙反光板的激光定位與導(dǎo)航方法。

背景技術(shù)：

基于激光傳感器的定位與導(dǎo)航技術(shù)是工業(yè)agv、智能機(jī)器人等領(lǐng)域的關(guān)鍵性技術(shù)，相比傳統(tǒng)有軌導(dǎo)航方式，該技術(shù)具有定位精度高、靈活多變等優(yōu)點(diǎn)，適用于復(fù)雜、高動(dòng)態(tài)的工業(yè)場(chǎng)景中。國內(nèi)外現(xiàn)有基于反光板的激光定位技術(shù)，均采用了幾何三角定位原理，每時(shí)每刻必須要求同時(shí)檢測(cè)到三塊反光板才可實(shí)現(xiàn)定位。這一數(shù)量要求實(shí)際上在復(fù)雜工業(yè)場(chǎng)景中很難滿足，極大地制約了激光定位與導(dǎo)航技術(shù)的實(shí)用性。

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

為了解決上述問題，本發(fā)明提供一種新型激光定位與導(dǎo)航方法，通過提取和利用激光測(cè)距信息中的復(fù)數(shù)域信息，將三塊反光板的最低要求降低為兩塊，有效解決了反光板最低數(shù)量的技術(shù)瓶頸，大大提高了激光導(dǎo)航技術(shù)的適用性。

本發(fā)明的目的是通過以下技術(shù)方案來實(shí)現(xiàn)的：一種基于雙反光板的激光定位與導(dǎo)航方法，包括以下步驟：

(1)在工業(yè)環(huán)境中布置反光板，預(yù)設(shè)反光板世界坐標(biāo)，生成反光板坐標(biāo)列表；

(2)安裝在移動(dòng)平臺(tái)上的激光傳感器向周圍呈輻射狀發(fā)射激光，并接收反射激光；

(3)篩選來源于反光板的有效光束：通過檢測(cè)反射激光強(qiáng)度i并與預(yù)設(shè)強(qiáng)度閾值σ相比較，判定激光照射物是反光板還是普通環(huán)境物體；

(4)確定當(dāng)前時(shí)刻照射到的反光板數(shù)量及其相對(duì)激光傳感器的相對(duì)坐標(biāo)：根據(jù)反射光束角度的連續(xù)性，判斷是否為同一塊反光板，或者，根據(jù)反射光束角度和距離的連續(xù)性，判斷是否為同一塊反光板；根據(jù)屬于同一反光板的反射光束得到該反光板相對(duì)于激光傳感器的相對(duì)坐標(biāo)，并存入反光板列表；

(5)初始化反光板列表，得到至少兩個(gè)反光板的世界坐標(biāo)：人工確定初始位置對(duì)應(yīng)的反光板列表中至少兩個(gè)反光板的世界坐標(biāo)；或者，激光傳感器在初始位置獲取至少三個(gè)反光板返回的角度和距離，計(jì)算兩兩反光板之間的距離，與根據(jù)反光板坐標(biāo)列表生成的反光板距離信息相匹配，得到至少兩個(gè)反光板的世界坐標(biāo)；

(6)在動(dòng)態(tài)過程中計(jì)算期望反光板列表：根據(jù)上一時(shí)刻對(duì)當(dāng)前時(shí)刻激光傳感器位置和角度的預(yù)測(cè)，估計(jì)激光傳感器與所有反光板之間的相對(duì)距離和角度，并存入期望反光板列表；

(7)在動(dòng)態(tài)過程中反光板列表的匹配：計(jì)算當(dāng)前時(shí)刻反光板列表與期望反光板列表中同一個(gè)反光板對(duì)應(yīng)的距離之差和角度之差，當(dāng)距離之差和角度之差均滿足預(yù)設(shè)閾值時(shí)，匹配成功；

(8)基于雙反光板數(shù)據(jù)的激光傳感器位姿計(jì)算：利用激光傳感器測(cè)量數(shù)據(jù)的復(fù)頻域信息，在匹配成功的反光板中任選兩塊：第l個(gè)和第k個(gè)，并計(jì)算：

zk＝xk+i*yk

zl＝xl+i*yl

其中，下標(biāo)l和k分別代表第l和第k個(gè)反光板；α和ρ分別代表在激光傳感器相對(duì)自身的極坐標(biāo)系下，反光板的角度和距離；x和y分別為反光板在x和y軸的分量；z為反光板在世界坐標(biāo)系下的復(fù)數(shù)坐標(biāo)；zk,l為根據(jù)第l和第k個(gè)反光板計(jì)算得到的激光傳感器世界坐標(biāo)，θk是根據(jù)第k塊反光板的數(shù)據(jù)計(jì)算得到的激光傳感器在世界坐標(biāo)系下的角度。

進(jìn)一步地，所述步驟(6)中，直接采用上一時(shí)刻激光傳感器的位置和角度作為當(dāng)前時(shí)刻的預(yù)測(cè)，或者采用濾波算法進(jìn)行預(yù)測(cè)。

進(jìn)一步地，所述步驟(8)之后，還包括多塊優(yōu)化的步驟：如果激光傳感器檢測(cè)到三個(gè)及以上的反光板數(shù)據(jù)，可以根據(jù)多組數(shù)據(jù)中任意兩組計(jì)算得到的位姿進(jìn)行數(shù)據(jù)融合，得到最終的移動(dòng)平臺(tái)及激光傳感器在世界坐標(biāo)系下的位置及角度。

本發(fā)明的有益效果如下：與傳統(tǒng)技術(shù)中的幾何三角定位不同，本發(fā)明深入挖掘并充分利用了反射光的復(fù)數(shù)域信息，打破了傳統(tǒng)意義上三塊反光板的技術(shù)瓶頸，其運(yùn)行條件為同時(shí)檢測(cè)到兩塊反光板的數(shù)據(jù)即可，大大提高了激光導(dǎo)航技術(shù)的適用性；雖然本發(fā)明最低要求為檢測(cè)到兩塊反光板，但實(shí)際應(yīng)用時(shí)可能會(huì)同時(shí)檢測(cè)到多個(gè)反光板，通過設(shè)計(jì)數(shù)據(jù)融合處理，能夠最大化利用反光板的信息，從而進(jìn)一步提高精度。本發(fā)明的具體性能為：定位誤差<1cm，角度誤差<0.5°，定位頻率>35hz。

附圖說明

圖1為本發(fā)明的硬件結(jié)構(gòu)示意圖；

圖2為移動(dòng)平臺(tái)運(yùn)動(dòng)學(xué)模型示意圖；

圖3為本發(fā)明方法整體流程圖。

具體實(shí)施方式

下面結(jié)合附圖，通過實(shí)施例對(duì)本發(fā)明作進(jìn)一步地說明。

本發(fā)明提供的一種基于雙反光板的激光定位與導(dǎo)航方法，該方法包括：

(1)在工業(yè)環(huán)境中布置反光板，預(yù)設(shè)反光板世界坐標(biāo)，生成反光板坐標(biāo)列表；

(2)安裝在移動(dòng)平臺(tái)上的激光傳感器向周圍呈輻射狀發(fā)射激光，并接收反射激光(激光傳感器安裝在agv、工業(yè)車輛等移動(dòng)平臺(tái)上)；

(3)篩選來源于反光板的有效光束：通過檢測(cè)反射激光強(qiáng)度i并與預(yù)設(shè)強(qiáng)度閾值σ相比較，判定激光照射物是反光板還是普通環(huán)境物體，篩選出來源于反光板的反射光數(shù)據(jù)；

(4)確定當(dāng)前時(shí)刻照射到的反光板數(shù)量及其相對(duì)激光傳感器的相對(duì)坐標(biāo)：根據(jù)反射光束角度的連續(xù)性，判斷是否為同一塊反光板，或者，根據(jù)反射光束角度和距離的連續(xù)性，判斷是否為同一塊反光板；根據(jù)屬于同一反光板的反射光束得到該反光板相對(duì)于激光傳感器的相對(duì)坐標(biāo)(方法為：平均、取最強(qiáng)光，加權(quán)平均，任取)，并存入反光板列表；

根據(jù)反射光束角度和距離的連續(xù)性，判斷是否為同一塊反光板，一種可能的實(shí)現(xiàn)方式如下：

對(duì)于data表中所有的反光強(qiáng)點(diǎn)：

如果第i個(gè)反光強(qiáng)點(diǎn)的序號(hào)step(i)等于第i-1個(gè)反光強(qiáng)點(diǎn)的序號(hào)step(i-1)+1，并且第i和i-1的激光強(qiáng)點(diǎn)測(cè)得的距離之差distance(step(i))-distance(step(i-1))小于預(yù)設(shè)的距離閾值distance_threshold，那么光束step(i)和step(i-1)照射到的是同一塊反光板，則將(step(i),,ρ)賦值到上一個(gè)landmark結(jié)構(gòu)體中；

否則照射到的是一塊新的反光板，開始一個(gè)新的landmark結(jié)構(gòu)體；計(jì)算：α＝(step(i)-1)·resolution+α；ρ＝distance(step(i))

將(step(i),α,ρ)寫入新的landmark結(jié)構(gòu)體中；

最終得到由landmark結(jié)構(gòu)體組成的所照射到的反光板列表measure_landmark_list；

對(duì)應(yīng)的偽代碼如下：

fori＝1:number_data

ifstep(i)＝step(i-1)+1&&distance(step(i))-distance(step(i-1))<distance_threshold

then光束step(i)和step(i-1)照射到的是同一塊反光板

else

α＝(step(i)-1)·resolution+α；ρ＝distance(step(i))

將(step(i),α,ρ)寫入landmark

endif

endfor

其中number_data是反光強(qiáng)點(diǎn)的個(gè)數(shù)，resolution是相鄰兩道激光光束的夾角(分辨率)，distance_threshold是判定是否照射到同一塊反光板的距離閾值，α和ρ分別代表在激光傳感器相對(duì)自身的極坐標(biāo)系下，該反光板的角度和距離，measure_landmark_list為照射到反光板的列表。

或者：僅根據(jù)反射光束角度的連續(xù)性，判斷是否為同一塊反光板，一種可能的實(shí)現(xiàn)方式如下：

對(duì)于data表中所有的反光強(qiáng)點(diǎn)：

如果第i個(gè)反光強(qiáng)點(diǎn)的序號(hào)step(i)等于第i-1個(gè)反光強(qiáng)點(diǎn)的序號(hào)step(i-1)+1，那么光束step(i)和step(i-1)照射到的是同一塊反光板，則將(step(i),α,ρ)賦值到上一個(gè)landmark結(jié)構(gòu)體中；

否則照射到的是一塊新的反光板，開始一個(gè)新的landmark結(jié)構(gòu)體，計(jì)算：

α＝(step(i)-1)·resolution+α；ρ＝distance(step(i))

將(step(i),α,ρ)寫入新的landmark結(jié)構(gòu)體中；

最終得到由landmark結(jié)構(gòu)體組成的所照射到的反光板列表measure_landmark_list；

(5)初始化反光板列表，得到至少兩個(gè)反光板的世界坐標(biāo)：人工確定初始位置對(duì)應(yīng)的反光板列表中至少兩個(gè)反光板的世界坐標(biāo)；或者，激光傳感器在初始位置獲取至少三個(gè)反光板返回的角度和距離，計(jì)算兩兩反光板之間的距離，與根據(jù)反光板坐標(biāo)列表生成的反光板距離信息相匹配，得到至少兩個(gè)反光板的世界坐標(biāo)；一種可能的實(shí)現(xiàn)方式如下：

計(jì)算任意兩個(gè)反光板之間的距離

計(jì)算任意兩個(gè)反光強(qiáng)點(diǎn)之間的距離

對(duì)于measure_landmark_list列表中所照射到的m個(gè)反光板所有兩兩組合(l,k)：找到i,j∈{1,…,n},使得|dk,l-ri,j|＜ri,j·threshold；則i添加到correspondence_list(k)，j添加到correspondence_list(l)；

找到correspondence_list(k)中出現(xiàn)頻次最高的值，即為反光強(qiáng)點(diǎn)k對(duì)應(yīng)的反光板c(k)，如果找不到這樣的點(diǎn)，則從measure_landmark_list中去掉點(diǎn)i，并且令m＝m-1；

如果measure_landmark_list中包含的反光強(qiáng)點(diǎn)個(gè)數(shù)m<＝2，則警告處理。

(6)在動(dòng)態(tài)過程中計(jì)算期望反光板列表：根據(jù)上一時(shí)刻對(duì)當(dāng)前時(shí)刻激光傳感器位置和角度的預(yù)測(cè)，估計(jì)激光傳感器與所有反光板之間的相對(duì)距離和角度，并存入期望反光板列表；(直接采用上一時(shí)刻激光傳感器的位置和角度作為當(dāng)前時(shí)刻的預(yù)測(cè)，或者采用濾波算法進(jìn)行預(yù)測(cè))一種可能的實(shí)現(xiàn)方式如下：

對(duì)于第i塊反光板，i取1到n，計(jì)算：

其中函數(shù)arctan2(x,y)返回原點(diǎn)至點(diǎn)(x,y)的方位角，取值為(-π,π]。

如果ρt|t-1小于最大距離閾值max_distance，并且αt|t-1大于激光傳感器掃描角度的最小值min_angle減去角度閾值threshold_angle，αt|t-1小于激光傳感器掃描角度的最大值max_angle加上角度閾值threshold_angle，則將添加到期望的反光列表estimated_landmark_list中；

對(duì)應(yīng)的偽代碼如下：

fori＝1:n

如果ρt|t-1＜max_distance&&min_angle-threshold_angle＜αt|t-1＜max_angle+threshold_angle

則將添加到期望的反光列表estimated_landmarks_list

end

(7)在動(dòng)態(tài)過程中反光板列表的匹配：計(jì)算當(dāng)前時(shí)刻反光板列表與期望反光板列表中同一個(gè)反光板對(duì)應(yīng)的距離之差和角度之差，當(dāng)距離之差和角度之差均滿足預(yù)設(shè)閾值時(shí)，匹配成功；一種可能的實(shí)現(xiàn)方式如下：

對(duì)于measure_landmark_list的所有反光板，找到{i,xi,yi,ρt|t-1,αt|t-1}，使得：

計(jì)算測(cè)量點(diǎn)(ρk,αk)和世界坐標(biāo)系的誤差：

如果dist<threshold_distance，則c(k)＝i

如果找不到這樣的點(diǎn)，則從measure_landmark_list中去掉點(diǎn)i，并令m＝m-1；如果measure_landmark_list中包含的反光強(qiáng)點(diǎn)個(gè)數(shù)m<＝2，則警告處理。

(8)基于雙反光板數(shù)據(jù)的激光傳感器位姿計(jì)算：利用激光傳感器測(cè)量數(shù)據(jù)的復(fù)頻域信息，在匹配成功的反光板中任選兩塊：第l個(gè)和第k個(gè)，并計(jì)算：

zk＝xk+i*yk

zl＝xl+i*yl

其中，下標(biāo)l和k分別代表第l和第k個(gè)反光板；α和ρ分別代表在激光傳感器相對(duì)自身的極坐標(biāo)系下，反光板的角度和距離；x和y分別為反光板在x和y軸的分量；z為反光板在世界坐標(biāo)系下的復(fù)數(shù)坐標(biāo)；zk,l為根據(jù)第l和第k個(gè)反光板計(jì)算得到的激光傳感器世界坐標(biāo)，θk是根據(jù)第k塊反光板的數(shù)據(jù)計(jì)算得到的激光傳感器在世界坐標(biāo)系下的角度；

(9)多塊優(yōu)化：如果激光傳感器檢測(cè)到三個(gè)及以上的反光板數(shù)據(jù)，可以根據(jù)多組數(shù)據(jù)中任意兩組計(jì)算得到的位姿進(jìn)行數(shù)據(jù)融合，得到最終的移動(dòng)平臺(tái)及激光傳感器在世界坐標(biāo)系下的位置及角度?？蛇x的方案有：選取反射光強(qiáng)最強(qiáng)的兩組數(shù)據(jù)的計(jì)算結(jié)果作為激光傳感器當(dāng)前位姿，或在多組數(shù)據(jù)中兩兩組合進(jìn)行位姿計(jì)算，然后求取他們的平均值，或加權(quán)平均值(如權(quán)重與反射光強(qiáng)度相關(guān))。

實(shí)施例1

如圖1所示，本發(fā)明的硬件構(gòu)成主要包括激光傳感器、工控機(jī)、車載控制電路與車載電源；所述的工控機(jī)、激光傳感器與車載控制電路通過rs232/can/spi等方式進(jìn)行通信；車載電源對(duì)激光傳感器和車載控制電路經(jīng)dc/dc模塊進(jìn)行直流供電，經(jīng)dc/ac模塊對(duì)車載工控機(jī)進(jìn)行交流供電。

進(jìn)一步地，在工業(yè)環(huán)境(地圖)中合適的位置放置一定數(shù)量的反光板，以agv為例，使得agv在地圖中的任何位置都能夠成功檢測(cè)到2塊或以上的反光板；激光傳感器發(fā)射激光并檢測(cè)反射信號(hào)。

進(jìn)一步地，工控機(jī)計(jì)算反光板間距，初始化時(shí)采用靜態(tài)匹配，隨后均采用動(dòng)態(tài)匹配來識(shí)別反光板，并從反光板列表中獲取反光板世界坐標(biāo)信息。

進(jìn)一步地，工控機(jī)提取激光傳感器測(cè)距數(shù)據(jù)中的復(fù)數(shù)域信息，利用兩塊反光板的數(shù)據(jù)進(jìn)行agv坐標(biāo)位置和姿態(tài)的計(jì)算；若同時(shí)檢測(cè)到3塊以上的反光板信息，則將反光板數(shù)據(jù)進(jìn)行兩兩組合，在每周組合下分別計(jì)算，然后以計(jì)算結(jié)果的平均值作為agv的位置和姿態(tài)。本發(fā)明的整體流程如圖3所示。