基于激光雷达与毫米波雷达的矿区路侧感知算法

江良玉

（中车株洲电力机车研究所有限公司，湖南 株洲 412001）

目前矿区安装在矿用卡车（简称“矿卡”）上的传感器主要存在感知盲区较大和传感器成本相对较高2个问题。矿卡高度一般在6～7 m，相当于一座移动的二层楼房，盲区往往有十几米之长［1］，因此现有车载感知系统很难实现对周围环境全方位、超视距的感知。为了实现车辆对外界环境的高效感知，需要安装多种多个传感器；
这不仅增加了成本，也使系统过于复杂，耦合程度高，不利于后续信息的处理。对此，在实现矿卡智能驾驶的过程中，有必要结合路侧感知系统来弥补车载感知系统的不足。

路侧感知系统是车路协同应用开发的重要组成部分，其通过在路侧部署传感器，将采集到的路面交通状态、交通环境、交通事件等信息反馈至地面控制中心，使地面控制中心可以对路面情况进行实时监控，从而提高交通系统的全局感知能力［2］。

矿区路面环境与一般城市道路有着明显的不同，主要是大型重载车辆，较少的行人参与且道路岔口多、路面崎岖、短期内道路变化快［3］。目前海外矿区自动驾驶商用落地市场中，卡特彼勒公司和株式会社小松制作所处于行业领先地位。其主要合作的矿企为力拓集团（Rio Tinto）、必和必拓公司（BHP）、智利国家铜业公司（Codelco）、Fortescue Metals Group Ltd.等。国内主要涉及的企业有中车株洲电力机车研究所有限公司、北京踏歌智行科技有限公司、青岛慧拓智能机器有限公司（慧拓智能）、北京易控智驾科技有限公司、长沙智能驾驶研究院（希迪智驾）、河南跃薪时代新能源科技有限公司，合作矿企主要有鄂尔多斯煤炭集团、大唐国际宝利煤矿、神华能源股份有限公司、包钢（集团）有限公司等［4-5］。其中慧拓智能已在无人矿山系统中采用了路侧感知单元，将其安装在宝利煤矿的最大交叉口以实现矿卡感知辅助和智能化调度［6］。

目前业界常用的激光雷达或者毫米波雷达的目标跟踪算法主要有卡尔曼（Kalman）滤波及其衍生算法，如扩展Kalman滤波、无迹滤波等。Kalman滤波是在时域上运用状态空间，通过递推得到的一种滤波算法，其便于在计算机上实时实现，且计算量和存储量小［7］。文献［8］采用Kalman滤 波 以 及GNN（global nearest neighbor）算法，在路口用变换参考点的方法，可以连续跟踪到障碍物，不会出现因一个障碍物轨迹断掉而导致错误统计的情况。文献［9］采用联合概率数据关联（joint probability data association，JPDA）算法和无迹滤波，在估计速度时用图像识别的结果校正激光雷达聚类的障碍物结果，然后输出更准确的速度信息以及轨迹；
但其图像校正受制于图像分辨率和计算机图形处理器（graphics processing unit，GPU）性能。矿区的大型车辆会对摄像头视野产生大面积遮挡，矿区大量灰尘也会对视觉检测造成极大干扰［10］。采用路侧激光雷达可进行自动背景构建和目标检测，但是目前主要研究集中在城市道路。文献［11］使用激光雷达的半定规则（semidefinite programming，SDP）很好地说明了车辆与行人的碰撞场景，但是主要数据都是基于轻量型车辆研究得到的，无法很好地移植到矿区路侧感知系统中。

路侧感知系统的传感器部署主要由激光雷达和毫米波雷达构成。激光雷达是一种通过发射激光束探测目标的位置、速度等特征量的雷达系统。激光雷达因为激光波长短、准直性高，测距性能优异；
然而激光雷达受大气及气象影响大，大气衰减和恶劣天气可降低其作用距离和测量精度［12］。

毫米波雷达通过发射无线电信号并接收反射信号来测量自车与障碍物之间的距离和相对速度。基于多普勒效应，毫米波雷达可以直接测速且测速精度高，能全天候全天时工作，不受天气和夜晚光线等因素的影响，可在雨雪、烟雾、粉尘等恶劣条件下工作，且探测精度高、探测距离长；
然而其也有无法检测障碍物的深度信息和角度分辨率较低的弱点［13］。

由此可见，不同的传感器均存在不同程度的优缺点。通过多传感融合技术，可以充分互补各传感器的不足并突出利用其各自的优点，这样，融合系统所实现的功能要远超各独立系统能够实现的功能总和。此外，使用不同种类的传感器可以在某一种传感器出现故障的情况下，提供一定冗余度。

因此，为了更好地适应矿区环境，完成路侧感知系统的跟踪功能，本文提出了一种基于激光雷达和毫米波雷达的Kalman滤波和 交互式多模型（interacting multiple model，IMM）的跟踪算法。实测结果表明，该算法可以很好地融合激光雷达和毫米波雷达各自的优点，过滤灰尘以及洒水车喷水的干扰，使得融合后的跟踪结果优于任一单个传感器的跟踪结果。

本节主要介绍融合算法的框架与问题建模。首先需考虑如下的离散时间线性系统：

式中：zm(k)和vm(k)分别表示k时刻传感器m的量测和量测噪声，其中，zm(k)∈Rlzm，k∈N，lzm表示量测向量的维数；
x(k)和wu(k)分别表示k时刻局部系统u的状态量和过程噪声，其中，x(k)∈Rlxu，lxu表示状态向量的维数；
F表示系统传递矩阵，G表示系统过程噪声矩阵，H表示观测矩阵。

假设系统满足以下条件：

（1）所有系统矩阵已知。

（2）量测噪声、过程噪声和状态量是前二阶矩已知的联合高斯（Gauss）分布。

（3）量测噪声和过程噪声是方差分别为Ru(k)和Qu(k)的白噪声，且互不相关。量测噪声和观测噪声矩阵如式（3）和式（4）所示，其中E表示期望。

（4）初 始 状 态xu(0)为Gauss分 布，即x(0)～N((0)，P(0))，是初始量测的均值，P是初始量测的误差协方差矩阵，x(0)与过程噪声和量测噪声互不相关。

当目标实施机动（突然转弯或加减速）时，仅采用基本的Kalman滤波算法往往得不到理想的结果，此时需要采用自适应算法。为了获得更加准确的状态估计量，本系统采用了IMM算法［7］，目前在机动目标跟踪领域得到了广泛的应用。该算法的思想是基于贝叶斯理论提出的模型间的自动识别与切换：在任意跟踪时刻，通过设置对应目标可能模型数量的模型滤波器来进行实时的机动模型检测，对每一个滤波器设置权重系数和模型更新的概率，最后加权计算得出当前最优估计状态，从而达到模型自适应跟踪的目的，可以很好地克服单模型估计误差较大的问题［9］。本系统根据矿区路侧感知系统所在路口实际情况，选用匀加速模型（CA）、匀速转弯模型（CT）及静止模型（Static）这3种作为IMM的输入模型。

利用IMM算法获得各传感器跟踪目标的局部航迹后，需要采用一定的算法求出融合后的目标全局航迹，如图1所示。图中，x1、x2分别表示局部目标航迹，xg表示IMM算法融合后的全局目标航迹。考虑到路侧现场实际采用的传感器为毫米波雷达和激光雷达，为了利用各自传感器的优势，采用表1中的融合策略，其中航迹关联算法采用全局最近邻（GNN）。状态向量xu=表示物体在x和y两个方向上的位置、速度和加速度。

图1 IMM算法示例Fig.1 Example of IMM algorithm

表1 路侧感知系统融合策略Tab.1 Fusion strategy of roadside perception system

本路侧感知系统实际应用于神延西湾露天煤矿运煤主干道口，处理单元为NVIDIA JETSON AGX XAVIER，毫米波雷达为大陆408，激光雷达为LIVOX HORIZON64线。文章选取2个有代表性的目标航迹以及IMM算法，得到跟踪过程中3种模型的变化量。如图2（a）轨迹所示，物体分别进行了近似直线运动和转弯运动。图2（b）中3种运动模型的概率变化表明，CA模型大部分时间内处主导地位，CT模型在物体转弯时概率明显增大，Static模型概率基本在0附近。如图2（c）轨迹所示，物体基本保持静止。图2（d）模型概率表明，物体在运动过程中基本以Static模型为主。

图2 目标轨迹以及IMM算法中相应运动模型概率变化Fig.2 Object tracks and their probability changes of kinematic models in IMM algorithm

图3示出通过融合算法后的轨迹和速度曲线。由图3（a）可以看出，融合后的轨迹（黑色曲线）主要以激光雷达跟踪轨迹（红色曲线）为主，避免了毫米波雷达跟踪轨迹中较大的波动（蓝色曲线）。同理，图3（b）表明，融合后的速度（黑色曲线）主要来自毫米波雷达跟踪速度（蓝色曲线），避免了激光雷达跟踪速度中较大的波动（红色曲线）。

图3 相应传感器跟踪模型位置和速度变化Fig.3 Position and velocity changes by different track sensors

在实际应用中，现场洒水车作业时喷出的水花会被激光雷达识别为障碍物，用现有的融合策略容易产生误报，如图4所示。通过分析激光雷达和毫米波雷达的数据，可以发现，毫米波雷达不会将水花识别成为障碍物，如图5所示。图5中，黑色点为激光雷达点云，红色方块为毫米波雷达的点云，可以看出，洒水车喷洒的水花在图片的下方形成了点云簇。

图4 洒水车作业示意Fig.4 Demonstration of sprinkler working

图5 激光雷达识别的障碍物（黑色）与毫米波雷达识别的障碍物（红色）Fig.5 Objects detected by LiDAR(black)and by millimeterwave radar(red)

通过分析数据，我们更改表中的融合策略，增加状态量毫米波雷达散射截面强度（radar cross section，RCS），新的状态向 量，其中，γ表示RCS值。新的融合策略如表2所示。

表2 修改后的路侧感知系统融合策略Tab.2 Improved fusion strategy of roadside perception system

本文主要研究了矿区路侧感知技术，提出了一种基于激光雷达和毫米波雷达的Kalman和交互式多模型算法。路侧感知系统软件将激光雷达、毫米波雷达检测到的目标数据进行融合，生成融合后的全局目标跟踪信息。通过对神延西湾露天煤矿现场部署的路侧感知系统进行测试，结果表明，该算法可以很好地融合激光雷达和毫米波雷达各自的优点，尤其是对现场洒水车作业喷出的水花有很强的抗干扰性，不会将其误识别为障碍物，提升了融合准确度。后期将基于深度学习雷达与视觉的融合进行研究，以继续提升本算法的性能。