一、位置速度环仅有光流typedef struct { s16 alt_ctrl_speed_set; float speed_set_h[VEC_XYZ]; float speed_set_h_cms[VEC_XYZ]; float speed_set_h_norm[VEC_XYZ];归一化遥感量 float speed_set_h_norm_lpf[VEC_XYZ];滤波后的归一化遥感量 }_flight_state_st; extern _flight_state_st fs;1、Flight_State_Taskfs.speed_set_h_norm[X] (my_deadzone(CH_N[CH_PIT],0,50) *0.0022f);fs.speed_set_h_norm[Y] (my_deadzone(-CH_N[CH_ROL],0,50) *0.0022f);输入的遥感信号最终被处理为-500500范围内my_deadzone的作用是如果遥感数据在0附近50范围内则数据为0作为死区如果在死区范围外则数值向中间靠拢50最后乘0.0022f是为了归一化遥感量。450*0.00220.99#define LPF_1_(hz,t,in,out) ((out) ( 1 / ( 1 1 / ( (hz) *6.28f *(t) ) ) ) *( (in) - (out) )) LPF_1_(3.0f,dT_ms*1e-3f,fs.speed_set_h_norm[X],fs.speed_set_h_norm_lpf[X]); LPF_1_(3.0f,dT_ms*1e-3f,fs.speed_set_h_norm[Y],fs.speed_set_h_norm_lpf[Y]);然后对数据进行低通滤波( 1 1 / ( (hz) *6.28f *(t) ) )为滤波系数记为。公式为if(switchs.of_flow_on !switchs.gps_on ) { max_speed_lim 3.0f *wcz_hei_fus.out; max_speed_lim LIMIT(max_speed_lim,50,200); } fc_stv.vel_limit_xy max_speed_lim;有光流没gps则最大速度为3倍的wcz_hei_fus.out计算融合出的当前高度然后进行限幅speed_set_tmp[X] fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[X] program_ctrl.vel_cmps_h[X] pc_user.vel_cmps_set_h[X]; speed_set_tmp[Y] fc_stv.vel_limit_xy *fs.speed_set_h_norm_lpf[Y] program_ctrl.vel_cmps_h[Y] pc_user.vel_cmps_set_h[Y]; length_limit(speed_set_tmp[X],speed_set_tmp[Y],fc_stv.vel_limit_xy,fs.speed_set_h_cms); fs.speed_set_h[X] fs.speed_set_h_cms[X]; fs.speed_set_h[Y] fs.speed_set_h_cms[Y];将遥控器速度、程控速度和用户api速度叠加length_limit计算xy方向速度和的模长是否超过了限制如果超了就等比例缩小知道模长为上限即最大速度上限是xy方向速度向量和的上限。最终输出为fs.speed_set_h[X] fs.speed_set_h_cms[X];fs.speed_set_h[Y] fs.speed_set_h_cms[Y];2、Loc_1level_Ctrl针对仅有光流的情况loc_ctrl_1.exp[X] fs.speed_set_h[X]; loc_ctrl_1.exp[Y] fs.speed_set_h[Y]; //imu_data.h_acc为加速度为滤波后的结果 LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[X],vel_fb_d_lpf[X]); LPF_1_(5.0f,dT_ms*1e-3f,imu_data.h_acc[Y],vel_fb_d_lpf[Y]);if(sens_hd_check.of_ok) { loc_ctrl_1.fb[X] ano_of.of2_dx_fix 0.03f *vel_fb_d_lpf[X]; loc_ctrl_1.fb[Y] ano_of.of2_dy_fix 0.03f *vel_fb_d_lpf[Y]; fb_speed_fix[0] ano_of.of2_dx_fix;//OF_DX2FIX; fb_speed_fix[1] ano_of.of2_dy_fix;//OF_DY2FIX; } else//sens_hd_check.of_df_ok { loc_ctrl_1.fb[X] of_rdf.gnd_vel_est_h[X] 0.03f *vel_fb_d_lpf[X]; loc_ctrl_1.fb[Y] of_rdf.gnd_vel_est_h[Y] 0.03f *vel_fb_d_lpf[Y]; fb_speed_fix[0] of_rdf.gnd_vel_est_h[X]; fb_speed_fix[1] of_rdf.gnd_vel_est_h[Y]; }不同的光流用不同的数据结果是一样的反馈值为光流速度加一点点加速度值修正速度直接为光流速度。参数说明typedef struct { u8 fb_d_mode; //0使用反馈值微分1使用传感器数值 float kp; //比例系数 float ki; //积分系数 float kd_ex; //微分系数 float kd_fb; //previous_d 微分先行 float k_ff; //前馈 }_PID_arg_st; typedef struct { float err; //误差 float exp_old; //上一次期望值 float feedback_old; //上一次反馈值 float fb_d; //反馈的微分mode 0 float fb_d_ex; //传感器读取的反馈微分值mode 1 float exp_d; //期望微分值 float err_i; //误差累积 float ff; // float pre_d; float out; //输出 }_PID_val_st;pid计算主pid加修正pidfor(u8 i 0;i2;i) { PID_calculate( dT_ms*1e-3f, //周期单位秒 loc_ctrl_1.exp[i] , //前馈值 loc_ctrl_1.exp[i] , //期望值设定值 loc_ctrl_1.fb[i] , //反馈值 loc_arg_1[i], //PID参数结构体 loc_val_1[i], //PID数据结构体 50,//积分误差限幅 10 *flag.taking_off //integration limit积分限幅 ) ; //fix PID_calculate( dT_ms*1e-3f, //周期单位秒 loc_ctrl_1.exp[i] , //前馈值 loc_ctrl_1.exp[i] , //期望值设定值 fb_speed_fix[i] , //反馈值 loc_arg_1_fix[i], //PID参数结构体 loc_val_1_fix[i], //PID数据结构体 50,//积分误差限幅 10 *flag.taking_off //integration limit积分限幅 ) ; loc_ctrl_1.out[i] loc_val_1[i].out loc_val_1_fix[i].out; //(PD)(I) }主 PID (loc_arg_1): kp0.22 ki0 kd_ex0 kd_fb0.05 k_ff0.02修正 PID (loc_arg_1_fix): kp0 ki0.03 kd_ex0 kd_fb0 k_ff0以下是pid计算流程float PID_calculate( float dT_s, //周期单位秒 float in_ff, //前馈值 float expect, //期望值设定值 float feedback, //反馈值 _PID_arg_st *pid_arg, //PID参数结构体 _PID_val_st *pid_val, //PID数据结构体 float inte_d_lim,//积分误差限幅 float inte_lim //integration limit积分限幅 ) { float differential,hz; hz safe_div(1.0f,dT_s,0); pid_val-exp_d (expect - pid_val-exp_old) *hz;//期望的微分 if(pid_arg-fb_d_mode 0)//反馈微分用差分值还是传感器值 { pid_val-fb_d (feedback - pid_val-feedback_old) *hz; } else { pid_val-fb_d pid_val-fb_d_ex; } //微分项为期望微分-反馈微分 differential (pid_arg-kd_ex *pid_val-exp_d - pid_arg-kd_fb *pid_val-fb_d); pid_val-err (expect - feedback); //误差 //误差积分对单次误差限幅 pid_val-err_i pid_arg-ki *LIMIT((pid_val-err ),-inte_d_lim,inte_d_lim )*dT_s; //对误差积分限幅 pid_val-err_i LIMIT(pid_val-err_i,-inte_lim,inte_lim); //计算输出输出前馈比例误差微分积分 pid_val-out pid_arg-k_ff *in_ff pid_arg-kp *pid_val-err differential pid_val-err_i; 更新数值 pid_val-feedback_old feedback; pid_val-exp_old expect; return (pid_val-out); }与一般的pid相比1、微分项用的是期望微分*系数-反馈微分*系数将误差的微分拆分成两个用不同的参数。在无人机飞行过程中遥感值会发生突变此时误差微分也会突变导致电机猛然加快反馈微分可对速度进行阻尼防止速度突变2、多了前馈期望值pid为反馈控制器有一定的延迟前馈没等误差计算就对输出产生影响二、角度环1、Att_2level_Ctrl角度环控制中将位置环的X、Y输出作为pitch、roll的期望然后进行限幅25°主要处理的是yaw角他的期望角速度是遥感量归一化值*最大速度200~250°/s程控用户遥感量为-500~500有65的死区500-65*0.0022≈1然后进行限幅set_yaw_av_tmp (s32)(0.0023f *my_deadzone(CH_N[CH_YAW],0,65) *max_yaw_speed) (-program_ctrl.yaw_pal_dps) pc_user.pal_dps_set; /*最大YAW角速度限幅*/ set_yaw_av_tmp LIMIT(set_yaw_av_tmp ,-max_yaw_speed,max_yaw_speed);下面是对他过180°的处理//增量限幅每次变化最多30°/s att_1l_ct.set_yaw_speed LIMIT((set_yaw_av_tmp - att_1l_ct.set_yaw_speed),-30,30); /*设置期望YAW角度*/ att_2l_ct.exp_yaw att_1l_ct.set_yaw_speed *dT_s; /*限制为-180度*/ if(att_2l_ct.exp_yaw-180) att_2l_ct.exp_yaw 360; else if(att_2l_ct.exp_yaw180) att_2l_ct.exp_yaw - 360; /*计算YAW角度误差*/ att_2l_ct.yaw_err (att_2l_ct.exp_yaw - att_2l_ct.fb_yaw); /*限制为-180度*/ if(att_2l_ct.yaw_err-180) att_2l_ct.yaw_err 360; else if(att_2l_ct.yaw_err180) att_2l_ct.yaw_err - 360;角度期望值为角速度期望值的积分然后直接计算误差角度误差保持在-180~180超过范围进行±360处理。最后就是pid的计算过程与位置环一致yaw角期望为yaw角误差反馈是0att_2l_ct.fb_yaw imu_data.yaw ; att_2l_ct.fb_rol (imu_data.rol ) ; att_2l_ct.fb_pit (imu_data.pit ) ; PID_calculate( dT_s, //周期单位秒 0 , //前馈值 att_2l_ct.exp_rol , //期望值设定值 att_2l_ct.fb_rol , //反馈值 arg_2[ROL], //PID参数结构体 val_2[ROL], //PID数据结构体 5,//积分误差限幅 5 *flag.taking_off //integration limit积分限幅 ) ; PID_calculate( dT_s, //周期单位秒 0 , //前馈值 att_2l_ct.exp_pit , //期望值设定值 att_2l_ct.fb_pit , //反馈值 arg_2[PIT], //PID参数结构体 val_2[PIT], //PID数据结构体 5,//积分误差限幅 5 *flag.taking_off //integration limit积分限幅 ) ; PID_calculate( dT_s, //周期单位秒 0 , //前馈值 att_2l_ct.yaw_err , //因为yaw角度存在过180°的现象所以直接传入计算好的误差 0 , //反馈值 arg_2[YAW], //PID参数结构体 val_2[YAW], //PID数据结构体 5,//积分误差限幅 5 *flag.taking_off //integration limit积分限幅 ) ;