1.采样路径的生成

用采样的离散点做前向模拟，基于机器人当前状态，预测一段范围内可能出现的情况。在这里插入图片描述生成沿着不同路径到达传感器范围边界的路径集合。这些示例路径是基于车辆运动约束生成的,每条路径生成三次样条曲线，一组路径可以看作是从起始状态到传感器范围边界的可行路径。

在导航过程中，感知传感器（激光雷达）检测到阻塞某些路径的障碍物,便将该部分路径删除，仅保留可通行的路径子集。以上路径组是脱机生成的,在生成路径的时，定义几个关键参数：

dis = 1.0;
angle = 27;
deltaAngle = angle / 3;
复制代码

这里定义的最大转弯半径angle 为27，deltaAngle 为9，因此会生成7条（27/9*2+1）路径组。

以第一条路径组为例，此时shift1偏移量即为-27，每条路径半径dis为1。

pathStartR = 0 : 0.01 : dis;
pathStartShift = spline(wayptsStart(:, 1), wayptsStart(:, 2), pathStartR)
复制代码

其中wayptsStart(:, 1)即为dis =(0,1)，wayptsStart(:, 2)为shift1=(0,-27)。

因此pathStartShift 为一个 $1*101$ 的行向量，代表对0->shift1进行100次数据均匀插值，即[0.00，-0.27，-0.54，-0.81，...，-26.73，-27.00]

pathStartX = pathStartR .* cos(pathStartShift * pi / 180);
pathStartY = pathStartR .* sin(pathStartShift * pi / 180);
pathStartZ = zeros(size(pathStartX));
复制代码

接着对pathStartR 这条0-1的直线，通过pathStartShift 转变成如下图所示的曲线

在这里插入图片描述

由于采用的三次样条模拟路径，第一段则由shift1 = -angle : deltaAngle : angle计算，当shift1 = -27时即如上图示例所示。在第二次第三次的样条采用时，原理同第一次。

for shift2 = -angle * scale + shift1 : deltaAngle * scale : angle * scale + shift1
     for shift3 = -angle * scale^2 + shift2 : deltaAngle * scale^2 : angle * scale^2 + shift2
     	...
     end
end
复制代码

相比于第一次，其角度进行了一定比例scale的缩放，此时的pathR相比于第一次的pathStartR ，从0-1变成了0-3的范围，从 $1*101$ 的行向量变成 $1*301$ 的行向量，相同的是他的值仍是以0.01递增。

下图为waypts ，1-101为第一次样条采样的路径点，第一栏代表距离从0-1，第二栏代表角度，而101-104则是第二次和第三次样条采样的路径点。通过spline对前一百次数据的拟合，采样出第二次和第三次的路径点。 waypts

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));
复制代码

在这里插入图片描述最终该一次循环所计算的路径，即如上图所示，分为三段，蓝色为第一次采样，橘色为第二次，黄色为第三次。

在一组完整的路径中，根据第一次采样分组groupID，即7组，再对第二次和第三次采样后的路径进行命名pathID。在这里插入图片描述根据以上命名方式会生成3个文件数据，分别是： startPaths.ply ：记录了第一次采样的路径点，是一个 $4*707$ 的矩阵，即一共7组路径组，每一个路径组包含了101个路径点，里面所保存的数据为：{x,y,z,groupID} 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
复制代码

在这里插入图片描述下面对scaleY的计算详细说明：如上图所示，以最上层边缘计算为例，即indY = 0的时候，此时可以看成一条线，而y = scaleY * offsetY，当 x = 0 时，y = searchRadius 当 x = offsetX 时，y = offsetY 得到： $y = {offsetY -searchRadius \over offsetX}x+searchRadius$

或通过相似三角形可得到：

y = { x \over offsetX}(offsetY - searchRadius)+searchRadius

因此：

scaleY = {y \over offsetY } = { { x \over offsetX}(offsetY - searchRadius)+searchRadius \over offsetY }

= { x \over offsetX} - { x*searchRadius \over offsetX*offsetY}+{searchRadius \over offsetY}

= { x \over offsetX} +{{(offsetX - x)searchRadius }\over offsetX*offsetY}

= { x \over offsetX} +{{searchRadius }\over offsetY} *{(offsetX - x) \over offsetX}

3.碰撞检测

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

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

记录路径点附近的所有体素网格。

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

1.采样路径的生成

2.体素网格产生

3.碰撞检测

猜你喜欢