大家分析分这个飞控控制程序 (amobbs.com 阿莫电子技术论坛) -

yufeng881005 发表于 2013-12-24 10:20:25

大家分析分这个飞控控制程序

板子是买网上的，第七实验室的，上面程序是移植MWC的，但是不知道自己PID参数调的不好还是怎么的，飞的很惨，后来接触了下APM（也是个皮毛），看他那个飞的效果很好，就看了一下程序，发现那个控制跟MWC还是有点不一样，他是有一个角速率PID内回路，外层角度回路，就试了一下，核心程序如下（在原来程序上改的），这个是角速率控制的核心程序，协商去感觉飞的还可以，至少比以前原始的MWC好，PID调试的时候，I，D设为0，P加一个数，然后一个手抓着，一个手打油们，感觉不怎么抖了就认为差不多了，只用P时候发现小油门还行，油门大了就抖（抖着抖着就掉下来了....摔了不少机架），后来加上D就好了....虽然飞的还凑合，但一直觉得还是没彻底弄明白，

   后一段贴的是角度PID，飞了几次也飞的很好，但又一次确突然抖了起来，越抖越厉害，然后直接掉下来......分析是当时没加积分和微分限幅那一项，突然来了一个异常数据，就发散了，不知道这么解释对不对，求大神分析，后来加上微分限幅的确没出现那种情况。

觉得这个参数调的差不多了，一直想弄个GPS悬停，可是一直没成功，买的第七实验室那个GPS程序，用的都是浮点数处理经纬度，MWC上用的是int32型的，怀疑是乘来乘去的把经度弄丢了....不知道到底是不是这个原因，有没有大神分享下GPS悬停的经验啊，感觉没加GPS回路这个四轴就是一个玩具....可惜本人能力实在有限，搞了很久还是搞不定....
               Desire_roll_rate=PID_ROLL /10;//遥控器指令+-500，除以10即期望角速率+-50度每秒
Desire_pitch_rate=PID_PITCH /10;
Desire_yaw_rate=PID_YAW /10;

Actual_roll_rate=IMU_GYROx/16.4;//MPU6050原始数据初一标度因子，转成度/秒
Actual_pitch_rate=IMU_GYROy/16.4;
Actual_yaw_rate=IMU_GYROz/16.4;

Roll_rate_error=Desire_roll_rate-Actual_roll_rate;//角速率误差
Pitch_rate_error=Desire_pitch_rate-Actual_pitch_rate;
Yaw_rate_error=Desire_yaw_rate-Actual_yaw_rate;

Roll_rate_error_I=Roll_rate_error_I+Roll_rate_error;
Math_fConstrain(Roll_rate_error_I,-50,+50);//限幅，这个限幅真不知道取多少合适
Pitch_rate_error_I=Pitch_rate_error_I+Pitch_rate_error;
Math_fConstrain(Pitch_rate_error_I,-50,+50);
Yaw_rate_error_I=Yaw_rate_error_I+Yaw_rate_error;
Math_fConstrain(Yaw_rate_error_I,-50,+50);

Roll_rate_error_D=Actual_roll_rate-Last_gx;//D项没有取误差微分，直接用角速率微分，不知道合理不合理
Roll_rate_error_D=Math_fConstrain(Roll_rate_error_D,-30,+30);
Pitch_rate_error_D=Actual_pitch_rate-Last_gy;
Pitch_rate_error_D=Math_fConstrain(Pitch_rate_error_D,-30,+30);
Yaw_rate_error_D=Actual_yaw_rate-Last_gz;
Yaw_rate_error_D=Math_fConstrain(Yaw_rate_error_D,-30,+30);

Roll_rate_error=Desire_roll_rate-Actual_roll_rate;
Pitch_rate_error=Desire_pitch_rate-Actual_pitch_rate;
Yaw_rate_error=Desire_yaw_rate-Actual_yaw_rate;

if(THROTTLE<1300)//这个地方这么写也不知道合理不合理
{
   Roll_rate_error_I=0;
   Pitch_rate_error_I=0;
   Yaw_rate_error_I=0;
}
PID_ROLL=Roll_rate_error*Stabilize_Roll.Kp+Roll_rate_error_D*Stabilize_Roll.Kd+Roll_rate_error_I*Stabilize_Roll.Ki;//+Roll_rate_error_I*Stabilize_Roll.Ki,
//+Roll_rate_error_D*Stabilize_Roll.Kd;

PID_PITCH=Pitch_rate_error*Stabilize_Pitch.Kp+Pitch_rate_error_D*Stabilize_Pitch.Kd+Pitch_rate_error_I*Stabilize_Pitch.Ki;//+Pitch_rate_error_I*Stabilize_Pitch.Ki,
//+Pitch_rate_error_D*Stabilize_Pitch.Kd;

PID_YAW=Yaw_rate_error*Stabilize_Yaw.Kp+Yaw_rate_error_D*Stabilize_Yaw.Kd+Yaw_rate_error_I*Stabilize_Yaw.Ki;//+Yaw_rate_error_I*Stabilize_Yaw.Ki,
//+Yaw_rate_error_D*Stabilize_Yaw.Kd;

Last_gx=IMU_GYROx;
Last_gy=IMU_GYROy;
Last_gz=IMU_GYROz;

角度控制回路：微分项直接用角速率输出的值，不知道这么做科学不科学.....
/**********由摇杆得到期望角度***********/
Desire_roll_angel=PID_ROLL/10+GPS_ROLL;
Desire_pitch_angel=PID_PITCH/10+GPS_PITCH;

/**********IMU实际角度************/
Actual_roll_angel=IMU_Roll;
Actual_pitch_angel=-IMU_Pitch;;

/*********角度误差************/
Roll_angel_error=Desire_roll_angel-Actual_roll_angel;
Pitch_angel_error=Desire_pitch_angel-Actual_pitch_angel;

/*********有角度误差得到期望角速率*******************/
Desire_roll_rate=Roll_angel_error*Level.Kp;
Desire_pitch_rate=Pitch_angel_error*Level.Kp;
/********偏航通道单独控制****************/
   Desire_yaw_rate=PID_YAW /10;
Actual_yaw_rate=IMU_GYROz/16.4;
Yaw_rate_error=Desire_yaw_rate-Actual_yaw_rate;
Yaw_rate_error_I=Yaw_rate_error_I+Yaw_rate_error;
Yaw_rate_error_I=Math_fConstrain(Yaw_rate_error_I,-50,+50);//限幅
Yaw_rate_error_D=Actual_yaw_rate-Last_gz;
Yaw_rate_error_D=Math_fConstrain(Yaw_rate_error_D,-40,+40);//限幅
/*********实际角速率**********************/
Actual_roll_rate=IMU_GYROx/16.4;
Actual_pitch_rate=IMU_GYROy/16.4;
/********角速率P*******************/
   Roll_rate_error=Desire_roll_rate-Actual_roll_rate;
Pitch_rate_error=Desire_pitch_rate-Actual_pitch_rate;
/********角速率I*******************/
   Roll_rate_error_I=Roll_rate_error_I+Roll_rate_error;
Roll_rate_error_I=Math_fConstrain(Roll_rate_error_I,-50,+50);//限幅
Pitch_rate_error_I=Pitch_rate_error_I+Pitch_rate_error;
Pitch_rate_error_I=Math_fConstrain(Pitch_rate_error_I,-50,+50);//限幅
/********角速率D*******************/
Roll_rate_error_D=Actual_roll_rate-Last_gx;
Roll_rate_error_D=Math_fConstrain(Roll_rate_error_D,-40,+40);
Pitch_rate_error_D=Actual_pitch_rate-Last_gy;
Pitch_rate_error_D=Math_fConstrain(Pitch_rate_error_D,-40,+40);
if(THROTTLE<1300)
{
   Roll_rate_error_I=0;
   Pitch_rate_error_I=0;
   Yaw_rate_error_I=0;
}

PID_ROLL=Roll_rate_error*Stabilize_Roll.Kp+Roll_rate_error_D*Stabilize_Roll.Kd+Roll_rate_error_I*Stabilize_Roll.Ki+IMU_GYROx*Level.Kd;//
PID_PITCH=Pitch_rate_error*Stabilize_Pitch.Kp+Pitch_rate_error_D*Stabilize_Pitch.Kd+Pitch_rate_error_I*Stabilize_Pitch.Ki+IMU_GYROy*Level.Kd;//
PID_YAW=Yaw_rate_error*Stabilize_Yaw.Kp+Yaw_rate_error_D*Stabilize_Yaw.Kd+Yaw_rate_error_I*Stabilize_Yaw.Ki;//

Last_gx=IMU_GYROx;
Last_gy=IMU_GYROy;
Last_gz=IMU_GYROz;

yufeng881005 发表于 2013-12-24 10:32:09

其实这个东西弄了很长时间，中间断断续续的，最近又捡了起来，但是一直没成功还是很郁闷的，刚在优酷上传了一段上次飞的视频，就是最后几秒就抖了起来，越抖越来厉害，然后就掉了下来.....当时没加积分和微分限幅，怀疑是这个原因，求分析

yufeng881005 发表于 2013-12-24 12:36:25

http://v.youku.com/v_show/id_XNjUyMjY0NzA0.html，这个是视频，最后掉了下来,大家分析分析原因，前面飞的怎么样

mbwhy 发表于 2013-12-26 20:15:26

楼主说的双回路，应该跟CRAZYFLY一样。不知道我理解的对不：外回路角度PID，输出给内回路角速度PID,然后输出给电机。至于楼主抖我也认为是积分的原因，可以先加限幅看看有没有效果

yufeng881005 发表于 2013-12-27 15:23:00

mbwhy 发表于 2013-12-26 20:15
楼主说的双回路，应该跟CRAZYFLY一样。不知道我理解的对不：外回路角度PID，输出给内回路角速度PID,然后输 ...

大概是这个意思，觉得角度控制差不多了，就差定高和外回路GPS悬停，实在有难度啊‘

页: [1]

amobbs.com 阿莫电子技术论坛's Archiver

大家分析分这个飞控控制程序