当前位置：首页 > news >正文

课题学习(二十四)---专栏终章：基于四元数和扩展卡尔曼滤波的姿态解算算法（MPU9250+STM32F103ZET6）

news 2025/10/5 5:17:14

声明：本人水平有限，博客可能存在部分错误的地方，请广大读者谅解并向本人反馈错误。
由于研究生生涯已经结束了，未来接触惯性导航方向的机会可能不多了，因此我研究生课题的系列博客也就到此结束了，关于本专栏的最终章，我想把基于四元数和扩展卡尔曼滤波的姿态解算算法向大家介绍一下。
如果有需要源程序代码，可以点击下面的链接下载我的资源：基于四元数的扩展卡尔曼滤波与双级卡尔曼滤波的姿态解算（使用STM32F103ZET6和MPU9250）或者基于四元数的扩展卡尔曼滤波/双级扩展卡尔曼滤波的姿态解算源代码

一、基于四元数的扩展卡尔曼滤波算法

1.1 扩展卡尔曼滤波算法

算法的一些详细介绍在这就不展开了，这里只是简单介绍下算法的流程。

1.1.1 计算先验状态变量

$xkˉ=f(xk−1^，uk)\bar{x_{k}}=f(\hat{x_{k-1}}，u_{k})$
$xkˉ\bar{x_{k}}$ 为状态变量的先验估计； $xk−1^\hat{x_{k-1}}$ 是k-1时刻的状态估计；u_{k}为输入量，在本课题中没有输入，因此不需要关系u_{k}； $f(xk−1^，uk)f(\hat{x_{k-1}}，u_{k})$ 则是系统的非线性函数。

1.1.2 计算先验协方差矩阵

$Pkˉ=FPk−1^FT+R′\bar{P_{k}}=F\hat{P_{k-1}}F^T+R'$
$Pkˉ\bar{P_{k}}$ 为误差的先验协方差矩阵； $F$ 为状态转移矩阵； $Pk−1^\hat{P_{k-1}}$ 为k-1时刻的后验协方差矩阵； $R$ 为观测噪声协方差矩阵。

1.1.3 计算卡尔曼增益

$K=PkˉHT(HPkˉHT+Q′)−1K=\bar{P_{k}}H^T(H\bar{P_{k}}H^T+Q')^{-1}$
K为卡尔曼增益，H为观测矩阵；Q为状态协方差矩阵。

1.1.4 更新状态估计

$xk^=xkˉ+K(zk−H(xkˉ))\hat{x_{k}}=\bar{x_{k}}+K(z_k-H(\bar{x_{k}}))$
$z_k$ 为k时刻的观测值。

1.1.5 更新协方差矩阵

$Pk^=(I−KH)Pkˉ\hat{P_{k}}=(I-KH)\bar{P_{k}}$
$I$ 为单位矩阵。

1.2 系统状态方程和测量方程

选取的状态变量为 $x_k=[q_0,q_1,q_2,q_3]^T$ ，测量值为加速度和磁力计的测量值： $z_k=[a_x,a_y,a_z,m_x,m_y,m_z]^T$ 。
然后建立如下所示的状态方程和测量方程：

1.2.1 状态方程

$xkˉ=f(xk−1^)\bar{x_{k}}=f(\hat{x_{k-1}})$
其中， $f(xk−1^)f(\hat{x_{k-1}})$ 的表达式为：
在这里插入图片描述
对上式求雅克比矩阵，就是状态转移矩阵F：

1.2.2 测量方程

$ykˉ=h(xk−1^)\bar{y_{k}}=h(\hat{x_{k-1}})$
$h(xk−1^)h(\hat{x_{k-1}})$ 如下式所示：
在这里插入图片描述
其中， $C_N^b=(C_b^N)^T$ ， $C_b^N$ 如下式所示：

对上式求雅克比矩阵，就是测量矩阵H：

其中各元素如下图所示：

上图中， $g，M_x^N，M_y^N，M_z^N$ 分别为参考表坐标系下，重力加速度的值以及磁力计在参考坐标系下三个方向的磁场大小。

二、软件代码

2.1 代码流程

代码除了使用扩展卡尔曼滤波算法计算姿态外，还包含传感器原始数据读取、数据整合、TFT显示数据等功能。下图为代码的流程图。
在这里插入图片描述

2.2 读取并处理MPU9250的数据

MPU9250其实是一个六轴MPU6050+三轴磁力计AK8963，有需要的可以去看一下数据手册：MPU9250数据手册。
上面的数据手册没有寄存器的说明，它寄存器的说明手册可以去下面链接下载：MPU9250寄存器说明。注：PDF前半部分为MPU6050的寄存器，后面是AK8963的寄存器说明。

2.2.1 读取磁力计数据

在读取磁力计数据时，一般先读取ST1寄存器的DRDY位是否为1，若是1表示数据已经准备好了，可以读取了。
在这里插入图片描述
然后读取0x03~0x08寄存器的数据，它们分别存储XYZ三轴磁力计数据的高8位和低8位：

下面的代码就是读取磁力计数据的代码：

/*---------------MPU9250获取磁力计数据----------------*/
/*---------------------------------------------------*/MPU_Read_Len(MPU_MAG_ADDRESS,MPU_ST1,1,&MPU_Mag_ST);// 读取磁力计的状态，DYDR位if((MPU_Mag_ST&0x01) == 1){MPU_Read_Len(MPU_MAG_ADDRESS,MPU_RA_MAG_XOUT_L,6,rawMagData);MPU_Write_Byte(MPU_MAG_ADDRESS,MPU_CNTL,0x11);   // 单独测量模式MagnetRawAd[0] = (rawMagData[1] << 8) | rawMagData[0];MagnetRawAd[1] = (rawMagData[3] << 8) | rawMagData[2];MagnetRawAd[2] = (rawMagData[5] << 8) | rawMagData[4];magX = MagnetRawAd[0]*0.15;magY = MagnetRawAd[1]*0.15;magZ = MagnetRawAd[2]*0.15;	}/*---------------------------------------------------*/
/*---------------MPU9250获取磁力计数据----------------*/

最后的*0.15是为了将数字量转为uT。
在这里插入图片描述

2.2.2 读取加速度和陀螺仪数据

该部分和读取MPU6050的代码一样，在此就不再详细介绍了：

/*---------------MPU9250获取加速度和陀螺仪数据-----------------*/
/*------------------------------------------------------------*/MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,rawAccData);accX = (rawAccData[0]<<8 |rawAccData[1]);accY = (rawAccData[2]<<8 |rawAccData[3]);accZ = (rawAccData[4]<<8 |rawAccData[5]);MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,rawGyroData);gyroX = (rawGyroData[0]<<8 |rawGyroData[1]);gyroY = (rawGyroData[2]<<8 |rawGyroData[3]);gyroZ = (rawGyroData[4]<<8 |rawGyroData[5]);
/*------------------------------------------------------------*/
/*---------------MPU9250获取加速度和陀螺仪数据-----------------*/

当然，到目前这一步加速度和陀螺仪的数据为数字量，可以先进行处理，转为物理量（我直接将数字量传入EKF函数的实参进行处理）：

/* 实参中乘以的系数意义：0.0023926 = 9.8/4096;0.0609756f = 1/16.4;*/
accX = accX*0.0023926f;
gyroY = gyroY*0.0609756f;

2.3 EKF姿态解算

下面是EKF的调用函数：

EKF_update(&AEKF,Algorithm_Euler,(float)(accX*0.0023926f),(float)(accY*0.0023926f),(float)(accZ*0.0023926f),(float)(gyroX*0.0609756f),(float)(gyroY*0.0609756f),(float)(gyroZ*0.0609756f),magX,magY,magZ,Gravity,dT);

下面是该函数形参的说明：

void EKF_update(ekf_t* ekf, float euler[3], float ax, float ay, float az, float p, float q, float r,  float mx, float my, float mz, float refGravity,float dt)

ekf：EKF的结构体，包含先验和后验协方差矩阵，测量噪声矩阵，过程噪声矩阵，卡尔曼增益，先验和后验估计状态变量；
euler[3]：计算的姿态角度结果；
ax,ay,az：三轴重力加速度的测量值；
p,q,r,：三轴陀螺仪的测量值；
mx,my,mz：三轴磁力计的测量值；
refGravity：当地重力加速度值；
dt：时间间隔（在此为定时器中断的时间）；

2.4 LCD显示

LCD的显示放在while循环内：

  while (1){LCD_ShowNum(75,10,Algorithm_Euler[2],3,16);LCD_ShowNum(75,36,Algorithm_Euler[1]+180,3,16);LCD_ShowNum(75,62,Algorithm_Euler[0],3,16);//		LCD_ShowNum(100,120,accX,4,12);
//		LCD_ShowNum(100,132,accY,4,12);
//		LCD_ShowNum(100,144,accZ,4,12);//		LCD_ShowNum(100,190,addressMAG,4,12);
//		LCD_ShowNum(100,206,addressMPU,4,12);
//		LCD_ShowNum(100,222,passMOD,4,12);//		LCD_ShowNum(100,190,addressMAG,4,12);
//		LCD_ShowNum(100,206,res,5,12);
//		LCD_ShowNum(100,222,magAzimuth,4,12);}

这里可以根据读者想要显示的内容进行调整，与此配合的是userInfoDisplay函数，这个函数用于显示固定的中文字符，在初始化时调用一次即可：

void userInfoDisplay()
{// 信息标志:
//	ShowMulChinese(10,240,0,7,LGRAY,RED);
//	ShowMulChinese(10,256,8,16,LGRAY,BLUE);
//	ShowMulChinese(10,272,17,20,LGRAY,DARKBLUE);
//	ShowMulChinese(10,288,21,23,LGRAY,BLACK);// 三角度信息ShowMulChinese(10,10,45,47,LGRAY,RED);ShowMulChinese(60,10,54,54,LGRAY,RED);ShowMulChinese(140,10,70,70,LGRAY,RED);ShowMulChinese(10,36,48,50,LGRAY,BLUE);ShowMulChinese(60,36,54,54,LGRAY,BLUE);ShowMulChinese(140,36,70,70,LGRAY,BLUE);ShowMulChinese(10,62,51,53,LGRAY,DARKBLUE);ShowMulChinese(60,62,54,54,LGRAY,DARKBLUE);ShowMulChinese(140,62,70,70,LGRAY,DARKBLUE);ShowMulChinese(10,100,71,82,LGRAY,RED);ShowMulChinese(10,120,83,90,LGRAY,BLUE);// 作者信息
//	ShowMulChinese(10,100,56,62,LGRAY,RED);
//	ShowMulChinese(120,100,54,54,LGRAY,RED);
//	
//	ShowMulChinese(10,170,63,69,LGRAY,BLUE);
//	ShowMulChinese(120,170,54,54,LGRAY,BLUE);// AS5600角度
//	ShowMulChinese(10,222,30,31,LGRAY,BLUE);
//	ShowMulChinese(60,222,54,54,LGRAY,BLUE);
}

下面是实际效果图：
在这里插入图片描述

三、专栏总结

最后，感谢大家对我的博客的支持，我的初心主要是把课题学习的过程分享出来，但是因为读研期间赶项目，以及最后赶大论文，没有及时更新，还望大家谅解。博客内容肯定会有问题，如果有读者发现希望不吝赐教。
好了，就到这吧，如果以后还做惯导这方面的工作，还会继续更新该专栏博客，再次感谢大家的支持，谢谢各位！

四、往期回顾

课题学习(一)----静态测量
课题学习(二)----倾角和方位角的动态测量方法（基于磁场的测量系统）
课题学习(三)----倾角和方位角的动态测量方法（基于陀螺仪的测量系统）
课题学习(四)----四元数解法
课题学习(五)----阅读论文《抗差自适应滤波的导向钻具动态姿态测量方法》
课题学习(六)----安装误差校准、实验方法
课题学习(七)----粘滑运动的动态算法
课题学习(八)----卡尔曼滤波动态求解倾角、方位角
课题学习(九)----阅读《导向钻井工具姿态动态测量的自适应滤波方法》论文笔记
课题学习(十)----阅读《基于数据融合的近钻头井眼轨迹参数动态测量方法》论文笔记
课题学习(十一)----阅读《Attitude Determination with Magnetometers and Accelerometers to Use in Satellite》
课题学习(十二)----阅读《Extension of a Two-Step Calibration Methodology to Include Nonorthogonal Sensor Axes》
课题学习(十三)----阅读《Calibration of Strapdown Magnetometers in Magnetic Field Domain》论文笔记
课题学习(十四)----三轴加速度计+三轴陀螺仪传感器-ICM20602
课题学习(十五)----阅读《测斜仪旋转姿态测量信号处理方法》论文
课题学习(十六)----阅读《Continuous Wellbore Surveying While Drilling Utilizing MEMS Gyroscopes Based…》论文
课题学习(十七)----姿态更新的四元数算法总结
课题学习(十八)----捷联测试电路设计与代码实现(基于MPU6050和QMC5883L)
课题学习(十九)----Allan方差：陀螺仪噪声分析
课题学习(二十)----阅读《近钻头井斜动态测量重力加速度信号提取方法研究》论文
课题学习(二十一)----姿态更新的四元数算法推导
课题学习(二十二)—《A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor …》
课题学习(二十三)—三轴MEMS加速度计芯片ADXL372