第五章 控制算法
该智能车驱动电机使用增量式PID控制算法、转向舵机使用增量式PD控制算法。整个小车的控制使用了自适应控制算法，小车可以识别弯道的大小。控制参数可根据弯道的大小和小车的行驶速度进行合理的调整，使小车快速稳定地行驶。该智能车的关键参数，包括小车的最大速度（max_speed）、最小速度（min_speed）、控制速度的比例增益（ks）、转向舵机的角度校正（regulate）、控制转向舵机的比例系数（k）、微分系数（kd）、十字路口个数等参数都可以通过两个拨码开关在线调整。
电机驱动控制算法的选择：刚开始的时候，电机控制和舵机控制都是使用增量式PID算法，经过一段时间的调试，电机适度控制达到了比较好的效果，但是舵机控制却不令人满意，在过一些大S弯道的时候总是会冲出跑道。故我们最后把舵机控制算法改为了增量式PID算法，经过几次调试就达到了比较好的效果。赛车冲出赛道的几率大大降低。
电机驱动增量式PID算法：
es1=(set_speed+get_PB+speed_control/10+min_speed-127)-detect_speed;
speed_temp=ks*(es1-es2)+ksi*es1+ksd*(es1-2*es2+es3);
es3=es2;
es2=es1；
speed_out=speed_out+speed_temp/10；
式中：
es1为速度当前误差；
es2为es1之后的误差；
es3为es2之后的误差；
detect_speed为速度传感器检测的速度；
set_speed为设定速度；
speed_out为速度的输出值（其决定驱动电机的转速和转动方向）；
ks为速度比例系数；
ksi为速度比例系数；
ksd为速度比例系数；
由上式看出，电机速度控制的主要参数是speed_control。
speed_control是通过看到赛道的有效行数来控制的。
speed_control=get_PAD* effective_count /CCD_COLS；
式中：
get_PAD为拨码开关设置的速度值；
CCD_COLS采集到的图像列数；
effective_count 为检测到路径的有效行数；
经过多次调试，这种通过有效行数来控制电机速度的方法获得了比较好的效果，赛车跑起来比较流畅。
转向舵机的控制算法：小车在高速行驶的过程中能够准确无误地循着黑线跑，必需要求其转向系统具有高的响应速度和低的超调量。考虑到这些问题，我们开始在转向舵机的控制方面考虑使用PID控制算法，该算法具有较高的响应速度、低的超调量和低的稳态误差。经过分析和我们使用的具体方案的结合，我们使用的方案对响应速度的要求很高，PID算法引入了积分环节，在一定程度上降低了响应速度。而对稳态误差的要求不大，恰好相反，我们正是利用了其稳态误差来识别弯道的曲率半径的。因此，我们选择了PD控制算法。其算法如下：
direct_out=k* e1+kd*(e1-e2)
式中：
direct_out为舵机输出量；
k为比例系数；
kd为微分系数；
e1为本次的偏差；
e2为上次的偏差；
