MiniFly微型四轴学习开发日志（二）——四轴PID算法

文章目录

限幅位置式PID介绍

角度环PID

角速度环PID

Z轴高度环PID

姿态控制量与实际油门值整合得到四轴电机PWM

---

有关MiniFly微型四轴基本框架介绍以及三个PID算法的大致框架，已经在日志（一）当中介绍，以下是链接。本文是基于日志（一）对PID进行进一步的介绍

https://blog.csdn.net/watchingdog/article/details/116089883?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522161924912016780269877037%2522%252C%2522scm%2522%253A%252220140713.130102334.wap%255Fall.%2522%257D&request_id=161924912016780269877037&biz_id=0&utm_medium=distribute.wap_search_result.none-task-blog-2~all~first_rank_v2~rank_v29-17-116089883.wap_first_rank_v2_rank_v29&utm_term=minifly四轴&spm=1018.2118.3001.4187。

限幅增量式PID介绍

首先，再介绍三个PID开始之前，先介绍限幅的位置式PID，以下介绍转引自以下博文：https://blog.csdn.net/as480133937/article/details/89508034?spm=1001.2014.3001.5501

位置式PID是对系统当前的实际位置与你想达到的语气位置的偏差进行校正的PID控制。其公式为

由公式看出，其输出与过去所有状态有关，积分项为误差的累加值，输出u(k)因此对应执行器的实际位置，一旦输出大幅度变化，系统将会剧变。

另一方面，积分项饱和时，误差仍在积分作用下累计，一旦误差开始反向变化，系统需要时间从饱和区退出。因而输出到达最小最大时，应停止积分作用，进行积分限幅和输出限幅。以下为算法基本例程：

typedef struct PID
{
  float P,I,D,limit;
}PID;
 
typedef struct Error
{
  float Current_Error;//当前误差
  float Last_Error;//上一次误差
  float Previous_Error;//上上次误差
}Error;
 
/*
 *  @brief      位置式PID
 *  @since      v1.0
 *  *sptr ：误差参数
 *  *pid:  PID参数
 *  NowPlace：当前位置
 *  Point：   预期位置  
 */
 
// 位置式PID控制
float PID_Realize(Error *sptr,PID *pid, int32 NowPlace, float Point)
{
 
    int32 iError,    // 当前误差
         Realize;   //实际输出    
 
    iError = Point - NowPlace;    // 计算当前误差
    sptr->Current_Error += pid->I * iError;    // 误差积分
      sptr->Current_Error = sptr->Current_Error > pid->limit?pid->limit:sptr->Current_Error;//积分限幅，成立取前一项，不成立取后一项
      sptr->Current_Error = sptr->Current_Error <-pid->limit?-pid->limit:sptr->Current_Error;
    Realize = pid->P * iError       //比例P
            + sptr->Current_Error   //积分I
            + pid->D * (iError - sptr->Last_Error);  //微分D
    sptr->Last_Error = iError;              // 更新上次误差
    return Realize;    // 返回实际值
}

而在MiniFly当中，其限幅位置式PID如下：

float pidUpdate(PidObject* pid, const float error)
{
  float output;
  pid->error = error; //当前误差
  pid->integ += pid->error * pid->dt;//误差积分
  if (pid->integ > pid->iLimit)//误差限幅
   {
    pid->integ = pid->iLimit;
   }
  else if (pid->integ < pid->iLimitLow)
   {
    pid->integ = pid->iLimitLow;
   }
  pid->deriv = (pid->error - pid->prevError) / pid->dt;//误差微分
  pid->outP = pid->kp * pid->error;
  pid->outI = pid->ki * pid->integ;
  pid->outD = pid->kd * pid->deriv;
  output = pid->outP + pid->outI + pid->outD; //计算pid输出
  pid->prevError = pid->error;//更新上次误差
  return output;//返回校正后的PID输出值
}

第一个入口参数为将被更新结构体对象的指针，第二个入口参数为当前偏差。

角度环PID

角度环PID代码如下：

void attitudeAnglePID(attitude_t *actualAngle, attitude_t *desiredAngle, attitude_t *outDesiredRate)
{/* 角度环 PID */
  outDesiredRate->roll = pidUpdate(&pidAngleRoll, desiredAngle->roll - actualAngle->roll);
  outDesiredRate->pitch = pidUpdate(&pidAnglePitch, desiredAngle->pitch -   actualAngle->pitch);
  float yawError = desiredAngle->yaw - actualAngle->yaw ;
  if (yawError > 180.0f) 
    yawError -= 360.0f;
  else if (yawError < -180.0) 
    yawError += 360.0f;
  outDesiredRate->yaw = pidUpdate(&pidAngleYaw, yawError);
}

入口参数为三个attitude_t结构体指针，attitude_t即姿态数据结构类型。

第一个结构体为数据融合之后的输出值，也就是日志一当中说的将处理过的加速计数据与陀螺仪数据结合之后得到的姿态数据，即测量的当前角度。

第二个结构体为期望的角度值，来自WIFI/遥控RC。

第三个结构体对象即PID后的输出。三者都含有姿态数据的roll/pitch/yaw三个参数，前两个参数只要调用限幅位置式的pid校正即可，yaw由于代表的偏航角度，要限制其范围在-180°-180°之后再进行限幅位置式pid校正。

outDesiredRate为角度环PID输出的结构体对象，将在角速度环PID当中调用。

角速度环PID

角速度环PID代码如下：

void attitudeRatePID(Axis3f *actualRate, attitude_t *desiredRate, control_t *output)
{/* 角速度环 PID */
  output->roll = pidOutLimit(pidUpdate(&pidRateRoll, desiredRate->roll - actualRate->x));
 output->pitch = pidOutLimit(pidUpdate(&pidRatePitch, desiredRate->pitch - actualRate->y));
  output->yaw = pidOutLimit(pidUpdate(&pidRateYaw, desiredRate->yaw - actualRate->z));
}

第一个入口参数是一个3轴数据结构体指针，也就是日志一当中处理过的陀螺仪数据——3轴陀螺仪结构体变量sensors→gyro，即当前角速度测量值。

第二个入口参数是一个姿态数据结构体指针，指向角度环PID输出outDesiredRate，即角速度期望值。

第三个入口参数是一个控制数据结构体指针，指向control结构体变量，此结构体包含角速度PID输出数据——姿态控制量数据，即用于控制电机的三个参数roll/pitch/yaw。

函数内部则是简单的进行限幅位置式PID之后，用pidOutlimit函数限制姿态控制量，调整范围再（-32768~32768），防止调整量过大难以控制。

Z轴高度环PID

Z轴高度换PID代码如下：

void altholdPID(float* thrust, const state_t *state, const setpoint_t *setpoint)
{
  float newThrust = 0.0;
  newThrust =THRUST_SCALE * runPidZ(&posPid.pidVZ, state->position.z, setpoint,  POS_UPDATE_DT);
  if(getCommanderKeyFlight()) /* 定高飞模式检测重量*/
    detecWeight(*thrust, newThrust, state->velocity.z);
    *thrust = newThrust + posPid.thrustBase;
  if (*thrust > 60000) 
   {
    *thrust = 60000;
   } 
}

首先，实际油门值=定高油门基准值posPid.thrustBase+newThrust。定高油门基准值即定高模式下，脱控时的油门，它对悬停作用大，影响定高效果。定稿油门基准值由detecWeight函数调整，它的功能是检测四轴的原理，原理为：每次起飞之后，实时监测Z轴方向速度。当连续一段时间Z轴速度接近0，即该油门值可悬停，稍作处理后即新的油门基准值。

另一方面，newThrust来源于Z轴PID*油门放大系数，其代码如下：

static float runPidZ(pidAxis_t *axis, float input, const setpoint_t *setpoint, float dt) 
{
  float out = 0.f;
  if (axis->preMode == false && setpoint->isAltHold == true)
   {
    flag = false;
    positionResetAllPID();
    axis->setpoint = input + START_HIRHT;
    posPid.thrustBase = limitThrustBase(configParam.thrustBase);
   }
    axis->preMode = setpoint->isAltHold;
  if(setpoint->isAltHold == true)
   {
    axis->setpoint += setpoint->velocity.z * dt;
    out = pidUpdate(&axis->pid, axis->setpoint - input);
   }
  return out;
}

这俩函数的第三个参数都是一个setpoint结构体指针，调用时是同一个结构体指针。

bool型参数成员变量isAltHold标志当前控制模式，如果是定高模式即为true，手动模式为false。

preMode为上次控制模式的记录。

因此，当发生手动模式切换到定稿模式，即前者true后者false时，先是用positionResetALLPID函数清楚之前的pid参数，计算当前高度期望值，并对定高油门基准值积分。

定高模式下，先是让高度期望值对Z轴油门速度进行积分；另一方面，测量高度值来自将姿态数据的去除重力之后的Z轴加速度与气压计BMP280经过双重滤波之后的数据进行融合得到。二者相减之后，得到偏差高度，输入到限幅位置式pid之中校正得到输出out，再乘上油门放大系数即newThrust。

之后将newThrust与油门基准值相加即实际的油门值。

姿态控制量与实际油门值整合得到四轴电机PWM

代码如下：

​

void powerControl(control_t *control) /*功率输出控制*/
{
  s16 r = control->roll / 2.0f;
  s16 p = control->pitch / 2.0f;
  motorPWM.m1 = limitThrust(control->thrust - r - p - control->yaw);
  motorPWM.m2 = limitThrust(control->thrust - r + p + control->yaw);
  motorPWM.m3 = limitThrust(control->thrust + r + p - control->yaw);
  motorPWM.m4 = limitThrust(control->thrust + r - p + control->yaw);
  if (motorSetEnable)
   {
    motorPWM=motorPWMSet;
   }
  motorsSetRatio(MOTOR_M1, motorPWM.m1); /*控制电机输出百分比*/
  motorsSetRatio(MOTOR_M2, motorPWM.m2);
  motorsSetRatio(MOTOR_M3, motorPWM.m3);
  motorsSetRatio(MOTOR_M4, motorPWM.m4); 
}

[点击并拖拽以移动]
​

x轴模式下的四轴如图：

箭头方向为正方向，即机头方向。正常情况下，M4、M2逆时针运行，M3、M1顺时针运行。

先对控制结构体control当中的roll/pitch/yaw/Thrust进行说明：输出的油门值，即电机PWM应该等于Thrust加上校正之后需要调整的r、p、yaw，而后三者的符号决定方式如下：

（1）roll代表着四轴围绕X轴左右倾斜的横滚角，以右为正方向。因此当向左偏移时，应该产生一个向右的力，即M3、M4同侧出力，二者的r为“+”；而M1、M2为反侧出力，即r符号位“-”；

（2）pitch代表着四轴围绕Y轴前后倾斜的俯仰角，以前为正方向。因此当向后偏移时，应该产生一个向前的力，即M3、M2同侧出力，二者的p为“+”；而M1、M4为反侧出力，即p符号位“-”；

（3）yaw代表着四轴围绕Z轴作旋转的旋转角，以逆时针为正方向。因此当顺时针偏移时，应该产生一个使其逆时针的力，即M2、M4同侧出力，二者的yaw为“+”；而M1、M3为反侧出力，即yaw符号位“-”；

之后通过motorSetRatio函数，即可赋予各个电机对应的PWM值。

这就是大致的四轴PID算法。