移植DMP到MPU6050获取欧拉角 |
您所在的位置：网站首页 › 欧拉视频放大怎么关闭 › 移植DMP到MPU6050获取欧拉角 |
移植DMP到MPU6050获取欧拉角
|
文章目录
1、较浅显的姿态解算介绍2、姿态解算是怎么来的1、加速度反求2、角速度积分
3、DMP介绍4、DMP移植5、源码
经过上一节的介绍，我们可以读出 MPU6050的加速度传感器和角速度传感器的原始数据。不过这些原始数据，对我们来说，用处不大，我们期望得到的是姿态数据，也就是欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。有了这三个角，我们就可以得到当前物体的姿态，这才是我们想要的结果。 1、较浅显的姿态解算介绍在飞行器中，飞行姿态是非常重要的参数，以飞机自身的中心建立坐标系，当飞机绕坐标轴旋转的时候，会分别影响偏航角、横滚角及俯仰角，坐标系描述如下所示： 我们的任务其实就是把这些角度给他解算出来，这里我们肯定就是基于我们之前用陀螺仪检测到的加速度，角速度来进行解算的，但是加速度和角速度不能之间变成我们需要的这些角度，所以下面就来说下到底是怎么变成我们的欧拉角的。 1、比较直观的角度检测器就是陀螺仪了 它可以检测物体绕坐标轴转动的“角速度”，如同将速度对时间积分可以求出路程一样，将角速度对时间积分就可以计算出旋转的“角度”，由于陀螺仪测量角度时使用积分，会存在积分误差，若积分时间 Dt 越小，误差就越小。这十分容易理解，例如计算路程时，假设行车时间为 1 小时，我们随机选择行车过程某个时刻的速度 Vt 乘以 1 小时，求出的路程误差是极大的，因为行车的过程中并不是每个时刻都等于该时刻速度的，如果我们每 5 分钟检测一次车速，可得到 Vt1、 Vt2、Vt3-Vt12 这 12 个时刻的车速，对各个时刻的速度乘以时间间隔(5 分钟)，并对这 12 个结果求和，就可得出一个相对精确的行车路程了，不断提高采样频率，就可以使积分时间 Dt 变小，降低误差。 但是我们要注意一点啊，积分是会有累积误差的，这个在PID里面我们也做了很多了解，这个是肯定的，所以我们如果长时间用角速度积分来计算欧拉角肯定是不行的。2、使用加速度计来获取欧拉角 如图所示水平仪，在重力作用下气泡会运动，在电子设备中，一般使用加速度传感器来检测倾角，它通过检测器件在各个方向的形变情况而采样得到受力数据，根据 F=ma 转换，传感器直接输出加速度数据，因而被称为加速度传感器。由于地球存在重力场，所以重力在任何时刻都会作用于传感器，当传感器静止的时候(实际上加速度为 0)，传感器会在该方向检测出加速度 g，不能认为重力方向测出的加速度为 g，就表示传感器在该方向作加速度为 g 的运动。  这个原理的缺点就很明显了，检测不了yaw轴啊，因为他是根据重力来推导的，yaw轴不受重力影响。，但是这个不会有累积误差，因为重力总是标准的，他也不存在积分得过程，所以综合来看，实际过程中我们来测量欧拉角一般是融合的方式，假如我们同时使用这两种传感器，并设计一个滤波算法，当物体处于静止状态时，增大加速度数据的权重，当物体处于运动状时， 增大陀螺仪数据的权重，从而获得更准确的姿态数据。
2、姿态解算是怎么来的
1、加速度反求
首先mpu6050本身有一个坐标系：（注意6轴陀螺仪测量的是加速度和角速度） 还有一个数据就是可以通过角速度积分就可以获取角度了，但是角速度积分会存在积分的通病，就是会产生累计误差，从而造成偏移，因此在水平方向就会产生一个误差，这个误差是六轴传感器无法避免的。 3、DMP介绍DMP就是MPU6050内部的运动引擎，全称Digital Motion Processor，直接输出四元数，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。Motion Driver是Invensense针对其运动传感器的软件包，并非全部开源，核心的算法部分是针对ARM处理器和MSP430处理器编译成了静态链接库，适用于MPU6050、MPU6500、MPU9150、MPU9250等传感器。 使用 MPU6050 的 DMP 输出的四元数是 q30 格式的，也就是浮点数放大了 2 的 30 次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以 2 的 30 次方，然后再进行计算，计算公式为： q0=quat[0] / q30; //q30 格式转换为浮点数 q1=quat[1] / q30; q2=quat[2] / q30; q3=quat[3] / q30;其中 quat[0]~ quat[3]是 MPU6050 的 DMP 解算后的四元数， q30 格式，所以要除以一个 2的 30 次方，其中 q30 是一个常量： 1073741824，即 2 的 30 次方，然后带入公式，计算出欧拉角。上述计算公式的 57.3 是弧度转换为角度，即 180/π，这样得到的结果就是以度（°）为单位的。 pitch = asin(-2 * q1 * q3 + 2 * q0* q2) roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1) yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) 4、DMP移植这里我看了下，觉得正点原子写得好，所以我决定抄正点原子的！！！
mpu6050.c /* * mpu6050.c * * Created on: Mar 5, 2022 * Author: LX */ #include "mpu6050.h" #include "../DMP/inv_mpu.h" #include "../DMP/inv_mpu_dmp_motion_driver.h" #include extern I2C_HandleTypeDef hi2c1; IMU_Parameter IMU_Data; #define MPU_ADDR 0xD0 float gyrox, gyroy, gyroz, accelx, accely, accelz, temp; uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data) { if(HAL_I2C_Mem_Write(&hi2c1,MPU_ADDR,reg,1,&data,1,0xff) == HAL_OK) return 0; else return 1; } uint8_t MPU_Read_Byte(uint8_t reg) { if(HAL_I2C_Mem_Read(&hi2c1, 0xD1, reg, 1, ®, 1, 0xff) == HAL_OK) return 0; else return 1; } //设置MPU6050陀螺仪传感器满量程范围 //fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps //返回值:0,设置成功 // 其他,设置失败 uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr) { return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr uint8_t data=0; if(lpf>=188)data=1; else if(lpf>=98)data=2; else if(lpf>=42)data=3; else if(lpf>=20)data=4; else if(lpf>=10)data=5; else data=6; return MPU_Write_Byte(MPU_CFG_REG,data);//设置数字低通滤波器 } //设置MPU6050的采样率(假定Fs=1KHz) //rate:4~1000(Hz) //返回值:0,设置成功 // 其他,设置失败 uint8_t MPU_Set_Rate(uint16_t rate) { uint8_t data; if(rate>1000)rate=1000; if(rate MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01); //设置CLKSEL,PLL X轴为参考 MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00); //加速度与陀螺仪都工作 MPU_Set_Rate(50); //设置采样率为50Hz }else return 1; return 0; } void MPU6050_GET_Data(void) { uint8_t buf[14]={0}; HAL_I2C_Mem_Read(&hi2c1,MPU_ADDR,MPU_ACCEL_XOUTH_REG,1,buf,14,1000); accelx = (float) (((int16_t) (buf[0] unsigned short b; if (row[0] > 0) b = 0; else if (row[0] 0) b = 1; else if (row[1] 0) b = 2; else if (row[2] float sens; unsigned short accel_sens; mpu_get_gyro_sens(&sens); gyro[0] = (long)(gyro[0] * sens); gyro[1] = (long)(gyro[1] * sens); gyro[2] = (long)(gyro[2] * sens); dmp_set_gyro_bias(gyro); mpu_get_accel_sens(&accel_sens); accel[0] *= accel_sens; accel[1] *= accel_sens; accel[2] *= accel_sens; dmp_set_accel_bias(accel); return 0; }else return 1; } //陀螺仪方向控制 uint8_t inv_orientation_matrix_to_scalar(const signed char *mtx) { unsigned short scalar; scalar = inv_row_2_scale(mtx); scalar |= inv_row_2_scale(mtx + 3) res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器 if(res)return 1; res=mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);//设置FIFO if(res)return 2; res=mpu_set_sample_rate(DEFAULT_MPU_HZ); //设置采样率 if(res)return 3; res=dmp_load_motion_driver_firmware(); //加载dmp固件 if(res)return 4; res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向 if(res)return 5; res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP| //设置dmp功能 DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO| DMP_FEATURE_GYRO_CAL); if(res)return 6; res=dmp_set_fifo_rate(DEFAULT_MPU_HZ); //设置DMP输出速率(最大不超过200Hz) if(res)return 7; res=run_self_test(); //自检 if(res)return 8; res=mpu_set_dmp_state(1); //使能DMP if(res)return 9; } return 0; } //得到dmp处理后的数据 //pitch:俯仰角 精度:0.1° 范围:-90.0° +90.0° //roll:横滚角 精度:0.1° 范围:-180.0° +180.0° //yaw:航向角 精度:0.1° 范围:-180.0° +180.0° //返回值:0,正常 // 其他,失败 uint8_t mpu_dmp_get_data(IMU_Parameter *IMU_Data) { float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f; unsigned long sensor_timestamp; short gyro[3], accel[3], sensors; unsigned char more; long quat[4]; if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1; if(sensors&INV_WXYZ_QUAT) { q0 = quat[0] / q30; //q30格式转换为浮点数 q1 = quat[1] / q30; q2 = quat[2] / q30; q3 = quat[3] / q30; //计算得到俯仰角/横滚角/航向角 IMU_Data->Angle_X = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch IMU_Data->Angle_Y = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll IMU_Data->Angle_Z = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw }else return 2; return 0; } uint8_t HAL_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data) { HAL_I2C_Mem_Write(&hi2c1, ((slave_addr float Accel_X; float Accel_Y; float Accel_Z; float offset_Accel_X; float offset_Accel_Y; float offset_Accel_Z; float Gyro_X; float Gyro_Y; float Gyro_Z; float offset_Gyro_X; float offset_Gyro_Y; float offset_Gyro_Z; float Angle_X; float Angle_Y; float Angle_Z; float Temp; }IMU_Parameter; uint8_t HAL_i2c_write(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data); uint8_t HAL_i2c_read(unsigned char slave_addr, unsigned char reg_addr, unsigned char length, unsigned char const *data); uint8_t MPU6050_Init(void); void MPU6050_GET_Data(void); uint8_t mpu_dmp_init(void); uint8_t mpu_dmp_get_data(IMU_Parameter *IMU_Data); #endif /* MPU6050_H_ */如果需要移植文件的话我放到gitee了，可以自行查看：lx外设库 |
当传感器的正方向 Z 轴垂直指向天空时， 由于此时受到地球重力的作用，此时加速度计 Z 轴的读数应为正， 而且理想情况下应为g(这是地球的重力造成的加速度)，下面来设想我们的陀螺仪角度发生了偏转： 
从上面可以看到，重力产生的加速度将会分解为两个方向，因此我们可以得到： 
其中ACC_Y还有ACC_Z是已知量，因此可以反求我们需要的角度： 
上面是求x轴方向时的角度，我们也可以来求y方向的，也是同理，如果都不是的话也只是两个方向的投影角度了，综合来看两个方向的欧拉角如下所示： 
但是这样的方法会有两个缺点：
然后我们找到正点原子的这个放DMP的文件夹，把这里的文件都复制到我们的工程里面（注意可能会乱码，因为是GBK，然后我用的是UTF-8的编码，不过不影响使用） 
这里因为单片机的不同，我们需要修改几个宏参数 
其中这里面iic的发送和读取函数改用硬件iic如下所示： 
加入DMP初始化函数 
加入DMP读数数据函数，这里我们获取到的为四元数，将四元数按照我们上面提到的解算函数解算为欧拉角 
之后就可以在主函数中进行调用了 
之后我们将程序下载到开发板中就可以看到数据了，移植成功！！！！ 
【本文地址】
公司简介
联系我们
今日新闻 |
点击排行 |
|
推荐新闻 |
图片新闻 |
|
专题文章 |