同步操作将从 一个杀手/mpu6050_Mahony 强制同步,此操作会覆盖自 Fork 仓库以来所做的任何修改,且无法恢复!!!
确定后同步将在后台操作,完成时将刷新页面,请耐心等待。
本项目包括底层iic驱动、mpu6050驱动、imu姿态解算代码,以stm32标准库为例。
MPU6050是一个集成了陀螺仪和加速度计的传感器,它能输出在直角坐标系下的x,y,z轴的角速度和加速度数据。
陀螺仪输出的格式为:绕x轴的旋转角速度,绕y轴的角速度,绕z轴的角速度(分别称为roll角速度,pitch角速度和yaw角速度)。
加速度计输出的格式为:x轴的加速度,y轴的加速度,z轴的加速度。
另外还需要关注传感器的其他参数如:
我们从MPU6050那就得到了陀螺仪数据gx,gy,gz,加速度数据az,ay,az
螺仪转换精度2^16=65536,65536/{2000-(-2000)}=16.4,实际1°等于adc值16.4
采样率就是数据的更新率,也就是我们每次读取数据的频率。
参考另一篇文章:基于Manony滤波算法的姿态解算
该部分代码参考了正点原子的MPU6050例程;主要修改以下初始化代码
/*
* MPU6050模块:绕x轴为roll,绕y轴为pitch,绕z轴为yaw
*/
uint8_t MPU_Init(void)
{
uint8_t res;
IIC_Init(); //初始化IIC总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80); //复位MPU6050
//等待复位完成
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00); //唤醒MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪量程+-2000
MPU_Set_Accel_Fsr(0); //加速度计量程+-2g
MPU_Set_Rate(1000); //1khz采样率
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //关闭IIC主模式
MPU_Write_Byte(MPU_FIFO_EN_REG,0X00); //关闭FIFO
MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80); //关闭INT
res=MPU_Read_Byte(MPU_DEVICE_ID_REG); //读取设备id,AD0引脚接地 故id应该为0x68
if(res==MPU_ADDR)
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置x轴为时钟
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //开启陀螺仪加速度计
MPU_Set_Rate(1000);
}else return 1;
return 0;
}
重新编写一个读取mpu6050数据的函数。使我们读取的数据是经过平均滤波的数据。
使用6个FIFO队列对数据(gx,gy,gz,ax,ay,az)进行缓存,每次读取一次数据,就将数据入队,并计算队列的平均值,对原始数据进行平滑滤波。
#define Buf_SIZE 10 //队列长度,越大,平滑性越高
int16_t MPU6500_FIFO[6][Buf_SIZE]; //6个FIFO队列;0-2:陀螺仪数据;3-5:加速度计数据
int16_t lastAx,lastAy,lastAz,lastGx,lastGy,lastGz;
static uint8_t Wr_Index = 0; //当前FIFO的写入下标
//将val入队
static void MPU6500_NewVal(int16_t* buf,int16_t val) {
buf[Wr_Index] = val;
}
//计算FIFO中的平均值
static int16_t MPU6500_GetAvg(int16_t* buf)
{
int i;
int32_t sum = 0;
for(i=0;i<Buf_SIZE;i++)
sum += buf[i];
sum = sum / Buf_SIZE;
return (int16_t)sum;
}
//读取经过滤波的陀螺仪,加速度数据
void MPU6500_readGyro_Acc(int16_t *gyro,int16_t *acc)
{
static short buf[6]; //缓存原始数据:0-2:陀螺仪数据;3-5:加速度计数据
static int16_t gx,gy,gz;
static int16_t ax,ay,az;
//正点原子的库函数,读取传感器原始数据
MPU_Get_Gyroscope(&buf[0],&buf[1],&buf[2]);
MPU_Get_Accelerometer(&buf[3],&buf[4],&buf[5]);
//将原始数据入队
MPU6500_NewVal(&MPU6500_FIFO[0][0],buf[0]);
MPU6500_NewVal(&MPU6500_FIFO[1][0],buf[1]);
MPU6500_NewVal(&MPU6500_FIFO[2][0],buf[2]);
MPU6500_NewVal(&MPU6500_FIFO[3][0],buf[3]);
MPU6500_NewVal(&MPU6500_FIFO[4][0],buf[4]);
MPU6500_NewVal(&MPU6500_FIFO[5][0],buf[5]);
//更新FIFO入口指针
Wr_Index = (Wr_Index + 1) % Buf_SIZE;
//计算队列平均值
gx = MPU6500_GetAvg(&MPU6500_FIFO[4][0]);
gy = MPU6500_GetAvg(&MPU6500_FIFO[5][0]);
gz = MPU6500_GetAvg(&MPU6500_FIFO[6][0]);
//陀螺仪数据要减去偏移量
gyro[0] = gx - imu.Roll_offset; //gyro
gyro[1] = gy - imu.Pitch_offset;
gyro[2] = gz - imu.Yaw_offset;
ax = MPU6500_GetAvg(&MPU6500_FIFO[0][0]);
ay = MPU6500_GetAvg(&MPU6500_FIFO[1][0]);
az = MPU6500_GetAvg(&MPU6500_FIFO[2][0]);
acc[0] = ax; //acc
acc[1] = ay;
acc[2] = az;
}
计算角速度偏移:当mpu初始化完成且保持静止时,在三轴的角速度上有一个非0的偏移,需要计算出这个偏移,在后面的角度更新过程中减去这个偏移,这样角度才不会累加。
static void MPU6500_Init_Offset(void)
{
unsigned int i;
int16_t temp[3],temp2[3];
int32_t tempgx=0,tempgy=0,tempgz=0;
int32_t tempax=0,tempay=0,tempaz=0;
imu.Pitch_offset = 0;
imu.Roll_offset = 0;
imu.Yaw_offset = 0;
//wait the mpu to be ready
for(i=0;i<100;i++){
delay_ms(10);
MPU6500_readGyro_Acc(temp,temp2);
}
//calculate the average of imu data as offset
for(i=0;i<OFFSET_CONUT;i++){
delay_ms(10);
MPU6500_readGyro_Acc(temp,temp2);
tempgx += temp[0];
tempgy += temp[1];
tempgz += temp[2];
tempax += temp2[0];
tempay += temp2[1];
tempaz += temp2[2];
}
imu.Pitch_offset = tempgy/OFFSET_CONUT;
imu.Roll_offset = tempgx/OFFSET_CONUT;
imu.Yaw_offset = tempgz/OFFSET_CONUT;
}
首先将陀螺仪的数据转换成角度,这里封装成一个函数
static void Get_IMU_Values(float *values)
{
int16_t gyro[3],acc[3];
MPU6500_readGyro_Acc(&gyro[0],&acc[0]);
for(int i=0;i<3;i++)
{
values[i]=((float) gyro[i])/16.4f; //gyro range +-2000; adc accuracy 2^16=65536; 65536/4000=16.4;
values[3+i]=(float) acc[i];
}
}
然后编写函数实现计算姿态角的功能,使用四元数计算姿态角的公式在理论分析中推导:
其中α为绕x轴旋转角即roll,β为绕y轴旋转角即pitch,γ为绕z轴旋转角即yaw。a,b,c,d即q0,q1,q2,q3.
//arcsin函数
float safe_asin(float v)
{
if (isnan(v)) {
return 0.0f;
}
if (v >= 1.0f) {
return PI/2;
}
if (v <= -1.0f) {
return -PI/2;
}
return asin(v);
}
void IMU_Update(void)
{
static float q[4];
float Values[6];
Get_IMU_Values(Values);
//change angle to radian,the calculate the imu with Mahony
MahonyAHRSupdateIMU(Values[0] * PI/180, Values[1] * PI/180, Values[2] * PI/180,
Values[3], Values[4], Values[5]);
//save Quaternion
q[0] = q0;
q[1] = q1;
q[2] = q2;
q[3] = q3;
imu.ax = Values[3];
imu.ay = Values[4];
imu.az = Values[5];
imu.Pitch_v = Values[0];
imu.Roll_v = Values[1];
imu.Yaw_v = Values[2];
//calculate the imu angle with quaternion
imu.Roll = (atan2(2.0f*(q[0]*q[1] + q[2]*q[3]),1 - 2.0f*(q[1]*q[1] + q[2]*q[2])))* 180/PI;
imu.Pitch = -safe_asin(2.0f*(q[0]*q[2] - q[1]*q[3]))* 180/PI;
imu.Yaw = -atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3 * q3 + 1)* 180/PI; // yaw
}
代码中MahonyAHRSupdateIMU()函数实现的就是四元数的更新算法。
逻辑上,首先用加速度计校准陀螺仪,方式是通过计算当前四元数姿态下的重力分量,与加速度计的重力分量作叉积,得到误差。 对误差作P(比例)和I(积分)运算后加到陀螺仪角速度上。最终由角速度计算新的四元数。
使用到的公式有:
四元数重力分量计算:
四元数旋转矩阵的转置表示:从地理坐标系转换到机体坐标系的旋转。重力向量设为[0,0,1],与四元数旋转矩阵的转置矩阵相乘,表示机体坐标系下的重力分量。
所以由四元数得到的机体坐标系下的重力向量为: 由于加速度计测的是在机体坐标系下的重力向量,故将两个向量作叉积,即可得到他们之间的误差。
四元数更新方程:
代码中的 sampleFreq 即执行姿态解算的频率,这里用定时器,以500HZ的频率调用get_angle();
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float halfvx, halfvy, halfvz; //1/2 重力分量
float halfex, halfey, halfez; //1/2 重力误差
float qa, qb, qc;
//加速度数据有效时才进行校准
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
//对加速度数据归一化
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// 由四元数计算重力分量
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;
// 将四元数重力分量 与 加速度计重力分量 作叉积 得到误差
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
// 使用积分?
if(twoKi > 0.0f) {
//对误差作积分
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
//反馈到角速度
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// 对误差作比例运算并反馈
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// 计算1/2 dt
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
// 更新四元数
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// 四元数归一化
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
项目地址:https://gitee.com/killerp/mpu6050_-mahony,下载代码,并将include和src目录下的文件复制到你的项目。
首先需要确保iic驱动能正常使用,如果使用的是HAL库,需要定义宏 IMU_USE_HAL
。
将代码中的gpio引脚修改成你的项目的iic引脚。例如,本项目使用stm32的gpio模拟iic,其实现是在myiic.h和myiic.c,需要修改如下:
本例子使用GPIOB_12作为SDA,GPIOB_13作为SCL
myiic.h:
//change gpio
#define IIC_SCL_GPIO_Port GPIOB
#define IIC_SDA_GPIO_Port GPIOB
#define IIC_SCL_Pin_Num 13
#define IIC_SDA_Pin_Num 12
myiic.c:
修改对应的IIC_Init()的时钟相关函数
#ifndef IMU_USE_HAL
void IIC_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
// fix RCC_AHB1Periph_GPIOB
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
GPIO_InitStructure.GPIO_Pin = IIC_SCL_Pin;
GPIO_Init(IIC_SCL_GPIO_Port, &GPIO_InitStructure);
// fix RCC_AHB1Periph_GPIOB
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE);
GPIO_InitStructure.GPIO_Pin = IIC_SDA_Pin;
GPIO_Init(IIC_SDA_GPIO_Port, &GPIO_InitStructure);
IIC_SCL(1);
IIC_SDA(1);
}
#else
void IIC_Init(void)
{
GPIO_InitTypeDef GPIO_Initure;
GPIO_Initure.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_Initure.Pull = GPIO_PULLUP;
GPIO_Initure.Speed = GPIO_SPEED_HIGH;
// fix __HAL_RCC_GPIOB_CLK_ENABLE
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_Initure.Pin = IIC_SCL_Pin;
HAL_GPIO_Init(IIC_SCL_GPIO_Port, &GPIO_Initure);
// fix __HAL_RCC_GPIOB_CLK_ENABLE
__HAL_RCC_GPIOB_CLK_ENABLE();
GPIO_Initure.Pin = IIC_SDA_Pin;
HAL_GPIO_Init(IIC_SDA_GPIO_Port, &GPIO_Initure);
IIC_SCL(1);
IIC_SDA(1);
}
注意其中的delay.h
是需要你自己实现延时函数。
在主函数中执行下面函数,初始化MPU6050及其数据
if(IMU_Init()!=0)
{
printf("MPU6050 Init fail\r\n");
return -1;
}
然后设置一个定时器,令其产生周期性中断,中断产生的频率要和Mahony.h的宏一样:
#define IMU_Update_Freq 100.0f //frequency in Hz must equal to the frequency of IMU_Update()
TIM3_Int_Init(100-1,8400-1); //10ms = (1000*840)/84 us = 10ms == 100hz = IMU_Update_Freq
在定时器3中断处理函数中执行IMU_Update()更新姿态角:
void TIM3_IRQHandler(void)
{
if(TIM_GetITStatus(TIM3,TIM_IT_Update)==SET)
{
IMU_Update();
}
TIM_ClearITPendingBit(TIM3,TIM_IT_Update);
}
假设出现了角度不断增加的情况,请查看角速度值是否为0,若明显大于0,可能是MPU6500_Init_Offset()函数得到的偏移值不准确。
由于加速度计对水平方向的旋转无能为力,故用此程序得到的yaw角数据会一直漂移,无法得到校准;通常的解决方法是增加一个磁场传感器,来获得一个准确的水平方向角来校准陀螺仪的漂移。MPU6050支持扩展一个IIC接口到磁场传感器,可通过配置MPU6050的IIC MASTER 来读取磁场传感器的数据。
在Mahony中提供了包含磁场数据的融合函数:
void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz);
此处可能存在不合适展示的内容,页面不予展示。您可通过相关编辑功能自查并修改。
如您确认内容无涉及 不当用语 / 纯广告导流 / 暴力 / 低俗色情 / 侵权 / 盗版 / 虚假 / 无价值内容或违法国家有关法律法规的内容,可点击提交进行申诉,我们将尽快为您处理。