智能车PID

马上要找工作了，最近在总结一些以前做过的项目，现在翻到17年做智能车的一些资料，想讨论一下PID。

1 PID控制理论

PID（比例-积分-微分控制器）是一种成熟而较为复杂的控制算法，在工业控制应用中最为常见的一种闭环自动控制技术。在智能车上应用PID算法能够显著地提高智能车的运行速度，减少完成时间。

2.1  什么是PID

PID的全称为比例积分微分控制，P为比例，I为积分，D为微分。PID往往都是应用于惰性系统，所谓惰性系统，就是变化较慢且无法精确控制和调节的对象。最经典的控制对象就为温度的控制。

下面举一个简单的例子进行说明。我们要对水箱里面的水进行加热，我们的加热目标为100°C，当温度加热到100°C以后，我们就停止加热，这个时候，虽然水温已经到达100且加热器已经不再通电加热，但是由于加热器的预热和水本身传递温度的惰性，导致水温会继续上升，经过一段时间后，水温会继续升高，并且会超过100，那么该系统就无法达到我们所预期的要求。

这个时候你想，停止加热后本身会继续升温，那等到温度到 90°C 左右以后，我们停止加热，然后利用水的惰性和加热器的散热，让水温 继续升温，正好达到 100℃ ，这样不就解决问题了吗？这么想是对的，但是水温 要达到 90 几度的时候我们停止加热呢？还有就是从停止加热到 100℃ 的时间 是多少？经过一段时间后，温度没有达到 100℃ ，而是小于 100 摄氏度以后温 度就达到了顶峰，这样怎么办。

上述所有的办法，可能能够解决水温到达 100℃ 的要求，但是其中很多环节 很多结果都是无法预测和无法控制的，即便经历了很麻烦的人为干预同时经过 了一个较长的时间达到了我们对水温加热到 100℃ 的要求，也要经历一个相当 复杂和相当漫长的时间才能达到，并且整个过程一直要有人为的干预，劳民伤财。

利用 PID 控制原理对水温进行加热控制，目标温度还是之前设定的 100℃，加热前我们首先要把这个温度值输入到加热器控制器中，此时温度计只是作为我们认为观察温度的一个参考，我们在水中放入一 个电子温度计，电子温度计测量的温度可以传送到控制器，使得我们的 PID 控制器知道当前的水温，在刚刚通电加热的时候，水温假定为室温 20℃，当前的水温会通过电子温度计测量并传输到 PID 控制器中，此温度作为当前温度值 C_TEM，我们的目标温度值设定的为 100℃ 作为 T_TEM，那么 CPU 通过计算 T_TEM-C_TEM=80，二者的差值较大为 80，说明水温远远没有达到我们 所需的 100℃，那么这个时候 CPU 就给加热器通上最高的供电电压，以全速对 水箱里面的水进行加热，随着加热的进行，水温会慢慢的增加，某一时刻，水温达到了 80℃，那么目标温度与当前温度做差，差值仅仅为 20，那么 CPU 就知道了当前的温度跟实际我们所希望的温度差别已经很小了，那么这个时候控制器就减小加热器的供电电压，以一个适当的加热量对水温进行加热。当到了一个时刻，水温达到了 95℃，目标温度和他的差值仅仅只有 5 了，说明温度已经十分接近目标温度，此时控制器可能已经早已停止对加热器供电或者以一个更加低电压的加热量对水温进行加热，直到水温加热到 100℃。当然水温不可能一直恒定在 100℃，水温一定会超过 100 ℃，但是由于采用 PID 闭环控制，水温不会超过的太多，比如某一时刻水温到达了 105℃，做差后，差值为-5℃，CPU 就知道此时温度已经超过了目标温度，那么将会在一段时间内停止加热，直到温度降低到 100℃ 或者 100℃ 以下后再进行进一步控制，所以当水温到达 100 ℃附近的时候，CPU 控制器将会频繁的对加热器进行控制，从而导致水温永远维持在 100℃ 左右附近，而且水温与 100℃ 的目标温度相差不会很大，宏观上通过平均测量，水温就是认为在 100℃ 了，其实根本上，水温是永远不会达到正好的 100℃，只能是在 100℃ 左右进行震荡，但是震荡始终不会太大，从而达到了我们控制目标对象的要求，使得加热系统变得更加科学更加可控。

当然上述所说明的 PID 加热状态是 PID 各项参数调节比较好的情况下的状态，PID 三个参数调整不好，会带来很多问题，比如系统震荡，超调等问题，所以调整 PID 参数也是一个比较重要的环节，这个我将会在下面进行说明。上述对水温的 PID 控制已经基本上说明了 PID 控制的核心，我们之所以利用 PID 进行闭环控制，就是为了让被控制对象快速稳定的达到目标值，从而实现对被控制对象的可控性，可预知性。

2.2 PID理论依据

2.2.1 PID控制原理框图

上图是PID控制原理框图，r(t) 是给定值，也可以说是当前值，比如当前小车的速度是 2m/s；y(t) 是设定值，也可以说是最终希望达到的值，比如我们希望把小车速度调节到 3m/s；e(t) 是控制偏差值，也就是 e(t)=y(t)-r(t), 比如目前偏差值就是 1m/s；u(t) 是控制量，也就是你要实际控制调节的目标，之前我们学过直流电机，我们知道电机速度是由 PWM 占空比调节的，这个 u(t) 就是PWM 占空比的改变值，增加多少或者减少多少。

了解了这些变量的含义后，我们用语言概括一下这个 PID 框图：当当前值 r(t) 和设定值 y(t) 存在控制偏差 e(t) 时，通过比例、积分、微分运算计算出控制量 u(t) 提供给执行机构（比如电机），当前值改变。如果此时调节目的完美达到（当前值和设定值一样），那很好，收工走人；如果还存在一丢丢误差，比如现在是 2.7m/s ，那么我们将此时的当前值再输入一次，计算控制量；如果存 在很大的误差，说明参数没调好 ，回去好好调参数吧。

2.2.2  比例、积分、微分运算

如果我们将偏差直接乘以一个比例系数作为控制量，也就是 PWM 占空比的改变值，可不可以呢？我知道大家很容易就会想到这种控制方式。占空比和转速成正相关，但不是正比，因此直接乘以一个比例系 数并不靠谱准确。所以 P（ proportion 比例）控制理论一般是不好使的，这时候科学家们就加入了积分（integral ）项和微分（ derivative ）项，总称为 PID 控制理论公式如下：

将上述的 PID 算法离散化就得到数字 PID 控制算法，分为位置式 PID 和增量式 PID 控制算法，智能车中常用增量式 PID 控制算法。知道大家不会看推导过程，这里我直接给出结论：

2.2.3 三个解释PID的小故事

最后我们用三个网上大神写的小故事加深对 PID 的理解。

1、小明加水

小明接到这样一个任务：有一个水缸点漏水 ( 而且漏水的速度还不一定固定不变) ，要求水面高度维持在某个位置，一旦发现水面高度低于要求位置，就要往水缸里加水。

小明接到任务后就一直守在水缸旁边，时间长就觉得无聊，就跑到房里看小说了，每 30 分钟来检查一次水面高度。水漏得太快，每次小明来检查时，水都快漏完了，离要求的高度相差很远，小明改为每 3 分钟来检查一次，结果每次来水都没怎么漏，不需要加水，来得太频繁做的是无用功。几次试验后，确定每 10 分钟来检查一次。这个检查时间就称为采样周期。

开始小明用瓢加水，水龙头离水缸有十几米的距离，经常要跑好几趟才加够水，于是小明又改为用桶加，一加就是一桶，跑的次数少了，加水的速度也快了，但好几次将缸给加溢出了，不小心弄湿了几次鞋，小明又动脑筋，我不用瓢也不用桶，老子用盆，几次下来，发现刚刚好，不用跑太多次，也不会让水溢出。这个加水工具的大小就称为比例系数。

小明又发现水虽然不会加过量溢出了，有时会高过要求位置比较多，还是有打湿鞋的危险。他又想了个办法，在水缸上装一个漏斗，每次加水不直接倒进水缸，而是倒进漏斗让它慢慢加。这样溢出的问题解决了，但加水的速度又慢

了，有时还赶不上漏水的速度。于是他试着变换不同大小口径的漏斗来控制加水的速度，最后终于找到了满意的漏斗。漏斗的时间就称为积分时间。

小明终于喘了一口，但任务的要求突然严了，水位控制的及时性要求大大提高，一旦水位过低，必须立即将水加到要求位置，而且不能高出太多，否则不给工钱。小明又为难了！于是他又开动脑筋，终于让它想到一个办法，常放一盆备用水在旁边，一发现水位低了，不经过漏斗就是一盆水下去，这样及时性是保证了，但水位有时会高多了。他又在要求水面位置上面一点将水凿一孔，再接一根管子到下面的备用桶里这样多出的水会从上面的孔里漏出来。这个水漏出的快慢就称为微分时间。

微分的比喻一点牵强，不过能帮助理解就行了 。故事中小明的试验是一步步独立做，但实际加水工具、漏斗口径、溢水孔的 大小同时都会影响加水的速度、水位、超调量的大小，做了后面的实验后，往往还要修改改前面实验的结果。

2、控制模型

假设，以 PID 控制的方式用往水杯里倒水，目标是到印有刻度的位置停止，定义：

设定值：水杯的刻度

实际值：水杯的实际水量

输出值：倒水数量和舀出水量

测量传感器：人的眼睛

执行对象：人

正执行：倒水

反执行：舀水

（1） P 比例控制

就是人看到水杯里水量有没有达到水杯的刻度，就按照一定水量往水杯里倒水或者从水杯里舀水出来，这个一个动作可能会造成不到刻度或者高于刻度就停下来。

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

（2） PI 积分控制

就是按照一定水量往水杯里倒，如果发现杯里的水量没到刻度就一直倒，后来发现水量超过了刻度，就从杯里往外面舀水，然后反复不够就倒水，多了就舀水，直到水量达到刻度。

说明：在积分 I 控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System with Steady-state Error ）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增

加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例 + 积分（ PI ）控制器，可以使系统在进入稳态后无稳态误差。

（3） PID 微分控制

就是人的眼睛看着杯里水量和刻度的距离，当差距很大的时候，就大量得倒水，当人看到水量快要接近刻度的时候，就减少的倒水量，慢慢的逼近刻度，直到停留在杯中的刻度。如果最后能精确停在刻度的位置，就是无静差控制；如果停在刻度附近，就是有静差控制。

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

3 PID算法的实现

由于位置式PID算法的计算量很大，大多数都是采取增量式PID算法。

3.1 理论依据

 增量式

 3.2 PID结构体

首先我们在PID.h中定义了一个PID结构体，结构体名为PID。

typedef struct PID{
  int SetPoint;//设定目标
  int Proportion;//比例常数
  int Integral;//积分常数
  int Derivative;//积分常数
  int LastError;//Error[-1]
  int PreError;//Error[-2]
  int SumError;
}PID;

与公式相对应，PID结构体中存有设定目标，比例常数、积分常数、微分常数、上一次的偏差量、上上次的偏差量、总偏差。

3.3 int PIDCal(PID *pp, int ThisError);

int PIDCal( PID *pp,int ThisError){

int pError, iError, dError;
long templ;
pError = ThisError−pp−>LastError ;
iError = ThisError ;
dError = ThisError −2*(pp−>LastError)+pp−>PreError ;

//增量计算
templ = pp->Proportion*pError + pp−>Integral*iError+pp−>Derivative*dError; // 增量

//存储误差用于下次计算
pp->PreError = pp->LastError;
pp->LastError = ThisError;
return templ;

}

PIDCal 是 PID 计算函数，它有两个形式参数， PID 指针 pp 和当前误差ThisError（当前值与设定值的差）。仔细阅读代码我们知道，这个 PID 计算函数的功能主要有两个：

1 、根据理论公式，计算得出控制修正量作为函数返回值

2 、更新总误差、上一次偏差量和上上次偏差量。

3.4 void PIDInit (PID *pp)

void PIDInit(PID *pp) 为 PID 变量初始化函数，这个函数的功能是初始化 PID变量中各个变量的值。设定值、比例常数、积分常数、微分常数应该在这个函数中进行修改设定。

3.5 int sensor (void)

int sensor (void) 这个函数在官方代码中是个空函数，我给它预期设计的功能是读取输入变量，也就是获得当前值，进而算出当前误差值。

3.7 函数的使用方法

要想使用 PID 算法，需要先定义一个 PID 变量，比如“ PID PIDcontrol;” ，然后定义一个 PID 指针使它指向这个 PID 变量，比如 ”PID *pidcontrol=&PIDcontrol”, 然后调用初始化函数将 PID 变量初始化，注意参数为指针，比如 PIDInit（pidcontrol ） ; 至此我们就做好了准备工作。

我们执行 PID 控制策略也是遵循这样一种每隔一段时间就去查看一次的思路，所以要与定时器结合使用。每当闹钟（定时器，一般可以取 100ms ）响起，我们调用 int sensor (void) 函数读取当前的水位并计算它与设定值的差ThisError，然后将这个值带入 int PIDCal( PID *pp, int ThisError ) 中计算控制修正量，也就是水龙头要调大调小多少，最后执行 void actuator(int rDelta)输出控制，也就是依据计算结果去操作水龙头，这样我们就完成一步操作，等到下次闹钟响起，周而复始。

4 PID 算法在智能车中的应用

整个智能车基本上就在做两件事，一个是方向的控制，一个是速度的控制（当然平衡车还多一个平衡的控制），只要能够将这两个量给控制的恰到好处（恰到好处不光是两者单独控制的很好，而是两者一起工作时配合的很好）。

4.1 PID 速度控制

PID 就是一种快速而准确地将系统的某一状态调节到设定值并保持稳定的控制算法。比如，我的小车现在是 0m/s 的速度，我想让它的速度稳定在 3m/s ，那我用 PID 算法就很容易做到。当然这一切的前提就是安装了编码器，车速有反馈，只有加上编码器，有了反馈，才能组成一个闭环系统。当然您也可以加上码盘，或者霍尔开关等一切可以返回车速的东西都可以。

我们先说说为什么我们要反馈车子速度，要知道车子当前的速度？因为我们的 CPU 利用传感器采集道路信息，通过 CPU 收集和处理变化，系统就能知道车子当前的状态，比如是在直道上？还是在进入弯道？还是在弯道内？还是正在出弯道？还是在上坡？还是穿过了起跑线？有了这些信息，我们首先就是对舵机的控制，控制车子根据什么道路情况来进行转弯，转弯的方向，大小，都是

靠这些数据来实现，当然当前的速度值也是控制舵机转向大小的一个附加因素， 这样更加的有利于我们及时处理不同弯道。知道了这些信息，就跟我们开车一样，我们要转弯了，得感知我们车的速度，怎么感知啊，就靠汽车自带的速度表，这样我们就可以通过控制刹车或者油门还有方向盘来控制车子平稳的转过弯道，不同的弯道，不同的速度，转弯的控制和刹车的控制都是不同的。如果

我们不知道当前的速度，那么很可能因为拐弯速度过快，导致翻车，或者侧滑， 造成危险。智能车情况跟真车是一样的，我们必须知道车子当前速度，比如我 们智能车拐弯安全速度是 3M/S，所谓安全速度就是车子顺利安全且没有较大侧滑过弯时的速度，在进入弯道前我们车子的速度是 4M/S ，当传感器感知要进入弯道了，通过跟安全速度对比，我们发现智能车的速度远大于安全弯道速度，这个时候我们就要刹车，把车子速度快速降低到 3M/S ，这样保证我们顺利快速的过弯。在这里您可能问 3M/S 的安全速度怎么来？其实很简单这个速度是靠实验来的，不用什么计算，完全属于经验值，根据跑道的材料、车子机械性能和弯道的角度来决定，所以为什么我们要比赛的时候要试车，试车中一个最重要的环节就是熟悉比赛跑道所用的材料，用于我们实验车子的安全速度等。

首先建议在车速比较慢的时候，采用 PID 算法来控制电机，为什么开始要建议您采用 PID 呢？主要是为了可以更加深刻理解 PID 算法的精髓和调试步骤方法等，有助于以后对控制算法更加深入的研究和书写。调试 PID 三个参数的方法，很多地方都提供了，我在这里简单的说下：首先将 ID 参数都变为 0 ， 先调整 P 比例参数，调整到速度基本上跟您给定的速度差不多，也就是说基本稳定在给定的脉冲数，当然这个时候会非常的震荡，不要担心，接下来调整I，调整 I 的结果就是震荡会消除很多，但是车速会变化缓慢，也就是说会有一些延迟，然后再调整 D ，调整 D 的结果就是增强调节的灵活性和预见性，在给定速度变化的过程中，能够以一个平稳过渡来变换，而且速度可以长时间稳定在给定速度附近，然后 PID 三个参数的基本范围就确定了，然后再根据实际的跑车来微调这些参数，当然在调试 PID 之前，请仔细阅读 PID 理论知识，这样有助于您的调试和理解，当把 PID 调整好以后，就基本上领会了 PID的精髓，对于以后的调试作用非常大。这里的给定速度，就相当于上面对温度例子中说的设定温度。意思是一样一样的。

如果我们一段时间要将车速稳定在 3m/s ，一段时间要将车速稳定在 2m/s怎么办？这种时候我们就需要两个 PID 变量、两个 PID 指针、两次调参数喽。

4.2 PID 方向控制

4.2.1 系统分析

方向和速度控制均为周期性控制，但控制的周期可能不一样。方向控制的主要步骤如下：

1 、采集方向传感器的信息，比如对于摄像头就是进行图像的采集；

2 、方向偏差信息的求取，对于摄像头来说就是图像中线的求取，进而得到得到车体当前位置与理想位置的偏差；

3 、方向控制模块根据这个偏差来计算应该输给舵机的 PWM 占空比，进而来调整车体的前进方向。

从控制系统的角度来看，典型的方向闭环反馈系统结构图如下图所示，主要由五部分构成：控制目标、被控对象、执行机构、测量反馈、控制算法（这里面就是我们常说的核心 PID 啦）。在构思控制算法之前你首先需要对执行机构、被控对象、测量反馈和控制目标这四个部分有足够的了解。控制目标是首当其冲要考虑的。本系统要控制的物理量是什么？

最开始想的肯定是保持车体的方向和位置在车道的正中央。仔细思考一下可 以发现，其实坚持的方向一直正确，最后的位置就一定会正确 ，而且车体的位置又是不可控（无能的舵机只能控制方向），因此其实控制目标可以换成只需要车体的方向保持正确就 OK 了，因此此时的系统变为如下，要控制的物理量是车体前 进方向，接下来要考虑执行机构与控制对象的特性。本系统中的控制对象是什么？值得注意的是，本系统的执行机构是舵机驱动与舵机，被控对象是车体。这一点重点关注执行机构与被控对象的输入-输出特性，即输入 PWM 的占空比舵机转角的关系，舵机转角和车体转角的关系（为方便这里均近似看成是一个线性的比例关系，只是近似而已。在接下来就该看测量反馈。一个控制系统的精度的上限是由反馈测量部分精度决定，但是和这个上限的接近程度是由控制器来决定的。控制策略再好，你测量的有误差也是白控制。因此才开始大家一定要注意将测量部分精度尽量提高，这样不仅能够大大减少控制部分的工作量，还能够有效提高控制精度。个

人感觉测量部分的重要性略大于控制部分。假设测量反馈的输入是摄像头图像， 输出是方向信息，关键是怎么由图像得到方向信息？方向信息怎么表示？

关键问题就在于，从系统的角度来看我们可以采取怎么的方法来提升整体的控制精度？整个系统的关键部分在哪？如何改善关键部分的性能？

1 ）执行机构与控制对象的改进。舵机的机械传递结构改善、轮胎的摩擦力改善使控制的灵敏度更高，响应更快；使总体的输入输出能更接近线性比例关系（线性越好越容易控制）

2）测量反馈的改善。如何让测量的信息能够更真实的反映出车辆的方向信 息。对于电磁传感器来说，那就是如何摆布各个电感的位置，如何使采集到的电压值更好的反应出方向信息（原始数据 滤波融合等）等；

3 ）控制策略的改善。动态 PID ，模糊控制，位置 / 增量 PID ，积分限幅，四轮车后轮双电机差速的协调控制等等。

4.2.2 PID的理解

PID 虽然是最简单的控制器，应用却是最广泛的，实际生活中 95% 以上的控制都是 PID 控制。还有很多其他控制方法，想法很好但是应用的实际效果却并不比 PID 好。因此对于 智能车控制来说， PID 完全足矣。

PID 的含义？（ Proportion 比例 +Differential 微分 +Integral 积分）

输出量 =P* 误差 +D* 误差的变化 +I* 误差的积分

一、 P （比例）控制， Kd 与 Ki 为 0 ，最简单的控制规律如直道（误差为 0 ）时舵机占空比 DUTY ，则当误差为 error 时，输出的占空比直接是 DUTY+Kp*error 。

二、 PD （比例微分）控制， Ki 为 0 考虑偏差及偏差的变化趋势，当误差为error 时，输出的占空比是 DUTY+Kp*error+Kd*(current_error-last_error) 。可以看出，对于同样的一个偏差：

1 ）若偏差正在减小的过程， PD 控制的量要比纯比例控制量小一些，这样能够避免过度控制

2 ）若偏差在逐渐增加的过程， PD 所要施加的量比纯比例要多一些，目的是为了抑制误差增大的趋势。所以 D 这一项有一些预测控制的味道 ，相比 P 而言要更智能一些。

三、 PID （比例微分积分控制）相比 PD 而言，多了一项积分项，目的是为了使系统无误差，将系统在整个过程中的误差考虑进去。当误差为 error 时，输出占空比 DUTY+Kp*error+Kd*(current_error-last_error)+Ki* error_integral 积分这一点对于方向控制来说意义不大，速度控制会需要。不过有兴趣的均可以尝试一下。

4.2.3 参数调试

PID 的控制算法非常简单，写起来总共也就 10 行不到，但是你们后期的很大一部分时间还得花在这上面，这是为什么？最耗时间的地方是什么？那就是—— 参数的调试，也是你们在调试时从不会间断的一件事。 理论上其实有很多参数调试方法，来快速得到一个合适的参数，但是经过实践检验的最实用的却还是—试凑，真是没办法，控制领域的科学家们努力了那么多年始终没能够找到一个特别行之有效的方法，只是因为工程实际与理论相差太远。不过试凑也是要讲科学的，试凑的依据就是下面同 P 不同 D 的曲线。其中横坐标为时间，纵坐标为输出（图中的 D 参数值只是示意大小关系，由此图可以大概得到某个参数应该调大还是调小），虚线为理想输出。可以看出：

同 P 不同 D 曲线示意图

1）对于同样的 P 而言，随着 D 的增加系统的超调量会越来越小，但是若 D增加的太大会造成系统响应太慢，达不到控制要求。

2 ）对于同样的 D 而言， P 增加过大会使系统震荡，超调大。对于方向控制而言，四轮车的 D 参数效果不太明显，但是平衡车的方向 D 参数调起来效果特别明显。

这个曲线的结论反映到你们实际的车上就是下面的行驶轨迹：

无 D 参数或 P 过大时，拐弯以后抖动比较厉害

 

 有 D 且 D 比较合适时，此时拐弯造成的抖动会明显减小

 D 参数太大时，此时将会拐不过去