CN110531770A - 一种基于改进的rrt路径规划方法和系统 - Google Patents

Abstract

本发明公开了一种基于改进的RRT路径规划方法和系统，其特征在于：包括以下步骤，避障模块通过快速扩展随机树生成随机点构建随机树，找出路径；分段拟合模块对所述快速扩展随机树找出的路径进行分段拟合求出路径控制点；信息提取模块以所述路径控制点为圆心和可调半径作圆，制作掩模mask，提取所述mask掩模的ROI区域，对ROI区域运用形态学图像处理，检测障碍物的轮廓，计算各个障碍物轮廓点到控制点的最小距离并提取该障碍物点的位置信息。本发明的有益效果：能够生成一条可执行的、避障效果好的、路径控制点稀疏的、周围障碍物信息可知的一条完整路径，能够更好的应用于无人车自主导航决策模块。

Description

一种基于改进的RRT路径规划方法和系统

技术领域

本发明涉及自动驾驶的技术领域，尤其涉及一种感知决策基于改进的RRT路径规划方法和系统。

背景技术

近年来路径规划是运动规划的主要研究内容之一。运动规划由路径规划和轨迹规划组成，连接起点位置和终点位置的序列点或曲线称之为路径，构成路径的策略称之为路径规划。路径规划在很多领域都具有广泛的应用。在高新科技领域的应用有：机器人的自主无碰行动；无人机的避障突防飞行；巡航导弹躲避雷达搜索、防反弹袭击、完成突防爆破任务等。在日常生活领域的应用有：GPS导航；基于GIS系统的道路规划；城市道路网规划导航等。在决策管理领域的应用有：物流管理中的车辆问题(VRP)及类似的资源管理资源配置问题。通信技术领域的路由问题等。凡是可拓扑为点线网络的规划问题基本上都可以采用路径规划的方法解决。本文建立在经典路径规划算法基础上，提出了一种改进的RRT路径规划算法。

发明内容

本部分的目的在于概述本发明的实施例的一些方面以及简要介绍一些较佳实施例。在本部分以及本申请的说明书摘要和发明名称中可能会做些简化或省略以避免使本部分、说明书摘要和发明名称的目的模糊，而这种简化或省略不能用于限制本发明的范围。

鉴于上述现有存在的问题，提出了本发明。

因此，本发明解决的一个技术问题是：提供一种改进的RRT路径规划算法，能够更好地用于轨迹规划决策导航。

为解决上述技术问题，本发明提供如下技术方案：一种基于改进的RRT路径规划方法，其特征在于：包括以下步骤，避障模块通过快速扩展随机树生成随机点构建随机树，找出路径；分段拟合模块对所述快速扩展随机树找出的路径进行分段拟合求出路径控制点；信息提取模块以所述路径控制点为圆心和可调半径作圆，制作掩模mask，提取所述mask掩模的ROI区域，对ROI区域运用形态学图像处理，检测障碍物的轮廓，计算各个障碍物轮廓点到控制点的最小距离并提取该障碍物点的位置信息。

作为本发明所述的基于改进的RRT路径规划方法的一种优选方案，其中：所述避障模块的路径生成包括以下步骤，基于Windows平台搭建VS2015+OPencv3.4.3+Egien3环境；VSLAM构建室外场景的3D点云图；将3D点云图转换到2D栅格地图；在地图上指定起点和终点位置信息，读入栅格地图；根据指定起始点和终止点坐标信息，利用快速扩展随机树算法，生成从起点到终点的路径。

作为本发明所述的基于改进的RRT路径规划方法的一种优选方案，其中：所述分段拟合模块包括以下步骤，根据生成路径点的离散信息绘制出离散点图；根据散点图构造数学模型；根据数学模型构造优化判据将离散点分段拟合，并求出各个拟合折线段的交点作为路径控制点。

作为本发明所述的基于改进的RRT路径规划方法的一种优选方案，其中：所述信息提取模块包括以下步骤，对ROI区域的障碍物信息做形态学图像处理，得到障碍物块；对障碍物块在进行轮廓查找，给障碍物块赋予索引信息；再用递归算法分别求出各个障碍物块距离控制点的最小距离，求出最小距离点。

作为本发明所述的基于改进的RRT路径规划方法的一种优选方案，其中：所述生成路径还包括以下步骤，将初始状态点q_init作为随机树的根节点；然后在状态空间内以均匀分布的概率生成一个随机采样点q_rand，遍历当前随机树中所有节点并找到距离q_rand点最近的子节点q_near，在q_rand和q_near的连线上以一定的步长step扩展一个新的子节点q_new；若q_new和q_near的连线段与障碍物未发生碰撞，则将q_near以及该连线段分别作为新的子节点和新的边加入到随机扩展树中。

作为本发明所述的基于改进的RRT路径规划方法的一种优选方案，其中：还包括以下步骤，若发生碰撞，则舍弃该点重新选择q_near。重复上述过程直至q_near到达q_goal或q_goal附近的目标区域，则成功找到一条从q_init到q_goal的规划路径；若超过一定数量的扩展节点仍未到达目标区域或客观上不存在一条从起点至终点的无碰路径，则返回规划失败。

作为本发明所述的基于改进的RRT路径规划方法的一种优选方案，其中：所述分段拟合模块包括以下步骤，已知的二维数据x_i、i，用绘图函数plot绘出其散点图；根据散点图，判断分段曲线的分段区间，同时判断每个分段区间内的散点符合某个数学模型；所述分段区间分别用拟合函数，去拟合其数学模型的系数；拟合后的数学模型的数值和已知的散点值作对比，判断拟合的数学模型是否合理，用决定系数R²来判断，当R²≈1拟合为合理。

本发明解决的另一个技术问题是：提供一种改进的RRT路径规划系统，上述方法能依托于本系统实现，能够更好地用于轨迹规划决策导航。

为解决上述技术问题，本发明提供如下技术方案：一种基于改进的RRT路径规划系统，其特征在于：包括雷达数据处理模块、定位信息处理模块、相机数据处理模块、数据融合模块、路径规划模块、底层控制模块和显示控制模块；所述路径规划模块包括所述避障模块、所述分段拟合模块以及所述信息提取模块。

作为本发明所述的基于改进的RRT路径规划系统的一种优选方案，其中：所述雷达数据处理模块、所述定位信息处理模块和所述相机数据处理模块负责对各个传感器采集数据的处理；所述数据融合模块对传感器数据融合至同一个坐标系，并进行时间对齐、相互验证处理，最终建立出环境模型；所述路径规划模块根据所述数据融合模块的环境数据进行实时规划，对障碍物信息、道边及行道线信息、交通标识及红绿灯信息等进行处理，选择最优路径，生成规划结果；所述底层控制模块接收所述规划结果，根据路径规划模块的规划结果，对车辆的机械系统进行操控，包括车辆的加减速、转向操作；所述显示控制模块用于对各模块工作状态的实时监控。

本发明的有益效果：能够生成一条可执行的、避障效果好的、路径控制点稀疏的、周围障碍物信息可知的一条完整路径，能够更好的应用于无人车自主导航决策模块。

附图说明

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

图1为本发明第一种实施例所述快速扩展随机树流程示意图；

图2为本发明第一种实施例所述基于改进的RRT路径规划方法的整体流程结构示意图；

图3为本发明第一种实施例所述快速扩展随机树扩展示意图；

图4为本发明第二种实施例所述基于改进的RRT路径规划系统应用于车辆路径控制的示意图；

图5为本发明第二种实施例所述基于改进的RRT路径规划系统的整体原理结构示意图；

图6为本发明所述基于改进的RRT路径规划在一个场景下的路径规划效果示意图；

图7为本发明所述基于改进的RRT路径规划在另一个场景下的路径规划效果示意图；

图8为本发明所述原始算法路径规划在一个场景下的路径规划效果示意图；

图9为本发明所述原始算法路径规划在另一个场景下的路径规划效果示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护的范围。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。

本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。

同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

本发明中除非另有明确的规定和限定，术语“安装、相连、连接”应做广义理解，例如：可以是固定连接、可拆卸连接或一体式连接；同样可以是机械连接、电连接或直接连接，也可以通过中间媒介间接相连，也可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

实施例1

智能车也即无人驾驶汽车，是一种集合了自动控制理论、人工智能理论、视觉计算理论、体系结构理论、组合导航技术等多种理论及技术于一体的综合技术。目前，智能车在民用和军用领域都开始扮演越来越重要的角色，它代表了一个国家的综合科技水平和工业水平，具有重要的研究价值和实用价值。而路径规划是车辆智能性的集中体现，属于整个智能车系统中的决策部分，对路径规划的研究不仅能提高车辆的智能水平，还能增强智能车的实用性。本文主要针对自主驾驶车辆路径规划提出一种基于改进的RRT路径规划方法，改进点包括一是利用避障模块算法；二是对路径分段拟合，简化路径点数量，求出控制点；三是以控制点为圆心，半径可调范围内，求出最近的障碍物块位置信息。经过以上的改进能够更好地用于轨迹规划决策导航，该方法已经用于无人车导航规划的实际场景，效果较好。具体的，

参照图1～3的示意，本实施例中该方法包括以下步骤，

S1：避障模块100通过快速扩展随机树生成随机点构建随机树，找出路径；避障模块100的路径生成包括以下步骤，

基于Windows平台搭建VS2015+OPencv3.4.3+Egien3环境；

VSLAM(基于视觉的定位与建图)构建室外场景的3D点云图；

将3D点云图转换到2D栅格地图；

在地图上指定起点和终点位置信息，读入栅格地图；

根据指定起始点和终止点坐标信息，利用快速扩展随机树算法，生成从起点到终点的路径。本步骤中VS2015+OPencv3.4.3+Egien3环境，其中为VS2015是Windows平台应用程序的集成开发环境，OpenCV是一个基于BSD许可(开源)发行的跨平台计算机视觉库，可以运行在Linux、Windows、Android和MacOS操作系统上，Eigen是一个高层次的C++库，有效支持线性代数，矩阵和矢量运算，数值分析及其相关的算法。

S2：分段拟合模块200对快速扩展随机树找出的路径进行分段拟合求出路径控制点；分段拟合模块200包括以下步骤，

根据生成路径点的离散信息绘制出离散点图；

根据散点图构造数学模型；

根据数学模型构造优化判据将离散点分段拟合，并求出各个拟合折线段的交点作为路径控制点。分段拟合模块200包括以下步骤，

已知的二维数据xi、i，用绘图函数plotx，y，'k*'绘出其散点图；

根据散点图，判断分段曲线的分段区间，同时判断每个分段区间内的散点符合某个数学模型；

分段区间分别用拟合函数，去拟合其数学模型的系数；拟合后的数学模型的数值和已知的散点值作对比，判断拟合的数学模型是否合理，用决定系数R²来判断，当R²≈1拟合为合理。

S2：信息提取模块300以路径控制点为圆心和可调半径作圆，制作掩模mask，提取mask掩模的ROI区域，对ROI区域运用形态学图像处理，检测障碍物的轮廓，计算各个障碍物轮廓点到控制点的最小距离并提取该障碍物点的位置信息。信息提取模块300包括以下步骤，

对ROI区域的障碍物信息做形态学图像处理，得到障碍物块；

对障碍物块在进行轮廓查找，给障碍物块赋予索引信息；

再用递归算法分别求出各个障碍物块距离控制点的最小距离，求出最小距离点。

本实施例还需要说明的是，快速扩展随机树是一种增量式的前向搜索算法，是基于微分约束的单查询随机采样规划方法。RRT算法分为搜索树的构造和路径产生两个阶段，不需要对车辆的位姿空间进行预处理。将起点位姿点加入随机树后，直接在空间中先随机采样一个位姿点，遍历整个随机树中的节点并选出距离该随机位姿点最近的节点，在满足约束的前提下，以一定的步长向着随机点方向生长，直至到达目标区域。从目标位姿点出发，依次寻找当前位姿点的父节点直至回到起始位姿点，即可得到规划路径。

RRT(快速探索随机树)作为一种高效的数据结构和算法，在算法执行前不需要进行预处理，直接通过随机扩展节点的方式快速地搜索整个状态空间，且能适应动态环境，具备快速重规划能力。通过将各种约束条件集成到算法中，RRT算法近年来在受微分约束的系统中得到了广泛的应用。且由于随机算法的固有属性，理论上只要路径客观存在，算法就能找到一条可通行路径，RRT具有概率完备性。

快速扩展随机树从状态空间中的初始点开始，通过一定的概率在状态空间中通过获取随机采样子节点的方式来增量构造随机树，当随机树中的子节点包含了目标点或目标区域的点时停止扩展，从目标点开始依次遍历父节点即可找到一条从根节点到目标点的路径。具体包括如下过程：

定义：

状态空间：一个拓扑空间X；

边界值：初始状态点x_init、目标状态点x_goal，x_init∈X且

碰撞检测：一个函数D：X→{true，false}，判断状态x是否满足无碰的全局约束；

输入：一个集合U，可以影响状态点的控制与行为的完整集合；

增量模拟器：对于给定的当前状态x(t)，在一定时间间隔内，计算x(t+Δt)；

度量函数：一个实值函数ρ：X×X→[0，∞)，描述了空间中两个点的距离，路径规划通常被定义为在状态空间x中找到一条从初始状态开始的连续路径，即从x_init点到目标区域或目标状态x_goal无碰路径，对于碰撞检测结果用X_free来表示满足碰撞检测约束的状态点的集合，称为自由空间，输入集合中的控制与行为和增量模拟器通常决定了微分约束。增量模拟器可被理解为系统在离散时间中的变化。度量函数用来描述状态空间中的点对之间的距离。

首先将初始状态点q_init作为随机树的根节点，然后在状态空间内以均匀分布的概率生成一个随机采样点q_rand，遍历当前随机树中所有节点并找到距离q_rand点最近的子节点q_near，在q_rand和q_near的连线上以一定的步长step扩展一个新的子节点q_new，若q_new和q_near的连线段与障碍物未发生碰撞，则将q_near以及该连线段分别作为新的子节点和新的边加入到随机扩展树中；若发生碰撞，则舍弃该点重新选择q_near。重复上述过程直至q_near到达q_goal或q_goal附近的目标区域，则成功找到一条从q_init到q_goal的规划路径，若超过一定数量的扩展节点仍未到达目标区域或客观上不存在一条从起点至终点的无碰路径，则返回规划失败。

根据均匀分布获取状态空间内的随机采样点q_rand，获取当前随机树中离随机采样点q_rand最近的子节点q_near，在q_rand和q_near的连线上以一定的步长step扩展新的子节点q_near，计算公式如下：

q_new＝q_near+step*(q_rand-q_near)/||q_rand-q_near||

若q_near满足碰撞检测等约束条件，则扩展成功，并将新的子节点q_near，以及q_near和q_new连线u_new作为边加入到随机树中，若不满足约束条件则重新获取q_new。当q_new与目标点q_goal之间的距离小于一定值时，则返回Reached，表示到达目标点q_goal，否则需要继续扩展子节点；返回值Advanced表示找到了新的扩展节点q_new，Trapped表示扩展失败，未找到新的子节点。

本实施例路径规划算法具备合理性：路径能完成基本避障任务，每一次算法返回的规划结果可以被车辆跟踪执行。完备性：如果客观上存在搜索空间内从起点至终点的无碰连续路径，则该规划算法一定能返回一条合理路径；若客观上不存在，则返回规划失败。最优性：算法返回的规划结果在时间花费、路径代价或其他某测度上最优。实时性：算法的时间空间复杂度能满足实时运动的需求。环境适应性：对于动态环境中的规划路径是否具有跟随环境而变化的能力。

场景一：

本文实例针对避障效果对比传统算法和本文改进的算法，根据实验数据输入地图、起点、终点等信息，进行路径规划，选取了两个不同的场景，分别通过本方法的改进后算法的路径规划和传统原始算法路径规划进行路径规划，其实际路径规划的效果如下图6～9所示。

效果说明：图6～7所示改进算法效果图，VSLAM构建一个真实场景的环境地图，设定起点和终点位置信息，蓝色折线段是对路径拟合后的路径，浅绿色是算法生成路径过程的随机树，黑色表示障碍物区域(本实施例中为便于灰度下的表示，在图中进行标注示意)。从效果图可以看出规划的路径能够很好地避障且折线段拐点的障碍点信息能够很好地感知到。

图8～9所示为原始算法效果图，起点位于绿色圆点，终点位于红色位置，同样分别设定的两个不同场景，首先来看，随机树生长和障碍物区域很近，规划出的路径不适合无人车落地行驶，其次路径未经平滑或者拟合处理，且障碍物信息紊乱，不能提供各个位置障碍物信息容易让车行驶过程中碰到障碍物。最后路径不够最简化，出现很多冗余拐点位置信息，不顺滑。

实施例2

参照图4～5的示意，示意为本实施例提出的一种基于改进的RRT路径规划系统，上述方法能够依托于本系统实现，具体的，该包括雷达数据处理模块400、定位信息处理模块500、相机数据处理模块600、数据融合模块700、路径规划模块、底层控制模块800和显示控制模块900；路径规划模块包括避障模块100、分段拟合模块200以及信息提取模块300。

进一步的，更加具体的，雷达数据处理模块400、定位信息处理模块500和相机数据处理模块600负责对各个传感器采集数据的处理；数据融合模块700对传感器数据融合至同一个坐标系，并进行时间对齐、相互验证处理，最终建立出环境模型；路径规划模块根据数据融合模块700的环境数据进行实时规划，对障碍物信息、道边及行道线信息、交通标识及红绿灯信息等进行处理，选择最优路径，生成规划结果；底层控制模块800接收规划结果，根据路径规划模块的规划结果，对车辆的机械系统进行操控，包括车辆的加减速、转向操作；显示控制模块900用于对各模块工作状态的实时监控。

应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (9)

1.一种基于改进的RRT路径规划方法，其特征在于：包括以下步骤，

避障模块(100)通过快速扩展随机树生成随机点构建随机树，找出路径；

分段拟合模块(200)对所述快速扩展随机树找出的路径进行分段拟合求出路径控制点；

信息提取模块(300)以所述路径控制点为圆心和可调半径作圆，制作掩模mask，提取所述mask掩模的ROI区域，对ROI区域运用形态学图像处理，检测障碍物的轮廓，计算各个障碍物轮廓点到控制点的最小距离并提取该障碍物点的位置信息。

2.如权利要求1所述的基于改进的RRT路径规划方法，其特征在于：所述避障模块(100)的路径生成包括以下步骤，

基于Windows平台搭建VS2015+OPencv3.4.3+Egien3环境；

VSLAM构建室外场景的3D点云图；

将3D点云图转换到2D栅格地图；

在地图上指定起点和终点位置信息，读入栅格地图；

根据指定起始点和终止点坐标信息，利用快速扩展随机树算法，生成从起点到终点的路径。

3.如权利要求1或2所述的基于改进的RRT路径规划方法，其特征在于：所述分段拟合模块(200)包括以下步骤，

根据生成路径点的离散信息绘制出离散点图；

根据散点图构造数学模型；

根据数学模型构造优化判据将离散点分段拟合，并求出各个拟合折线段的交点作为路径控制点。

4.如权利要求3所述的基于改进的RRT路径规划方法，其特征在于：所述信息提取模块(300)包括以下步骤，

对ROI区域的障碍物信息做形态学图像处理，得到障碍物块；

对障碍物块在进行轮廓查找，给障碍物块赋予索引信息；

再用递归算法分别求出各个障碍物块距离控制点的最小距离，求出最小距离点。

5.如权利要求1～2或4任一所述的基于改进的RRT路径规划方法，其特征在于：所述生成路径还包括以下步骤，

将初始状态点q_init作为随机树的根节点；

然后在状态空间内以均匀分布的概率生成一个随机采样点q_rand，遍历当前随机树中所有节点并找到距离q_rand点最近的子节点q_near，在q_rand和q_near的连线上以一定的步长step扩展一个新的子节点q_new；

若q_new和q_near的连线段与障碍物未发生碰撞，则将q_near以及该连线段分别作为新的子节点和新的边加入到随机扩展树中。

6.如权利要求5所述的基于改进的RRT路径规划方法，其特征在于：还包括以下步骤，

若发生碰撞，则舍弃该点重新选择q_near。重复上述过程直至q_near到达q_goal或q_goal附近的目标区域，则成功找到一条从q_init到q_goal的规划路径；

若超过一定数量的扩展节点仍未到达目标区域或客观上不存在一条从起点至终点的无碰路径，则返回规划失败。

7.如权利要求6所述的基于改进的RRT路径规划方法，其特征在于：所述分段拟合模块(200)包括以下步骤，

已知的二维数据x_i、i，用绘图函数plot(x，y，'k*')绘出其散点图；

根据散点图，判断分段曲线的分段区间，同时判断每个分段区间内的散点符合某个数学模型；

所述分段区间分别用拟合函数，去拟合其数学模型的系数；

拟合后的数学模型的数值和已知的散点值作对比，判断拟合的数学模型是否合理，用决定系数R²来判断，当R²≈1拟合为合理。

8.一种基于改进的RRT路径规划系统，其特征在于：包括雷达数据处理模块(400)、定位信息处理模块(500)、相机数据处理模块(600)、数据融合模块(700)、路径规划模块、底层控制模块(800)和显示控制模块(900)；所述路径规划模块包括所述避障模块(100)、所述分段拟合模块(200)以及所述信息提取模块(300)。

9.如权利要求8所述的基于改进的RRT路径规划系统，其特征在于：所述雷达数据处理模块(400)、所述定位信息处理模块(500)和所述相机数据处理模块(600)负责对各个传感器采集数据的处理；

所述数据融合模块(700)对传感器数据融合至同一个坐标系，并进行时间对齐、相互验证处理，最终建立出环境模型；

所述路径规划模块根据所述数据融合模块(700)的环境数据进行实时规划，对障碍物信息、道边及行道线信息、交通标识及红绿灯信息等进行处理，选择最优路径，生成规划结果；

所述底层控制模块(800)接收所述规划结果，根据路径规划模块的规划结果，对车辆的机械系统进行操控，包括车辆的加减速、转向操作；

所述显示控制模块(900)用于对各模块工作状态的实时监控。