[發明專利]一種位姿優化方法及設備

在審

• 申請號: 202110755390.3
• 申請日: 2021-07-05
• 公開（公告）號: CN113483762A
• 公開（公告）日: 2021-10-08
• 發明（設計）人: 劉群坡;岳勝杰;王紅旗;張建軍;吳中華
• 申請（專利權）人: 河南理工大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C22/00
• 代理公司: 焦作市科彤知識產權代理事務所(普通合伙) 41133
• 代理人: 楊曉彤

1.一種位姿優化方法，其特征在于，所述方法包括：

獲取機器人當前狀態信息，并利用激光里程計方法得到激光里程計位姿；

基于所述當前狀態信息，利用擴展卡爾曼濾波融合通過輪式里程計方法和姿態解算方法得到的位姿數據，得到初步位姿；

計算所述激光里程計位姿和所述初步位姿的位姿偏差值，若所述位姿偏差值大于位姿偏差閾值，則基于所述激光里程計位姿和所述初步位姿，計算整體最優加權因子和局部動態權重因子，調整所述機器人位姿累計誤差，得到最優位姿。

2.根據權利要求1所述的方法，其特征在于，所述位姿優化方法還包括：

若所述偏差值小于等于所述位姿偏差閾值，則計算所述激光里程計位姿和所述初步位姿的位姿平均值作為所述最優位姿。

3.根據權利要求2所述的方法，其特征在于，所述基于所述激光里程計位姿和所述初步位姿，計算整體最優加權因子和局部動態權重因子，調整所述機器人位姿累計誤差，得到最優位姿，包括：

分別計算所述激光里程計位姿和所述初步位姿的方差，并結合多元函數極值方法得到激光里程計位姿整體最優加權因子和初步位姿整體最優加權因子；

分別計算所述激光里程計位姿和所述初步位姿與上一時刻修正后位姿的偏差信息，基于所述偏差信息通過平方差計算方法得到激光里程計位姿局部動態權重因子和初步位姿局部動態權重因子；

將所述激光里程計位姿整體最優加權因子、所述初步位姿整體最優加權因子、所述激光里程計位姿局部動態權重因子、所述初步位姿局部動態權重因子進行融合得到修正權重因子，通過所述修正權重因子進行當前時刻的位姿矯正得到最優位姿。

4.根據權利要求1所述的方法，其特征在于，所述獲取機器人當前狀態信息，并利用激光里程計方法得到激光里程計位姿，包括：

選擇PL-ICP的點云匹配的方法構建激光里程計，并計算得到所述激光里程計位姿。

5.根據權利要求1所述的方法，其特征在于，所述基于所述當前狀態信息，利用擴展卡爾曼濾波融合通過輪式里程計方法和姿態解算方法得到的位姿數據，得到初步位姿，包括：

慣性傳感器測量得到機器人角速度和加速度后，通過Mahony姿態結算算法計算得到姿態結算位姿數據；

通過輪式里程計方法得到輪式里程計位姿數據；

利用所述擴展卡爾曼濾波將所述姿態結算位姿數據和所述輪式里程計位姿數據進行融合得到所述初步位姿。

6.一種計算機可讀介質，其上存儲有計算機可讀指令，所述計算機可讀指令可被處理器執行時，使所述處理器實現如權利要求1至5中任一項所述的方法。

7.一種位姿優化設備，其特征在于，該設備包括：

一個或多個處理器；

計算機可讀介質，用于存儲一個或多個計算機可讀指令，

當所述一個或多個計算機可讀指令被所述一個或多個處理器執行，使得所述一個或多個處理器實現如權利要求1至5中任一項所述的方法。