CN113341978A - 一种基于梯型障碍物的智能小车路径规划方法 - Google Patents

Abstract

本发明涉及一种基于梯型障碍物的智能小车路径规划方法，利用视觉SLAM技术，对智能小车周围环境进行点云建模；利用已有点云地图信息，构建三维栅格地图，并对地图障碍物进行识别，标识梯型障碍物，并记录梯型障碍物的起点与坡度大小；调用改进RRT算法，在扩展节点时，先对节点进行判定，若新节点处于梯型障碍物区域内，则按照梯型障碍物的坡度与起点位置设定新节点，使其作为路径点；若新节点不处于梯型障碍物内，则对新节点进行无碰检测。若其是安全的，则将其加入路径扩展树中；将改进RRT算法规划的路径，发送至智能小车的执行机构，使其安全到达目标点。

Description

一种基于梯型障碍物的智能小车路径规划方法

技术领域

本发明属于自主移动小车的避障领域，涉及一种基于梯型障碍物的智能小车路径规划方法，具体涉及一种针对梯型障碍物的智能小车路径规划方法

背景技术

近年来，随着无人系统技术的快速发展，具有自主移动能力的智能小车进入人们的视野。在无人进行外部的干预下，未知环境中智能小车的自主移动能力成为检测智能小车自主性的一个重要指标。智能小车在未知环境中，在无人干预的情况下进行自主移动的关键技术是路径规划技术，路径规划技术即根据智能小车的自身位置以及对其周围环境进行地图建模，实时规划一条可供智能小车的执行的路径，使其在到达目标点的过程中，避开障碍物。而目前智能小车的运行环境只限制与二维场景中，移动过程中应对障碍物的方法是绕开障碍物，造成智能小车的移动路径增加，增加能量的损耗。

在日常生活中，梯型障碍物随处可见，如阶梯上的斜坡以及有一定坡度的物体等，如附图所示。这样的障碍物给智能小车的移动增加了多样性，智能小车都具有不同程度的爬坡能力，因此在遇到类似梯型的障碍物时，可以令智能小车沿着梯型障碍物坡度表面进行移动，减少移动总路径长度。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种基于梯型障碍物的智能小车路径规划方法，解决智能小车遇到可跨越的梯型障碍物时，仍选择绕过障碍物的方式进行避障的问题。

首先根据传感器的量测信息，调用SLAM技术得到智能小车周围环境的信息以及自身的位置信息，将智能小车的周围环境进行三维点云建模，并根据智能小车爬坡能力对障碍物的坡度设定一个阈值，之后将智能小车周围的障碍物进行聚合识别，将属于梯型的障碍物进行标示，改进RRT算法中节点裁剪步骤，使其能够针对梯型障碍物进行爬坡处理，调用改进RRT算法规划智能小车的避障轨迹，将其发送到智能小车的执行机构，使其安全到达目标点。

本发明不仅可以满足智能小车的避障需求，并且在遇到可跨越的梯型障碍物时，也可以智能的选择一条更短的轨迹，使得智能小车的能耗降低，可运行距离增长。

技术方案

一种基于梯型障碍物的智能小车路径规划方法，其特征在于步骤如下：

步骤1：采用视觉SLAM技术，对智能小车周围环境进行点云建模；

步骤2：根据自主移动机器人的尺寸调整地图分辨率resolution，并对上步得到的周围环境点云地图进行栅格化处理：

X_num＝x/resolution

Y_num＝y/resolution

Z_num＝z/resolution

其中X_num、Y_num、Z_num表示栅格地图的索引值，resolution表示栅格地图分辨率，x,y,z表示点的坐标值；

将障碍物点所处的的栅格状态设置为占据：cell_(x,y,z).status＝occupied；

对点地图进行栅格化后，对栅格地图进行筛选标识，标识出梯型障碍物的起点与坡度大小，步骤如下：

将障碍物栅格进行聚合处理，将所有的邻居栅格合并在一起，并记录高度值Z最大的栅格坐标，并对其计数Z_max_num，计算其中心点坐标(x_max,y_max,z_max)之后记录高度值Z最小的栅格坐标，并对其计数Z_min_num，计算其中心点坐标(x_min,y_min,z_min)，若是数量超过设定的阈值，则利用下述公式计算坡度大小：

将坡度θ大于智能小车爬坡能力的梯型障碍物标识为普通障碍物，将坡度θ小于智能小车爬坡能力的梯型障碍物标识为梯型障碍物，并记录坡度大小与起点， [θ,(x_min,y_min,z_min)]；

步骤3、改进RRT算法的节点裁剪步骤：在设置新节点时，对节点优先进行判断，判断其是否落在梯型障碍物区域内若新节点处于梯型障碍物区域内，则按照梯型障碍物的坡度与起点位置设定新节点，使其作为路径点；若新节点不处于梯型障碍物内，则对新节点进行无碰检测；若其是安全的，则将其加入路径扩展树中；

步骤4：将改进RRT算法规划的路径，发送至智能小车的执行机构，使其安全到达目标点。

所述RRT算法的节点裁剪步骤：

随机采样节点x_random

选择与随机采样节点最近的点x_near

连接x_near与x_random，选择阈值范围的点为新节点x_new

判断x_new是否处于终点区域，如处于终点区域，则算法结束，

如不处于终点区域，则判断x_new是否处于梯型障碍物内，若处于，则根据梯型障碍物的坡度以及起点计算沿障碍物表面的路径为新节点

若不处于，则判断x_new是否处于普通障碍物内，若处于，则回到第一步随机采样节点，

若不处于，则将x_new作为新的树节点，之后回到第一步随机采样节点

有益效果

本发明提出的一种基于梯型障碍物的智能小车路径规划方法，利用视觉SLAM技术，对智能小车周围环境进行点云建模；利用已有点云地图信息，构建三维栅格地图，并对地图障碍物进行识别，标识梯型障碍物，并记录梯型障碍物的起点与坡度大小；调用改进RRT算法，在扩展节点时，先对节点进行判定，若新节点处于梯型障碍物区域内，则按照梯型障碍物的坡度与起点位置设定新节点，使其作为路径点；若新节点不处于梯型障碍物内，则对新节点进行无碰检测。若其是安全的，则将其加入路径扩展树中；将改进RRT算法规划的路径，发送至智能小车的执行机构，使其安全到达目标点。

采用本发明提出的基于梯型障碍物的智能小车路径规划方法，智能小车在自主移动过程中，若是遇到一些可跨越的障碍物或者斜坡等物体时，利用步骤二提出的梯型障碍物检测算法可以检测出梯型障碍物的坡度以及起点位置；调用步骤三改进的RRT 算法对识别到的梯型障碍物进行特别规划，可以使智能小车不再选择将其绕过去，而是直接将其跨越，减少智能小车的移动距离，降低运动损耗，在能源有限的情况下使智能小车能够运行更长时间。

附图说明

图1：智能小车图

图2：梯型障碍物示意图

图3：障碍物点云建模图

图4：梯型障碍物检测结果图

图5：改进RRT算法框图

图6：梯型障碍物路径规划结果图；其中深色线条表示改进RRT算法得到的路径，而浅色的线条表示小车的执行路径。

具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明的目的在于为智能小车提供一种跨越梯形障碍物的路径规划方法，将障碍物高度信息以及坡度信息加入路径规划的约束条件中，使智能小车在遇到可跨越的障碍物或者斜坡时，不必进行绕弯对其进行避障，而是可以直接跨越该障碍物，减少机器人的移动距离，降低运动损耗，在能源有限的情况下使智能小车能够运行更长时间。

本发明的思路是利用视觉SLAM技术构建智能小车周围环境的模型，并将其以点云地图形式表示出来；并根据智能小车爬坡能力对障碍物的坡度设定一个阈值，之后将智能小车周围的障碍物进行聚合识别，将属于梯型的障碍物进行标示，改进RRT算法中节点裁剪步骤，使其能够针对梯型障碍物进行爬坡处理，调用改进RRT算法规划的智能小车的避障轨迹，将其发送到智能小车的执行机构，使其安全到达目标点。

具体过程如下：

1.利用视觉传感器得到周围环境的深度图像，通过智能小车携带的传感器信息计算出机器人当前位姿，并且调用坐标转换关系将每帧深度图像的像素坐标转换至世界坐标系下：

ZP_u,v＝K(RP_w+t)＝KTP_w (1)

其中Z表示每帧深度图像中每一个像素点的深度信息，P_u,v表示深度图像的每一个像素坐标，K表示相机内参，

其中相机的位姿R,t又称为相机的外参数，T表示转换矩阵，P_w即特征点在世界坐标系下的坐标，其中

然后对世界坐标系下的多帧点云进行拼接，由此可以得到周围环境的精确三维点云模型。如图3所示：

2.根据自主移动机器人的尺寸调整地图分辨率resolution，并对上步得到的周围环境点云地图进行栅格化处理：

其中X_num、Y_num、Z_num表示栅格地图的索引值，resolution表示栅格地图分辨率，x,y,z表示点的坐标值。

将障碍物点所处的的栅格状态设置为占据

cell_(x,y,z).status＝occupied (3)

无障碍物点的栅格状态设置为空闲

cell_(x,y,z).status＝free (4)

对点地图进行栅格化后，对栅格地图进行筛选标识，标识出梯型障碍物的起点与坡度大小，步骤如下：

将障碍物栅格进行聚合处理，将所有的邻居栅格合并在一起，并记录高度值Z最大的栅格坐标，并对其计数Z_max_num，计算其中心点坐标(x_max,y_max,z_max)之后记录高度值Z最小的栅格坐标，并对其计数Z_min_num，计算其中心点坐标(x_min,y_min,z_min)，若是数量超过设定的阈值，则利用下述公式计算坡度大小

将坡度θ大于智能小车爬坡能力的梯型障碍物标识为普通障碍物，将坡度θ小于智能小车爬坡能力的梯型障碍物标识为梯型障碍物，并记录坡度大小与起点， [θ,(x_min,y_min,z_min)]。

图1梯型障碍物检测结果图。

3.改进RRT算法的节点裁剪步骤，在设置新节点时，对节点优先进行判断，判断其是否落在梯型障碍物区域内若新节点处于梯型障碍物区域内，则按照梯型障碍物的坡度与起点位置设定新节点，使其作为路径点；若新节点不处于梯型障碍物内，则对新节点进行无碰检测。若其是安全的，则将其加入路径扩展树中。具体算法框图如图 5；

4.根据规划的路径发送到智能小车的执行机构，使智能小车能够安全到达目标点。结果如图6所示梯型障碍物路径规划结果图。

其中深色线条表示改进RRT算法得到的路径，而浅色的线条表示小车的执行路径。

Claims (2)

1.一种基于梯型障碍物的智能小车路径规划方法，其特征在于步骤如下：

步骤1：采用视觉SLAM技术，对智能小车周围环境进行点云建模；

步骤2：根据自主移动机器人的尺寸调整地图分辨率resolution，并对上步得到的周围环境点云地图进行栅格化处理：

X_num＝x/resolution

Y_num＝y/resolution

Z_num＝z/resolution

其中X_num、Y_num、Z_num表示栅格地图的索引值，resolution表示栅格地图分辨率，x,y,z表示点的坐标值；

将障碍物点所处的的栅格状态设置为占据：cell_(x,y,z).status＝occupied；

对点地图进行栅格化后，对栅格地图进行筛选标识，标识出梯型障碍物的起点与坡度大小，步骤如下：

将障碍物栅格进行聚合处理，将所有的邻居栅格合并在一起，并记录高度值Z最大的栅格坐标，并对其计数Z_max_num，计算其中心点坐标(x_max,y_max,z_max)之后记录高度值Z最小的栅格坐标，并对其计数Z_min_num，计算其中心点坐标(x_min,y_min,z_min)，若是数量超过设定的阈值，则利用下述公式计算坡度大小：

将坡度θ大于智能小车爬坡能力的梯型障碍物标识为普通障碍物，将坡度θ小于智能小车爬坡能力的梯型障碍物标识为梯型障碍物，并记录坡度大小与起点，[θ,(x_min,y_min,z_min)]；

步骤3、改进RRT算法的节点裁剪步骤：在设置新节点时，对节点优先进行判断，判断其是否落在梯型障碍物区域内若新节点处于梯型障碍物区域内，则按照梯型障碍物的坡度与起点位置设定新节点，使其作为路径点；若新节点不处于梯型障碍物内，则对新节点进行无碰检测；若其是安全的，则将其加入路径扩展树中；

步骤4：将改进RRT算法规划的路径，发送至智能小车的执行机构，使其安全到达目标点。

2.根据权利要求1所述，其特征在于：所述RRT算法的节点裁剪步骤：

随机采样节点x_random

选择与随机采样节点最近的点x_near

连接x_near与x_random，选择阈值范围的点为新节点x_new

判断x_new是否处于终点区域，如处于终点区域，则算法结束，

如不处于终点区域，则判断x_new是否处于梯型障碍物内，若处于，则根据梯型障碍物的坡度以及起点计算沿障碍物表面的路径为新节点

若不处于，则判断x_new是否处于普通障碍物内，若处于，则回到第一步随机采样节点，

若不处于，则将x_new作为新的树节点，之后回到第一步随机采样节点。

Images