[发明专利]一种基于梯型障碍物的智能小车路径规划方法

权利要求书

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

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

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

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

其中Z表示每帧深度图像中每一个像素点的深度信息，P_u,v表示深度图像的每一个像素坐标，K表示相机内参，其中相机的位姿R,t又称为相机的外参数，T表示转换矩阵，P_w即特征点在世界坐标系下的坐标，其中

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

步骤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作为新的树节点，之后回到第一步随机采样节点。