[发明专利]姿态测量方法及终端设备

权利要求书

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

在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量；

利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测；

根据计算的姿态角四元数构建卡尔曼滤波的量测系数矩阵，利用计算的姿态角四元数确定载体的横滚角、俯仰角和航向角，并计算卡尔曼滤波的量测信息，根据加速度计的输出计算加速度计输出的方差，并根据所述方差调整卡尔曼滤波的量测噪声矩阵；

根据加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，根据计算得到的四元数构建系数矩阵，根据构建的系数矩阵计算增益矩阵，根据计算的增益矩阵和航向角，更新卡尔曼滤波状态向量；

若计算的加速度计输出的方差小于预设阈值，则判断载体处于静止状态，根据更新的卡尔曼滤波状态向量对陀螺零偏进行更新，利用更新后的零偏信息进行反馈补偿；

其中，所述在载体静止状态下，根据静态场景下加速度计的输出确定载体的横滚角和俯仰角，根据磁强计的输出或者外部航向信息确定载体的航向角，根据确定的横滚角、俯仰角和航向角计算载体的姿态角四元数，并利用计算的姿态角四元数初始化卡尔曼滤波状态向量包括：

根据表达式确定载体的横滚角φ，根据表达式确定载体的俯仰角θ，其中，表示静态场景下加速度计x、y、z轴输出的平均值；

根据表达式确定载体的航向角ψ，其中，表示磁强计经过校正后的x、y、z轴的均值，γ_m表示初始对准当地的磁偏角，表示经过倾斜补偿后磁强计x、y、z轴的输出，m^w表示磁强计输出经过倾斜补偿后的输出向量；

根据表达式确定载体的姿态角四元数向量其中，其中q₀、q₁、q₂、q₃表示姿态角四元数；

根据表达式初始化卡尔曼滤波状态向量X₀。

2.如权利要求1所述的姿态测量方法，其特征在于，所述利用陀螺的输出更新计算的姿态角四元数，并确定卡尔曼滤波的状态转移矩阵，根据所述状态转移矩阵对状态向量和误差矩阵进行预测包括：

根据表达式更新姿态角四元数q₀、q₁、q₂、q₃，其中，q₀₊、q₁₊、q₂₊、q₃₊表示更新后的姿态角四元数，ω_x、ω_y、ω_z表示陀螺x、y、z轴的输出；

根据表达式确定卡尔曼滤波的状态转移矩阵F；

根据表达式对状态向量进行预测，根据表达式对误差矩阵进行预测，其中，和表示第k-1历元预测后的状态向量和预测前的状态向量，k表示历元数，P_k-1和P_k,k-1表示第k-1历元预测前的卡尔曼滤波方差矩阵和预测后的卡尔曼滤波方差矩阵，Q_k-1表示第k-1历元的量测噪声矩阵。