[發明專利]一種3D點云地圖更新方法和系統在審
| 申請號： | 202011641143.2 | 申請日： | 2020-12-31 |
| 公開（公告）號： | CN112762923A | 公開（公告）日： | 2021-05-07 |
| 發明（設計）人： | 褚衍超;章海兵;汪中原;左桐舟;劉子明 | 申請（專利權）人： | 合肥科大智能機器人技術有限公司;科大智能科技股份有限公司;科大智能電氣技術有限公司 |
| 主分類號： | G01C21/00 | 分類號： | G01C21/00;G01C21/20;G06T7/521 |
| 代理公司： | 合肥天明專利事務所(普通合伙) 34115 | 代理人： | 苗娟 |
| 地址： | 230088 安*** | 國省代碼： | 安徽;34 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 一種 地圖 更新 方法 系統 | ||
1.一種3D點云地圖更新方法，其特征在于：包括以下步驟：
構建局部地圖，獲取機器人粗位姿，然后對機器人位姿進行優化更新；
其中，優化更新步驟包括由粗到細的匹配算法，如下：
Step1.對Qk采用K-D樹描述，對中的每個激光點尋找在Qk中的最近點，建立配對；
Step2.建立非線性方程，目標使得所有配對的距離最小；
Step3.利用L-M法求解非線性優化問題，求解Topt＝(R,t)；
Step4.則k+1時刻的機器人位姿為
其中Qk表示k時刻建立的點云地圖，表示k+1時刻當前幀轉換到世界坐標系下，表示k時刻機器人在世界坐標系下的位姿，表示k+1時刻機器人的位姿增量也即上步粗匹配過程中輸出量。
2.根據權利要求1所述的3D點云地圖更新方法，其特征在于：優化更新步驟還包括增量式優化算法，如下：
Step21.記t時刻的機器人位姿為xt，傳感器觀測為zt，控制量為ut，x1:t＝{x1,…,xt}為機器人從起始到t時刻的行駛軌跡，則SLAM問題等價于估計后驗概率分布P(x1:t,m|z1:t,u0:t-1)，其中m為環境地圖；
由式1可知，地圖m依賴于機器人運動軌跡及其對應的傳感器觀測；
Step22.由上可知SLAM可簡化為機器人軌跡估計問題：
Step23.基于位姿圖的SLAM方法并不直接求取P(x1:t,m|z1:t,u0:t-1)，而僅求取其極大似然：
Step24.假定運動學模型及觀測模型噪聲若滿足高斯分布，則運動學模型及觀測模型均可轉換為如下形式：
其中，運算表示坐標變換，Tij和Σij分別為xi與xj之間的相對關系和協方差，可由上述匹配算法獲得；在二維情形下，x＝(x,y,θ)T，則T可表示為(xT,yT,θT)T，則運算的具體形式可以表示為：
于是，式(3)可寫成：
Step25.令其為非線性函數，將其線性化，即有：
式中，是fj(xi)的雅可比矩陣；
Step26.令對式(6)進行簡化有：
記并寫成一般形式有：
其中，Q(R 0)T是A的QR分解。
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于合肥科大智能機器人技術有限公司;科大智能科技股份有限公司;科大智能電氣技術有限公司，未經合肥科大智能機器人技術有限公司;科大智能科技股份有限公司;科大智能電氣技術有限公司許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.17sss.com.cn/pat/books/202011641143.2/1.html，轉載請聲明來源鉆瓜專利網。
