飞思卡尔 所有的算法

飞思卡尔所有的算法

飞思卡尔所有的算法

第五章智能汽车软件设计

第1章

1.1.直流电机控制策略：

电机掌控

针对本文所研究的智能车来说，车体速度是大惯性的被控对象。算法输出的控制量只

能对电机输出力进行控制。而有一定负载时电机的输出力无论对车轮的转速还是车体的形

式速度都是不成正比的，车在刚开始启动的时候速度是零，而电机的输出可能很大；车在

匀速行驶的时候速度很快，而电机的输出可能并不是很大。而且电池电量、车体重量都会

对车速造成影响。因此只有用闭环才能对车速进行良好的控制。在车轮对地面不打滑的情

况下车体的速度和后轮的转动速度是成正比的。因此我们可以直接用光电码盘对后轮的转

速进行控制。

对于这样一个小惯性系统，我们采用pid和鲁棒结合的办法展开速度掌控。电路的设

定值由经验值确认。考虑到速度掌控地下通道的时间落后比较大，因此使用pid掌控方案，并在展开提失速掌控时，导入了“棒棒掌控”。

u(k+1)=u(k)+p1*e(k)+p2*(e(k)ce(k-1))+p3*((e(k)ce(k-1)c(e(k-1)ce(k-2)));

公式1-1pid的公式

其中第一项为积分项；第二项为比例项；第三项为微分项。考虑到被控对象（车体速度）本身是一个大的积分环节，公式中可以将第一项省略，即采用pd控制。e为误差。

同时预设误差门限，在误差比较小的时候使用小输入掌控电机，将误差在最为短时间

内增大至所建议的范围，这就是鲁棒控制的思想。

电机控制策略，其实就是对模型车速度的控制策略，它是继路径识别之后非常重要的

策略，直接关系到整个模型车比赛的性能。速度控制得当，小车才能以最好的状态，在最

短的时间完成比赛路程。1.1.1.弯道速度控制

在模型车过弯时刻自身利益稳定性考量搞失速掌控。失速原则就是在原来弯道速度设

定值的基础上，增大速度设定值至低速挡，确保模型车可以安全过弯。另一方面，过弯之后，为了转弯时模型车能不显著的左右转动并使用较好的姿态通过弯路，

第五章智能汽车软件设计

而令车速与偏差成线性关系，偏差越大速度设定值越大。速度预设方法例如………………………………………………………………………….公式1-2。

vs(k)=v-k1*e(k)………………………………………………………………………….公式1-2

v(k)为速度闭环设定值；其中：sv模型车全程运动平均速度设定值；e(k)车体偏移理想轨迹的当前偏差值；

k1为减速控制比例系数。

同时，通过实验辨认出模型车过弯之后，而令模型车以某一线性规律快速运转

可以使车在不冲出轨道的前提下以更短的时间通过弯道。控制规律如……………………………………………………………………….公式1-3。

vs(k)=v+ck*cp……………………………………………………………………….公式1-3

其中：ck为弯道加速系数；

cp超车快速变量；

ck为常数，初始化时设定。cp入弯时刻初始化为0，每个控制周期累加1。

1.1.

2.弯道速度掌控

直道采用匀速控制方案，速度设定在此控制系统下可以控制的最大入弯速度。在从弯道驶出进入直道后须进行加速控制，增大速度设定值。因速度控制回路有一定的惯性，所以在提速时为了使速度迅速改变，加入了“棒棒控制”。控制规律如…………………………………………………………………公式1-4。

vs(k)=v+100-ka*kp其中：

…………………………………………………………………公式1-4

kakp补偿效果掌控系数；变频补偿变量。为常数，初始化时预设。

kakp出弯时刻初始化为0，每个控制周期累加1。

1.1.3.pid闭环控制算法

根据路径识别的情况，如果当前路径为直道，则需要加速；若是弯道，则需要降速，而且根据不同的弯道速度也是有所区别。系统利用测速模块反馈的当前速度

第五章智能汽车软件设计

值，通过增量式pid算法进行调节，从而控制直流电机对当前路径进行快速反应。增

量式pid的算式为：

ukkpekek1kiekkdek2ek1ek2

在增量式pid处理的过程中，有一个步骤需要注意，即在算完△u(k)后，需要把它赋

值给电机控制对应的pwm通道信号，这时要判断该△u(k)的值，如果它小于0，则把pwm

信号赋值为0，如果它大于pwm信号的最大值，即大于pwm整个周期所对应的数值时，要

把pwm信号赋值为该最大值。然后，再存储本次△u(k)，和上次△u(k)。

error=speed_v-infrared_value7;

pwmtemp=pwmdty23+pid_p*(error-

last_error)+pid_i*(error)+pid_d*(error+pre_error-2*last_error);

由上述代码中，speed_v则表示标准速度，infrared_value7则表示atd1切换的经公

式4.5排序出来的即时速度值，通过排序它们的差值error，再利用增量pid控制算法排

序出来pwmtemp，再对pwmtemp展开处置，最后pid程序处理的结果就是得出一个pwm信号，而这个信号就是驱动电机的，当error小，即为标准速度和即时速度差距的比较多时，pwmtemp的绝对值相对比较小，则给电机的pwm信号相对很大，这样电机输出功率相对较慢；反之，电机输出功率则相对较快。由此可以窥见，pid算法主要的功能就是，在闭环

系统中，利用即时速度的意见反馈，使即时速度迫近标准速度的时间尽量变长，这样小车

就可以根据路径辨识得出结论的速度标准值，及时调整自己的速度，以适应环境各种路况。比如，小车正在弯道上前进，而且弯道的时速必须就是各种赛道中最快的，当系统的路径

辨识算法察觉到前方发生弧度小的超车后，系统可以根据事先调试的结果，得出小弧度超

车的速度标准值，然后pid算法根据速度标准值speed_v和即时速度意见反馈值

infrared_value7对电机的输出信号pwm展开及时调整，充分反映至实际中，就是小车及

时失速并成功通过超车。

在增量式pid处理的过程中，有一个步骤需要注意，即在算完△u(k)后，需要把它赋

值给电机控制对应的pwm通道信号，这时要判断该△u(k)的值，如果它小于0，则把pwm

信号赋值为0，如果它大于pwm信号的最大值，即大于pwm整个周期所对应的数值时，要

把pwm信号赋值为该最大值。

第五章智能汽车软件设计

1.2.反向制动算法

必须并使赛车在较短的时间顺利完成比赛，速度自然越高越不好，似乎速度太高超车

就是过不去的，如果以超车的极限速度匀速走，又浪费了弯道的时间。所以最佳的策略就

是弯道以较低的速度走，至超车时再尽快将速度调高。在过弯减速时如果只依靠赛道的摩

擦阻力效果似乎就是比较理想的。为此我们导入逆向刹车算法。