第五章 软件设计
5.1 图像采集与处理
5.1.1赛道信息采集
由于如果对每一行图像都进行采集存储，会增大MC9S12DG128数据处理和存储的负担，而且智能车转向控制只要有前方几行的信息分辨率就足够了，因此只需要对特定行进行采集。本方案只采集指定的5行图像信息。
设置一个行计数器（row_counter），当IOC0捕捉到一个上升沿时，TFLG1_C0F寄存器置位，表明一个行同步信号刚刚过去，新的一行刚刚开始，将row_counter加1。当row_counter加到row_interval时，表明指定采集的行视频信号已到，此时进行图像采集。
图像采集利用了MC9S12DG128单片机的ATD模数转换，受其转换速度的限制，需要对MC9S12DG128进行超频，采集到的信息才满足横向分辨率的要求。
MC9S12DG128锁相环初始化程序如下：
void PLL_initial(void)
{
REFDV=3;
SYNR=7; //bus clock=16*(7+1)/(3+1)=32M;
while(CRGFLG_LOCK==0);
CLKSEL=0x80;
}
超频后的MC9S12DG128每行能进行71次AD转换即能采集71个点（image_point）。若采用外部纳秒级转换速度的外部AD转换芯片可采集到更多的横向赛道信息。考虑到71个点的横向分辨率已经足够，故直接使用MC9S12DG128片内的ATD模块。
ATD模块初始化程序如下：
void AD_initial(void)
{
ATD0CTL2=0xC0;
//AD上电，读取结果寄存器使转换完成标志位自动清零
ATD0CTL3=0x08; //0 0001 000，启动AD通道数为1
ATD0CTL4=0x81;
//采样时间为2个AD时钟周期,ADclock_max=8MHz,AD转换时间1.5us
ATD0CTL5=0xA0; //1010 0 000，右对齐，无符号，单通道0采样
ATD0DIEN=0x00; //禁止数字输入
}
采集后的单行图像信息存入image_line[line_max]一维数组，单场图像信息存入image_field[row_max][line_max]二维数组，方便利用串行通信在上位机上使用VB、VC自编软件或LabVIEW、MATLAB进行调试。
图像采集与处理流程图如图5.1所示：
图5.1 图像采集与处理流程图
曲率半径为50cm的典型赛道图像经MATLAB二值化后的5*71的图像如图5.2所示：
图5.2 智能车眼中的赛道
5.1.2图像特征提取
视频信号可以反应出黑线中心线的模拟位置，算出黑线中心线的位置即相当于提取出了赛道特征信息，进而对舵机和直流电机进行控制。
5.1.3黑线中心线提取算法
黑线中心线提取算法主要基于边沿提取算法。边沿提取算法主要有两种方案。
1）只要提取出黑线的左边沿（blackline_leftedge），就可以估计出黑线中心线的位置。
2）分别提取出黑线的左边沿和右边沿（blackline_rightedge），那么黑线中心线的位置就是（blackline_leftedge+ blackline_rightedge）/2，也可算出黑线宽度（blackline_width）。
由于黑线和白色跑道间颜色差别明显，故黑线在视频信号中表现的非常明显，即呈现一陡直的负脉冲，所以可以设定一个阈值（graylevel_threshold）来区分白色视频信号和黑线视频信号，从而区分黑线和白色跑道。可以通过判断相邻或相近点image_point的差graylevel_edge来提取出黑线边沿。
图5.3黑色中心线提取示意图
由于方案一计算黑线中心线的位置更加简单且同样可靠，故采用方案一进行算法设计。方案一的算法过程如下：
从某行图像最左端有效点开始依次向右（line++）进行阈值判断，由于黑白赛道边沿可能会有模糊偏差，故设置变量blackline_edge_add，如果image_line[line]-image_line[line+blackline_edge_add] >=graylevel_threshold，则说明该行视频信号模拟黑线中心线位置为
blackline_center[row]=line+blackline_edge_add。
实验中发现，由于图像传感器的前瞻性较强，经常会出现弯道内切现象。应用上述算法，当智能车严重偏离黑线且视野中失去黑线目标后，能够保持舵机控制量，从而控制舵机转向使赛车重新回到赛道上。
5.2 舵机控制
5.2.1 PWM控制信号
舵机的PWM控制信号的频率范围为50HZ到200HZ。而舵盘的输出转角仅与PWM控制信号的高电平时间成正比，当高电平时间为1500μs时，输出转角为0度。智能车的实际最大转角约40度，对应的高电平时间分别为1856μs和1144μs。其关系曲线如图5.4所示：
图5.4 PWM控制信号与转向的关系曲线
MC9S12DG128的PWM初始化程序如下：
void PWM_initial(void)
{
PWME = 0x00 ; // PWM disabled
PWMCTL_CON01 = 1; // PWM0,1两通道合并一个16 bit通道来控制舵机
PWMPRCLK = 0x33; // clockA=clockB=32M/8=4M
PWMSCLA = 1; // clockSA=clockA/2/1=2000k
PWMSCLB = 100; // clockSB=clockB/2/100 =20k
PWMCLK = 0b00000000; // PWM0,1-clockA
PWMPOL = 0xff; // 位极性＝duty＝High Time
PWMCAE = 0x00; // 对齐方式－左对齐
PWMPER0 = 0x9c; // Steer PWM Frequency=clockA/40000=100Hz
PWMPER1 = 0x40;
PWMDTY0 = 0x19; //0x1900=6000 舵机duty初始值
PWMDTY1 = 0x00;
PWME_PWME1 = 1; // Steer enable
PWMPER5 = 200 ; // DC motor PWM Frequency=SB/200=10K
PWME_PWME5 = 1 ; // DC motor enable
}
上述程序中，初始化后的PWM通道输出高电平为1500μS，频率为100HZ的PWM控制信号。起始舵机转角Θ为0度。
舵机转角计算公式为：转角Θ=（steer_control*10000/40000-1500）*90度。理论0度转角位置处steer_control=6000，理论+40度转角位置处steer_control=7424，理论-40度转角位置处steer_control=4576。
5.2.2舵机控制算法
黑线中心线位置计算出以后，只要和当前赛车中心线或视野中心线（line_center）做减法，即可算出黑线偏差（blackline_error）。由于图像传感器采集5行信息，故可以得到blackline_error[5]数组。若blackline_error[row]为正，说明该行采集到的黑线中心线位于视野中心线的右方，若为负则相反。如图5.5所示：
图5.5 黑线中心线偏差示意图
在设计算法之前，需要一些经验数据，来方便进行舵机算法的设计。分别按循线、微切、重切三种情况进行实验，如图5.6所示：
图5.6 实验示意图
在曲率半径为50cm的赛道上得到的经验数据分别如表5.1、5.2、5.3所示：
表5.1 循线经验数据表
|
blackline_error
|
位置
|
|||||||||||
|
1
|
2
|
3
|
4
|
5
|
6
|
7
|
8
|
9
|
10
|
11
|
12
|
|
|
blackline_error[0]
|
1
|
-20
|
-29
|
-29
|
-29
|
-29
|
-29
|
-29
|
-20
|
-14
|
-8
|
0
|
|
blackline_error[1]
|
1
|
-10
|
-16
|
-25
|
-29
|
-27
|
-29
|
-29
|
-18
|
-14
|
-8
|
0
|
|
blackline_error[2]
|
1
|
-6
|
-10
|
-16
|
-25
|
-27
|
-29
|
-29
|
-16
|
-12
|
-8
|
0
|
|
blackline_error[3]
|
1
|
-2
|
-6
|
-10
|
-16
|
-20
|
-29
|
-25
|
-14
|
-10
|
-8
|
0
|
|
blackline_error[4]
|
0
|
0
|
-2
|
-6
|
-10
|
-14
|
-22
|
-20
|
-12
|
-8
|
-6
|
1
|
表5.2 微切经验数据表
|
blackline_error
|
位置
|
|||||||||||
|
1
|
2
|
3
|
4
|
5
|
6
|
7
|
8
|
9
|
10
|
11
|
12
|
|
|
blackline_error[0]
|
-2
|
-14
|
-25
|
-29
|
-29
|
-29
|
-29
|
-29
|
-29
|
-23
|
-16
|
-12
|
|
blackline_error[1]
|
0
|
-6
|
-12
|
-25
|
-29
|
-29
|
-29
|
-29
|
-29
|
-22
|
-14
|
-12
|
|
blackline_error[2]
|
0
|
1
|
-6
|
-14
|
-23
|
-29
|
-27
|
-27
|
-29
|
-20
|
-14
|
-10
|
|
blackline_error[3]
|
0
|
3
|
0
|
-4
|
-12
|
-20
|
-20
|
-27
|
-29
|
-16
|
-12
|
-8
|
|
blackline_error[4]
|
-1
|
5
|
3
|
1
|
-4
|
-8
|
-10
|
-16
|
-22
|
-14
|
-10
|
-6
|
表5.3重切经验数据表
|
blackline_error
|
位置
|
|||||||||||
|
1
|
2
|
3
|
4
|
5
|
6
|
7
|
8
|
9
|
10
|
11
|
12
|
|
|
blackline_error[0]
|
0
|
-6
|
-10
|
-20
|
-27
|
-29
|
-29
|
-27
|
-18
|
-12
|
-6
|
-4
|
|
blackline_error[1]
|
0
|
0
|
-2
|
-4
|
-10
|
-14
|
-16
|
-16
|
-8
|
-6
|
-4
|
-1
|
|
blackline_error[2]
|
0
|
3
|
3
|
3
|
0
|
1
|
1
|
1
|
0
|
0
|
0
|
1
|
|
blackline_error[3]
|
0
|
5
|
7
|
7
|
7
|
11
|
13
|
13
|
7
|
5
|
3
|
3
|
|
blackline_error[4]
|
0
|
5
|
9
|
11
|
11
|
19
|
23
|
23
|
15
|
11
|
7
|
7
|
考虑到稳定性第一的原则，按微切情况的数据进行算法设计，主要有两种方案对进行舵机控制。
1）利用一行的blackline_error[row]进行舵机控制
该控制算法的优点是方法简单，如果采用前端的行进行控制还可以在一场图像未采集完成之前提前控制舵机，从而减小舵机的纯滞后环节。缺点是控制量单一，不能很好的反应赛道的曲率变化信息。
2）利用5行的blackline_error[row]之和blackline_sumerror进行控制
我们可以根据5行图像中每行的视野中心线和黑线中心线的偏差，在此基础上，可以求出5行的偏差和（blackline_sumerror）。最后根据该值的大小，并结合实际赛道实验数据，来确定弯道和直道之间的阈值，进而大致识别出赛道类型，为下面的舵机和直流电机控制提供信息。
该控制算法的优点是能够很好的反应赛道曲率的变化情况，且加入中值滤波后数据相对较可靠。缺点无法提前控制舵机，舵机存在20ms的电延迟。
考虑到赛道弯道处的曲率半径不变，即固定弯道弯曲程度不变，故采用方案一进行舵机控制。利用blackline_error[row]使用P比例系数计算出相应的PWM控制量steer_control，从而可实现对舵机的开环控制。控制量可使用线性或非线性方法进行计算，分别如图5.6和图5.7所示：
图5.6 线性控制
图5.7 非线性控制
考虑到舵机左右性能不平衡以及赛道适应性能因素，使用非线性控制能达到更好的转向效果。
5.3 直流电机控制
5.3.1速度检测
我们采用PT1（IOC1）通道作为速度脉冲信号输入，ECT模块初始化步骤如下：
1）设置TIOS寄存器，设置PT1通道为输入；
2）设置TCTL4寄存器，使得上升沿和下降沿（任何沿）均能得到捕捉；
3）设置ICOVW_NOVW寄存器，保护脉冲累加器的数据；
4）置位ICPAR_PA1EN，使能脉冲累加器。设置此寄存器之后，脉冲累加器开始计数；
5）通过读取PACN1寄存器，即可以获取当前的脉冲累加值。
初始化程序如下：
void ECT_initial(void) //ECT初始化
{
DDRT_DDRT1=0; //置PT1(IOC1)脚为输入
TIOS_IOS0=0;
TIOS_IOS1=0; //通道1为输入捕捉
TCTL4=0b00001101; //通道1为任何沿捕捉
TSCR1_TEN=1; //计数器1使能
ICOVW_NOVW1 = 1; //保护
ICPAR_PA1EN = 1; //脉冲累加器使能
}
在每一控制周期开始时，MC9S12DG128读取脉冲累加器中的数值（average[5]），然后与前5个控制周期的脉冲累加器值求和（all_speed）再求平均值，做为当前速度反馈值（speed）。程序流程图如图5.8所示：
图5.8 直流电机测速流程图
我们分别采用以下两种方法对电机测速部分进行测试。
1）让智能车在赛道上行驶，每20ms将赛车当前速度值通过SCI串口发送到上位机上，并利用串口调试器进行监控。对正好在一圈当中赛车行驶的速度值进行累加求和，再乘以20ms，得到的总行驶距离约为27m，而模拟赛道总长约为26m，两者的相对误差不到4%。这说明，速度传感器测量基本准确。
2）直流电机空载运行时，改变脉冲捕捉方式，在上升沿、下降沿和任何沿捕捉方式间进行切换。不改变驱动电机占空比设置，理想情况下，单位时间内捕捉的脉冲数满足：上升沿获取下的脉冲数=下降沿获取下的脉冲数=任何沿获取下的脉冲数/2。在脉冲捕捉方式不变的情况下，改变PWM信号占空比（即改变速度给定值），检测的速度值与占空比近似成线性比例关系。以上间接说明脉冲检测的可靠性。
5.3.2速度控制算法
智能车直流电机负载运行时，响应特性较软，而控制周期相对于电机的响应时间较短。我们采用简单的控制算法来实现赛车速度的闭环控制。在每一个控制周期中，检测一次赛车的当前速度值。若速度值小于给定的速度值，则将直流电机PWM的占空比置为100%（speed_add）；若速度大于预定的速度值，则将直流电机PWM 输入的占空比置为50%。控制流程图如图5.9所示：
图5.9 无赛道区分的速度控制流程图
初期调试时候出现了一个严重的问题：速度稳定性不足。原因在于：
1）速度传感器输出信号不稳定不规则；
2）速度期望值不宜定的过死，应加入允许偏差；
3）加速度应根据赛道状况而定。比如直道上可以尽量加速（PWM占空比设置为100%）,弯道上的加速度不能过高（PWM占空比80%）。改进后的速度控制算法流程图如图5.10所示：
图5.10 有赛道区分的速度控制流程图
在此基础上，我们模拟现实中汽车多档速度控制，对速度期望值进行分级，以实现更稳定、更精确的控制。每一级的控制器仅在比例系数K上有所不同。分级控制的稳定性可以由每个控制器接入闭环的时间来决定，只要每个控制器接入闭环的时间足够长，系统就可以达到稳定；快速性受到P控制算法的限制，调节时间较长，但由于分级系统的速度期望值连续性较好，一定程度上克服调节时间长的不足；准确性同样受到P控制算法的限制，存在一定的稳态误差，同样，分级系统可以弥补这一缺陷。
