无人机中的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 Signal
    float 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 Gain
    pid.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 Calculate
    pid.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 Parameters
    pid.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);

	// Gains
	matrix::Vector3f _gain_p; ///< rate control proportional gain for all axes x, y, z
	matrix::Vector3f _gain_i; ///< rate control integral gain
	matrix::Vector3f _gain_d; ///< rate control derivative gain
	matrix::Vector3f _lim_int; ///< integrator term maximum absolute value
	matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters

	// States
	matrix::Vector3f _rate_int; ///< integral term of the rate controller

	bool _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 saturation
		if (_mixer_saturation_positive[i]) {
			rate_error(i) = math::min(rate_error(i), 0.f);
		}

		// prevent further negative control saturation
		if (_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 method
		float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt;

		// do not propagate the result if out of range or invalid
		if (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 saturation
		if (_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 error
	Vector3f 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 landed
	if (!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$为一般的情况下，也有类似的分析，主要就是提高了截止频率，使得带宽增加，相角裕度减小，超调量增加，系统快速性增强。