[發明專利]機器人狀態檢測方法、裝置及設備

在審

• 申請號: 202011042717.4
• 申請日: 2020-09-28
• 公開（公告）號: CN112269187A
• 公開（公告）日: 2021-01-26
• 發明（設計）人: 林燕龍;張偉
• 申請（專利權）人: 廣州視源電子科技股份有限公司;廣州視睿電子科技有限公司
• 主分類號: G01S17/86
• 分類號: G01S17/86;G01S17/89;G01S17/06;G01C21/16;G01P3/36
• 代理公司: 廣州駿思知識產權代理有限公司 44425
• 代理人: 潘桂生

本發明涉及一種機器人狀態檢測方法、裝置及設備，所述機器人狀態檢測方法包括：將當前機器人狀態節點加入預先創建的機器人狀態檢測窗口；獲取機器人狀態檢測窗口內除當前機器人狀態節點外的其余機器人狀態節點的初始值以及機器人狀態檢測窗口內每兩個相鄰機器人狀態節點之間的激光相對約束、慣性預積分約束和輪計預積分約束；根據當前機器人狀態節點的預測值、其余機器人狀態節點的初始值以及預設的非線性優化算法，對激光相對約束、慣性預積分約束和輪計預積分約束進行非線性優化，獲取所述機器人狀態檢測窗口內所有機器人狀態節點的優化值。相對于現有技術，本申請降低了機器人狀態檢測的誤差，提高了機器人位姿估計的準確性。

技術領域

本申請實施例涉及機器人領域，尤其涉及一種機器人狀態檢測方法、裝置及設備。

背景技術

目前，機器人已經逐步應用于各行各業，例如安防巡邏、抗震救災、農業植保、航空測繪、制造業、倉儲物流等。在機器人工作的過程中，需要對機器人的狀態進行檢測，以保持機器人控制的穩定性，提高其作業的安全性。

通常對機器人狀態進行檢測的方法，是先根據機器人的上一狀態以及傳感器采集的數據各自預測機器人的當前狀態，再把各個預測到的機器人當前狀態組合成一個優化問題進行求解，利用上述方法檢測到的機器人狀態誤差積累較快，難以對機器人位姿進行準確預估。

發明內容

本申請實施例提供了一種機器人狀態檢測方法、裝置及設備，可以解決機器人狀態檢測結果的準確性較低的問題，所述技術方案如下:

第一方面，本申請實施例提供了一種機器人狀態檢測方法，包括：

接收當前激光測量時刻采集的機器人激光點云，獲得當前激光測量時刻對應的當前機器人狀態節點的預測值，將所述當前機器人狀態節點加入預先創建的機器人狀態檢測窗口；其中，所述機器人狀態檢測窗口依次記錄有多個激光測量時刻對應的機器人狀態節點，且所述機器人狀態檢測窗口每加入一個新的機器人狀態節點時，所述機器人狀態檢測窗口中最早加入的一個機器人狀態節點被刪除；

獲取所述機器人狀態檢測窗口內除所述當前機器人狀態節點外的其余機器人狀態節點的初始值以及所述機器人狀態檢測窗口內每兩個相鄰機器人狀態節點之間的激光相對約束、慣性預積分約束和輪計預積分約束；其中，所述其余機器人狀態節點的初始值為未加入當前機器人狀態節點前所述機器人狀態檢測窗口內機器人狀態節點的優化值；

根據所述當前機器人狀態節點的預測值、所述其余機器人狀態節點的初始值以及預設的非線性優化算法，對所述激光相對約束、所述慣性預積分約束以及所述輪計預積分約束進行非線性優化，使所述機器人狀態檢測窗口內每兩個相鄰所述機器人狀態節點之間的激光相對約束、慣性預積分約束和輪計預積分約束之和最小，獲取所述機器人狀態檢測窗口內所有機器人狀態節點的優化值。

可選的，所述獲取所述機器人狀態檢測窗口內所有機器人狀態節點的初始狀態，包括步驟：

獲取所述當前激光測量時刻與上一激光測量時刻之間的慣性數據；

根據所述慣性數據，獲取所述當前機器人狀態節點的預測值。

可選的，獲取所述機器人狀態檢測窗口中每兩個相鄰機器人狀態節點之間的激光相對約束，包括步驟：

獲取所述機器人狀態檢測窗口內每個所述機器人狀態節點對應的激光點云；

獲取激光地圖；其中，所述激光地圖由所有已刪除的機器人狀態節點對應的激光點云拼接而成；

將所述激光地圖和所述激光點云變換到目標坐標系下，計算每個所述激光點云與所述激光地圖之間的激光點線約束和激光點面約束；其中，所述目標坐標系為所述激光點云對應的機器人狀態節點的前一相鄰機器人狀態節點所對應的坐標系；

根據所述激光點線約束和激光點面約束，得到每兩個相鄰所述機器人狀態節點之間的激光相對約束。