[發明專利]一種用于室內外場景的人為回環即時定位和建圖方法及系統在審
| 申請號: | 202011411092.4 | 申請日: | 2020-12-03 |
| 公開(公告)號: | CN112697158A | 公開(公告)日: | 2021-04-23 |
| 發明(設計)人: | 朱文俊;謝亮亮;易陽;馮李航 | 申請(專利權)人: | 南京工業大學 |
| 主分類號: | G01C21/32 | 分類號: | G01C21/32 |
| 代理公司: | 南京燦爛知識產權代理有限公司 32356 | 代理人: | 吳亞 |
| 地址: | 211816 *** | 國省代碼: | 江蘇;32 |
| 權利要求書: | 查看更多 | 說明書: | 查看更多 |
| 摘要: | |||
| 搜索關鍵詞: | 一種 用于 內外 場景 人為 回環 即時 定位 方法 系統 | ||
1.一種用于室內外場景的人為回環即時定位和建圖方法,其特征在于,包括以下步驟:
步驟一,定義位姿關系SLAM初始因子圖;
步驟二,采集機器人定位數據,對第i幀定位數據輸入校正因子,通過EM算法估算校正因子;
步驟三,通過矩陣變換串聯機器人前后變換的位姿,優化校正因子;
步驟四,通過校正因子約束優化機器人位姿,消除累計誤差,得到全局一致性地圖。
2.根據權利要求1所述的一種用于室內外場景的人為回環即時定位和建圖方法,其特征在于:步驟一具體包括以下步驟:
定義機器人在地圖中的初始位姿及其約束關系的初始因子圖G0={X,F};其中X為機器人初始行駛軌跡上的位姿估計集合,F={R},R表示測量值和觀測值之間的相對位姿。
3.根據權利要求1所述的一種用于室內外場景的人為回環即時定位和建圖方法,其特征在于:步驟二具體包括以下步驟:
(201),采集機器人定位數據,在機器人定位數據的第i幀輸入用于修正地圖的校正因子;
(202),實例化一組校正因子,將校正因子添加到初始因子圖中,得到矯正后因子圖G1={X,F∪H},H表示校正因子,H=Pa,Pb,Sa,Sb,Xa,Xb,m,Pa,Pb表示繪制的兩條線段a,b的端點集,是實數集,表示的是正實數;Sa,Sb表示由兩個線段a,b分別選擇的觀測值集,S是觀測值;表示從X1:t中得到的一組位姿Xa和Xb,Xa指線段a所選擇的機器人位姿,Xb是線段b選擇的機器人位姿,X1:t指的是機器人從1到t時刻的位姿集合;m∈M,m表示當前工作的校正模式,M是校正模式的集合,校正模式包括共點,共線,垂直和平行。
4.根據權利要求3所述的一種用于室內外場景的人為回環即時定位和建圖方法,其特征在于:估算校正因子H具體包括以下幾個步驟:
步驟2001,輸入線段a,b的端點集Pa,Pb并選擇校正模式;
步驟2002,給定初始輸入線段端點和校正模式m,是線段a所選擇的初始輸入端點,是線段b所選擇的初始輸入端點,基于EM算法對輸入端點θ={Pa,Pb}做極大似然估計,確定輸入端點θ={Pa,Pb},計算出校正因子H。
5.根據權利要求4所述的一種用于室內外場景的人為回環即時定位和建圖方法,其特征在于:
基于EM算法對輸入端點θ={Pa,Pb}做極大似然估計具體包括以下步驟:
將對數似然函數l最大化,
其中θ={Pa,Pb}是輸入有效線段的端點,θold是使得當前似然期望值達到最大化的參數值θ的前一次迭代值;si∈S是觀測值,si表示第i幀定位數據的觀測值,變量zi為觀測值si的隱變量;p(zi|si,θold)和p(zi,si|θ)的表達式基于正態分布N(μ(θ),σ2)的誤差生成模型,σ是指定點時準確度的標準偏差;μ(θ)是特征的中心。
該專利技術資料僅供研究查看技術是否侵權等信息,商用須獲得專利權人授權。該專利全部權利屬于南京工業大學,未經南京工業大學許可,擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作,請聯系【客服】
本文鏈接:http://www.szxzyx.cn/pat/books/202011411092.4/1.html,轉載請聲明來源鉆瓜專利網。





