企业官网建设流程全解析

一、位置速度环（仅有光流）

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_Task

fs.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);

输入的遥感信号最终被处理为-500，500范围内，my_deadzone的作用是如果遥感数据在0附近50范围内，则数据为0，作为死区，如果在死区范围外，则数值向中间靠拢50，最后乘0.0022f是为了归一化遥感量。450*0.0022=0.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计算x，y方向速度和的模长是否超过了限制，如果超了就等比例缩小知道模长为上限，即最大速度上限是x，y方向速度向量和的上限。最终输出为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加修正pid

for(u8 i =0;i<2;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): kp=0.22 ki=0 kd_ex=0 kd_fb=0.05 k_ff=0.02
修正 PID (loc_arg_1_fix): kp=0 ki=0.03 kd_ex=0 kd_fb=0 k_ff=0
以下是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_yaw>180) 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_err>180) att_2l_ct.yaw_err -= 360;

角度期望值为角速度期望值的积分，然后直接计算误差，角度误差保持在-180~180，超过范围进行±360处理。

最后就是pid的计算，过程与位置环一致，yaw角期望为yaw角误差，反馈是0

att_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，积分限幅 ) ;