A*算法项目实践之四:将栅格路径转换为直角坐标下的路径
在上文—— A*算法项目实践之三:优化A*的方法——不只是双向A*,我们优化加速A*算法后得到了目标栅格路径,接下需要将栅格路径转换为直角坐标下的路径,本文就笔者实际项目的一些经验来谈谈相关的方法,若有错误,请在评论区提醒笔者予以斧正 。
本项目基于VS2017,整个项目的代码以及上传到码云: A算法项目实践(正在更新中~)
提取栅格路径的转弯栅格
栅格路径是搜索算法基于栅格地图得到的的一个栅格序列(数组),在A*算法项目实践之一:栅格法的使用与障碍物栅格的生成介绍了如何将实际二维地图转换为栅格地图,栅格路径无法应用于实际地图中,需要将其进行处理——将栅格路径转换为直角坐标系下的路径。
那么直角坐标系下的路径是什么呢?相信大家都用过导航软件,仔细想想,导航软件给出的路径实质上就是多个转弯点与它们之间的连线。
因此,提取出栅格路径中的转弯栅格,并将转弯栅格坐标转换为直角坐标,就能得到直角坐标系下的路径。
首先,需要明确转弯栅格的定义,对栅格路径path:
转弯栅格p的定义:
(1)p 是栅格路径 path 序列(数组)的一个元素;
(2)p 相对于前一个栅格的方向与下一个栅格相对于 p 的方向不同;
(3)栅格路径 path 经过该栅格之后,相邻栅格的相对方向(后栅格相对于前栅格)保持不变,且不变的栅格数量*栅格宽度不小于载具车长的1/4;
定义中的(1)不难理解,(2)中,两个相邻栅格的方向有8种,如下图所示,有上、下、左、右、上左、上右、下左和下右
(3)的解释为:
- 经过对A*算法的优化加速,其最终搜索出的路径有许多小幅度的“转向”,例如栅格路径某一段先往右2个栅格再往上5个栅格,这在算法中可能很常见,而载具(例如无人船、无人车等)实际的行驶要求得到的直角坐标系的路径不应有太多的转向操作,因此需要删除这类小幅度“转向”;
- 这里判断小幅度“转向”用一句话概括为“栅格路径发生转向后,需要保持其方向行进,且行进距离至少能够超过载具车长的1/4”,这里笔者定下的阈值为载具车长的1/4,读者可根据实际情况进行调整。
然后,确定提取栅格路径转弯栅格的算法:
初始化direction_counts[8]全为0,用来记录记录8个方向的行进栅格数;
初始化栅格对象last,用来存储上一个栅格对象;
初始化nodesOfDirectionChange[]为空,用来存储转弯栅格;
从头遍历栅格路径,对每一个栅格对象p:
若p为起点,则将p存入nodesOfDirectionChange;
若p为终点,则判断 direction_counts[] 的最大值 * 栅格宽度是否小于车长的 1 / 4,若是,则删除 nodesOfDirectionChange 的最后一个栅格;
若p为中间节点,则:
计算 p 是相对于 last (前一个栅格)的方向 direction_type;
若 direction_counts[direction_type] 为 0,则表示发生了转向:
首先判断上一个转弯栅格是否需要删除:计算 direction_counts[] 的最大值*栅格宽度是否小于车长的 1 / 4,若是,则删除 nodesOfDirectionChange 的最后一个栅格;
然后将 direction_counts[] 置为全0;
direction_counts[ direction_type ]++;
若 direction_counts[] 的最大值为 1,表示发生了转向,将 last 栅格存入 nodesOfDirectionChange[];
更新 last 栅格为 p;
最后,代码实现:
//************************************
// Method: GetTurningPointsByPath
// FullName: GuidePointGenerator::GetTurningPointsByPath
// Access: private
// Returns: void
// Qualifier:根据路径栅格点序列获得转向点
// Parameter: Point start 起点
// Parameter: Point end 终点
// Parameter: list<Point * > & path 路径栅格点序列
// Parameter: vector<Point> & nodesOfDirectionChange 转向点序列
//************************************
void GuidePointGenerator::GetTurningPointsByPath(Point start, Point end, list<Point> &path, vector<Point> &nodesOfDirectionChange)
{
//方向左上方为0,按行列顺序增加:0 1 2 3 4 5 6 7 表示 左上 上 右上 左 右 左下 下 右下
//记录8个方向的行进栅格数
vector<int> direction_counts(8, 0);
Point last(0, 0);
for (auto &p : path)
{
if (p.x == start.x && p.y == start.y)//如果是起始点,将起点保存
{
//保存起点
direction_counts[4]++;
nodesOfDirectionChange.push_back(p);
}
else if (p.x == end.x && p.y == end.y)//如果是终点,将终点保存
{
//判断上一个转向栅格是否满足条件
int max_len = *max_element(direction_counts.begin(), direction_counts.end());
if (max_len * 0.2 < car_length / 4 )
nodesOfDirectionChange.pop_back();
//保存终点
nodesOfDirectionChange.push_back(p);
}
else//如果是中间栅格
{
//判断当前栅格是上一栅格的那个方向
//方向左上方为0,按行列顺序增加:0 1 2 3 4 5 6 7 表示 左上 上 右上 左 右 左下 下 右下
int direction_type = 0;
for (int x = last.x - 1