[發(fā)明專利]一種基于協(xié)方差交叉融合SLAM的機(jī)器人定位方法及裝置

在審

• 申請?zhí)枺?/td>: 201910294760.0
• 申請日: 2019-04-12
• 公開（公告）號: CN110118556A
• 公開（公告）日: 2019-08-13
• 發(fā)明（設(shè)計）人: 董輝;周伯男;楊旭升;何青;俞立;陳子豪;張文安
• 申請（專利權(quán)）人: 浙江工業(yè)大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 杭州君度專利代理事務(wù)所(特殊普通合伙) 33240
• 代理人: 楊天嬌

1.一種基于協(xié)方差交叉融合SLAM的機(jī)器人定位方法，其特征在于，所述基于協(xié)方差交叉融合SLAM的機(jī)器人定位方法，包括：

通過機(jī)器人的雙目相機(jī)采集周邊環(huán)境的圖像并進(jìn)行校正處理，得到校正后的圖像；

對圖像進(jìn)行圖像特征提取及匹配計算獲得相機(jī)的位姿，并通過雙目立體計算得到圖像匹配特征點(diǎn)的深度信息，進(jìn)而構(gòu)建相應(yīng)的地圖點(diǎn)，得到機(jī)器人的狀態(tài)空間模型以及機(jī)器人對路標(biāo)的觀測模型；

根據(jù)前一時刻機(jī)器人位姿的優(yōu)化估計值預(yù)測當(dāng)前時刻機(jī)器人位姿的預(yù)測值，根據(jù)機(jī)器人對路標(biāo)的觀測模型得到當(dāng)前時刻的觀測量，結(jié)合當(dāng)前時刻的觀測量和預(yù)測值，采用基于CI融合的濾波方法進(jìn)行修正更新，得到當(dāng)前時刻機(jī)器人位姿的優(yōu)化估計值；

不斷進(jìn)行迭代，對機(jī)器人的位姿進(jìn)行修正更新。

2.根據(jù)權(quán)利要求1所述的基于協(xié)方差交叉融合SLAM的機(jī)器人定位方法，其特征在于，所述根據(jù)前一時刻機(jī)器人位姿的優(yōu)化估計值預(yù)測當(dāng)前時刻機(jī)器人位姿的預(yù)測值，根據(jù)機(jī)器人對路標(biāo)的觀測模型得到當(dāng)前時刻的觀測量，結(jié)合當(dāng)前時刻的觀測量和預(yù)測值，采用基于CI融合的濾波方法進(jìn)行修正更新，得到當(dāng)前時刻機(jī)器人位姿的優(yōu)化估計值，包括：

由k-1時刻機(jī)器人位姿的優(yōu)化估計值預(yù)測k時刻機(jī)器人位姿的預(yù)測值其中：u_k是k時刻的系統(tǒng)控制量，A和B是系統(tǒng)參數(shù)；

由k-1時刻狀態(tài)空間模型的誤差協(xié)方差P_k-1|k-1和過程噪聲Q_k-1預(yù)測新的誤差協(xié)方差P_k|k-1，其中：P_k|k-1＝AP_k-1|k-1A^T+Q_k-1，A是系統(tǒng)參數(shù)；

計算濾波增益W_k：

其中H_k是觀測模型z_k的雅各布矩陣，R_k是觀測噪聲的協(xié)方差，S_k是更新向量的協(xié)方差，更新向量為z_k是k時刻的觀測量，為預(yù)測值，ω是參數(shù)，取值范圍為ω∈[0,1]；

根據(jù)計算所得的濾波增益，結(jié)合當(dāng)前時刻的觀測量z_k和預(yù)測值采用基于CI融合的濾波方法進(jìn)行修正更新，得到當(dāng)前時刻機(jī)器人位姿的優(yōu)化估計值

其中

3.根據(jù)權(quán)利要求1所述的基于協(xié)方差交叉融合SLAM的機(jī)器人定位方法，其特征在于，所述根據(jù)前一時刻機(jī)器人位姿的優(yōu)化估計值預(yù)測當(dāng)前時刻機(jī)器人位姿的預(yù)測值，根據(jù)機(jī)器人對路標(biāo)的觀測模型得到當(dāng)前時刻的觀測量，結(jié)合當(dāng)前時刻的觀測量和預(yù)測值，采用基于CI融合的濾波方法進(jìn)行修正更新，得到當(dāng)前時刻機(jī)器人位姿的優(yōu)化估計值，包括：

由k-1時刻機(jī)器人位姿的優(yōu)化估計值預(yù)測k時刻機(jī)器人位姿的預(yù)測值其中：u_k是k時刻的系統(tǒng)控制量，A和B是系統(tǒng)參數(shù)；

由k-1時刻狀態(tài)空間模型的誤差協(xié)方差P_k-1|k-1和過程噪聲Q_k-1預(yù)測新的誤差協(xié)方差P_k|k-1，其中：P_k|k-1＝AP_k-1|k-1A^T+Q_k-1，A是系統(tǒng)參數(shù)；

計算濾波增益W_k：

其中H_k是觀測模型z_k的雅各布矩陣，R_k是觀測噪聲的協(xié)方差，S_k是更新向量的協(xié)方差，更新向量為z_k是k時刻的觀測量，為預(yù)測值，ω是參數(shù)，取值范圍為ω∈[0,1]，σ為預(yù)設(shè)的糾正參數(shù)；

根據(jù)計算所得的濾波增益，結(jié)合當(dāng)前時刻的觀測量z_k和預(yù)測值采用基于CI融合的濾波方法進(jìn)行修正更新，得到當(dāng)前時刻機(jī)器人位姿的優(yōu)化估計值

其中