普通人类也能看懂的魔法daze⭐️

关于挂载17mmBB弹发射装置的(武装阿帕奇)四旋翼无人机その一 MPU6050

这一部分是无人机的技术难点之一

原理部分

MPU6050

MPU6050是世界首款集成式六轴运动跟踪设备,结合三轴陀螺仪、三轴加速度计,MPU6050还配有4x4x0.9mm的小尺寸DMP™(Digital Motion Processing™)(数字运动处理器)可以进行融合运动融合(MotionFusion™)解算,可以将处理器从繁琐解算姿态中释放出来。如果比较硬核的话当然可以自己进行运动融合解算。目前我发现的算法比较好的有AECF(自适应显性互补滤波算法)算法(这一部分之后再实现,先用好DMP就可以有不错的效果了)说实话AECF只比DMP好一点点而且还难,用DMP简单暴力,直接输出四元数

MPU6050采用I2C总线协议(I2C总线下次再写咕咕咕

三轴陀螺仪

三轴陀螺仪可以测量出无人机的角速度,从而解算出无人机的姿态。
具体方法是对角速度进行积分算出无人机角度,然而现实的各种噪声导致这种测量的精度不够,导致陀螺仪漂移的产生(而且温飘很严重)。因此与加速度计进行融合解算就很有必要了。

三轴加速度计

三轴加速度计通过测量受力来测量加速度。所以对于自由落体它测量不出加速度(这一点可能要注意一下)


四元数

通过DMP解算出来的姿态是用四元数来保存的。

为什么要用四元数这倒霉玩意???

好处太多了,全是好处。就是对人类不是很友好就是了。四元数无法直观进行理解,但是可以看看这个

四元数乘法

这些wiki上都有的

四元数与空间旋转

这些wiki上也有

不会写LaTex这部分好难写啊

总之
q是MPU6050的DMP解算出的四元数
然后p是一个纯四元数且代表坐标轴
带入公式后可以转化成类似于余弦矩阵的东西

然后转化成欧拉角就ojbk了


$\psi$ 代表yaw,相当于 z 轴

$\theta$ 代表pitch,相当于 y 轴

$\phi$ 代表row,相当于 x 轴



代码实现

四元数的获取


我们需要的是从FIFO中读取DMP的数据,我翻遍了MPU6050的Datasheet也没找到DMP的寄存器地址。最后还是上GitHub找到一个写了DMP的地址的库,看了几千行代码,费劲千辛万苦去找DMP的寄存器。然而后来扬神告诉我有官方的DMP驱动(滑稽抽烟流泪.jpg)。

此处引用了InvenSense的官方驱动,并且有官方pdf介绍这个库
主要是使用里面的inv_mpu.h 和 inv_mpu_dmp_motion_driver.h这两个库
这里我们基于这个库写了两函数。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
uint8_t mpu_dmp_init(void)
{
uint8_t res=0;
IIC_Init(); //初始化 IIC 总线
if(mpu_init()==0)
{
//初始化 MPU6050
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_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL |DMP_FEATURE_SEND_CAL_GYRO|DMP_FEATURE_GYRO_CAL); //设置 dmp 功能
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); //使能MPU
if(res)
return 9;
}
return 0;
}

此函数先初始化I2C接口,再初始化MPU,然后启用DMP模式。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
uint8_t mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
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;
*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;// roll
*yaw= atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;//yaw
}
else return 2;

return 0;
}

从FIFO中读取封包,并将四元数转化成欧拉角。

移植官方的库文件需要定义i2c_write,i2c_read,delay_ms,get_ms,reg_int_cb,labs,fabsf,min函数这些根据选用的
最后main函数里只要引用mpu_dmp_init函数和mpu_dmp_get_data函数就可以得到稳稳的数据了。
我是基于stm32f103的HAL库写的,由于篇幅原因,代码就不放出来了。

四元数转换成欧拉角

1
2
3
4
5
6
7
8
q0=quat[0] / q30; //q30 格式转换为浮点数
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;

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; //航向角

拿出这一部分代码分析一下
其中57.3是$180/\pi$ , 把弧度转化成角度
而q30是$2^{30}$,因为MPU中数据以q30格式保存,除以一个q30可以将读取的数据转化成浮点数


Author: Azusa
Link: http://azukatze.moe/2018/10/29/Quadrotor_drone/
Copyright Notice: All articles in this blog are licensed under CC BY-NC-SA 4.0 unless stating additionally.