[发明专利]一种确定终端姿态的方法及装置

权利要求书

1.一种确定终端姿态的方法，其特征在于，包括：

在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值；

根据补偿的采样角速度值和初始姿态矩阵，确定更新姿态矩阵，其中，所述初始姿态矩阵由所述终端的方位角、俯仰角和翻滚角确定，

所述更新姿态矩阵(1)为通过将公式(2)代入公式(3)得到：

上述公式中，k,k+1分别表示离散化前后两个采样周期的时刻；Δt表示采样周期；ω_x,ω_y,ω_z为陀螺仪的输出的角速度经过陀螺仪的系统误差和陀螺仪的随机补偿之后的值；

其中，反对称阵公式为：

每个采样周期的姿态矩阵公式为：

上述公式(3)中，表示每个采样周期的姿态矩阵；根据所述补偿的采样角速度值确定卡尔曼滤波器的预测值，根据所述更新姿态矩阵确定所述卡尔曼滤波器的量测值；

根据确定的所述量测值修正所述预测值，得到所述终端的最优姿态估计值，其中，所述最优姿态估计值包括姿态角误差、陀螺仪随机误差、加速度传感器随机误差；根据当前采样周期的姿态最优估值获得下一个周期的姿态更新和滤波过程，即根据当前采样周期最优姿态估计中的所述陀螺仪的随机误差，以及陀螺仪的系统误差，补偿下一个采样周期陀螺仪输出的采样角速度值。

2.如权利要求1所述的方法，其特征在于，所述初始姿态矩阵根据以下方法确定：

根据加速度传感器的系统误差，确定所述终端的俯仰角和翻滚角；

根据所述终端的俯仰角和翻滚角，确定所述终端的初始姿态矩阵。

3.如权利要求2所述的方法，其特征在于，所述根据加速度传感器的系统误差，确定终端的俯仰角和翻滚角，包括：

在接收到传感器触发指令时，获取所述加速度传感器输出的初始加速度值；

根据所述初始加速度值和所述加速度传感器的系统误差，确定所述俯仰角和所述翻滚角；

所述根据所述终端的俯仰角和翻滚角，确定所述终端的初始姿态矩阵，包括：

在接收到传感器触发指令时，获取磁传感器输出的方位角；

根据所述方位角、所述俯仰角和所述翻滚角确定所述终端的初始姿态矩阵。

4.如权利要求1所述的方法，其特征在于，所述终端的最优姿态估计包括陀螺仪的随机误差；

所述补偿确定的所述采样角速度值，包括：

根据上一个采样周期最优姿态估计中的所述陀螺仪的随机误差，以及陀螺仪的系统误差，补偿当前采样周期陀螺仪输出的采样角速度值。

5.如权利要求2所述的方法，其特征在于，所述在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值之前，还包括：

在接收到传感器校正指令时，获取所述加速度传感器输出的校验加速度的值，根据所述校验加速度的值确定所述加速度传感器的系统误差。

6.如权利要求4所述的方法，其特征在于，所述在当前采样周期，确定终端的陀螺仪输出的采样角速度值，并补偿确定的所述采样角速度值之前，还包括：

在接收到传感器校正指令时，获取所述陀螺仪输出的校验角速度的值，根据所述校验角速度的值确定所述陀螺仪的系统误差。