一种基于激光点云的目标车辆位置速度估计方法与流程

1.本发明属于计算机视觉和自动化交叉领域的一种车辆参数估计方法，涉及一种目标车辆的速度位置估计方法。


背景技术：

2.无人驾驶车辆需要对交通环境中其他参与者进行检测，从而进一步规划自身行驶轨迹。在规划自身行驶轨迹的过程中，需要目标车辆的信息，例如车辆的位置和速度信息。本方法针对该问题，提出了一种仅使用无人车搭载的激光雷达所扫描到的点云数据就可以对其他车辆的位置和速度进行位置估计的方法。
3.对于其他车辆位置速度估计方法，一般包括如下2个步骤：对目标车辆的识别与位置估计，估计跟踪的目标车辆的速度。
4.对于目标车辆的识别与位置估计问题，yipeng li等人)y.li,y.wang,and y.xie,“using consecutive point clouds for pose and motion estimation of tumbling non
‑
cooperative target,”advances in space research,vol.63,no.5,pp.1576
–
1587,2019)提出使用最近邻迭代算法来确定目标车辆的位置。随着深度卷积三维目标检测网络技术的发展，shaoshuai shi(s.shi,x.wang,and h.li,“pointrcnn:3d object proposal generation and detection from point cloud,”in proceedings of the ieee conference on computer vision and pattern recognition,jun 2019,pp.770
–
779)提出了使用pointnet 三维目标检测网络来搭建目标检测器，从而能够在点云中识别不同类型的目标车辆。
5.对于估计跟踪的目标车辆的速度的问题，目前方法普遍使用对其它目标车辆的位置跟踪结果作为输入，使用卡尔曼滤波器或者扩展卡尔曼滤波器进行滤波，从而得到目标车辆的速度。但卡尔曼滤波器和扩展卡尔曼滤波器在内部计算存在矩阵求逆过程，使得这类滤波器在运算速度上较慢，导致速度的估计滞后严重。并且，由于矩阵求逆的过程对于奇异转状态不鲁棒性，所以这类滤波器有可能在某些状态失效。
6.综上，虽然对于在对其他车辆的位置和速度进行位置估计问题上，已有方法已经取得了一定成果，但仍然存在一些不足：
7.1)现有方法使用点云数据作为输入，对于目标车辆的识别的位置检测还不够精准，使得最终的位置估计偏差较大。
8.2)现有方法使用卡尔曼滤波器或者扩展卡尔曼滤波器估计目标车辆的速度，其速度估计带有滞后性。


技术实现要素：

9.为了解决无人车对于目标车辆的位置速度估计问题，本方法设计了仅使用点云数据的对目标车辆的位置速度估计方法。
10.本发明采用的技术方案是：
11.本发明仅使用点云数据就可以对目标车辆的位置和速度进行同时的估计。具体来说，本方法分为两个部分，首先是使用三维目标检测网络估计目标车辆的位置，其次是使用降阶观测器估计目标车辆的速度。
12.方法以激光雷达所扫描到的原始点云数据作为输入，构建三维目标检测网络对目标车辆的位置和朝向进行估计，构建降阶观测器对目标车辆的速度进行估计。
13.所述的三维目标检测网络，使用pointnet 网络作为特征提取主干网络，以原始点云作为输入，提取每个点的高级特征，然后将每个点的高级特征送入目标车辆位置回归子网络，经目标车辆位置回归子网络回归处理获得目标车辆的中心位置以及朝向；
14.所述的降阶观测器，使用三维目标检测网络估计的目标车辆的中心位置作为输入，估计获得目标车辆的速度。
15.所述的目标车辆位置回归子网络，包括选取属于目标车辆的点和回归目标车辆的位置及朝向的两个步骤；
16.选取属于目标车辆的点，根据每个点的高级特征，预测每个点是否属于目标车辆的概率值，选取概率值超过预设阈值以上的点作为目标车辆点；
17.回归目标车辆的位置及朝向，利用目标车辆点及其高级特征作为输入，利用多层感知机回归目标车辆的中心位置和朝向角。
18.所述的三维目标检测网络结构具体处理为：
19.1.1将激光雷达扫描到的原始点云及其初始特征p0作为输入，送入包含sa层的特征提取主干网络中提取高级特征，得到特征提取主干网络的输出点云及其高级特征集合；
20.1.2、对于特征提取主干网络的输出点云及其高级特征集合p4中的每个点，以每个点的高级特征为多层感知机的输入，利用共享参数的多层感知机，各个点的多层感知机共享参数，回归获得每个点对应的目标车辆的中心位置[c
x
,c
y
]；
[0021]
所述1.2中的多层感知机的构成表示为：fc bn relu fc bn relu，其中，fc代表全连接层，bn代表批归一化层，relu代表以relu函数为激活函数的激活层。
[0022]
1.3、以上述目标车辆的中心位置[c
x
,c
y
]为原点，再使用单个sa层，对原始点云及其初始特征p0中处理提取局部区域特征，获得第五阶输出点云及其局部区域特征集合p5；
[0023]
对特征提取主干网络的输出点云及其高级特征集合p4和第五阶输出点云及其局部区域特征集合p5做连接操作，即将两种点云中每个同一点的高级特征和局部区域特征进行连接获得融合特征，获得第六阶输出点云及其融合特征集合p6；
[0024]
1.4、对于特征提取主干网络的输出点云及其高级特征集合p4中的每个点，以每个点的融合特征为多层感知机的输入，利用共享参数的多层感知机，回归获得每个点对应的目标车辆的朝向[c
o
]；
[0025]
所述1.4中的多层感知机的构成表示为：fc bn relu fc bn relu，其中，fc代表全连接层，bn代表批归一化层，relu代表以relu函数为激活函数的激活层。
[0026]
1.5、对于特征提取主干网络的输出点云及其高级特征集合p4中的每个点，以每个点的融合特征为多层感知机的输入，利用共享参数的多层感知机，回归获得每个点对应的目标车辆的置信度s，取置信度s最高的点所回归获得的目标车辆的中心位置和朝向作为三维目标检测网络的结果输出。
[0027]
所述的特征提取主干网络是由四个sa层依次连接构成，每个sa层均采用pointnet
网络中提出的标准网络结构。
[0028]
所述降阶观测器的设计如下：
[0029][0030][0031]
p
k
＝[c
x
,c
y
,c
o
]
t
[0032][0033]
其中，p
k
为以第k帧原始点云作为输入经三维目标检测网络所检测到的目标车辆的位置和朝向向量，上标t表示向量转置；α为常量参数矩阵，β
k
是第k帧原始点云的辅助向量，为第k帧原始点云的目标车辆的速度，t为激光雷达扫描时间间隔；
[0034]
第k帧激光雷达所扫描到的原始点云利用三维目标检测网络处理获得目标车辆的位置和朝向向量p
k
，再通过降阶观测器计算得到第k帧原始点云的辅助向量β
k
与第k帧原始点云的目标车辆的速度。
[0035]
本发明特殊构建了上述降阶观测器，上述降阶观测器能够在相邻帧间传递辅助向量，通过传递该辅助变量完成帧间位置速度信息传递，使得降阶观测器对于速度的估计可以依赖多帧历史信息，能够提升速度估计的准确性，同时实现了抑制位置估计误差引起的速度波动。
[0036]
本发明的有益效果是：
[0037]
本发明的创新点在于利用pointnet 中sa网络结构搭建目标物体位置朝向估计网络，并设计了降阶观测器用于估计目标物体的速度。本发明的优势在于，仅使用激光雷达这种单一传感器所获取的点云数据，就可以对目标物体的位置和速度这两组状态同时进行估计。本发明所设计的目标物体的位置朝向估计网络以激光雷达扫描的原始点云作为输入，不需要任何的前处理，可以对复杂环境中的目标物体的位置朝向进行估计。本发明设计的降阶观测器形式简单，计算高效，并可以抑制位置估计误差对速度估计的影响，从而使得目标物体的速度估计精确。
附图说明
[0038]
图1是本发明的方法流程图；
[0039]
图2是仿真环境示意图；
[0040]
图3是仿真结果图；
[0041]
图4是实验环境示意图；
[0042]
图5是实验结果图。
具体实施方式
[0043]
下面结合附图及具体实施例对本发明作进一步详细说明。
[0044]
本发明实施例如下：
[0045]
1、对用于估计目标车辆位置的三维目标检测网络，三维目标检测网络结构具体处理为：
[0046]
1.1将激光雷达扫描到的原始点云及其初始特征p0作为输入，送入包含sa层的特征提取主干网络中提取高级特征，得到特征提取主干网络的输出点云及其高级特征集合p4，即第四阶输出点云及其中所有点的第四阶高级特征组成集合；
[0047]
具体实施中，使用pointnet 网络来构建特征提取主干网络，其中包含4个依次连接的sa层(sa层为pointnet 中提出的标准网络结构)，4个sa层分别表示为
[0048]
每一个sa层构建所需要的的参数如下表给出：
[0049][0050][0051]
每个sa层的操作表示如下：
[0052][0053]
其中，为第i个sa层的操作，p
i
‑1是第i个sa层的输入点云及其特征，p
i
是第i个sa层的输出点云及其特征。
[0054]
原始点云中每个点的初始特征为每个点的三维坐标，特征提取主干网络中各sa层获得的点云中每个点的特征均为依赖sa层提取的每个点的高级特征。
[0055]
1.2、对于特征提取主干网络的输出点云及其高级特征集合p4中的每个点，以每个点的高级特征为多层感知机的输入，利用共享参数的多层感知机，各个点的多层感知机共享参数，回归获得每个点对应的目标车辆的中心位置[c
x
,c
y
]，c
x
,c
y
分别为目标车辆的中心位置在xy三个坐标轴的分量；
[0056]
多层感知机的构成表示为：fc bn relu fc bn relu，即两个全连接归一模块依次连接构成，每个全连接归一模块均由全连接层、批归一化层、激活层依次连接构成。其中，fc代表全连接层，bn代表批归一化层，relu代表以relu函数为激活函数的激活层。其中，第一个fc层的通道数为512，第二个fc层的通道数为2。
[0057]
1.3、以上述目标车辆的中心位置[c
x
,c
y
]为原点，再使用单个sa层，对原始点云及其初始特征p0中处理提取局部区域特征，获得第五阶输出点云及其局部区域特征集合p5；
[0058]
具体实施中单个sa层的具体参数如下：
[0059]
降采样点的数量64邻域半径2.0米邻域内点的数量32多层状态感知机通道数量1024
[0060]
单个sa层中的采样点数和特征提取主干网络中的第四个sa层的采样点数相同，单个sa层的输出第五阶输出点云及其局部区域特征集合p5包含点的数量和特征提取主干网络的输出点云及其高级特征集合p4相同。
[0061]
对特征提取主干网络的输出点云及其高级特征集合p4和第五阶输出点云及其局部区域特征集合p5做连接操作concatenate，即将两种点云中每个同一点的第四阶高级特征和局部区域特征进行连接获得融合特征，获得第六阶输出点云及其融合特征集合p6；
[0062]
根据做连接操作concatenate的操作性质，第六阶输出点云及其融合特征集合p6中包含的点的总数量与特征提取主干网络的输出点云及其高级特征集合p4、第五阶输出点云及其局部区域特征集合p5中包含的点的总数量均相同。
[0063]
具体实施通过事先设定点云中点的序号对各个点云之间的点进行对应，实现连接操作。
[0064]
1.4、对于特征提取主干网络的输出点云及其高级特征集合p4中的每个点，以每个点的融合特征为多层感知机的输入，利用共享参数的多层感知机，即各个点的多层感知机共享参数，回归获得每个点对应的目标车辆的朝向[c
o
]；
[0065]
多层感知机的构成表示为：fc bn relu fc bn relu，即两个全连接归一模块依次连接构成，每个全连接归一模块均由全连接层、批归一化层、激活层依次连接构成。其中，fc代表全连接层，bn代表批归一化层，relu代表以relu函数为激活函数的激活层。其中，第一个fc层的通道数为512，第二个fc层的通道数为1。
[0066]
1.5、对于特征提取主干网络的输出点云及其高级特征集合p4中的每个点，以每个点的融合特征为多层感知机的输入，利用共享参数的多层感知机，即各个点的多层感知机共享参数，回归获得每个点对应的目标车辆的置信度s，取置信度s最高的点所回归获得的目标车辆的中心位置和朝向作为三维目标检测网络的结果输出。
[0067]
上述1.3
‑
1.5构成了目标车辆位置回归子网络。
[0068]
2、三维目标检测网络的训练和待测处理过程：
[0069]
2.1训练过程：
[0070]
使用kitti数据集中的训练集对三维目标检测网络进行训练。训练过程的损失函数按照3dssd中的损失函数进行设计。优化器使用sgd优化器，学习率设置为0.001进行mini
‑
batch训练，mini
‑
batch设置为4。通过迭代训练32000次，保存训练过程中损失最小的网络参数权重。
[0071]
2.2、待测过程：将每一帧点云作为输入，送入已经训练好的三维目标检测网络估计目标车辆的位置[c
x
,c
y
,c
z
]和朝向信息[c
o
]。
[0072]
3、建立降阶观测器，以三维目标检测网络对目标车辆的位置和朝向结果作为输入，输出目标车辆的速度估计。
[0073]
降阶观测器的设计如下：
[0074][0075][0076]
p
k
＝[c
x
,c
y
,c
o
]
t
[0077][0078]
其中，p
k
为以第k帧原始点云作为输入经三维目标检测网络所检测到的目标车辆的位置和朝向向量，上标t表示向量转置；α为常量参数矩阵，β
k
是第k帧原始点云的辅助向
量，为第k帧原始点云的目标车辆的速度，t为激光雷达扫描时间间隔；
[0079]
如图1所示，第1帧激光雷达所扫描到的原始点云利用三维目标检测网络处理获得目标车辆的位置和朝向向量p1，通过降阶观测器获得计算得到β1与其他时刻的每一帧以此类推，假设当前时刻的帧序数为k，并且已知第k
‑
1帧的目标车辆的速度和辅助向量β
k
‑1，此时接收到第k帧激光雷达所扫描到的原始点云利用三维目标检测网络处理获得目标车辆的位置和朝向向量p
k
，再通过降阶观测器计算得到第k帧原始点云的辅助向量β
k
与第k帧原始点云的目标车辆的速度从而获得第k时刻的目标车辆速度
[0080]
具体实施中，车载激光雷达扫描频率为10hz，所以t设置为0.1s，α设置为t和α在运算过程中为常量，不会改变。
[0081]
第0帧原始点云的辅助向量β0和第0帧原始点云的目标车辆的速度的初始值均为[5,5,5]
t
，初值的选取不影响速度估计的收敛性。
[0082]
具体实施情况如下：
[0083]
1、在matlab中进行仿真验证本发明中提出的方法。使用simulink中的automated driving toolbox搭建仿真环境，其中仿真场景为该toolbox提供的“uscityblock”场景，场景样例为图2左图所示。
[0084]
在场景中设置搭载激光雷达的无人车后车，以及目标物体前车，样例如图2左图所示。无人车所搭载的激光雷达所扫描的到的点云如图2中图所示。无人车与目标物体的路径设置为图2右图所示，两车的行驶状态包括对向行驶、十字路口相遇、双移线工况跟车、十字路口跟车和直线跟车。利用本专利提出方法，以无人车所搭载的激光雷达所扫描到的点云作为输入，可以对目标物体车辆的速度以及位置进行估计。
[0085]
估计结果如图3所示，目标物体中心位置真值和目标物体速度真值由matlab的仿真环境提供，目标物体中心位置估计值和目标物体速度估计值为本方法输出。可以看到，本方法可以准确的估计目标物体车辆的位置和速度。
[0086]
2、在实际环境中进行仿真验证本发明中提出的方法。在校园场景中设置搭载激光雷达的无人观光车，周围动态目标车辆为一辆suv，如图4所示。图中实线示意无人车的行驶路径，虚线示意目标车辆suv的行驶路径。
[0087]
利用两辆车上搭载的高精度的组合惯导系统可以获取suv的真实位置和速度作为真值，如图5中蓝线。使用无人观光车上搭载的激光雷达扫描的点云数据作为输入，利用本专利中的方法，可以对suv的速度进行估计，为图5中红线。可以看到，本方法可以准确的估计目标车辆suv的位置和速度。