[發明專利]基于陀螺儀和加速度計的運動角度估計方法和系統

有效

• 申請號: 202011141659.0
• 申請日: 2020-10-22
• 公開（公告）號: CN112304337B
• 公開（公告）日: 2022-09-16
• 發明（設計）人: 盧成林
• 申請（專利權）人: 上海立可芯半導體科技有限公司
• 主分類號: G01C25/00
• 分類號: G01C25/00
• 代理公司: 上海專利商標事務所有限公司 31100
• 代理人: 駱希聰

本申請提供了一種基于陀螺儀和加速度計的運動角度估計方法、系統和計算機可讀介質。該方法包括：獲取加速度計的當前時刻的加速度、陀螺儀的多組角速度均值、預設陀螺儀校正系數以及預設加速度計校正系數；獲取陀螺儀偏差和加速度計偏差；根據加速度計偏差對當前時刻的加速度進行校準；使用預設陀螺儀校正系數和多組角速度均值對陀螺儀偏差進行IIR濾波；獲取濾波后的上一時刻的加速度；使用預設加速度計校正系數和濾波后的上一時刻的加速度對校準后的當前時刻的加速度進行IIR濾波；以及根據濾波后的陀螺儀偏差和濾波后的當前時刻的加速度估計最終運動角度。該方法通過改進的IIR濾波器修正陀螺儀偏差和加速度，能夠實現對運動角度的精準估計。

技術領域

本申請主要涉及數據處理領域，尤其涉及一種基于陀螺儀和加速度計的運動角度估計方法、系統和計算機可讀介質。

背景技術

隨著智能手機和智能家居的普及以及智能化程度地不斷完善，人們可以在經濟承受范圍內購選適合智能手機和掃地機器人等智能家居。以掃地機器人為例，在其工作過程中，定位方式至關重要，決定了掃地機器人能否把地面打掃干凈。掃地機器人定位以及環境地圖的構建是路徑規劃的核心部分，而掃地機器人的運動角度估計是定位的關鍵。

掃地機器人運動角度估計方法可以分為三類：基于光電編碼器的方法、基于電子羅盤的方法、基于陀螺儀的方法。基于光電編碼器的方法是利用光電轉換原理將輸入到軸的角度量轉換成相應的電脈沖或數字量的角度檢測裝置，但需要依賴機械裝置轉換、難以克服滑差，且會受到輪子打滑等不確定因素造成的角度測量不準?；陔娮恿_盤的方法是利用地球磁場檢測電子羅盤與地磁場方向的偏轉角度，因此容易受電磁干擾的影響?；谕勇輧x的方法是通過測量掃地機器人的角速度來獲取其運動角度，能夠解決前兩種方法所存在的問題。并且陀螺儀傳感器具有造價低廉、不受環境因素的影響和實時性較好等特點，應用比較廣泛。

但在實際情況中，大部分廉價陀螺儀傳感器都存在著較大的偏差，使得最終估計出的角度值與實際值有較大誤差。為了解決該誤差問題，需要用戶在使用掃地機器人之前或掃地機器人開機時進行陀螺儀的校準，這大大降低了用戶的使用體驗。而且在長時間使用后，陀螺儀仍會出現累計偏差，依然會對掃地機器人造成運動角度估計結果不理想等問題?，F有的運動姿態估計算法大都采用基于卡爾曼濾波或者互補濾波的算法，但是卡爾曼濾波的計算量較大，對硬件要求比較高，實現起來也相對比較復雜。

一般來說，陀螺儀和加速度計的誤差分為系統誤差和隨機誤差。系統誤差通?？梢酝ㄟ^傳感器的校準消除；而陀螺儀的隨機誤差(又稱零偏)通常是在一定范圍內隨機飄移，因此需要對陀螺儀的零偏進行糾正。加速度計比較靈敏，但是其抖動和噪聲會產生累積誤差，因此需要對校準后的加速度計數據進行糾正。

因此，本領域技術人員亟需一種基于陀螺儀和加速度計的運動角度估計方法能夠更好地修正陀螺儀偏差和加速度計數據，來實現更精準地運動角度估計。

發明內容

本申請要解決的技術問題是提供基于陀螺儀和加速度計的運動角度估計方法、系統和計算機可讀介質，能夠更好地修正陀螺儀偏差和加速度計數據，實現更精準地運動角度估計。

為解決上述技術問題，本申請提供了一種基于陀螺儀和加速度計的運動角度估計方法，包括：獲取加速度計的當前時刻的加速度、陀螺儀的多組角速度均值、預設陀螺儀校正系數以及預設加速度計校正系數；在所述陀螺儀和所述加速度計保持水平放置且靜止的情況下，獲取陀螺儀偏差和加速度計偏差；根據所述加速度計偏差對所述當前時刻的加速度進行校準，并得到校準后的當前時刻的加速度；使用所述預設陀螺儀校正系數和所述多組角速度均值對所述陀螺儀偏差進行IIR濾波，并得到濾波后的陀螺儀偏差；獲取濾波后的上一時刻的加速度，若所述濾波后的上一時刻的加速度不存在，則將所述校準后的當前時刻的加速度作為所述濾波后的上一時刻的加速度；使用所述預設加速度計校正系數和所述濾波后的上一時刻的加速度對所述校準后的當前時刻的加速度進行IIR濾波，并得到濾波后的當前時刻的加速度；以及根據所述濾波后的陀螺儀偏差和所述濾波后的當前時刻的加速度估計最終運動角度。