一种考虑速度因素的激光雷达测距的快速数值仿真方法与流程

本发明涉及激光雷达测距的数值仿真领域，尤其涉及一种考虑速度因素的激光雷达测距的快速数值仿真方法。

背景技术：

自动驾驶仿真技术，尤其是车辆传感器仿真技术，一直是自动驾驶领域的技术焦点之一。其中，对于激光雷达的仿真是不可或缺的重要组成部分。

激光雷达的仿真方法有很多，例如黄曦在cn104268323a发明专利申请公开说明书中提出一种基于光线跟踪的激光雷达场景仿真方法，该方法通过模拟激光射线的反射轨迹生成具有物理真实感的仿真成像；苏虎在cn107966693a发明专利申请公开说明书中提出一种基于深度渲染的车载激光雷达仿真方法，该方法周期性地对测试场景进行扇形区域的深度渲染，得到仿真数据图像。然而，这些方法对激光雷达自身的运动和扫描过程，以及场景中物体的运动的仿真不够精细，在仿真过程中将一段时间内激光雷达向各个方向发出的激光都认为是在某一时刻同时发出的，且在这段时间内场景中的所有物体相对于激光雷达而言保持静止。这和实际的激光雷达的工作原理并不一致，会导致仿真误差。

技术实现要素：

本发明的目的在于针对现有技术的不足，提供一种考虑速度因素的激光雷达测距的快速数值仿真方法，该方法在仿真中考虑环境物体相对激光器的运动和激光器自身的扫描旋转方式，通过对场景采样和动态更新的方式保持了计算过程的高效率，很好地平衡的仿真准确度和仿真效率。

本发明的目的是通过以下技术方案来实现的：一种考虑速度因素的激光雷达测距的快速数值仿真方法，包括以下步骤：

一种考虑速度因素的激光雷达测距的快速数值仿真方法，包括以下步骤：

（1）设待仿真的机械旋转式激光雷达器为lidar，设定其工作方式和参数如下：lidar具有nl个激光发射器，这些激光发射器以频率f同步发射激光射线，每个激光发射器发出1条激光射线，其起点均为lidar上的同一个点，称该点为基准点，所有激光发射器围绕着过基准点的一根直线进行定轴旋转,称该直线为转轴，垂直于转轴的平面为基准面，同一时刻激光发射器发射的nl条射线位于垂直于基准面的平面内，任选转轴一侧方向为转轴方向，这nl条激光射线与转轴方向形成的夹角依次为q0，q1，q2，...，qnl-1，满足qi<qj,0<=i<j<nl；lidar每个扫描周期开始时刻射出的激光射线在基准面上的垂直投影都重合于由基准点发出的一条射线，该射线称为基准线，在一个扫描周期t时间内激光发射器所旋转过的角度为fmax=ωt，ω为在扫描周期t内激光发射器的旋转角速度，在扫描周期结束后激光发射器回到扫描周期开始时的位置与姿态；lidar的最大探测距离为dmax；lidar上的基准点、基准线、基准面和转轴位置与姿态均定义在固定于lidar的物体坐标系中；

（2）选择正整数k，将扫描角度范围[0,fmax]分成为k个扫描角度区间[f0,f1],[f1,f2]，...，[fk-1,fk],使得每个水平扫描角度区间小于180度，其中f0=0，fk=fmax；

（3）开始lidar一个水平扫描周期的测距仿真：设此时的仿真时刻为tnt，对每个仿真时刻tk=tnt+fk/ω，其中k∈{0,1,...k-1}，进行如下处理：

（3.1）计算更新在tk时刻lidar以及lidar周围可反射激光的物体的位置与姿态；

（3.2）采样lidar周围可反射激光的物体表面，计算生成点集bk,对于任意采样点q∈bk，点q满足j(q)∈[fk,fk+1]、q(q)∈[q0,qnl-1]且基准点到点q距离小于等于dmax；其中，点q为r(q)与lidar周围可反射激光的物体表面相交的最近的一个交点，r(q)为从基准点出发经过点q的射线，j(q)为r(q)在基准面上的投影与基准线的夹角，q(q)为r(q)与转轴方向的夹角；

（3.3）生成一个具有ml列nl行的二维数据结构ck，并将每个元素初始化为非有效值，其中ml为大于等于(fk+1-fk)f/ω的最小整数，对每个i∈{0,1,2...ml-1}，采用如下步骤计算ck的第i列元素：

（3.3.1）当i为0时，直接执行步骤（3.3.2）；当i大于0时，计算更新在tk+i×f^-1时刻lidar和其周围可反射激光的物体的位置与姿态；

（3.3.2)遍历bk中每个点q，根据点q所属物体的位置与姿态，计算更新在tk+i×f^-1时刻点q的位置，并判断点q是否满足以下条件：

（i）|j(q)-fk-(i/ml)(fk+1-fk)|≤d1

(ii)|q(q)-qjj|≤d2

其中，d1为第一预设阈值，d2为第二预设阈值，qjj为序列{q0,q1,q2,...,qnl-1}中与q(q)最接近的值，jj是该值在序列中的序号；

（3.3.3)若点q同时满足上述条件(i)(ii),则用基准点与点q的距离来更新ck的第i列第jj行元素ck[i,jj]；若点q没有同时满足上述条件(i)(ii),则检验下一个点q是否满足条件(i)(ii)；

（3.4）输出c0，c1,...ck-1，这些数据结构为当前扫描周期的lidar的仿真测距结果，其中第k个数据结构ck的第i列元素所保存的数值是仿真时刻tnt+fk/ω+i×f^-1的nl个激光发射器的仿真测距结果；

（4）若仿真没有达到终止条件，执行步骤3）；否则结束仿真过程。

进一步地，所述步骤（3.2)中所生成的点集bk中的每个点包含该点在所属物体的物体坐标系内的位置坐标，以及用来直接或间接得到该点所属物体的物体坐标系位置与姿态的信息。

进一步地，所述步骤（3.3.3)中用基准点与点q的距离来更新ck的第i列第jj行元素ck[i,jj]时，采用如下更新准则：若ck[i,jj]为初始化时设置的非有效值，则将ck[i,jj]设置为基准点到点q的距离；若ck[i,jj]不是初始化时设置的非有效值且基准点与点q的距离小于ck[i,jj]，则将ck[i,jj]设置为基准点与点q的距离；若ck[i,jj]不是初始化时设置的非有效值且基准点与点q的距离大于等于ck[i,jj]，则检验下一个点q是否满足条件(i)(ii)。

本发明的有益效果是，本发明在仿真过程中充分考虑了激光雷达自身和周围环境中物体的运动，其中激光雷达自身的运动不仅包括其装置的整体运动，还包括其激光器的旋转扫描运动，提供了数值仿真的准确度，同时通过引入采样点集合简化了计算量，达到了使用较少计算量来提高了仿真的准确度的效果。该方法特别适用于场景中激光雷达自身和/或周围物体都处于高速运动状态时，此时该方法能实现的仿真精度较之前方法有显著提高。

附图说明

图1为激光雷达物体坐标系示意图；

图2为激光雷达与环境物体被采样点的相互位置关系示意图；

图3为多类纹理图像间的数据关联示意图；

图4为本发明提出的激光雷达仿真方法的效果示意图。

具体实施方式

下面根据附图和优选实施例详细描述本发明，本发明的目的和效果将变得更加明白，应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明提出了一种考虑速度因素的激光雷达测距的快速数值仿真方法，包括以下步骤：

（1）设待仿真的水平扫描的激光雷达器为lidar，如图1和2所示，设定其工作方式和参数如下：lidar的扫描周期为0.1秒；lidar具有32个激光发射器，这些激光发射器以14400hz的频率f同步发射激光射线，每个激光发射器发出1条激光射线，其起点均为lidar上的同一个点，该点为基准点，所有激光发射器围绕着过基准点的一根直线进行定轴旋转，称该直线为转轴。垂直于转轴的平面为基准面。同一时刻激光器发射的32条射线位于垂直于基准面的平面内，设转轴向上的方向为转轴方向，这32条射线与转轴方向形成的夹角为一个等差数列，依次为q0=60°，q1=62°，q2=64°，...，qnl-1=122°，其中nl=32；lidar每个扫描周期开始时射出的激光射线在基准面上的投影都重合于由基准点发出的一条射线，该射线称为基准线，在一个扫描周期0.1秒内激光发射器所旋转过的角度fmax=360°,激光发射器的旋转角速度ω=3600°/s；lidar的最大探测距离dmax=100米；lidar的刚体运动由固定于lidar的物体坐标系的刚体运动来表示，lidar周围可反射激光的任意物体的刚体运动由固定于该物体上的物体坐标系的刚体运动来表示，其涉及的空间坐标均以米为单位；lidar上的基准点、基准线、基准面和转轴位置与姿态均定义在固定于lidar的物体坐标系中，如图1所示；场景中所有物体均用三角网格描述各自表面的形状。

（2）将扫描范围[0°,360°]平均分为6个扫描区间,[f0,f1],[f1,f2],...,[f5,f6]，其中f0=0°,f1=60°,f2=120°，...,f6=360°，记每个扫描区间角度范围δf=60°。

（3）开始lidar一个水平扫描周期的测距仿真：设此时的仿真时刻为tnt，对每个仿真时刻tk=tnt+k/60，其中k∈{0,1,2，...,5}，进行如下处理：

（3.1）计算更新在tk时刻lidar以及lidar周围可反射激光的物体的位置和姿态。

（3.2）通过三维图形绘制的方法来获得lidar周围可反射激光的物体表面上的采样点,计算生成采样点集bk。激光雷达与被采样点的相互位置关系如图2所示。具体采样步骤如下：

（3.2.1）设tk时刻lidar基准点位置为向量eye=[eyex,eyey,eyez]，设lidar转轴方向为向量up=[upx,upy,upz]。以eye为起点在基准面内做一条射线，该射线与基准线方向夹角为（2k+1)•δf/2，在该射线上取点center=[centerx,centery,centerz]，center和eye的距离等于eye模长。使用opengl函数库中的函数glulookat(eyex,eyey,eyez,centerx,centery,centerz,upx,upy,upz)来构造三维图形绘制时所需的观察矩阵mview。

（3.2.2）使用opengl函数库中的函数glfrustum(left,right,bottom,top,near,far)来构造三维图形绘制时的投影矩阵mproj,其中far取为lidar的最大探测距离为100，near可取为0.1，left=-near•tan(δf/2),right=near•tan(δf/2),top=near•ctan(q0),bottom=near•ctan(qnl-1）。

（3.2.3）采用上述的mview和mproj设置三维图形绘制时的照相机观察投影参数。

（3.2.4）创建一张纹理图像点集bk,具备240列,32行，像素格式为rgba32。采用z-buffer消隐算法对lidar周围可反射激光的物体表面三角网格逐一进行绘制，并将绘制结果保存于纹理图像bk中。对于所绘制的任意一个物体，它表面由三角网格表示。绘制每个三角形时，将每个三角形所属的物体的坐标系的位置和顶点所属物体的序号id作为每个顶点的属性，传入gpu进行计算。在vertexshader计算阶段，计算三角形顶点坐标在经过物体本身的模型变换和mview矩阵变换后的结果输出到位置输出通道，同时将顶点的物体坐标系的位置和所属物体id号传递给pixelshader。在pixelshader计算阶段，在最终输出的rgb通道内写入顶点的物体坐标系的位置，在最终输出的a通道内写入顶点所属的物体id。最终的绘制结果纹理图像点集bk中的一个像素描述了一个物体表面采样点，且符合如下条件：

j(q)∈[fk,fk+1],q(q)∈[q0,qnl-1]，基准点到点q距离小于等于dmax；

其中，点q为r(q)与lidar周围可反射激光的物体表面相交的最近的一个交点，r(q)为从基准点出发经过点q的射线，j(q)为r(q)在基准面上的投影与基准线的夹角，q(q)为r(q)与转轴方向的夹角。

并且，对纹理图像点集bk中的任意像素，通过其a通道存储的采样点所属物体的序号id得到该物体的物体坐标系位置和姿态信息，进而可知纹理图像bk所存储的信息符合如下条件：

所生成的点集bk中的每个点包含该点在所属物体的物体坐标系内的位置坐标，以及用来直接或间接得到该点所属物体的物体坐标系位置与姿态的信息。

（3.3）采用纹理图像的方式创建一个二维数据结构ck，具备ml=(fk+1-fk)f/ω=240列,nl=32行,像素格式为r32,用来存储深度值。ck所有像素值初始化为非有效值10⁸。对于i∈{0,1,2...239}，计算ck的第i列元素。

（3.3.1）当i为0时，直接执行步骤（3.3.2）；当i大于0时，更新ti=tk+i/14400时刻lidar的位置和姿态，再按照步骤（3.2.1）的方法，得到ti时刻的观察矩阵mview。更新lidar和其周围可反射激光物体的位置和姿态，计算出每个物体在ti时刻的模型变换矩阵。设共有n个物体，记第n个物体的模型变换矩阵为mfn。将n个物体对应的模型变换矩阵存贮于一张n列4行的纹理图像ek中，ek的像素格式为rgba32，ek的第n列第0,1,2,3行像素分别存储了mfn的四个行向量。建立一张物体序号id与对应物体变换矩阵在纹理图像ek中的列位置的查找表vl。

（3.3.2）用computershader来处理bk中的每个像素，将计算结果输出至二维数据结构ck中。对bk中的每一个像素p执行以下步骤：

（3.3.2.1）用在p的a通道中存贮的物体序号id在查找表vl中找到对应的模型变换矩阵在纹理图像ek中的列位置n，从ek取出第n列的第0,1,2,3行像素，形成模型变换矩阵mfn。

（3.3.2.2）取出在p的rgb通道中存贮的坐标向量p.rgb，计算三维向量q=mview•mfn•p.rgb,其中q即为像素p对应采样点在ti时刻的位置。

（3.3.2.3）计算q¢=(90°-q0)-arctan(q.y/q.z),j¢=δf/2-arctan(q.x/q.z)。

（3.3.2.4）计算整数下标jj=round(32q¢/(qnl-1-q0))和ix=round(240j¢/δf)。round为舍入取整函数。

（3.3.2.5）如果jj<0或jj>=32，或者ix不等于i，则忽略该像素p，返回步骤(3.3.2.1)继续处理下个像素。设第一预设阈值d1为(fk+1-fk)/ml=0.25°，第二预设阈值d2为32条激光射线间的公差2°，可以验证，如果像素p没有被忽略，则像素p对应的采样点位置q符合如下条件：

（i）|j(q)-fk-(i/ml)(fk+1-fk)|≤d1

(ii)|q(q)-qjj|≤d2

其中，d1为第一预设阈值，d2为第二预设阈值，qjj为序列{q0，q1，q2，...，qnl-1}中与q(q)最接近的值，jj是该值在序列中的序号；

（3.3.2.6）计算q的向量模长‖q‖，并比较ck中第i列第jj行的像素值ck[i,jj]和‖q‖的大小：如果‖q‖<ck[i,jj]，则设置ck中第i列第jj行的ck[i,jj]=‖q‖，否则忽略该像素p，返回步骤(3.3.2.1)继续处理下个像素。可以验证，对ck中像素值的更新方式符合如下的更新原则：

若ck[i,jj]为初始化时设置的非有效值，则将ck[i,jj]设置为基准点到点q的距离；若ck[i,jj]不是初始化时设置的非有效值，且若基准点与点q的距离小于ck[i,jj]，则将ck[i,jj]设置为基准点与点q的距离；若ck[i,jj]不是初始化时设置的非有效值，且若基准点与点q的距离大于等于ck[i,jj]，则检验下一个点q是否满足条件(i)(ii)。

（3.4）输出c0,c1,...,c5，这些纹理图像为当前扫描周期的lidar的仿真测距结果，其中第k张数据结构ck的第i列元素所保存的数值是仿真时刻ti的32个激光器发射激光后得到的仿真测距结果。上述各类纹理图像间的数据关联示意图如图3所示。

（4）若激光雷达没有达到预设的仿真时间，或者仿真程序中途退出，继续执行步骤（3）；否则结束仿真结果。

最终的仿真结果示意图如图4所示，场景中的白色点云为画面正中车辆的雷达扫描仿真结果。图中，画面左侧的卡车和正中车辆都处于运动中。可以看到，卡车被扫描形成的白色点云位置与卡车的实际位置存在一定的位移偏差，这正是考虑到车辆相对运动和激光器旋转才能形成的仿真结果，更接近实际的雷达扫描过程，体现了本发明的有益效果。

本领域普通技术人员可以理解，以上所述仅为发明的优选实例而已，并不用于限制发明，尽管参照前述实例对发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在发明的精神和原则之内，所做的修改、等同替换等均应包含在发明的保护范围之内。