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

有效

• 申請號: 201710571102.2
• 申請日: 2017-07-13
• 公開（公告）號: CN107356931B
• 公開（公告）日: 2019-10-18
• 發明（設計）人: 林志赟;邢浩;金智
• 申請（專利權）人: 浙江科鈦機器人股份有限公司
• 主分類號: G01S17/48
• 分類號: G01S17/48;G01S17/50;G01S7/481;G01S7/48
• 代理公司: 杭州求是專利事務所有限公司 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

遍歷所有組合后，得到：