尝试制作这个四旋翼飞控的过程,感触颇多,整理了思绪之后,把重要的点一一记下来;
这个飞控是基于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算法来得到姿态角所对应的四元数,之后只需要经过简单运算便可转换为实时欧拉角。
#define sampleFreq 512.0f // sample frequency in Hz#define betaDef 0.1f // 2 * proportional gain
volatile float beta = betaDef;
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
float invSqrt(float x);
void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Auxiliary variables to avoid repeated arithmetic
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_4q0 = 4.0f * q0;
_4q1 = 4.0f * q1;
_4q2 = 4.0f * q2;
_8q1 = 8.0f * q1;
_8q2 = 8.0f * q2;
q0q0 = q0 * q0;
q1q1 = q1 * q1;
q2q2 = q2 * q2;
q3q3 = q3 * q3;
// Gradient decent algorithm corrective step
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);
// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}
float invSqrt(float x) {
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
return y;
}
3.获取期望姿态;
也就是遥控部分了,让用户介入控制。
本着拿来主义的原则,用上”圆点博士开源项目”提供的安卓的开源蓝牙控制端。
圆点博士给出了数据包格式,同过HC-06蓝牙模块接连到STM32串口1,再无线连接到控制端,这样我们就可以获得控制端不断发送的数据包了,并实时更新期望姿态角,这里只需要注意输出的姿态角和实时姿态角方向一致以及数据包的校验就行了。
4.PID控制算法;
由于简单的线性控制不可能满足四轴飞行器这个灵敏的系统,引入PID控制器来更好的纠正系统。
简介:PID实指“比例proportional”、“积分integral”、“微分derivative”,这三项构成PID基本要素。每一项完成不同任务,对系统功能产生不同的影响。
以Pitch为例:
error为期望角减去实时角度得到的误差;
iState为积分i参数对应累积过去时间里的误差总和;
if语句限定iState范围;
微分d参数为当前姿态减去上次姿态,估算速度;
总调整量为p,i,d三者之和;
这样,P代表控制系统的响应速度,越大,响应越快。
I,用来累积过去时间内的误差,修正P无法达到的期望姿态值(静差);
D,加强对机体变化的快速响应,对P有抑制作用。
PID各参数的整定需要综合考虑控制系统的各个方面,才能达到最佳效果。
输出PWM信号:
PID计算完成之后,便可以通过STM32自带的定时资源很容易的调制出四路pwm信号,采用的电调pwm格式为50Hz,高电平持续时间0.5ms-2.5ms;
至此,一个用stm32和mpu6050搭建的飞控系统已经实现。
剩下的就是痛苦并无止境的调试PID参数了。
30