四旋翼飛行器的飛控實現
I2C_SendByte((u8) REG_Address);
本文引用地址:http://m.butianyuan.cn/article/201701/342563.htmI2C_WaitAck();
I2C_Start();
I2C_SendByte(SlaveAddress+1);
I2C_WaitAck();
REG_data= I2C_RadeByte();
I2C_NoAck();
I2C_Stop();
return REG_data;
}
2.mpu6050;
然后用寫好的模擬i2c函數讀取mpu6050,根據mpu6050手冊的各寄存器地址,讀取到了重力加速計和陀螺儀的各分量;
傳感器采樣率設置為200Hz,這個值是因為我電調頻率為200Hz,也就是說,我的程序循環(huán)一次0.005s,一般來說,采樣率高點沒問題,別比執(zhí)行一次閉環(huán)控制的周期長就行了;
陀螺儀量程±2000°/s,加速計量程±2g, 量程越大,取值越不精確;
這里注意,由于我們沒有采用磁力計,而陀螺儀存在零偏,所以最終在yaw方向上沒有絕對的參考系,不能建立絕對的地理坐標系,這樣最好的結果也僅僅是在yaw上存在緩慢漂移。
3.互補濾波;
融合時,陀螺儀的積分運算很大程度上決定了飛行器的瞬時運動情況,而重力加速計通過長時間的累積不斷矯正陀螺儀產生的誤差,最終得到準確的機體姿態(tài)。
這里我們采用Madgwick提供的UpdateIMU算法來得到姿態(tài)角所對應的四元數,之后只需要經過簡單運算便可轉換為實時歐拉角。感謝Madgwick大大為開源做出的貢獻。
1 #define sampleFreq 512.0f // sample frequency in Hz
2 #define betaDef 0.1f // 2 * proportional gain
3
4
5 volatile float beta = betaDef;
6 volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
7
8 float invSqrt(float x);
9
10 void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
11 float recipNorm;
12 float s0, s1, s2, s3;
13 float qDot1, qDot2, qDot3, qDot4;
14 float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
15
16 // Rate of change of quaternion from gyroscope
17 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
18 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
19 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
20 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
21
22 // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
23 if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
24
25 // Normalise accelerometer measurement
26 recipNorm = invSqrt(ax * ax + ay * ay + az * az);
27 ax *= recipNorm;
28 ay *= recipNorm;
29 az *= recipNorm;
30
31 // Auxiliary variables to avoid repeated arithmetic
32 _2q0 = 2.0f * q0;
33 _2q1 = 2.0f * q1;
34 _2q2 = 2.0f * q2;
35 _2q3 = 2.0f * q3;
36 _4q0 = 4.0f * q0;
37 _4q1 = 4.0f * q1;
38 _4q2 = 4.0f * q2;
39 _8q1 = 8.0f * q1;
40 _8q2 = 8.0f * q2;
41 q0q0 = q0 * q0;
42 q1q1 = q1 * q1;
43 q2q2 = q2 * q2;
44 q3q3 = q3 * q3;
45
46 // Gradient decent algorithm corrective step
47 s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
48 s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
49 s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
50 s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
51 recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
52 s0 *= recipNorm;
53 s1 *= recipNorm;
54 s2 *= recipNorm;
55 s3 *= recipNorm;
56
57 // Apply feedback step
58 qDot1 -= beta * s0;
59 qDot2 -= beta * s1;
60 qDot3 -= beta * s2;
61 qDot4 -= beta * s3;
62 }
63
64 // Integrate rate of change of quaternion to yield quaternion
65 q0 += qDot1 * (1.0f / sampleFreq);
66 q1 += qDot2 * (1.0f / sampleFreq);
67 q2 += qDot3 * (1.0f / sampleFreq);
68 q3 += qDot4 * (1.0f / sampleFreq);
69
70 // Normalise quaternion
71 recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
72 q0 *= recipNorm;
73 q1 *= recipNorm;
74 q2 *= recipNorm;
75 q3 *= recipNorm;
76 }
77
78
79 float invSqrt(float x) {
80 float halfx = 0.5f * x;
81 float y = x;
82 long i = *(long*)&y;
83 i = 0x5f3759df - (i>>1);
84 y = *(float*)&i;
85 y = y * (1.5f - (halfx * y * y));
86 return y;
87 }
四元數轉歐拉角的算法可參考 http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html
4.獲取期望姿態(tài);
也就是遙控部分了,讓用戶介入控制。
本著拿來主義的原則,用上”圓點博士開源項目”提供的安卓的開源藍牙控制端。
圓點博士給出了數據包格式,同過HC-06藍牙模塊接連到STM32串口1,再無線連接到控制端,這樣我們就可以獲得控制端不斷發(fā)送的數據包了,并實時更新期望姿態(tài)角,這里只需要注意輸出的姿態(tài)角和實時姿態(tài)角方向一致以及數據包的校驗就行了。
5.PID控制算法;
由于簡單的線性控制不可能滿足四軸飛行器這個靈敏的系統(tǒng),引入PID控制器來更好的糾正系統(tǒng)。
簡介:PID實指“比例proportional”、“積分integral”、“微分derivative”,這三項構成PID基本要素。每一項完成不同任務,對系統(tǒng)功能產生不同的影響。
以Pitch為例:
error為期望角減去實時角度得到的誤差;
iState為積分i參數對應累積過去時間里的誤差總和;
if語句限定iState范圍,繁殖修正過度;
微分d參數為當前姿態(tài)減去上次姿態(tài),估算當前速度(瞬間速度);
總調整量為p,i,d三者之和;
這樣,P代表控制系統(tǒng)的響應速度,越大,響應越快。
I,用來累積過去時間內的誤差,修正P無法達到的期望姿態(tài)值(靜差);
D,加強對機體變化的快速響應,對P有抑制作用。
PID各參數的整定需要綜合考慮控制系統(tǒng)的各個方面,才能達到最佳效果。
輸出PWM信號:
PID計算完成之后,便可以通過STM32自帶的定時資源很容易的調制出四路pwm信號,采用的電調pwm格式為50Hz,高電平持續(xù)時間0.5ms-2.5ms;
我以1.0ms-2.0ms為每個電機的油門行程,這樣,1ms的寬度均勻的對應電調的從最低到最高轉速。
至此,一個用stm32和mpu6050搭建的飛控系統(tǒng)就算實現了。
剩下的調試PID參數了。
評論