機器人同步定位與地圖創(chuàng)建方法及裝置的制造方法
【技術領域】
[0001] 本發(fā)明涉及機器人同步定位與地圖創(chuàng)建領域，尤其涉及一種機器人同步定位與地 圖創(chuàng)建方法及裝置。
【背景技術】
[0002] 機器人定位和地圖創(chuàng)建問題是移動機器人導航研宄中的關鍵問題，可靠導航是自 主機器人最基本、最重要的一項功能，對移動機器人的研宄有非常重要的意義。
[0003] 目前解決移動機器人同步定位與地圖創(chuàng)建問題的主要方法是應用卡爾曼濾波計 算。例如，申請?zhí)枮?01410581627.0的申請文件提出一種卡爾曼濾波方法，根據(jù)機器人 的運動學方程和基于相對方位的量測方程，給出多移動機器人自定位的動態(tài)模型；采用 SR-UKF濾波算法，以相對方位為輸入對系統(tǒng)狀態(tài)進行整體更新，實現(xiàn)多移動機器人的協(xié)同 自定位。申請文件201410581627. 0采用的卡爾曼濾波法姿態(tài)解算方法建模簡單、實時性較 好，但是忽略了非線性因素尤其是載體位移加速度對姿態(tài)測量信息的影響。
[0004] 為了提高機器人的姿態(tài)解算精度，申請?zhí)枮?01410489632. 9的專利申請文件提 出一種基于改進的擴展卡爾曼濾波算法的兩輪自平衡機器人姿態(tài)解算方法，利用STM32交 互采集處理兩輪自平衡機器人慣性姿態(tài)測量數(shù)據(jù)；利用兩輪自平衡機器人姿態(tài)測量系統(tǒng)建 立MEMS陀螺儀誤差數(shù)學模型；利用兩輪自平衡機器人姿態(tài)測量系統(tǒng)建立MEMS MU姿態(tài)解 算數(shù)學模型；利用MEMS IMU姿態(tài)解算數(shù)學模型構建擴展卡爾曼濾波器模型；利用改進的擴 展卡爾曼濾波算法對兩輪自平衡機器人姿態(tài)測量數(shù)據(jù)進行數(shù)據(jù)融合；利用二階近似解算減 少非線性函數(shù)非線性截斷誤差；利用位移加速度判斷公式，自適應量測噪聲矩陣對兩輪自 平衡機器人姿態(tài)進行補償；利用兩步量測更新的方法提高改進的擴展卡爾曼濾波的先驗解 算精度。
[0005] 但是該種方法未充分考慮到機器人模型非線性化的程度，如果機器人模型非線性 化程度較高，易產(chǎn)生較大的位姿估計誤差，影響非線性的環(huán)境下的系統(tǒng)定位精度。

【發(fā)明內(nèi)容】

[0006] 本發(fā)明實施例提供一種機器人同步定位與地圖創(chuàng)建方法及裝置，能夠減小采樣點 在高度非線性環(huán)境下的失真。
[0007] 本發(fā)明實施例采用如下技術方案：
[0008] -方面提供一種機器人同步定位與地圖創(chuàng)建方法，包括：
[0009] 根據(jù)外部傳感器采集的外部環(huán)境信息，得到觀測信息數(shù)據(jù)集；
[0010] 根據(jù)當前時刻的后驗概率分布和下一時刻的運動控制信息，得到下一時刻的先驗 概率分布；
[0011] 利用所述下一時刻的先驗概率分布和所述觀測信息數(shù)據(jù)集更新后驗概率分布；
[0012] 根據(jù)更新后的后驗概率分布更新當前時刻地圖特征信息并更新當前時刻機器人 控制信息。
[0013] 可選的，所述根據(jù)外部傳感器采集的外部環(huán)境信息，得到觀測信息數(shù)據(jù)集之前，還 包括：建立機器人運動模型；
[0014] 建立所述機器人運動模型應用的機器人運動方程為：
【主權項】
1. 一種機器人同步定位與地圖創(chuàng)建方法，其特征在于，包括： 根據(jù)外部傳感器采集的外部環(huán)境信息，得到觀測信息數(shù)據(jù)集； 根據(jù)當前時刻的后驗概率分布和下一時刻的運動控制信息，得到下一時刻的先驗概率 分布； 利用所述下一時刻的先驗概率分布和所述觀測信息數(shù)據(jù)集更新后驗概率分布； 根據(jù)更新后的后驗概率分布更新當前時刻地圖特征信息并更新當前時刻機器人控制 信息。
2. 根據(jù)權利要求1所述的方法，其特征在于，所述根據(jù)外部傳感器采集的外部環(huán)境信 息，得到觀測信息數(shù)據(jù)集之前，還包括：建立機器人運動模型； 建立所述機器人運動模型應用的機器人運動方程為：
其中，mk表示機器人位姿，T表示系統(tǒng)內(nèi)部采樣間隔，vk表示機器人行進速度，ak表示 轉(zhuǎn)角，L表示輪距，mk表示K時刻機器人的位姿，mk包括機器人K時刻X方向上的位移（mx，k)， K時刻Y方向上的位移（my，k)，K時刻角度位移（m0，k)，相應的參數(shù)如表示為機器人K-l 時刻X方向上的位移，ak為機器人轉(zhuǎn)角。
3. 根據(jù)權利要求1或2所述的方法，其特征在于，所述根據(jù)外部傳感器采集的外部環(huán)境 信息，得到觀測信息數(shù)據(jù)集之前，還包括：建立觀測模型； 建立所述觀測模型應用的機器人觀測方程為：
其中，lk表示機器人與觀測到地圖特征點的距離，0 k表示機器人與觀測到地圖特征點 的夾角，為機器人觀測到的地圖特征點i的位置，zk表示機器人相對于觀測到的 地圖特征點的位置。
4. 根據(jù)權利要求3所述的方法，其特征在于，所述根據(jù)當前時刻的后驗概率分布和下 一時刻的運動控制信息，得到下一時刻的先驗概率分布包括： 確定采樣容積點集，其中各容積點包含了機器人的運動信息，控制信息，和 地圖特征信息；其數(shù)學表達式為：其中，和#,由數(shù)學公式
t理而來，其中機器人狀態(tài)信息Sk_i和運動信息uk要增廣為 高斯噪聲變量，1表示為互相正交的完全對稱容積點集，表示為其中Ii存在2(ns+nu)列，
將采樣的容積點通過非線性模型傳遞，得到隨機變量的統(tǒng)計數(shù)據(jù)，進而得到下一時刻 關于每個容積點的機器人運動信息、控制信息和地圖特征信息的先驗估計； 通過容積變換，將每個容積點的先驗估計進行擬合，得到下一時刻運動控制信息的先 驗概率分布，并且對產(chǎn)生的協(xié)方差誤差矩陣做QR分解處理，得到平方根因子用于下一步傳 遞，其中容積變換公式為：
協(xié)方差誤差矩陣為：
QR分解為：[QR] =qr(A./)，0^4=RT。
5.根據(jù)權利要求4所述的方法，其特征在于，所述利用所述下一時刻的先驗概率分布 和所述觀測信息數(shù)據(jù)集更新后驗概率分布包括： 通過所述觀測模型建立新的容積點集，任意一個容積點迭代初始值分別為Sk^ 和，第1次迭代機器人位姿信息和平方根因子分別為.由數(shù)學公式： 1 、 5
到更新的容積點集； 根據(jù)所述下一時刻運動控制信息的先驗估計和根據(jù)所述觀測模型得到的觀測數(shù)據(jù)，得 到下一時刻機器人運動信息的后驗估計；經(jīng)由所述觀測方程，容積點傳遞后得到的容積點 觀測信息
擬合各容積點信息
QR分解新的協(xié)方差誤差矩罔
新的協(xié)方差矩陣由： 7
進一步求得卡爾曼增益
計算下一時刻平方根因子，用于遞增更新，迭代1 + 1次時機器人位姿信息和平方 根因子d
設置迭代終止條件，1 = 迭代最大次數(shù)，是預先設置的固定常量，當?shù)K止 時，各數(shù)據(jù)實現(xiàn)更新 7
~
6. -種機器人同步定位與地圖創(chuàng)建裝置，其特征在于，包括： 第一確定模塊，用于根據(jù)外部傳感器采集的外部環(huán)境信息，得到觀測信息數(shù)據(jù)集； 第二確定模塊，用于根據(jù)當前時刻的后驗概率分布和下一時刻的運動控制信息，得到 下一時刻的先驗概率分布； 第一更新模塊，用于根據(jù)所述下一時刻的先驗概率分布和所述觀測信息數(shù)據(jù)集更新后 驗概率分布； 第二更新模塊，用于根據(jù)更新后的后驗概率分布更新當前時刻地圖特征信息并更新當 前時刻機器人控制信息。
7. 根據(jù)權利要求6所述的裝置，其特征在于，還包括：第一建模模塊，用于建立機器人 運動模型； 建立所述機器人運動模型應用的機器人運動方程為：
其中，mk表示機器人位姿，T表示系統(tǒng)內(nèi)部采樣間隔，vk表示機器人行進速度，ak表示 轉(zhuǎn)角，L表示輪距，mk表示K時刻機器人的位姿，mk包括機器人K時刻X方向上的位移（mx，k)， K時刻Y方向上的位移（my，k)，K時刻角度位移（m0，k)，相應的參數(shù)如表示為機器人K-l 時刻X方向上的位移，ak為機器人轉(zhuǎn)角。
8. 根據(jù)權利要求6或7所述的裝置，其特征在于，還包括：第二建模模塊，用于建立觀 測模型； 建立所述觀測模型應用的機器人觀測方程為：
其中，lk表示機器人與觀測到地圖特征點的距離，0 k表示機器人與觀測到地圖特征點 的夾角，為機器人觀測到的地圖特征點i的位置，zk表示機器人相對于觀測到的 地圖特征點的位置。
9. 根據(jù)權利要求8所述的裝置，其特征在于，所述第二確定模塊具體用于： 確定采樣容積點集，其中各容積點包含了機器人的運動信息，控制信息，和地圖特征信 息；其數(shù)學表達式為：4 +仏,其中，&和私由數(shù)學公式
處理而來，其中機器人狀態(tài)信息Sk_i和運動信息uk要增廣為高斯噪聲變量，| ,表示為互相 正交的完全對稱容積點集，表示為其中L存在2(ns+nu)列，
將采樣的容積點通過非線性模型傳遞，得到隨機變量的統(tǒng)計數(shù)據(jù)，進而得到下一時刻 關于每個容積點的機器人運動信息、控制信息和地圖特征信息的先驗估計； 通過容積變換，將每個容積點的先驗估計進行擬合，得到下一時刻運動控制信息的先 驗概率分布，并且對產(chǎn)生的協(xié)方差誤差矩陣做QR分解處理，得到平方根因子用于下一步傳 遞，其中容積變換公式為：
協(xié)方差誤差矩陣為：
QR分解為：[QR] =qr(A./)，0^4=RT。
10. 根據(jù)權利要求9所述的裝置，其特征在于，所述第一更新模塊具體用于： 通過所述觀測模型建立新的容積點集，任意一個容積點迭代初始值分別為Sk^ 和，第1次迭代機器人位姿信息和平方根因子分別為.由數(shù)學公式：
i到更新的容積點集； 根據(jù)所述下一時刻運動控制信息的先驗估計和根據(jù)所述觀測模型得到的觀測數(shù)據(jù)，得 到下一時刻機器人運動信息的后驗估計；經(jīng)由所述觀測方程，容積點傳遞后得到的容積點 觀測信息
，合各容積點信息
QR分解新的協(xié)方差誤差矩P
新的協(xié)方差矩陣由：
進一步求得卡爾曼增益
9 計算下一時刻平方根因子，用于遞增更新，迭代1 + 1次時機器人位姿信息和平方 根因子d
設置迭代終止條件，1 = 迭代最大次數(shù)，是預先設置的固定常量，當?shù)K止 時，各數(shù)據(jù)實現(xiàn)更新
【專利摘要】本發(fā)明實施例公開一種機器人同步定位與地圖創(chuàng)建方法及裝置，屬于機器人同步定位與地圖創(chuàng)建領域。該方法包括：根據(jù)外部傳感器采集的外部環(huán)境信息，得到觀測信息數(shù)據(jù)集；根據(jù)當前時刻的后驗概率分布和下一時刻的運動控制信息，得到下一時刻的先驗概率分布；利用所述下一時刻的先驗概率分布和所述觀測信息數(shù)據(jù)集更新后驗概率分布；根據(jù)更新后的后驗概率分布更新當前時刻地圖特征信息并更新當前時刻機器人控制信息。本發(fā)明實施例通過容積變換，避免了直接線性化過程，減少了計算雅可比矩陣的計算量，提高了精度，減小了截斷誤差對于系統(tǒng)穩(wěn)定性的影響，改進了通過迭代測量更新，改進了容積點采樣規(guī)則，減小采樣點在高度非線性環(huán)境下的失真。
【IPC分類】G01C21-20
【公開號】CN104807465
【申請?zhí)枴緾N201510206216
【發(fā)明人】陳孟元, 陶明, 凌有鑄
【申請人】安徽工程大學
【公開日】2015年7月29日
【申請日】2015年4月27日