

分享一个串级ＰＩＤ程序源代码

/**********************************************************************************************************************
******串级PID控制程序、外环采用角度控制，内环采用角速度控制（直接由陀螺仪输出）外环角速为欧拉角（硬件DMP）*****
***********************************************************************************************************************
******本子程序为四轴飞行器核心算法 ********************************************************************************/
/****************************
   Y(Roll)
   顺时针转 | 逆时针转
   motor1  |  motor4
   正桨 | 反桨
--------------------X(Pitch)
   逆时针转 | 顺时针转
   motor2  |  motor3
   反桨 | 正桨
            |
****************************/
#include "control.h"
#include "led.h"
#include "delay.h"
#include "sys.h"
#include "usart.h"
#include "key.h"
#include "pwm_out.h"
#include "mpu6050.h"
#include "ahrs.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

//即将要更新的ACC加速度 GYRO陀螺仪
S_INT16_XYZ MPU6050_GYRO_LAST;

T_RC_Dat  Rc_D={0,0,0,1613};//遥控通道数据;
u8 lock=0; //飞控上锁标志位
extern u8 txbuf[4];  //发送缓冲
extern u8 rxbuf[4];  //接收缓冲
extern u16 test1[3]; //接收到NRf24L01数据

float thr=0;//油门

//PID1 PID_Motor;
/***************************************************************/
float Pitch_i,Roll_i,Yaw_i;             //积分项
float Pitch_old,Roll_old,Yaw_old;       //角度保存
float Pitch_d,Roll_d,Yaw_d;             //微分项

float RC_Pitch=0,RC_Roll=0,RC_Yaw=0;          //期望的姿态角

float Pitch_shell_out,Roll_shell_out,Yaw_shell_out;//外环总输出
      //外环PID参数
float Pitch_shell_kp=140;
float Pitch_shell_ki=0.2;
float Pitch_shell_kd=0.8;
/*********************************/
float Roll_shell_kp=150;
float Roll_shell_ki=0.2;
float Roll_shell_kd=0.8;
/*********************************/
float Yaw_shell_kp=15;//0.87
float Yaw_shell_ki=0.002;
float Yaw_shell_kd=0.02;
float Gyro_radian_old_x,Gyro_radian_old_y,Gyro_radian_old_z;//陀螺仪保存
float pitch_core_kp_out,pitch_core_ki_out,pitch_core_kd_out,Roll_core_kp_out,\
Roll_core_ki_out,Roll_core_kd_out,Roll_core_ki_out,Yaw_core_kp_out,\
Yaw_core_ki_out,Yaw_core_kd_out;//内环单项输出

float Pitch_core_out,Roll_core_out,Yaw_core_out;//内环总输出

//内环PID参数
float Pitch_core_kp=0.02;
float Pitch_core_ki=0;
float Pitch_core_kd=0.001;//0.001

float Roll_core_kp=0.02;
float Roll_core_ki=0;
float Roll_core_kd=0.001;

float Yaw_core_kp=0.001;
float Yaw_core_ki=0;
float Yaw_core_kd=0.001;

int16_t moto1=1613,moto2=1613,moto3=1613,moto4=1613;//电机的PWM值

/////////////////////////////////////////////////////////////////
//PID 控制函数
//输入：采样的姿态欧拉角数据
//反回：无
//备注：Pitch、Roll_i采用内环、外环PID控制，yaw采用内环控制
////////////////////////////////////////////////////////////////
void CONTROL_PID(float pit, float rol, float yaw)
{

  RC_Pitch=(Rc_D.pitch -1499.0f )/50;
  RC_Pitch-=3.3f;
//////////////////外环(PID)/////////////
  Pitch_i+=(pit-RC_Pitch);
//-------------Pitch积分限幅----------------//
  if(Pitch_i>300) Pitch_i=300;
  else if(Pitch_i<-300) Pitch_i=-300;
//-------------Pitch微分--------------------//
  Pitch_d=pit-Pitch_old;
//-------------Pitch  PID-------------------//
  Pitch_shell_out = Pitch_shell_kp*(pit-RC_Pitch) + Pitch_shell_ki*Pitch_i + Pitch_shell_kd*Pitch_d;
//保存角度
  Pitch_old=pit;
/*********************************************************/

RC_Roll=(Rc_D.roll-1502)/50;
Roll_i+=(rol-RC_Roll);
//-------------Roll积分限幅----------------//
  if(Roll_i>300) Roll_i=300;
  else if(Roll_i<-300) Roll_i=-300;
//-------------Roll微分--------------------//
  Roll_d=rol-Roll_old;
//-------------Roll  PID-------------------//
  Roll_shell_out  = Roll_shell_kp*(rol-RC_Roll) + Roll_shell_ki*Roll_i + Roll_shell_kd*Roll_d;
//------------Roll保存角度------------------//
  Roll_old=rol;

  //RC_Yaw=(Rc_D.yaw-1501)*10;
//-------------Yaw微分--------------------//
  Yaw_d=MPU6050_GYRO_LAST.Z-Yaw_old;
//-------------Roll  PID-------------------//
  Yaw_shell_out  = Yaw_shell_kp*(MPU6050_GYRO_LAST.Z-RC_Yaw) + Yaw_shell_ki*Yaw_i + Yaw_shell_kd*Yaw_d;
//------------Roll保存角度------------------//
  Yaw_old=MPU6050_GYRO_LAST.Z;

////////////////////////内环角速度环(PD)///////////////////////////////
//陀螺仪测得是角速度
  pitch_core_kp_out = Pitch_core_kp * (Pitch_shell_out + MPU6050_GYRO_LAST.Y );
  pitch_core_kd_out = Pitch_core_kd * (MPU6050_GYRO_LAST.Y - Gyro_radian_old_y);

  Roll_core_kp_out  = Roll_core_kp  * (Roll_shell_out  + MPU6050_GYRO_LAST.X );
  Roll_core_kd_out  = Roll_core_kd  * (MPU6050_GYRO_LAST.X - Gyro_radian_old_x);

  Yaw_core_kp_out  = Yaw_core_kp  * (Yaw_shell_out  + MPU6050_GYRO_LAST.Z);
  Yaw_core_kd_out  = Yaw_core_kd  * (MPU6050_GYRO_LAST.Z - Gyro_radian_old_z);

  Pitch_core_out = pitch_core_kp_out + pitch_core_kd_out;
  Roll_core_out  = Roll_core_kp_out  + Roll_core_kd_out;
  Yaw_core_out = Yaw_core_kp_out + Yaw_core_kd_out;

  Gyro_radian_old_y = MPU6050_GYRO_LAST.X;
  Gyro_radian_old_x = MPU6050_GYRO_LAST.Y;
  Gyro_radian_old_z = MPU6050_GYRO_LAST.Z;

//--------------------将输出值融合到四个电机--------------------------------//

      if(Rc_D.THROTTLE>1613)
      {
thr=Rc_D.THROTTLE;
moto1=(int16_t)(thr - Roll_core_out - Pitch_core_out- Yaw_core_out);
moto2=(int16_t)(thr + Roll_core_out - Pitch_core_out+ Yaw_core_out);
moto3=(int16_t)(thr + Roll_core_out + Pitch_core_out- Yaw_core_out);
moto4=(int16_t)(thr - Roll_core_out + Pitch_core_out+ Yaw_core_out);

}
      else
      {
moto1 = 1613;
moto2 = 1613;
moto3 = 1613;
moto4 = 1613;
      }
      Motor_PWM_Update(moto1,moto2,moto3,moto4);
}

void ahrs_control_PID_moto(void)//关键字：航姿控制串级PID 电机
{
if(lock==1)
{

   if(mpu_dmp_get_data(&Angle.pitch ,&Angle.roll,&Angle.yaw)==0)//得到欧拉角
{
//MPU_Get_Accelerometer(&Acc.X ,&Acc.Y ,&Acc.Z ); //得到加速度传感器数据
      MPU_Get_Gyroscope(&Gyro.X ,&Gyro.Y ,&Gyro.Z);//读取陀螺仪数据
MPU_Get_GYRO();//陀螺仪数据更新函数
CONTROL_PID(Angle_SET.pitch,Angle_SET.roll,Angle_SET.yaw);//PID计算pwm值
   if(report) Data_Exchange();//航姿数据更新到匿名上位机
}

}
else//飞机上锁
{
      Motor_PWM_Update(moto1-100,moto2-100,moto3-100,moto4-100);//飞控锁定，还没完善
}
}

网上的串级串级PID控制算法都大同小异。

提问者：长安老庄黄宝宏地点：- 浏览次数：7581 提问时间：05-16 00:16

我有更好的答案

8条回答: 苏州名之卓 05-16 19:44

谢谢楼主分享

 评论0  赞83; 望望Ll 05-22 10:57

我看你的积分项里的累加之后没有乘采样时间和微分项相减后没有除采样时间，这个没事吧？
还有一个就是我在有些地方看到的说外环算出来的数据是角速度的期望值，但是我看楼主的算法里是将外环算出来的用作误差值的，是不是我看到的那个文章说错了呢？

 评论0  赞18; kaishengdianzi 05-16 22:11

楼主这个代码写的真的蛮不错的，有逻辑易懂

 评论0  赞44; YBJ_zjzjzx 05-21 18:46

能问个问题吗？请问下串级PID调试，只需要内环就可以稳定的飞起来，加上外环反而容易震荡，于是减小内环P、I，始终找不到合适的值，有种说法“当飞机偏离30°，则需要以90°/s的速度修正”的意思是外环的P是3吗？在此基础调内环？

 评论0  赞60; 幽冥幻雪 05-22 19:58

楼主  我觉得你的内环没写对  不知道我讲的对不对
pitch_core_kp_out = Pitch_core_kp * (Pitch_shell_out + MPU6050_GYRO_LAST.Y );
  pitch_core_kd_out = Pitch_core_kd * (MPU6050_GYRO_LAST.Y - Gyro_radian_old_y);

  Roll_core_kp_out  = Roll_core_kp  * (Roll_shell_out  + MPU6050_GYRO_LAST.X );
  Roll_core_kd_out  = Roll_core_kd  * (MPU6050_GYRO_LAST.X - Gyro_radian_old_x);

  Yaw_core_kp_out  = Yaw_core_kp  * (Yaw_shell_out  + MPU6050_GYRO_LAST.Z);
  Yaw_core_kd_out  = Yaw_core_kd  * (MPU6050_GYRO_LAST.Z - Gyro_radian_old_z);

外环给定的输入不应该是与实际角速度做差吗？得到偏差  而楼主你却把内环的期望输入和实际角速度相加了。。。。。。。。。。。。
还有微分项应该是这一次的偏差减去上一次的偏差  这才能算微分吧  微分项不是应该是偏差的微分么？
希望可以交流下 QQ1292938163

 评论0  赞38; cmh26 05-21 14:14

融合电机那几个错了吧？

 评论0  赞46; hjhevildask 05-25 07:24

正需要这个，太感谢了

 评论0  赞25; scmywkf 05-24 23:48

想和楼主探讨一个问题，在外环控制里头，我想实现姿态控制翻跟头，怎么添加代码（比如在哪修改）？

 评论0  赞28

撰写答案: 提交

1 / 3