[發(fā)明專利]一種室內(nèi)機器人定位與建圖方法及裝置有效
| 申請?zhí)枺?/td> | 201910052711.6 | 申請日： | 2019-01-21 |
| 公開（公告）號： | CN109798896B | 公開（公告）日： | 2023-01-03 |
| 發(fā)明（設(shè)計）人： | 李冰;蘇聰;王剛;張林;王亞洲;劉勇;董乾;趙霞 | 申請（專利權(quán)）人： | 東南大學(xué) |
| 主分類號： | G01C21/20 | 分類號： | G01C21/20 |
| 代理公司： | 南京經(jīng)緯專利商標代理有限公司 32200 | 代理人： | 朱小兵 |
| 地址： | 214135 江*** | 國省代碼： | 江蘇;32 |
| 權(quán)利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關(guān)鍵詞： | 一種 室內(nèi) 機器人 定位 方法 裝置 | ||
1.一種室內(nèi)機器人定位與建圖方法，其特征在于，包括以下步驟：
步驟1、基于環(huán)境信息進行粒子采樣，獲取k時刻每個粒子的位姿，并且根據(jù)粒子個數(shù)平均分配各粒子的權(quán)值；
步驟2、將獲取的k時刻下機器人與目標或障礙物之間距離和偏離角度作為k時刻的實際觀測值，根據(jù)k時刻的實際觀測值和每個粒子的位姿，對每個粒子的權(quán)值進行更新并歸一化處理；
步驟3、根據(jù)步驟2更新并歸一化處理后每個粒子的權(quán)值，計算得到有效粒子數(shù)；判斷有效粒子數(shù)小于設(shè)定的閾值時，對粒子進行重采樣，并結(jié)合k時刻的實際觀測值和重采樣粒子的位姿，計算并更新重采樣粒子的權(quán)值；及判斷有效粒子數(shù)大于設(shè)定的閾值時，保持原采樣粒子及每個粒子的權(quán)值；
步驟4、根據(jù)步驟3所得重采樣粒子權(quán)值或原采樣粒子權(quán)值，利用加權(quán)公式計算出當前狀態(tài)的期望值對k時刻的機器人的實際位姿進行更新；
步驟5、根據(jù)步驟1所獲取k時刻每個粒子的位姿和k時刻的實際觀測值，利用卡爾曼濾波算法更新每個粒子地圖的均值和方差，以得全部粒子的地圖信息，具體為：
根據(jù)更新所得重采樣粒子權(quán)值或原采樣粒子權(quán)值的大小對粒子進行排序，判斷更新后各粒子組成的粒子集是否為空，如果判斷為空則結(jié)束更新，反之依次取出粒子集中的粒子；
判斷所取出的粒子集中的每個粒子與上一個粒子是否相似，即判斷兩個粒子的位姿和權(quán)值是否在設(shè)定范圍內(nèi)保持一致，是則對上一個粒子更新后的權(quán)值進行復(fù)制操作，反之則重新利用卡爾曼濾波進行粒子地圖的均值和方差更新，以得到全部粒子的地圖信息；
并由步驟4所更新k時刻機器人的實際位姿結(jié)合k時刻的實際觀測值、機器人所在實際位姿對應(yīng)粒子的地圖信息，建立機器人實際地圖。
2.根據(jù)權(quán)利要求1所述室內(nèi)機器人定位與建圖方法，其特征在于，所述步驟1還包括判斷k時刻是否為初始時刻，是則生成初始時刻的粒子位姿，否則根據(jù)上一代粒子集中粒子的位姿結(jié)合運動模型得到k時刻每個粒子的預(yù)估位姿。
3.根據(jù)權(quán)利要求1所述室內(nèi)機器人定位與建圖方法，其特征在于，所述步驟2中對每個粒子的權(quán)值進行更新，采用公式：
其中，表示更新后的k時刻第i個粒子的權(quán)值；表示在k-1時刻第i個粒子的權(quán)值；p表示k時刻的概率密度函數(shù)；表示k時刻第i個粒子的實際觀測值；表示k時刻第i個粒子的位姿。
4.根據(jù)權(quán)利要求1所述室內(nèi)機器人定位與建圖方法，其特征在于，所述步驟2中對更新的每個粒子權(quán)值進行歸一化處理，采用公式：
其中，表示歸一化后的k時刻第i個粒子的權(quán)值；表示更新后的k時刻第i個粒子的權(quán)值；N表示粒子個數(shù)。
5.根據(jù)權(quán)利要求1所述室內(nèi)機器人定位與建圖方法，其特征在于，所述步驟3計算有效粒子數(shù)Neff，采用公式：
其中，表示歸一化后的k時刻第i個粒子的權(quán)值；N表示粒子個數(shù)。
6.根據(jù)權(quán)利要求1所述室內(nèi)機器人定位與建圖方法，其特征在于，所述步驟5中利用卡爾曼濾波算法更新每個粒子地圖的均值和方差，采用公式：
其中，表示第i個粒子在t時刻更新后的位姿；m為卡爾曼增益，表示第i個粒子在t時刻的預(yù)估位姿，zt代表t時刻的實際觀測值；表示第i個粒子在t時刻的預(yù)估觀測值；表示第i個粒子在t時刻預(yù)估位姿與實際觀測值之間的協(xié)方差；I表示單位矩陣；H表示轉(zhuǎn)換矩陣；表示第i個粒子在t時刻預(yù)估位姿與實際觀測值之間的預(yù)估協(xié)方差。
7.一種室內(nèi)機器人定位與建圖裝置，其特征在于，包括：
運動控制模塊，用于獲取機器人的速度信息，并結(jié)合機器人的速度信息獲取k時刻每個粒子的位姿；
激光雷達測距模塊，用于獲取k時刻下機器人與目標或障礙物之間距離和偏離角度并作為k時刻的實際觀測值；
定位與建圖模塊，用于根據(jù)粒子個數(shù)平均分配各粒子的權(quán)值；并根據(jù)k時刻的實際觀測值和每個粒子的位姿，對分配的每個粒子權(quán)值進行更新并歸一化處理后，計算得到有效粒子數(shù)；判斷有效粒子數(shù)小于設(shè)定的閾值時，控制運動控制模塊對粒子進行重采樣，并計算和更新重采樣粒子的權(quán)值；及判斷有效粒子數(shù)大于設(shè)定的閾值時，保持原采樣粒子及每個粒子權(quán)值；根據(jù)所得重采樣粒子權(quán)值或原采樣粒子權(quán)值，利用加權(quán)公式計算出當前狀態(tài)的期望值對k時刻的機器人的實際位姿進行更新；根據(jù)所獲取k時刻每個粒子的位姿和k時刻的實際觀測值，利用卡爾曼濾波算法更新每個粒子地圖的均值和方差，以得到 全部粒子的地圖信息；并由所更新k時刻機器人的實際位姿結(jié)合k時刻的實際觀測值、機器人所在實際位姿對應(yīng)粒子的地圖信息，建立機器人實際地圖；
其中，所述得到全部粒子的地圖信息，具體為：根據(jù)更新所得重采樣粒子權(quán)值或原采樣粒子權(quán)值的大小對粒子進行排序，判斷更新后各粒子組成的粒子集是否為空，如果判斷為空則結(jié)束更新，反之依次取出粒子集中的粒子；判斷所取出的粒子集中的每個粒子與上一個粒子是否相似，即判斷兩個粒子的位姿和權(quán)值是否在設(shè)定范圍內(nèi)保持一致，是則對上一個粒子更新后的權(quán)值進行復(fù)制操作，反之則重新利用卡爾曼濾波進行粒子地圖的均值和方差更新，以得到全部粒子的地圖信息。
該專利技術(shù)資料僅供研究查看技術(shù)是否侵權(quán)等信息，商用須獲得專利權(quán)人授權(quán)。該專利全部權(quán)利屬于東南大學(xué)，未經(jīng)東南大學(xué)許可，擅自商用是侵權(quán)行為。如果您想購買此專利、獲得商業(yè)授權(quán)和技術(shù)合作，請聯(lián)系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/201910052711.6/1.html，轉(zhuǎn)載請聲明來源鉆瓜專利網(wǎng)。
