[發明專利]一種移動機器人的姿態估計方法及終端設備有效
| 申請號： | 201910985090.7 | 申請日： | 2019-10-16 |
| 公開（公告）號： | CN110645976B | 公開（公告）日： | 2021-09-14 |
| 發明（設計）人： | 石鵬;林輝;盧維;殷俊 | 申請（專利權）人： | 浙江華睿科技股份有限公司 |
| 主分類號： | G01C21/00 | 分類號： | G01C21/00;G01C21/16 |
| 代理公司： | 深圳市威世博知識產權代理事務所(普通合伙) 44280 | 代理人： | 唐雙 |
| 地址： | 310051 浙江省杭*** | 國省代碼： | 浙江;33 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 一種 移動 機器人 姿態 估計 方法 終端設備 | ||
1.一種移動機器人的姿態估計方法，所述移動機器人包括車體，其中所述車體上固定安裝有IMU和里程計，其特征在于，包括：
對所述IMU所采集的IMU數據進行互補濾波，以估計所述移動機器人的運動姿態，其中所述運動姿態包括偏航角、俯仰角和橫滾角；以及
利用所述里程計的更新周期和所述里程計所采集的里程計數據校正所述運動姿態中的偏航角，并將校正后的偏航角更新為所述運動姿態中的偏航角；
其中，所述里程計所采集的里程計數據包括當前時刻t(k)的里程計角速度w′bz(k)和更新時刻t(k-m)的里程計角速度w′bz(k-m)；
所述利用所述里程計的更新周期和所述里程計所采集的里程計數據校正所述運動姿態中的偏航角包括：
獲取通過互補濾波所估計的所述當前時刻t(k)的偏航角ψk和所述更新時刻t(k-m)的偏航角ψk-m，并根據公式1，計算所述更新時刻t(k-m)至所述當前時刻t(k)的第一偏航角增量
其中，所述更新周期為mΔT；
根據所述當前時刻t(k)的里程計角速度w′bz(k)和所述更新時刻t(k-m)的里程計角速度w′bz(k-m)，計算所述更新時刻t(k-m)至所述當前時刻t(k)的第二偏航角增量
確定所述第二偏航角增量的加權值α，其中0≤α≤1；
根據公式2，計算最終偏航角增量
根據公式3，計算所述當前時刻t(k)的校正后的偏航角ψ’k，
其中，對所述當前時刻t(k)的里程計角速度w′bz(k)在導航坐標系Z軸的分量w′nz(k)和所述更新時刻t(k-m)的里程計角速度w′bz(k-m)在導航坐標系Z軸的分量w′nz(k-m)進行加權平均計算，進而得到角速度
基于所述角速度計算所述更新時刻t(k-m)至所述當前時刻t(k)的第二偏航角增量
2.如權利要求1中所述的方法，其特征在于，
所述對所述當前時刻t(k)的里程計角速度w′bz(k)在導航坐標系Z軸的分量w′nz(k)和所述更新時刻t(k-m)的里程計角速度w′bz(k-m)在導航坐標系Z軸的分量w′nz(k-m)進行加權平均計算，進而得到角速度包括：
根據公式4，分別計算所述當前時刻t(k)的里程計角速度w′bz(k)在導航坐標系Z軸的分量w′nz(k)和所述更新時刻t(k-m)的里程計角速度w′bz(k-m)在導航坐標系Z軸的分量w′nz(k-m)，
其中，分別為時刻t(ki)時所述移動機器人的俯仰角和橫滾角；
根據公式5，計算角速度
其中，λ0為根據所述里程計的更新頻率而變化的參數，且0≤λ0≤1；
所述基于所述角速度計算所述更新時刻t(k-m)至所述當前時刻t(k)的第二偏航角增量包括：
根據公式6，計算所述第二偏航角增量
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于浙江華睿科技股份有限公司，未經浙江華睿科技股份有限公司許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/201910985090.7/1.html，轉載請聲明來源鉆瓜專利網。
