[發明專利]一種基于激光與慣導融合的足式機器人本體狀態估計方法

在審

• 申請號: 201911424943.6
• 申請日: 2019-12-30
• 公開（公告）號: CN110954100A
• 公開（公告）日: 2020-04-03
• 發明（設計）人: 蘇澤榮;周雪峰;唐觀榮;吳鴻敏;鄢武
• 申請（專利權）人: 廣東省智能制造研究所
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/20
• 代理公司: 佛山市廣盈專利商標事務所(普通合伙) 44339
• 代理人: 李俊

本發明公開了一種基于激光與慣導融合的足式機器人本體狀態估計方法，所述方法包括：采集足式機器人本體的IMU數據以及激光數據；對所述IMU數據與所述激光數據進行時間戳同步與預處理；在所述時間戳同步與預處理之后，對所述IMU數據進行積分，得到積分數據，同時對所述激光數據進行激光掃描和幀間點線匹配，得到增量運動變化；將所述積分數據作為狀態預測輸入至遞歸貝葉斯濾波器，且將所述得到的增量運動變化作為狀態校正與更新輸入至所述遞歸貝葉斯濾波器；所述遞歸貝葉斯濾波器通過對比所述狀態預測所輸入的數據與所述狀態校正與更新所輸入的數據來進行誤差測量，得到所述足式機器人本體運動狀態。在本發明實施中，提高狀態估計的可靠性。

技術領域

本發明涉及一種足式機器人本地運動狀態的技術領域，尤其涉及一種基于激光與慣導融合的足式機器人本體狀態估計方法。

背景技術

移動機器人需要對其狀態進行良好的估計，才能在結構化和非結構化環境中進行閉環控制。許多現有的算法都依賴來自多個傳感器的數據融合以計算這些估計值。該技術領域常用本體感應傳感器的測量信息，如加速度、機器人內部的電機速度、關節角度，以及來自外部特征的測量信息，如光強度、距離測量、聲音振幅等進行融合，然而這些數據來源并不總是可靠的。輪式機器人通常使用IMU以及電機編碼器在與地面保持平穩可靠接觸下能較好的進行里程和狀態估計，而腿式機器人通過間歇性的腳底接觸與環境接觸交互，對IMU和編碼器測量中引入了額外的噪聲，足式機器人的里程狀態通常采用卡爾曼濾波器，需要仔細調參，單靠IMU和關節傳感器進行位置常發生飄逸。而傳統足式機器人基于本體IMU容易受到噪聲的影響，同時純靠激光傳感器信息容易受遮擋出現丟幀，無法匹配進行估計的現象。

發明內容

本發明的目的在于克服現有技術的不足，本發明提供了一種基于激光與慣導融合的足式機器人本體狀態估計方法，融合內部感應傳感器和外部測量傳感器信息，避免依賴單一數據來源，采用點線匹配算法的二次收斂特性加速算法運行，利用擴展卡爾曼濾波對激光里程計和IMU里程計數據進行融合，最終得到本機器人本體狀態的可靠估計。

為了解決上述技術問題，本發明實施例提供了一種基于激光與慣導融合的足式機器人本體狀態估計方法，其特征在于，所述方法包括：

采集足式機器人本體的IMU數據以及激光數據；

對所述IMU數據與所述激光數據進行時間戳同步與預處理；

在所述時間戳同步與預處理之后，對所述IMU數據進行積分，得到積分數據，同時對所述激光數據進行幀間點線匹配，得到增量運動變化；

將所述積分數據作為狀態預測輸入至遞歸貝葉斯濾波器，且將所述得到的增量運動變化作為狀態校正與更新輸入至所述遞歸貝葉斯濾波器；

所述遞歸貝葉斯濾波器通過對比所述狀態預測所輸入的數據與所述狀態校正與更新所輸入的數據來進行誤差測量，得到所述足式機器人本體運動狀態。

可選的，其特征在于，所述采集足式機器人本體的IMU數據以及激光數據包括：

通過IMU傳感器中的加速度計采集足式機器人本體的線性加速度；

通過IMU傳感器中的陀螺儀采集足式機器人本體的角速度；

通過激光雷達采集足式機器人本體的激光數據。

可選的，所述對所述IMU數據與所述激光數據進行時間戳同步與預處理包括：

對所述IMU數據與所述激光數據進行時間戳同步，得到同步的IMU數據和激光數據；

基于所述IMU數據和所述激光數據，對所述IMU數據與所述激光數據進行預處理，其中，所述預處理包括將所述IMU數據和所述激光數據進行去噪，以及將所述IMU數據和所述激光數據進行坐標系轉化。

可選的，所述將所述IMU數據進行坐標系轉化包括：