手把手教你搭建ROS阿克曼转向小车之（增量式PID代码实现）

在上一篇文章中我们已经成功的把编码器的反馈值给计算出来，这篇文章将会讲解怎么使用反馈回来的速度值进行PID计算，从而闭环控制电机的速度。

PID算法介绍

1.开环控制系统

开环控制系统(open-loop control system)是指被控对象的输出(被控制量)对控制器(controller)的输出没有影响。在这种控制系统中，不依赖将被控量反送回来以形成任何闭环回路。

2.闭环控制系统

闭环控制系统(closed-loop control system)的特点是系统被控对象的输出(被控制量)会反送回来影响控制器的输出，形成一个或多个闭环。闭环控制系统有正反馈和负反馈，若反馈信号与系统给定值信号相反，则称为负反馈( NegativeFeedback)，若极性相同，则称为正反馈，一般闭环控制系统均采用负反馈，又称负反馈控制系统。闭环控制系统的例子很多。比如人就是一个具有负反馈的闭环控制系统，眼睛便是传感器，充当反馈，人体系统能通过不断的修正最后作出各种正确的动作。如果没有眼睛，就没有了反馈回路，也就成了一个开环控制系统。另例，当一台真正的全自动洗衣机具有能连续检查衣物是否洗净，并在洗净之后
能自动切断电源，它就是一个闭环控制系统。

3.阶跃响应

阶跃响应是指将一个阶跃输入（step function）加到系统上时，系统的输出。稳态误差是指系统的响应进入稳态后﹐系统的期望输出与实际输出之差。控制系统的性能可以用稳、准、快三个字来描述。稳是指系统的稳定性(stability)，一个系统要能正常工作，首先必须是稳定的，从阶跃响应上看应该是收敛的﹔准是指控制系统的准确性、控制精度，通常用稳态误差来(Steady-state error)描述，它表示系统输出稳态值与期望值之差﹔快是指控制系统响应的快速性，通常用上升时
间来定量描述。

4.PID控制的原理和特点

将偏差的比例（Proportion）、积分（Integral）和微分（Differential）通过线性组合构成控制量，用这一控制量对被控对象进行控制，这样的控制器称 PID 控制器。

PID 控制器问世至今已有近 70 年历史，它以其结构简单、稳定性好、工作可靠、调整方便而成为工业控制的主要技术之一。当被控对象的结构和参数不能完全掌握，或得不到精确的数学模型时，控制理论的其它技术难以采用时，系统控制器的结构和参数必须依靠经验和现场调试来确定，这时应用 PID 控制技术最为方便。即当我们不完全了解一个系统和被控对象﹐ 或不能通过有效的测量手段来获得系统参数时，最适合用 PID 控制技术。 PID 控制，实际中也有 PI 和 PD控制。 PID 控制器就是根据系统的误差，利用比例、积分、微分计算出控制量进行控制的。

以电机转速控制为例。之前的直流减速电机章节已经介绍了调节 PWM 占空比可以实现电机调试，编码器可以检测当前电机转速。那现在我需要控制电机转速为 3 圈/s（目标速度），并且是不同负载下都控制在这个速度。开始电机处于停止状态此时PWM占空比为0，然后我们改变占空比为45%，电机旋转，通过编码器我们得到当前的速度只有 2.5 圈/s，此时我们需要加大占空比，给到 50%，编码器得到速度才 2.8 圈/s；没办法，我们还需要再加占空比，改为 55%，编码器得到 3.1 圈/s，惨了给大了，再调，改为 54%，这次幸运了，编码器速度再 3 圈/s 左右变动，勉强满足要求。

如果现在为电机加了一些负载，本来占空比 54%有 3 圈/s 的速度的，现在下降为 2.3 圈/s 了，现在为达到 3 圈/s 速度，又要类似上面的尝试修改过程，改为60%，只有 2.5 圈/s，改为 80%，超了，到了 3.2 圈/s，改为 77%，差一点点，改为 78%，效果还不错。

上面的占空比修改过程，是通过我们人为根据编码器反馈回来的数据数据经过我们大脑处理后优化出来的调整过程。如果我现在想要编程实现这个自动调整过程：就是不管增加负载还是减少负载，都让程序自己调整占空比使得电机转速控制在 3 圈/s，程序自动调整占空比过程，不外乎当速度小了就加大占空比，速度大了就减少占空比，主要是问题是究竟大多少或者减多少，我们大脑的一般想法就是当前速度与目标速度差别大那占空比修改的幅度就大，差别小那就修改幅度小。但是，这些终究是我们自己想的，在程序里边要怎么实现呢？比较高效的做法就是使用一个数学计算公式实现，该公式有一个变量：当前速度与目标速度的速度差值（有正负值之分），公式的计算结果是占空比的修改幅度值（有正负值之分）。一般在程序中的实现方法都是把这个数学计算公式用一个函数实现。PID 算法就是解决这个问题的数学公式。实际上，我们不仅仅想通过数学公式实现占空比自动调整，并且是希望可以在很短的时间内就可以实现稳定在目标速度。
所以，一般 PID 算法要实现：快准狠。

比例（P）控制
比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。

积分（I）控制
在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分(PI)控制器，可以使系统在进入稳态后无稳态误差。

微分（D）控制
在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。其原因是由于存在有较大惯性组件（环节）或有滞后(delay)组件，具有抑制误差的作用，其变化总是落后于误差的变化。解决的办法是使抑制误差的作用的变化“超前”，即在误差接近零时，抑制误差的作用就应该是零。这就是说，在控制器中仅引入“比例”项往往是不够的，比例项的作用仅是放大误差的幅值，而目前需要增加的是“微分项”，它能预测误差变化的趋势，这样，具有比例+微分的控制器，就能够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。所以对有较大惯性或滞后的被控对象，比例+微分(PD)控制器能改善系统在调节过程中的动态特性。

5.数字PID控制

数字式 PID 控制算法可以分为位置式 PID 和增量式 PID 控制算法。

5.1位置式PID

由于计算机控制是一种采样控制，它只能根据采样时刻的偏差计算控制量，而不能像模拟控制那样连续输出控制量量，进行连续控制。 $U(t) = Kp[e(t) + \frac{1}{Ti}\int_{0}^{t}e(t)dt+Td\frac{de(t)}{dt}]$ (式1-1)

公式1-1的积分项和微分项不能直接使用，必须进行离散化处理。离散化处理的方法为：以 T 作为采样周期， k 作为采样序号，则离散采样时间 kT 对应着连续时间t，用矩形法数值积分近似代替积分，用一阶后向差分近似代替微分，可作如下近似变换：

$\left\{\begin{matrix} t\approx kT(k = 0,1,2 ...) & & \\ \int_{0}^{t}e(t)dt \approx T\sum_{j=0}^{k}e(jT)=T\sum_{j=0}^{k}e_{j} & & \\ \frac{de(t)}{dt} \approx \frac{e(kT)-e\begin{bmatrix} (k-1)T \end{bmatrix} }{T} = \frac{e_{k}-e_{k-1}}{T} \end{matrix}\right.$ (式1-2)

上式中，为了方便表示，将类似于e(kT)简化为ek,将(式1-2)代入(式1-1)就可以得到离散的PID表达式为：

$u_{k} = Kp\begin{bmatrix} e_{k}+\frac{T}{Ti}\sum_{j=0}^{k}e_{j}+Td\frac{e_{k}-e_{k-1}}{T} \end{bmatrix}$ （式1-3）

$u_{k} =Kp*e_{k} + Ki\sum_{j=0}^{k}e_{j} + Kd(e_{k}-e_{k-1})$ （式1-4）

其中：k -> 采样序号，k=0,1,2,...； $u_{k}$ -> 第k次采样时刻的计算输出值； $e_{k}$ -> 第k次采样时刻输入的偏差值； $e_{k-1}$ -> 第k-1次采样时刻输入的偏差值；Ki -> 积分系数，Ki = Kp*T/Ti；Kd -> 微分系数，Kd = Kp*Td/T；如果采样周期足够小，则离散控制过程与连续控制过程十分接近。

5.2增量式PID

所谓增量式 PID 是指数字控制器的输出只是控制量的增量∆uk。当执行机构需要的控制量是增量，而不是位置量的绝对数值时，可以使用增量式 PID 控制算法进行控制。增量式 PID 控制算法可以通过公式 1-3推导出。由公式 1-3 可以得到控制器的第 k－1 个采样时刻的输出值为：

$u_{k-1} = Kp\begin{bmatrix} e_{k-1}+\frac{T}{Ti}\sum_{j=0}^{k-1}e_{j}+Td\frac{e_{k-1}-e_{k-2}}{T} \end{bmatrix}$ (式1-5)

将公式1-3与公式1-5相减并整理得到增量式PID控制算法的公式为：
$\Delta u_{k} = u _{k}-u_{k-1} = Kp(e_{k}-e_{k-1}+\frac{T}{Ti}e_{k}+Td\frac{e_{k}-2e_{k-1}+e_{k-2}}{T})$

$=Kp(1+\frac{T}{Ti}+\frac{Td}{T})e_{k}-Kp(1+\frac{2Td}{T})e_{k-1}+Kp\frac{Td}{T}e_{k-2}$
$=Ae_{k}-Be_{k-1}+Ce_{k-2}$ (式1-6)

其中： $A=Kp(1+\frac{T}{Ti}+\frac{Td}{T})$ ， $B=Kp(1+\frac{2Td}{T})$ ， $C=Kp\frac{Td}{T}$

5.3控制器参数整定

6、代码讲解

PID代码文件在Middlewares/PID.cpp中，具体内容如下：

#include "PID.h"
PID::PID()
{
	
}

void PID::update(int min_val, int max_val, float kp, float ki, float kd)
{	
	min_val_ = min_val;
	max_val_ = max_val;
	kp_ = kp;
	ki_ = ki;
	kd_ = kd;
}

int PID::compute(int setpoint, int measured_value)
{	

  double error = 0;
  double pid = 0;

  //setpoint is constrained between min and max to prevent pid from having too much error
  if(setpoint == 0){
	  integral_ = 0;
	  derivative_ = 0;
	  prev_error_ = 0;
	  return 0;
  }
  error = setpoint - measured_value;
  if(abs(error)<0.1)error=0;
  integral_ += error;
  derivative_ = error - prev_error_;
  if(setpoint == 0 && error == 0){
    integral_ = 0;
  }
  pid = (kp_ * error) + (ki_ * integral_) + (kd_ * derivative_);
  prev_error_ = error;

  return constrain(pid, min_val_, max_val_);
}
void PID::updateConstants(float kp, float ki, float kd)
{
    kp_ = kp;
    ki_ = ki;
    kd_ = kd;
}

代码是使用C++写的，需要自己进行实例化。具体调用代码在moveBase_Task.cpp中

PID motor1_pid;
PID motor2_pid;
PID motor3_pid;
PID motor4_pid;
void setPidParam(void)
{
	switch(configParam.RobotType){
		case 1:{//d2 t2 
				motor1_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M1.K_p, configParam.p_M1.K_i,configParam.p_M1.K_d);
				motor2_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M2.K_p, configParam.p_M2.K_i,configParam.p_M2.K_d);
		}break;
		case 3:{//a1 转向舵机+两个减速电机
				motor1_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M1.K_p, configParam.p_M1.K_i,configParam.p_M1.K_d);
				motor2_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M2.K_p, configParam.p_M2.K_i,configParam.p_M2.K_d);
		}break;			
		case 4:{//a2 转向舵机+一个动力电机
			if(MotorType_t == M_ESC_ENC){		
				motor1_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
							  configParam.p_M1.K_p, configParam.p_M1.K_i,configParam.p_M1.K_d);
			}
		}break;
		case 5:{//o3
				motor1_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M1.K_p, configParam.p_M1.K_i,configParam.p_M1.K_d);
				motor2_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M2.K_p, configParam.p_M2.K_i,configParam.p_M2.K_d);
				motor3_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M3.K_p, configParam.p_M3.K_i,configParam.p_M3.K_d);
		}break;
		case 2: //d4 t4
		case 6: //o4
		case 7:{//m4
				motor1_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M1.K_p, configParam.p_M1.K_i,configParam.p_M1.K_d);
				motor2_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M2.K_p, configParam.p_M2.K_i,configParam.p_M2.K_d);
				motor3_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M3.K_p, configParam.p_M3.K_i,configParam.p_M3.K_d);
				motor4_pid.update(-configParam.Max_PWM, configParam.Max_PWM, 
								  configParam.p_M4.K_p, configParam.p_M4.K_i,configParam.p_M4.K_d);
		}break;
	}
}


mDeb_p.M1.Pwm_Out  =  motor1_pid.compute(mDeb_p.M1.Expectations,mDeb_p.M1.Feedback);
mDeb_p.M2.Pwm_Out  =  motor2_pid.compute(mDeb_p.M2.Expectations,mDeb_p.M2.Feedback);
mDeb_p.M3.Pwm_Out  =  motor3_pid.compute(mDeb_p.M3.Expectations,mDeb_p.M3.Feedback);
mDeb_p.M4.Pwm_Out  =  motor4_pid.compute(mDeb_p.M4.Expectations,mDeb_p.M4.Feedback);

通过函数update()对参数进行赋值，根据期望值和反馈值调用compute计算PID系统的输出PWM值。