本帖最后由 冷不丁 于 2017-5-11 19:37 编辑
经过接近两个月的努力（折腾了一个寒假QAQ），终于成功使用NXP K64实现了多旋翼飞控的制作，和程序编写。
接下来，我将进行控制算法部分的调参讲解。
飞行器的姿态控制算法使用的是串级PID，即角速度作为内环，角度为外环，两层PID级联的控制方式。
关于串级PID的一些比较好的资料，我以文档的方式进行分享。
我主要对于，串级PID参数整定分享一些经验。
以下是我使用到的串级PID整定策略：
单极PID适合线性系统，当输出量和被控制量呈线性关系时单极PID能获得较好的效果，但是四轴不是线性系统，现代学者认为，四轴通常可以简化为一个二阶阻尼系统。为什么四轴不是线性系统呢？首先，输出的电压和电机转速不是呈正比的，其次，螺旋桨转速和升力是平方倍关系，故单极PID在四轴上很难取得很好效果，能飞，但是不好飞。 为了解决这个问题，我们提出了串级PID这个解决方法。 串级PID就是两个PID串在一起，分为内环和外环PID。在此，我们使用内环PID控制，外环PI控制。 单极PID输入的是期望角度，反馈的是角度数据，串级PID中外环输入反馈的也是角度数据，内环输入反馈的便是角速度数据。通俗来讲，内环就是你希望将四轴以多少度每秒的速度运动，然后他给你纠正过来，外环就是根据角度偏差告诉内环你该以多少度一秒运动。这样，即使外环数据剧烈变化，四轴的效果也不会显得很僵硬。 在内环中，PID三个数据作用分别是：P（将四轴从偏差角速度纠正回期望角速度）D（抑制系统运动）I（消除角速度控制静差） 外环PI中，两个数据的作用是：P（将四轴从偏差角度纠正回期望角度）I（消除角度控制静差） 整定方法： 1，将内外环PID都归0，适当增加内环的P，调整P至四轴从正面朝上自然转动到正面朝下时能感受到阻力，且没有抖动，有抖动就应减小P，当P减小到无抖动或者轻微抖动时即可。 2，让内环的D慢慢增加，到你用手能明显感受到转动四轴产生排斥外力的阻力即可，D能抑制P产生的振荡，但是D过大也会导致高频振荡，调整D至系统无振荡且能抑制外界的力即可。 3，给内环一点点I，注意的是I的积分要在油门开启后才开始，油门关闭就清0，且必须有积分限幅。I推荐取越小越好，我取的是0.01，I取大了会导致系统振荡。 4，将内环P减半，将外环P调至内环的50-70倍，根据系统产生的高频振荡降低内环的D，直至高频振荡消除即可。 5，给外环一点点I，同3. 6，根据实际情况对参数进行优化调整，调整过程中要注意区分各个参数的作用，时刻记住，P是回复力，大了会低频振荡，D是抑制力，大了会高频振荡，I是静差消除力，越小越好，大了会产生振荡。 高度控制使用PD算法。 位置控制使用串级PID算法。贴出我的姿态控制，进行了部分注释，输入目前姿态和目标姿态，输出PWM直接控制电机。 void CONTROL(float rol_now, float pit_now, float yaw_now, float rol_tar, float pit_tar, float yaw_tar)
{
uint16_t moto1=0,moto2=0,moto3=0,moto4=0;
S_INT16_XYZ ACC_Angle_Offset;
/*____________________________________________assignment argument______________________________________________________*/
float error_rol = (-rol_tar) - (-rol_now);
float error_pit = (-pit_tar) - (-pit_now);
float error_yaw = (-yaw_tar) - (-yaw_now);
float g_x = MPU6050_GYRO_NOW.Y*0.0610351;
float g_y = MPU6050_GYRO_NOW.X*0.0610351;
float g_z = MPU6050_GYRO_NOW.Z*0.0610351;
round_error_rol=error_rol;
round_error_pit=error_pit;
/*__________________________________________==assignment argumen==_____________________________________________________*/
/*__________________________________________________zero set___________________________________________________________*/
if((Temp_Throttle<180) && ((MPU6050_GYRO_LAST.Z > 1) || (MPU6050_GYRO_LAST.Z < -1)))//
{
Gyro_Z_Offset = Gyro_Z_Offset + MPU6050_GYRO_LAST.Z;
GYRO_I.Z = 0;
}
if((Temp_Throttle<180) && ((MPU6050_GYRO_LAST.X > 1) || (MPU6050_GYRO_LAST.X < -1)))//
{
Gyro_X_Offset = Gyro_X_Offset + MPU6050_GYRO_LAST.X;
GYRO_I.X = 0;
}
if((Temp_Throttle<180) && ((MPU6050_GYRO_LAST.Y > 1) || (MPU6050_GYRO_LAST.Y < -1)))//
{
Gyro_Y_Offset = Gyro_Y_Offset + MPU6050_GYRO_LAST.Y;
GYRO_I.Y = 0;
}
if(Temp_Throttle<180)//加速度计零偏值校验
{
Dataanl_flag = 0;
Accel_X_Offset = ACC_AVG.X;
Accel_Y_Offset = ACC_AVG.Y;
Accel_Z_Offset = ACC_AVG.Z - 8192;
}
else
{
Dataanl_flag = 1;
}
/*________________________________________________==zero set==_________________________________________________________*/
// PID_ROL.IMAX = 600;
// PID_PIT.IMAX = PID_ROL.IMAX;
/*_____________________________________________________PID_____________________________________________________________*/
/*_________________________________PID_X_OU_________________________________________*/
PID_PIT.OU_I_integral = PID_PIT.OU_I_integral + error_pit;//外环角度误差积分
if(PID_PIT.OU_I_integral >1000)//积分限幅：100需调整
PID_PIT.OU_I_integral = 1000;
else if(PID_PIT.OU_I_integral < -1000)
PID_PIT.OU_I_integral = -1000;
PID_PIT.OU_OUT = error_pit*PID_PIT.OU_P + PID_PIT.OU_I_integral*PID_PIT.OU_I;//角度环PI计算
/*_______________________________==PID_X_OU==_______________________________________*/
/*_________________________________PID_X_IN_________________________________________*/
// Last_Gyro_y = gy;//保存本次陀螺仪数据，下次使用
PID_PIT.IN_I_integral = PID_PIT.IN_I_integral+(PID_PIT.OU_OUT - (-g_y));
if(PID_PIT.IN_I_integral>500)// 积分限幅
PID_PIT.IN_I_integral = 500;
else if(PID_PIT.IN_I_integral < -500)
PID_PIT.IN_I_integral = -500;
if(LOCK_THR==1)
PID_PIT.IN_I_integral = 0;
/*____________________________________bu_wan_quan_wen_fen_______________________________________________________________*/
PID_PIT.in_dout = PID_PIT.NOW_IN_D_RATE*(last_g_y - g_y) + (1.0-PID_PIT.NOW_IN_D_RATE)*PID_PIT.in_dout;
/*__________________________________==bu_wan_quan_wen_fen==_____________________________________________________________*/
PID_PIT.IN_OUT =(PID_PIT.OU_OUT - (-g_y))*PID_PIT.IN_P + PID_PIT.IN_I_integral*PID_PIT.IN_I - PID_PIT.in_dout*PID_PIT.IN_D;//内环PID计算输出
if(PID_PIT.IN_OUT > 1000)
PID_PIT.IN_OUT = 1000;
if(PID_PIT.IN_OUT < -1000)
PID_PIT.IN_OUT = -1000;
last_g_y = g_y;//保存本次陀螺仪数据，下次使用
/*_______________________________==PID_X_IN==_______________________________________*/
/*----------------------------------X_END-------------------------------------------*/
/*----------------------------------X_END-------------------------------------------*/
/*_________________________________PID_Y_OU_________________________________________*/
PID_ROL.OU_I_integral = PID_ROL.OU_I_integral + error_rol;//外环角度误差积分
if(PID_ROL.OU_I_integral >1000)//积分限幅：100需调整
PID_ROL.OU_I_integral = 1000;
else if(PID_ROL.OU_I_integral < -1000)
PID_ROL.OU_I_integral = -1000;
PID_ROL.OU_OUT = error_rol*PID_ROL.OU_P + PID_ROL.OU_I_integral*PID_ROL.OU_I;//角度环PI计算
/*_______________________________==PID_Y_OU==_______________________________________*/
/*_________________________________PID_Y_IN_________________________________________*/
// Last_Gyro_y = gy;//保存本次陀螺仪数据，下次使用
PID_ROL.IN_I_integral = PID_ROL.IN_I_integral+ PID_ROL.OU_OUT - g_x;
if(PID_ROL.IN_I_integral>200)// 积分限幅
PID_ROL.IN_I_integral = 200;
else if(PID_ROL.IN_I_integral < -200)
PID_ROL.IN_I_integral = -200;
if(LOCK_THR==1)
PID_ROL.IN_I_integral = 0;
/*____________________________________bu_wan_quan_wen_fen_______________________________________________________________*/
PID_ROL.in_dout = PID_ROL.NOW_IN_D_RATE*(g_x - last_g_x) + (1.0-PID_ROL.NOW_IN_D_RATE)*PID_ROL.in_dout;
/*__________________________________==bu_wan_quan_wen_fen==_____________________________________________________________*/
PID_ROL.IN_OUT =(PID_ROL.OU_OUT - g_x)*PID_ROL.IN_P + PID_ROL.IN_I_integral*PID_ROL.IN_I - PID_ROL.in_dout*PID_ROL.IN_D;//内环PID计算输出
if(PID_ROL.IN_OUT > 1000)
PID_ROL.IN_OUT = 1000;
if(PID_ROL.IN_OUT < -1000)
PID_ROL.IN_OUT = -1000;
Ax_del = (last_g_x - g_x);
last_g_x = g_x;//保存本次陀螺仪数据，下次使用
/*_______________________________==PID_Y_IN==_______________________________________*/
/*__________________________________PID_Z___________________________________________*/
PID_YAW.ou_pout = PID_YAW.OU_P * error_yaw;
int16_t yaw_d;
yaw_d = MPU6050_GYRO_NOW.Z*0.0610351;
PID_YAW.ou_dout = PID_YAW.OU_D * yaw_d/0.02;
PID_YAW.OU_OUT = PID_YAW.ou_pout + PID_YAW.ou_dout;
/*________________________________==PID_Z==_________________________________________*/
/*___________________________________________________==PID==___________________________________________________________*/
PID_ROL.OUT = PID_ROL.IN_OUT;
PID_PIT.OUT = PID_PIT.IN_OUT;
PID_YAW.OUT = PID_YAW.OU_OUT;
//PID_YAW.OUT = PID_Z;
/*___________________________________________________To-moto___________________________________________________________*/
if(throttle>=200)//Temp_Throttle
{
moto1 = throttle + PID_ROL.OUT - PID_PIT.OUT - PID_YAW.OUT + PID_HIGH.OUT - PID_POSITION_ROL.OUT + PID_POSITION_PIT.OUT;
moto2 = throttle - PID_ROL.OUT - PID_PIT.OUT + PID_YAW.OUT + PID_HIGH.OUT + PID_POSITION_ROL.OUT + PID_POSITION_PIT.OUT;
moto3 = throttle - PID_ROL.OUT + PID_PIT.OUT - PID_YAW.OUT + PID_HIGH.OUT + PID_POSITION_ROL.OUT - PID_POSITION_PIT.OUT;
moto4 = throttle + PID_ROL.OUT + PID_PIT.OUT + PID_YAW.OUT + PID_HIGH.OUT - PID_POSITION_ROL.OUT - PID_POSITION_PIT.OUT;
}
else
{
moto1 = 0;
moto2 = 0;
moto3 = 0;
moto4 = 0;
}
moto_view1=moto1;
moto_view2=moto2;
moto_view3=moto3;
moto_view4=moto4;
if (LOCK_THR == 0) Moto_Control(moto1,moto2,moto3,moto4);
else Moto_Control(0,0,0,0);
/*____________________________________________________roun_____________________________________________________________*/
round_rol = error_rol;
round_pit = error_pit;
round_yaw = error_yaw;
round_g_x = g_x;
round_g_y = g_y;
round_g_z = g_z;
/*__________________________________________________==roun==___________________________________________________________*/
/*_________________________________________________==To-moto==_________________________________________________________*/
}
避障策略使用漫反射红外对管，实现可调距离的对目标检测，然后配合光流模块实现自主避障飞行。
主函数中wihile循环及执行策略是 2ms执行一次姿态计算和控制 20ms执行一次光流数据读取和位置及高度控制 while (1)
{
if (true == mission_attitude)//2ms一次计算
{
Prepare_Data();
Recursive_Fliter(&MPU6050_GYRO_LAST,&MPU6050_GYRO_NOW ,5);
Get_Attitude(); //姿态计算
round_x = Q_ANGLE.X;
round_y = Q_ANGLE.Y;
round_z = Q_ANGLE.Z;
RC_to_tar_angle();
CONTROL(Q_ANGLE.Y,Q_ANGLE.X, Q_ANGLE.Z, tar_angle_X, tar_angle_Y, 0);
counter++;
mission_attitude=false;
}
if (true == mission_senser)//20ms一次计算
{
I2C_ReadFlow();
if(LOCK_THR==0)
{
senser.x_sumdistance = senser.x_sumdistance + (float)senser.flow_comp_x_speed*0.01;
senser.y_sumdistance = senser.y_sumdistance + (float)senser.flow_comp_y_speed*0.01;
}
mission_senser=false;
}
if(true == mission_high) //10ms
{
if (LOCK_THR==0 && (position_star==1))
{
High_control();
}
else
{
PID_HIGH.OUT=0;
PID_HIGH.ou_iout = 0;
}
mission_high = false;
}
if(true == mission_position) //20ms
{
if(Rc_Get.AUX1 > 1500 && Rc_Get.AUX1 < 1750 && position_star == 0)
{
senser.y_sumdistance_offset = senser.y_sumdistance;
senser.x_sumdistance_offset = senser.x_sumdistance;
position_star = 1;
}
if((Rc_Get.AUX1 > 1000 && Rc_Get.AUX1 < 1500) || LOCK_THR==1)
{
senser.y_sumdistance_offset = 0;
senser.x_sumdistance_offset = 0;
position_star = 0;
}
if(position_star == 1)
{
if(AUTO_FLY_FLAG == 1)
{
move_control(0,0);
}
Position_control();
}
if(position_star == 0)
{
PID_POSITION_ROL.ou_iout=0;
PID_POSITION_ROL.OUT=0;
PID_POSITION_PIT.ou_iout=0;
PID_POSITION_PIT.OUT=0;
}
mission_position = false;
}
至此我的多旋翼飞控制作就到此结束了...快开学了，祝21IC的各位工作学习愉快！
资料： 3，我搜集到的全部关于K64的有用的资料（太大了，只能分享到百度云上） 4，选用的配件模块清单和各模块的资料详细在以前的帖子都有介绍，详细可以翻我的【多旋翼飞控】系列帖查看 | 
楼主
标题置顶
标题高亮
