飞思卡尔__所有的算法.pdf

《飞思卡尔__所有的算法.pdf》由会员分享，可在线阅读，更多相关《飞思卡尔__所有的算法.pdf（69页珍藏版）》请在三一文库上搜索。

1、第五章 智能汽车软件设计 第第第第 1 1 1 1 章章章章电机控制电机控制电机控制电机控制 1.1.1.1.1.1.1.1.直流电机控制策略：策略：策略：策略： 针对本文所研究的智能车来说，车体速度是大惯性的被控对象。算法输出的控 制量只能对电机输出力进行控制。而有一定负载时电机的输出力无论对车轮的转速 还是车体的形式速度都是不成正比的，车在刚开始启动的时候速度是零，而电机的 输出可能很大；车在匀速行驶的时候速度很快，而电机的输出可能并不是很大。而 且电池电量、车体重量都会对车速造成影响。因此只有用闭环闭环才能对车速进行良好 的控制。在车轮对地面不打滑的情况下车体的速度和后轮的转动速度是成正

2、比的。 因此我们可以直接用光电码盘对后轮的转速进行控制。 对于这样一个大惯性系统，我们选用 PIDPIDPIDPID 和鲁棒和鲁棒相结合的办法进行速度控制。 回路的设定值由经验值确定。 考虑到速度控制通道的时间滞后比较小， 因此采用 PID 控制方案，并在进行加减速控制时，引入了“棒棒控制” 。 U(kU(kU(kU(k+ + + + 1)1)1)1) = U(K)U(K)U(K)U(K) + P1P1P1P1 * e(k) + P2P2P2P2 * (e(k) e(k - 1) + P3P3P3P3 * (e(k) e(k-1) (e(k-1) e(k - 2); 公式 1 1 PID 的公

3、式 其中第一项为积分项； 第二项为比例项； 第三项为微分项。 考虑到被控对象 （车 体速度）本身是一个大的积分环节，公式中可以将第一项省略，即采用 PD 控制。 E 为误差。 同时设定误差门限，在误差比较大的时候采用大输出控制电机，将误差在最短 时间内减小到所要求的范围，这就是鲁棒控制鲁棒控制的思想。 电机控制策略，其实就是对模型车速度的控制策略，它是继路径识别之后非常 重要的策略，直接关系到整个模型车比赛的性能。速度控制得当，小车才能以最好 的状态，在最短的时间完成比赛路程。 1.1.1.1.1.1.1.1.1.1.1.1. 弯道速度控制弯道速度控制 在模型车入弯时刻出于稳定性考虑做减速控制

4、。减速原则是在原来直道速度设 定值的基础上，减小速度设定值到低速挡，保证模型车可以安全入弯。另一方面， 入弯之后，为了过弯时模型车能够不明显的左右摆动并采用较好的姿态通过弯路， 第五章 智能汽车软件设计 令车速与偏差成线性关系，偏差越大速度设定值越小令车速与偏差成线性关系，偏差越大速度设定值越小。 速度设定方法如公式 1 2。 s1 v (k) = v-k * e(k) .公式 1 2 其中： s v (k) 为速度闭环设定值； v 模型车全程运动平均速度设定值； e(k) 车体偏离理想轨迹的当前偏差值； 1 k 为减速控制比例系数。 同时，通过实验发现模型车入弯之后，令模型车以某一线性规律加

5、速运行 可以使车在不冲出轨道的前提下以更短的时间通过弯道。控制规律如.公式 1 3。 s v (k) = v+Ck*Cp .公式 1 3 其中：Ck为弯道加速系数； Cp 弯道加速变量； Ck为常数，初始化时设定。Cp入弯时刻初始化为 0，每个控制周期累加 1。 1.1.2.1.1.2.1.1.2.1.1.2. 直道速度控制直道速度控制 直道采用匀速控制方案，速度设定在此控制系统下可以控制的最大入弯速度最大入弯速度。 在从弯道驶出进入直道后须进行加速控制，增大速度设定值。因速度控制回路有一 定的惯性，所以在提速时为了使速度迅速改变提速时为了使速度迅速改变，加入了“棒棒控制” 。控制规律如 公式

6、 1 4。 sap v (k)= v+100- K *k 公式 1 4 其中： a K 补偿效果控制系数； p K 调速补偿变量。 a K 为常数，初始化时设定。 p K 出弯时刻初始化为0，每个控制周期累加1。 1.1.3.1.1.3.1.1.3.1.1.3. PIDPIDPIDPID 闭环控制算法闭环控制算法 根据路径识别的情况，如果当前路径为直道，则需要加速；若是弯道，则需要 降速，而且根据不同的弯道速度也是有所区别。系统利用测速模块反馈的当前速度 值，通过增量式 PID 算法进行调节，从而控制直流电机对当前路径进行快速反应。 增量式 PID 的算式为： 第五章 智能汽车软件设计 ( )

7、( )()( )( )()()1212 PID u ke ke ke ke ke ke k KKK =+ 在增量式 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-la

8、st_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 信号相对 较大

9、，这样电机转速相对较快；反之，电机转速则相对较慢。由此可以看出，PID 算法主要的功能是，在闭环系统中，利用即时速度的反馈，使得即时速度逼近标准 速度的时间尽量变短，这样小车就可以根据路径识别得出的速度标准值，及时调整 自己的速度，以适应各种路况。例如，小车正在直道上行进，而且直道的时速应该 是各种赛道中最快的，当系统的路径识别算法察觉到前方出现弧度大的弯道后，系 统会根据事先调试的结果，给出大弧度弯道的速度标准值，然后 PID 算法根据速度 标准值speed_v和即时速度反馈值infrared_value7对电机的输入信号PWM进行及时 调整，反映到实际中，就是小车及时减速并顺利通过弯道。

10、在增量式 PID 处理的过程中，有一个步骤需要注意，即在算完u (k)后，需要 把它赋值给电机控制对应的 PWM 通道信号，这时要判断该u (k)的值，如果它小 于 0，则把 PWM 信号赋值为 0，如果它大于 PWM 信号的最大值，即大于 PWM 整 个周期所对应的数值时，要把 PWM 信号赋值为该最大值。 1.2.1.2.1.2.1.2.反向制动算法反向制动算法反向制动算法反向制动算法 要使赛车在较短的时间完成比赛，速度自然越高越好，显然速度太高弯道是过 不去的，如果以弯道的极限速度匀速跑，又浪费了直道的时间。所以最佳的策略是 直道以较高的速度跑，到弯道时再尽快将速度降下来。在入弯减速时如

11、果只靠赛道 第五章 智能汽车软件设计 的摩擦阻力效果显然是不够理想的。为此我们引入反向制动算法。 由于MC33886芯片集成的H桥驱动电路本身就具有反向制动功能，所以不需再外 加硬件电路。反向制动流程图如图： 是 否 为 弯 道 当前车速是否 超出该曲率对应 极 限 车 速 判 断 曲 率 大 小 反 向 制 动 加 速 到 目 标 车 速 否 是 否 是 R S3 8 0 电 机 在制动时，电机反向电动势对整个系统电路造成的冲击，从而引起单片机工作 不稳定，电流过大导致电机过热、电机频繁换向导致电刷打火剧烈导致电机寿命缩 短等一系列问题都是需要考虑的。 1.3.1.3.1.3.1.3.速度控

12、制策略速度控制策略速度控制策略速度控制策略 1、为了达到好的速度控制效果，对速度进行闭环控制是必须的。这里所说的速 度控制策略是指设定速度的确定方法设定速度主要由道路与直道的偏差来决 第五章 智能汽车软件设计 定，道路越接近直道，设定速度越高，反之越低。 车模行驶中的最低速度是这样确定的：令车模以较低的速度匀速行使，在保证 无犯规行为出现的前提下，逐渐提高匀速行使的速度，直到车模出现犯规行为，此 速度再减去一个安全量，即为所需的最低速度。简单的说，变速行使的最低速度等 于匀速行使的最高速度。 车模行驶中的最高速度是这样确定的：在确定最低速度以后，加入变速策略， 不断提高最高速度的设定值， 直到

13、模型车出现犯规行为， 此速度再减去一个安全量， 即为所需的最高速度。 车模行驶过程中难免出现“失去道路”的情况，对此需要采取一定的安全策略， 防止赛车“盲跑”而导致犯规。 2、针对本文所研究的智能车来说，车体速度是大惯性的被控对象。算法输出的 控制量只能对电机输出力进行控制。而有一定负载时电机的输出力无论对车轮的转 速还是车体的形式速度都是不成正比的，车在刚开始启动的时候速度是零，而电机 的输出可能很大；车在匀速行驶的时候速度很快，而电机的输出可能并不是很大。 而且电池电量、车体重量都会对车速造成影响。因此只有用闭环才能对车速进行良 好的控制。 在车轮对地面不打滑的情况下车体的速度和后轮的转动

14、速度是成正比的。 因此我们可以直接用光电码盘对后轮的转速进行控制。 对于这样一个大惯性系统，我们选用 PID 和棒棒相结合的办法进行速度控制。 回路的设定值由经验值确定。 考虑到速度控制通道的时间滞后比较小， 因此采用 PID 控制方案，并在进行加减速控制时，引入了“棒棒控制” 。 U(k + 1) = U(K) + P1 * e(k) + P2 * (e(k) e(k - 1) + P3 * (e(k) e(k-1) (e(k-1) e(k - 2); 公式 4 3 PID 的公式 其中第一项为积分项； 第二项为比例项； 第三项为微分项。 考虑到被控对象 （车 体速度）本身是一个大的积分环节

15、，公式中可以将第一项省略，即采用 PD 控制。 E 为误差。同时设定误差门限，在误差比较大的时候采用大输出控制电机，将误差 在最短时间内减小到所要求的范围，这就是棒棒控制的思想。 电机控制策略，其实就是对模型车速度的控制策略，它是继路径识别之后非常 重要的策略，直接关系到整个模型车比赛的性能。速度控制得当，小车才能以最好 的状态，在最短的时间完成比赛路程。 第五章 智能汽车软件设计 1.3.1.1.3.1.1.3.1.1.3.1. 弯道速度控制弯道速度控制 在模型车入弯时刻出于稳定性考虑做减速控制。减速原则是在原来直道速度设 定值的基础上，减小速度设定值到低速挡，保证模型车可以安全入弯。另一方

16、面， 入弯之后，为了过弯时模型车能够不明显的左右摆动并采用较好的姿态通过弯路， 令 车 速与偏 差 成线性 关 系，偏差 越大速 度 设定值 越 小。速 度 设定 方法 如.公式 4 4。 .公式 4 4 其中：为速度闭环设定值； 模型车全程运动平均速度设定值； 车体偏离理想轨迹的当前偏差值；为减速控制比例系数同时，通过实验发现模 型车入弯之后，令模型车以某一线性规律加速运行可以使车在不冲出轨道的前提下 以更短的时间通过弯道。控制规律如.公式 4 5。 .公式 4 5 其中：为弯道加速系数； 弯道加速变量； 为常数，初始化时设定。入弯时刻初始化为 0，每个控制周期累加 1。 1.3.2.1.3

17、.2.1.3.2.1.3.2. 直道速度控制直道速度控制 直道采用匀速控制方案，速度设定在此控制系统下可以控制的最大入弯速度。 在从弯道驶出进入直道后须进行加速控制，增大速度设定值。因速度控制回路有一 定的惯性，所以在提速时为了使速度迅速改变，加入了“棒棒控制” 。控制规律 如公式 4 6。 公式 4 6 其中：补偿效果控制系数； 调速补偿变量。 为常数，初始化时设定。出弯时刻初始化为，每个控制周期累加 。 第五章 智能汽车软件设计 1.3.3.1.3.3.1.3.3.1.3.3. 速度及加速度信号采集子程序速度及加速度信号采集子程序 当图像数据采集处理完后，我们读取速度传感器和加速度传感器的

18、值，采集这 两个传感器信号的频率也为 60Hz。流程图如图 4.3 所示。 图 4.3 速度及加速度信号采集子程序流程图 1.4.1.4.1.4.1.4.速度速度速度速度 PIDPIDPIDPID 算法算法算法算法 根据偏差的比例（Proportional） 、积分（Integral） 、微分（Derivative）的线性组合进行反馈控制 （简称 PID 控制） ，数字 PID 控制算法是电机微机控制中常用的一种基本控制算法9。 在连续系统中，模拟 PID 调节器是一种线性调节器，控制系统原理框图如图5.10。 图 5.10 PID 控制系统原理框图 控制规律为： 子程序入口 读速度传感器的值

19、 读加速度传感器的值 读两个 16 位脉冲计数器的值 返回主程序 第五章 智能汽车软件设计 0 1( ) ( ) ( )( ) t PD I de t u tKe te t dtT Tdt =+ （5.1） 式中： p K：比例增益， p K的倒数称为比例带； I T：积分时间常数； D T：微分 时间常数；( )u t：控制量；( )e t：偏差，等于给定量与反馈量的差。 在计算机控制系统中， 数字 PID 控制算法通常又分为位置式 PID 和增量式 PID。 本次设计中，我们采用增量式 PID。 增量型算法与位置型算法相比，具有以下优点：增量型算法不需要做累加，增 量的确定仅与最近几次偏差

20、采样值有关，计算精度对控制量的计算影响较小，而位 置型算法要用到过去偏差的累加值，容易产生大的累加误差；增量型算法得出的是 控制量的增量，而位置型算法的输出是控制量的全量输出，误动作影响大；采用增 量型算法，易于实现手动到自动的无冲击切换。 在实际应用中，采样的反馈值 ( )y k即为脉冲累加器中的 PACN32 中的脉冲数， 预设门限值A在参数整定时根据实际情况调节，输出( )u k并不能直接用来控制电 机，需要将其转换为控制 PWM 占空比，然后用增大或减小 PWM 占空比的方法来 实现对电机的加减速的控制。换句话说，在求偏差量时，实际上用的是每 20ms 电 机转过的齿轮数和实际期望电机

21、转过的齿轮数，通过二者的差值，再乘以相应的系 数， 即 KP、 KI、 KD的协调控制， 计算出相应的 PWM 占空比， 实际上用的是 PWMDTY 的值10。 本设计中综合考虑各种因素，最后选用的采样周期为 20ms，即每 20ms 对电机 进行一次 PID 调节。由于在程序中，对图像的采集使用的是 PH 口的中断程序，因 此， PID 采样周期的选择实际上是受限制与图像采集， 因为每行的扫描周期为 64s， 有效扫描时间为 52s， 采用的是隔行扫描的方式， 即每隔 6 行采集一行图像的信息， 如果在每行之间加入 PID 调节的话，那么处理 PID 子程序的时间必须控制在 64*5 320

22、s 之内，另外图像采集只是采集了奇场中的行数，在偶场中没有采集，因此 PID 子程序的执行是不均匀的，并没有达到预期的效果，同时还可能会影响到视频 采集，因此，经过分析，最终决定将 PID 的采样周期定为 20ms，即当进行一次场 采集进行一次 PID 调节。而且经过最终的检验，这样能够满足对速度控制的需要。 电机调速 第五章 智能汽车软件设计 本作品采用增量式的 PI 控制。 增量式 PI 调 节 set_value 电机 输出 测速检测 图 4.12 增量式 PI 算法: pwm=Kp*(errork0-errork1)/100+Ki*errork0/100+pwm; 1.5.1.5.1.

23、5.1.5.直流驱动电机控制子程序直流驱动电机控制子程序直流驱动电机控制子程序直流驱动电机控制子程序 本设计通过调整方波的占空比来实现电机的调速。经过初始化子程序的设置， PWM0 和 PWM5 都能输出频率为 5KHz 的方波。由硬件部分可知，PWM0 产生的方波经 MC33886 功率放大后接到电机的负极，PWM5 产生的方波经 MC33886 功率放大后接到 电机的正极。车模向前行驶时，PWM0 输出方波的占空比为零，通过调整 PWM5 输出 方波的占空比实现加速或缓慢减速。当需要快速减速时，PWM5 输出方波的占空比为 零，PWM0 输出方波的占空比为合适的值，电机反转实现车模制动。该

24、部分程序工作 流程图如图 4.7 所示。 第五章 智能汽车软件设计 1.6.1.6.1.6.1.6.舵机控制舵机控制舵机控制舵机控制 1.6.1.1.6.1.1.6.1.1.6.1. 舵机转角的决策舵机转角的决策 由于 CCD 图像传感器的应用，大大提高了车的前瞻性。经测量，本系统预测距 离可达 1 米以上。但是，考虑到各部件延时严重且具有很大不确定性，所以预测距 离越远，控制算法就会越复杂。综合各种因素考虑，我们最终将预测距离定在 80100cm。 黑线中心位置轨迹中包含着两方面的信息， 一方面是反映前方道路趋势的信息， 另一方面是反映车体与轨道偏离程度的信息。经过反复试验，我们发现，前者可

25、以 通过计算黑线的斜率来很好的描述。而后者，恰恰就是屏幕最下端黑线的位置。最 后用这二者的线性组合得到舵机转角。有了这两方面信息，可以使智能车既能充分 利用前瞻性， 又能不过分地偏离轨道。 从而得到了较理想的转角控制策略解决方案。 1.6.2.1.6.2.1.6.2.1.6.2. 偏航距离的计算偏航距离的计算 由于已经获得了赛道中心线的位置，所以计算偏航距离的问题是选取何处的中 心线的距离为当前的偏航距离。控制算法的执行周期为 40ms，如果赛车的速度为 2m/s，则在两次控制算法的执行中间，赛车要前进 8cm，赛车所处的环境将发生比 Y N 图 4.7 驱动电机控制子程序流程图 是否需要制

26、动 返回主程序 修改 PWMDTY5 的值 获得速度控制值 子程序入口 修改 PWMDTY0 及PWMDTY5 值 返回主程序 第五章 智能汽车软件设计 较大的改变，所以赛车的控制只能算是半实时控制，这是所有使用摄像头作为主要 寻线传感器的参赛队都避免不了的问题。因为算法的滞后性，赛车需要将“当前位 置”进行适当前移。前移量应该跟赛车当前速度成正比，但实际中我们发现，适当 增加一些前移距离是有好处的，因为可以在入弯处提前转弯，使得赛车沿弯道内侧 行驶，缩短了过弯距离。 1.6.3.1.6.3.1.6.3.1.6.3. 舵机参数测定舵机参数测定 通过实验测出已安装在模型车上的舵机的中位PWM值（

27、Servo_center） ，模型车 转向轮左右极限时舵机PWM（Servo_left、Servo_right）值是舵机控制的基础。 测试舵机中位时，先在舵机控制模块中给舵机PWM设计一个全局变量 （Pwm_Servo） ，将赛车放在长直道上，保证车身放正并处于赛道正中间，利用BDM 查看此时Pwm_Servo的值，将此值直接赋给舵机PWM，并将赛车放到直道上跑，看是 否跑偏，微调Pwm_Servo的值，直到赛车在直道上不再跑偏为止。 测定舵机左右极限时，可根据转向轮的左右极限转角，利用BDM查看舵机 Servo_left和Servo_right的值。 注意转向轮在左右极限转角时， 转向轮不可

28、被赛车 底盘卡住，这样不但会增加转向阻力，严重时还可能使转向轮抱死，失去转向能力。 舵机的PWM计算公式为： Pwm_Servo=Servo_center+(Servo_left-Servo_right)/Simage_number*offline_on e;公式四 式中Simage_number为每行采集的点数， offline_one为黑线在整幅图像中的位置。 1.6.4.1.6.4.1.6.4.1.6.4. 路径规划路径规划 要想以最短的时间完成比赛，并不需要严格按照黑色引导线行驶，我们需要找 出相对较短的行驶路径，即路径规划。例如 过普通弯道的时候，以同样的速度走内 弯显然比走中间或外

29、弯所花时间少， 过小S形弯时。 直接过去和严格循迹相比速度快， 行驶距离短等等。 （图） 如果能提前预知前方赛道信息，那么我们就有可能对路径进行规划。由于我们 采用的是摄像头，扫描距离较大，这就为我们进行路径规划提供了条件，这也是我 们选择摄像头的原因之一。 由于摄像头看得较远，每幅图像反映的赛道信息量较大，我们需要针对赛车在 不同位置，对其有侧重的采用。具体就是将采到的15行划分为几个区域，针对不同 路况对每个区域赋以不同的权值。其原理图如图： 第五章 智能汽车软件设计 参 数 建 表路 径 判 别 直 道 对 应 参 数 及 算 法 弯 道 对 应 参 数 及 算 法 小 S 道 对 应

30、参 数 及 算 法 大 S 对 应 参 数 及 算 法 舵 机 打 角 直道 弯道 小 S 大 S 图5.10 路径规划示意图 1.7.1.7.1.7.1.7.偏航距离的计算偏航距离的计算偏航距离的计算偏航距离的计算 由于已经获得了赛道中心线的位置， 所以计算偏航距离的问题是选取何处的中 心线的距离为当前的偏航距离。控制算法的执行周期为 40ms，如果赛车的速度为 2m/s，则在两次控制算法的执行中间，赛车要前进 8cm，赛车所处的环境将发生比 较大的改变，所以赛车的控制只能算是半实时控制，这是所有使用摄像头作为主要 寻线传感器的参赛队都避免不了的问题。因为算法的滞后性，赛车需要将“当前位 置

31、”进行适当前移。前移量应该跟赛车当前速度成正比，但实际中我们发现，适当 增加一些前移距离是有好处的，因为可以在入弯处提前转弯，使得赛车沿弯道内侧 行驶，缩短了过弯距离。 1.7.1.1.7.1.1.7.1.1.7.1. 偏航角度的计算偏航角度的计算 计算偏航角度的实质是直线拟合问题，因为赛道中心线所在的直线确定了， 而 直线的斜率与偏航角度一一对应。直线拟合最有效的方法是最小二乘法 7，但是直 接应用存在一个问题，即如何确定进行直线拟合的区间？在整个成功识别出赛道的 区间内进行直线拟合显然是欠缺考虑的，因为在弯道的情况下，这种方法拟合出的 是一条弦线，而不是当前该弯道处的切线。摄像头视野越大，

32、弯道曲率越大，弦线 偏离切线的程度也就越大。 为了能够在直道和弯道上都能正确的拟合出正确的直线， 我们采用了直线检测的方法，即首先根据残差的大小确定直线的范围，然后在这一 范围内进行直线拟合。 1.7.2.1.7.2.1.7.2.1.7.2. 曲率的计算曲率的计算 如果说斜率的计算需要某种技巧的话，计算曲率则更是一种技巧的应用。首届 时很多参赛队针对各自的实际需要，提出了自己的方法。其中最普遍的是根据斜率 的导数来计算曲率8。但是斜率的计算本身就很不准确，特别是某个点的斜率，对 斜率求导就更不准确，所以使用这种方法只能得出一个大致的结果。本文作者提出 了另外一种方法，首先对获得的路径进行滤波，

33、使得路径尽可能平滑，然后取其两 个端点和中间点，计算这 3 个点组成的三角形的外接圆的半径，半径的倒数就是这 段路径的曲率。经过多次实验，这种方法的误差一般不大于 20%，对智能车的控制 来说已经足够了。 图 5.8 是实验赛道的照片，让赛车从起跑线开始，在赛道上行驶一圈，记录下 每个时刻的曲率，如图 5.9。从图 5.9 可以看出，计算出的曲率能较为正确的反映实 际赛道的弯曲情况。 但是这种方法得出的曲率不是摄像头所看到的当前位置的曲率， 而是摄像头所看到的 路径的整体曲率，因为这种算法仅仅与路径中三个点的位置有关。然而实验中 却发现，这种特点反而给赛车带来了一个好处：即摄像头在小S型弯道时

34、舵机几乎 不跟随路径的摆动而摆动，而是直线冲过。这是因为较小的S型路径其弯曲部分能 完整的显现在摄像头的视野中，而算法中对路径进行了滤波，滤除了中间部分的弯 曲，使得路径变直了，赛车因此就直线走过。 1.8.1.8.1.8.1.8.车辆横向控制算法车辆横向控制算法车辆横向控制算法车辆横向控制算法 本设计采用的横向控制算法是模拟人驾驶汽车的方式控制车模转向。首先，根 据图像数据处理后的数据判断车模偏离黑线的距离。其次，综合考虑车速传感器获 得的当前速度值，计算目标转角。然后，在下一次采样时，与上次采样值比较，判 断偏离距离的变化趋势，再次计算目标转角，当车模回到中线位置时，前轮回正。 该部分程序

35、流程图如图 4.4 所示。 图 4.4 横向控制算法流程图 返回主程序 返回主程序 Y 子程序入口 对近距离光电传感器编 码，获得车辆偏离距离 是否偏离黑 线？ 前轮回正 与上次采样值比较，判 断偏离距离变化趋势 结合车速信号，得到目 标转角 N 1.9.1.9.1.9.1.9.车辆纵向控制算法车辆纵向控制算法车辆纵向控制算法车辆纵向控制算法 本设计中模型车的速度控制采用 PID 控制算法。首先，图像数据处理后的数据 判断车模当前行驶的路况，得到目标速度值。其次，根据车速度传感器的值，判断 车速是否达到目标速度，进行加减速。第三，结合加速度传感器的值判断模型车是 否发生滑移，如果滑移进行防滑处

36、理。该部分程序流程图如图 4.5 所示。 图 4.5 纵向控制算法流程图 1.10.1.10.1.10.1.10.转向控制子程序转向控制子程序转向控制子程序转向控制子程序 由于舵机反应速度的局限， 本设计每 10ms 对舵机控制一次。 可知 PWM23 已经在 N 加速控制 Y N Y Y 子程序入口 由前轮速度传感器 的值计算车速 是否达到目 标速度？ 加速控制 是否发生滑 转？ 是否进入弯 道？ 防滑处理 返回主程序 返回主程序 制动控制 N 初始化子程序中设置完毕，其能产生周期为 20ms 的方波，在此我们只需修改 PWMDTY23 寄存器即可改变方波的占空比，进而控制舵机转向。本车舵机

37、的安装位置 和 PWM23 通道的设置使得前轮在中间位置时， PWMDTY23 的值为 4960， 左极限位置时 值为 5300，右极限位置时为 4640，即将左极限位置到右极限位置的转向级别分为 660 级。为了防止舵机转向超过极限位置，发生卡死现象，损坏舵机，本设计通过 软件限定极限转角。该子程序工作过程如图 4.6 所示。 1.11.1.11.1.11.1.11.车体控制策略：车体控制策略：车体控制策略：车体控制策略： 对于硬件结果已经确定的智能车来说，算法控制的实际上就是车体的方向和速 度，即前轮舵机的转角和后轮的转速。如果前面图像识别和虑噪环节做的好的话， 对于控制量的给出的算法就相

38、对容易，但是需要花很大精力进行参数的调整，使得 车的行驶轨迹更加优化，并且让速度尽可能的提高。 对于前轮的控制来说，由于执行机构（舵机）和被控对象的惯性相对较小，换 句话说算法给出的控制量在误差允许 的范围内可以认为由执行机构准确执行， 可以 进行开环的控制2。 N 图 4.6 舵机控制子程序流程图 N 是否超出范 围？ 返回主程序 修改 PWMDTY23 的值 获得转向角度值 子程序入口 赋给极限 角度值 Y 我们的控制算法是在图像中分别取上上、 中中、 下下三个点， 即在 23232323 行行的数组中取 3 3 3 3、 、 12121212、21212121 三个点三个点，计算出图像的

39、位置，然后用最上面一点和最下面一点的差最上面一点和最下面一点的差做为图像 的斜率斜率。然后用两者的线性组合算出舵机的转角两者的线性组合算出舵机的转角。用下面公式给出： Warp = P1 * (Average2 +Average11 + Average20) + P2 * (Average20 Average2) 公式 1 5 舵机的转角公式 其中，warp 是车体对于黑线的误差，也是舵机应该转动的角度。 同时，我们发现在车运行过程当中经常会出现图像中只有半幅图像有线，或者 整幅图像都没有黑线的情况。在半幅图像有线的情况下，我们就用图像的在视野中 的终点、图像最下面的点及两者的中点进行控制；如

40、果整幅图像都没有黑线，就让 车的舵机保持上一次的转角，一直到图像重新恢复到视野。 对于前轮的控制来说，由于执行机构（舵机）和被控对象的惯性相对较小，换 句话说算法给出的控制量在误差允许 的范围内可以认为由执行机构准确执行， 可以 进行开环的控制。 第第第第 2 2 2 2 章章章章图象采集图象采集图象采集图象采集 2.1.2.1.2.1.2.1.采集采集采集采集 1 1 1 1、一场图像采集完成后，得到一幅 20*20 的原始二值图像信息，如图 4.3（A） 所示，同时给标志变量 VideoFinFlag 置 1。主程序中检测到此标志的时候，启动滤 噪程序。考虑到图像中有效的信息为简单的黑色线

41、条，必然具有很强的连续性。所 以，可以采用从图像一侧到另一侧，逐行邻域查询的办法。具体来说，就是根据已 经确定的前行黑线中心位置，在本行的一个邻域内查找黑点。超出这个邻域的黑点 都认为是无效的。这样，只要能够确定某一行黑线的中心位置，就能逐行确定各行 黑线的中心位置，从而，也就提取出了图像中最基本的信息黑线中心位置轨迹 （如图 4.3（B）所示） 。 原始图像信息处理后的信息 000000000000100000002 000000000000100000002 000000000001000000001 000000000001000000001 000000000010000000000

42、000110000010000000000 000100000010000000000 000000000011000000000 000000000011000000000 00000000011000000000-1 00000000011000000000-1 000000000111000000000 000000000111000000000 000000000111000000000 00000000111000000000-1 00000000111000000000-1 00000001110000010000-2 00000001110000000000-2 00000001

43、110000000000-2 00000011110000000000-3 (A)(B) 图 4.3处理前后的图像信息 2 2 2 2、阈值分割法是一种基于区域的分割技术，它对物体与背景有较强对比的景物 的分割特别有用。它计算简单，而且总能用封闭且连通的边界定义不交叠的区域。 阈值分割法的关键在于阈值的确定。如果阈值是不随时间和空间而变的，称为静态 阈值；如果阈值随时间或空间而变化，称为而动态阈值。基于静态阈值的分割方法 算法简单，计算量小，但是适应性差。基于动态阈值的分割方法其复杂程度取决于 动态阈值的计算方法6。针对本智能车系统，普通的双峰法就能适合绝大部分情况， 因为智能车的运行环境是比

44、赛赛道，背景和前景区分明显，且背景简约。但是实验 环境并不理想，由于受到光线斜射的影响，有时背景和前景的对比十分不明显。结 合实际需要， 提出了一种新的计算阈值的方法， 这种方法的思想与迭代法有些相似。 首先我们假定，智能车系统运行时，开始时采集的第一幅图像是良好的。这个 “良好” 含义是：第一行也就是最近处的一行，完整的包含了导航线，并且使用“双 峰法”能正确提取。其次，我们对每一行都定义一个阈值 Tr，每一行都用该行的阈 值 Tr 进行分割。在此基础上按照以下规则进行阈值传递： a)如果第 r 行分割出来的黑色区域（线段）是连续的，那么计算目标区域和 背景区域的平均灰度值，并取其中值，做为

45、当前行和下一行的 Tr。 b)如果第 r 行分割出来的黑色区域是空集或者不连续，则保持当前行和下一 行的阈值不变。 下滤波算法： 当某点的灰度值与其左边和右边点的灰度值的差同时大于某个正 阈值或同时小于某个负阈值时，认为该点是干扰点，取其左右两边灰度值的平均值 作为该点的灰度值。 2.2.2.2.2.2.2.2.图像抗干扰处理图像抗干扰处理图像抗干扰处理图像抗干扰处理 抗干扰处理包括以下方面的内容： 1）消除信号产生和传输的过程中造成的噪声； 2）消除赛道不理想或光线不均匀形成的干扰； 3）消除赛道中的交叉线、断续线； 4）识别起跑线。 信号产生和传输的过程中造成的噪声具有普遍性， 但是只有当

46、其非常严重时才 对路径的识别造成影响。所以采取以下滤波算法：当某点的灰度值与其左边和右边 点的灰度值的差同时大于某个正阈值或同时小于某个负阈值时， 认为该点是干扰点， 取其左右两边灰度值的平均值作为该点的灰度值。对于赛道不理想或光线不均匀形 成的干扰，其一般特点是散布于赛道黑线的两边，所以采用以下算法处理： 步骤 1 从最近的第一行开始，寻找这样一个行：该行只有一段连续黑色线段， 且线段的左右边均不是图像的左右边。标记该行为 Start 行。 步骤 2 在下一行中寻找这样一段连续黑色线段，满足这样的条件：其左边缘 不在上一行黑线右边缘的右边， 且其右边缘不在上一行黑线左边缘的左 边。 步骤 3

47、 如果存在步骤 2 中的黑色线段，认为该段黑线是赛道黑线，其余黑点 （线段）均认为是干扰，跳步骤 2。否则，标记该行为 End 行，并计算 该行与 Start 行之差，如果大于 3，则结束搜索，如果小于等于 3，则返 回步骤 1。 这种算法的实质是搜索连续的黑线。因为赛道是连续的，而干扰却大部分是离 散分布的。 对于赛道中的交叉线，可以统计一行中黑点的个数，因为正常的黑线只有 2.5cm，在图像中不超过 6 个点。所以把 6 作为一个阈值 TN，如果一行黑点的个数 超过 TN 个点，就可以认为是交叉干扰。实际上，我们做了更复杂的处理，就是根 据当前赛道的倾斜程度来调节 TN，因为当赛道倾斜时其

48、宽度将发生变化。并且， 我们不只是识别出交叉干扰，而且“砍掉”它的左右两支，保留一条连续的赛道黑 线。 起跑线是在赛道黑线的左右两边，对称存在两条长度各为 10cm，相距 6cm， 且 与赛道黑线垂直的黑色线段。正确的识别起跑线是应用更高级控制算法如记忆算法 的基础。我们使用如下算法识别起跑线：在已识别出赛道黑线的行，保证该行不是 交叉行（前面已经识别出） ，然后确定其左右两边各一个矩形区域，统计两个区域中 黑点的个数，如果均大于某一阈值，则判断该行出现了起跑线。 比赛中将设置限速段， 限速段的一种可能的标记法就是把赛道黑线做成断续的， 断续宽度 2cm，间隔 2cm，所以并不是所有不连续的黑

49、点都是干扰。这给参赛选手 带来了很大的挑战。我们采取的应对算法是，在遇到断续的行时，按当前已识别出 的黑线的走向趋势延伸黑线到该行，如果在 3 行内发现了可以连接的黑线，则认为 延伸成功，否则结束搜索。 经过以上处理，我们得到了如下信息： 首先是两个标记 Start 和 End，这两个标记标记了成功识别出的赛道的最近一 行和最远一行； 其次是在 Start 和 End 之间的每一行， 记录了其赛道的左边缘位置和 右边缘位置，并且通过坐标变换算法，计算出每行赛道中心线的实际位置。最后留 下了一些其它标志，比如每行是否是交叉、起跑线或者断续线。这些标志在我们调 试的过程中都用某种方式以图像的形式反映出来。