Tare_Planner的学习笔记(三)
CMU的自主环境探索利用matlab生成前向三次样条路径点
在CMU开源的自主环境探索算法中，机器人或者无人机的主要是靠matlab进行前向三次样条曲线生成路径点的方式进行的，这部分程序在matlab中运行并且生成相应的文件，从而为local_planner提供帮助。
1.采样路径的生成
利用车辆的运动学特性，进行时间维度上的离散，从而通过预测得到未来一段时间机器人的状态，从而实现了采样路径的生成。下面首先分析这一部分的代码。
dis = 1.0; // 前向预测的距离 单位:mangle = 27;// 车辆转向的角度 单位:度deltaAngle = angle / 3;// 将角度按照3°进行平分scale = 0.65;// 角度的缩放系数在上述定义了一些机器人的初始的状态参数，可以根据你的机器人进行参数的修改
pathStartAll = zeros(4, 0);pathAll = zeros(5, 0);pathList = zeros(5, 0);pathID = 0;groupID = 0;这些是一些关于路径输出的一些存储值。
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]; for shift2 = -angle * scale + shift1 : deltaAngle * scale : angle * scale + shift1 for shift3 = -angle * scale^2 + shift2 : deltaAngle * scale^2 : angle * scale^2 + shift2 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); pathShift = spline(waypts(:, 1), waypts(:, 2), pathR); pathX = pathR .* cos(pathShift * pi / 180); pathY = pathR .* sin(pathShift * pi / 180); pathZ = zeros(size(pathX)); path = [pathX; pathY; pathZ; ones(size(pathX)) * pathID; ones(size(pathX)) * groupID]; pathAll = [pathAll, path]; pathList = [pathList, [pathX(end); pathY(end); pathZ(end); pathID; groupID]]; pathID = pathID + 1; plot3(pathX, pathY, pathZ); end end groupID = groupID + 1end上述代码开启了三个for循环，也是路径生成的主要函数，我们一个一个for循环来进行解析；
wayptsStart = [0, 0, 0; dis, shift1, 0]; pathStartR = 0 : 0.01 : dis; pathStartShift = spline(wayptsStart(:, 1), wayptsStart(:, 2), pathStartR);上述中定义了wayptsStart矩阵，然后对(0,0)到(1,-27)按照0.01进行了数据的插值.
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];这些变量主要是进行路径起始点的转换与存储的，最终所有的路径都存在了pathStartAll变量中。
for shift2 = -angle * scale + shift1 : deltaAngle * scale : angle * scale + shift1 for shift3 = -angle * scale^2 + shift2 : deltaAngle * scale^2 : angle * scale^2 + shift2 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); pathShift = spline(waypts(:, 1), waypts(:, 2), pathR); pathX = pathR .* cos(pathShift * pi / 180); pathY = pathR .* sin(pathShift * pi / 180); pathZ = zeros(size(pathX)); path = [pathX; pathY; pathZ; ones(size(pathX)) * pathID; ones(size(pathX)) * groupID]; pathAll = [pathAll, path]; pathList = [pathList, [pathX(end); pathY(end); pathZ(end); pathID; groupID]]; pathID = pathID + 1; plot3(pathX, pathY, pathZ); end end这是内层的两个循环，可以看到shift2在一个值固定的情况下，shift3开始循环，按照上述的循环方式，一次shift3生成一条路径，编号加1，这样一个固定的shift2的情况下，会生成7条路径，而一个shift2循环会循环7次，所以在两层内循环中会生成49条路径。此时路径组的groupID=0，path_IDmax =49。由于在插值的时候，每一个shitf2下的七条路径都会经经过(1，27)点和(2，shift2)点，所以就会出现下图中每组路径有两个交点的情况。至于为什么这么设计，楼主也没想明白，根据后续算法需求，需要进行探测距离的转换，所以才这么设计了交点吧。
整个循环结束后，我们可以得到如下的路径图。
从上图中可以看出，总共有7个group，每个group有49条路径，总共有343条路径。如果是无人的话，由于具有z轴方向的路径，所以无人机的路径会更多。
fileID = fopen('startPaths.ply', 'w');fprintf(fileID, 'ply\n');fprintf(fileID, 'format ascii 1.0\n');fprintf(fileID, 'element vertex %d\n', size(pathStartAll, 2));fprintf(fileID, 'property float x\n');fprintf(fileID, 'property float y\n');fprintf(fileID, 'property float z\n');fprintf(fileID, 'property int group_id\n');fprintf(fileID, 'end_header\n');fprintf(fileID, '%f %f %f %d\n', pathStartAll);fclose(fileID);fileID = fopen('paths.ply', 'w');fprintf(fileID, 'ply\n');fprintf(fileID, 'format ascii 1.0\n');fprintf(fileID, 'element vertex %d\n', size(pathAll, 2));fprintf(fileID, 'property float x\n');fprintf(fileID, 'property float y\n');fprintf(fileID, 'property float z\n');fprintf(fileID, 'property int path_id\n');fprintf(fileID, 'property int group_id\n');fprintf(fileID, 'end_header\n');fprintf(fileID, '%f %f %f %d %d\n', pathAll);fclose(fileID);fileID = fopen('pathList.ply', 'w');fprintf(fileID, 'ply\n');fprintf(fileID, 'format ascii 1.0\n');fprintf(fileID, 'element vertex %d\n', size(pathList, 2));fprintf(fileID, 'property float end_x\n');fprintf(fileID, 'property float end_y\n');fprintf(fileID, 'property float end_z\n');fprintf(fileID, 'property int path_id\n');fprintf(fileID, 'property int group_id\n');fprintf(fileID, 'end_header\n');fprintf(fileID, '%f %f %f %d %d\n', pathList);fclose(fileID);从上述程序中可以看出存储了三个文件startPaths.ply,paths.ply,pathList.ply 三个文件，其中startPaths.ply中存储的是记录了第一次采样的路径点，是一个4×707的矩阵，其中一个矩阵大小为{x,y,z,groupID}。这里解释一下为啥是707个矩阵，是一个这个路径只保存了0-1之间的路径，一条路径只有101个点，所以7组路径是707个路径点。
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); endend面对scaleY的计算详细说明：
根据相似三角形，可以算出黑色框包围的地方，这些地方就是需要生成体素网格的地方，按照提速网格的大小可以直接遍历进行生成，如果需要推导公式，则参考链接(https://blog.csdn.net/qq_39266065/article/details/119734986?spm=1001.2014.3001.5502)。
3.碰撞检测
在生成体素网格后，为了在局部路径的筛选中更高效，将每个网格与路径索引离线计算。通过查找路径集合中的路径点周围指定距离内的邻居，并且该邻居点属于体素网格的集合。最终得到每一个体素网格与待遮挡路径之间的索引关系表。
rangesearch()函数：查找某个点周围指定距离内的所有邻居。
[ind, dis] = rangesearch(pathAll(1 : 2, :)', voxelPoints, searchRadius);最后记录了每个记录每个体素网格附近对应的路径号，从而生成了碰撞检测的文件。
