[發明專利]一種基于濾波的雙反光柱激光定位與導航方法

有效

• 申請號: 201710572154.1
• 申請日: 2017-07-13
• 公開（公告）號: CN107346025B
• 公開（公告）日: 2019-09-24
• 發明（設計）人: 林志赟;邢浩;金智
• 申請（專利權）人: 浙江科鈦機器人股份有限公司
• 主分類號: G01S17/88
• 分類號: G01S17/88
• 代理公司: 杭州求是專利事務所有限公司 33200
• 代理人: 劉靜;邱啟旺

1.一種基于濾波的雙反光柱激光定位與導航方法，其特征在于，該方法包括：

(1)在工業環境中布置反光柱，預設反光柱世界坐標，生成反光柱坐標列表；

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

(3)篩選來源于反光柱的有效光束：通過檢測反射激光強度I并與預設強度閾值σ相比較，判定激光照射物是反光柱還是普通環境物體；

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

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

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

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

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

z_k＝X_k+i*Y_k

z_l＝X_l+i*Y_l

其中，下標l和k分別代表第l和第k個反光柱；α和ρ分別代表在激光傳感器相對自身的極坐標系下，反光柱的角度和距離；X和Y分別為反光柱在X和Y軸的分量；z為反光柱在世界坐標系下的復數坐標；z_k,l為根據第l和第k個反光柱計算得到的激光傳感器世界坐標，θ_k是根據第k個反光柱的數據計算得到的激光傳感器在世界坐標系下的角度；

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

2.根據權利要求1所述的基于濾波的雙反光柱激光定位與導航方法，其特征在于，所述步驟(9)之前，還包括多個優化的步驟：如果激光傳感器檢測到三個及以上的反光柱數據，可以根據多組數據中任意兩組計算得到的位姿進行數據融合。

3.根據權利要求1所述的基于濾波的雙反光柱激光定位與導航方法，其特征在于，所述步驟(9)中，根據移動平臺運動學模型進行位姿預測，公式如下：

θ_t|t-1＝θ_t-1|t-1+Δθ_c

其中，輸入量為：上一時刻的位姿(x_t-1|t-1,y_t-1|t-1,θ_t-1|t-1)，電機編碼器的分辨率resolution，減速比為speed_reduction_ratio，左、右輪編碼器此刻的讀數count_left、count_right，左、右輪編碼器上一刻的讀數last_count_left、last_count_right，采樣周期ΔT，左右輪半徑r，左、右輪的角速度ω_L、ω_R，移動平臺在上一時刻的線速度v_t-1，移動平臺在上一時刻的轉向角速度ω_t-1，移動平臺在上一時刻世界坐標系下的朝向角度θ_t-1，激光傳感器距離兩個輪子中心點的距離d；輸出量為：相對上時刻的運動增量(Δx,Δy,Δθ)，根據上一時刻數據預測的當前時刻位姿(x_t|t-1,y_t|t-1,θ_t|t-1)；

根據上一時刻預測的位姿，并結合當前時刻的激光傳感器測量數據，對當前時刻的位姿進行濾波，具體如下：若當前時刻僅檢測到2個反光柱，記為l和k，采用線性卡爾曼濾波的公式如下：

其中，輸入量為：當前時刻預測的位置上一時刻的協方差矩陣P_t-1|t-1，過程噪聲的協方差矩陣Q，測量噪聲的協方差矩陣R；輸出量為：激光傳感器在世界坐標系下的坐標當前時刻更新的協方差矩陣P_t|t；

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

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

z_k＝X_k+i*Y_k

z_l＝X_l+i*Y_l

遍歷所有組合后，得到：

。