[发明专利]一种移动机器人的姿态估计方法及终端设备

权利要求书

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(k_i)时所述移动机器人的俯仰角和横滚角；

根据公式5，计算角速度

其中，λ₀为根据所述里程计的更新频率而变化的参数，且0≤λ₀≤1；

所述基于所述角速度计算所述更新时刻t(k-m)至所述当前时刻t(k)的第二偏航角增量包括：

根据公式6，计算所述第二偏航角增量