|
- UID
- 1029342
- 性别
- 男
|
-------------------------------------------------------------------------------------------------------------------
尝试制作这个四旋翼飞控的过程，感触颇多，整理了思绪之后，把重要的点一一记下来；
这个飞控是基于STM32，整合了MPU6050，即陀螺仪和重力加速计，但没有融合电子罗盘；
另外，四旋翼飞行器的运动方式请百度百科，不太复杂，具体不再赘述；
首先来看看飞控程序的控制流程：
这大概就是程序的控制流程了。
有几个点：
1.使用的STM32单片机以i2c通信方式获取mpu6050传感器的初始数据；
2.获得数据后采用互补滤波的方式得到飞行器的实时姿态；
3.与此同时，从HC06蓝牙模块接收到上位机发送的数据，并解算，得到期望姿态；
4.之后因实时姿态和期望姿态作差，差值送入PID闭环控制器；
1.读取mpu6050初值；
采用GPIO模拟i2c通信的方式，根据mpu6050手册的各寄存器地址，读取到了重力加速计和陀螺仪的各分量;
传感器采样率设置为200Hz,这个值是因为我电调频率为200Hz，也就是说，我的程序循环一次0.005s,一般来说，采样率高点没问题，别比执行一次闭环控制的周期长就行了；
陀螺仪量程±2000°/s，加速计量程±2g， 量程越大，取值越不精确；
这里注意，由于我们没有采用磁力计，而陀螺仪存在零偏，所以最终在yaw方向上没有绝对的参考系，不能建立绝对的地理坐标系，这样最好的结果也仅仅是在yaw上存在缓慢漂移。
2.互补滤波；
融合时，陀螺仪的积分运算很大程度上决定了飞行器的瞬时运动情况，而重力加速计通过长时间的累积不断矫正陀螺仪产生的误差，最终得到准确的机体姿态。
这里我们采用Madgwick提供的UpdateIMU算法来得到姿态角所对应的四元数，之后只需要经过简单运算便可转换为实时欧拉角。感谢Madgwick大大为开源做出的贡献。
view sourceprint?
01.1 #define sampleFreq 512.0f // sample frequency in Hz
02.2 #define betaDef 0.1f // 2 * proportional gain
03.3
04.4
05.5 volatile float beta = betaDef;
06.6 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
07.7
08.8 float invSqrt(float x);
09.9
10.10 void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
11.11 float recipNorm;
12.12 float s0, s1, s2, s3;
13.13 float qDot1, qDot2, qDot3, qDot4;
14.14 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
15.15
16.16 // Rate of change of quaternion from gyroscope
17.17 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
18.18 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
19.19 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
20.20 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
21.21
22.22 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
23.23 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
24.24
25.25 // Normalise accelerometer measurement
26.26 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
27.27 ax *= recipNorm;
28.28 ay *= recipNorm;
29.29 az *= recipNorm;
30.30
31.31 // Auxiliary variables to avoid repeated arithmetic
32.32 _2q0 = 2.0f * q0;
33.33 _2q1 = 2.0f * q1;
34.34 _2q2 = 2.0f * q2;
35.35 _2q3 = 2.0f * q3;
36.36 _4q0 = 4.0f * q0;
37.37 _4q1 = 4.0f * q1;
38.38 _4q2 = 4.0f * q2;
39.39 _8q1 = 8.0f * q1;
40.40 _8q2 = 8.0f * q2;
41.41 q0q0 = q0 * q0;
42.42 q1q1 = q1 * q1;
43.43 q2q2 = q2 * q2;
44.44 q3q3 = q3 * q3;
45.45
46.46 // Gradient decent algorithm corrective step
47.47 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
48.48 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
49.49 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
50.50 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
51.51 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
52.52 s0 *= recipNorm;
53.53 s1 *= recipNorm;
54.54 s2 *= recipNorm;
55.55 s3 *= recipNorm;
56.56
57.57 // Apply feedback step
58.58 qDot1 -= beta * s0;
59.59 qDot2 -= beta * s1;
60.60 qDot3 -= beta * s2;
61.61 qDot4 -= beta * s3;
62.62 }
63.63
64.64 // Integrate rate of change of quaternion to yield quaternion
65.65 q0 += qDot1 * (1.0f / sampleFreq);
66.66 q1 += qDot2 * (1.0f / sampleFreq);
67.67 q2 += qDot3 * (1.0f / sampleFreq);
68.68 q3 += qDot4 * (1.0f / sampleFreq);
69.69
70.70 // Normalise quaternion
71.71 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
72.72 q0 *= recipNorm;
73.73 q1 *= recipNorm;
74.74 q2 *= recipNorm;
75.75 q3 *= recipNorm;
76.76 }
77.77
78.78
79.79 float invSqrt(float x) {
80.80 float halfx = 0.5f * x;
81.81 float y = x;
82.82 long i = *(long*)&y;
83.83 i = 0x5f3759df - (i>>1);
84.84 y = *(float*)&i;
85.85 y = y * (1.5f - (halfx * y * y));
86.86 return y;
87.87 }
四元数转欧拉角的算法可参考 http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html |
|
