新闻中心

EEPW首页 > 嵌入式系统 > 设计应用 > 四旋翼飞行器的飞控实现

四旋翼飞行器的飞控实现

作者:时间:2017-01-06来源:网络收藏

  I2C_SendByte((u8) REG_Address); 

本文引用地址:http://www.eepw.com.cn/article/201701/342563.htm

  I2C_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,也就是说,我的程序循环一次0.005s,一般来说,采样率高点没问题,别比执行一次闭环控制的周期长就行了;

  陀螺仪量程±2000°/s,加速计量程±2g, 量程越大,取值越不精确;

  这里注意,由于我们没有采用磁力计,而陀螺仪存在零偏,所以最终在yaw方向上没有绝对的参考系,不能建立绝对的地理坐标系,这样最好的结果也仅仅是在yaw上存在缓慢漂移。

  3.互补滤波;

  融合时,陀螺仪的积分运算很大程度上决定了的瞬时运动情况,而重力加速计通过长时间的累积不断矫正陀螺仪产生的误差,最终得到准确的机体姿态。

  这里我们采用Madgwick提供的UpdateIMU算法来得到姿态角所对应的四元数,之后只需要经过简单运算便可转换为实时欧拉角。感谢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.获取期望姿态;

  也就是遥控部分了,让用户介入控制。

  本着拿来主义的原则,用上”圆点博士开源项目”提供的安卓的开源蓝牙控制端。

   

 

  圆点博士给出了数据包格式,同过HC-06蓝牙模块接连到串口1,再无线连接到控制端,这样我们就可以获得控制端不断发送的数据包了,并实时更新期望姿态角,这里只需要注意输出的姿态角和实时姿态角方向一致以及数据包的校验就行了。

  5.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计算完成之后,便可以通过自带的定时资源很容易的调制出四路pwm信号,采用的电调pwm格式为50Hz,高电平持续时间0.5ms-2.5ms;

  我以1.0ms-2.0ms为每个电机的油门行程,这样,1ms的宽度均匀的对应电调的从最低到最高转速。

  至此,一个用stm32和mpu6050搭建的飞控系统就算实现了。

  剩下的调试PID参数了。


上一页 1 2 下一页

关键词: STM32 飞行器

评论


相关推荐

技术专区

关闭