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

说明书

本发明涉及一种姿态获取方法，包括如下步骤：获取当前时刻角速度数据、加速度数据和地磁场强度数据；获取上一时刻系统状态量，获取上一时刻系统误差协方差矩阵值，进行迭代计算系统误差协方差矩阵，根据所述当前时刻协方差矩阵值和所述系统测量方程的雅克比矩阵值获取卡尔曼增益矩阵值；根据所述卡尔曼增益矩阵值、上一时刻系统状态量、系统测量量和所述系统测量方程获取当前时刻系统状态量；根据所述当前时刻系统状态量中的四元数获取姿态角。还提供了对应装置。动态响应快，可以捕捉高于5Hz的人体动作，静态误差在±0.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、根据所述当前时刻系统状态量中的四元数获取姿态角。

本发明第二方面，提供了一种姿态获取控制器，包括外部数据缓存单元、内部数据缓存单元和与所述外部数据缓存单元、内部数据缓存单元连接的计算单元，所述外部数据缓存单元用于缓存当前时刻角速度数据、加速度数据和地磁场强度数据；所述内部数据缓存单元用于缓存上一时刻系统状态量和上一时刻系统误差协方差矩阵值，所述计算单元用于执行上述的姿态角获取方法获取姿态角。

本发明第三方面，提供了一种姿态角获取装置，包括地磁传感器、角速度传感器和加速度传感器，还包括上述控制器，所述控制器与所述地磁传感器、角速度传感器和加速度传感器连接。