为了实现PID 控制所需要的等间隔采样，我们使用了一个定时中断，每2ms 进行一次数据采样和PID 计算。与此并行，系统中还设计了一个转速脉冲检测中断，从而实现了转速检测。为了调试的需要，程序中还在main{}函数中加入了相关的调试代码，这部分代码有最低的优先级，可以在保证不影响控制策略的情况下实现发送调试数据等功能。检测环节对整个控制系统的质量起到至关重要的作用
4.3.2 PID控制调整速度
本系统采用的是增量式数字PID 控制，通过每一控制周期（10ms）读入脉冲数间接测得小车当前转速vi_FeedBack，将vi_FeedBack与模糊推理得到的小车期望速度vi_Ref比较，由以下公式求得速度偏差error1与速度偏差率d_error。
error1 = vi_Ref– vi_FeedBack; （公式3）
d_error = error1 –vi_PreError; （公式4）
公式4中, vi_PreError为上次的速度偏差。考虑到控制周期较长，假设按2.5m/s的平均速度计算，则一个控制周期小车大概可以跑过2.5cm，如果按这种周期用上述PID 调节速度，则会导致加速减速均过长的后果，严重的影响小车的快速性和稳定性。为了解决这个问题，可以在PID 调速控制中加入BANG-BANG 控制思想：根据error1的大小，如果正大，则正转给全额占空比；如果负大，则自由停车或给一个反转占空比；否则就采用PID
计算的占空比。
PID 控制算法
为了使赛车平滑得保持在黑线中央，即使赛车的偏移量平滑地保持在0，实用了PID 控制算法。
P 为比例参数，D为微分参数。基准值为0，PID输入为水平偏移量X0，PID输出为转角，转角方向：向左转为正，向右转为负。
P 参数在智能车控制器中表示水平偏差量的权，D参数在智能车控制器中表示水平偏差速度的权。
水平偏差量直接反映了赛车偏离黑线的程度，例如赛车偏向黑线的左边越厉害，则赛车的右转角度将越大。水平偏差量，是PID 控制器的P 部分。
水平偏差速度则直接反映了赛车的运动倾向，因为有了赛车的水平偏差速度，对赛车的掌握，将更加精确。例如赛车偏向黑线左边，然而它的运动方向是向右的，那么，他的转角将比向左运动时的转角要小，因为，我知道赛车已经开始朝正确的方向调整了。水平偏差速度，是PID 控制器的D 部分。
通过两个相隔一定采样时间的水平偏差量的差，来得到赛车的水平偏差速度。然而，这个时间间隔多少比较合适呢？
图3.3
上述函数图像的横坐标为采样时间t，每小格为4ms。第1幅图像的纵坐标为赛车水平偏差量；第2幅图像的纵坐标为间隔为1的时候水平偏差速度；第3幅图像的纵坐标为间隔为3时候水平偏差速度；第3幅图像的纵坐标为间隔为5的时候的水平偏差速度。
由上图可知：相邻采样点越远，数据的值域越大，更有利于描述车辆的偏差程度，但是，会降低赛车判断的响应度；相邻采样点越近，数据的值域越小，0状态越多，不利于描述赛车的偏差程度，但是有利于响应赛车的偏差程度，所以，这个相邻的数量要适中。上图中，间隔3比较合适。
Proportion 为的P 参数，Derivative 为的D 参数，LastError[]为的水平偏差量队列，水平偏差速度即队尾-队头，队列长度为LAST_ERROR_NUM，即表示间间隔。通过实验，长度20比较合适。
有了偏差Error，有了dError=LastError[0]- LastError[LAST_ERROR_NUM]，输出值为 PID_Output = Proportion * Error + Derivative * dError。
3.6 PID控制算法的改进
通过实验和软件仿真，发现，PID控制器并不是在任何情况下，都是最优的选择，比如，在直线上，PID控制器的调整时间，远大于枚举调整法，而在严重偏离的时候，由于赛车的水平偏移量已经固定不变，所以，也没有必要使用PID 控制了。所以，将赛车的状态分成了3种，下面逐一介绍并说明智能车在这一状态下的控制算法。
1：赛车处于直线状态：如果赛车居中，则转角=0。如果赛车在右边，则转角=2。如果赛车在左边，则转角=-2。电机占空比均为最高。
2：赛车处于严重偏离状态：如果赛车严重偏左，则赛车右转最大角度。如果赛车严重偏右，则赛车左转最大角度。电机占空比均反向，表示刹车。
3：赛车处于一般弯道状态（即赛车有偏移，但能检测到黑线）：则将赛车的水平偏移量作为PID 控制器的输入，然后经过PID 运算，得出的值为转角的映射。最后，通过一个一次函数，将输出映射成转角的值即可。对于电机的控制，抽象出3种情况。第1种，是稳定过弯，即水平偏移速度很小，这时候，采用加速过完的策略。第2种，是极不稳定的情况，即水平偏移速度很大，这时候，采用刹车策略。第3种，是一般情况，此时，采用匀速通过转弯的策略。实验证明，这种抽象可以使赛车适应几乎任何曲率半径小于500mm 的弯道。
模拟量的采集和PD 控制
模拟量的采集和位置、角度计
模拟量采集
传感器仍然是红外光耦传感器，接收管输出不经过施密特触发器转化成数字量，而是接到单片机的A/D转换接口进行转换。
位置计算
全局求位置法
通过各个传感器的信号可以计算出连续的黑线位置，较简单的办法是用类似于长杆求重心的方法，将各个传感器输出分别乘以传感器位置再求和，除以所有传感器输出之和即可得到连
续的位置信息。这之前需要做一些准备工作，由于不同传感器具有差异，直接用A/D转换的结果计算位置误差较大。为了避免传感器差异造成的影响，我们采用了先定标再用相对值进行加权计算的办法，具体过程为：程序开始前让每个传感器在赛道上进行扫描，分别记录每个传感器输出信号的最大值max（对应读到黑线中心的情况）和最小值min（对应远离黑线读到白色赛道的情况），用最大值减去最小值得到每个传感器在赛道上的输出范围，小车行使过程中，将每个传感器输出的信号减去最小值，再除以该传感器的输出范围即可得到其相对输出值，再
n 器的相对值n 与传感器位置作加权平均得到的结果即为黑线位置。公式为用每个传感
pos =(v n p n ) /(v n ) ，其中pos 为位置，v n 为第n 个传感器的相对输出值，p n 为第∑i =1∑i =1n 个传感器的位置。
这种方法消除了传感器差异造成的影响，并能够得到相对十分连续的位置信息，相邻两次位置间隔小于0.5mm。然而，全局求位置法在某些入弯情况会造成计算错误，黑线与传感器排列方向夹角越小错误越大。这是由于此时多个传感器离黑线较近，因此输出值较大，原本黑线位置较大时经过全局加权平均计算结果将偏小，用LCD（自制的调试手段，用LCD 实时显示程序变量等信息）显示测量结果，误差可达36%（正确位置为7cm，显示值为4.5cm），这种情况将造成小车转向不足，冲出赛道。解决办法是采用下面介绍的对称求位置法。
1.1.1.1. 对称求位置法
对称求位置法的准备工作与全局求位置法相同，使用的是传感器的相对输出值，先判断出输出值最大的传感器，也就是离黑线最近的传感器，然后根据该传感器进行对称加权平均求黑线位置。对称求位置又分为奇数对称求位置和偶数对称求位置，参与计算传感器个数在传感器总个数的约束下应尽可能大，即两端的传感器总是至少有一个参与计算。
在前面所述情况下，误差为7%（正确位置为7cm，显示值为6.5cm）。对称求位置法与全局求位置法相比稳定性强，但由于有公式的切换，位置的连续性不如全局求位置法。但当传感器调节得较好时，二者可以有相同的连续性，因为公式切换时被舍弃的传感器和新加入计算的传感器输出均为0，这一点可以通过后面介绍的去除定标误差办法实现。
1.1.2. 角度计算
1.1.2.1. 多项式逼近
双排直线传感器分别计算黑线位置，将结果之差除以传感器间距再进行反正切运算即可得到角度值。反正切运算通过多项式逼近用多项式计算实现，我们的传感器测量角度范围约为±40︒，与舵机转向范围相同，在这个范围内，一次多项式对反正切函数的逼近效果很好。
1.1.3. 去除干扰
1.1.3.1. 去除定标误差
由于程序开始前确定每个传感器的最大值和最小值过程存在一定误差，即该最大值和最小值并不分别等于小车运行时传感器读到黑线中心和读到远离黑线的白色赛道时的输出值，这就造成很可能所有传感器相对输出均不为0，在用对称求位置法计算时，切换公式时会使计算结果产生跳动，连续性变差，影响微分控制的效果。解决这一问题的办法是：扫描求出最大值和最小值后，将最小值加上输出范围的a%（如10%）得到新的最小值，再计算每个传感器的输出范围。通过调整传感器角度、位置，调节接收管上拉电阻和a 的值，用LCD 显示每个传感器的相对输出值，得到以下效果为最好：相对输出不为0的传感器个数只为2或3，若最边上的传感器相对输出不为0，则该个数只能为2。这个条件不难满足，满足条件后（实际条件要更宽松一些）对称求位置法的连续性将与全局求位置法的连续性相同。
1.1.3.2. 去除地图干扰
由于地图中的十字线、起点线和地图外的场地会导致计算出错误的黑线位置，使小车控制出错，通过设定以下约束可以解决这些问题：a.所有传感器输出都低于一个阈值时不计算新位置b.输出超过阈值的所有传感器并非连续安放的传感器时不计算新位置c.输出最大且超过阈值的传感器与上一个满足该要求的传感器不相邻时不计算新位置。
1.1.4. 连续性检测
用LCD 输出位置计算结果，相邻两次位置间隔小于0.5mm，相邻两次角度间隔小于0.1 。
1.2. PD 控制
1.2.1. 比例控制
位置控制中的比例控制采用了分段比例控制，位置较小时与位置较大时设置不同的比例带，原因是传感器宽度有限，检测的位置范围也就有限，统一的比例带过大会导致小车振荡，过小导致最大控制量偏小，小车转向不足，过弯时冲出赛道。使用分段比例控制既方便又可以解决以上两种问题。角度控制设置了死区，由于检测角度为0时，即使在直道上小车轴线与黑线也不严格平行，因此角度控制需要设置死区，以避免由此引起的小车直道振荡。
1.2.2. 微分控制
传感器输出模拟量的情况下检测的位置连续性较好，因此不再需要不完全微分的PD 控制，用一般形式的位置和角度微分控制即可达到很好的效果。由于比例带过小造成小车振荡时，适
当增加微分控制；振荡频率较高时则应该减小微分控制，因为此时D 控制量过大。此外，加大微分控制可以使小车稳定性和跟随性变好，防止冲出赛道。
为了实现PID 控制所需要的等间隔采样，我们使用了一个定时中断，每2ms 进行一次数据采样和PID 计算。与此并行，系统中还设计了一个转速脉冲检测中断，从而实现了转速检测。为了调试的需要，程序中还在main{}函数中加入了相关的调试代码，这部分代码有最低的优先级，可以在保证不影响控制策略的情况下实现发送调试数据等功能。检测环节对整个控制系统的质量起到至关重要的作用
4.3.2 PID控制调整速度
本系统采用的是增量式数字PID 控制，通过每一控制周期（10ms）读入脉冲数间接测得小车当前转速vi_FeedBack，将vi_FeedBack与模糊推理得到的小车期望速度vi_Ref比较，由以下公式求得速度偏差error1与速度偏差率d_error。
error1 = vi_Ref– vi_FeedBack; （公式3）
d_error = error1 –vi_PreError; （公式4）
公式4中, vi_PreError为上次的速度偏差。考虑到控制周期较长，假设按2.5m/s的平均速度计算，则一个控制周期小车大概可以跑过2.5cm，如果按这种周期用上述PID 调节速度，则会导致加速减速均过长的后果，严重的影响小车的快速性和稳定性。为了解决这个问题，可以在PID 调速控制中加入BANG-BANG 控制思想：根据error1的大小，如果正大，则正转给全额占空比；如果负大，则自由停车或给一个反转占空比；否则就采用PID
计算的占空比。
PID 控制算法
为了使赛车平滑得保持在黑线中央，即使赛车的偏移量平滑地保持在0，实用了PID 控制算法。
P 为比例参数，D为微分参数。基准值为0，PID输入为水平偏移量X0，PID输出为转角，转角方向：向左转为正，向右转为负。
P 参数在智能车控制器中表示水平偏差量的权，D参数在智能车控制器中表示水平偏差速度的权。
水平偏差量直接反映了赛车偏离黑线的程度，例如赛车偏向黑线的左边越厉害，则赛车的右转角度将越大。水平偏差量，是PID 控制器的P 部分。
水平偏差速度则直接反映了赛车的运动倾向，因为有了赛车的水平偏差速度，对赛车的掌握，将更加精确。例如赛车偏向黑线左边，然而它的运动方向是向右的，那么，他的转角将比向左运动时的转角要小，因为，我知道赛车已经开始朝正确的方向调整了。水平偏差速度，是PID 控制器的D 部分。
通过两个相隔一定采样时间的水平偏差量的差，来得到赛车的水平偏差速度。然而，这个时间间隔多少比较合适呢？
图3.3
上述函数图像的横坐标为采样时间t，每小格为4ms。第1幅图像的纵坐标为赛车水平偏差量；第2幅图像的纵坐标为间隔为1的时候水平偏差速度；第3幅图像的纵坐标为间隔为3时候水平偏差速度；第3幅图像的纵坐标为间隔为5的时候的水平偏差速度。
由上图可知：相邻采样点越远，数据的值域越大，更有利于描述车辆的偏差程度，但是，会降低赛车判断的响应度；相邻采样点越近，数据的值域越小，0状态越多，不利于描述赛车的偏差程度，但是有利于响应赛车的偏差程度，所以，这个相邻的数量要适中。上图中，间隔3比较合适。
Proportion 为的P 参数，Derivative 为的D 参数，LastError[]为的水平偏差量队列，水平偏差速度即队尾-队头，队列长度为LAST_ERROR_NUM，即表示间间隔。通过实验，长度20比较合适。
有了偏差Error，有了dError=LastError[0]- LastError[LAST_ERROR_NUM]，输出值为 PID_Output = Proportion * Error + Derivative * dError。
3.6 PID控制算法的改进
通过实验和软件仿真，发现，PID控制器并不是在任何情况下，都是最优的选择，比如，在直线上，PID控制器的调整时间，远大于枚举调整法，而在严重偏离的时候，由于赛车的水平偏移量已经固定不变，所以，也没有必要使用PID 控制了。所以，将赛车的状态分成了3种，下面逐一介绍并说明智能车在这一状态下的控制算法。
1：赛车处于直线状态：如果赛车居中，则转角=0。如果赛车在右边，则转角=2。如果赛车在左边，则转角=-2。电机占空比均为最高。
2：赛车处于严重偏离状态：如果赛车严重偏左，则赛车右转最大角度。如果赛车严重偏右，则赛车左转最大角度。电机占空比均反向，表示刹车。
3：赛车处于一般弯道状态（即赛车有偏移，但能检测到黑线）：则将赛车的水平偏移量作为PID 控制器的输入，然后经过PID 运算，得出的值为转角的映射。最后，通过一个一次函数，将输出映射成转角的值即可。对于电机的控制，抽象出3种情况。第1种，是稳定过弯，即水平偏移速度很小，这时候，采用加速过完的策略。第2种，是极不稳定的情况，即水平偏移速度很大，这时候，采用刹车策略。第3种，是一般情况，此时，采用匀速通过转弯的策略。实验证明，这种抽象可以使赛车适应几乎任何曲率半径小于500mm 的弯道。
模拟量的采集和PD 控制
模拟量的采集和位置、角度计
模拟量采集
传感器仍然是红外光耦传感器，接收管输出不经过施密特触发器转化成数字量，而是接到单片机的A/D转换接口进行转换。
位置计算
全局求位置法
通过各个传感器的信号可以计算出连续的黑线位置，较简单的办法是用类似于长杆求重心的方法，将各个传感器输出分别乘以传感器位置再求和，除以所有传感器输出之和即可得到连
续的位置信息。这之前需要做一些准备工作，由于不同传感器具有差异，直接用A/D转换的结果计算位置误差较大。为了避免传感器差异造成的影响，我们采用了先定标再用相对值进行加权计算的办法，具体过程为：程序开始前让每个传感器在赛道上进行扫描，分别记录每个传感器输出信号的最大值max（对应读到黑线中心的情况）和最小值min（对应远离黑线读到白色赛道的情况），用最大值减去最小值得到每个传感器在赛道上的输出范围，小车行使过程中，将每个传感器输出的信号减去最小值，再除以该传感器的输出范围即可得到其相对输出值，再
n 器的相对值n 与传感器位置作加权平均得到的结果即为黑线位置。公式为用每个传感
pos =(v n p n ) /(v n ) ，其中pos 为位置，v n 为第n 个传感器的相对输出值，p n 为第∑i =1∑i =1n 个传感器的位置。
这种方法消除了传感器差异造成的影响，并能够得到相对十分连续的位置信息，相邻两次位置间隔小于0.5mm。然而，全局求位置法在某些入弯情况会造成计算错误，黑线与传感器排列方向夹角越小错误越大。这是由于此时多个传感器离黑线较近，因此输出值较大，原本黑线位置较大时经过全局加权平均计算结果将偏小，用LCD（自制的调试手段，用LCD 实时显示程序变量等信息）显示测量结果，误差可达36%（正确位置为7cm，显示值为4.5cm），这种情况将造成小车转向不足，冲出赛道。解决办法是采用下面介绍的对称求位置法。
1.1.1.1. 对称求位置法
对称求位置法的准备工作与全局求位置法相同，使用的是传感器的相对输出值，先判断出输出值最大的传感器，也就是离黑线最近的传感器，然后根据该传感器进行对称加权平均求黑线位置。对称求位置又分为奇数对称求位置和偶数对称求位置，参与计算传感器个数在传感器总个数的约束下应尽可能大，即两端的传感器总是至少有一个参与计算。
在前面所述情况下，误差为7%（正确位置为7cm，显示值为6.5cm）。对称求位置法与全局求位置法相比稳定性强，但由于有公式的切换，位置的连续性不如全局求位置法。但当传感器调节得较好时，二者可以有相同的连续性，因为公式切换时被舍弃的传感器和新加入计算的传感器输出均为0，这一点可以通过后面介绍的去除定标误差办法实现。
1.1.2. 角度计算
1.1.2.1. 多项式逼近
双排直线传感器分别计算黑线位置，将结果之差除以传感器间距再进行反正切运算即可得到角度值。反正切运算通过多项式逼近用多项式计算实现，我们的传感器测量角度范围约为±40︒，与舵机转向范围相同，在这个范围内，一次多项式对反正切函数的逼近效果很好。
1.1.3. 去除干扰
1.1.3.1. 去除定标误差
由于程序开始前确定每个传感器的最大值和最小值过程存在一定误差，即该最大值和最小值并不分别等于小车运行时传感器读到黑线中心和读到远离黑线的白色赛道时的输出值，这就造成很可能所有传感器相对输出均不为0，在用对称求位置法计算时，切换公式时会使计算结果产生跳动，连续性变差，影响微分控制的效果。解决这一问题的办法是：扫描求出最大值和最小值后，将最小值加上输出范围的a%（如10%）得到新的最小值，再计算每个传感器的输出范围。通过调整传感器角度、位置，调节接收管上拉电阻和a 的值，用LCD 显示每个传感器的相对输出值，得到以下效果为最好：相对输出不为0的传感器个数只为2或3，若最边上的传感器相对输出不为0，则该个数只能为2。这个条件不难满足，满足条件后（实际条件要更宽松一些）对称求位置法的连续性将与全局求位置法的连续性相同。
1.1.3.2. 去除地图干扰
由于地图中的十字线、起点线和地图外的场地会导致计算出错误的黑线位置，使小车控制出错，通过设定以下约束可以解决这些问题：a.所有传感器输出都低于一个阈值时不计算新位置b.输出超过阈值的所有传感器并非连续安放的传感器时不计算新位置c.输出最大且超过阈值的传感器与上一个满足该要求的传感器不相邻时不计算新位置。
1.1.4. 连续性检测
用LCD 输出位置计算结果，相邻两次位置间隔小于0.5mm，相邻两次角度间隔小于0.1 。
1.2. PD 控制
1.2.1. 比例控制
位置控制中的比例控制采用了分段比例控制，位置较小时与位置较大时设置不同的比例带，原因是传感器宽度有限，检测的位置范围也就有限，统一的比例带过大会导致小车振荡，过小导致最大控制量偏小，小车转向不足，过弯时冲出赛道。使用分段比例控制既方便又可以解决以上两种问题。角度控制设置了死区，由于检测角度为0时，即使在直道上小车轴线与黑线也不严格平行，因此角度控制需要设置死区，以避免由此引起的小车直道振荡。
1.2.2. 微分控制
传感器输出模拟量的情况下检测的位置连续性较好，因此不再需要不完全微分的PD 控制，用一般形式的位置和角度微分控制即可达到很好的效果。由于比例带过小造成小车振荡时，适
当增加微分控制；振荡频率较高时则应该减小微分控制，因为此时D 控制量过大。此外，加大微分控制可以使小车稳定性和跟随性变好，防止冲出赛道。