[发明专利]姿态角获取方法及装置

权利要求书

1.一种姿态角获取方法，其特征在于，包括如下步骤：

S110、获取当前时刻角速度数据、加速度数据和地磁场强度数据；

S120、获取上一时刻系统状态量，根据当前时刻角速度数据和上一时刻系统状态量求取系统状态矩阵值；

S130、获取上一时刻系统误差协方差矩阵值，根据上一时刻系统误差协方差矩阵值和所述系统状态矩阵值获取当前时刻系统误差协方差矩阵值；

S140、根据所述当前时刻地磁场强度数据和上一时刻系统状态量，建立系统测量方程h，利用所述上一时刻系统状态量对所述系统测量方程求偏导，获取系统测量方程的雅克比矩阵值；

S150、根据所述当前时刻系统误差协方差矩阵值和所述系统测量方程的雅克比矩阵值获取卡尔曼增益矩阵值；

S160、根据所述卡尔曼增益矩阵值、上一时刻系统状态量、系统测量量和所述系统测量方程获取当前时刻系统状态量，所述系统测量量表示为Y＝(a_x a_y a_z m_x m_y m_z)^T，其中，Y表示系统测量量，a_x、a_y、a_z表示当前时刻加速度数据在芯片坐标系下x、y、z轴的分量，m_x、m_y、m_z表示当前时刻地磁场强度数据在芯片坐标系下x、y、z轴的分量；

S170、根据所述当前时刻系统状态量中的四元数获取姿态角；

其中，所述系统状态量表示为X＝(q₀ q₁ q₂ q₃ Δω_x Δω_y Δω_z)^T，X表示系统状态量，q₀、q₁、q₂、q₃表示四元数，Δω_x、Δω_y、Δω_z表示系统计算的陀螺仪x、y、z轴角速度的误差；

所述步骤S140包括，根据上一时刻系统状态量中的四元数、当前时刻地磁场强度数据，建立系统测量方程，利用所述上一时刻系统状态量对所述系统测量方程求偏导获取系统测量方程的雅克比矩阵值；

具体地，所述系统测量方程表示为其中，表示加速度估计值，表示地磁场强度数据处理后值，所述建立系统测量方程的步骤如下：

根据上一时刻系统状态量中四元数获取姿态余弦矩阵

根据所述姿态余弦矩阵通过如下公式获取所述加速度估计值，其中，q₀、q₁、q₂、q₃表示上一时刻系统状态量中四元数；

将所述当前时刻地磁场强度数据通过如下公式转化到导航坐标系中，其中，所述导航坐标系的三个坐标轴分别指向东、北和地的上方，表示地磁场强度数据在导航坐标系下的表示；

去除当前时刻地磁场强度数据在导航坐标系下的x轴分量，通过如下公式将地磁场强度数据转化到芯片坐标系中

其中为导航坐标系下去除x轴分量后的当前时刻地磁场强度数据；

根据第一向量h_(1～3)和第二向量h_(4-6)组合形成系统测量方程如下：

所述系统测量方程的雅克比矩阵表示为：

所述步骤S120中采用一阶龙格库塔方法建立系统状态方程，利用上一时刻系统状态量对所述系统状态方程求偏导获取所述系统状态矩阵值。