智能车控制的主要目的，是实现小车快速、稳定的在跑道上运行，赛道主 要由直线、弯道以及 S 道组成。控制赛车运动主要就是控制前轮的舵机转向和 后轮的电机速度。由于在探测路径中，我们采用的是 8 对光电对管，相比 CCD 而言，前瞻性较差，所以在无法预知赛道的情况下，速度不会太高，以防止速 度控制响应不及时，赛车冲出跑道。但如果采用记忆路径的话，赛道信息就会 在第一圈后记录下来，大大提高了第二圈的前瞻性，可以实现赛车的快速行驶 和智能转弯，所以本次赛车我们采用记忆路径的算法。
5.1 赛道检测
为了更好的适应环境变化，我们采用将红外对管的输出电压经放大后送入 AD 模块（AD0 ）采样，如下图所示，放大倍数通过滑动变阻器可调，并且对赛 道进行现场采样，判断黑白的阈值，这样使得赛车具有很好的适应性。
AD0 初始化代码：
ATD0CTL2 = 0xc0;//AD0 模块使能，读取转换结果后自动清零
ATD0CTL3 = 0x00;// 8 个转换序列，No FIFO, Freeze 模式下继续转换
ATD0CTL4 = 0b10100101;//8 位精度
ATD0CTL5 = 0xb0;// 右对齐无符号，连续转换，多通道转换
ATD0DIEN = 0x00; //禁止数字输入
现场采样程序：
void GetSample(void)
{
while(!ATD0STAT0_SCF);
array[0] = ATD0DR7L;
array[3] = ATD0DR4L;
array[4] = ATD0DR3L;
array[7] = ATD0DR0L;
Sample=((array[0]+array[3])/2+(array[7]+array[4])/2)/2-10;
}
5.2 舵机转向控制
5.2.1 舵机转角方案
方案一：不同的光电对管对应不同的舵机角度。该算法简单易行，响应实 时性好，赛车可以轻松过弯。但存在的问题是转角生硬，赛车直道运行容易摆 动，不平稳。
方案二：PID 控制算法。PID 控制即比例、积分、微分控制。基本偏差 e(t)， 表示当前测量值与设定目标值的差，设定目标是被减数，结果可正可负，正数 表示还没有达到，负数表示已经超过了设定值，这是面向比例项用的变动数据。 累计偏差∑e(t)= e(t)+ e(t-1) +e(t-2) +…+e(1),这是我们每一次测量得到的偏差值 的总和，是面向积分项的一个变动数据。基本偏差的相对偏差e(t) -e(t-1),,用本次 的基本偏差减去上次的基本偏差，用于考察当前控制对象的变化趋势，作为快 速反映的依据，是面向微分项的一个变动还数据。三个基本参数：kp，ki，kd， 是做好控制的关键常数，分别为比例常数，积分常数和微分常数，需要经过大 量的实验测试才能得到较好的效果，也是件较头疼的事情。
PID 控标准的计算公式：
比例调节可以加快调节，减少误差，但是过大的比例，会使得系统的稳定 性下降，甚至造成系统的不稳定。积分调节的作用是消除稳态误差，提高无差 度，有误差，积分调节就进行，直至无差，加入积分调节会使系统的稳定性下 降，动态响应变慢。微分调节的作用是预见偏差的变化趋势，所以可以产生超 前的控制作用，在偏差没有形成前，已被非分调节作用消除，因此可以改善系 统的动态性能，在微分常数选择合适的情况下可以减少超调和调节时间。 PID 控制算法的优点是控制平稳，可以使赛车在直道上平稳的运行。但存在 的问题是调节的实时性不是很好，赛车容易在过弯时转角响应不及时冲出跑道。
5.2.2 舵机转角算法
1. 低速行驶舵机转向控制
我们的检测路径光电传感器其布局如图 5.2 所示。
通过大量实验我们确定了在电机占空比为 60％，并且最小曲率为50cm 时， 赛车不冲出跑道所需要的最大转角，此时最左边或者最右边的光电对管检测到 黑线。初始化时设置中间检测到黑线对应舵机90 度，则通过测试若要赛车在上 述情况下不冲出跑道，左边对应舵机角度最小为 62 度，右边对应舵机角度最大 为 118 度。然后按照光电对管对中心的距离线性分配角度，如表 5.2 所示。
在实际测试时，发现上述舵机角度分配虽然在弯道时可以轻松过弯，但直 道时有很明显的左右摆动，其原因是中间四个光电管的拐角变化太大，在直道 时一旦有小的摆动，大的转角会使其更剧烈的摆动，对此我们在中间的四个拐 角中又添加了两个拐角，使得在直道上拐角的变化不是很剧烈，保证赛车在整 个赛道上的平稳运行。添加后的拐角表如 5.3 所示。
因为采用记忆路径，这也正是我们第一圈的舵机转向策略。第一圈低速平 稳运行，有利于路径记忆，简单易行的线性角度分配即可满足赛车的平稳运行。
2. 高速行驶舵机转向策略
有了第一圈已经记忆的路径，第二圈的前瞻性有了大大提高，高速行驶有 了可能，第一圈的舵机转向控制虽然弯道转向实时性好，但直道却不能平稳运行，直道的摆动很大程度上抑制了整体速度的提高，因此已经不能在第二圈中 适应需要了。 PID 控制策略在平稳性方面不失为一种很好的策略，但同时存在实时性差的 缺点。因此在第二圈的转向控制中，我们采用转角线性分配和PID 结合的策略。 直道PID，赛车平稳运行；过弯转角线性分配，响应实时，轻松过弯。 上述低速、高速的转角策略可以使赛车有很好的稳定性，不会轻易冲出跑 道。
5.2.3 舵机转角程序代码
舵机转角在软件上是通过控制 PWM 波占空比实现的。 PWM 波频率为 50Hz，高电平为 1500us 时舵机为0 度转角，赛车正前方行驶；高电平为 1100us 时舵机为左向45 度转角；高电平为 1900us 时舵机为右向45 度转角。高电平在 1100us 和 1900us 之间时舵机的动作方向和高电平持续时间基本为线性关系，舵 机转角与高电平持续时间对应关系如图 5.3 所示。
实际应用中我们采用PWM 模块的 6、7 通道级联，由 7 通道输出的频率为 50Hz 的PWM 波控制舵机的转向。舵机电路如下图所示：
PWM 初始化代码如下：
PWME = 0; //关闭所有通道
PWMCTL_CON67 = 1;//6 7 通道级联组合成一个 16bit 通道
PWMPOL = 0xff;//设置67 通道为正极性方波
PWMCAE = 0;//设置67 通道输出波形为左对齐方式
PWMCLK = 0x83;// SB 作为67 通道的时钟源
PWMSCLB = 133;//CLOCK SB = CLOCK B /(2*133)
PWMPER6 = 0x07;
PWMPER7 = 0x0d;//6 7 通道周期计数器设置，周期20ms
PWMDTY6 = 0;
PWMDTY7 = 135;//6 7 通道占空比初始化，角度为正前方 90 度
PWME = 0x83;//打开67 通道，舵机使能
5.3 速度控制
5.3.1 速度控制方案
方案一：开环控制。通过大量实验测量，获得赛道的不同情况下对应的最 佳速度，根据检测到的赛道信息决定速度的大小，控制电机的占空比。开环控 制的优点是简单易行，不同的赛道对应不同的电机占空比；缺点是速度控制精 度不高，抗干扰性差，因为赛车在不同的状况下，如电源电压等，即使相同的 电机占空比也不会对应相同的赛车速度。
方案二：闭环控制。采用速度反馈，速度的确定不再仅仅基于电机占空比，可以实时获得赛车的实际速度，因此速度控制精确，配合赛道运行策略可以获 得平稳高速的速度控制。
5.3.2 速度控制算法
1. 第一圈速度控制
由于第一圈低速行驶，赛车运行稳定，不存在冲出跑道与高速控制的问题， 所以我们采用简单易行的开环控制。直道速度通过按键设置，入弯减速。实现 方法为，不同的光电管对应不同的赛道信息，分配相应的舵机转角和电机占空比。同时在赛车刚入弯时速度减速程度大，确保平稳入弯；入弯后减速程度相应较小，保证弯道速度，争取弯道运行时间。
2. 第二圈速度控制
第二圈总体实现高速行驶。直道全速行驶，入弯提前减速，减速程度采用 闭环控制。具体实现是测量一定时间的脉冲数，得到当前速度，速度反馈可以 只在直道入弯和 S 道时采用，控制直道入弯速度和 S 道速度。采用上述策略可 以使赛车在直道时全速行驶，入弯减速，轻松过弯，S 道平稳运行，提高赛车整 体速度。
5.3.3 速度控制程序代码
电机的速度和舵机的转角控制一样，在软件上均是通过控制PWM 波占空比 实现的。实际应用中采用PWM 模块的 0 通道控制电机反转，1 通道控制电机正 转，0，1 通道均输出频率为 1kHz 的PWM 波。实际应用中PWM 波频率应小于 10kHz。
PWME = 0; //关闭所有通道
PWMPOL = 0xff;//设置0 1 通道为正极性方波
PWMCAE = 0;//设置0 1 通道输出波形为左对齐方式
PWMCLK = 0x83;//SA 作为0 1 通道的时钟源
PWMSCLA = 47;//CLOCK SA = CLOCK A /(2*47)
PWMPER0 = 0xff;//0 通道周期计数器设置，周期 1ms
PWMPER1 = 0xff;//1 通道周期计数器设置，周期 1ms
PWMDTY0 = 0;//0 通道占空比初始化，速度为 0
PWMDTY1 = 0;//1 通道占空比初始化，速度为 0
PWME = 0x83;//打开0 1 通道，驱动电机使能
5.4 坡道策略
5.4.1 坡道检测
由于今年的赛道不同于去年，一大特点就是在直道中增加了坡道，对于没 有速度闭环控制的赛车来说，就会有可能在上坡时动力不足，无法顺利完成比 赛，或者在下坡时速度太快，冲出赛道。我们的第一辆赛车采用红外检测，为 了实现赛车顺利上坡，我们在红外对管的固定时加高了红外离地面的高度，同 时在车底盘正中央安装了三轴加速度传感器，MMA6263Q 用于 xy 轴， MMA1260D 由于z 轴，感知赛车上坡状态。安装的加速度传感器x 轴指向赛车 的正前方，y 轴指向赛车的右侧，z 轴指向赛车的上方，如果检测到赛车正在上 坡中，则加足马力，顺利上坡。
由于重力加速度的存在，加速度传感器在不同的状态下会给出不同的电压 值，加速度传感器的状态与给出电压值关系如以下图所示：
通过上述的状态电压值可以分析得到在赛车上坡时，加速度传感器的 xz 两 轴会给出比平路时小的电压，通过此就可以判断赛车上坡，控制电机占高空比， 全速上坡。
5.4.2 坡道程序代码
将三轴加速度传感器给出的电压送至AD 模块（AD1 ）采样。首先在平路上 赛车尚未行使时时分别采样xz 轴的一般值，作为以后的采样比较值；然后在赛 车行使过程中不断采样xz 轴的输出电压，当xz 轴采样值同时满足小于平路上的 采样值减一标定值时，判为上坡，标定值由上坡角度决定。
AD1 初始化代码：
ATD1CTL2 = 0xc0;//AD1 模块使能，读取转换结果后自动清零
ATD1CTL3 = 0x18;//选择 3 个转换序列，使能转换映射
ATD1CTL4 = 0x76;//10 位精度，采样时间为 16 个AD 转换时钟，46 分频
ATD1CTL5 = 0xb0;// 右对齐无符号，连续转换，多通道转换
ATD1DIEN = 0x00; //禁止数字输入
5.5 记忆路径算法描述
赛车采用记忆路径的算法。第一圈低速平稳运行，记忆赛道直道和弯道起 止位置信息；第二圈利用赛道信息，直道全速、微调舵机角度，入弯减速、实 时调整舵机角度。
5.5.1 记忆路径算法的实现
路径的记忆只能是根据红外的检测状态，我们采用对不同的红外对管对应 不同的状态标志Flag，其值分配如下表：
当 Flag 为 3、4 或者－3、－4 时说明舵机的转角很大，即为赛车入弯，将 此时的脉冲记录，记为弯道开始（直道结束）标志，等 Flag 变为 0，记为弯道 结束（直道开始），这样记录下去，就会得到直道和弯道的位置，还可以根据弯 道脉冲数的多少计算弯道的曲率。
ECT 模块初始化：
PBCTL_PBEN = 1;
TIOS = 0x00;
TCTL4 = 0b00000010;
ICOVW_NOVW1 = 1; // 保护
ICPAR_PA1EN = 1;
记忆路径程序代码如下：
void Record(void)
{
//////////////////////////直道///////////////////////////////////////
if(Flag == 0 && PreFlag!=0 ) //中间
{
RoadTrace[RoadNode]=PACN10;
RoadNode++;
PreFlag=0;
}
////////////////////////////弯///////////////////////////////////
else if( (Flag == 3|| Flag==-3) && PreFlag==0 )
{
RoadTrace[RoadNode]=PACN10;
RoadNode++;
PreFlag=1;
}
}
5.6 算法流程图
算法整体流程图如图 9.3 所示：
5.7 本章小结
本章主要介绍了赛车的舵机转向和速度控制策略，是整个算法的核心与灵 魂，好的控制策略才会获得更快更平稳的速度，是赛车智能的具体体现。
