无人机之姿态控制篇

姿态控制篇

拿四旋翼无人机为例，四个桨叶旋转是的无人机产生了升力，但是这个升力要怎么匹配才能使得无人机能够平稳的飞行呢，这个就是无人机姿态控制的关键。

如图四个电机1、2、3、4，水平位置X，Y。要想使无人机平稳的飞行则水平位置XY的升力必须达到平衡。这是姿态控制就要发挥它的作用了。
本次我使用的姿态控制算法也是传统的串级PID，首先我们需要先了解一下姿态控制需要用到哪些数据：
1、输入型数据：欧拉角（水平位置的反馈信息）、陀螺仪数据（可以理解为欧拉角的微分，这个数据可以实时检测到飞机的XY方向的角速度）
2、输出型数据：输出有三个轴的信息需要输出，X轴、Y轴、Z轴。X轴控制左右方向，比如检测到飞机往右边倾斜了X轴数据的数据会加大34电机的PWM，从而是的飞机恢复水平，反之检测到飞机往左边倾斜则X轴数据会加大12电机的PWM是的飞机恢复平衡。Y轴同理X轴。剩下就是Z轴也称Yaw轴，无人机桨叶在旋转的时候同时会带动飞机往反方向旋转，所以Yaw控制13电机24电机的平衡。
原理写完了直接上代码：

void Extern_pid(_PID_extern* par,float tar,float cur,float ratio_p,float ratio_i,float ratio_d)
{
float err = 0, i_err= 0, d_err = 0, results = 0;
 err = tar-cur;
 i_err = par->i_err+ratio_i*err;
 d_err = err-par->pre_err;
 par->pre_err = err;
 par->i_err = i_err;
 d_err = d_err*ratio_d;
 par->d_err = d_err;
 err = err*ratio_p;
 results = err+par->i_err+d_err;
 par->results = results;
}//先上一个PID的框架，后面就直接调用这个PID函数了
//各个参数的意义熟悉PID的同学应该很快就能看出来，下面就是串级PID的精髓了：
//position control
  Extern_pid((_PID_extern*)&pitch_angle,eulerPitchDesired,eulerPitchActual,
    ANGLE_PitchRoll_KP,ANGLE_PitchRoll_KI,ANGLE_PitchRoll_KD);
  Extern_pid((_PID_extern*)&roll_angle,eulerRollDesired,eulerRollActual,
    ANGLE_PitchRoll_KP,ANGLE_PitchRoll_KI,ANGLE_PitchRoll_KD);
Extern_pid((_PID_extern*)&yaw_angle,eulerYawDesired,flight_yaw(eulerYawActual,eulerYawDesired),
    ANGLE_Yaw_KP,ANGLE_Yaw_KI,ANGLE_Yaw_KD);
 //eulerPitchActual，eulerRollActual，flight_yaw(eulerYawActual,eulerYawDesired)这三个参数就是姿态解算出来的欧拉角，欧拉角作为串级PID的外环。
 //angular velocity control
  Extern_pid((_PID_extern*)&pitch_gyro,pitch_angle.results,gyro_x,
    VEL_PitchRoll_KP,VEL_PitchRoll_KI,VEL_PitchRoll_KD);
  Extern_pid((_PID_extern*)&roll_gyro,roll_angle.results,gyro_y,
    VEL_PitchRoll_KP,VEL_PitchRoll_KI,VEL_PitchRoll_KD);
  Extern_pid((_PID_extern*)&yaw_gyro,yaw_angle.results,gyro_z,
    VEL_Yaw_KP,VEL_Yaw_KI,VEL_Yaw_KD);
    //这个内环也解释一下，用外环PID的运算值加入到内环PID的期望值，这个思想等同于模糊PID即外环误差越大内环误差成倍数增大，类似于内环动态的KP，KI,KD值。内环主要是角速度环，在整个串级PID来说内环相当于单极PID的D值，只是这个D值反应很迅速，所以这个算法在无人机姿态控制效果很好。
    MotorValue[0]=(int16)(LIMIT(thr_out+pitch_filter_control+roll_filter_control+yaw_filter_control,0,960));
 MotorValue[1]=(int16)(LIMIT(thr_out-pitch_filter_control+roll_filter_control-yaw_filter_control,0,960));
 MotorValue[2]=(int16)(LIMIT(thr_out-pitch_filter_control-roll_filter_control+yaw_filter_control,0,960));
 MotorValue[3]=(int16)(LIMIT(thr_out+pitch_filter_control-roll_filter_control-yaw_filter_control,0,960));
 //然后就是把XYZ三轴经过PID运算的数据传到各个对应的电机

到此，姿态平稳控制完成，希望对你有用，有关无人机各个模块讲解持续更新中…