机器人自主探索系列解析(二)——三次样条生成路径点

三次线条法生成离线路径

紧接着前面的内容，这里主要介绍cmu方法的路径规划方法。它通过采样离散点的方法，事先生成了可能出现的路径集合(343条)，接着根据传感器获取的点云数据，删除不可通过路径，对剩下的路径进行评估，得到当前最优的无碰撞路径。
在代码中，主要是path_generator.m生成离散路径，并进行体素膨胀检测。

1.采样路径的生成

采用的是三次线条法采样离散点，模拟机器人前向运动， 基于机器人当前状态，预测未来一段范围内可能出现的情况。

生成沿着不同路径到达传感器范围边界的路径集合。这些路径是基于车辆运动约束生存的，每条路径生成一个三次样条曲线，一组路径可以看成从启始状态到传感器感知范围边的可行路径。

首先是对一些相关参数进行了定义。 dis表示的是采样的距离，angle为最大转弯半径，deltaAngle为角度间隔，scale为路径采样缩放因子。经过一次采样得到7(27/9*2+1)条路径组,上边下边各三条，中间一条。

dis = 1.0;
angle = 27;
deltaAngle = angle / 3;
scale = 0.65;

同时定义了一些变量存放结果，其中pathStartAll存放的是第一次采样得到的结果，pathAll是所有采样得到的最终结果
pathList为路径数组， pathID为路径编号,groupID为路径所在组。

pathStartAll = zeros(4, 0);
pathAll = zeros(5, 0);
pathList = zeros(5, 0);
pathID = 0;
groupID = 0;

这里我们以第一条路径为例，起始点是(0,0)，目标点是(dis,shift1)，即(1,-27)。在(0,0)和(1,-27)之间以0.01为间隔进行插值，得到101条路径分支。

for shift1 = -angle : deltaAngle : angle
    wayptsStart = [0, 0, 0;
                   dis, shift1, 0];
    
    pathStartR = 0 : 0.01 : dis;
    pathStartShift = spline(wayptsStart(:, 1), wayptsStart(:, 2), pathStartR);
    
    pathStartX = pathStartR .* cos(pathStartShift * pi / 180);
    pathStartY = pathStartR .* sin(pathStartShift * pi / 180);
    pathStartZ = zeros(size(pathStartX));
    
    pathStart = [pathStartX; pathStartY; pathStartZ; ones(size(pathStartX)) * groupID];
    pathStartAll = [pathStartAll, pathStart];

这里的pathStartShift = spline(wayptsStart(:, 1), wayptsStart(:, 2), pathStartR)
对应的wayptsStart(:, 1)为(0,1)，wayptsStart(:, 2)为(1,-27)。使用spline样条函数均匀插值，

由于采用的是三次样条法，后续两个循环模拟的原理同第一次。不同点在于添加了路径缩放因子，对角度进行了一定比例scale的缩放

for shift2 = -angle * scale + shift1 : deltaAngle * scale : angle * scale + shift1
        for shift3 = -angle * scale^2 + shift2 : deltaAngle * scale^2 : angle * scale^2 + shift2

通过spline对前一百次数据的拟合，采样出第二次和第三次的路径点。

waypts = [pathStartR', pathStartShift', pathStartZ';
          2 * dis, shift2, 0;
          3 * dis - 0.001, shift3, 0;
          3 * dis, shift3, 0];

pathR = 0 : 0.01 : waypts(end, 1); % 301维的行向量
pathShift = spline(waypts(:, 1), waypts(:, 2), pathR); % 采样出第二次和第三次的路径点

pathX = pathR .* cos(pathShift * pi / 180);
pathY = pathR .* sin(pathShift * pi / 180);
pathZ = zeros(size(pathX));

最终循环一次得到的路径结果如下，其中蓝色线条为第一次采样，橘色为第二次，黄色巍峨第三次。
在一组完整的路径中，采用第一次采样分组group为7组，在对第二第三次的路径精进行命名pathID

根据以上命名方式会生成3个文件数据，分别是：

• startPaths.ply ：记录了第一次采样的路径点，是一个 4 ∗ 707 的矩阵，即一共7组路径组，每一个路径组包含了101个路径点，里面所保存的数据为：
• pathAll:该文件则记录了三次采样所生成的所有路径点，是一个 5 ∗ 103243 的矩阵，即在每一个路径组中，包含14749个路径点（103243/7 = 14749）。而每一条路径包括301个路径点（14749/7/7 = 301）。
• pathList 该文件记录了每条路径的最后一个路径点，是一个 5 ∗ 343的矩阵，即 7 ∗ 7 ∗ 7 个末端点。

2. 体素网格生成

为了碰撞检查，生成了覆盖传感器范围的体素网格，根据样条距离，设置传感器范围为3.2*4.5。在这里考虑了车辆半径的遮挡，在生成体素网格时，定义了以下参数:

voxelSize = 0.02; // 体素方格大小
searchRadius = 0.45; //
offsetX = 3.2;  // 传感器范围
offsetY = 4.5;
voxelNumX = 161; //数量 offsetX/voxelSize 
voxelNumY = 451;

在外层循环中，从外向内计算，同采样的路径一样，靠近车体的位置Y的宽度也会随着scaleY 减小。

for indX = 0 : voxelNumX - 1
    x = offsetX - voxelSize * indX;
    scaleY = x / offsetX + searchRadius / offsetY * (offsetX - x) / offsetX;
    for indY = 0 : voxelNumY - 1
        y = scaleY * (offsetY - voxelSize * indY);
    end
end

这里采用延长梯形斜边与x坐标轴相交，推导出scaleY的表达

3.碰撞检测

在生成体素网格后，为了在局部路径的筛选中更高效，将每个网格与路径索引离线计算。通过查找路径集合中的路径点周围指定距离内的邻居，并且该邻居点属于体素网格的集合。最终得到每一个体素网格与待遮挡路径之间的索引关系表。
rangesearch()函数：查找某个点周围指定距离内的所有邻居。

[ind, dis] = rangesearch(pathAll(1 : 2, :)', voxelPoints, searchRadius);

记录路径点附近的所有体素网格。最终输出：
（1）第一次采样的路径点startPaths（2）路径点集合 paths

（3）每条路径的最后一个路径点 pathList（4）路径点和碰撞体素网格的索引关系 correspondences

startPaths存放的是第一次采样的路径，图中绿色部分，包含7条路径，将[0-1]划分为100等分插值，有7*101个路径点。[1-2]范围在原来的7条路径上每条路径再线性插值分出7条路径，共7*7=49条路径；[1-2]范围在原来的49条路径上再线性插值每条路径分出7条路径，共7*7*7=343条路径。
paths存放的是所有的点，即7*7*7*301，用于可视化。
pathList存放的是所有路径的最后一个路径点,即图中红色的点。

参考内容

Matlab用采样的离散点做前向模拟三次样条生成路径点

CMU团队开源的局部路径导航算法 localPlanner 代码详解以及如何适用于现实移动机器人