CN115113206B - 井下有轨车辅助驾驶的行人与障碍物检测方法 - Google Patents

Abstract

本发明涉及目标感知技术领域，尤其涉及一种井下有轨车辅助驾驶的行人与障碍物检测方法，包括：完成相机和激光雷达的时间同步和空间同步；在井下有轨机车上安装相机和激光雷达，在运行环境中实现点云数据、视频采集与预处理；实现视频中的目标检测，获得目标在图像中的坐标信息，对行人与障碍物等目标实现检测与跟踪；按照输入视频的时间序列；利用图像的坐标信息实现点云的粗分割和精细分割；利用K‑means算法对目标点云进行聚类，去除地面点云干扰，则为目标点云；计算每个点云与雷达之间的距离求相应的平均值，则获得目标距离；结合相邻两帧的时间差，计算目标运动速度。

Description

井下有轨车辅助驾驶的行人与障碍物检测方法

技术领域

本发明涉及井下有轨车辅助驾驶中目标感知技术领域，尤其涉及一种井下有轨车辅助驾驶的行人与障碍物检测方法。

背景技术

井下有轨车无人驾驶是井下运输发展趋势，也是实现矿山企业智能化的关键环节。可以极大程度减少人员投入、提高生产效率、降低生产能耗，增强作业安全。无人驾驶的有轨车在行驶过程中需要识别人、机车、石头、遗落工具、木头等其他类型障碍物，且要知悉车前运动目标的位置、距离或速度。

激光雷达可以获得周边环境的点云数据，其中包括三维坐标信息以及激光反射强度4个维度的信息，若仅仅根据周围环境的点云数据，辅助驾驶有轨车仍然无法分辨目标种类与实现目标点云分割。现有技术CN 113569915 A公开了一种基于激光雷达的多策略轨道交通障碍物识别防范，包括，在实际列车运行环境中激光雷达实时扫描列车运行环境数据采集，将获取的点云样本用于建立列车前方障碍物点云数据集；使用VoxelGrid体素滤波方法对点云进行采样滤波；创建新点云容器，遍历过滤后的点云，按照距离远近保存点云；针对室内及室外轨道交通列车运行场景使用不同策略进行地面分割，在分割过程中对轨道区域进行初步提取，最后对提取的轨道区域进行精确限界并用空间曲线拟合；根据不同距离进行多策略障碍物检测，基于距离的多策略进行分区域识别筛选，获取筛选结果。

现有技术的识别检测方法未考虑到矿井环境中光线不均，灰尘大，障碍物种类繁杂的特点，通常采用视觉摄像机、激光雷达等多传感器去感知有轨车前行人与障碍物的信息。为了有轨车辅助驾驶中实现目标点云自动分割，通常采用多传感器信息融合的方式进行目标检测，并获取该目标距离、速度。但是如何在图像目标检测基础上确保有效的目标点云分割，仍然面临着很多问题。

发明内容

本发明所解决的技术问题是提供一种简单且准确度较高的井下行人与障碍物目标检测方法，相比常规的激光雷达检测方法，能够提高目标检测的有效性及准确度。

为达此目的，本发明采用以下技术方案，一种井下有轨车辅助驾驶的行人与障碍物检测方法，包括以下步骤：

S1：对相机和激光雷达这两种类型传感器进行联合标定，使得所述相机采集的视频和所述激光雷达采集的三维点云数据在空间和时间上同步，完成同步后进入步骤S2；

所述联合标定的具体过程如下：

SS1：对相机和激光雷达的硬件配置文件进行修改，将相机和激光雷达数据采集频率设置为相同实现时间同步；

SS2：在有轨车上固定相机和激光雷达传感器，采用张正友标定法标定相机内部参数；利用线与平面的对应关系求解相机与激光雷达的联合外部参数，实现相机和激光雷达的空间同步；

S2：采集运行环境中的视频数据和激光雷达点云数据，分别对所述视频数据与所述激光雷达点云数据进行预处理，完成预处理后进入步骤S3；

S3：通过Labelme对采集的视频数据中行人与障碍物目标进行标注并保存相应的文件，基于采集的视频数据及相应的标注，训练基于Yolo V5的行人与障碍物目标检测模型，模型训练完成后，获取目标类别以及所有目标在图像中的坐标信息，利用投影变换公式获取所有目标的激光雷达点云数据，将所有目标图像信息与点云数据建立一一对应的关系后分别进入步骤S4和步骤S5；

所述目标图像坐标信息为：U_Min、U_Max、V_Min、V_Max，则其中U_Min、U_Max分别为该目标在图像坐标U方向上的最小值和最大值，V_Min、V_Max分别为该目标在图像坐标V方向上的最小值和最大值；

S4：将标注好的视频数据中的同一目标图像进行裁剪，只保留含有目标标定框部分的图像，并将每个目标图像按照不同种类分别放在不同文件夹中；进一步的，基于裁剪后的目标图像，训练基于DeepSORT特定目标的特征提取网络，通过Yolo V5-DeepSORT采用多线程操作实现不同类别多目标跟踪算法；进一步的，利用Yolo V5-DeepSORT对不同目标分别进行检测与跟踪，按照输入视频的时间序列图像，将在前后帧中属于同一目标的分配相同的ID 号后进入S8；

S5：根据所述目标图像坐标信息U_Min、U_Max、V_Min、V_Max，进行目标点云的粗分割，粗分割后的点云实质上缩小了目标点云检测范围，粗分割后的目标点云区域为3D锥体点云，完成后进入步骤S6；

S6：计算所述3D锥体点云中每个点云与所述激光雷达传感器的距离，将获得的距离数据划分为N个区间，并统计每个区间内点云个数，点云个数最多的区间为目标点云，从而实现点云的精细分割；

S7：利用K-means聚类算法对步骤S6中精细分割的点云实现聚类，点云数量较多的类别为目标点云，并计算每个所述目标点云与激光雷达传感器之间的距离，然后对所有距离取平均值，该平均值则为目标点云与雷达之间的距离；

S8：基于步骤S4中的在前后帧中属于同一目标的被分配了相同的ID号以及步骤S7中的平均值，在相邻两帧中对具有相同ID的目标分别进行距离测量，并结合前后两帧数据采集的时间差，完成对目标速度的测量。

进一步的，所述外部参数的求解步骤如下：

1)将标定板放置井下有轨车前方，通过移动标定板，激光雷达和相机分别采集标定板的图像信息和点云信息；

2)提取三维点云数据中标定板平面的法向量和边界；同时提取图像中标定板平面的法向量和边界；根据所述标定板平面的两个法向量和边界，建立激光雷达三维点云和图像中平面与边界的对应关系；

3)根据所述对应关系获得相机和雷达的联合外部参数，包括旋转矩阵Rt和平移矢量T；利用投影变换公式实现相机和激光雷达的空间同步；其中投影变换公式定义为：

μ×[u(i) v(i) 1]^T＝K×[Rt|T]×[x(i) y(i) z(i) 1]^T，i＝0,1,2......K(1)

公式(1)中，K∈R^3×3代表矫正后的相机内参；Rt∈R^3×3代表相机与激光雷达坐标变换的旋转矩阵；T∈R^3×1代表相机与激光雷达坐标变换的平移向量，[Rt|T]表示将旋转矩阵Rt与平移向量T合成为3行4列的矩阵，[x(i) y(i) z(i) 1]^T是激光雷达坐标系下的点云坐标， [u(i) v(i) 1]^T是三维点云经过投影后对应的图像坐标，i为点云的索引，μ为一个常量； K×[Rt|T]是一个3行4列的矩阵，若K×[Rt|T]表达式为：

其中A₀、A₁、A₂、A₃、B₀、B₁、B₂、B₃、C₀、C₁、C₂、C₃分别表示该矩阵中元素；

基于公式(1)和公式(2)进一步可得：

进一步的，基于目标图像坐标的约束条件实现目标点云的粗分割，约束条件如下：

其中u(i)表示第i个点云投影在图像坐标中U方向的坐标值，v(i)表示第i个点云投影在图像坐标中V方向的坐标值。

进一步的，所述目标点云精细分割的条件定义为：

公式(5)中，粗分割后的点云被划分为N个区间，N_n表示第n个区间，Num_n表示第n个区间内的点云数量；点云个数最多的区间则对应目标点云范围，结合公式(4)与式公(5)即可以获得精细分割的点云。

进一步的，3D锥体点云中每个点云与雷达的距离计算公式如下：

其中d(i)表示第i个点云与雷达之间的距离。

进一步的，经过K-means聚类去除噪声点云后，若ID号相同的目标点云个数为m，将该目标点云可记为{[x_sn(1),y_sn(1),z_sn(1)],…[x_sn(m),y_sn(m),z_sn(m)]}，则有轨机车与目标距离计算公式如下：

其中sn为目标的ID号；若固定激光雷达，车体不发生移动，则利用前后两帧数据中目标与雷达的距离来计算该目标的速度，计算公式如下：

v＝(D_sn(t-1)-D_sn(t))/Δt (8)

公式(8)中，D_sn(t-1)与D_sn(t)分别是第t-1帧与第t帧图像中目标与雷达的距离，Δt是两帧之间的时间间隔；

若激光雷达固定在车体上与地面发生相对运动，则结合车体当前速度v₀以及前后两帧数据中同一目标与雷达的距离来计算目标的速度，计算公式如下：

v＝(D_sn(t-1)-D_sn(t)-v₀*Δt)/Δt (9)

进一步的，所述激光雷达垂直扫描角度范围为-15°～+15°，水平扫描角度范围为360°。

本发明的有益效果：本发明的一种井下辅助驾驶有轨车的行人与障碍物检测方法，包括：利用相机和激光雷达的硬件配置文件，完成相机和激光雷达的时间近似同步；对相机和激光雷达进行联合标定实现两种传感器的空间同步；在井下有轨机车上安装相机和激光雷达，在运行环境中实现点云数据、视频采集与预处理；实现图像中的目标检测，获得目标在图像中的坐标信息，并利用Yolo V5-DeepSORT对行人与障碍物等目标实现检测与跟踪；按照输入视频的时间序列图像，给前后两帧中同一目标分配相同的ID号；利用图像的坐标信息实现点云的粗分割，获得3D锥体点云；分析3D锥体点云的点云特征，计算3D锥体点云中每个点云与雷达的距离，将距离划分为N个区间，采用遍历的方法统计点云个数，点云个数最多的区间则对应目标点云范围，从而实现点云的精细分割；利用K-means算法对目标点云进行聚类，点云数量最多的类别为目标点云；计算每个点云与雷达之间的距离并对所有距离取平均值，该平均值为目标距离。最后在相邻两帧中对具有同一ID号的目标分别进行距离测量，并结合相邻两帧的时间差，完成对目标速度的测量。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图作简单地介绍。显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的的检测方法流程图；

图2为本发明的检测方法的原始点云图；

图3为本发明的3D锥体点云图；

图4为本发明的目标点云精细分割结果图；

图5为本发明的图像目标检测结果图。

具体实施方式

下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本专利的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

如图1至图5所示，一种井下辅助驾驶有轨车的行人与障碍物检测方法，包括如下步骤：

为了融合相机采集的视频和激光雷达采集的点云数据(参见附图2的原始点云)，首先要完成激光雷达与相机之间在时间与空间上的同步。由于两种不同类型传感器，工作频率和视野范围不同，在有轨机车将两种类型传感器固定安装好之后，实现相机与激光雷达联合标定，从而保证视频数据和三维点云数据在空间和时间上保持同步。

修改相机和激光雷达的硬件配置文件，将相机和激光雷达采集频率设置为相同数值，实现时间上的同步。

所述联合标定的具体过程如下：

SS1：对相机和激光雷达的硬件配置文件进行修改，将相机和激光雷达数据采集频率设置为相同实现时间同步；

SS2：在有轨车上固定相机和激光雷达传感器，采用张正友标定法标定相机内部参数；利用线与平面的对应关系求解相机与激光雷达的联合外部参数，实现相机和激光雷达的空间同步；所述外部参数的求解步骤如下：

1)将标定板放置井下有轨车前方，通过移动标定板，激光雷达和相机分别采集标定板的图像信息和点云信息；

2)提取三维点云数据中标定板平面的法向量和边界；同时提取图像中标定板平面的法向量和边界；根据所述标定板平面的两个法向量和边界，建立激光雷达三维点云和图像中平面与边界的对应关系；

3)根据所述对应关系获得相机和雷达的联合外部参数，包括旋转矩阵Rt和平移矢量T；利用投影变换公式实现相机和激光雷达的空间同步；其中投影变换公式定义为：

μ×[u(i) v(i) 1]^T＝K×[Rt|T]×[x(i) y(i) z(i) 1]^T，i＝0,1,2......K(1)

公式(1)中，K∈R^3×3代表矫正后的相机内参；Rt∈R^3×3代表相机与激光雷达坐标变换的旋转矩阵；T∈R^3×1代表相机与激光雷达坐标变换的平移向量，[Rt|T]表示将旋转矩阵Rt与平移向量T合成为3行4列的矩阵，[x(i) y(i) z(i) 1]^T是激光雷达坐标系下的点云坐标，[u(i) v(i) 1]^T是三维点云经过投影后对应的图像坐标，i为点云的索引，μ为一个常量；K×[Rt|T]是一个3行4列的矩阵，若K×[Rt|T]表达式为：

其中A₀、A₁、A₂、A₃、B₀、B₁、B₂、B₃、C₀、C₁、C₂、C₃分别表示该矩阵中元素；

基于公式(1)和公式(2)进一步可得：

在井下有轨机车驾驶运行场景中，启动相机和激光雷达，采集周围环境的视频和激光雷达的点云数据，对视频与点云数据进行预处理，图像预处理包括数据清洗与标注；点云的预处理包括：删除不在图像坐标边界范围内的点云数据。

基于Labelme对视频中目标图像进行标注并建立相应的标注文件，构造目标检测的训练数据集。基于Yolo V5完成视频中目标检测模型的训练与测试。通过模型测试，可以获得视频中目标图像类别以及相应目标在图像中的坐标信息，目标检测结果参见如附图5的图像目标检测结果图，然后利用投影变换公式，激光雷达采集的点云数据，投影到图像坐标系中，从而获得不同目标在二维空间中的点云范围。假设Yolo V5网络输出的某一目标的坐标信息为：U_Min、U_Max、V_Min、V_Max，其中U_Min、U_Max分别为目标在图像坐标U方向上的最小值和最大值，V_Min、V_Max分别为目标在图像坐标V方向上的最小值和最大值。将所有目标图像信息与点云数据建立一一对应的关系后分别进行目标图像的裁剪以及目标点云的粗分割，

所述目标图像的裁剪是将标注好的视频数据中的同一目标图像进行裁剪，只保留含有目标标定框部分的图像，并将每个目标图像按照不同种类分别放在不同文件夹中；进一步的，基于裁剪后的目标图像，训练基于DeepSORT特定目标的特征提取网络，通过YoloV5-DeepSORT采用多线程操作实现不同类别多目标跟踪算法；进一步的，利用Yolo V5-DeepSORT对不同目标分别进行检测与跟踪，按照输入视频的时间序列图像。

基于目标在图像中的坐标约束条件实现目标点云的粗分割，约束条件如下：

公式(4)中，u(i)表示第i个点云投影在图像坐标中U方向的坐标，v(i)表示第i个点云投影在图像坐标中在V方向的坐标。

所述目标点云的粗分割是根据目标图像的坐标信息U_Min、U_Max、V_Min、V_Max，实现目标点云的粗分割；粗分割后的点云实质上大大缩小了目标点云的搜索范围；同时，因采用的激光雷达垂直扫描角度范围为-15°～+15°，水平扫描角度范围为360°，粗分割后的目标点云区域如同锥体状，被称为3D锥体点云，参见附图3。

井下工作环境恶劣、灰尘较大，基于获得的3D锥体点云数据虽已大大缩小了目标点云范围，但仍然包含大量噪声干扰；根据观察可知，属于同一目标的点云与雷达的距离大致相同，因此距离变化范围在一个极其有限区间内；然而，不同目标与雷达距离不同，因此距离变化范围差异很大。根据这一特点，计算3D锥体点云中每个点云与雷达的距离，并将距离划分为N个区间，统计每个区间内点云的个数。根据这一特点，目标点云精细分割的条件可定义为：

公式(5)中，N表示划分区间的个数，N_n表示第n个区间，Num_n表示第n个区间点云数量。由于3D锥体点云中绝大部分为目标点云，因此点云个数最多的区间则为目标点云的精细分割结果。结合式(4)、式(5)、式(6)即可得到目标点云的精细分割结果，参见附图4。

计算3D锥体点云中每个点云与雷达的距离，计算公式如下：

其中d(i)表示第i个点云与雷达之间的距离

由于目标有可能与地面接触，则精细分割的目标点云中会存在少量的地面点云，为了减少地面点云对目标测距的影响，利用K-means聚类算法对精细分割的目标点云聚类，点云数量较多的类别为目标点云；然后，计算目标点云中每个点云与雷达之间的距离，并对所有距离取平均值，该平均值为目标距离。经过K-means聚类后，若ID号同为sn的目标点云个数为m，将该目标点云可记为{[x_sn(1),y_sn(1),z_sn(1)],…[x_sn(m),y_sn(m),z_sn(m)]}，则有轨机车与目标距离计算公式如下：

若激光雷达固定，车体不发生移动，则利用前后两帧数据中目标与雷达的距离来计算该目标的速度，计算公式如下：

v＝(D_sn(t-1)-D_sn(t))/Δt (8)

其中D_sn(t-1)与D_sn(t)分别是第t帧与第t-1帧图像中目标(ID编号同为sn)与雷达的距离，Δt是两帧之间的时间间隔。

若激光雷达固定在车体上与地面发生相对运动，同样可以结合车体当前速度v₀以及前后两帧数据中同一目标与雷达的距离来计算目标的速度，计算公式如下：

v＝(D_sn(t-1)-D_sn(t)-v₀*Δt)/Δt (9)

需要声明的是，上述具体实施方式仅仅为本发明的较佳实施例及所运用技术原理。本领域技术人员应该明白，还可以对本发明做各种修改、等同替换、变化等等。但是，这些变换只要未背离本发明的精神，都应在本发明的保护范围之内。另外，本申请说明书和权利要求书所使用的一些术语并不是限制，仅仅是为了便于描述。

Claims (7)

1.一种井下有轨车辅助驾驶的行人与障碍物检测方法，其特征在于，包括以下步骤：

S1：对相机和激光雷达这两种类型传感器进行联合标定，使得所述相机采集的视频和所述激光雷达采集的三维点云数据在空间和时间上同步，完成同步后进入步骤S2；

所述联合标定的具体过程如下：

SS1：对相机和激光雷达的硬件配置文件进行修改，将相机和激光雷达数据采集频率设置为相同实现时间同步；

SS2：在有轨车上固定相机和激光雷达传感器，采用张正友标定法标定相机内部参数；利用线与平面的对应关系求解相机与激光雷达的联合外部参数，实现相机和激光雷达的空间同步；

S2：采集运行环境中的视频数据和激光雷达点云数据，分别对所述视频数据与所述激光雷达点云数据进行预处理，完成预处理后进入步骤S3；

S3：通过Labelme对采集的视频数据中行人与障碍物目标进行标注并保存相应的文件，基于采集的视频数据及相应的标注，训练基于Yolo V5的行人与障碍物目标检测模型，模型训练完成后，获取目标类别以及所有目标在图像中的坐标信息，利用投影变换公式获取所有目标的激光雷达点云数据，将所有目标图像信息与点云数据建立一一对应的关系后分别进入步骤S4和步骤S5；

所述目标图像坐标信息为：U_Min、U_Max、V_Min、V_Max，则其中U_Min、U_Max分别为该目标在图像坐标U方向上的最小值和最大值，V_Min、V_Max分别为该目标在图像坐标V方向上的最小值和最大值；

S4：将标注好的视频数据中的同一目标图像进行裁剪，只保留含有目标标定框部分的图像，并将每个目标图像按照不同种类分别放在不同文件夹中；进一步的，基于裁剪后的目标图像，训练基于DeepSORT特定目标的特征提取网络，通过Yolo V5-DeepSORT采用多线程操作实现不同类别多目标跟踪算法；进一步的，利用Yolo V5-DeepSORT对不同目标分别进行检测与跟踪，按照输入视频的时间序列图像，将在前后帧中属于同一目标的分配相同的ID号后进入S8；

S5：根据所述目标图像坐标信息U_Min、U_Max、V_Min、V_Max，进行目标点云的粗分割，粗分割后的点云实质上缩小了目标点云检测范围，粗分割后的目标点云区域为3D锥体点云，完成后进入步骤S6；

S6：计算所述3D锥体点云中每个点云与所述激光雷达传感器的距离，将获得的距离数据划分为N个区间，并统计每个区间内点云个数，点云个数最多的区间为目标点云，从而实现点云的精细分割；

S7：利用K-means聚类算法对步骤S6中精细分割的点云实现聚类，点云数量较多的类别为目标点云，并计算每个所述目标点云与激光雷达传感器之间的距离，然后对所有距离取平均值，该平均值则为目标点云与雷达之间的距离；

S8：基于步骤S4中的在前后帧中属于同一目标的被分配了相同的ID号以及步骤S7中的平均值，在相邻两帧中对具有相同ID的目标分别进行距离测量，并结合前后两帧数据采集的时间差，完成对目标速度的测量。

2.根据权利要求1所述的井下有轨车辅助驾驶的行人与障碍物检测方法，其特征在于，所述外部参数的求解步骤如下：

1)将标定板放置井下有轨车前方，通过移动标定板，激光雷达和相机分别采集标定板的图像信息和点云信息；

2)提取三维点云数据中标定板平面的法向量和边界；同时提取图像中标定板平面的法向量和边界；根据所述标定板平面的两个法向量和边界，建立激光雷达三维点云和图像中平面与边界的对应关系；

3)根据所述对应关系获得相机和雷达的联合外部参数，包括旋转矩阵Rt和平移矢量T；利用投影变换公式实现相机和激光雷达的空间同步；其中投影变换公式定义为：

μ×[u(i) v(i) 1]^T＝K×[Rt|T]×[x(i) y(i) z(i) 1]^T，i＝0,1,2......K (1)

公式(1)中，K∈R^3×3代表矫正后的相机内参；Rt∈R^3×3代表相机与激光雷达坐标变换的旋转矩阵；T∈R^3×1代表相机与激光雷达坐标变换的平移向量，[Rt|T]表示将旋转矩阵Rt与平移向量T合成为3行4列的矩阵，[x(i) y(i) z(i) 1]^T是激光雷达坐标系下的点云坐标，[u(i) v(i) 1]^T是三维点云经过投影后对应的图像坐标，i为点云的索引，μ为一个常量；K×[Rt|T]是一个3行4列的矩阵，若K×[Rt|T]表达式为：

其中A₀、A₁、A₂、A₃、B₀、B₁、B₂、B₃、C₀、C₁、C₂、C₃分别表示该矩阵中元素；

基于公式(1)和公式(2)进一步可得：

3.根据权利要求1或2所述的井下有轨车辅助驾驶的行人与障碍物检测方法，其特征在于，基于目标图像坐标的约束条件实现目标点云的粗分割，约束条件如下：

其中u(i)表示第i个点云投影在图像坐标中U方向的坐标值，v(i)表示第i个点云投影在图像坐标中V方向的坐标值。

4.根据权利要求3所述的井下有轨车辅助驾驶的行人与障碍物检测方法，其特征在于，所述目标点云精细分割的条件定义为：

公式(5)中，粗分割后的点云被划分为N个区间，N_n表示第n个区间，Num_n表示第n个区间内的点云数量；点云个数最多的区间则对应目标点云范围，结合公式(4)与公式(5)即可以获得精细分割的点云。

5.根据权利要求4所述的井下有轨车辅助驾驶的行人与障碍物检测方法，其特征在于，3D锥体点云中每个点云与雷达的距离计算公式如下：

其中d(i)表示第i个点云与雷达之间的距离。

6.根据权利要求5所述的井下有轨车辅助驾驶的行人与障碍物检测方法，其特征在于，经过K-means聚类去除噪声点云后，若ID号相同的目标点云个数为m，将该目标点云可记为{[x_sn(1),y_sn(1),z_sn(1)]，…，[x_sn(m),y_sn(m),z_sn(m)]}，则有轨机车与目标距离计算公式如下：

其中sn为目标的ID号；若固定激光雷达，车体不发生移动，则利用前后两帧数据中目标与雷达的距离来计算该目标的速度，计算公式如下：

v＝(D_sn(t-1)-D_sn(t))/Δt (8)

公式(8)中，D_sn(t-1)与D_sn(t)分别是第t-1帧与第t帧图像中目标与雷达的距离，Δt是两帧之间的时间间隔；

若激光雷达固定在车体上与地面发生相对运动，则结合车体当前速度v₀以及前后两帧数据中同一目标与雷达的距离来计算目标的速度，计算公式如下：

v＝(D_sn(t-1)-D_sn(t)-v₀*Δt)/Δt (9)。

7.根据权利要求6所述的井下有轨车辅助驾驶的行人与障碍物检测方法，其特征在于，所述激光雷达垂直扫描角度范围为-15°～+15°，水平扫描角度范围为360°。