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

在審

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

本申請的目的是提供一種位姿優化方法及設備，本申請利用擴展卡爾曼融合算法和激光里程計方法分別進行機器人姿態估計、利用自適應加權和卡爾曼增益思想的修正算法，通過分析調整最優加權因子和局部動態加權因子，構建修正算法對機器人初步位姿進行修正，得到機器人準確位姿即最優位姿，減少了位姿累計誤差，提高了姿態估計準確度和魯棒性。

技術領域

本申請涉及計算機領域，尤其涉及一種位姿優化方法及設備。

背景技術

隨著移動機器人被要求在復雜多變的環境下執行越來越嚴峻的任務，機器人實現對自身位姿的精確估計就顯著尤為重要。目前對于在室內的未知條件下機器人位姿累計誤差較大，單一傳感器精度不夠的問題，大多數采取的方法是加入視覺相機和激光雷達等傳感器來進一步估計位姿。然而，視覺相機易受光照條件限制，不利于保證機器人位姿估計的精確度。雖然激光雷光的測量精度很高，并且可以在無光環境工作，但因激光數據是對外部環境的觀測，所以激光雷達構建的激光里程計極其容易受環境影響，故需要結合其他傳感器實現對機器人位姿精確估計。當使用不同傳感器在室內未知環境下定位時，僅使用單一的位姿估計方法會造成魯棒性低的問題。

因此，實現對機器人位姿進行修正，得到更精確的機器人位姿信息，減少位姿累計誤差，提高姿態估計魯棒性是本領域需要繼續研究的方向。

發明內容

本申請的一個目的是提供一種位姿優化方法及設備，以解決現有技術中環衛車如何減少位姿累計誤差，提高姿態估計精確度和魯棒性的問題。

根據本申請的一個方面，提供了一種位姿優化方法，包括：

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

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

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

進一步地，所述位姿優化方法還包括：

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

進一步地，上述位姿優化方法中，所述基于所述激光里程計位姿和所述初步位姿，計算整體最優加權因子和局部動態權重因子，調整所述機器人位姿累計誤差，得到最優位姿，包括：

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

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

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

進一步地，上述位姿優化方法中，所述獲取機器人當前狀態信息，并利用激光里程計方法得到激光里程計位姿，包括：

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

進一步地，上述位姿優化方法中，所述基于所述當前狀態信息，利用擴展卡爾曼濾波融合通過輪式里程計方法和姿態解算方法得到的位姿數據，得到初步位姿，包括：

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