CN111273279A - 一种基于加速度噪声参数的多雷达数据处理方法 - Google Patents

Abstract

本发明涉及一种基于加速度噪声参数的多雷达数据处理方法，提出了基于加速度噪声的迭代扩展卡尔曼滤波(Extended Kalman Filter,EKF)和前置多雷达系统的实现方式。本发明将加速度变化率的方差定义为加速度噪声参数，用来表征物体受外界噪声影响的程度。在进行EKF滤波时，通过加速度噪声参数实时计算过程噪声协方差矩阵，并迭代估计目标的位置。硬件系统包括前置的两个毫米波雷达、一个四线激光雷达以及一台运行机器人操作系统Robot Operating System(ROS)的工控机。两侧的毫米波雷达安装有一定的偏航角，使测量区域存在交叉。上位机分别运行雷达的数据节点以及滤波节点。数据节点解析传感器的原始数据，滤波节点实现基于加速度噪声的迭代EKF方法，以此提高测量准确性，实现目标的跟踪。

Description

一种基于加速度噪声参数的多雷达数据处理方法

技术领域

本发明涉及无人驾驶车辆的数据处理领域，特别涉及一种基于加速度噪声参数的多雷达数据处理方法。

背景技术

无人驾驶车辆是一种可以连续自主行驶的新型智能车辆。它通过车载传感器系统感知环境信息，在此基础上结合具体任务实时规划路径，并最终到达指定区域。在这个过程中，需要车辆感知周围其他交通参与者的信息，其中对于动态障碍物的跟踪尤其重要。

应用于自动驾驶环境感知中的车载雷达主要有激光雷达和毫米波雷达，激光雷达容易受外界环境如雨、雪、灰尘等的干扰但其对目标的测量精度较高。毫米波雷达则与之相反，其不易受目标表面形状和颜色的影响，也不受天气光照等影响，环境适应能力强，但毫米波雷达其测量精度较低。现有的针对单一种类雷达的数据处理方法比较复杂，且对于提升测量整体效果并不明显。因此，相比于单一传感器系统，多个雷达组合而成的前置车载雷达系统，稳定性更强。

将不同传感器的测量结果进行匹配合并之后，需要采用滤波方法进行目标的跟踪与运动状态的估计，在众多相关的信息融合与目标跟踪算法中，EKF更适用于非线性系统，并且能够克服普通卡尔曼滤波只针对线性系统和无迹卡尔曼滤波的采样散布问题会严重降低其滤波性能的缺陷。传统的EKF方法虽然适用于非线性系统，但进行单次滤波并且采用固定的噪声协方差矩阵会导致其环境适应性较差，不能实时反映出外界干扰的变化，降低估计的精度。

发明内容

为了解决上述问题,本发明提供一种基于加速度噪声参数的多雷达数据处理方法。将基于加速度噪声的迭代EKF滤波算法应用到车辆的前置多雷达测量系统中，并在基于Linux的ROS系统中完成算法的实现以及功能模块的构建。

通过不同安装方式的前置雷达对目标进行交叉观测，将同一目标的不同雷达的观测结果进行合并，得到当前帧中的目标序列，将当前帧的目标序列与历史目标序列进行匹配，对成功匹配的目标进行基于加速度噪声的迭代EKF，估算出目标的运动状态。基于加速度噪声的迭代EKF方法将各种干扰的效果等效到加速度的噪声干扰中，通过定义加速度噪声参数，计算实时的过程噪声协方差矩阵。另外，该方法还将第一次EKF的结果作为一次虚拟输入，进行第二次的滤波操作，可以使得探测结果更加稳定，使系统整体的实用性更强。

本发明是通过以下技术方案实现的：一种基于加速度噪声参数的多雷达数据处理方法，包括如下步骤：

采用基于加速度噪声的迭代EKF实现滤波跟踪，所述前置多雷达测量系统包括由两个毫米波雷达传感器、一个四线激光雷达传感器、运行ROS的车载工控机，用于实现车辆前方目标的跟踪，所述方法包括如下步骤：

步骤1：初始化雷达传感器，获取多个雷达传感器数据并判断各个雷达传感器输出是否正常，如果存在输出异常，则终止程序；将不同雷达传感器的检测结果进行合并，并将当前的目标与历史目标进行关联匹配；

步骤2：将各种干扰的效果等效到加速度的噪声干扰中，计算待跟踪目标的实时加速度噪声参数，通过加速度噪声参数，更新当前的过程噪声协方差矩阵Q_t；

步骤3：进行基于加速度噪声的迭代EKF，将由测量数据产生的第一次EKF结果重新作为一次虚拟的测量输入，进行第二次EKF滤波，得到跟踪目标的估计，并以ROS消息形式，发布当前的跟踪结果。

进一步的，所述步骤2中，两个车体坐标轴方向，即车体中轴线方向与其垂线方向上的加速度噪声参数n_x、n_y的计算方法为：

其中N为周期长度，

为N个周期中x方向加速度分量的一阶导数，

为N个周期中y方向加速度分量的一阶导数，T＝{t,t-1,t-2,…t-N+1},E()为计算数列平均值的函数，t:表示当前时刻。

进一步的，所述步骤2中，通过加速度噪声参数更新过程噪声协方差矩阵Q_t，计算公式如下：

其中dt、dt_2、dt_3以及dt_4分别表示传感器两帧之间的时间间隔及其2次、3次、4次方。

进一步的，所述的前置多雷达系统中两个毫米波雷达安装在车辆前部左右两侧，毫米波雷达与激光雷达的水平高度一致，每个雷达均有5度的偏航角，使两个雷达的测量区域存在交叉，四线激光雷达安装在车辆前部中央。

进一步的，通过不同安装方式的前置雷达对目标进行交叉观测，首先将同一目标的不同雷达的观测结果通过位置信息进行合并，得到当前帧中的目标序列；将历史帧中的目标序列按照估算的运动状态，计算其在当前时刻的理论位置，将当前的目标与历史数据中的目标通过最邻近原则进行关联，即经过运动补偿后的当前目标位置与历史目标集中最近的一个目标进行匹配关联。

进一步的，关联完成后将当前的目标信息添加到相应目标的跟踪序列中，之后应用基于加速度噪声的迭代EKF提高目标的跟踪精度；对于没有关联成功的当前帧中的目标视为第一次出现的目标，予以保留；对于当前帧中无对应观测结果的历史目标视为运动出了观测范围，不再进行跟踪。

进一步的，使用基于加速度噪声的迭代的EKF处理数据后得到目标状态的估计，包括目标的位置与速度信息，定义ROS中的消息格式：

Target＝{x,y,vx,vy}

分别表示直角坐标系坐标以及两个方向的速度分量，对于当前帧，将其中包括的所有目标的状态信息通过Target消息组的形式以10Hz的固定频率发布出来；其中x，y为目标的坐标位置，vx，vy为目标的在x、y方向上的速度。

与现有技术相比，本发明的技术方案所带来的有益效果是：

本发明提供的一种基于加速度噪声参数的多雷达数据处理方法，将基于加速度噪声的迭代EKF方法应用在了无人驾驶汽车的车载多雷达系统中。通过毫米波与四线激光雷达的结合可以适应不同的场景，且整体成本较低，将毫米波倾斜5°安装，保证了车辆前方重点区域内的目标可以获得较多的观测结果。另外，通过实时计算加速度噪声参数来更新过程噪声协方差矩阵，并将滤波结果进行一次迭代，使得本方法相较于其他滤波算法有较高的准确度，且相对于其他雷达系统，本发明中提出的应用以上改进EKF方法的前置多雷达系统的稳定性更强，适用范围也更广泛。

附图说明

此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定。

图1为应用本发明的雷达安装布局图；

图2为本发明的基于加速度噪声参数的多雷达数据处理方法流程图；

图3为本发明的实例结果图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域的普通技术人员在不付出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。

如图1所示为本发明的传感器布局图，包括两个前置的毫米波雷达与一个四线激光雷达组成的车载雷达系统。工控机上运行的机器人操作系统ROS，在ROS的节点集合中存在雷达传感器节点和滤波估计器节点，从而完成对雷达系统所接收的数据的融合，实现目标跟踪。

如图2所示为本发明的一种基于加速度噪声参数的多雷达数据处理方法方法流程图，其具体包括以下步骤：

(1)系统初始化

初始化系统，获取各个传感器的数据，并检测传感器状态，如果传感器出现异常，系统需要进行异常处理，结束任务；否则，系统读取传感器数据进行后续处理。

(2)初始化EKF相关参数

初始化状态协方差矩阵P_t与测量噪声协方差矩阵R_t。由于四线激光雷达与毫米波雷达输出数据都是极坐标格式(r,θ,v)，其中r为雷达返回目标的距离，θ为目标与雷达坐标系x轴正方向的夹角,v为估算的速度。因此，各个目标的初始状态量可以由一个传感器的输出数据进行初始化Xt＝[r*cos(θ),r*sin(θ),v*cos(θ),v*sin(θ)]^T即目标的位置x,y以及速度信息vx,vy。

(3)目标匹配关联

由于三个雷达的检测区域存在重叠，首先进行三个雷达检测目标的合并，之后与历史帧中的目标进行匹配。匹配方法采用最邻近距离的方法，即经过运动补偿后的当前目标位置与历史目标集中最近的一个目标进行匹配关联。

(4)对于关联成功的目标采用基于加速度噪声的迭代EKF方法进行滤波处理

卡尔曼滤波的相关公式有：

1)基础公式：

x_t＝A_tx_t-1+B_tu_t+w_t；z_t＝H_tx_t+v_t

2)预测公式：

x_t|t-1＝A_tx_t-1|t-1+B_tu_t；

3)更新公式：

x_t|t＝x_t|t-1+K_t(z_t-H_t·x_t|t-1)；P_t|t＝(I-K_tH_t)P_t|t-1

其中A_t：状态转移矩阵，B_t：输入控制矩阵，H_t：测量转换矩阵；w_t：过程激励噪声，w_t～N(0,Q_t)；v_t：测量噪声，v_t～N(0,R_t)；x_t：状态量；z_t：观测量；u_t：控制输入；I：单位矩阵；K：卡尔曼增益；t:表示当前时刻，t|t-1下标表示从t-1时刻到t时刻的中间参数，t|t下标表示t时刻的最终结果。

进行迭代处理时，将x_t|t视为新的z_t，按照预测公式计算新的x^new _t|t-1与k_t ^new,按照更新公式计算迭代后的输出：

具体的，包括以下步骤：

4.1.赋值观测量

Z_t＝[r*cos(θ),r*sin(θ),v*cos(θ),v*sin(θ)]

4.2.计算状态转移矩阵A_t与过程噪声协方差矩阵Q_t

定义dt、dt_2、dt_3以及dt_4分别表示传感器两帧之间的时间间隔及其2次、3次与4次方。

由物体运动学方程可得状态转移矩阵：

由于关联方程中的加速度量是未知的，因此将其影响添加到噪声中来，定义加速度噪声参数n_x、n_y，考虑加速度噪声的过程噪声协方差矩阵Q_t为：

4.3.由t-1时刻预测t时刻的状态矩阵与状态协方差矩阵：

X_t|t-1＝A_t*X_t-1|t-1；

P_t|t-1＝A_t*P_t-1|t-1*A_t ^T+Q_t；

4.4.在EKF算法中状态转移矩阵和量测矩阵是由二者的的雅克比矩阵代替的。因此，计算测量转换矩阵的雅克比矩阵：

其中定义px，py，vx，vy分别表示目标在直角坐标系下的坐标以及两个方向的速度分量，

参数c1＝px*px+py*py；

c3＝c1*c2。

4.5.按照前面所述的EKF滤波公式以及加速度噪声参数的公式更新状态：

S＝H_t*P_t|t-1*H_t ^T+R_t

K＝PH_t*S^-1

X_t|t＝X_t|t-1+K_t(Z_t-H_t*X_t|t-1)

P_t＝(I-K_t*H_t)*P_t|t-1

4.6.迭代计算

将由测量数据产生的估算结果重新作为一次测量输入估算目标的位置。如图3为本发明的实例模拟计算结果图。

(5)发布跟踪结果

使用基于加速度噪声的迭代的EKF处理数据后得到目标状态的估计，包括目标的位置与速度信息。定义ROS中的消息格式：

Target＝{x,y,vx,vy}

分别表示直角坐标系坐标以及两个方向的速度分量。对于当前帧，将其中包括的所有目标的状态信息通过Target消息组的形式以10Hz的固定频率发布出来。

本发明未详细阐述部分属于本领域技术人员的公知技术。

以上内容是结合具体的实施方式对本发明进行的详细说明，但并不能认定本发明的具体实施只限于这些内容。在不脱离本发明的原理和精神的前提下，本领域技术人员可以对这些实施进行若干调整、修改，本发明的保护范围有所附权利要求及其等同内容限定。

Claims (7)

1.一种基于加速度噪声参数的多雷达数据处理方法，其特征在于，包括如下步骤：

采用基于加速度噪声的迭代EKF实现滤波跟踪，所述前置多雷达测量系统包括由两个毫米波雷达传感器、一个四线激光雷达传感器、运行ROS的车载工控机，用于实现车辆前方目标的跟踪，所述方法包括如下步骤：

步骤1：初始化雷达传感器，获取多个雷达传感器数据并判断各个雷达传感器输出是否正常，如果存在输出异常，则终止程序；将不同雷达传感器的检测结果进行合并，并将当前的目标与历史目标进行关联匹配；

步骤2：将各种干扰的效果等效到加速度的噪声干扰中，计算待跟踪目标的实时加速度噪声参数，通过加速度噪声参数，更新当前的过程噪声协方差矩阵Q_t；

步骤3：进行基于加速度噪声的迭代EKF，将由测量数据产生的第一次EKF结果重新作为一次虚拟的测量输入，进行第二次EKF滤波，得到跟踪目标的估计，并以ROS消息形式，发布当前的跟踪结果。

2.根据权利要求1所述的基于加速度噪声参数的多雷达数据处理方法，其特征在于：所述步骤2中，两个车体坐标轴方向，即车体中轴线方向与其垂线方向上的加速度噪声参数n_x、n_y的计算方法为：

其中N为周期长度，

为N个周期中x方向加速度分量的一阶导数，

为N个周期中y方向加速度分量的一阶导数，T＝{t,t-1,t-2,…t-N+1},E()为计算数列平均值的函数，t:表示当前时刻。

3.根据权利要求2所述的基于加速度噪声参数的多雷达数据处理方法，其特征在于：所述步骤2中，通过加速度噪声参数更新过程噪声协方差矩阵Q_t，计算公式如下：

其中dt、dt_2、dt_3以及dt_4分别表示传感器两帧之间的时间间隔及其2次、3次、4次方。

4.根据权利要求1所述的基于加速度噪声参数的多雷达数据处理方法，其特征在于：

所述的前置多雷达系统中两个毫米波雷达安装在车辆前部左右两侧，毫米波雷达与激光雷达的水平高度一致，每个雷达均有5度的偏航角，使两个雷达的测量区域存在交叉，四线激光雷达安装在车辆前部中央。

5.根据权利要求4所述的基于加速度噪声参数的多雷达数据处理方法，其特征在于：

通过不同安装方式的前置雷达对目标进行交叉观测，首先将同一目标的不同雷达的观测结果通过位置信息进行合并，得到当前帧中的目标序列；将历史帧中的目标序列按照估算的运动状态，计算其在当前时刻的理论位置，将当前的目标与历史数据中的目标通过最邻近原则进行关联，即经过运动补偿后的当前目标位置与历史目标集中最近的一个目标进行匹配关联。

6.根据权利要求4所述的基于加速度噪声参数的多雷达数据处理方法，其特征在于：

关联完成后将当前的目标信息添加到相应目标的跟踪序列中，之后应用基于加速度噪声的迭代EKF提高目标的跟踪精度；对于没有关联成功的当前帧中的目标视为第一次出现的目标，予以保留；对于当前帧中无对应观测结果的历史目标视为运动出了观测范围，不再进行跟踪。

7.根据权利要求1所述的基于加速度噪声参数的多雷达数据处理方法，其特征在于：

使用基于加速度噪声的迭代的EKF处理数据后得到目标状态的估计，包括目标的位置与速度信息，定义ROS中的消息格式：

Target＝{x,y,vx,vy}

分别表示直角坐标系坐标以及两个方向的速度分量，对于当前帧，将其中包括的所有目标的状态信息通过Target消息组的形式以10Hz的固定频率发布出来；其中x，y为目标的坐标位置，vx，vy为目标的在x、y方向上的速度。

Images