CN112287557B - 辅助驾驶仿真测试的雷达点云数据在环回放方法及系统 - Google Patents

Abstract

本发明提供了辅助驾驶仿真测试的雷达点云数据在环回放方法及系统，根据虚拟车辆模型和数据采集车辆之间的运动关系，不停修正道路上采集到的雷达点云数据，实现了将道路中采集的传感器数据闭环回放给控制器和虚拟车辆模型组成的仿真系统的功能。本发明对实车道路采集到的雷达点云数据只进行点云位置的修正，保留了实采数据中的噪声和会影响到感知信息处理单元误识别的相关信息，可对控制器性能进行接近于真实情况的测试。

Description

辅助驾驶仿真测试的雷达点云数据在环回放方法及系统

技术领域

本发明属于辅助驾驶技术领域，具体涉及辅助驾驶仿真测试的雷达点云数据在环回放方法及系统。

背景技术

无人驾驶车辆是一个集环境感知、规划决策、多等级辅助驾驶等功能于一体的综合系统，可以根据驾驶员需求进行自动驾驶。

点云是指通过测量仪器得到的产品外观表面的点数据集合。通过不同类型的传感器获得的点云数据包含的信息丰富度不同，通过雷达获得的点云数据主要包括目标物的方位、速度和轮廓大小等深度信息。

回放是从某种已存在的、记录的来源进行播放，也就是我们通常所说的“播放”。此处的“回放”具有一定的场景特殊性，加以说明：车辆在现实环境中采集了激光雷达的数据，通过特定的格式接口注入控制单元，控制单元对点云数据进行分析进行目标识别进而根据目标进行车辆动力学控制。

MIL(model in the loop，模型在环)测试的测试对象是虚拟的软件模型，是用模型驱动进行嵌入式系统的开发时，在开发阶段初期及建模阶段中进行的仿真方式，通过给模型注入特定格式的信息观测算法模型的输出是否满足期望要求。

HIL(hardware in the loop，控制器在环)测试的测试对象是真实的控制单元，通过给控制器单元注入特定格式的信息观测控制单元的输出是否满足期望要求。

感知信息处理单元用于处理感知元器件(摄像头、雷达等)所提供的基础信息，并向车辆动作控制单元输出目标级信息。

车辆运动控制单元包括运动规划和车辆运动控制模块，负责场景下的路径规划和路径跟随。

场景-控制器-虚拟车辆-场景的闭环测试系统的控制单元在某特定场景下通过感知单元接收并处理场景数据，感知单元处理后的目标级信息输出给控制单元，控制单元对虚拟车辆进行运动学控制并更新试验车辆在场景中的位置信息，形成场景-控制器-虚拟车辆-场景的闭环测试系统。如果虚拟车辆的运动信息无法反馈至场景中，则为场景-控制器-虚拟车辆的开环测试系统。上述描述中，“场景”可细分为通过感知传感器(虚拟或者真实)采集到的道路环境中的静态(道路结构)以及动态(交通车运动)的原始信号。

在无人驾驶车辆功能开发过程中，MIL测试和HIL测试都是十分重要的环节，在不同的开发阶段通过上述两种测试方法可以较早地定位控制软件的Bug并优化软件不足之处。雷达作为无人驾驶车辆的重要感知部件，主要负责给车辆运动控制单元提供目标位置以及速度信息等。以HIL测试为例，为了给控制单元注入其所需的感知层信息，现有3种实现途径。一种方式是由场景构建软件给控制器提供目标物的虚拟真值信息，即仿真软件通过计算某时刻虚拟场景下试验车辆和目标物间的相对位置关系提供出的一种满足控制器需求的真值信息，不包含实际情况中的噪声和误处理误识别等数据信息；另一种方式是通过雷达回波模拟器的方式给控制单元提供目标级的模拟信号，同样无法模拟现实情况中的噪声和误识别等情况；第三种是车辆在实际道路中采集真实环境下的目标信息，并以特定信息格式回放给车辆运动控制单元。

图1给出了一种通过场景软件提供目标物真值信息给感知信息处理单元的一种方法。感知信息处理单元接收目标物的真值信息，经处理后将相关信息传输给车辆运动控制单元，继而控制虚拟车辆在场景中行驶。图1所示通过给感知信息处理单元注入目标物真值信息和模拟信息是比较常用的仿真手段，发展也比较成熟。但由于其本身所提供信息的局限性导致输入给感知信息处理单元的数据同无人驾驶车辆在道路运行过程中所处理的数据差别较大，差别主要体现在真实数据包括数据噪声、对目标的误识别和数据跳变等信息。故通过注入真值或者模拟值的仿真方式无法对感知信息处理单元的性能进行测试，缺陷较大。

另有现有技术手段可以通过离线回放的方式将某一时间段道路上采集到的雷达点云数据输入给感知信息处理单元，处理单元可在此输入下产生特定的目标级数据的输出，具体工作形式如图2中箭头传递路径。由于车辆动作控制单元的输出(车速、加速度和方向盘转角等)同实际道路采集过程中的驾驶员行为不一致，导致感知信息处理单元输出的目标级信息无法持续传递给车辆运动控制单元，故无法在回放雷达点云数据过程中同虚拟的车辆模型形成闭环仿真。举例说明无法实现闭环的原因：

表1中给出了一组数据，此数据的描述如下：某直线道路环境中，前方有一静止车辆，此时驾驶员踩踏制动踏板，两者的相对距离由50m逐步变为20m，在雷达传感器性能良好的情况下，会将相关的点云数据记录至存储单元。假设上述场景对车辆测试意义重大，想要在实验室环境将T0时刻上述场景中的点云数据回放给控制器(感知信息处理单元+车辆动作执行单元)时，控制器控制虚拟车辆可能进行了更为激进的制动动作。考虑到虚拟车辆同数据采集车辆的行为不一致，故无法再将T0时刻后的雷达数据继续回放给控制器以控制虚拟车辆。

在感知信息处理和车辆运动控制单元同时存在的控制器测试需求下，通过传统的雷达点云数据回放的形式无法实现虚拟的车辆模型对点云数据的反馈，也就不能实现实测点云数据在控制器(感知信息处理+车辆运动控制模块)测试中的在环回放。

发明内容

本发明要解决的技术问题是：提供辅助驾驶仿真测试的雷达点云数据在环回放方法及系统，用于将道路中采集的传感器数据闭环回放给控制器和虚拟车辆模型组成的仿真系统。

本发明为解决上述技术问题所采取的技术方案为：辅助驾驶仿真测试的雷达点云数据在环回放方法，包括以下步骤：

S1：搭建辅助驾驶仿真测试的雷达点云数据在环回放系统，包括传感单元、雷达点云处理单元、控制单元；传感单元设置在数据采集车辆上，传感单元的信号输出端连接雷达点云处理单元的信号输入端，雷达点云处理单元的信号输出端连接控制单元的信号输入端；

S2：传感单元按照特定的采样频率在时间轴上划分为T₀时刻、T₁时刻、T₂时刻、…、T_n时刻采集交通环境中物体的点云数据并将数据发送给雷达点云处理单元；

S3：雷达点云处理单元对T₀时刻的点云数据进行聚类和特征提取操作，提取和追踪交通目标，并将目标级数据发送给控制单元，控制单元规划路径并控制虚拟车辆的动作；设i＝1；

S4：雷达点云处理单元计算T_i-1时刻至T_i时刻时间段内虚拟车辆与实际车辆的运动误差；

S5：雷达点云处理单元计算T₀时刻至T_i时刻时间段内虚拟车辆与实际车辆的累积运动误差，修正T_i时刻的点云数据并根据修正后的点云数据进行目标物的聚类和追踪，将目标级数据发送给控制单元，控制单元规划路径并控制虚拟车辆的动作；i的取值加1，从步骤S5开始循环直至i＝n。

按上述方案，所述的步骤S2中，标志物信息包括标志物与试验车辆的距离、目标物与试验车辆的偏转角。

按上述方案，所述的步骤S3中，虚拟车辆的动作包括加速、减速、匀速和横向动作。

按上述方案，所述的步骤S4中，运动误差包括纵向误差和横向误差。

进一步的，所述的步骤S4中，具体步骤为：

设T_i-1时刻的虚拟车辆的纵向速度为

T_i-1时刻的虚拟车辆纵的向加速度为

T_i-1时刻的实际车辆的纵向运行速度为

T_i时刻的实际车辆的纵向运行速度为

车辆运动控制单元的计算周期为t；

则T_i-1时刻至T_i时刻虚拟车辆与实际车辆的纵向运动误差为：

进一步的，所述的步骤S4中，具体步骤为：

设T_i-1时刻的车辆横向速度为

T_i-1时刻的车辆横向加速度为

T_i-1时刻的数据采集车辆的横向运行速度为

T_i时刻的数据采集车辆的横向运行速度为

则T_i-1时刻至T_i时刻虚拟车辆与实际车辆的横向运动误差为：

进一步的，所述的步骤S5中，具体步骤为：

当t＝n时，虚拟车辆和数据采集车辆之间的累积纵向误差为：

虚拟车辆和数据采集车辆之间的累积横向误差为：

设未修正的Tn时刻的点云的三维坐标为(X_{n_ori}，Y_{n_ori}，Z_{n_ori})^T，则修正后的Tn时刻的点云的三维坐标(X_{n_revise}，Y_{n_revise}，Z_{n_revise})^T为：

辅助驾驶仿真测试的雷达点云数据在环回放系统，包括传感单元、雷达点云处理单元、规划单元；传感单元设置在实际车辆上，传感单元的信号输出端连接雷达点云处理单元的信号输入端，雷达点云处理单元的信号输出端连接规划单元的信号输入端；传感单元用于在实际车辆行进过程中采集实际车辆点云数据和场景中的标志物信息；雷达点云处理单元用于根据实际车辆点云数据修正虚拟车辆点云数据和虚拟车辆运动数据；规划单元用于根据虚拟车辆点云数据和场景信息规划虚拟车辆的动作和路径。

进一步的，虚拟车辆点云数据包括点云中每个点的三维坐标(X，Y，Z)，设雷达点云数据模块的核心部件的位置为三维坐标的原点，X轴、Y轴、Z轴的方向与车辆的运动方向一致，X轴指向车辆的前进方向，Z轴指向垂直向上，X、Y、Z三者的方向满足右手坐标系定则。

进一步的，虚拟车辆运动数据包括实时位置、车辆纵向速度、车辆纵向加速度、车辆方向盘转角、车辆横向速度和横向加速度。

本发明的有益效果为：

1.本发明的辅助驾驶仿真测试的雷达点云数据在环回放方法及系统，根据虚拟车辆模型和数据采集车辆之间的运动关系，不停修正道路上采集到的雷达点云数据，实现了将道路中采集的传感器数据闭环回放给控制器和虚拟车辆模型组成的仿真系统的功能。

2.本发明对实车道路采集到的雷达点云数据只进行点云位置的修正，保留了实采数据中的噪声和会影响到感知信息处理单元误识别的相关信息，可对控制器性能进行接近于真实情况的测试。

附图说明

图1是现有技术一的功能框图。

图2是现有技术二的功能框图。

图3是本发明实施例的功能框图。

图4是本发明实施例的数据流图。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步详细的说明。

参见图1，本发明的实施例的辅助驾驶仿真测试的雷达点云数据在环回放系统，包括数据采集车辆模块、感知信息处理单元、车辆动作规划单元、车辆运动控制单元；

数据采集车辆提供两方面信息，一是本身的车辆运动学和动力学信息，二是通过雷达点云数据呈现出的交通目标的位置和运动学信息；本身车辆的运动学和动力学信息包括车辆在大地坐标系下的实时位置(GPS)、车辆纵向速度、车辆纵向加速度、车辆方向盘转角、车辆横向速度和横向加速度等。

目标的点云数据包括采集到的每个反射点的三维坐标(X，Y，Z)，且若雷达点云的原始数据为极坐标表示，则需转换为三维坐标；其中雷达核心部件的位置为三维坐标的原点，X、Y、Z的方向与车辆的运动方向一致，X指向车辆的前进方向，Z向垂直向上，X、Y、Z三者的方向满足右手坐标系定则。

按照雷达的采样频率将点云数据在时间轴上划分为T0、T1、T2.......Tn，其中T0时刻数据直接回放给控制单元，此时感知信息处理单元按照特定的算法规则处理点云基础信息并形成目标级信息(目标的同试验车辆距离、偏转角等)并输至车辆动作规划单元。车辆动作规划单元中在目标信息的基础上规划出车辆动作(减速、减速、匀速以及横向动作等)。

此处具体描述如何确定T0至T1时间段内虚拟车辆和数据采集车辆的运动误差：运动误差包括2部分，即纵向误差E_T1_vehicle_long和横向误差E_T1_vehicle_lateral。纵向误差的计算公式的输入为T0时刻规划出的车辆纵向速度

控制器规划的T0时刻车辆加速度

和T0时刻数据采集车辆的运行速度

T1时刻的数据采集车辆运行速度

以及车辆运动控制单元的计算周期t。纵向误差的计算公式的输出为T0至T1时刻虚拟车辆和数据采集车辆的运动误差。

公式(1)和(2)描述了T0至T1时间段内，控制器控制的虚拟车辆模型和数据采集车辆之间的运动误差的计算方法。T_n-1至T_n时间段内的纵向和横向误差计算方式类似，不再赘述。

此处介绍如何通过T0至T1时间段虚拟车辆和数据采集车辆的运动误差E_T1_vehicle_long和E_T1_vehicle_lateral修正T1时刻的雷达点云数据。修正过程中的输入为运动误差E_T1_vehicle_long和E_T1_vehicle_lateral以及未修正的T1时刻雷达点云数据(X_{1_ori}，Y_{1_ori}，z_{1_ori}，)^T，输出为修正后的T1时刻雷达点云数据(X_{1_revise}，Y_{1_revise}，Z_{1_revise}，)^T，修正过程为：

T2时刻的雷达点云数据需要根据T0至T2时间段内的误差进行修正，以此类推当t＝n时，

上述给出了t＝0时刻至t＝n-1时刻，通过累积虚拟车辆和数据采集车辆之间的位移误差，对T1，T2，T3，…，Tn时刻的雷达点云数据进行修正以实现虚拟车辆同实车采集的传感器数据形成闭环关系的目的。

以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。

Claims (10)

1.辅助驾驶仿真测试的雷达点云数据在环回放方法，其特征在于：包括以下步骤：

S1：搭建辅助驾驶仿真测试的雷达点云数据在环回放系统，包括传感单元、雷达点云处理单元、控制单元；传感单元设置在数据采集车辆上，传感单元的信号输出端连接雷达点云处理单元的信号输入端，雷达点云处理单元的信号输出端连接控制单元的信号输入端；

S2：传感单元按照特定的采样频率在T_i时刻采集交通环境中物体的点云数据并将数据发送给雷达点云处理单元，i＝1，2，…，n；

S3：雷达点云处理单元对T₀时刻的点云数据进行聚类和特征提取操作，提取和追踪交通目标，并将目标级数据发送给控制单元，控制单元规划路径并控制虚拟车辆的动作；设i＝1；

S4：雷达点云处理单元计算T_i-1时刻至T_i时刻时间段内虚拟车辆与数据采集车辆的运动误差；

S5：雷达点云处理单元计算T₀时刻至T_i时刻时间段内虚拟车辆与数据采集车辆的累积运动误差，修正T_i时刻的点云数据并根据修正后的点云数据进行目标物的聚类和追踪，将目标级数据发送给控制单元，控制单元规划路径并控制虚拟车辆的动作；i的取值加1，从步骤S4开始循环直至i＝n。

2.根据权利要求1所述的辅助驾驶仿真测试的雷达点云数据在环回放方法，其特征在于：所述的步骤S2中，标志物信息包括标志物与试验车辆的距离、目标物与试验车辆的偏转角。

3.根据权利要求1所述的辅助驾驶仿真测试的雷达点云数据在环回放方法，其特征在于：所述的步骤S3中，虚拟车辆的动作包括加速、减速、匀速和横向动作。

4.根据权利要求1所述的辅助驾驶仿真测试的雷达点云数据在环回放方法，其特征在于：所述的步骤S4中，运动误差包括纵向误差和横向误差。

5.根据权利要求4所述的辅助驾驶仿真测试的雷达点云数据在环回放方法，其特征在于：所述的步骤S4中，具体步骤为：

设T_i-1时刻的虚拟车辆的纵向速度为

T_i-1时刻的虚拟车辆纵的向加速度为

T_i-1时刻的数据采集车辆的纵向运行速度为

T_i时刻的数据采集车辆的纵向运行速度为

车辆运动控制单元的计算周期为t；

则T_i-1时刻至T_i时刻虚拟车辆与数据采集车辆的纵向运动误差为：

6.根据权利要求5所述的辅助驾驶仿真测试的雷达点云数据在环回放方法，其特征在于：所述的步骤S4中，具体步骤为：

设T_i-1时刻的车辆横向速度为

T_i-1时刻的车辆横向加速度为

T_i-1时刻的数据采集车辆的横向运行速度为

T_i时刻的数据采集车辆的横向运行速度为

则T_i-1时刻至T_i时刻虚拟车辆与数据采集车辆的横向运动误差为：

7.根据权利要求6所述的辅助驾驶仿真测试的雷达点云数据在环回放方法，其特征在于：所述的步骤S5中，具体步骤为：

当i＝n时，虚拟车辆和数据采集车辆之间的累积纵向误差为：

虚拟车辆和数据采集车辆之间的累积横向误差为：

设未修正的Tn时刻的点云的三维坐标为(X_{n_ori}，Y_{n_ori}，Z_{n_ori})^T，则修正后的Tn时刻的点云的三维坐标(X_{n_revise}，Y_{n_revise}，Z_{n_revise})^T为：

8.用于权利要求1至7中任意一项所述的辅助驾驶仿真测试的雷达点云数据在环回放方法的在环回放系统，其特征在于：

包括传感单元、雷达点云处理单元、规划单元；传感单元设置在数据采集车辆上，传感单元的信号输出端连接雷达点云处理单元的信号输入端，雷达点云处理单元的信号输出端连接规划单元的信号输入端；

传感单元用于在数据采集车辆行进过程中采集数据采集车辆点云数据和场景中的标志物信息；

雷达点云处理单元用于根据数据采集车辆点云数据修正虚拟车辆点云数据和虚拟车辆运动数据；

规划单元用于根据虚拟车辆点云数据和场景信息规划虚拟车辆的动作和路径。

9.根据权利要求8所述的一种在环回放系统，其特征在于：虚拟车辆点云数据包括点云中每个点的三维坐标(X，Y，Z)，设雷达点云数据模块的核心部件的位置为三维坐标的原点，X轴、Y轴、Z轴的方向与车辆的运动方向一致，X轴指向车辆的前进方向，Z轴指向垂直向上，X、Y、Z三者的方向满足右手坐标系定则。

10.根据权利要求9所述的一种在环回放系统，其特征在于：虚拟车辆运动数据包括实时位置、车辆纵向速度、车辆纵向加速度、车辆方向盘转角、车辆横向速度和横向加速度。

Images