CN108732556A - 一种基于几何求交运算的车载激光雷达仿真方法 - Google Patents

Abstract

本发明公开一种基于几何求交运算的车载激光雷达仿真方法，仿真前，建立道路交通场景的三维几何模型，仿真时，将三维几何模型加载到计算机的内存中，根据需要模拟的车载激光雷达的型号、特性参数、在车上的安装位置与角度，初始化激光雷达仿真模型并渲染，进入周期性仿真，每个仿真周期中，利用几何体与激光线段的求交算法，计算激光线段与三维几何模型的交点数据，如有相交，将从光线起点出发的首个几何体交点作为扫描点数据，如无相交，这条光线无扫描结果，将所获得的仿真扫描数据，按激光雷达数据协议输出或保存，进入到下一仿真周期，直至仿真运行结束。

Description

一种基于几何求交运算的车载激光雷达仿真方法

技术领域

本发明涉及车载激光雷达仿真领域，特别是一种基于几何体求交的车载激光雷达仿真方法。

背景技术

在无人驾驶架构中，传感层被比作为汽车的“眼睛”，其包括车载摄像头等视觉系传感器和车载毫米波雷达、车载激光雷达和车载超声波雷达等雷达系传感器。其中激光雷达已经被认为是实现自动驾驶的必要基础，在无人驾驶车辆的研发中，往往要经历各种测试，而在试车测试前，首先需要通过仿真测试，其中对车载激光雷达的仿真，是开展仿真测试的重要基础。

发明内容

本发明的目的在于克服现有技术的不足，提供一种基于几何体求交的车载激光雷达仿真方法。

本发明的目的通过以下技术方案来实现：一种基于几何体求交的车载激光雷达仿真方法，包括以下步骤：

S1加载高精度道路场景的三维几何模型到计算机内存，加载完成后，根据几何体的顶点分布情况，生成一个管理几何模型的层次四叉树。

S2加载激光雷达仿真模型，根据仿真激光雷达的线数、扫描频率、扫描角分辨率、探测范围进行初始化，计算出以激光雷达中心为坐标原点的坐标系中所有激光扫描线段的起点坐标、终点坐标；

S3启动周期性的仿真计算，包括如下子步骤：

S31首先获得所仿真车辆在世界坐标系中的位置、姿态角度；

S32根据在激光雷达在所仿真车辆中的虚拟安装位置、安装姿态角，计算得到激光雷达坐标系与世界坐标系的变换关系；

S33根据所述变换关系，计算出第二步获得的全部激光扫描线段在世界坐标系的起点坐标、终点坐标；

S4遍历所述的层次四叉树，根据步骤S3获得的全部激光扫描线段的起点坐标、终点坐标，找到线段所在区域对应的层次四叉树末级叶节点，遍历叶节点对应的所有几何模型，计算和扫描线段的交点，并根据交点与线段起点的距离进行排序；

S5将距离扫描线段起点最近的交点坐标、扫描线的编号记录下来，作为所仿真激光雷达对应扫描点上的仿真数据；将全部仿真数据，以指定的接口输出或存储；

S6如果是超实时仿真，直接返回到步骤S3直至仿真运行结束；如果是实时仿真，需要等待到下一个仿真周期开始，再返回到步骤S3，直至仿真运行结束。

进一步的，所述层次四叉树中末级节点对应场景模型区域为长方体，其中长、宽均为所仿真激光雷达有效探测范围的两倍，高度为区域内所有几何对象顶点坐标的最大Z值与最小Z值之差，一个父节点包括四个子节点，所述层次四叉树根节点对应的区域覆盖三维几何模型坐标的全部范围。

进一步的，还包括一个预处理步骤，其根据仿真激光雷达的线数、扫描频率、扫描角分辨率、探测范围，对所有激光扫描线段在激光雷达坐标系中的起点坐标、终点坐标进行预计算，以降低仿真计算时的计算工作量。

进一步的，层次四叉树中遍历查找的具体过程是，当第一条扫描线寻找到对应叶节点后，其它扫描线只需要在该叶节点相邻的八个邻叶节点中进行寻找，每新找到一个叶节点，一下条扫描线的搜索范围则被限定到已找到所有叶节点共同的邻叶节点，因规定了末级节点对应区域的尺寸，末级叶节点的最大可能数量为四个，一旦找满四个叶节点，则停止查找，进行下一步计算。

进一步的，通过对激光扫描线段与其空间位置匹配的末级节点对应区域的几何体进行求交运算，模拟激光雷达的工作原理，将获得的一系列交点，根据距离扫描线段起点的距离进行排序，距离起点最近的那个交点坐标作为该条激光线的扫描结果。

进一步的，还包括建立高精度道路场景三维几何模型，所述三维几何模型建立在世界坐标系中，仅包含多边形顶点坐标、多边形拓扑结构等几何信息。

本发明具有以下优点：

1.采用层次四叉树结构进行几何体求交，加快仿真进程；

2.根据所仿真激光雷达的参数进行预计算，降低仿真时的计算工作量，缩短仿真时间；

3.可实现超实时仿真，进一步缩短仿真时间，提高工作效率。

附图说明

图1 为本发明的仿真方法流程图；

图2 为本发明的水平方向虚拟激光扫描线生成示意图；

图3 为本发明的垂直方向虚拟激光扫描线生成示意图。

具体实施方式

为使发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和标示出的本发明实施例的组件可以以各种不同的配置来布置和设计。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本实施例提供一种基于几何体求交的车载激光雷达仿真方法，首先建立表征道路环境几何特征的三维几何模型，所述三维几何模型建立在世界坐标系中，仅包含多边形顶点坐标、多边形拓扑结构等几何信息，不包含纹理、光照材质、法向量等数据；步骤如下：

第一步：进行仿真前，将第一步建立的三维几何模型加载到计算机内存中，加载完成后，根据几何体的顶点分布情况，生成一个管理几何模型的层次四叉树。

第二步：根据所仿真激光雷达的线数、扫描频率、扫描角分辨率、探测范围进行初始化计算，计算出以激光雷达中心为坐标原点的坐标系中所有激光扫描线段的起点坐标、终点坐标；

第三步：启动周期性的仿真计算，首先获得所仿真车辆在世界坐标系中的位置、姿态角度，再根据在激光雷达在所仿真车辆中的虚拟安装位置、安装姿态角，计算得到激光雷达坐标系与世界坐标系的变换关系，根据该变换关系，计算出第二步获得的全部激光扫描线段在世界坐标系的起点坐标、终点坐标；

第四步：遍历第二步生成的层次四叉树，根据第三步获得的全部激光扫描线段的起点坐标、终点坐标，找到线段所在区域对应的层次四叉树末级叶节点，遍历叶节点对应的所有几何模型，计算和扫描线段的交点，并根据交点与线段起点的距离进行排序；

第五步：将距离扫描线段起点最近的交点坐标、扫描线的编号记录下来，作为所仿真激光雷达对应扫描点上的仿真数据；将全部仿真数据，以指定的接口输出或存储；

第六步：如果是超实时仿真，直接返回到第三步，直至仿真运行结束，如果是实时仿真，需要等待到下一个仿真周期开始，再返回到第三步，直至仿真运行结束。

根据所仿真激光雷达有效探测范围，动态构建管理几何模型的层次四叉树，四叉树中末级节点对应场景模型区域为长方体，其中长、宽均为所仿真激光雷达有效探测范围的两倍，高度为区域内所有几何对象顶点坐标的最大Z值与最小Z值之差。

根据所仿真激光雷达的线数、扫描频率、扫描角分辨率、探测范围，对所有激光扫描线段在激光雷达坐标系中的起点坐标、终点坐标进行预计算，以降低仿真计算时的计算工作量。

根据激光扫描线段在世界坐标系的起点、终点坐标，通过在层次四叉树中进行遍历查找，寻找到包含线段或与线段相交的区域对应的四叉树末级节点，当第一条扫描线寻找到对应叶节点后，其它扫描线只需要在该叶节点相邻的八个邻叶节点中进行寻找，每新找到一个叶节点，一下条扫描线的搜索范围则被限定到已找到所有叶节点共同的邻叶节点，因规定了末级节点对应区域的尺寸，末级叶节点的最大可能数量为四个，一旦找满四个叶节点，则停止查找，进行下一步计算。

通过对激光扫描线段与其空间位置匹配的末级节点对应区域的几何体进行求交运算，来模拟激光雷达的工作原理，将获得的一系列交点，根据距离扫描线段起点的距离进行排序，距离起点最近的那个交点坐标作为该条激光线的扫描结果。

Claims (6)

1.一种基于几何体求交的车载激光雷达仿真方法，其特征在于，包括以下步骤：

S1加载高精度道路场景的三维几何模型到计算机内存，加载完成后，根据几何体的顶点分布情况，生成一个管理几何模型的层次四叉树；

S2加载激光雷达仿真模型，根据仿真激光雷达的线数、扫描频率、扫描角分辨率、探测范围进行初始化，计算出以激光雷达中心为坐标原点的坐标系中所有激光扫描线段的起点坐标、终点坐标；

S3启动周期性的仿真计算，包括如下子步骤：

S31首先获得所仿真车辆在世界坐标系中的位置、姿态角度；

S32根据在激光雷达在所仿真车辆中的虚拟安装位置、安装姿态角，计算得到激光雷达坐标系与世界坐标系的变换关系；

S33根据所述变换关系，计算出第二步获得的全部激光扫描线段在世界坐标系的起点坐标、终点坐标；

S4遍历所述的层次四叉树，根据步骤S3获得的全部激光扫描线段的起点坐标、终点坐标，找到线段所在区域对应的层次四叉树末级叶节点，遍历叶节点对应的所有几何模型，计算和扫描线段的交点，并根据交点与线段起点的距离进行排序；

S5将距离扫描线段起点最近的交点坐标、扫描线的编号记录下来，作为所仿真激光雷达对应扫描点上的仿真数据；将全部仿真数据，以指定的接口输出或存储；

S6如果是超实时仿真，直接返回到步骤S3直至仿真运行结束；如果是实时仿真，需要等待到下一个仿真周期开始，再返回到步骤S3，直至仿真运行结束。

2.根据权利要求1所述的一种基于几何体求交的车载激光雷达仿真方法，其特征在于：所述层次四叉树中末级节点对应场景模型区域为长方体，其中长、宽均为所仿真激光雷达有效探测范围的两倍，高度为区域内所有几何对象顶点坐标的最大Z值与最小Z值之差，一个父节点包括四个子节点，所述层次四叉树根节点对应的区域覆盖三维几何模型坐标的全部范围。

3.根据权利要求1所述的一种基于几何体求交的车载激光雷达仿真方法，其特征在于：还包括一个预处理步骤，其根据仿真激光雷达的线数、扫描频率、扫描角分辨率、探测范围，对所有激光扫描线段在激光雷达坐标系中的起点坐标、终点坐标进行预计算，以降低仿真计算时的计算工作量。

4.根据权利要求1所述的一种基于几何体求交的车载激光雷达仿真方法，其特征在于：层次四叉树中遍历查找的具体过程是，当第一条扫描线寻找到对应叶节点后，其它扫描线只需要在该叶节点相邻的八个邻叶节点中进行寻找，每新找到一个叶节点，一下条扫描线的搜索范围则被限定到已找到所有叶节点共同的邻叶节点，因规定了末级节点对应区域的尺寸，末级叶节点的最大可能数量为四个，一旦找满四个叶节点，则停止查找，进行下一步计算。

5.根据权利要求1所述的一种基于几何体求交的车载激光雷达仿真方法，其特征在于：通过对激光扫描线段与其空间位置匹配的末级节点对应区域的几何体进行求交运算，模拟激光雷达的工作原理，将获得的一系列交点，根据距离扫描线段起点的距离进行排序，距离起点最近的那个交点坐标作为该条激光线的扫描结果。

6.根据权利要求1所述的一种基于几何体求交的车载激光雷达仿真方法，其特征在于：还包括建立高精度道路场景三维几何模型，所述三维几何模型建立在世界坐标系中，仅包含多边形顶点坐标、多边形拓扑结构等几何信息。