偏航角的控制与俯仰角以及翻滚角不同。俯仰角以及翻滚角都是要随着遥控器改变,特别是遥控器归中的时候,就俯仰角以及翻滚角也要归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 ; //前一个误差值
}