无人机中的PID控制代码略解

无人机中的PID控制代码略解

• PID的控制规律：

参考：Amov实验室-PX4中级课程-PID基础

频域函数：
$$\frac{u(s)}{err(s)}=k_p+\frac{k_i}{s}+k_{d}s$$
写成时域的函数：
$$u(t)=k_p(err(t)+\frac{1}{T_i}\int err(t)dt+T_d\frac{derr(t)}{dt})\text{\hspace{1em}(1)}$$

假设采样间隔为T，则在第K个T时刻，有：
$$err(K)=R_{in}(K)-C_{out}(K)$$

对于积分环节，按照定义则有：
$$I(K)=T\ast (err(0)+err(1)+\cdots +err(K))=T\sum _{i=0}^{K}err(i)\text{\hspace{1em}(2)}$$
对于微分环节，按照定义则有：
$$D(K)=\frac{err(K)-err(K-1)}{T}\text{\hspace{1em}(3)}$$
将$(2),(3)$代入回$(1)$则有：
$$u(K)=k_p[err(K)+\frac{T}{T_i}\sum _{i=0}^{K}err(i)+T_d\frac{err(K)-err(K-1)}{T}]$$
化简一下则有:
$$u(K)=k_{p}err(K)+k_i\sum _{i=0}^{K}err(i)+k_d[err(K)-err(K-1)]\text{\hspace{1em}(4)}$$
为了减小存储空间，还有增量式PID，可写为：
$$\Delta u(K)=k_p[err(K)-err(K-1)]+k_i\cdot err(K)+k_d[err(K)-2err(K-1)+err(K-2)]$$
则$u(K)$还可以写为：
$$u(K)=u(K-1)+\Delta u(K)$$

• PID的C语言代码初步实现：

//定义pid结构体，定义主要参数
struct _pid{float SetSpeed;float ActualSpeed;float err;float err_last;	//err(K-1)float Kp,Ki,Kd;float voltage;//Object Signalfloat integral;float err_llast;//err(K-2)
}pid;//初始化pid模块
void PID_init(){pid.SetSpeed = 0;pid.ActualSpeed=0.0;pid.err = 0;pid.err_last = 0;pid.voltage = 0;pid.integral = 0;//Set Each PID Gainpid.Kp = 0.4;pid.Ki = 0.3;pid.Kd = 0.2;pid.err_llast = 0;
}//pid实现，包括增量形式的pid，输入为目标速度，输出为当前的速度
float PID_realize(float speed){pid.SetSpeed = speed;pid.err = pid.SetSpeed - pid.ActualSpeed;//PID Calculatepid.voltage = pid.Kp*pid.err + \pid.Ki*pid.integral + \pid.Kd*(pid.err-pid.err_last);//Increment Type
/*  float incrementVoltage;incrementVoltage = pid.Kp*(pid.err-pid.err_last)+\pid.Ki*pid.err+\pid.Kd*(pid.err-2*pid.err_last+pid.err_llast);pid.voltage += incrementVoltage;pid.err_llast = pid.err_last;
*/    //Update Parameterspid.err_last = pid.err;pid.ActualSpeed = pid.voltage*1.0;return pid.ActualSpeed;
}

• PX4中姿态控制介绍：

PX4中的控制主要分为***姿态控制*** 和***位置控制***，这两者都采用***串级$PID$*** 控制，这样的好处是可以增加系统的稳定性，抗干扰，因为参数更多了，直观上自然能够适应的情况就相对来说多一点。位置控制中外环是位置，内环是速度，由内环得到的速度积分就是位置的改变量。而姿态控制中，外环是角度差，内环是角速度差，这是因为角度的变化同时也是由角速度过渡来的。参考：Pixhawk-串级pid介绍

因为时间关系，我下面介绍一下内环角速度PID控制的实现：

1. 首先是角速度PID环控制器的类，成员函数主要由：设置初始PID增益，设置积分器的最大绝对值，设置前馈增益，饱和状态（和Mixer有关），重设积分项，获取状态信息，积分项特别调控以及PID最终的计算结果实现。不要看其这么复杂，其实主要的函数就是最后两个，而这两个函数也只是在我之前介绍的简易PID实现的基础上加了一点点工业上的限制。

class RateControl
{
public:RateControl() = default;~RateControl() = default;/*** Set the rate control gains* @param P 3D vector of proportional gains for body x,y,z axis* @param I 3D vector of integral gains* @param D 3D vector of derivative gains*/void setGains(const matrix::Vector3f &P, const matrix::Vector3f &I, const matrix::Vector3f &D);/*** Set the mximum absolute value of the integrator for all axes* @param integrator_limit limit value for all axes x, y, z*/void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };/*** Set direct rate to torque feed forward gain* @see _gain_ff* @param FF 3D vector of feed forward gains for body x,y,z axis*/void setFeedForwardGain(const matrix::Vector3f &FF) { _gain_ff = FF; };/*** Set saturation status* @param status message from mixer reporting about saturation*/void setSaturationStatus(const MultirotorMixer::saturation_status &status);/*** Run one control loop cycle calculation* @param rate estimation of the current vehicle angular rate* @param rate_sp desired vehicle angular rate setpoint* @param dt desired vehicle angular rate setpoint* @return [-1,1] normalized torque vector to apply to the vehicle*/matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp,const matrix::Vector3f &angular_accel, const float dt, const bool landed);/*** Set the integral term to 0 to prevent windup* @see _rate_int*/void resetIntegral() { _rate_int.zero(); }/*** Get status message of controller for logging/debugging* @param rate_ctrl_status status message to fill with internal states*/void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status);private:void updateIntegral(matrix::Vector3f &rate_error, const float dt);// Gainsmatrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, zmatrix::Vector3f _gain_i; ///< rate control integral gainmatrix::Vector3f _gain_d; ///< rate control derivative gainmatrix::Vector3f _lim_int; ///< integrator term maximum absolute valuematrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters// Statesmatrix::Vector3f _rate_int; ///< integral term of the rate controllerbool _mixer_saturation_positive[3] {};bool _mixer_saturation_negative[3] {};
};

2. 说一下积分项特别调控和PID的计算输出两个模块：

积分项特别调控：

void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
{for (int i = 0; i < 3; i++) {// prevent further positive control saturationif (_mixer_saturation_positive[i]) {rate_error(i) = math::min(rate_error(i), 0.f);}// prevent further negative control saturationif (_mixer_saturation_negative[i]) {rate_error(i) = math::max(rate_error(i), 0.f);}// I term factor: reduce the I gain with increasing rate error.// This counteracts a non-linear effect where the integral builds up quickly upon a large setpoint// change (noticeable in a bounce-back effect after a flip).// The formula leads to a gradual decrease w/o steps, while only affecting the cases where it should:// with the parameter set to 400 degrees, up to 100 deg rate error, i_factor is almost 1 (having no effect),// and up to 200 deg error leads to <25% reduction of I.float i_factor = rate_error(i) / math::radians(400.f);i_factor = math::max(0.0f, 1.f - i_factor * i_factor);// Perform the integration using a first order methodfloat rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt;// do not propagate the result if out of range or invalidif (PX4_ISFINITE(rate_i)) {_rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));}}
}

首先是积分饱和的限制：

		// prevent further positive control saturation	if (_mixer_saturation_positive[i]) {rate_error(i) = math::min(rate_error(i), 0.f);}// prevent further negative control saturationif (_mixer_saturation_negative[i]) {rate_error(i) = math::max(rate_error(i), 0.f);}

在实际过程中，如果系统一直存在一个方向的偏差，PID的输出因为积分项的存在可能会变得非常大，但是执行器的输入是有限的。当执行器的输入达到上限，PID的输出持续增大，此时就进入了饱和区，这主要是由于积分项饱和造成的。一旦系统出现了反向的偏差，此时积分项就要逐渐减小，但是由于进入了饱和区，不可能一下从饱和区中退出来，使得执行机构的输入一直在极限状态，无法立刻减小，进入饱和区时间越长，越难退出来，这就导致了像是失控一样现象。所以为了抑制这种现象，必须让积分项在PID的输出达到执行器饱和的时候停止让PID输出的绝对值继续增加，包括正向和负向，这就有了上面这两句函数。

其次注意这个函数中以下两句函数：

		float i_factor = rate_error(i) / math::radians(400.f);i_factor = math::max(0.0f, 1.f - i_factor * i_factor);float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt;

这个对于积分项的计算可以看成如下的公式：
$$RateIntegral(K)=\sum _{i=0}^{K-1}err(i)+i_{factor}\ast k_i\ast err(K)\ast dt$$
其中，$i_{factor}$的计算如下：
$$i_{factor}=1-(\frac{err}{400^{\circ }}{)}^2$$
整个函数限制了积分增益不能太大，当期望的角速度误差在$200^{\circ }$的时候，就可以看到积分增益减少到25%了，而当角速度误差超过$400^{\circ }$的时候，此时积分的增益就会完全为0。这样做是为了防止积分项导致PID的输出非常巨大，产生超调，甚至引起积分饱和。

PID最终的计算：

Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,const float dt, const bool landed)
{// angular rates errorVector3f rate_error = rate_sp - rate;// PID control with feed forward// 带有前馈的PID控制// torque=p*rate_error(比例项)+_rate_int(积分项)-d*angular_accel(微分项)+ff*rate_sp(前馈)const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp);// update integral only if we are not landedif (!landed) {updateIntegral(rate_error, dt);}return torque;
}

这里大家可能会有疑惑，为什么要加一个前馈的控制呢？实际上这和积分项的滞后有关，加入前馈调节可以使得系统的控制速度更快，提高系统的响应速度：参考：多旋翼姿态控制中前馈的作用

可以看到，其中在外环已经产生一个角速度期望控制了，可是由于内环积分器的滞后（分析积分环节的频域响应可知，中高频段的相角滞后严重)，前馈的作用是跳过积分器，直接从输入到输出，以提高系统的响应速度。

设PID控制环节的传递函数$G_c(s)=K_p+\frac{K_i}{s}+K_{d}s$：

则有不加前馈的开环传递函数（从${\theta }_d$到$\theta$）:
$$G_1(s)=\frac{P{G}_c}{J{s}^2+G_{c}s}$$
加了前馈的开环传递函数为：
$$G_1(s)=\frac{PK+P{G}_c}{J{s}^2+G_{c}s}$$
假如我们简略一点，把$G_c$看成常数，也就是说只有比例增益，至少很容易看出此时这个二阶系统的增益变大了，这样的话系统的响应速度变快，稳态误差减小；同时系统的自然频率变大，阻尼比不变，相应的系统的闭环带宽增加，系统能够响应更多的高频分量使得控制系统的快速性提高。$G_c$为一般的情况下，也有类似的分析，主要就是提高了截止频率，使得带宽增加，相角裕度减小，超调量增加，系统快速性增强。