A* 算法优化

前面 A* 算法的效果可以看到，虽然能规划出路径，但是会出现路径紧贴障碍物的情况，导致路径并不完全安全

并且由于 A* 节点的拓展方式，会向周围拓展出很多的节点，对内存的压力较大

图中灰色部分为被拓展过的节点，假设路径较长，拓展的节点将会覆盖大部分的地图

优化代价函数

为了优化此问题，在代价函数中新增一项参数 $c$ ，总的代价函数可以描述为

f(n) = \frac{g(n) + h(n)}{c(n)}

其中 $c(n)$ 表示当节点距离最近的障碍物的距离，用来模拟对障碍物进行膨胀的过程，为了简化计算，我们可以依据需要膨胀的大小来引入一个参数 padding，取内边距的意思，通过如下步骤计算出 $c(n)$

首先根据设定的 padding 大小，节点以 $p$ 为半径的圆形范围的区域，寻找到距离当前节点最近的障碍物点

for (int i = -pad_radius_; i <= pad_radius_; ++i) {
    for (int j = -pad_radius_; j <= pad_radius_; ++j) {
        if (i == 0 && j == 0) { continue; }
        // ...
    }
}

假设找到了最近的障碍物点 $(i,j)$ , 根据其与节点的欧式距离 std::hypot(i, j)，计算得到 $c(n)$

auto distance = static_cast<uchar>(255 - 255 * (1 - std::hypot(i, j) / (pad_radius_ + 1)));

(pad_radius_ + 1) 避免除以 0
255 - 255 * (...) 将结果归一化到 $[0, 255]$ 范围
当最终结果越小，则表示当前节点越接近障碍物，越大则表示越远离，假设周围完全没有障碍物，则为最大值 255

对单独节点的计算结果放在一起，有点类似于 OpenCV 中的距离变换的效果，这里只对访问到的节点做这个操作

最终得到的 $c(n) 的计算代码为

    uint8_t AStar_Planner::astar_Padding_Cost(int x, int y) {
        uint8_t min_distance = 255;
        for (int i = -pad_radius_; i <= pad_radius_; ++i) {
            for (int j = -pad_radius_; j <= pad_radius_; ++j) {
                if (i == 0 && j == 0) { continue; }
                if (std::hypot(i, j) > pad_radius_) { continue; }
                int nx = x + i, ny = y + j;
                // 超出地图的点不考虑
                if (nx < 0 || nx >= (*grid_map_ptr_).width() || ny < 0 || ny >= (*grid_map_ptr_).height()) { continue; }
                if ((*grid_map_ptr_).get_Point_Val(nx, ny) == 0) {
                    auto distance = 255 - static_cast<uint8_t>(255 * (1 - std::hypot(i, j) / (pad_radius_ + 1)));
                    min_distance = std::min(distance, min_distance);
                }
            }
        }
        return min_distance;
    }