峰岹51单片机+ICM42670+四元数解算+互补滤波计算欧拉角

凹凸曼的大表哥

已于 2025-02-28 15:37:18 修改

阅读量564

点赞数 12

文章标签：单片机 c语言无人机

于 2025-02-25 15:19:51 首次发布

本文链接：https://blog.csdn.net/daxing198612/article/details/145852663

版权

常用的IMU陀螺仪计算欧拉角方式为：Cortex M0/M3/M4的单片机+六轴陀螺仪如MPU6050，通过MPU6050自带的DMP库，直接输出四元数q0~q4，然后通过欧拉角转换公式直接转换,如：

pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; //俯仰角
roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; //横滚角
yaw   = atan2(2 * (q1 * q2 + q0 * q3), q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.3; //航向角

若采用51核的峰岹FU6572N+不带DMP库的ICM42670，可否也能计算出欧拉角呢?

结果当然是OK！并且占用MCU资源少，程序运行快，上电即稳态！

关于MCU用IIC或SPI驱动ICM42670的代码网上非常多，可以从别的文章摘录，这里只分享四元数解算欧拉角的方法：

1、第一步配置全局变量：

#include <FU6522.H>
#include "Angle.h"
// 参数配置
#define Kp  10.0f    // 比例增益
#define Ki  0.008f  // 积分增益
#define halfT 0.01f // 半采样周期（采样率50Hz）
// 全局/静态变量声明
float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // 初始四元数
float exInt = 0.0f, eyInt = 0.0f, ezInt = 0.0f;    // 积分误差
float Q_ANGLE_X = 0.0f, Q_ANGLE_Y = 0.0f, Q_ANGLE_Z = 0.0f; // 输出姿态角

2.配置IMU的量程及采样周期等：

因51单片机主频低，采样周期设置小点，50Hz即可；

void ICM42670_Init(void)
{  
	MPU42670_WriteReg(0x20, 0x6A );//角速度量程 250dps 采样周期50hz
	MPU42670_WriteReg(0x21, 0x4A );//加速度计量程4g 采样周期50HZ
	MPU42670_WriteReg(0x23, 0x03 );//陀螺仪低通滤波带宽 011:73hz
	MPU42670_WriteReg(0x24, 0x76 );//加速度计平均滤波器111:64x  低通滤波带宽110:25HZ
	MPU42670_WriteReg(0x06, 0x03 );//011:INT1 推挽输出 高电平有效
	MPU42670_WriteReg(0x1F, 0x0F );//1111://将陀螺仪置于低噪声（LN）模式下  加速度计置于低噪声（LN）模式下
}

3.读取出IMU内加速度计和陀螺仪的原始数据函数（int16型数据，有符号，满量程±32768）：

typedef struct
{
    int16   GYRO_X;
    int16   GYRO_Y;
    int16   GYRO_Z;
	uint8   GYRO_Init_Flag;
    uint8   GYRO_Ready_Flag;
	uint8   DelayInitCnt;
	int16   ACCEL_X;
	int16   ACCEL_Y;
	int16   ACCEL_Z;
} IICTypeDef;

IICTypeDef	 xdata IIC;

void ICM42670_ReadGyro(void)
{
    uint8 buf[7];
	MPU42670_ReadData(0x11,buf,6); //读取三轴陀螺仪原始数据
    IIC.GYRO_X  = ((int16)(buf[0] << 8) | buf[1]);
	IIC.GYRO_Y  = ((int16)(buf[2] << 8) | buf[3]);
    IIC.GYRO_Z  = ((int16)(buf[4] << 8) | buf[5]);   
}

void ICM42670_ReadACCEL(void)
{
    uint8 buf[7];
	MPU42670_ReadData(0x0b,buf,6);  //读取三轴加速度计原始数据
    IIC.ACCEL_X  = ((int16)(buf[0] << 8) | buf[1]);
	IIC.ACCEL_Y  = ((int16)(buf[2] << 8) | buf[3]);
	IIC.ACCEL_Z  = ((int16)(buf[4] << 8) | buf[5]);   
}

4.四元数解算欧拉角函数：

参考链接：https://blog.csdn.net/m0_73221129/article/details/128929450

//gx,gy,gz单位为弧度dps，ax,ay,az单位为重力单位g
void Ola_42670(float gx, float gy, float gz, float ax, float ay, float az) 
{
   	float q0temp,q1temp,q2temp,q3temp;//四元数暂存变量，求解微分方程时要用	
	float norm; 
    float vx, vy, vz;
    float ex, ey, ez;	
	float q0q0 = q0*q0;
	float q0q1 = q0*q1;
	float q0q2 = q0*q2;
	float q1q1 = q1*q1;
	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);//单位化加速度计，
	ax = ax * norm;// 这样变更了量程也不需要修改KP参数，因为这里归一化了
	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;  //将误差PI（比例和积分项）补偿到陀螺仪角速度
	gy = gy + Kp*ey + eyInt;
	gz = gz + Kp*ez + ezInt;  

	//下面进行姿态的更新，也就是四元数微分方程的求解
	q0temp=q0;
	q1temp=q1;
	q2temp=q2;
	q3temp=q3;
	//采用一阶毕卡解法，相关知识可参见《惯性器件与惯性导航系统》P212
	q0 = q0temp + (-q1temp*gx - q2temp*gy -q3temp*gz)*halfT;
	q1 = q1temp + (q0temp*gx + q2temp*gz -q3temp*gy)*halfT;
	q2 = q2temp + (q0temp*gy - q1temp*gz +q3temp*gx)*halfT;
	q3 = q3temp + (q0temp*gz + q1temp*gy -q2temp*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航向角由于加速度计对其没有修正作用，因此此处直接用陀螺仪积分代替
	Q_ANGLE_Z = Q_ANGLE_Z + gz*halfT*2*57.3; // yaw
	Q_ANGLE_Y = asin(-2 * q1 * q3 + 2 * q0* q2)*57.3; // pitch
	Q_ANGLE_X = atan2(2 * q2 * q3 + 2 * q0 * q1,-2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

}

5.main函数读取并打印欧拉角（单位°）：

void main(void)
{
    uint16 xdata  Uart_tx_delay_time = 0;    
    HardwareInit();
	IIC.GYRO_Ready_Flag = MPU6887_ReadID();
	if(IIC.GYRO_Ready_Flag==1) //IMU ID读取
	{
		ICM42670_Init();    	
		LED5_ON;										
	}
	else if(IIC.GYRO_Ready_Flag!=1) 
	{
		LED5_OFF;			
	}	

	
    while (1)
    {
		ICM42670_ReadACCEL();	//读取加速度计原始数据，量程4g						
		ICM42670_ReadGyro();    //读取陀螺仪原始数据,量程250弧度
        Ola_42670((float)IIC.GYRO_X*250/32768/57.3, (float)IIC.GYRO_Y*250/32768/57.3, (float)IIC.GYRO_Z*500/57.3/65536,	(float)IIC.ACCEL_X*4/32768, (float)IIC.ACCEL_Y*4/32768, (float)IIC.ACCEL_Z*4/32768);

        if(Uart_tx_delay_time++ >=500)
        {	
	        printf("pitch:%.2f roll:%.2f yaw:%.2f\r\n",Q_ANGLE_X,Q_ANGLE_Y,Q_ANGLE_Z);
            Uart_tx_delay_time=0;
	    }			
		        
     }       
    
}

6.串口打印

其中偏航角yaw会一直偏移，只有加装磁力计才能解决。

附送匿名上位机V7版本对接程序：

uint8 xdata IMU_Ola_Buff[13];	//欧拉角数据
void IMU_Ola_Data(int16 Roll, int16 Pitch,int16 Yaw)	//姿态数据发送
{
	int i;
	uint8 sumcheck = 0;
	uint8 addcheck = 0;
	IMU_Ola_Buff[0] = 0xAA;						//帧头，匿名上位机固定为0xAA
	IMU_Ola_Buff[1] = 0xFF;						//目标地址，0xFF代表无特定目标，用于数据广播型输出，可见于通信协议手册
	IMU_Ola_Buff[2] = 0X03;						//功能码，0X01:惯性传感器数据
	IMU_Ola_Buff[3] = 7;						//数据长度即字节数	6个传感器数据12字节+1字节震动状态
	IMU_Ola_Buff[4] = (Roll*100);				//小端模式，低8位在前；直接把16位制数赋值给8位制数Buff[4]，这样Buff[4]保留的是A的低8位
	IMU_Ola_Buff[5] = (Roll*100)>>8;			//先把Roll右移8位，再赋值给给8位制数Buff[5]，这样Buff[5]保留的是A的高8位
	IMU_Ola_Buff[6] = (Pitch*100);					
	IMU_Ola_Buff[7] = (Pitch*100)>>8;
	IMU_Ola_Buff[8] = (Yaw*100);					
	IMU_Ola_Buff[9] = (Yaw*100)>>8;
	IMU_Ola_Buff[10] = 1;
	/**** 校验的部分如下 ****/
	for(i = 0; i < (IMU_Ola_Buff[3] + 4); i++) 
	{
		sumcheck += IMU_Ola_Buff[i];
		addcheck += sumcheck;
	}
	IMU_Ola_Buff[11] = sumcheck;	
	IMU_Ola_Buff[12] = addcheck;			
}
IMU_AG_Data(IIC.ACCEL_X,IIC.ACCEL_Y,IIC.ACCEL_Z,IIC.GYRO_X,IIC.GYRO_Y,IIC.GYRO_Z);//填充传感器数据
Sendstring_USART1(IMU_Ola_Buff,sizeof(IMU_Ola_Buff));

匿名V7上位机测试视频