一種基于雙反光柱的激光定位與導航方法與流程
本發明屬于智能倉儲物流領域，具體涉及一種基于雙反光柱的激光定位與導航方法。
背景技術：
基于激光傳感器的定位與導航技術是工業agv、智能機器人等領域的關鍵性技術，相比傳統有軌導航方式，該技術具有定位精度高、靈活多變等優點，適用于復雜、高動態的工業場景中。國內外現有基于反光柱的激光定位技術，均采用了幾何三角定位原理，每時每刻必須要求同時檢測到三個反光柱才可實現定位。這一數量要求實際上在復雜工業場景中很難滿足，極大地制約了激光定位與導航技術的實用性。
技術實現要素：
為了解決上述問題，本發明提供一種新型激光定位與導航方法，通過提取和利用激光測距信息中的復數域信息，將三個反光柱的最低要求降低為兩個，有效解決了反光柱最低數量的技術瓶頸，大大提高了激光導航技術的適用性。
本發明的目的是通過以下技術方案來實現的：一種基于雙反光柱的激光定位與導航方法，該方法包括：
(1)在工業環境中布置反光柱，預設反光柱世界坐標，生成反光柱坐標列表；
(2)安裝在移動平臺上的激光傳感器向周圍呈輻射狀發射激光，并接收反射激光；
(3)篩選來源于反光柱的有效光束：通過檢測反射激光強度i并與預設強度閾值σ相比較，判定激光照射物是反光柱還是普通環境物體；
(4)確定當前時刻照射到的反光柱數量及其相對激光傳感器的相對坐標：根據反射光束角度的連續性，判斷是否為同一個反光柱，或者，根據反射光束角度和距離的連續性，判斷是否為同一個反光柱；根據屬于同一反光柱的反射光數據，結合反光柱半徑進行數據修正，得到該反光柱相對于激光傳感器的相對坐標，并存入反光柱列表；
(5)初始化反光柱列表，得到至少兩個反光柱的世界坐標：人工確定初始位置對應的反光柱列表中至少兩個反光柱的世界坐標；或者，激光傳感器在初始位置獲取至少三個反光柱返回的角度和距離，計算兩兩反光柱之間的距離，與根據反光柱坐標列表生成的反光柱距離信息相匹配，得到至少兩個反光柱的世界坐標；
(6)在動態過程中計算期望反光柱列表：根據上一時刻對當前時刻激光傳感器位置和角度的預測，估計激光傳感器與所有反光柱之間的相對距離和角度，并存入期望反光柱列表；
(7)在動態過程中反光柱列表的匹配：計算當前時刻反光柱列表與期望反光柱列表中同一個反光柱對應的距離之差和角度之差，當距離之差和角度之差均滿足預設閾值時，匹配成功；
(8)基于雙反光柱數據的激光傳感器位姿計算：利用激光傳感器測量數據的復頻域信息，在匹配成功的反光柱中任選兩個：第l個和第k個，并計算：
zk＝xk+i*yk
zl＝xl+i*yl
其中，下標l和k分別代表第l和第k個反光柱；α和ρ分別代表在激光傳感器相對自身的極坐標系下，反光柱的角度和距離；x和y分別為反光柱在x和y軸的分量；z為反光柱在世界坐標系下的復數坐標；zk,l為根據第l和第k個反光柱計算得到的激光傳感器世界坐標，θk是根據第k個反光柱的數據計算得到的激光傳感器在世界坐標系下的角度。
進一步地，所述步驟(6)中，直接采用上一時刻激光傳感器的位置和角度作為當前時刻的預測，或者采用濾波算法進行預測。
進一步地，所述步驟(8)之后，還包括多個優化的步驟：如果激光傳感器檢測到三個及以上的反光柱數據，可以根據多組數據中任意兩組計算得到的位姿進行數據融合，得到最終的移動平臺及激光傳感器在世界坐標系下的位置及角度。
本發明的有益效果如下：與傳統技術中的幾何三角定位不同，本發明深入挖掘并充分利用了反射光的復數域信息，打破了傳統意義上三個反光柱的技術瓶頸，其運行條件為同時檢測到兩個反光柱的數據即可，大大提高了激光導航技術的適用性；雖然本發明最低要求為檢測到兩個反光柱，但實際應用時可能會同時檢測到多個反光柱，通過設計數據融合處理，能夠最大化利用反光柱的信息，從而進一步提高精度。本發明的具體性能為：定位誤差<1cm，角度誤差<0.5°，定位頻率>35hz。
附圖說明
圖1為本發明的硬件結構示意圖；
圖2為反光柱測距與數據修正示意圖；
圖3為移動平臺運動學模型示意圖；
圖4為本發明方法整體流程圖。
具體實施方式
下面結合附圖，通過實施例對本發明作進一步地說明。
本發明提供的一種基于雙反光柱的高精度激光定位與導航方法，該方法包括：
(1)在工業環境中布置反光柱，預設反光柱世界坐標，生成反光柱坐標列表；
(2)安裝在移動平臺上的激光傳感器向周圍呈輻射狀發射激光，并接收反射激光(激光傳感器安裝在agv、工業車輛等移動平臺上)；
(3)篩選來源于反光柱的有效光束：通過檢測反射激光強度i并與預設強度閾值σ相比較，判定激光照射物是反光柱還是普通環境物體，篩選出來源于反光柱的反射光數據；
(4)確定當前時刻照射到的反光柱數量及其相對激光傳感器的相對坐標：根據反射光束角度的連續性，判斷是否為同一個反光柱，或者，根據反射光束角度和距離的連續性，判斷是否為同一個反光柱；根據屬于同一反光柱的反射數據，結合反光柱半徑r進行數據修正，如圖2所示，得到該反光柱相對于激光傳感器的相對坐標，并存入反光柱列表；
根據反射光束角度和距離的連續性，判斷是否為同一個反光柱，一種可能的實現方式如下：
對于data表中所有的反光強點：
如果第i個反光強點的序號step(i)等于第i-1個反光強點的序號step(i-1)+1，并且第i和i-1的激光強點測得的距離之差range(step(i))-range(step(i-1))小于預設的距離閾值range_threshold，那么光束step(i)和step(i-1)照射到的是同一個反光柱，則將(step(i),range)賦值到當前第k個landmark子列表中，且該子列表中光束數據的組數number＝number+1；
否則，照射到的是一個新的反光柱，則當前第k個landmark子列表結束；
如果number＝1，則將(k,α,ρ)寫入measure_landmark_list，其中α＝(step-1)·resolution+min_angle，ρ＝range+r；min_angle為激光傳感器掃描角度的最小值；
如果number>＝2，則提取當前landmark子列表中的反光最強點的兩組數據(step1,range1)和(step2,range2)，計算：
如果range1<range2
否則
同時，新建第k+1個landmark子列表，組數初始化為number＝0；將(step(i),range)賦值到新的landmark子列表中，number＝number+1；繼續讀取數據；
最終得到所照射到的反光柱列表measure_landmark_list；
對應的偽代碼如下：
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)賦值到當前第k個landmark子列表中
number＝number+1；
else
ifnumber＝1，
then將(k,α,ρ)寫入measure_landmark_list，
其中α＝(step-1)·resolution+min_angle，ρ＝range+r；
else
提取當前landmark子列表中的反光最強點的兩組數據(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是反光強點的個數，resolution是相鄰兩道激光光束的夾角(分辨率)，range_threshold是判定是否照射到同一個反光柱的距離閾值，α和ρ分別代表在激光傳感器相對自身的極坐標系下，該反光柱的角度和距離，measure_landmark_list為照射到反光柱的列表。
或者：僅根據反射光束角度的連續性，判斷是否為同一個反光柱，一種可能的實現方式如下：
對于data表中所有的反光強點：
如果第i個反光強點的序號step(i)等于第i-1個反光強點的序號step(i-1)+1，那么光束step(i)和step(i-1)照射到的是同一個反光柱，則將(step(i),range)賦值到當前第k個landmark子列表中，且該子列表中光束數據的組數number＝number+1；
否則，照射到的是一個新的反光柱，則當前第k個landmark子列表結束；
如果number＝1，則將(k,α,ρ)寫入measure_landmark_list，其中α＝(step-1)·resolution+min_angle，ρ＝range+r；min_angle為激光傳感器掃描角度的最小值；
如果number>＝2，則提取當前landmark子列表中的反光最強點的兩組數據(step1,range1)和(step2,range2)，計算：
如果range1<range2
否則
同時，新建第k+1個landmark子列表，組數初始化為number＝0；將(step(i),range)賦值到新的landmark子列表中，number＝number+1；繼續讀取數據；
最終得到所照射到的反光柱列表measure_landmark_list；
(5)初始化反光柱列表，得到至少兩個反光柱的世界坐標：人工確定初始位置對應的反光柱列表中至少兩個反光柱的世界坐標；或者，激光傳感器在初始位置獲取至少三個反光柱返回的角度和距離，計算兩兩反光柱之間的距離，與根據反光柱坐標列表生成的反光柱距離信息相匹配，得到至少兩個反光柱的世界坐標；一種可能的實現方式如下：
計算任意兩個反光柱之間的距離
計算任意兩個反光強點之間的距離
對于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)中出現頻次最高的值，即為反光強點k對應的反光柱c(k)，如果找不到這樣的點，則從measure_landmark_list中去掉點i，并且令m＝m-1；
如果measure_landmark_list中包含的反光強點個數m<＝2，則警告處理。
(6)在動態過程中計算期望反光柱列表：根據上一時刻對當前時刻激光傳感器位置和角度的預測，估計激光傳感器與所有反光柱之間的相對距離和角度，并存入期望反光柱列表；(直接采用上一時刻激光傳感器的位置和角度作為當前時刻的預測，或者采用濾波算法進行預測)一種可能的實現方式如下：
對于第i個反光柱，i取1到n，計算：
其中函數arctan2(x,y)返回原點至點(x,y)的方位角，取值為(-π,π]。
如果ρt|t-1小于最大距離閾值max_distance，并且αt|t-1大于激光傳感器掃描角度的最小值min_angle減去角度閾值threshold_angle，αt|t-1小于激光傳感器掃描角度的最大值max_angle加上角度閾值threshold_angle，則將
對應的偽代碼如下：
fori＝1:n
如果ρt|t-1＜max_distance&&min_angle-threshold_angle＜αt|t-1＜max_angle+threshold_angle
則將
end
(7)在動態過程中反光柱列表的匹配：計算當前時刻反光柱列表與期望反光柱列表中同一個反光柱對應的距離之差和角度之差，當距離之差和角度之差均滿足預設閾值時，匹配成功；一種可能的實現方式如下：
對于measure_landmark_list的所有反光柱，找到{i,xi,yi,ρt|t-1,αt|t-1}，使得：
計算測量點(ρk,αk)和世界坐標系
如果dist<threshold_distance，則c(k)＝i
如果找不到這樣的點，則從measure_landmark_list中去掉點i，并令m＝m-1；如果measure_landmark_list中包含的反光強點個數m<＝2，則警告處理。
(8)基于雙反光柱數據的激光傳感器位姿計算：利用激光傳感器測量數據的復頻域信息，在匹配成功的反光柱中任選兩個：第l個和第k個，并計算：
zk＝xk+i*yk
zl＝xl+i*yl
其中，下標l和k分別代表第l和第k個反光柱；α和ρ分別代表在激光傳感器相對自身的極坐標系下，反光柱的角度和距離；x和y分別為反光柱在x和y軸的分量；z為反光柱在世界坐標系下的復數坐標；zk,l為根據第l和第k個反光柱計算得到的激光傳感器世界坐標，θk是根據第k個反光柱的數據計算得到的激光傳感器在世界坐標系下的角度；
(9)多個優化：如果激光傳感器檢測到三個及以上的反光柱數據，可以根據多組數據中任意兩組計算得到的位姿進行數據融合。可選的方案有：選取反射光強最強的兩組數據的計算結果作為激光傳感器當前位姿，或在多組數據中兩兩組合進行位姿計算，然后求取他們的平均值，或加權平均值(如權重與反射光強度相關)。
實施例1
如圖1所示，本發明的硬件構成主要包括激光傳感器、工控機、車載控制電路與車載電源；所述的工控機、激光傳感器與車載控制電路通過rs232/can/spi等方式進行通信；車載電源對激光傳感器和車載控制電路經dc/dc模個進行直流供電，經dc/ac模個對車載工控機進行交流供電。
進一步地，在工業環境(地圖)中合適的位置放置一定數量的反光柱，以agv為例，使得agv在地圖中的任何位置都能夠成功檢測到2個或以上的反光柱；激光傳感器發射激光并檢測反射信號。
進一步地，工控機計算反光柱間距，初始化時采用靜態匹配，隨后均采用動態匹配來識別反光柱，并從反光柱列表中獲取反光柱世界坐標信息。
進一步地，工控機提取激光傳感器測距數據中的復數域信息，利用兩個反光柱的數據進行agv坐標位置和姿態的計算；若同時檢測到3個以上的反光柱信息，則將反光柱數據進行兩兩組合，在每周組合下分別計算，然后以計算結果的平均值作為agv的位置和姿態。本發明的整體流程如圖4所示。
- 還沒有人留言評論。精彩留言會獲得點贊！