MPU6050姿态解算——Mahony互补滤波_mahony互补滤波 kp和ki(1)

2401_85015097

于 2024-05-17 03:59:07 发布

阅读量708

点赞数 19

分类专栏： 2024年程序员学习文章标签：物联网嵌入式硬件面试

本文链接：https://blog.csdn.net/2401_85015097/article/details/138985387

版权

2024年程序员学习专栏收录该内容

16 篇文章 0 订阅

订阅专栏

既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上物联网嵌入式知识点，真正体系化！

由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、电子书籍、讲解视频，并且后续会持续更新

需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）

如果你需要这些资料，可以戳这里获取

在概述中我们分析了陀螺仪中角速度测量可能存在一个误差，这个误差在短时间内可以忽略，但是随着时间的累积，误差会越来越大。陀螺仪中的加速度计对高频信号敏感，在振动环境中高频干扰大。

陀螺仪得到的角速度数据短期内可信，长期由于积分误差不可信。加速度计得到的加速度数据，短期内由于高频抖动干扰不可信，长期可信。

拿出我们的中学二年级知识，地球上一切物体的重力加速度都是9.8m/s^2，我们用地理坐标系的重力加速度g，乘以一个从地理坐标系n到机体坐标系b的姿态转换矩阵，可以推导出机体坐标系的理论重力加速度v。在这里插入图片描述

误差补偿

在这里插入图片描述

由上图可以看出，陀螺仪测出角速度解算出四元数，再由四元素推导出理论重力加速度v，而加速度计测出实际重力加速度a又称gb(b系下的重力加速度)，如果陀螺仪测出的角速度完全可靠，那么实际重力加速度gb应该是等于理论重力加速度vb的，但是很不幸，角速度数据存在误差，vb和gb就不会重合。

灵魂来了
用一个叉积来描述这个误差的大小

几何意义表示：
在这里插入图片描述

在这里插入图片描述
叉积公式(两种表达形式，第二种在程序里体现)：

aXb = |a|*|b|*sin θ
对于向量a（x1， y1）， b（x2， y2），叉乘公式为x1 * y2 - x2 * y1

小角近似(sin θ的值与θ的值在θ很小的时侯很相近)：
在这里插入图片描述
在上面得到了理论重力加速度与实际重力加速度的误差erroor，这个error是一个实际的角度误差，我们用加速度计的值间接得出了误差变化的角度相关值，现在借助PID中的PI思想将这个误差补偿，这样就通过加速度计间接的得到了角度误差值(KP=1.5，KI=0.005，是哪篇论文研究过我忘记了，参数可调，按实际来)：
在这里插入图片描述

偏差角度很小的情况下，我们可以将陀螺仪角速度误差和加速度计求得的角度差看做正比的关系：
在这里插入图片描述
得到准确的角速度后就可以用四元数反解出正确可靠的欧拉角了。

STM32程序实例

#include "imu.h"
#include "math.h"
#include "mpu6050.h"
#include "stm32f10x.h"

#define Acc\_Gain 0.0001220f //加速度转换单位(初始化加速度计量程+-4g，由于mpu6050的数据寄存器是16位的，LSBa = 2\*4 / 65535.0)
#define Gyro\_Gain 0.0609756f //角速度转换为角度(LSBg = 2000\*2 / 65535)
#define Gyro\_Gr 0.0010641f //角速度转换成弧度(3.1415 / 180 \* LSBg)
#define G 9.80665f // m/s^2

extern short ax,ay,az;
extern short gx,gy,gz;


static float invSqrt(float x) 		//快速计算 1/Sqrt(x)
{
	float halfx = 0.5f \* x;
	float y = x;
	long i = \*(long\*)&y;
	i = 0x5f3759df - (i>>1);
	y = \*(float\*)&i;
	y = y \* (1.5f - (halfx \* y \* y));
	return y;
}


void Prepare\_Data(void)
{
	MPU\_Get\_Accelerometer(&ax,&ay,&az);			//获取加速度计原始值
	MPU\_Get\_Gyroscope(&gx,&gy,&gz);				//获取角速度原始值
	
	//将加速度原始AD值转换为m/s^2
	ax = (float)ax \* Acc_Gain \* G;
	ay = (float)ay \* Acc_Gain \* G;
	az = (float)az \* Acc_Gain \* G;
	
	//将陀螺仪AD值转换为 弧度/s
	gx = (float)gx \* Gyro_Gr;
	gy = (float)gy \* Gyro_Gr;
	gz = (float)gz \* Gyro_Gr;
}



#define Kp 1.50f
#define Ki 0.005f
#define halfT 0.0025f //计算周期的一半，单位s

extern float Yaw,Pitch,Roll;				//我要给其他文件用所以定义了extern，不用管
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;		//四元数
float exInt = 0, eyInt = 0, ezInt = 0;		//叉积计算误差的累计积分


void Imu\_Update(float gx,float gy,float gz,float ax,float ay,float az)
{
	u8 i;
	float vx,vy,vz;							//实际重力加速度
	float ex,ey,ez;							//叉积计算的误差
	float norm;
	
 	float q0q0 = q0\*q0;
 	float q0q1 = q0\*q1;
	float q0q2 = q0\*q2;
	float q0q3 = q0\*q3;
	float q1q1 = q1\*q1;
 	float q1q2 = q1\*q2;
 	float q1q3 = q1\*q3;
	float q2q2 = q2\*q2;
	float q2q3 = q2\*q3;
	float q3q3 = q3\*q3;
	
	if(ax\*ay\*az == 0)
		return;
	
	//加速度计测量的重力方向(机体坐标系)
	norm = invSqrt(ax\*ax + ay\*ay + az\*az);			//之前这里写成invSqrt(ax\*ax + ay+ay + az\*az)是错误的，现在修改过来了
	ax = ax \* norm;
	ay = ay \* norm;
	az = az \* norm;
	
	//四元数推出的实际重力方向(机体坐标系)
	vx = 2\*(q1q3 - q0q2);												
  	vy = 2\*(q0q1 + q2q3);
  	vz = q0q0 - q1q1 - q2q2 + q3q3;
	
	//叉积误差
	ex = (ay\*vz - az\*vy);
	ey = (az\*vx - ax\*vz);
	ez = (ax\*vy - ay\*vx);
	
	//叉积误差积分为角速度
	exInt = exInt + ex \* Ki;
	eyInt = eyInt + ey \* Ki;
	ezInt = ezInt + ez \* Ki;
	
	//角速度补偿
	gx = gx + Kp\*ex + exInt;
	gy = gy + Kp\*ey + eyInt;
	gz = gz + Kp\*ez + ezInt;
	
	//更新四元数
  	q0 = q0 + (-q1\*gx - q2\*gy - q3\*gz)\*halfT;
  	q1 = q1 + (q0\*gx + q2\*gz - q3\*gy)\*halfT;
  	q2 = q2 + (q0\*gy - q1\*gz + q3\*gx)\*halfT;
  	q3 = q3 + (q0\*gz + q1\*gy - q2\*gx)\*halfT;	
	
	//单位化四元数
  	norm = invSqrt(q0\*q0 + q1\*q1 + q2\*q2 + q3\*q3);
  	q0 = q0 \* norm;
  	q1 = q1 \* norm;
  	q2 = q2 \* norm;  
  	q3 = q3 \* norm;
	
	//四元数反解欧拉角
	Yaw = atan2(2.f \* (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3)\* 57.3f;
	Pitch = -asin(2.f \* (q1q3 - q0q2))\* 57.3f;


![img](https://img-blog.csdnimg.cn/img_convert/3ab5dc3911b78e0f418076bab4b3c575.png)
![img](https://img-blog.csdnimg.cn/img_convert/607bac57d9c7dc7f09b70ff2952f849d.png)

**既有适合小白学习的零基础资料，也有适合3年以上经验的小伙伴深入学习提升的进阶课程，涵盖了95%以上物联网嵌入式知识点，真正体系化！**

**由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、电子书籍、讲解视频，并且后续会持续更新**

**需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）**

**[如果你需要这些资料，可以戳这里获取](https://bbs.csdn.net/topics/618679757)**



**由于文件比较多，这里只是将部分目录截图出来，全套包含大厂面经、学习笔记、源码讲义、实战项目、大纲路线、电子书籍、讲解视频，并且后续会持续更新**

**需要这些体系化资料的朋友，可以加我V获取：vip1024c （备注嵌入式）**

**[如果你需要这些资料，可以戳这里获取](https://bbs.csdn.net/topics/618679757)**