CN112379392A - 基于单线激光雷达通过隧道的无人车导航控制方法 - Google Patents

Abstract

本发明公开了一种基于单线激光雷达通过隧道的无人车导航控制方法，包括：1)选用车载单线激光雷达对周围环境进行高频扫描，生成对应的局部二维激光点云图；2)通过累计概率霍夫线变换算法初步提取激光点云中拟合线段的点集合，即线段点集合；3)通过加权线段拟合算法来构建基于线段的二维激光地图；4)在隧道的场景下，通过对二维激光地图进一步的筛选，提取出拟合隧道两边的平行线段，计算两边平行线段的中心平行线，以在中心平行线上同时位于单线激光雷达前方固定距离的点，作为目标追踪点；5)利用pure pursuit算法对目标追踪点进行跟踪控制，便可顺利通过隧道。本发明仅使用单线激光雷达，不需要复杂的硬件设施，便可以安全通过隧道，简单可靠，稳定高效。

Description

基于单线激光雷达通过隧道的无人车导航控制方法

技术领域

本发明涉及无人车驾驶的技术领域，尤其是指一种基于单线激光雷达通过隧道的无人车导航控制方法。

背景技术

伴随着5G技术的出现和应用，无人驾驶技术正在不断的发展和应用，大量的企业和公司开始深耕无人驾驶这块领域，如Google的Waymo，百度的阿波罗无人驾驶项目，特斯拉的辅助驾驶，随着无人驾驶技术不断的发展和进步，L4级的无人驾驶也在特定场景下，变成现实。随着人工智能时代的到来，以自动驾驶为代表的行业被给予了前所未有的关注。但是，无人驾驶依然面临着诸多问题，如无人驾驶系统稳定性不足，硬件价格高昂，无人驾驶的伦理道德问题等。尤其是无人驾驶系统造价高昂，如何降低成本，成为目前各大厂商亟需解决的问题。现有的无人驾驶系统通常使用多传感器进行融合，使用了摄像头，激光雷达，超声波等传感器，来增强系统的稳定性和可靠性，但同时也导致了系统的造价高昂。

无人驾驶包含多个技术领域，如道路边界检测、车辆检测、行人检测，行为决策，车辆控制等。同时包含各种复杂的环境，如高速公路，隧道，大桥等。本发明主要针对各种隧道提出一种基于单线激光雷达通过隧道的无人车导航控制方法。

发明内容

本发明的目的在于克服现有技术的缺点与不足，提出了一种基于单线激光雷达通过隧道的无人车导航控制方法，该方法仅使用单线激光雷达，不需要复杂的硬件设施，便可以安全通过隧道，简单可靠，稳定高效，相比于复杂的路径规划的方法，该方法具有更高的灵活性，不仅能够满足实时的控制需求，而且可以根据路况，对目标追踪点进行调整，来避开前方的障碍物，对于过隧道的场景，该方法有很强的实用性，能够应用于实际场景中，如无人驾驶的货车通过较长的隧道时，可以直接转用单线激光雷达切换到无人驾驶状态。

为实现上述目的，本发明所提供的技术方案为：基于单线激光雷达通过隧道的无人车导航控制方法，包括以下步骤：

1)选用车载单线激光雷达，在行车时，对周围环境进行高频扫描，生成对应的局部二维激光点云图；

2)使用步骤1)中生成的局部二维激光点云图，通过累计概率霍夫线变换算法初步提取激光点云中拟合线段的点集合，即线段点集合；

3)使用步骤2)中的线段点集合，通过加权线段拟合算法来构建基于线段的二维激光地图；

4)在隧道的场景下，通过对步骤3)中生成的二维激光地图进一步的筛选，提取出拟合隧道两边的平行线段，计算两边平行线段的中心平行线，以在中心平行线上同时位于单线激光雷达前方固定距离的点，作为目标追踪点；

5)使用步骤4)得到的目标追踪点，利用pure pursuit算法，对目标追踪点进行跟踪控制，便能够顺利通过隧道。

在步骤1)中，生成局部二维激光点云图，包括以下步骤：

1.1)将车载单线激光雷达安装在无人车上；

1.2)利用单线激光雷达，对周围环境进行实时的扫描，获取生成的局部二维激光点云图。

在步骤2)中，对步骤1)获取得到的局部二维激光点云图，使用累计概率霍夫线变换算法，来提取所有可能的线段点集合，具体过程如下：

2.1)在极坐标系下，将通过坐标点(x₀,y₀)的所有直线表达为r＝x₀cosθ+y₀sinθ，其中，r表示极径，θ表示极角；然后，在直角坐标系中，对于单个点(x₀,y₀)，将θ作为X轴，r作为Y轴，绘制出其对应的正弦曲线；将局部二维激光点云图中的所有点，绘制对应的正弦曲线，如果有多个点对应的正弦曲线存在交点，则说明这些点属于同一条直线，通过设置一个阈值，来判断是否检测到一条直线，而累计概率霍夫线变换算法，则能够输出线段的端点(x₀,y₀,x₁,y₁)，其中，(x₀,y₀)为左端点坐标，(x₁,y₁)为右端点坐标；

2.2)使用步骤2.1)的方法，将局部二维激光点云图中所有可能是线段的点集合进行保存。

在步骤3)中，针对步骤2)提取的线段点集合，加权线段拟合算法的目标是通过使用传感器噪声模型以及极大似然估计方法，来估计一个最优拟合线段的集合，以最小的误差来生成基于线段的二维激光地图，具体情况如下：

所述传感器噪声模型包含距离噪声模型和角度噪声模型，构建的距离噪声模型为

角度噪声模型为

其中，

是距离真实值，ε_d是传感器的随机噪声，

是单线激光雷达的距离测量值，

是角度测量值，

是角度真实值，ε_φ是测量噪声；利用传感器噪声模型能够通过噪声的高斯分布，计算

来近似真实距离测量值

来近似真实角度测量值

使用上述传感器噪声模型对步骤2)提取的线段点集合进行重新估计，减少误差，并获取得到新的线段点集合；

将上述重新估计得到的新的线段点集合，通过极大似然估计方法，利用公式L({δ_k}|L)，来拟合直线L(r,θ,s)，使得误差δ_k最小化，其中，(r,θ)表示极坐标系下的直线，s表示线段上点的加权平均位置，能够有效地表示线段的位置中心，L表示拟合的直线，δ_k表示点集合拟合估计的直线所产生的误差；利用极大似然估计方法，对线段点集合做出最优估计，计算出所有满足条件的线段，生成基于线段的二维激光地图；

对生成的基于线段的二维激光地图，作进一步的线段融合，将可能的线段匹配对转换到参考帧中，利用卡方检验来计算线段匹配对之间的相似性，将相似性超过阈值的线段匹配对进行融合，最后再输出融合后的基于线段的二维激光地图。

在步骤4)中，利用步骤3)所生成的二维激光地图，提取表征隧道的直线，并获取目标追踪点，包括以下步骤：

4.1)根据线段的角度斜率关系，筛除线段与无人车夹角超过规定范围的线段，即|θ|＞θ_max，将满足要求的线段作为备选的一个集合；其中，θ为线段和无人车车体之间的夹角，θ_max为规定范围角度；

4.2)将步骤4.1)中筛选的集合，利用线段距离进一步筛选，由于隧道的左右的封闭性，提取无人车两端距离雷达最远的线段，作为新的线段集合；

4.3)计算步骤4.2)中线段集合的角度关系以及和单线激光雷达的位置关系，将|θ₁-θ₂|＜θ_threshold且在单线激光雷达两侧的线段对提取出来，即表征隧道边缘的平行线段；其中θ₁,θ₂分别表示无人车左右两侧的线段的角度，θ_threshold表示两侧线段的夹角阈值；

4.4)通过步骤4.3)提取出来的表征隧道边缘的平行线段，计算这对平行线段的中心平行线方程，同时选取中心平行线上前方固定距离的一个点作为目标追踪点。

在步骤5)中，根据选取的目标追踪点，通过pure pursuit算法，使无人车按照目标追踪点自动行驶，包括以下步骤：

5.1)无人车的转向模型符合阿克曼转向几何模型，其前轮的平均转向角称为阿克曼角

W_l为车辆的轴距，C_r为给定转向角下，车辆中心遵循的圆的半径；

5.2)在pure pursuit算法中，将阿克曼转向几何模型简化为自行车模型，自行车模型将四轮的模型转换为两轮的模型，并假定在平面上行驶，前轮转角满足几何关系tan(δ)＝V_l/B_r，δ表示前轮的转角，V_l为车辆的轴距，B_r为给定转向角下，后轮遵循的圆的半径；pure pursuit算法以车的后轴为切点，车辆纵向车身为切线，通过控制车辆的前轮的转角，车辆能够沿着经过目标追踪点的圆弧行驶，对应的前轮转角的计算公式由上述公式tan(δ)＝V_l/B_r可进一步推导出前轮转角δ＝argtan(2V_lsin(α)/l_d)，其中，l_d为车辆后轴到目标追踪点的距离，α为当前车身姿态和目标追踪点夹角，通过每次计算前轮转角，获取驾驶角度，从而实现对目标追踪点的追踪；

5.3)将步骤4)中计算得到的目标追踪点，进行坐标系转换，步骤4)计算得到的目标追踪点是激光坐标系下的坐标，而车辆存在于另一个二维的坐标系，在使用purepursuit算法前，将目标追踪点转换到车辆的坐标系下，通过二维标定，得到车辆与激光的坐标系变换矩阵，并保存，在之后的运行中，将目标追踪点直接转换到车辆坐标系下，再使用pure pursuit算法计算前轮的转角，从而计算驾驶角度，实现对目标追踪点的追踪。

本发明与现有技术相比，具有如下优点与有益效果：

1、本发明所需要的硬件资源只使用了单线激光雷达，能够实现无人驾驶状态下，自动安全地通过隧道，相比多线激光雷达方案，成本更低，更可靠高效。

2、本发明充分利用了隧道的建筑结构，巧妙的运用了基于线段的二维激光地图，提取了表征隧道的线段结构，利用几何关系，获取车辆的目标追踪点，使车辆可以稳定的自行通过隧道路面，相比基于视觉的方案，其稳定性更高，效果更好。

3、本发明使用的控制方法，在隧道平面具有良好的效果，pure pursuit算法简单可靠，只需要调整个别参数，就可以对车辆达到很好的控制效果，使车辆能够准确平稳的追踪目标追踪点。

4、本发明获取目标追踪点的思路简洁，并且有很强的灵活性，可以结合目标检测等视觉方法，调整中心平行线位置，通过微调目标追踪点，以达到避障和变换车道等目的。

附图说明

图1为本发明方法的流程图。

图2为基于线段的二维激光地图的示意图。

具体实施方式

下面结合具体实施例对本发明作进一步说明。

如图1所示，本实施例所提供的基于单线激光雷达通过隧道的无人车导航控制方法，包括以下步骤：

1)选用车载单线激光雷达，如思岚RPLIDAR S1和镭神的单线激光雷达，在行车时，对周围环境进行高频扫描，生成对应的局部二维激光点云图。

2)对步骤1)获取得到的局部二维激光点云图，使用累计概率霍夫线变换算法，来提取所有可能的线段点集合，具体过程如下：

2.1)在极坐标系下，可以将通过坐标点(x₀,y₀)的所有直线表达为r＝x₀cosθ+y₀sinθ，其中，r表示极径，θ表示极角；然后，在直角坐标系中，对于单个点(x₀,y₀)，将θ作为X轴，r作为Y轴，可以绘制出其对应的正弦曲线；将局部二维激光点云图中的所有点，绘制对应的正弦曲线，如果有多个点对应的正弦曲线存在交点，则说明这些点属于同一条直线，通过设置阈值threshold＝100，即超过100个点，则判断检测到一条直线，通过累计概率霍夫线变换算法，输出线段的端点(x₀,y₀,x₁,y₁)，其中，(x₀,y₀)为左端点坐标，(x₁,y₁)为右端点坐标；

2.2)使用步骤2.1)的方法，将局部二维激光点云图中所有可能是线段的点集合进行保存。

3)针对步骤2)提取的线段点集合，加权线段拟合算法的目标是通过使用传感器噪声模型以及极大似然估计方法，来估计一个最优拟合线段的集合，以最小的误差来生成基于线段的二维激光地图，具体情况如下：

所述传感器噪声模型包含距离噪声模型和角度噪声模型，距离传感器通常会受到随机噪声的影响，而单线激光雷达的噪声服从高斯分布，所以构建的距离噪声模型为

角度噪声模型为

其中，

是距离真实值，ε_d是传感器的随机噪声，

是单线激光雷达的距离测量值，

是角度测量值，

是角度真实值，ε_φ是测量噪声；利用传感器噪声模型能够通过噪声的高斯分布，计算

来近似真实距离测量值

来近似真实角度测量值

使用上述传感器噪声模型对步骤2)提取的线段点集合进行重新估计，减少误差，并获取得到新的线段点集合。

将上述重新估计得到的新的线段点集合，通过极大似然估计方法，利用公式L({δ_k}|L)，来拟合直线L(r,θ,s)，使得误差δ_k最小化，其中，(r,θ)表示极坐标系下的直线，s表示线段上点的加权平均位置，能够有效地表示线段的位置中心，L表示拟合的直线，δ_k表示点集合拟合估计的直线所产生的误差；利用极大似然估计方法，对线段点集合做出最优估计，计算出所有满足条件的线段，生成基于线段的二维激光地图，如图2所示，圈起来的轨迹就是拟合激光点云图中的线段。

对生成的基于线段的二维激光地图，作进一步的线段融合，将可能的线段匹配对转换到参考帧中，利用卡方检验来计算线段匹配对之间的相似性，将相似性超过阈值的线段匹配对进行融合，最后再输出融合后的基于线段的二维激光地图。

4)在隧道的场景下，通过对步骤3)中生成的二维激光地图进一步的筛选，提取出拟合隧道两边的平行线段，计算两边平行线段的中心平行线，以在中心平行线上同时位于单线激光雷达前方固定距离的点，作为目标追踪点，具体包括以下步骤：

4.1)根据线段的角度斜率关系，筛除线段与无人车夹角超过规定范围的线段，即|θ|＞θ_max，将满足要求的线段作为备选的一个集合；其中，θ为线段和无人车车体之间的夹角，θ_max为规定范围角度，设置为45°；

4.2)将步骤4.1)中筛选的集合，利用线段距离进一步筛选，由于隧道的左右的封闭性，提取无人车两端距离雷达最远的线段，作为新的线段集合；

4.3)计算步骤4.2)中线段集合的角度关系以及和单线激光雷达的位置关系，将|θ₁-θ₂|＜θ_threshold且在单线激光雷达两侧的线段对提取出来，即表征隧道边缘的平行线段；其中θ₁,θ₂分别表示无人车左右两侧的线段的角度，θ_threshold表示两侧线段的夹角阈值，设置为5°；

4.4)通过步骤4.3)提取出来的表征隧道边缘的平行线段，计算这对平行线段的中心平行线方程，同时选取中心平行线上前方固定距离的一个点作为目标追踪点。

5)根据选取的目标追踪点，通过pure pursuit算法，使无人车按照目标追踪点自动行驶，包括以下步骤：

5.1)无人车的转向模型符合阿克曼转向几何模型，其前轮的平均转向角称为阿克曼角

W_l为车辆的轴距，C_r为给定转向角下，车辆中心遵循的圆的半径；

5.2)在pure pursuit算法中，将阿克曼转向几何模型简化为自行车模型，自行车模型将四轮的模型转换为两轮的模型，并假定在平面上行驶，前轮转角满足几何关系tan(δ)＝V_l/B_r，δ表示前轮的转角，V_l为车辆的轴距，B_r为给定转向角下，后轮遵循的圆的半径；pure pursuit算法以车的后轴为切点，车辆纵向车身为切线，通过控制车辆的前轮的转角，车辆能够沿着经过目标追踪点的圆弧行驶，对应的前轮转角的计算公式由上述公式tan(δ)＝V_l/B_r可进一步推导出前轮转角δ＝argtan(2V_lsin(α)/l_d)，其中，l_d为车辆后轴到目标追踪点的距离，α为当前车身姿态和目标追踪点夹角，通过每次计算前轮转角，获取驾驶角度，从而实现对目标追踪点的追踪；

5.3)将步骤4)中计算得到的目标追踪点，进行坐标系转换，步骤4)计算得到的目标追踪点是激光坐标系下的坐标，而车辆存在于另一个二维的坐标系，在使用purepursuit算法前，将目标追踪点转换到车辆的坐标系下，通过二维标定，得到车辆与激光的坐标系变换矩阵，并保存，在之后的运行中，将目标追踪点直接转换到车辆坐标系下，再使用pure pursuit算法计算前轮的转角，从而计算驾驶角度，实现对目标追踪点的追踪。

综上所述，本发明方法是一种特定环境下的导航控制方法，该方法有很强的实用性，能够应用于实际场景中，如无人驾驶的货车通过较长的隧道时，可以直接转用单线激光雷达切换到无人驾驶状态，通过迁移可以切换到类似的场景下，具有较高的实际应用价值，值得推广。

以上所述实施例只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (6)

1.基于单线激光雷达通过隧道的无人车导航控制方法，其特征在于，包括以下步骤：

1)选用车载单线激光雷达，在行车时，对周围环境进行高频扫描，生成对应的局部二维激光点云图；

2)使用步骤1)中生成的局部二维激光点云图，通过累计概率霍夫线变换算法初步提取激光点云中拟合线段的点集合，即线段点集合；

3)使用步骤2)中的线段点集合，通过加权线段拟合算法来构建基于线段的二维激光地图；

4)在隧道的场景下，通过对步骤3)中生成的二维激光地图进一步的筛选，提取出拟合隧道两边的平行线段，计算两边平行线段的中心平行线，以在中心平行线上同时位于单线激光雷达前方固定距离的点，作为目标追踪点；

5)使用步骤4)得到的目标追踪点，利用pure pursuit算法，对目标追踪点进行跟踪控制，便能够顺利通过隧道。

2.根据权利要求1所述的基于单线激光雷达通过隧道的无人车导航控制方法，其特征在于，在步骤1)中，生成局部二维激光点云图，包括以下步骤：

1.1)将车载单线激光雷达安装在无人车上；

1.2)利用单线激光雷达，对周围环境进行实时的扫描，获取生成的局部二维激光点云图。

3.根据权利要求1所述的基于单线激光雷达通过隧道的无人车导航控制方法，其特征在于，在步骤2)中，对步骤1)获取得到的局部二维激光点云图，使用累计概率霍夫线变换算法，来提取所有可能的线段点集合，具体过程如下：

2.1)在极坐标系下，将通过坐标点(x₀,y₀)的所有直线表达为r＝x₀cosθ+y₀sinθ，其中，r表示极径，θ表示极角；然后，在直角坐标系中，对于单个点(x₀,y₀)，将θ作为X轴，r作为Y轴，绘制出其对应的正弦曲线；将局部二维激光点云图中的所有点，绘制对应的正弦曲线，如果有多个点对应的正弦曲线存在交点，则说明这些点属于同一条直线，通过设置一个阈值，来判断是否检测到一条直线，而累计概率霍夫线变换算法，则能够输出线段的端点(x₀,y₀,x₁,y₁)，其中，(x₀,y₀)为左端点坐标，(x₁,y₁)为右端点坐标；

2.2)使用步骤2.1)的方法，将局部二维激光点云图中所有可能是线段的点集合进行保存。

4.根据权利要求1所述的基于单线激光雷达通过隧道的无人车导航控制方法，其特征在于，在步骤3)中，针对步骤2)提取的线段点集合，加权线段拟合算法的目标是通过使用传感器噪声模型以及极大似然估计方法，来估计一个最优拟合线段的集合，以最小的误差来生成基于线段的二维激光地图，具体情况如下：

所述传感器噪声模型包含距离噪声模型和角度噪声模型，构建的距离噪声模型为

角度噪声模型为

其中，

是距离真实值，ε_d是传感器的随机噪声，

是单线激光雷达的距离测量值，

是角度测量值，

是角度真实值，ε_φ是测量噪声；利用传感器噪声模型能够通过噪声的高斯分布，计算

来近似真实距离测量值

来近似真实角度测量值

使用上述传感器噪声模型对步骤2)提取的线段点集合进行重新估计，减少误差，并获取得到新的线段点集合；

将上述重新估计得到的新的线段点集合，通过极大似然估计方法，利用公式L({δ_k}|L)，来拟合直线L(r,θ,s)，使得误差δ_k最小化，其中，(r,θ)表示极坐标系下的直线，s表示线段上点的加权平均位置，能够有效地表示线段的位置中心，L表示拟合的直线，δ_k表示点集合拟合估计的直线所产生的误差；利用极大似然估计方法，对线段点集合做出最优估计，计算出所有满足条件的线段，生成基于线段的二维激光地图；

对生成的基于线段的二维激光地图，作进一步的线段融合，将可能的线段匹配对转换到参考帧中，利用卡方检验来计算线段匹配对之间的相似性，将相似性超过阈值的线段匹配对进行融合，最后再输出融合后的基于线段的二维激光地图。

5.根据权利要求1所述的基于单线激光雷达通过隧道的无人车导航控制方法，其特征在于，在步骤4)中，利用步骤3)所生成的二维激光地图，提取表征隧道的直线，并获取目标追踪点，包括以下步骤：

4.1)根据线段的角度斜率关系，筛除线段与无人车夹角超过规定范围的线段，即|θ|＞θ_max，将满足要求的线段作为备选的一个集合；其中，θ为线段和无人车车体之间的夹角，θ_max为规定范围角度；

4.2)将步骤4.1)中筛选的集合，利用线段距离进一步筛选，由于隧道的左右的封闭性，提取无人车两端距离雷达最远的线段，作为新的线段集合；

4.3)计算步骤4.2)中线段集合的角度关系以及和单线激光雷达的位置关系，将|θ₁-θ₂|＜θ_threshold且在单线激光雷达两侧的线段对提取出来，即表征隧道边缘的平行线段；其中θ₁,θ₂分别表示无人车左右两侧的线段的角度，θ_threshold表示两侧线段的夹角阈值；

4.4)通过步骤4.3)提取出来的表征隧道边缘的平行线段，计算这对平行线段的中心平行线方程，同时选取中心平行线上前方固定距离的一个点作为目标追踪点。

6.根据权利要求1所述的基于单线激光雷达通过隧道的无人车导航控制方法，其特征在于，在步骤5)中，根据选取的目标追踪点，通过pure pursuit算法，使无人车按照目标追踪点自动行驶，包括以下步骤：

5.1)无人车的转向模型符合阿克曼转向几何模型，其前轮的平均转向角称为阿克曼角

W_l为车辆的轴距，C_r为给定转向角下，车辆中心遵循的圆的半径；

5.2)在pure pursuit算法中，将阿克曼转向几何模型简化为自行车模型，自行车模型将四轮的模型转换为两轮的模型，并假定在平面上行驶，前轮转角满足几何关系tan(δ)＝V_l/B_r，δ表示前轮的转角，V_l为车辆的轴距，B_r为给定转向角下，后轮遵循的圆的半径；purepursuit算法以车的后轴为切点，车辆纵向车身为切线，通过控制车辆的前轮的转角，车辆能够沿着经过目标追踪点的圆弧行驶，对应的前轮转角的计算公式由上述公式tan(δ)＝V_l/B_r进一步推导出前轮转角δ＝argtan(2V_lsin(α)/l_d)，其中，l_d为车辆后轴到目标追踪点的距离，α为当前车身姿态和目标追踪点夹角，通过每次计算前轮转角，获取驾驶角度，从而实现对目标追踪点的追踪；

5.3)将步骤4)中计算得到的目标追踪点，进行坐标系转换，步骤4)计算得到的目标追踪点是激光坐标系下的坐标，而车辆存在于另一个二维的坐标系，在使用pure pursuit算法前，将目标追踪点转换到车辆的坐标系下，通过二维标定，得到车辆与激光的坐标系变换矩阵，并保存，在之后的运行中，将目标追踪点直接转换到车辆坐标系下，再使用purepursuit算法计算前轮的转角，从而计算驾驶角度，实现对目标追踪点的追踪。

Images