CN113701780A - 基于a星算法的实时避障规划方法 - Google Patents

Abstract

本发明公开了一种基于A星算法的实时避障规划方法，包括：步骤一，在车辆的实行行进中，通过对实时检测到的障碍物设定尺寸的方式，对人形障碍物和非人形障碍物进行分类；步骤二，基于设定的障碍物尺寸，通过搜索算法计算绕障碍物的目标点；步骤三，基于目标点，采用A星算法进行实时绕障路径的初始规划。本发明提供一种基于A星算法的实时避障规划方法，通过在主流的基于图搜索的路径规划A星算法的基础上进行改进，能够解决传统避障规划算法耗费计算资源和耗时以及不易实施等问题，能够稳定的规划一条避障路径，解决了当前大量农机设备无法实时避障的问题。

Description

基于A星算法的实时避障规划方法

技术领域

本发明涉及一种无人驾驶路径规划领域。更具体地说，本发明涉及一种用在大型农机无人驾驶情况下使用的基于A星算法的实时避障规划方法。

背景技术

无人驾驶领域中的路径规划是指给无人车辆规划一条安全稳定的路径，即给定起始点和目标点，规划一条符合车辆约束的安全行进路径。同时为了保证车辆的行驶安全性和乘坐的舒适性，采用优化和平滑算法进行路径的优化，使无人车辆能够沿着规划路径预期连续性行进。

目前，熟知的A星路径规划算法存在以下缺点：(1)仅仅适用于已知整体环境，即静态地图下进行规划；(2)传统A星规划算法未用于实时避障；(3)存在规划路径曲折，超过最大转向角度问题；(4)由于A星路径规划算法属于全局路径规划，故需预先给定目标点，然而实时避障规划无法手动给定目标点，故需要采用搜索算法进行目标点计算后，给出目标点。同时A星路径规划算法存在以下优点：(1)算法本身含有启发式部分，能快速收敛；(2)相比于其他传统算法其规划路径较优；(3)其基于图搜索的特性更适用于算法实际的应用情况。

同时因为传统实时避障规划算法由于存在参数复杂性以及实时规划存在不稳定性等问题，在大型农机上应用容易发生不同于小型无人驾驶车辆上的情况，比如发生碰撞，侧翻等，造成较大经济财产损失，故不适用于大型农业生产设备。

发明内容

本发明的一个目的是解决至少上述问题和/或缺陷，并提供至少后面将说明的优点。

为了实现根据本发明的这些目的和其它优点，提供了一种基于A星算法的实时避障规划方法，包括：

步骤一，在车辆的实行行进中，通过对实时检测到的障碍物设定尺寸的方式，对人形障碍物和非人形障碍物进行分类；

步骤二，基于设定的障碍物尺寸，通过搜索算法计算绕障碍物的目标点；

步骤三，基于目标点，采用A星算法进行实时绕障路径的初始规划。

优选的是，还包括：

步骤四，对初始规划路径的节点角度进行计算，根据计算结构判断节点角度值是否超过车辆的最大转向角度

如判断结果为超过，则采用内敛力方式对点位进行初次更新；

步骤五：对初次更新后的点位进行判断，以判断该点位是否超出最大转向角度，若超出则重复步骤四，以得到有效节点；

步骤六：对有效节点进行多次B样条优化曲线进行路径点拟合，并将拟合点位发送给路径跟踪控制模块，实现路径跟踪。

优选的是，在步骤一中，包括：

S10、所述车辆基于其上搭载的激光测距仪，获取当前障碍物与车辆之间的实时距离dist_free，当dist_free≥dist_threshold则认为其满足实时避障规划的条件；

S11、当车辆运动到障碍物前时，通过车辆上的传感器进行障碍物地图映射，获取当前障碍物平行于车头平面的最长长度L_max，通过不等式一L_max≤L_threshold进行障碍物大小阈值判断，在不等式一判断为真时认定障碍物为人，设定障碍物长度为L_obs＝2(m)，否则设定障碍物长度为L_obs＝5(m)。

优选的是，在步骤二中，采用暴力搜索全局GPS路径对路径终点GPS选取，此终点的选取方式为：

S20、基于实时差分定位RTK获取当前车辆所处的GPS节点；

S21、在全局GPS路径中获取与GPS节点匹配的最近GPS邻近节点g_i；

S22、以g_i作为搜索起始点，以叠加下一个GPS点g_i+1之间的距离dist(i，i+1)；

S23、在距离dist(i，i+1)满足不等式二中的点位时，等到搜索的目标GPS点；

其中，在S22中，dist(i，i+1)＝||g_i，g_i+1||；

在S23中，不等式二配置为：

所述n为第一个满足不等式二的求解值。

优选的是，在步骤四-五中，根据搜索起点与目标GPS点之间间距离的平方square_i，i+1有公式一：

square_i，i+1＝||x_i，x_i+1||*||x_i，x_i+1||；

则当前A星规划节点X_i的角度

计算公式二为：

通过判断当前节点

是否超过最大转向角度

若超过最大转向角度

则通过公式三将当前节点X_i为X_new：

其中，两控制点间的向量为

a、k均为参数系数，在节点角度超过最大转向角度1.3倍时，则将a、k的值设置为大于1，否则均将值设定为1。

优选的是，在步骤六中，根据三次B样条曲线拟合公式四：

其中，公式四中的基函数为公式五：

公式四中的X_i为初始规划路径中的每个点位，基于公式四、公式五得到拟合曲线点公式六：

公式(8)中t∈[0，1]，其中X(t)为四个控制点根据三次B样条曲线拟合得到的点集。

本发明至少包括以下有益效果：本发明在主流的基于图搜索的路径规划A星算法的基础上进行改进，能够解决传统避障规划算法耗费计算资源和耗时以及不易实施等问题，能够稳定的规划一条避障路径，解决了当前大量农机设备无法实时避障的问题。

本发明能够在运用在大型农机上解决未知障碍物避障路径规划的问题，同时也能够快速的实现和实施，是一种高效简单容易实施的避障方式。

本发明的其它优点、目标和特征将部分通过下面的说明体现，部分还将通过对本发明的研究和实践而为本领域的技术人员所理解。

附图说明

图1为本发明的一个实施例中改进A星平滑避障算法整体框架图；

图2为图1左半部分的放大示意图；

图3为图1右半部分的放大示意图；

图4为本发明避障算法流程示意图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

本发明为了克服在实时避障规划过程中无法整体感知障碍物的整体形态大小而无法进行绕障终点选取问题，采用一种检测位置和估算障碍物整体长度的方式，障碍物的整体形态大小可根据当前特定场景进行相应设定，能够快速容易的解决当前无法找到规划终点的问题，在实际的农机上测试效果容易实施和有效，并且为了让改进算法能够规划一条平滑的行进路径，采用角度检测方式进行路径点角度检测，同时采用B样条曲线进行路径平滑优化。

具体来说，本发明针对基本场景下的障碍物进行分类处理，设定其相应的障碍物最大尺寸，根据前方距离障碍物和障碍物最大尺寸即可选取绕障规划终点。而大型农机进行实验时，由于道路环境单一，障碍物分类主要为非人障碍物和行人障碍物，采用检测尺寸与设定阈值进行比较，自动进行障碍物尺寸设定，同时获取当前点位距离障碍物距离是否大于无碰撞距离，即可快速规划一条满足条件的绕障路径。

本发明基于改进A星的平滑避障算法整体框架图如图1-3，其算法处理流程如图4，主要的技术原理包括如下步骤：

步骤一：利用激光测距的准确性，获取到当前障碍物和车辆的距离dist_free，此刻存在情况：可能障碍物距离车辆太近，但是车辆属于阿克曼转向模型，故不能原地旋转，所以必须满足dist_free≥dist_threshold，dist_threshold是当前车辆能够避障的最小距离，因为当障碍物距离车辆太近时，无法进行避障，故当满足条件时即可进行避障规划，如果条件不满足时，则车辆则无法进行避障规划，车辆通过控制模块会自动刹车报警。

步骤二：为了解决在实际的农机跟踪全局路径时的实时绕障问题，同时考虑整体路面宽度，采用两种固定的障碍物检测模型进行避障规划，非人障碍物和人，分别设置如下：

表1障碍物类型设置表

障碍物类型	移动人形障碍物	非人形障碍物
			障碍物长度(m)	2	5

据上表可知，当检测到障碍物属于人时，障碍物长度L_obs＝2(m)；当前方障碍物属于非人时，障碍物长度L_obs＝5(m)。具体来说，在无人驾驶农机上装配激光雷达和相机，前提是已经进行联合标定，相机采用深度学习算法进行人体识别。当车辆运动到障碍物前时，传感器首先进行障碍物地图映射，获取当前障碍物平行于车头平面的最长长度L_max，进行障碍物大小阈值判断L_max≤L_threshold，L_threshold为当前判断障碍物为人或者非人的中间阈值，当小于该阈值时，障碍物判定为人，大于时判定为非人障碍物，在实际操作中，当该不等式判断为真时设置为L_obs＝2(m)，否则则设定为L_obs＝5(m)。

步骤三：采用暴力搜索全局GPS路径进行路径终点GPS选取，此终点的选取方式为：由于当前车辆所处GPS点可以根据RTK获取得到，可以在全局GPS路径中快速确定当前位置所处在全局GPS路径中的最近邻点，即搜索起始点。起始点开始叠加下一GPS点的距离，当找到距离满足不等式二的点位时，即为搜索的目标GPS点，自动发送目标点，开始调用A星算法进行绕障碍物规划。具体来说，根据当前激光雷达检测到的障碍物与当前农机的距离dist_free，以及障碍物的整体长度设置。然后在当前位置处用邻近节点搜索的方式进行终点GPS搜索，在实际应用中，对障碍物宽度的识别以及以最贴近的距离避让障碍物属于A星算法的现有技术，故在此不再叙述：

当前全局路径GPS节点g_i和下一个GPS点g_i+1间的距离：

dist(i，i+1)＝||g_i，g_i+1||；

根据不等式二：

其中，n为第一个满足不等式(2)的求解值，也即为当前可绕障目标点位置信息，根据当前位置即可计算出该目标点的位姿。当该值计算得到之后，即可通过已知的GPS路径获取该点的位置，将其设定为绕障规划的终点。采用A星算法进行图搜索规划后，获得初始路径。

步骤四：在通过A星初始化规划算法进行算法优化，通过角度算法优化当前超过最大转向角度的位姿，即计算初始规划节点角度，判断是否超过最大转向角度

当超过最大转向角度时则需要对点位进行更新，采用内敛力方式进行更新，具体来说，根据两点间距离的平方square_i，i+1有：

square_i，i+1＝||x_i，x_i+1||*||x_i，x_i+1||；

当前A星规划节点X_i角度计算公式为：

通过判断当前节点

是否超过最大转向角度

若超过最大转向角度

则更新当前节点：

其中两控制点间的向量为

更新当前节点X_i为X_new：

其中a、k均为参数系数，可进行调节当前节点的更新程度，当节点角度超过最大转向角度1.3倍时，则可进行大于1的设置，否则均设定为1。

更新完成后判断该点是否满足最大转向角度，不满足则继续更新该点。

步骤五：当更新完节点后进行B样条曲线拟合路径点，使路径能够更加平滑，让车辆在跟踪路径时能够进行连续性跟踪，采用B样条优化算法对搜索路径进行平滑处理，满足车辆运动学规划，即在车辆不停止的情况下进行连续转向。

根据三次B样条曲线拟合公式：

其中基函数为：

其中X_i为初始规划路径中每个点位。

根据三次B样条曲线拟合公式和基函数公式得拟合曲线点公式：

公式(8)中t∈[0，1]，其中X(t)为四个控制点根据三次B样条曲线拟合得到的点集。

步骤六：通过拟合点位发送给路径跟踪控制模块，进行路径跟踪。

以上方案只是一种较佳实例的说明，但并不局限于此。在实施本发明时，可以根据使用者需求进行适当的替换和/或修改。

这里说明的设备数量和处理规模是用来简化本发明的说明的。对本发明的应用、修改和变化对本领域的技术人员来说是显而易见的。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用。它完全可以被适用于各种适合本发明的领域。对于熟悉本领域的人员而言，可容易地实现另外的修改。因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

Claims (6)

1.一种基于A星算法的实时避障规划方法，其特征在于，包括：

步骤一，在车辆的实行行进中，通过对实时检测到的障碍物设定尺寸的方式，对人形障碍物和非人形障碍物进行分类；

步骤二，基于设定的障碍物尺寸，通过搜索算法计算绕障碍物的目标点；

步骤三，基于目标点，采用A星算法进行实时绕障路径的初始规划。

2.如权利要求1所述的基于A星算法的实时避障规划方法，其特征在于，还包括：

步骤四，对初始规划路径的节点角度进行计算，根据计算结构判断节点角度值是否超过车辆的最大转向角度

如判断结果为超过，则采用内敛力方式对点位进行初次更新；

步骤五：对初次更新后的点位进行判断，以判断该点位是否超出最大转向角度，若超出则重复步骤四，以得到有效节点；

步骤六：对有效节点进行多次B样条优化曲线进行路径点拟合，并将拟合点位发送给路径跟踪控制模块，实现路径跟踪。

3.如权利要求1所述的基于A星算法的实时避障规划方法，其特征在于，在步骤一中，包括：

S10、所述车辆基于其上搭载的激光测距仪，获取当前障碍物与车辆之间的实时距离dist_free，当dist_free≥dist_threshold则认为其满足实时避障规划的条件；

S11、当车辆运动到障碍物前时，通过车辆上的传感器进行障碍物地图映射，获取当前障碍物平行于车头平面的最长长度L_max，通过不等式一L_max≤L_threshold进行障碍物大小阈值判断，在不等式一判断为真时认定障碍物为人，设定障碍物长度为L_obs＝2(m)，否则设定障碍物长度为L_obs＝5(m)。

4.如权利要求1所述的基于A星算法的实时避障规划方法，其特征在于，在步骤二中，采用暴力搜索全局GPS路径对路径终点GPS选取，此终点的选取方式为：

S20、基于实时差分定位RTK获取当前车辆所处的GPS节点；

S21、在全局GPS路径中获取与GPS节点匹配的最近GPS邻近节点g_i；

S22、以g_i作为搜索起始点，以叠加下一个GPS点g_i+1之间的距离dist(i，i+1)；

S23、在距离dist(i，i+1)满足不等式二中的点位时，等到搜索的目标GPS点；

其中，在S22中，dist(i，i+1)＝||g_i，g_i+1||；

在S23中，不等式二配置为：

所述n为第一个满足不等式二的求解值。

5.如权利要求2所述的基于A星算法的实时避障规划方法，其特征在于，在步骤四-五中，根据搜索起点与目标GPS点之间间距离的平方square_i，i+1有公式一：

则当前A星规划节点X_i的角度

计算公式二为：

通过判断当前节点

是否超过最大转向角度

若超过最大转向角度

则通过公式三将当前节点X_i为X_new：

其中，两控制点间的向量为

a、k均为参数系数，在节点角度超过最大转向角度1.3倍时，则将a、k的值设置为大于1，否则均将值设定为1。

6.如权利要求2所述的基于A星算法的实时避障规划方法，其特征在于，在步骤六中，根据三次B样条曲线拟合公式四：

其中，公式四中的基函数为公式五：

公式四中的X_i为初始规划路径中的每个点位，基于公式四、公式五得到拟合曲线点公式六：

公式(8)中t∈[0，1],其中X(t)为四个控制点根据三次B样条曲线拟合得到的点集。

Images