智能车PID算法实现原理讲解

1、为了实现PID控制所需要的等同隔采样，我们便用了一个定时中Bi,毎2ms a展一次数船 采样和PIDit算。与此并行，系筑中还设it 了一个转速脉冲检测巾斷，从而实现了转速检测。 为了调试的需要，程序中还在main函数中参加了相关的调试代昭，这局部代码有最低的优先 级，可以在保ME不影晌控斟策略的悄猊下实现发送调试数据等助能。检测坏节对整个控制系筑 的质量起到至关重要的作用控匍调整速度本系筑果用的是帽量式数字PID控制，通il毎一腔制周期(10ms)读人脉冲数间接测借 小车当前转速vi_FeedBack,将vi_FeedBack牙触fij推理借到的小车期望速IS vi_Ref比拥，由以 下公式

2、求得速度漏差errorl与速度偏差率d_errorQerrorl = vi_Ref- vi_FeedBack;公式3d_error = errorl-vi_PreError;公式4公式4中，vLPreError为上次的速度偏差。考虑到控制周期较长，假设按2.5m/s的平均速度it 算，那么一 f控制周期小车大Ift可以胞12 2.5cm,如果按这种周期用上述PID调节速IJ, SIJ么 会导致加速械速血11长的后果，严重的影哨小车的快速性和稳定性。为了解决逆f问题，可以 在PID调速控斟中参加BANG-BANG控初思想：根« errorl H大小,血果正大，期么正转给全 额占空比；如

3、果负大，册么自由停车或给一个反转占空比；否期么就果用PIDit算的占空比。4. 2.3 PID算法程序实现PID算法在参数设垃合理的悄况卜可以达到很好的控制效果。通过理论分析， 加入积分环节可以消除稳态误差，加入微分可以提高响应速度。通过实际对小 车的试验中，积分环节效果十分明显，即便在积分系数I很小的时候，依然会造 成舵机响应的缓慢，使得小车难以及时对赛道的变化作出响应；同时在比例控 制匚小车在直道行驶过程中已经基本没有稳态误差存在，因此加入积分环节 意义不大。在实际系统中，小车的PID参数是枢据路况来进行动态调整的，尽帚鼓大限 度的扬长避短。小车系统中，PID控制器以出例（P）控制为主，对

4、于积分（I）和微分（D） 采取有条件的选用,具体操作方法如F：当小车进入弯道的时候,加入微分环 节，以使舵机迅速反应，克服弯道扰动；当进入直道叭加入积分控制，使小 车保持平稳运行，但积分系数（I）取值微小，避免山于反应过慢耐I起的小车 运行不就图4中的振蕩分析棟块对于控制过程至关重要，既是采用PI或者PD控制的 依抚同时也是系统对于采用其他辅助控制算法的依据。该模块通过计算近一 段时间（50次传處器采样）内偏差1：的均方差来得岀小车运行路铳同实际路线 脑散程度即判断小车是否运行稳定，具胸节将在5.1节中进行介纽根据振荡分析模剜输岀结果，程序会判断是采用PI控制还是PD控制或者 单纯的P控制，使

5、得小车控制更具针对性,效果更好。图4中的振荡分析模块对于控制过程至关重要，既是采用PI或者PD控制的 依据，同时也是系统対于采用其他辅助控制算法的依据。该模块通过计算近- 段时间（50次传感器采样）内偏差量的均方差来得出小车运行路线同实际路线 脈散程度即判断小车是否运行稳定，具体细节将在5.1节中进行介绍。根据振荡分析模块的刪结果,程序会判断是采用PI控制还是PD控制或者 单纯的P控制，使得小车控制更具针对性，效果更好。位置信息%例控制图4 PID算法示意團5. 1振荡检测本项口中小车对于行驶策略的选择都是依据小车对振荡情况的判断。该振荡 检测模块就是实现判别小车振荡剧烈程度（即稳定性系数）的

6、功能。对于振荡检测，通过分析小车最近一段时间的振荡状况来进行判断。该算法 使用一个链表来存储位置偏差信息，记录最近50次的偏差值，在第51次传感 器信息到来时，把放开始第一次的数据整个存储空间中弹岀，以保证链表中始 终保存最近50次的位置倍息。将这50次偏差的均方差和标准差，作为稳定性 系数。每次采样结束，程序进行一次链表的操作，进行一次均方差和标准差的 计算，并对稳定系数进行判断，是否进入振荡状态。的存储空间。当然，具体压缩效率山指令变化频繁程度血有所不同。当小车首圈运行结束，通过判断赛道的起始线来确定是否调用记忆的路况。 则在以后数圈内，小车运行到赛道的特定部位时，可以不依赖传感器，而根据

7、 经验来进行更有效的控制。本项口中，小车通过计算进入直道的时间，可以在 弯道到来之前进行减速，在连续小幅弯道到来时时，可以宜接从道路中心穿越 而不必完成连续拐弯的动作。PID控胃算法为了使赛车平滑得保持在黑线中央，H1使赛车的18務量平滑地保持在0,实用了 PID控制 算法。P为比例参数，D为18分参数。基准值为0, PID输人为水平偏務量X0, PID输出为转角, 转角方向：向左转为正，向右转为负。P参数在智能车控斟器中表示水平偏差量的权，D参数在智能车腔制器巾表示水平偏差速 度的权。水平偏差量直接反映了赛车偏离黑线的程度，例如赛车偏向黑线的左边越厉害，朋么赛车 的右转角H将趟大。水平偏差量

8、，是PID控制器的P局部。水平徧差速废朋么直接反映了赛车的运动II向,因为有了赛车的水平偏差速度，对赛车的 掌握，稱更加准8L例如赛车偏向黒线左边，然而它的运动方向是向右的，期么，他的转角将 比向左运动时的转角要小，因为，我知道赛车已经开场朝正确的方向调整了。水平偏差速度, 是PID控制器的D局部。通11两个相隔一定果样时间的水平偏差量的差，来得到赛车的水平偏差速度。然而，这个 时间间隔名少比抓适宜昵？爪水平僱移呈3210-J23（相邻个肘间间隔）-2水水平偏移述度（相邻3个时讪询隔22i0 - -I -2 -3偏移速度（相邻5个时间间隔）21Q -I-2图3.3上述函数图像的横坐标为果样Wf

9、ilt,毎小格为4ms0第1 §图像的纵坐标为赛车水平偏 差量;第2幅图像的纵坐标为间隔为1的时候水平偏差速度；第3幅图像的纵坐标为间隔为3 时候水平偏差速度；第3帕图像的纵坐标为间隔为5的时候的水平偏差速度。由上图可知：相邻果样点越远，数据的值域越大，更有利于描述车辆的偏差程®, ffl是， 会降低赛车判斷的响应度；相邻采样点極近，数弼的值域強小，0状态越名，不利于描述赛车 的偏差程度，但是有利于哨应赛车的偏差程度，所以，这个相邻的数量要适中。上图中，间隔3比HI适宜。Proportion为的P参数,Derivative为的D参数，LastError为的水平偏差量臥列，水

10、平偏差 速aUliPA尾-臥头，臥列长度为LAST_ERROR_NUM,即表示同同隔。通过实验，长18 20比拥 适宜。有了 偏差 Error,有 了 dError=LastError0- LastErrorLAST_ERROR_NUM,输出值为PID_Output = Proportion * Error + Derivative * dError03.6 PID控斟算法的改良通il实騎和軟件仿真，发现，PID控制器并不是在任何悄况下，都是最优的选择，比方， 在直线上，PlDg制器的调整时间，远大于枚举调整法，而在严重偏离的时候，由干赛车的水 平偏杨量已经固定不变，所以，也没有必要使用PID

11、控抽了。所以，将赛车的状态分成了 3种, 下面逐一介绍并说明智能车在这一状态下的腔制算法。1：赛车处于直线状态：如果赛车居中，影么转ffi=0o如果赛车在右边，那么转角=2。« 果赛车在左21, »么转ffl=-20电机占空比均为最高。2：赛车处于严重偏离状态：如果赛车严重偏左，期么赛车右转最夫用度。如果赛车严重 偏右，册么赛车左转最大角度。电机占空比均反向，表示刹车。3：赛车处于一般弯道状态即赛车有偏務，但能检測到黒线：影么將赛车的水平偏杨量 作为PID控制器的输人，然后经il PID运算，得岀的值为转用的映射。最后，通过一个一次函 数,将输岀映射成转角的值即可。对于电机

12、的控制，抽象出3种悄况。第1种,是隐定过弯, 即水平偏杨速度很小，这时候，呆用加速过完的策昭。第2种,是机不稳定的悄况，即水平偏 杨速度很大，这时候，釆用刹车策略。第3种，是一般悄猊，此时，采用匀速通il转弯的策略。 实騎込明，这种抽象可以使赛车适应几平任何曲率半径小于500m m的弯道。模抑量的采集和PD控制模jfl量的果集和位置、ft ® it模si量果集传感器仍熟是红外光耦传感器，接收管输岀不经过施密特触发器转化成数字量，而是接到 单片机的A/D转换接口进展转换。位置it算全局求位置法通过各f传感器的信号可以it算出连续的黒线位置，较简单的方法是用类做干长杆求重心 的方法，將各

13、个传感器输出分别乘以传感器位置再求和,除以所有传感器输出之和即可得舅连 续的位置信息。这之前需要做一些准备工作，由于不同传感器具有差异，直接用A/D转换的结 果廿算位置误差较大。为了肪止传感器差异造成的影响，我们采用了先定标再用相对值进展加 权廿算的方法，具体过程为：程序开场前辻毎个传感器在赛道上进展扫描，分别记录毎个传感 器输出信号的最大fflmaxf对应读到黑线中心的悄况和最小值mint对应远离黑线读到白色 赛道的悄况，用EtfflM去最小值得到毎个传感器在赛道上的输岀围，小车行使il程中，将 毎个传感器输岀的信号减去最小値，再除以该传感器的输岀围即可得到其相对输岀值，再用毎 个传感器曲相

14、对値5传感器位置作加权平为得到的结果即为黑线位置。公式为 p°s =（E 几）/（IX）,其中pos为位置，叫为第n个传感器的相对输岀值，几为第n 个传感器置。 J这种方法消除了传感器差异造成的辦,并能够得到相对十分连续的位置信息,相邻两次 位置间隔小于0.5mm0然而，全局求位置法在某些人弯悄说会造成it算錯误,黑线与传最器排 列方向夹角越小錯炭越大。这是由于此时多个传感器离黒线较近，因此输出值较夫，原本黑线 位置较大时经过全局加权平均it算结果将偏小，用LCD自制的调试手段,用LCD实时显示程 序变量等信息显示测量结果，误差可这36%正确位置为7cm,显示値为4.5cm，这种悄

15、况将造成小车转向触乏，冲出赛道。解决方法是果用下面介鉛的对称求位置袪。对称求位置法的准备工作与全局求位置法一样，使用的是传感器的相对输岀值，先判斷岀 输岀值最大的传感器，也就是离黑线最近的传感器，然后根据该传感器进展对称加权平沟求黑 线位置。对称求位置Q分为奇数对称求位置和偶数对称求位置，参与it算传感器个数在传感器 总个数的约東下应尽可能大，即两常的传感器总是至少有一个参与it算。在前面所述悄况下，炭差为7%正确位置为7cm,显示值为& 5cm。对称求位置法与全 局求位置法相比稳定11強，但由于有公式的幼换，值置的连续性不如全局求也置法。(S当传感 器调节得较好时,二者可以有一样的连

16、续牲,因为公式切换时被舍弃的传感器和新参加廿算的 传感器输岀均为0,这一点可以通过后面介鉛的去除定标蹊差方法实现。1.1.2. 角度计算1.1.2.1. 多项式逼近双排直线传感器分别it算黒线位置，将结果之差除以传感器间ffifia展反正训运算即可得 到反正切运算通过名顶式逼近用多顶式廿算实现，我们的传感器測量角度围约为 ±40。,与征机转向围一样，在这个围，一次多顶式对反正切函数的iiifi效果很好。1.1.3. 去除干扰1.1.3.1. 去除定标误差由于程序开场前确定毎f传感器的最大值和最小值过程存在一定误差，即该最大值和最小 值并不分别等干小车运行时传感器读到黑线中心和读到远离

17、黑线的白色赛道时的输岀值，迪就 造成很可能所有传感器相对输出均不为0,在用对称求位置法廿算时，切换公式时会使it算结 果产生践动，连续II变差，影响(3分控斟的效果。解决这一冋題的方法是：甘描求出晟大it和 晟小值后，将最小値加上输岀围的a% (81110%)棉到新的最小值,再it算每个传感器的输出 围。通iliH整传感器角度、位置，调节接收管上也电阿和a的值，用LCD显示毎个传感器的相 对输岀値，得到以下效果为相对输岀不为0的传感器f数只为2或3,假设最边上的传 感器相对输岀不为0,那么该f数只能为2。这个条件不难满足，满足条件后实际条件要更 宽松一些対称求位置法的连续性将与全局求位置法的连

18、续性一样。1.1.3.2. 去除地图干扰由于地图中的十字线、起点线和地图外的场地会导致廿算岀错误的黑线也置，使小车腔制 岀塔，通过设定以下约束可以解决逆些问题：a.所有传感器输岀都低于一个阖值时不ilSIS E 置b.输出超过同值的所有传感器并非连续安故的传感器时不廿算新位置c.输岀最大且起过側 値的传感器与上一个满足该要求的传感器不相邻时不计算新位置。1.1.4. 连续性检测用LCD输出位置廿算给果,相邻两次位置同隔小T 0.5mm，相邻两次角度间隔小于。1°。1.2. PD控制1.2.1. 比例控制位置腔翎中的比例控制采用了分段比例控制，位置较小时与位置较夫时设置不同的比例 帶，原因是传感器宽度有限，枪测的位置围也就有限，貌一的比例带il大会导致小车振蒲，11 小导致最大腔制量偏小，小车转向缺乏，过弯时冲岀赛道。使用分段比例控制既方便Q可以解 决以上两种间题。闻度控制设置了死区，由于检测角度为0时，即使在直道上小车轴线与黑线 也不严恪平行，因此角度腔制需要设置死区，以肪止由此引起的小车直道振蒲。1.2.2. 攒分腔制传感器输岀模#1量的悄况下检測的位置连续性较好，因此不再需要不完全攒分的PD腔制