[发明专利]姿态估计方法、装置及计算机可读存储介质

说明书

本发明提供了一种姿态估计方法、装置及计算机可读存储介质，涉及姿态估计领域，用于解决姿态估计的准确度较低的问题。其中，该姿态估计方法包括：计算当前时刻的加速度测量值和加速度估计值之间的误差；根据计算得到的误差，通过比例‑积分‑微分控制算法计算当前时刻的角速度控制量；获取当前时刻的角速度测量值，根据计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；根据计算得到的角速度校正值和当前时刻的姿态四元数估计值，计算得到下一时刻的姿态四元数估计值。所述姿态估计方法用于姿态估计。

技术领域

本发明涉及姿态估计技术领域，尤其涉及一种姿态估计方法、装置及计算机可读存储介质。

背景技术

很多应用场景中，例如：多旋翼无人机的飞行中，卫星的发射中，导弹的导航中，以及近几年兴起的VR(Virtual Reality，虚拟现实)技术中，均需要对某一目标三维物体的姿态进行估计。在目标三维物体的姿态估计中，通常根据加速度计、磁力计和陀螺仪等传感器所获取的相应测量数据，进行目标三维物体的姿态估计计算。

在现有技术中，常用的姿态估计方法有扩展卡尔曼滤波法、互补滤波法、梯度下降法等，然而上述现有姿态估计方法由于种种原因，在姿态估计时的准确度较低。例如，扩展卡尔曼滤波法和梯度下降法在进行姿态估计时计算量较大，造成姿态估计延迟、姿态估计的频率较低，导致姿态估计的准确度下降；互补滤波法的估计精度较低，导致姿态估计的准确度较低。

发明内容

针对上述现有技术中的问题，本发明的实施例提供一种姿态估计方法、装置及计算机可读存储介质，以解决上述现有技术中姿态估计的准确度较低的问题。

为达到上述目的，本发明的实施例采用如下技术方案：

第一方面，本发明的实施例提供了一种姿态估计方法，包括计算当前时刻的加速度测量值和加速度估计值之间的误差；根据计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量；获取当前时刻的角速度测量值，根据计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；根据计算得到的角速度校正值和当前时刻的姿态四元数估计值，计算得到下一时刻的姿态四元数估计值。

在上述姿态估计方法中，在对角速度测量值的校正过程中，采用了比例-积分-微分控制计算输出角速度控制量，在比例控制使加速度测量值和加速度估计值之间的误差快速缩小的基础上，积分控制能够消除控制所过程中所产生的加速度测量值和加速度估计值之间的稳态误差，提高了所输出的角速度控制量的准确度；微分控制能够预测控制过程中的加速度测量值和加速度估计值之间的误差的变化趋势，提前抑制超调的形成，使控制过程趋向稳定状态，提高了所输出的角速度控制量的稳定性。因此，根据上述角速度控制量对角速度测量值进行校正后，所得到的角速度校正值的准确度和稳定性较高。从而根据该角速度校正值进行积分运算，所得到的下一时刻的姿态四元数估计值的准确度和稳定性均较高，即提高了姿态估计的准确度和稳定性。

此外，由于通过上述姿态估计方法获取姿态四元数估计值时无需经过大量的计算，因此其进行姿态估计的速度较快，从而提高了姿态估计的频率、减少了姿态估计的延迟，进一步提高了姿态估计的准确度。

基于上述技术方案，可选的，所述计算当前时刻的加速度测量值和加速度估计值之间的误差的步骤，包括：获取当前时刻T的加速度测量值a_c(x_c,y_c,z_c)；获取当前时刻T的姿态四元数估计值Q_T(q₀,q₁,q₂,q₃)，根据如下公式(1)计算当前时刻T的加速度估计值a_g：