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

背景技術(shù)：

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

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

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

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

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

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

(3)篩選來源于反光柱的有效光束：預(yù)設(shè)強度閾值σ，通過檢測反射激光強度i，并與σf(range)相比較，判定激光照射物是反光柱還是普通環(huán)境物體，其中f(range)與距離range負相關(guān)；

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

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

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

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

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

zk＝xk+i*yk

zl＝xl+i*tl

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

(9)激光傳感器位置濾波：首先根據(jù)移動平臺運動學(xué)模型進行位姿預(yù)測，然后根據(jù)上一時刻預(yù)測的位姿，并結(jié)合當(dāng)前時刻的激光傳感器測量數(shù)據(jù)，對當(dāng)前時刻的位置進行濾波，從而得到最終的移動平臺及激光傳感器在世界坐標(biāo)系下的準確的位置與姿態(tài)。

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

進一步地，所述步驟(9)之前，還包括多個優(yōu)化的步驟：如果激光傳感器檢測到三個及以上的反光柱數(shù)據(jù)，可以根據(jù)多組數(shù)據(jù)中任意兩組計算得到的位姿進行數(shù)據(jù)融合。

進一步地，所述步驟(9)中，根據(jù)移動平臺運動學(xué)模型進行位姿預(yù)測，公式如下：

θt|t-1＝θt-1|t-1+δθc

其中，輸入量為：上一時刻的位姿(xt-1|t-1,yt-1|t-1,θt-1|t-1)，電機編碼器的分辨率resolution，減速比為speed_reduction_ratio，左、右輪編碼器此刻的讀數(shù)count_left、count_right，左、右輪編碼器上一刻的讀數(shù)last_count_right、last_count_right，采樣周期δt，左右輪半徑r，左、右輪的角速度ωl、ωr，移動平臺在上一時刻的線速度vt-1，移動平臺在上一時刻的轉(zhuǎn)向角速度ωt-1，移動平臺在上一時刻世界坐標(biāo)系下的轉(zhuǎn)向角度θt-1，激光傳感器距離兩個輪子中心點的距離d；輸出量為：相對上時刻的運動增量(δx,δy,δθ)，根據(jù)上一時刻數(shù)據(jù)預(yù)測的當(dāng)前時刻位姿(xt|t-1,yt|t-1,θt|t-1)；

根據(jù)上一時刻預(yù)測的位姿，結(jié)合當(dāng)前時刻的位姿計算結(jié)果，對當(dāng)前時刻的位姿進行濾波，具體如下：若當(dāng)前時刻僅檢測到2個反光柱，記為l和k，采用線性卡爾曼濾波的公式如下：

其中，輸入量為：當(dāng)前時刻預(yù)測的位置上一時刻的協(xié)方差矩陣pt-1|t-1，過程噪聲的協(xié)方差矩陣q，測量噪聲的協(xié)方差矩陣r；輸出量為：激光傳感器在世界坐標(biāo)系下的坐標(biāo)當(dāng)前時刻更新的協(xié)方差矩陣pt|t；

若當(dāng)前時刻檢測到3個及以上的反光柱，則采用多個數(shù)據(jù)融合的思想進行濾波；采用貫序卡爾曼濾波的公式如下：

對于檢測到的m個反光柱中的任意兩兩組合，計算：

zk＝xk+i*yk

zl＝xl+i*yl

遍歷所有組合后，得到：

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

附圖說明

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

圖2為反光柱測距與數(shù)據(jù)修正示意圖；

圖3為移動平臺運動學(xué)模型示意圖；

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

具體實施方式

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

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

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

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

(3)篩選來源于反光柱的有效光束：

預(yù)設(shè)強度閾值σ，通過檢測反射激光強度i，并與σf(range)相比較，判定激光照射物是反光柱還是普通環(huán)境物體，其中f(range)與距離range負相關(guān)；優(yōu)選地，可設(shè)置f(range)＝1/range；一種可能的實現(xiàn)方式如下：對于所有接收到的反射光束，如果第step個激光束的激光強度intensity(step)大于預(yù)設(shè)強度閾值intensity_threshold/激光測量的距離range(step)，則將(step,intensity(step),range(step))寫入激光強點的數(shù)據(jù)列表data中；對應(yīng)的偽代碼如下：

forstep＝1:number_laser

ifintensity(step)>intensity_threshold/range(step)

將(step,intensity(step),range(step))寫入data表中

endif

endfor

(4)確定當(dāng)前時刻照射到的反光柱數(shù)量及其相對激光傳感器的相對坐標(biāo)：根據(jù)反射光束角度的連續(xù)性，判斷是否為同一個反光柱，或者，根據(jù)反射光束角度和距離的連續(xù)性，判斷是否為同一個反光柱；根據(jù)屬于同一反光柱的反射數(shù)據(jù)，結(jié)合反光柱半徑r進行數(shù)據(jù)修正，如圖2所示，得到該反光柱相對于激光傳感器的相對坐標(biāo)，并存入反光柱列表；

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

對于data表中所有的反光強點：

如果第i個反光強點的序號step(i)等于第i-1個反光強點的序號step(i-1)+1，并且第i和i-1的激光強點測得的距離之差range(step(i))-range(step(i-1))小于預(yù)設(shè)的距離閾值range_threshold，那么光束step(i)和step(i-1)照射到的是同一個反光柱，則將(step(i),range)賦值到當(dāng)前第k個landmark子列表中，且該子列表中光束數(shù)據(jù)的組數(shù)number＝number+1；

否則，照射到的是一個新的反光柱，則當(dāng)前第k個landmark子列表結(jié)束；

如果number＝1，則將(k,α,ρ)寫入measure_landmark_list，其中α＝(step-1)·resolution+min_angle，ρ＝range+r；min_angle為激光傳感器掃描角度的最小值；

如果number>＝2，則提取當(dāng)前l(fā)andmark子列表中的反光最強點的兩組數(shù)據(jù)(step1,range1)和(step2,range2)，計算：

如果range1<range2

否則

同時，新建第k+1個landmark子列表，組數(shù)初始化為number＝0；將(step(i),range)賦值到新的landmark子列表中，number＝number+1；繼續(xù)讀取數(shù)據(jù)；

最終得到所照射到的反光柱列表measure_landmark_list；

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

fori＝1:number_data

ifstep(i)＝step(i-1)+1&&range(step(i))-range(step(i-1))<range_threshold

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

將(step(i),range)賦值到當(dāng)前第k個landmark子列表中

number＝number+1；

else

ifnumber＝1，

then將(k,α,ρ)寫入measure_landmark_list，

其中α＝(step-1)·resolution+min_angle，ρ＝range+r；

else

提取當(dāng)前l(fā)andmark子列表中的反光最強點的兩組數(shù)據(jù)(step1,range1)和(step2,range2)，計算：

ifrange1<range2

else

endif

endif

新建第k+1個landmark子列表；

number＝0；

將(step(i),range)賦值到新的landmark子列表中，

number＝number+1

endif

endfor

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

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

對于data表中所有的反光強點：

如果第i個反光強點的序號step(i)等于第i-1個反光強點的序號step(i-1)+1，那么光束step(i)和step(i-1)照射到的是同一個反光柱，則將(step(i),range)賦值到當(dāng)前第k個landmark子列表中，且該子列表中光束數(shù)據(jù)的組數(shù)number＝number+1；

否則，照射到的是一個新的反光柱，則當(dāng)前第k個landmark子列表結(jié)束；

如果number＝1，則將(k,α,ρ)寫入measure_landmark_list，其中α＝(step-1)·resolution+min_angle，ρ＝range+r；min_angle為激光傳感器掃描角度的最小值；

如果number>＝2，則提取當(dāng)前l(fā)andmark子列表中的反光最強點的兩組數(shù)據(jù)(step1,range1)和(step2,range2)，計算：

如果range1<range2

否則

同時，新建第k+1個landmark子列表，組數(shù)初始化為number＝0；將(step(i),range)賦值到新的landmark子列表中，number＝number+1；繼續(xù)讀取數(shù)據(jù)；

最終得到所照射到的反光柱列表measure_landmark_list；

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

計算任意兩個反光柱之間的距離

計算任意兩個反光強點之間的距離

對于measure_landmark_list列表中所照射到的m個反光柱所有兩兩組合(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)頻次最高的值，即為反光強點k對應(yīng)的反光柱c(k)，如果找不到這樣的點，則從measure_landmark_list中去掉點i，并且令m＝m-1；

如果measure_landmark_list中包含的反光強點個數(shù)m<＝2，則警告處理。

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

對于第i個反光柱，i取1到n，計算：

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

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

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

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

計算測量點(ρk,αk)和世界坐標(biāo)系的誤差：

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

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

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

zk＝xk+i*yk

zl＝xl+i*yl

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

(9)多個優(yōu)化：如果激光傳感器檢測到三個及以上的反光柱數(shù)據(jù)，可以根據(jù)多組數(shù)據(jù)中任意兩組計算得到的位姿進行數(shù)據(jù)融合?？蛇x的方案有：選取反射光強最強的兩組數(shù)據(jù)的計算結(jié)果作為激光傳感器當(dāng)前位姿，或在多組數(shù)據(jù)中兩兩組合進行位姿計算，然后求取他們的平均值，或加權(quán)平均值(如權(quán)重與反射光強度相關(guān))。

(10)激光傳感器位置濾波：首先根據(jù)移動平臺運動學(xué)模型進行位姿預(yù)測，公式如下：

θt|t-1＝θt-1|t-1+δθc

其中，輸入量為：上一時刻的位姿(xt-1|t-1,yt-1|t-1,θt-1|t-1)，電機編碼器的分辨率resolution，減速比為speed_reduction_ratio，左、右輪編碼器此刻的讀數(shù)count_left、count_right，左、右輪編碼器上一刻的讀數(shù)last_count_right、last_count_right，采樣周期δt，左右輪半徑r，左、右輪的角速度ωl、ωr，移動平臺在上一時刻的線速度vt-1，移動平臺在上一時刻的轉(zhuǎn)向角速度ωt-1，移動平臺在上一時刻世界坐標(biāo)系下的轉(zhuǎn)向角度θt-1，激光傳感器距離兩個輪子中心點的距離d；輸出量為：相對上時刻的運動增量(δx,δy,δθ)，根據(jù)上一時刻數(shù)據(jù)預(yù)測的當(dāng)前時刻位姿(xt|t-1,yt|t-1,θt|t-1)；

然后根據(jù)上一時刻預(yù)測的位姿，結(jié)合當(dāng)前時刻的位姿計算結(jié)果，對當(dāng)前時刻的位置進行濾波；若當(dāng)前時刻僅檢測到2個反光柱，記為l和k，一種可能的線性卡爾曼濾波的公式如下：

其中，輸入量為：當(dāng)前時刻預(yù)測的位置上一時刻的協(xié)方差矩陣pt-1|t-1，過程噪聲的協(xié)方差矩陣q，測量噪聲的協(xié)方差矩陣r；輸出量為：激光傳感器在世界坐標(biāo)系下的坐標(biāo)當(dāng)前時刻更新的協(xié)方差矩陣pt|t；

若當(dāng)前時刻檢測到3個及以上的反光柱，則采用多個數(shù)據(jù)融合的思想進行濾波；一種可能的方案是貫序卡爾曼濾波，偽代碼如下：

對于檢測到的m個反光柱中的任意兩兩組合，計算：

zk＝xk+i*yk

zl＝xl+i*yl

遍歷所有組合后，得到：

實施例1

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

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

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

進一步地，工控機提取激光傳感器測距數(shù)據(jù)中的復(fù)數(shù)域信息，利用兩個反光柱的數(shù)據(jù)進行agv坐標(biāo)位置和姿態(tài)的計算；若同時檢測到3個以上的反光柱信息，則將反光柱數(shù)據(jù)進行兩兩組合，在每周組合下分別計算，然后以計算結(jié)果的平均值作為agv的位置和姿態(tài)。

進一步地，工控機建立agv運動模型，并在此基礎(chǔ)上利用卡爾曼濾波來精確估計agv的真實位置與姿態(tài)。本發(fā)明的整體流程如圖4所示。