[發(fā)明專利]基于改進(jìn)粒子提議分布和自適應(yīng)粒子重采樣的FastSLAM方法

有效

• 申請?zhí)枺?/td>: 201611001062.X
• 申請日: 2016-11-14
• 公開（公告）號: CN106599368B
• 公開（公告）日: 2019-06-25
• 發(fā)明（設(shè)計）人: 陳耀武;徐巍軍;黃余格
• 申請（專利權(quán)）人: 浙江大學(xué)
• 主分類號: G06F17/50
• 分類號: G06F17/50
• 代理公司: 杭州天勤知識產(chǎn)權(quán)代理有限公司 33224
• 代理人: 胡紅娟

1.一種基于改進(jìn)粒子提議分布和自適應(yīng)粒子重采樣的FastSLAM方法，包括以下步驟：

(1)將運(yùn)動噪聲和測量噪聲同時增廣至機(jī)器人位姿狀態(tài)向量中，利用平方根轉(zhuǎn)換無味卡爾曼濾波器對最優(yōu)粒子提議分布進(jìn)行估計，并根據(jù)該粒子提議分布對機(jī)器人位姿狀態(tài)進(jìn)行采樣，具體過程為：

(1-1)將k時刻的機(jī)器人位姿和路標(biāo)特征地圖的聯(lián)合后驗概率表示為如下N_k個粒子組成的粒子集：

其中，i表示粒子序號，表示k時刻第i個粒子的權(quán)重，和分別表示k時刻第i個機(jī)器人位姿狀態(tài)的估計值及其協(xié)方差平方根因子；和分別表示k時刻第i個粒子中關(guān)于第m個路標(biāo)特征路標(biāo)的位置均值和相應(yīng)的協(xié)方差平方根因子；

(1-2)將運(yùn)動噪聲和測量噪聲信息同時增廣至機(jī)器人位姿狀態(tài)向量及其協(xié)方差矩陣的平方根因子中，保證協(xié)方差的半正定性；

(1-3)根據(jù)機(jī)器人運(yùn)動函數(shù)、增廣后的機(jī)器人位姿狀態(tài)向量均值及其協(xié)方差矩陣的平方根因子，計算機(jī)器人位姿狀態(tài)的預(yù)測均值及其協(xié)方差平方根因子，具體過程為：

(1-3-1)根據(jù)增廣后的機(jī)器人位姿狀態(tài)向量均值和協(xié)方差矩陣的平方根因子，計算轉(zhuǎn)換無味Sigma點集：

其中，和分別為增廣后的機(jī)器人位姿狀態(tài)向量均值和協(xié)方差矩陣的平方根因子，表示增廣狀態(tài)向量的維度，表示第j個轉(zhuǎn)換無味Sigma點，每個轉(zhuǎn)換無味Sigma點包含了機(jī)器人位姿狀態(tài)分量運(yùn)動噪聲分量和測量噪聲分量為n_a維向量，其元素由下式確定：

其中，r表示向量中的元素序號，表示取下整運(yùn)算符，當(dāng)n_a為奇數(shù)時，最后一個元素為

(1-3-2)根據(jù)機(jī)器人運(yùn)動函數(shù)生成機(jī)器人位姿狀態(tài)的預(yù)測轉(zhuǎn)換無味Sigma點：

其中，f(·)表示給定的機(jī)器人運(yùn)動函數(shù)，u_k表示當(dāng)前時刻的控制輸入；

(1-3-3)根據(jù)預(yù)測的轉(zhuǎn)換無味Sigma點計算機(jī)器人位姿狀態(tài)的預(yù)測均值及其協(xié)方差平方根因子：

其中，qr(·)表示矩陣的QR分解運(yùn)算，Π_v，k|k-1定義為以下加權(quán)中心偏差矩陣：

(1-4)當(dāng)?shù)趌個路標(biāo)特征被機(jī)器人再次成功觀測到時，根據(jù)路標(biāo)特征測量值z_k，計算機(jī)器人位姿狀態(tài)后驗估計均值及其協(xié)方差矩陣平方根因子；

(1-5)計算粒子的提議分布，并根據(jù)此分布采樣生成新一代粒子；

(2)利用平方根容積卡爾曼濾波器更新每個粒子對應(yīng)的路標(biāo)特征地圖信息；

(3)在粒子重采樣階段，利用基于相對熵的自適應(yīng)粒子重采樣方法確定當(dāng)前時刻所需的粒子個數(shù)；

(4)根據(jù)重采樣后的粒子集確定機(jī)器人的位姿狀態(tài)和路標(biāo)特征地圖信息。

2.如權(quán)利要求1所述基于改進(jìn)粒子提議分布和自適應(yīng)粒子重采樣的FastSLAM方法，其特征在于：步驟(1-4)的具體步驟為：

(1-4-1)當(dāng)?shù)趌個路標(biāo)特征被機(jī)器人再次成功觀測到時，根據(jù)測量函數(shù)h(·)生成測量值Sigma點：

其中，表示第l個路標(biāo)特征在k-1時刻的位置狀態(tài)估計值；h(·)表示給定的機(jī)器人觀測函數(shù)；

(1-4-2)根據(jù)測量值Sigma點計算預(yù)測測量值均值：

(1-4-3)利用QR分解，計算預(yù)測測量值均值的協(xié)方差矩陣的平方根因子：

其中，Π_z，k|k-1定義為以下加權(quán)中心偏差矩陣：

(1-4-4)計算機(jī)器人位姿狀態(tài)的交叉協(xié)方差矩陣和卡爾曼增益：

(1-4-5)根據(jù)路標(biāo)特征的測量值z_k，計算機(jī)器人位姿狀態(tài)后驗估計均值及其協(xié)方差矩陣平方根因子：

其中，cholupdate(·)函數(shù)表示矩陣的Cholesky更新分解運(yùn)算，‘-’表示對參數(shù)先進(jìn)行矩陣減法運(yùn)算。