查看: 9535|回复: 12

[原创DIY] 圆点博士小四轴算法快速入门

  [复制链接]

107

主题

11

好友

1583

积分

版主

Rank: 7Rank: 7Rank: 7

  • TA的每日心情
    奋斗
    2013-7-4 23:49
  • 签到天数: 21 天

    [LV.4]偶尔看看III

    论坛版主职务勋章

    发表于 2014-5-29 13:24:52 |显示全部楼层
    飞控的算法代码一般包括下面三个部分:滤波,姿态,PID
    1,滤波可以用互补滤波来实现,互补滤波的资料很多,大家随便就能找到。基本公式是:

    互补滤波

    互补滤波



    2,滤波完就是四元数拉。直接用老外Madgwick的IMU就可以。超级简单
    unsigned char BS004_IMU_Update(float ax,float ay,float az,float gx,float gy,float gz)
    {
            float norm;
            float vx, vy, vz;
            float ex, ey, ez;  
      //       
            //圆点博士:四元数乘法运算
            float q0q0 = q0 * q0;                                                       
            float q0q1 = q0 * q1;
            float q0q2 = q0 * q2;
            float q1q1 = q1 * q1;
            float q1q3 = q1 * q3;
            float q2q2 = q2 * q2;
            float q2q3 = q2 * q3;
            float q3q3 = q3 * q3;
            //       
            //圆点博士:归一化处理
            norm = sqrt(ax*ax + ay*ay + az*az);     
            if(norm==0) return 0;       
            ax = ax / norm;
            ay = ay / norm;
            az = az / norm;   
      //       
            //圆点博士:建立小四轴坐标系       
            vx = 2*(q1q3 - q0q2);                                                               
            vy = 2*(q0q1 + q2q3);
            vz = q0q0 - q1q1 - q2q2 + q3q3;
            //
            //圆点博士:坐标系和重力叉积运算
            ex = (ay*vz - az*vy);                                                               
            ey = (az*vx - ax*vz);
            ez = (ax*vy - ay*vx);
            //
            //圆点博士:比例运算
            exInt = exInt + ex*bs004_quad_Ki;
            eyInt = eyInt + ey*bs004_quad_Ki;
            ezInt = ezInt + ez*bs004_quad_Ki;
            //
            //圆点博士:陀螺仪融合
            gx = gx + bs004_quad_Kp*ex + exInt;
            gy = gy + bs004_quad_Kp*ey + eyInt;
            gz = gz + bs004_quad_Kp*ez + ezInt;
            //
            //圆点博士:整合四元数率
            q0 = q0 + (-q1*gx - q2*gy - q3*gz)*bs004_quad_halfT;
            q1 = q1 + (q0*gx + q2*gz - q3*gy)*bs004_quad_halfT;
            q2 = q2 + (q0*gy - q1*gz + q3*gx)*bs004_quad_halfT;
            q3 = q3 + (q0*gz + q1*gy - q2*gx)*bs004_quad_halfT;  
            //
            //圆点博士:归一化处理
            norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
            if(norm==0) return 0;       
            q0 = q0 / norm;
            q1 = q1 / norm;
            q2 = q2 / norm;
            q3 = q3 / norm;
            //
            //圆点博士:欧拉角转换
            bs004_imu_roll=asin(-2*q1q3 + 2*q0q2)*57.30f;
      bs004_imu_pitch=atan2(2*q2q3 + 2*q0q1, -2*q1q1-2*q2q2 + 1)*57.30f;
      bs004_imu_yaw=bs004_imu_yaw-gz*bs004_mpu6050_gyro_scale;
            //
            return 1;       
    }


    3, PID的代码其实也很简单,主要是要了解其中的原理,才能更好地调整参数。为了方便新手们理解,楼主建立了一个数学模型来让大家了解。(只针对新手,老手就算了)

    ========圆点博士小四轴之PID控制模式分析=======
    PID控制的P是Proportional的缩写, 是比例的意思,I是Integral的缩写,是积分的意思,D是Derivative的缩写,是微分的意思。所以,PID就是我们常说的比例,积分,微分控制。
    我们首先来看一个PID控制模型曲线图:
    该图包含了比例控制,比例+积分控制,比较+积分+微分控制的电机响应图的对比。

    PID模型

    PID模型

    下面我们对曲线进行具体分析:
    PID中的比例控制是最容易理解的,比例控制就是把角度的误差乘以一个常数作为输出驱动。假定我们有一个理想模型的电机,1V电压的变化会带来小四轴1度的角度改变。假定现在电机控制电压是5V,小四轴在某一轴上的偏角是5度,目标角度是100度。我们把当前的电压量定义为Vin,把输出控制量定义为Vout。假定P等于0.2,那么比例控制的结果就是:
    第一次:Vout=Vin+(100-5)*P=5V+19V=24V,得到电机电压是24V,对应的小四轴角度是24度,距离目标角度的误差是100-24=76度。
    第二次:Vout=Vin+(100-24)*P=24V+15V=39V, 从而引起的角度是39度。
    我们看到,在这么的一个比例控制系统下,小四轴角度在慢慢地向目标角度靠近。

    PID中的积分控制就是把把所有角度误差相加起来,然后乘上一个常数作为输出驱动。在上述例子中,假定I=0.2, 我们来看看比例和积分控制同时起作用下的系统反应。
    第一次:Vout=Vin+(100-5)*P+(100-5)*I=5V+19V+19V=43V,这时候小四轴角度为43度。
    由于第一次控制前的误差是100-5=95,第二次控制前的误差是100-43=57,所以积分结果是152。
    第二次:Vout=Vin+(100-43)*P+((100-5)+(100-43))*I=43V+11V+30V=84V, 这时候小四轴角度变为84度。
    第三次:Vout=Vin+(100-84)*P+((100-5)+(100-43)+(100-84))*I=84+3V+33V=120V。这时小四轴角度变为120度。
    我们看到,在增加了积分控制后,小四轴角度在快速向目标角度靠近。

    PID中的微分控制就是把角度的变化乘上一个常数来作为电机驱动输出。在上述例子中,假定D=0.2, 我们来看看比例,积分和微分共同控制下的系统反应。假定第一次前,电机转速保持5转,那么第一次前的角度变化为0。
    第一次:Vout=Vin+(100-5)*P+(100-5)*I-(5-5)*D=5V+19V+19V-0V=43V,这时候小四轴角度为43度。
    和上一次相比,角度从5度变化到了43度,所以小四周角度变化是43-5=38度。
    第二次:Vout=Vin+(100-43)*P+((100-5)+(100-43))*I-(43-5)*D=43V+11V+30V-7V=77V, 这时候小四周角度77度。

    把上述的计算结果列出来,我们看到:

    PID计算

    PID计算


    从上面的数据,我们可以看到:
    1,单独比例控制的时候,数据慢慢接近目标 (图表中的红色线)
    2,加入积分控制之后,数据快速接近目标 (图表中的蓝色线)
    3,微分控制起到抑制变化的作用。(图表中的绿色线)

    有了这些理论基础,就可以写PID控制代码拉。

    ========圆点博士小四轴之PID控制代码分析=======
    在圆点博士小四轴2014版代码里,我们只使用到PD参数。
    首先我们来看PID中的比例控制。跟上一节模型提到的一样,比例是针对误差的控制。
    首先我们获取小四轴当前角度。
    bs004_angle_cur_pitch=bs004_imu_pitch;
    bs004_angle_cur_roll =bs004_imu_roll;
    把当前角度和目标角度相减,就可以得到角度偏差。
    bs004_angle_err_pitch=bs004_angle_cur_pitch-bs004_angle_target_pitch;
    bs004_angle_err_roll=bs004_angle_cur_roll-bs004_angle_target_roll;
    然后进行比例控制:
    bs004_fly_m1=bs004_fly_m1
    +bs004_pitch_p*bs004_angle_err_pitch
    -bs004_roll_p *bs004_angle_err_roll
    -bs004_yaw_p*bs004_angle_err_yaw;
    bs004_fly_m2=bs004_fly_m2
    -bs004_pitch_p*bs004_angle_err_pitch
    -bs004_roll_p *bs004_angle_err_roll
    +bs004_yaw_p*bs004_angle_err_yaw;

    bs004_fly_m3=bs004_fly_m3
    -bs004_pitch_p*bs004_angle_err_pitch
    +bs004_roll_p *bs004_angle_err_roll
    -bs004_yaw_p*bs004_angle_err_yaw;
    bs004_fly_m4=bs004_fly_m4
    +bs004_pitch_p*bs004_angle_err_pitch
    +bs004_roll_p *bs004_angle_err_roll
    +bs004_yaw_p*bs004_angle_err_yaw;

    在上一节模型中,我们提到PID中的微分控制针对的是角度变化而进行的控制。
    所以我们首先要得到当前角度和上一次角度的差异。
    bs004_angle_dif_pitch=bs004_angle_cur_pitch-bs004_angle_last_pitch;
    bs004_angle_dif_roll =bs004_angle_cur_roll-bs004_angle_last_roll;
    bs004_angle_dif_yaw  =bs004_angle_last_yaw-bs004_angle_cur_yaw;
    然后进行微分控制:
    bs004_fly_m1=bs004_fly_m1
    +bs004_pitch_d*bs004_angle_dif_pitch
    -bs004_roll_d *bs004_angle_dif_roll
    -bs004_yaw_d*bs004_angle_dif_yaw;
    bs004_fly_m2=bs004_fly_m2
    -bs004_pitch_d*bs004_angle_dif_pitch
    -bs004_roll_d *bs004_angle_dif_roll
    +bs004_yaw_d*bs004_angle_dif_yaw;
    bs004_fly_m3=bs004_fly_m3
    -bs004_pitch_d*bs004_angle_dif_pitch
    +bs004_roll_d *bs004_angle_dif_roll
    -bs004_yaw_d*bs004_angle_dif_yaw;
    bs004_fly_m4=bs004_fly_m4
    +bs004_pitch_d*bs004_angle_dif_pitch
    +bs004_roll_d *bs004_angle_dif_roll
    +bs004_yaw_d*bs004_angle_dif_yaw;

    从上面的描述我们可以看出,小四轴的PID控制还是比较简单的。





    回复

    使用道具 举报

    59

    主题

    18

    好友

    2977

    积分

    进士

    Rank: 5Rank: 5

  • TA的每日心情
    奋斗
    2016-9-9 09:13
  • 签到天数: 293 天

    [LV.8]以坛为家I

    爱板会员勋章 活跃会员勋章

    发表于 2014-5-29 16:31:10 |显示全部楼层
    必须顶一个~~~~~~~~~~~~·
    第五年了,这次一定要坚持到底,不留遗憾!!!
    回复

    使用道具 举报

    5

    主题

    5

    好友

    2284

    积分

    进士

    Rank: 5Rank: 5

  • TA的每日心情
    奋斗
    2016-5-9 16:36
  • 签到天数: 356 天

    [LV.8]以坛为家I

    活跃会员勋章 爱板会员勋章

    发表于 2014-5-29 21:05:07 |显示全部楼层
    博士 好高深啊 ,求一份源码 1185753569@qq.com
    从不依靠,从不寻找。
    非常沉默,非常骄傲。
    回复

    使用道具 举报

    21

    主题

    7

    好友

    2492

    积分

    进士

    Rank: 5Rank: 5

  • TA的每日心情
    奋斗
    2015-1-22 18:04
  • 签到天数: 189 天

    [LV.7]常住居民III

    发表于 2014-6-1 03:15:21 |显示全部楼层
    不错,学习了
    回复

    使用道具 举报

    19

    主题

    8

    好友

    1021

    积分

    进士

    Rank: 5Rank: 5

  • TA的每日心情
    奋斗
    2014-9-5 15:28
  • 签到天数: 67 天

    [LV.6]常住居民II

    发表于 2014-7-6 23:02:46 |显示全部楼层
    博士的原创整理必须顶
    回复

    使用道具 举报

    4

    主题

    4

    好友

    373

    积分

    秀才

    Rank: 3Rank: 3

  • TA的每日心情
    慵懒
    2016-1-6 12:39
  • 签到天数: 38 天

    [LV.5]常住居民I

    发表于 2014-7-20 13:22:53 |显示全部楼层
    不错哦,好好学习
    回复

    使用道具 举报

    1

    主题

    5

    好友

    209

    积分

    秀才

    Rank: 3Rank: 3

  • TA的每日心情

    2014-10-1 08:46
  • 签到天数: 50 天

    [LV.5]常住居民I

    发表于 2014-8-3 10:23:55 |显示全部楼层
    高端啊!必须顶一个
    回复

    使用道具 举报

    2

    主题

    0

    好友

    418

    积分

    秀才

    Rank: 3Rank: 3

  • TA的每日心情
    奋斗
    2015-1-9 12:49
  • 签到天数: 104 天

    [LV.6]常住居民II

    爱板会员勋章

    发表于 2014-8-5 16:39:38 |显示全部楼层
    PID讲的太好了,我看了好的资料都没看懂,还是这个比较直观,一下就明白了
    回复

    使用道具 举报

    0

    主题

    0

    好友

    20

    积分

    白丁

    Rank: 1

    该用户从未签到

    发表于 2015-5-16 19:43:27 |显示全部楼层
    简单明了,多谢。
    回复

    使用道具 举报

    0

    主题

    0

    好友

    8

    积分

    白丁

    Rank: 1

    该用户从未签到

    发表于 2015-5-24 17:54:02 |显示全部楼层
    终于找到你了,
    回复

    使用道具 举报

    您需要登录后才可以回帖 登录 | 立即注册

    关闭

    站长推荐上一条 /5 下一条


    手机版|爱板网 |网站地图  

    GMT+8, 2016-12-11 19:52 , Processed in 0.310211 second(s), 20 queries , Memcache On.

    苏公网安备 32059002001056号

    Powered by Discuz!

    回顶部