第九章 控制算法
9.1 舵机转向的控制算法
根据CCD摄像头采集并经单片机处理后得到的16行路面信息，我们提取出每行偏离车子中心的位移值。由于前瞻性对智能车高速行驶时起到提前转弯的作用，因此我们采用大前瞻的方法。利用16行中最前面4行位移的平均值作为主要的转向依据。当车子处在直道或者S弯时，需要的转角相对于普通弯道来说比较小，所以我们调用小角度模式，即求出最前面4行平均值和最底2行平均值乘以权重，利用它们的差值线性对应到角度PWM值，作为转向的依据，具体程序如下：
根据CCD摄像头采集并经单片机处理后得到的16行路面信息，我们提取出每行偏离车子中心的位移值。由于前瞻性对智能车高速行驶时起到提前转弯的作用，因此我们采用大前瞻的方法。利用16行中最前面4行位移的平均值作为主要的转向依据。当车子处在直道或者S弯时，需要的转角相对于普通弯道来说比较小，所以我们调用小角度模式，即求出最前面4行平均值和最底2行平均值乘以权重，利用它们的差值线性对应到角度PWM值，作为转向的依据，具体程序如下：
if(r_s) //S弯或直道
{
tmp2=(*(gg1+0)+*(gg1+1)+*(gg1+2)+*(gg1+3))/4; //最前面4行平均值
tmp=(*(gg1+15)+*(gg1+14))/6; //最底2行平均值乘以权重：1/3
tmp0=(char)((tmp2-tmp)*9/14);// 两者差值乘以微调系数：9/14
(*gx)=tmp0+(tmp0-*gx)/4; //P、D调节，其中P为1，D为1/4
}
由于车子直线行驶速度较高，在入弯时减速需要一段时间，所以当车子处在一般弯道的时候，采用大角度策略，即只用最前面4行位移平均值作为转弯的依据，具体程序如下：
else //一般弯
{
tmp2=(*(gg1+0)+*(gg1+1)+*(gg1+2)+*(gg1+3))*5/24;// 最前面4行位移平
//均值乘以微调系数5/24
tmp0=(uchar)(tmp2);
(*gx)=tmp0+(tmp0-*(gx+6))/4; //P、D调节，其中P为1，D为1/4
}
由上面的程序可以看到，在小角度和大角度策略中，我们加入了PD调节，使车子在转向时舵机能够更好的对给定值做出响应，走起来更加顺畅。
转向控制算法的流程图如下图所示：
图9.1 转向控制流程图
9.2 电机转速的控制算法
测量速度主要有两种方法：在相等时间内计算车通过的路程（测频法）；计算通过相等路程所用的时间（测时法）。
考虑到硬件使用自制的黑白相间的码盘和装配上的精度问题，测时法有较快的反应速度（车前进5mm测量一次）但因车轮转动时每个脉冲的宽度不等，会使测量结果有较大的误差，不能准确反应实际速度。测频法主要的缺点是为获取高的准确度需较长的测量时间（需要有较多的脉冲才能测出），测量时间短会带有较大的误差（±1的误差）。为了获取较高的精度，同时有较短的反应时间，测量主要采用了滑动平均值法。
假设车速是2m/S，则小车20mS前进4cm，传感器输出8个脉冲，最大测量误差为12.5%，为使误差下降到2.5%以下，将测量时间设定为100mS，车速2m/S时每个测量周期就有40个脉冲，误差就可以下降到2.5%以内。当然，随着速度的上升，每个测量周期可测得的脉冲数越多，测量误差就越小。考虑到CCD一帧的时间是20mS，速度测量控制周期也设为20mS，而测速需100mS才能获得期望的精度，故每个测量周期所测量出来的是最近5个控制周期的平均速度。使用滑动平均值法测量速度，可以获得较高的测量精度，而100mS小车大约前进20cm，能够较及时地反应实际速度。
由于电机的机械特性较软，而控制周期相对于电机的响应特性较短，为使电机快速达到设定速度，我们使用了bang-bang 控制算法。
当车速小于设定速度10cm/S时，使用100%占空比，使小车快速加速达到设定速度；当车速在设定速度附近时，当小车在直道上时使用40%占空比，在转弯时使用50%占空比，使小车保持当前速度前进。当车速大于设定速度10cm/S时，使用0%占空比，让小车滑行减速；当车速大于设定速度20cm/S时，启用刹车，使车速快速下降。 设定一速度上小车匀速行驶，实际运行效果如图，可以看出小车在设定速度波动在15%以内，而小车在直路上行驶速度在±2%内波动。可见该控制算法可使小车稳定在设定速度上下。
图9.2：小车在塞道上匀速行驶
图9.3是小车的加速特性曲线，小车从速度0加速度到290cm/S的80%只需要0.58S，达到稳定速度需0.76S，可以看到该控制算法可以使小车迅速加速到设定速度，并稳定在设定值附近。
图9.3：小车直线加速特性（横坐标每单位为20mS）
图9.4转速控制流程图
