使用江科大代码实现姿态解算

吃茶去咯

于 2025-03-14 21:58:44 发布

阅读量828

点赞数 18

文章标签：人工智能嵌入式硬件单片机程序人生

本文链接：https://blog.csdn.net/2301_76574679/article/details/146267591

版权

之前跟人做了一个平衡车，但是是使用的HAL库，我一开始学习使用的是标准库，跟着江科大学习的。所以，我一直准备用标准库实现一次，这几天学习了MPU6050的姿态解算，学习了一下四元数，欧拉角。

我的理解就是，使用欧拉角进行计算时，会出现多解。

A` = 旋转矩阵*A，进行公示求解时，

当出现90度时，cos90 = 0

所以旋转矩阵会退化，只要第二次出现90度时，只要a - y 的值定了，那么任何角都是一样的，所以会出现【30 90 60】和【40 90 70】是一样的。

所以，引入四元数

q = q0 + q1i +q2j +a3k, 具体公式网上都有的，我们这里主要使用

后面就是一系列计算，算出欧拉角了

附上我的代码

江科大的I2C我只修改了引脚，和启用PB3和PB4的i/o功能

void MyI2C_Init(void)
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE); //JTAG-DP 禁用 + SW-DP 使能。只将PA15 ,PB3 ,PB4变成普通IO口。
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_3;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitStructure);//默认是低电平
	
	GPIO_SetBits(GPIOB,GPIO_Pin_4|GPIO_Pin_3);
	
}

#include "stm32f10x.h"                  // Device header
#include "MyI2C.h"
#include "MPU6050_Reg.h"

#define MPU6050_ADDRESS 0xD0


uint8_t FLAG;




/**
  * 函    数：MPU6050初始化
  * 参    数：无
  * 返 回 值：无
  */
void MPU6050_Init(void)
{
	MyI2C_Init();
	MPU6050_WriteReg(MPU6050_PWR_MGMT_1,0x01);
	MPU6050_WriteReg(MPU6050_PWR_MGMT_2,0x00);
	MPU6050_WriteReg(MPU6050_SMPLRT_DIV,0x04);     //采样率分频寄存器，配置采样率 1000/（4+1） =  100
	MPU6050_WriteReg(MPU6050_CONFIG,0x06);         //配置寄存器，配置DLPF 设置数字低通滤波器(DLPF)
	MPU6050_WriteReg(MPU6050_GYRO_CONFIG,0x00);   //陀螺仪配置寄存器，选择满量程为±250°/s
	MPU6050_WriteReg(MPU6050_ACCEL_CONFIG,0x00);  //加速度计配置寄存器，选择满量程为±2g
	
	// 配置中断为低电平有效
    MPU6050_WriteReg(MPU6050_INT_PIN_CFG, 0x80);   // INT 低电平有效，推挽输出
    MPU6050_WriteReg(MPU6050_INT_ENABLE, 0x01);    // 启用 Data Ready 中断
	
	//配置外部中断(INT引脚触发)
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB,ENABLE);
	GPIO_InitTypeDef GPIO_InitTypeStrcutrue;
	GPIO_InitTypeStrcutrue.GPIO_Mode = GPIO_Mode_IPU;
	GPIO_InitTypeStrcutrue.GPIO_Pin = GPIO_Pin_5;
	GPIO_InitTypeStrcutrue.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOB,&GPIO_InitTypeStrcutrue);
	
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOB,GPIO_PinSource5);
	EXTI_InitTypeDef EXTI_InitTypeStruct;
	EXTI_InitTypeStruct.EXTI_Line = EXTI_Line5;
	EXTI_InitTypeStruct.EXTI_LineCmd = ENABLE;
	EXTI_InitTypeStruct.EXTI_Mode = EXTI_Mode_Interrupt;
	EXTI_InitTypeStruct.EXTI_Trigger = EXTI_Trigger_Falling;
	EXTI_Init(&EXTI_InitTypeStruct);
	
	//配置NVIC
	NVIC_InitTypeDef NVIC_InitTypeStrcut;
	NVIC_InitTypeStrcut.NVIC_IRQChannel = EXTI9_5_IRQn;
	NVIC_InitTypeStrcut.NVIC_IRQChannelCmd = ENABLE;
	NVIC_InitTypeStrcut.NVIC_IRQChannelPreemptionPriority = 1;
	NVIC_InitTypeStrcut.NVIC_IRQChannelSubPriority = 0;
	NVIC_Init(&NVIC_InitTypeStrcut);
	
}

uint8_t MPU_GetFlag(void)
{
	if(FLAG == 1)
	{
		FLAG = 0;
		return 1;
	}
	return 0;
	
}

//中断函数
void EXTI9_5_IRQHandler(void)
{
	if(EXTI_GetITStatus(EXTI_Line5) != RESET)
	{
		//进行PID
		
		 FLAG =  1;
		// 清除中断标志位
		EXTI_ClearITPendingBit(EXTI_Line5);
	}
	
}

#include "stm32f10x.h"
#include "MPU6050.h"
#include "math.h"

#define RtA (57.2957795f)  // 弧度转角度，180/π ≈ 57.2957795
#define Ki  0.005f         // 积分系数
#define DT  0.005f          // 计算周期的一半，单位s

// 加速度和角速度
int16_t AX, AY, AZ, GX, GY, GZ;

typedef struct {
    float c1, c2, c3, a1, a2, a3, a;
} Degree;

Degree d;

typedef struct {
    float q0, q1, q2, q3;
    float exInt, eyInt, ezInt;
} Quater;

Quater q = {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};

void Dmp_Init(void) {
    MPU6050_Init();
}

void MPU6050_Read(void) {
    MPU6050_GetData(&AX, &AY, &AZ, &GX, &GY, &GZ);
    
    // 转换为物理量
    d.c1 = GX / 131.0f;  // 角速度（°/s）
    d.c2 = GY / 131.0f;
    d.c3 = GZ / 131.0f;
    
    // 转换为rad/s
    d.c1 /= RtA;
    d.c2 /= RtA;
    d.c3 /= RtA;
    
    d.a1 = AX / 16384.0f;  // 加速度（g）
    d.a2 = AY / 16384.0f;
    d.a3 = AZ / 16384.0f;
    
    // 加速度归一化
    float norm = sqrtf(d.a1 * d.a1 + d.a2 * d.a2 + d.a3 * d.a3);
    if (norm > 1e-6f) {  // 防止除零
        float inv_norm = 1.0f / norm;
        d.a1 *= inv_norm;
        d.a2 *= inv_norm;
        d.a3 *= inv_norm;
        d.a = 1.0f;
    } else {
        d.a = 0.0f;
    }
}

void ComputeEulerAngles(float *pitch, float *roll, float *yaw) {
    MPU6050_Read();
    
    // 计算姿态误差
    float gx = d.c1, gy = d.c2, gz = d.c3;
    float gravity_x = 2.0f * (q.q1 * q.q3 - q.q0 * q.q2);
    float gravity_y = 2.0f * (q.q0 * q.q1 + q.q2 * q.q3);
    float gravity_z = q.q0*q.q0 - q.q1 * q.q1 - q.q2 * q.q2 + q.q3*q.q3;
    
    float error_x = d.a2 * gravity_z - d.a3 * gravity_y;
    float error_y = d.a3 * gravity_x - d.a1 * gravity_z;
    float error_z = d.a1 * gravity_y - d.a2 * gravity_x;
    
    // 更新四元数
    float Kp = 0.5f;
    
    // 误差积分
    q.exInt += Ki * error_x ;
    q.eyInt += Ki * error_y ;
    q.ezInt += Ki * error_z ;
    
    // 修正角速度
    gx += Kp * error_x + q.exInt;
    gy += Kp * error_y + q.eyInt;
    gz += Kp * error_z + q.ezInt;
    
    // 四元数微分方程
    float q0_dot = (-q.q1 * gx - q.q2 * gy - q.q3 * gz) * DT; 
    float q1_dot = (q.q0 * gx - q.q3 * gy + q.q2 * gz) * DT;
    float q2_dot = (q.q3 * gx + q.q0 * gy - q.q1 * gz) * DT;
    float q3_dot = (-q.q2 * gx + q.q1 * gy + q.q0 * gz) * DT;
    
    // 更新四元数
    q.q0 += q0_dot;
    q.q1 += q1_dot;
    q.q2 += q2_dot;
    q.q3 += q3_dot;
    
    // 归一化
    float norm = sqrtf(q.q0 * q.q0 + q.q1 * q.q1 + q.q2 * q.q2 + q.q3 * q.q3);
    if (norm > 1e-6f) {
        float inv_norm = 1.0f / norm;
        q.q0 *= inv_norm;
        q.q1 *= inv_norm;
        q.q2 *= inv_norm;
        q.q3 *= inv_norm;
    }
    
    // 计算欧拉角
    float q0q0 = q.q0 * q.q0;
    float q1q1 = q.q1 * q.q1;
    float q2q2 = q.q2 * q.q2;
    float q3q3 = q.q3 * q.q3;
    
    *roll = atan2f(2.0f * (q.q2 * q.q3 + q.q0 * q.q1), q0q0 - q1q1 - q2q2 + q3q3);
    *pitch = asinf(-2.0f * (q.q1 * q.q3 - q.q0 * q.q2));
    *yaw = atan2f(2.0f * (q.q1 * q.q2 + q.q0 * q.q3), q0q0 + q1q1 - q2q2 - q3q3);
    
//    // 转换为度
//    *roll *= RtA;
//    *pitch *= RtA;
//    *yaw *= RtA;
}

如果想更好的理解我建议看一下这个视频视频连接