|
????????偏航角的控制与俯仰角以及翻滚角不同。俯仰角以及翻滚角都是要随着遥控器改变，特别是遥控器归中的时候，就俯仰角以及翻滚角也要归0。但是偏航角最大的区别就是遥控器归中后不需要恢复。
蓝鸟飞行器：
对于Z轴偏航角的控制：
是对接受来的遥控器数据进行分段处理，当大于某一值的时候，对期望的偏航角度值进行累加。由于是分段控制，所以遥控器摇杆归位以后，期望的偏航角度值并不归0。所以不会出现摇杆归位后，飞机又转回去的现象。
特点：
偏航角数据分段处理不同担心在打油门的时候误触偏航角
Z轴有角度环--角速度环PID控制，所以在未起飞的时候对飞机的偏航角进行改变后，飞机会将认为偏航角的改变是外界的误差干扰，起飞时，为了消除误差会回到初始化时(一般是开机时)的偏航角。
同样的，在飞行的时候，如果强行改变飞行器的偏航角，由于Z轴有角度环PID控制，飞机也会很快的调整到原来的偏航角
简要代码节选：
//遥控器
const float roll_pitch_ratio = 0.04f;//将控制量限制在-20~20之间
?
pidPitch.desired = -(Remote.pitch-1500)*roll_pitch_ratio
pidRoll.desired = -(Remote.roll-1500)*roll_pitch_ratio;
?
if(Remote.yaw>1820)//当且仅当舵量里中心值（1500）偏移较多时，才对偏航角进行加和 ?
{
pidYaw.desired += 0.75f;
}
else if(Remote.yaw <1180)
{
pidYaw.desired -= 0.75f;
}
?
?
//PID控制
pidRateX.measured = MPU6050.gyroX * Gyro_G; //陀螺仪测量的角速度
pidRateY.measured = MPU6050.gyroY * Gyro_G;
pidRateZ.measured = MPU6050.gyroZ * Gyro_G;
?
pidPitch.measured = Angle.pitch; //姿态结算的实际姿态角
pidRoll.measured ?= Angle.roll;
pidYaw.measured ? = Angle.yaw;
?
pidUpdate(&pidRoll,dt); ? ?
pidRateX.desired = pidRoll.out; //将角度环的输出值作为角速度环的输入值
pidUpdate(&pidRateX,dt); ?
?
pidUpdate(&pidPitch,dt); ? ?
pidRateY.desired = pidPitch.out; ?
pidUpdate(&pidRateY,dt); ?
?
CascadePID(&pidRateZ,&pidYaw,dt);
?
//PID算法
void pidUpdate(PidObject* pid,const float dt)
{
float error;
float deriv;
?
error = pid->desired - pid->measured; //比例
pid->integ += error * dt; //积分
deriv = (error - pid->prevError)/dt; //微分
pid->out = pid->kp * error + pid->ki * pid->integ + pid->kd * deriv;
pid->prevError = error; ?//更新误差（用于微分计算）
}
小马哥飞行器：
对于Z轴偏航角的控制：
没有对接受来的遥控器数据进行分段处理，和翻滚角以及俯仰角的处理方法相同。但是对偏航角只进行单级的角速度PID控制，而没有角度PID控制。将期望的偏航角度值，直接输入。这样一来也可以实现偏航角不随遥控器摇杆归中的效果。
特点：
没有了偏航角-角度环的控制，对偏航角PID控制仅使用角速度环，在未起飞的时候对飞机的偏航角进行改变后，起飞时飞机会按照已经改变的方向起飞；
飞机在空中飞行的时候，偏航角不像俯仰角与翻滚角，收到外界的干扰相对而言还是少一些的，所以去掉偏航角的角度环问题不大
简要代码节选：
//遥控器
Target_Angle.rol = (float)((rc_in->ROLL-1500)/12.0f);
Target_Angle.pit = (float)((rc_in->PITCH-1500)/12.0f);
Target_Angle.yaw = (float)((1500-rc_in->YAW)/12.0f);
?
//角度环
PID_Postion_Cal(&PID_ROL_Angle,Target_Angle.rol,Measure_Angle.rol);//ROLL角度环PID （输入角度 输出角速度）
PID_Postion_Cal(&PID_PIT_Angle,Target_Angle.pit,Measure_Angle.pit);//PITH角度环PID （输入角度 输出角速度）
// PID_Postion_Cal(&PID_YAW_Angle,Target_Angle.yaw,Measure_Angle.yaw);//YAW角度环PID （输入角度 输出角速度）
//角速度环
PID_Postion_Cal(&PID_ROL_Rate,PID_ROL_Angle.OutPut,(gyr_in->Y*RadtoDeg)); //ROLL角速度环PID （输入角度环的输出，输出电机控制量）
PID_Postion_Cal(&PID_PIT_Rate,PID_PIT_Angle.OutPut,-(gyr_in->X*RadtoDeg)); //PITH角速度环PID （输入角度环的输出，输出电机控制量）
PID_Postion_Cal(&PID_YAW_Rate,Target_Angle.yaw*PID_YAW_Angle.P,gyr_in->Z*RadtoDeg); //YAW角速度环PID （输入角度，输出电机控制量）`
?
//PID算法
void PID_Postion_Cal(PID_TYPE*PID,float target,float measure)
{
PID->Error ?= target - measure; ? ? ? ? ? ? ?//误差
PID->Differ = PID->Error - PID->PreError; ? ?//微分量
?
PID->Pout = PID->P * PID->Error; ? ? ? ? ? ? ? ? ? ? ? ?//比例控制
PID->Iout = PID->Ilimit_flag * PID->I * PID->Integral; ?//积分控制（Ilimit_flag可以为1或者0）
PID->Dout = PID->D * PID->Differ; ? ? ? ? ? ? ? ? ? ? ? //微分控制
?
PID->OutPut = ?PID->Pout + PID->Iout + PID->Dout; ? ? ? //比例 + 积分 + 微分总控制
?
if(Airplane_Enable == 1&&RC_Control.THROTTLE >= 180) ? ?//飞机解锁之后再加入积分,防止积分过调
{
if(measure > (PID->Ilimit)||measure < -PID->Ilimit) ? //积分分离
{PID->Ilimit_flag = 0;}
else
{
PID->Ilimit_flag = 1; ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? //加入积分(只有测量值在-PID->Ilimit~PID->Ilimit 范围内时才加入积分)
PID->Integral += PID->Error; ? ? ? ? ? ? ? ? ? ? ? ?//对误差进行积分
if(PID->Integral > PID->Irang) ? ? ? ? ? ? ? ? ? ? ?//积分限幅
PID->Integral = PID->Irang;
if(PID->Integral < -PID->Irang) ? ? ? ? ? ? ? ? ? ? //积分限幅
? ?PID->Integral = -PID->Irang; ? ? ? ? ? ? ? ? ? ?
}
}else
{PID->Integral = 0;}
PID->PreError = PID->Error ; ? ? ? ? ? ? ? ? ? ? ? ? ? ?//前一个误差值
? }
|