申请/专利权人：盐城数智科技有限公司

申请日：2023-04-03

公开（公告）日：2024-04-16

公开（公告）号：CN116817896B

主分类号：G01C21/16

分类号：G01C21/16;G01C21/18;G01C21/20;G01C17/38;G01R33/00

优先权：

专利状态码：有效-授权

法律状态：2024.04.16#授权;2023.10.24#实质审查的生效;2023.09.29#公开

摘要：本发明公开了一种基于扩展卡尔曼滤波的姿态解算方法，包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块，加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块；本发明属于导航技术领域，尤其涉及惯性导航领域，具体是指本发明提供一种基于扩展卡尔曼滤波的姿态解算方法，本发明的优点为：可将磁强计误差和畸变矫正与姿态解算方法同时进行，考虑了传感器误差和由磁强计安装引起的磁扰动，并考虑到了与惯性传感器的轴对齐问题，进而使位姿估计更加准确；使用扩展卡尔曼滤波，使估计结果越来越可靠；将过程噪声协方差矩阵和测量误差协方差矩阵设为常值矩阵，在满足实时性要求的同时获得精度较高的结果。

主权项：1.一种基于扩展卡尔曼滤波的姿态解算方法，包括在线磁强计校正模块、状态变量确定模块、外推方程构建模块，加速度计与磁力计估计模块、姿态偏置状态外推模块和姿态偏置状态更新模块；在线磁强计校正模块的操作步骤如下：S1、建立磁强计测量模型：S2、最大似然建模：S3、构建最大似然公式：S4、求解极大似然问题；所述状态变量确定模块的处理步骤如下：S1、定义状态变量： ，上式中，q0是四元数的实部，q1，q2，q3分别是四元数虚部的三个分量；b1，b2，b3分别是陀螺仪在x，y，z三个方向的偏差；偏置项指的是陀螺仪在单位时间内的漂移量，可将式（15）写成如下形式： ，上式中，q表示姿态四元数，b表示偏执置向量；S2、定义四元数求导法则： ，其中：Sω、q、Sq、ω的表达式分别为： ， ，S3、补偿陀螺仪的偏差，在方程中加入偏差项； ，S4、使用一阶线性化模型将上述方程进行离散化： ，上式中，T是k时刻到k+1时刻的采样时间，qk是k时刻的姿态四元数，qk+1是k+1时刻的姿态四元数，是k时刻的姿态四元数关于时间的导数； ，将式（18）代入式（19）中得： ，进一步有： ，状态方程构建模块的计算步骤如下：S1、结合方程（15）、（18）和（21）系统状态方程可以写成： ，因此，系统状态方程可以写成： ，式（23）的展开形式为： ，S2、将式（24）可化简为： ，矩阵A为状态转移矩阵，矩阵B为输入转移矩阵，为k时刻的状态变量，为k时刻的输入向量，为外推的状态向量；上式就是扩展卡尔曼滤波中的状态外推方程；所述加速度计与磁力计估计模块包括加速度估计和磁场估计两个步骤：S1、加速度估计，加速度估计的线性化结果为：首先其中加速度估计将重力矢量作为参考矢量，重力矢量总是指向当地水平坐标系的下方，有： ，上式中，是加速度计在传感器坐标系下估计的测量加速度；是当地水平坐标系到传感器坐标系的旋转矩阵；g是当地水平坐标系下的重力向量，g=[001]T；是传感器坐标系下的加速度计零偏，当对加速度计做了校正后是0；因为可以确定等号右边的所有变量，因此可以估计加速度计的测量值，然后使用加速度计的预测测量值与实际测量值进行比较，以确定方向误差， ，S2、其次，磁场估计的参考矢量是地球磁场提供的一个始终指向磁北方向的矢量，这个矢量在地球表面上会发生变化，矢量的高度取决于海拔，假设北参考矢量的变化是可以忽略不计的，因为传感器不会移动太多，只在房间内；假设磁强计数据在水平面上提供的参考矢量是准确的，因此要从磁强计参考矢量中去掉1维； ，上式中，h表示真实的磁场，hm表示磁强计测得的磁场；D-1表示失真矩阵的逆矩阵，q是偏移向量，都是在线磁强计校正模块的处理结果；接下来，为了从磁强计矢量中移除1维，由于想移除的磁场垂直分量存在于当地水平坐标系中，因此首先将坐标从传感器坐标系转换为当地水平坐标系，因此，由四元数生成旋转矩阵，如下所示： ，上式中， ，，上式中，r的每个分量是传感器坐标系下校正之后的磁强计数据，r’的每个分量是在当地水平坐标系下校正之后的磁强计数据，令r’的第二个分量等于零，然后重新标准化向量，使它保持为一个单位向量，然后将旋转它回到传感器坐标系，并使用结果向量代替实际校准的测量数据；使用北矢量作为磁强计的参考，因为北矢量恰好指向x轴的正方向，所以只是把正x轴作为参考矢量； ，其中，是磁强计在传感器坐标系下的估计的磁场强度值，是当地水平坐标系到传感器坐标系的旋转矩阵，是当地水平坐标系下参考磁场的北向分量，且有=[100]T；是传感器坐标系下的磁强计零偏值，当对磁强计进行校正后，为零，旋转矩阵的表达式为： ，化简得： ，上式依然是一个非线性方程，利用泰勒展开可以化简为： ，因此，线性化之后的表达式为： ，所述姿态偏置状态外推模块的处理过程如下：S1、进行状态外推：；在状态外推中，表示由k-1时刻的四元数生成的状态转移矩阵、是k-1时刻的状态变量、表示由k-1时刻的四元数生成的输入转移矩阵、为k时刻输入，也就是k时刻的陀螺仪的测量值，是外推结果，也就是外推得到的状态变量；S2、进行协方差外推：；在协方差外推中，Q为过程噪声协方差矩阵，是k-1时刻的状态误差协方差矩阵，表示由k-1时刻的四元数生成的状态转移矩阵；表示外推得到的k时刻的状态误差协方差矩阵；所述姿态偏置状态更新模块的处理过程如下：S1、计算卡尔曼增益矩阵：，上式中R是测量协方差矩阵，C是线性系数矩阵，也称为测量矩阵，表示外推得到的k时刻的状态误差协方差矩阵，Kk为卡尔曼增益矩阵；S2、进行状态更新：，上式中是外推得到的状态变量，C是线性系数矩阵，也称为测量矩阵，Kk为卡尔曼增益矩阵，是k时刻的加速度计测量和磁强计测量的组合向量，是更新得到的状态变量；S3、进行协方差更新：，上式中，I是7×7的单位矩阵，Kk为卡尔曼增益矩阵，表示外推得到的k时刻的状态误差协方差矩阵，C是线性系数矩阵，Rk是测量误差协方差矩阵，是更新后的状态误差协方差矩阵。

全文数据：

权利要求：

百度查询： 盐城数智科技有限公司 一种基于扩展卡尔曼滤波的姿态解算方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD