针对小车状态感知的卡尔曼滤波多传感器融合算法

任其亮，程昊东

(重庆交通大学 交通运输学院， 重庆 400074)

0 引言

随着无人驾驶的蓬勃发展，定位技术一直是当前的研究热点。但是，现存的室内定位系统主要搭载的是大型家用车，移动小车的定位始终没有突破性的进展。当下移动小车定位研究最大瓶颈是：与大型小车相比，小车所处的环境更加复杂，无法精确标定小车位置[1]，并且小车定位精度与定位成本难以兼顾。因此，探索一种高精度、低成本的小车室外定位系统很有必要。

在室外定位系统中，标定自主移动小车相对于车道线的参考位置是实现小车在未知环境里自主导航、状态感知等功能的重要基础。针对标定过程普遍存在的定位精度差这一问题，大多学者提出了多个传感器相融合的方法来解决，成悦等[2]提出基于置信度的加权特征融合相关滤波跟踪方法，将互补的梯度和颜色特征进行融合；付廷强[3]提出了基于GNSS、惯性导航系统、视觉传感器相融合的定位方法；Zhang等[4]提出了IMU、视觉、激光3种传感器相融合的方法。上述方法有效提升了定位精度和稳定性，改善了单一传感器的测量缺陷，但是存在着安装繁琐、计算复杂、成本较高并且缺乏位姿预测等问题。基于以上分析，本文选取精确度较高的视觉传感器和雷达作为小车在实际坐标系下相对于车道线位置的测量工具，并对多种传感器获得的原始数据进行处理，尽可能消除传感器自身存在的漂移特性及其他因素对测量结果精度的影响。数据滤波方法有很多，其中应用最广的是卡尔曼滤波。卡尔曼滤波是一种利用线性系统的状态方程，通过系统的观测数据，对系统状态进行最优估计的算法，Zong等[5]提出了用2个扩展卡尔曼滤波同时处理的方法估计小车状态；李刚等[6]提出了一种基于改进的Sage-Husa自适应扩展卡尔曼滤波的车辆行驶状态估计算法。传统卡尔曼滤波在非线性的算法下无法达到精度要求，而扩展卡尔曼滤波实时性较强，能够对采集到的数据进行更新和处理，适合用于多传感器的融合定位[7]。在本文中，由于小车运动呈非线性状态，且短时间内很难发生状态突变，故采用改进扩展卡尔曼滤波在测量方差已知的情况下，从一系列夹杂噪声的数据中，估计动态系统的状态[8]。

1 相关模型的建立

1.1 小车运动模型

根据复杂程度可以将常用的运动模型分为一次运动模型(也别称为线性运动模型)和二次运动模型，其中二次运动模型可分为恒定转率和加速度模型(constant turn rate and acceleration，CTRA)和恒定转率和速度模型(constant turn rate and velocity，CTRV)。考虑到非全向四轮小车的运动特性以及传感器测量间隔，本文中默认小车的运动模型为恒定转率和速度模型(CTRV)[7]。当该小车运动时，其运动实时状态量为:

X(t)=(x,y,v,θ,ω)

(1)

式中：θ为偏航角，取值范围是[0,2π]；ω为偏航角速度。CTRV的状态转移函数为：

(2)

同时还存在一个问题，那就是ω=0的情况，此时的状态转移函数公式中的(x,y)将变成无穷大。因此需要将ω=0(即直线行驶)时的情况拆解开来单独分析：此时运动模型为恒定速度模型(constant velocity)，所以(x,y)的计算公式为：

x(t+Δt)=vcos(θ)Δt+x(t)

(3)

y(t+Δt)=vsin(θ)Δt+y(t)

(4)

1.2 扩展卡尔曼滤波模型

考虑到小车的运动是非线性的，需要引入扩展卡尔曼滤波来处理，则原卡尔曼滤波器预测的第一步变成了如下非线性函数：

xk+1=g(xk,u)

(5)

式中：g为状态转移函数；u为噪声。

由于该状态转移函数是非线性，因此需要对其进行线性化，也就是求解相应的雅可比矩阵[9]。在CTRV模型中，对各个元素求偏导数就可以得到相应状态雅可比矩阵。J1为ω≠0时的状态转移雅可比矩阵，J2为ω=0时的状态转移雅可比矩阵。

(6)

(7)

1.3 传感器测量模型

假设有雷达和视觉传感器，分别以一定的频率来测量[10]。

1) 视觉传感器：在里程计坐标系(ODOM)上测量目标车辆的坐标(x,y,v,θ,ω)。这里的x,y是相对于车辆坐标系的，即车辆为坐标系的原点，车头为X轴，车的左侧为Y轴，车头偏角为θ。其测量模型是线性的，对应测量矩阵为：

(8)

2) 雷达：通过雷达坐标系反馈出当前运动车辆和中心线的相对位置关系(d,φ)，反映的是车道线与车辆的距离和角度关系，转化为雷达坐标系下的标准形式，其测量矩阵为：

(9)

2 整体融合方法

由于2个传感器测量对象并非完全相同，因此最佳融合过程为交叉融合，在载体运动的短时间内，雷达测量的数据精度高，但由于白噪声以及长时间的工作会累计误差，精度降低；视觉传感器提供车辆实时的状态信息。因此采用信息融合的方法，将彼此的优缺点进行互补，计算出更为准确的小车与中心线的距离和角度[11]。整体实现方法如图1所示，对视觉传感器测得的小车实时运动数据进行误差补偿和滤波，并将其视为预测值，和雷达所得到的观测值进行融合，可以得到融合后的结果。

图1 传感器融合算法流程示意图

视觉传感器和雷达在测量数据是一个线性离散随机的过程，各状态参量之间的转换关系由图2所示。

图2 雷达与视觉传感器数据转换示意图

2.1 构建EKF方程

转换完成后，可以开始滤波，注意实验目的为得到小车与中心线的距离和角度，因此小车实时状态可表现为:

(9)

则姿态角相应状态方程和测量方程分别为：

X(k)=Ax(k-1)+BU(k)+ω(k)

(10)

Z(k)=HX(k)+v(k)

(11)

式中：视觉传感器的角度为预测角度，ω(k)为噪声误差；雷达的角度为实时测量角度，v(k)为噪声误差；A、B为系统矩阵；U(k)为控制矩阵；H为测量矩阵。

根据卡尔曼滤波系统，k-1时刻的状态用于预测k时刻的先验估计,构建滤波方程。

1) 状态先验方程。

X(k/(k-1))=AX((k-1)/(k-1))+BU(k)

(12)

2) 先验均方误差方程。

P(k/k)=AP(k/(k-1))AT+Q

(13)

式中：Q为视觉传感器噪声协方差。

3) 状态估计方程。

X(k/k)=X(k/(k-1))+

Kg(k)[Z(k)-HX(k/(k-1))]

(14)

式中：Kg(k)为k时刻的卡尔曼增益，其表达式为：

Kg(k)=P(k/(k-1))HT/[HP(k/(k-1))HT+R]

(15)

式中：R为雷达噪声协方差。

4) 估计均方误差方程。

P(k/k)=[I-Kg(k)H]P(k/(k-1))

(16)

式中，X(k/k)为当前时刻状态的滤波值，为得到最优滤波效果，协方差矩阵Q,R需要时刻更新。

2.2 Q,R的更新求解

在本模型中，状态的影响表达矩阵为：

(17)

式中：ua为直线加速度误差；uw为偏航角加速度误差。

Q是处理噪声的协方差矩阵，根据协方差矩阵定义可推导出其表达式为：

G*E[u*uT]*GT

(18)

视觉传感器，雷达的测量误差为独立的零均值白噪声序列，由上面状态影响表达矩阵可得，在某一时刻k，视觉传感器和雷达的状态估计误差可分别表示为:Δtkuo，Δtkul。

(19)

相应的协方差矩阵为：

cov(f,f)=E[(f-Ef)2]=

(20)

由于传感器具有单一独立性，其测量误差与上一时刻相比相互独立，因此应有：

则：

(21)

对于本小车而言，其搭载的雷达误差远小于视觉传感器，且前后相邻时刻传感器误差相差极小，因此则有如下合理假设:

(22)

(23)

将式(22)—(23)代入式(12)中，可得：

(24)

对于该随机变量，其2阶中心矩为其方差，因此，R为：

(25)

将滤波融合后的Q和R用于数据更新，即可得到最优的状态。

3 实验及其误差分析

3.1 实验小车模型介绍

本小车在车顶搭载了ZED双目摄像头，并在相邻位置安装了雷达和视觉传感器，图3为模型原理图，表1为视觉传感器参数图，表2为雷达传感器参数图。

表1 视觉传感器参数

表2 雷达传感器参数

图3 实验平台

图3(a)则为斜45°方向上小车在SOLIDWORKS中的实体模型。图3(b)说明了视觉传感器设置位置及其感知区域的侧视图和俯视图，其测量坐标系通过ROS系统转化为ODOM的中心坐标系。图3(c)说明了雷达设置位置及其俯视图下的测量原理，通过ODOM的雷达坐标系，直接测量小车与中心线的距离和角度。

3.2 实验及数据分析

让实体小车运动起来，并获取视觉传感器和雷达测量的相应数据，利用所获原始数据计算出小车的实时状态，并按照图1的流程图对多传感器数据进行扩展卡尔曼滤波融合，得到优化后的小车状态曲线。并将直接采集到的雷达数据与多传感器融合值进行比对，图4显示了对比结果。

图4 雷达与中心线状态滤波前后对比

由图4可知，经过滤波后的小车状态曲线更加平滑，数据更为精确，几乎不存在零点漂移。

此外，为了验证此方法的合理性，本车在lgsvl模拟器中进行了测试，建立的模拟地图如图5所示，其中白点为小车运动的起始点和终止点，红点为避障桩，小车会根据嵌入的Apollo避障算法进行避障。

在建立的地图上，使用卡尔曼滤波器处理后的结果如图6所示，可以看出使用卡尔曼滤波器处理后，雷达的观测噪声被消除了，卡尔曼滤波处理后的结果比雷达反馈的结果更加贴近真实值和运动学理论。

图6 lgsvl测试结果图

在相同实验数据下，将传统扩展卡尔曼滤波与所提改进方法进行比较，表3为2种方法各运行1 000次后与小车实验数据的最大误差、平均方差和均方误差。

表3 传统扩展卡尔曼滤波和改进扩展卡尔曼滤波对比

由表3可以看出，2种方法都能有效地提高位置的精度。但是传统扩展卡尔曼滤波波动较大，相较于改进扩展卡尔曼滤波，传统扩展卡尔曼滤波的最大误差增大了55%，平均误差增大了44%，均方误差增大了17%，且运行处理数据时间更长。改进扩展卡尔曼滤波由于具有交叉融合的特性，具有精度高、波动小、效率快的优点，在该小车融合定位中有更强的适用性。

4 结论

针对车周边环境信息时视觉传感器和雷达获取的数据漂移问题，采用的改进扩展卡尔曼滤波，对视觉传感器和雷达采集到的非全向小车关于道路中心线的状态数据进行交叉融合，得到小车的状态参数。根据小车的特点，将视觉传感器作为预测值，根据预测k时刻的先验状态得到该时刻的先验协方差矩阵，然后通过改进扩展卡尔曼滤波算法不断更新递归，最终估算出最佳的状态。实验结果表明该算法具有较强的实用性和鲁棒性。