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

权利要求书

1.一种姿态估计方法，其特征在于，包括：

计算当前时刻的加速度测量值和加速度估计值之间的误差；

根据计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量；

获取当前时刻的角速度测量值，根据计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；

根据计算得到的角速度校正值和当前时刻的姿态四元数估计值，计算得到下一时刻的姿态四元数估计值。

2.根据权利要求1所述的姿态估计方法，其特征在于，所述计算当前时刻的加速度测量值和加速度估计值之间的误差的步骤，包括：

获取当前时刻T的加速度测量值a_c(x_c,y_c,z_c)；

获取当前时刻T的姿态四元数估计值Q_T(q₀,q₁,q₂,q₃)，根据如下公式(1)计算当前时刻T的加速度估计值a_g：

根据如下公式(2)计算得到当前时刻T的加速度测量值a_c和加速度估计值a_g之间的误差E_T：

3.根据权利要求2所述的姿态估计方法，其特征在于，所述根据计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量的步骤，包括：根据如下公式(3)计算当前时刻T的角速度控制量μ：

其中，表示从初始时刻0至当前时刻T的加速度测量值a_c和加速度估计值a_g之间的误差的积分；E_T-1表示上一时刻T-1的加速度测量值a_c和加速度估计值a_g之间的误差；K_p表示比例控制系数；K_i表示积分控制系数；K_d表示微分控制系数。

4.根据权利要求3所述的姿态估计方法，其特征在于，所述计算得到当前时刻T的角速度校正值的步骤，包括：根据如下公式(4)计算得到当前时刻T的角速度校正值ω(ω_x,ω_y,ω_z)：

ω＝ω_c+μ (4)；

其中，ω_c表示当前时刻T的角速度测量值。

5.根据权利要求4所述的姿态估计方法，其特征在于，所述计算得到下一时刻的姿态四元数估计值的步骤，包括：根据如下公式(5)计算下一时刻T+1的姿态四元数估计值Q_T+1(q₀,q₁,q₂,q₃)：

其中，ΔT表示角速度测量值的采样时间间隔。

6.一种姿态估算装置，其特征在于，所述姿态估算装置包括：

误差计算器，配置为计算当前时刻的加速度测量值和加速度估计值之间的误差；

与所述误差计算器相连的比例-积分-微分控制器，配置为根据所述误差计算器计算得到的误差，通过比例-积分-微分控制算法计算当前时刻的角速度控制量；

与所述比例-积分-微分控制器相连的角速度校正器，配置为获取当前时刻的角速度测量值，并根据所述比例-积分-微分控制器计算得到的角速度控制量对当前时刻的角速度测量值进行校正，计算得到当前时刻的角速度校正值；

与所述角速度校正器相连的姿态估计器，配置为根据当前时刻的姿态四元数估计值和所述角速度校正器计算得到的角速度校正值，计算得到下一时刻的姿态四元数估计值。

7.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有一个或多个指令，以执行如权利要求1～5中任一项所述的姿态估计方法中的一个或多个步骤。