基于卡尔曼滤波的赛道检测优化算法

刘新宁

（长安大学 汽车学院，陕西 西安 710064）

目前，汽车正朝着智能化方向迅猛发展，高 级驾驶辅助系统发展迅速，各大汽车厂商都在发展智能车。主流的智能汽车都在使用摄像头对前方道路进行识别，因此，做好摄像头智能车是研究智能辅助驾驶的重要前提。

卡尔曼滤波算法在智能辅助驾驶方面有着广阔的应用前景。卡尔曼滤波算法的关键是根据某一时刻“实际测量值”和上一时刻的“预测估计值”以及估计误差和测量误差，通过加权处理计算得到该时刻的最优结果，再对下一时刻的数据进行预测。随着数据的不断更新，误差也在不断迭代，从而实现数据的不断更新[1]。

本文的主要研究工作是通过对比嵌入卡尔曼滤波算法前后，摄像头智能车对赛道的检测情况，来判断卡尔曼滤波算法是否可对摄像头智能车识别赛道起到优化作用。

1 智能车的结构及原理

摄像头智能车的结构主要包括车模、英飞凌TC264单片机、主板、驱动板、摄像头、显示屏、电池、编码器等[2]。

摄像头智能车以32位的英飞凌TC264单片机作为主控制器。通过制作主控板及驱动板，使摄像头智能车的各个模块之间相互配合，共同控制小车的运行。通过摄像头采集赛道信息，并对采集的图像进行矫正处理，以获取准确的道路信息[3]。随后微控制器处理赛道信息和车辆自身运行的各项参数，计算出输送给电机和舵机的占空比，完成对摄像头智能车的控制。

2 硬件系统设计

摄像头智能车硬件系统主要划分为6个模块：主控制器模块、电源供电模块、摄像头图像信息采集模块、图像显示模块、方向控制模块和电机驱动模块[4]，摄像头智能车的硬件系统整体设计框图如图1所示。

图1 摄像头智能车硬件整体设计框图

电机驱动模块是保证摄像头智能车按照赛道轨迹行驶的关键。对于电机来说，只要给一定的电压值，电机就可以转动。但是对于摄像头智能车来说，希望其电机转速可以按照期望值变化，同时还可以实现正反转，这时施加给电机的电压就不能是某一定值，因此，需要设计驱动电路，实现电机调速与正反转。

目前最常见的电机驱动电路之一就是H桥驱动电路，通过金属氧化物半导体管（Metal Oxide Semiconductor, MOS）的开合来控制电机正反转，还可以直接通过控制占空比的大小来改变电机两端电压，占空比越大，则电机两端获得的电压值越大，电机转速越高，从而使得摄像头智能车可以获得更快的行驶车速。

组成H桥驱动电路的方式有很多，可以使用分立元件组装成H桥驱动电路，也可以直接使用电机驱动芯片。为了保证工作稳定可靠，同时降低成本，采用BTN系列的电机驱动芯片。只有驱动芯片还不足以驱动电机，需要按照芯片的原理构建驱动电路，将驱动芯片分别与电阻、电容、电源以及电机相连。为了防止反向电流过大造成单片机的损坏，应加入隔离芯片。利用 BTN7971电机驱动芯片和74LVC245隔离芯片来构建H桥电机驱动电路，因为74LVC245芯片需要使用5 V供电，而电源电压为7.2 V，所以还需要使用稳压芯片将7.2 V降低至5 V来给隔离芯片供电，采用AMS1117稳压芯片进行降压。

3 软件系统设计

3.1 赛道识别

摄像头智能车主要依靠摄像头对赛道进行识别。首先由于光线的存在，摄像头前方的景物会生成光学图像，传感器将光信号转化为电信号，经过图像处理器（Image Signal Process, ISP）和数字信号处理芯片（Digital Signal Process, DSP）处理后[5]，就可以通过屏幕观察到拍摄到的图像。智能车的摄像头也是一样的道理。

智能车的摄像头通过场中断、行中断和直接存储器访问（Direct Memory Access, DMA）中断来采集图像。采集图像时开启场中段，当检测完一行之后，开启行中断，摄像头将采集到的数据存入数组中。采集完成后，行中断和场中段都会关闭，从而为下一次图像采集做好准备[6]。

3.2 图像二值化处理

图像二值化处理，简单来说就是选择一个合适的阈值，根据阈值将图像分割成黑或者白，反映图像的局部特征，从而实现目标物的识别[7]。

图2展示了五种二值化方法处理后的图像，其中原图代表初始拍摄图像。A图像的二值化方法为超过阈值部分取最大值255，否则取0；B图像的二值化方法为超过阈值部分取最小值0，否则取255；C图像的二值化方法为大于阈值部分设为阈值，小于阈值部分不变；D图像的二值化方法为大于阈值部分不变，小于阈值部分设为0；E图像的二值化方法为小于阈值部分不变，大于阈值部分设为0。

图2 图像二值化

从图中可以看出，通过二值化处理，图像非黑即白。传统的二值化算法先设定一个固定的灰度值，图像按此值进行二值化分割，大于该值的设置为255，小于该值的设置为0。但是由于赛道环境千变万化，干扰太多，传统分割方法已经无法实现对赛道的准确分割[8]，故采用大津法，也就是俗称的最大类间方差法对赛道进行二值化处理。

3.3 赛道边线获取

首先根据摄像头采集到的数据获取该帧图像的像素矩阵，通过软件二值化，将灰度图像像素矩阵转化为黑白图像像素矩阵，利用该矩阵寻找赛道的黑白黑跳变点。为了保证准确度，将有效行设置为18行至57行。图3为赛道边线获取的流程图。

图3 赛道边线获取流程图

第一次从第57行的第47列开始向两侧扫描，直到找到黑白跳变点，停止扫描，记录这两条边线，然后计算出赛道的中线。第二次以第57行的中线位置作为基准向两侧扫描。随着摄像头不断采集赛道图像信息，每帧图像不断更新，使得摄像头智能车不断计算前方赛道的中线。

3.4 弯道上赛道中线的计算

当摄像头智能车在弯道上行驶时，会出现丢线的现象，如图4所示。

图4 丢线现象

从图4可以看出，摄像头识别到的图像只有左侧边界，而无右侧边界，如果按照直道上的计算方法获取中线位置，那么通过计算得到的中线会出现错误，影响摄像头智能车的循迹功能，此时便需要进行补线操作。

假如左丢线，则令左边线为1。接下来再根据右边线的位置来判断右边线是否越界。如果没有越界，就根据该行宽度的一半来推断中线的位置。如果右边线越界，那么就以左边线作为中线的位置。图5为赛道中线获取流程图。

图5 赛道中线获取流程图

3.5 PID控制算法

比例-积分-微分（Proportion Integration Differentiation, PID）控制算法可分为两种类型：位置式PID控制算法和增量式PID控制算法。在摄像头智能车电机控制中，主要采用增量式 PID控制算法。位置式 PID控制算法容易产生累积误差，而增量式不会产生累积误差。

在调位置式速度闭环控制时先加大Kp值，同时令KI、KD为0，查看编码器值和目标值的差值，当反馈回来的编码器值在目标值上下震荡时加入KI值即可。增量式的KI就是位置式的Kp，因此，对于增量式PID控制先加入KI，再加入Kp。在实际调试过程中，当KI很小时，电机就能达到目标值。

在对舵机进行控制时，根据摄像头采集到的图像信息获得赛道实际中线位置，通过其与期望的中线位置做差便可得到偏差，然后再乘以比例系数Kp便得到此时舵机应该接收到的占空比，从而控制舵机转向。因为对于舵机的控制，不需要再去记录之前的偏差，因此，控制舵机时不再需要积分环节的嵌入。具体的Kp、KD参数调节，应该通过多次实际测试获得。总的来说应该先确定Kp值并使KD=0，KI=0，保证摄像头智能车可以正确循迹。然后逐渐增大Kp的值，以增加摄像头智能车的转向响应速度，但是如果Kp值太大的话会使小车在直线行驶过程中出现震荡现象，影响摄像头智能车的行驶稳定性，所以此时应通过增大KD的值来抑制震荡。当摄像头智能车不再震荡后，可以继续增加Kp值，直到不管怎样增大KD值，摄像头智能车仍会出现震荡，此时应该适当降低Kp值。经过多次测试，找到最合适的一组参数。

4 卡尔曼滤波算法

4.1 数据融合

假设现在测量某人身高，获得两组数据h1=180 cm和h2=185 cm，这两组数据均不准确，标准差分别为σ1=1 cm和σ2=2 cm，均符合正态分布。接下来要根据h1和h2来估计真实值。令ˆ=h1+K(h2-h1)，这就是卡尔曼滤波算法的关键，其中，K为Kalman Gain。所以应求得K使得标准差σhˆ最小，即方差Var(ˆ)最小。

对方差求导可得

推出

将σ1=1 cm，σ2=2 cm带入得

4.2 状态空间表达

图6为车身振动的单质量系统模型。该模型由车身质量m2、弹簧刚度ε、减震器阻尼系数C的悬架组成。q是输入的路面不平度函数[9]；z是车身垂向位移；是车身垂向速度；˙z˙是车身垂向加速度。根据牛顿第二定律，系统运动的微分方程为

图6 车身振动的单质量系统模型

进一步整理可得

令μ=+εq，即为系统输入，故有

再定义测量量Mea1=z1，且Mea2=z2。

式（9）和式（10）体现了变量随时间的连续变化，可以将其离散化为

式中，zk为k时刻的系统状态量；zk-1为k-1时刻的系统状态量；μk-1为k-1时刻的系统输入量；Meak为k时刻的系统测量量。

然而在实际生活中，存在着很多不确定性，这就导致所建立的状态空间方程会随着这些不确定性而发生变化。

那么状态空间方程变为

式中，wk为过程噪音，体现了模型的不准确性；vk为测量噪音，体现了测量的不准确性。

4.3 推导卡尔曼增益

首先由于在计算过程中忽略了过程噪音wk-1，此时zk并不准确，令先验估计值，为上一时刻的估计值。

由

推出

式中，为测量结果。

根据卡尔曼滤波的思想

式中，ˆ为后验估计值，G为权重系数。为了避免出现求逆运算，再令G=KH，K即为卡尔曼增益。

那么

此外，有P(wk)～(0,Q)，协方差矩阵且有P(vk)～(0,R)，协方差矩阵分别是过程噪音和测量噪音的协方差矩阵。为了获得最优解，要寻找K使得估计值最接近于实际值zk。

令误差 E rrork=zk-，则有P(Errork)～(0,Pk)，误差的协方差矩阵为Pk，从而推出

4.4 卡尔曼滤波算法应用

利用卡尔曼滤波的基本原理，对预测值和测量值进行加权处理，加权系数即为卡尔曼增益（Kalman Gain）。预测值即为赛道二十六行的中线位置，测量值为赛道第三十行的中线位置。为了使摄像头智能车可以做到提前转向，要对前方赛道进行预测，但是预测值会与实际值有偏差，所以不能完全相信预测值，应该将预测值与测量值进行数据融合，得到一个最优的结果，即最优的占空比，使得摄像头智能车可以提前转向，提高摄像头智能车的行驶稳定性。

以第三十行中线AverageCenter为基准，与理论中线进行分析比对，从而计算出此时应该传递给舵机的占空比。为了预测前方赛道，取第二十六行中线数据AverageCenter_Predict作为预测值。但是该预测数据是不准确的，故引入卡尔曼滤波算法，对预测值 AverageCenter_Predict与测量值AverageCenter进行加权处理，既不完全相信预测值，也不完全相信测量值。

对于卡尔曼滤波算法嵌入，首先应设定两个初值，令占空比分子=710，由于变量只有一个，所以误差、过程噪音、测量噪音的协方差矩阵便成为了一个数值，即为Po=10，Q=5，R=5。

嵌入过程总共分为五步：

1）可以由k=0时刻的最优占空比分子去预测k=1时刻系统的占空比分子先验估计，则有=+Bμ0。对于本例可以直接获取，假设= 7 20。

2）由k=0时刻的误差协方差P0和过程噪声的协方差Q预测k=1时刻的误差协方差的先验估计P1-，可得

3）计算卡尔曼增益：

4）进行校正更新：

这里可以假设测量值Mea1=715。求得

则该值便为k=1时刻的最优占空比分子。

5）接下来要对误差的协方差矩阵进行更新，P1= (I-KH)P1-=3.36，这样依次迭代，便完成了卡尔曼滤波算法的嵌入。

采用卡尔曼滤波算法的思想，对某一时刻占空比分子进行最优化求解。对于卡尔曼增益K的选择，即不可以过大，也不可以过小，即不过分听信预测值，也不过分听信测量值。K取得过大或过小，会影响摄像头智能车的行驶稳定性，故K的选择必须经过多次实际测试获得。首先赋值0.5，观察摄像头智能车行驶稳定性，然后逐渐增加，最后发现当卡尔曼增益取值为0.54时，摄像头智能车可以获得最佳的占空比，实现转向的最优控制。

5 数据分析

为了验证卡尔曼滤波算法在摄像头智能车上的实际应用效果，采用无线串口模块接收摄像头智能车返回的卡尔曼滤波算法控制下的占空比分子和非卡尔曼滤波算法控制下的占空比分子，以此分析卡尔曼滤波算法是否能对赛道起到预测作用，图7是两种算法返回的占空比分子。

图7 舵机的占空比分子

从图中可以看出，当使用卡尔曼滤波算法时，舵机的占空比分子的波动幅度要大于非卡尔曼滤波算法，也就是说舵机转向幅度会变大。

如图8所示，图中ServoPWM代表非卡尔曼滤波算法控制下舵机的占空比分子；ServoPWM_Finall代表卡尔曼滤波算法控制下舵机的占空比分子。在赛道的同一位置处，非卡尔曼滤波算法所计算出的占空比分子为710，卡尔曼滤波算法计算出的占空比分子为706。

图8 摄像头智能车的屏幕显示

图9为摄像头智能车在赛道上的位置。为了保证变量单一的原则，将摄像头智能车放在赛道上，向电机输入的占空比设置为0。此时摄像头智能车静止，通过卡尔曼滤波算法和非卡尔曼滤波算法同时计算出此时舵机的占空比，观察卡尔曼滤波算法控制下，舵机是否可以获得比非卡尔曼滤波算法控制下更优的占空比，从而实现提前转向的功能。

图9 摄像头智能车在赛道上的位置

结合图8与图9可以看出，当摄像头智能车前方不远处出现弯道时，非卡尔曼滤波算法控制下，摄像头智能车不能发现前方出现弯道，仍认为前方是直道，所以占空比分子的值为710，舵机保持中值。但是使用卡尔曼滤波算法之后，在同一位置处摄像头智能车已经能够识别出前方出现弯道，因此，会令占空比分子的值为706，提前转向，从而提高摄像头智能车的过弯稳定性。

6 结论

本文以全国大学生智能汽车竞赛为背景，通过车模的搭建、硬件电路设计，结合图像二值化处理、赛道左右边线识别，使得摄像头智能车能够按照赛道轨迹行驶，并加入 PID控制算法，保证摄像头智能车的行驶稳定性。经过多次调试，摄像头智能车已经可以实现循迹功能，速度可达1.4 m/s且过弯稳定。在此基础上，嵌入卡尔曼滤波算法，选择合适的卡尔曼增益系数，将预测值与测量值进行加权处理，使得摄像头智能车的舵机获得最优的占空比。将嵌入卡尔曼滤波算法的程序与未嵌入卡尔曼滤波算法的程序分别写入单片机，观察摄像头智能车在弯道处的行驶情况。试验结果表明，嵌入卡尔曼滤波算法之后，摄像头智能车可以更早地发现前方出现弯道，从而提前转向，避免摄像头智能车在弯道处因转向不及时而冲出赛道。因此，卡尔曼滤波算法对赛道识别可以起到优化作用，提高摄像头智能车的行驶稳定性。