自定义路径生成和路径跟踪算法

一、预定义路径生成

确定起点和终点两个坐标点，然后在两者之间生成一系列目标点，这些点定义了期望的路径。

二、路径规划算法

• A*算法：适用于网格地图和复杂的路径规划，能够找到最短路径。
• Dijkstra算法：适用于找到从起点到终点的最短路径，但比A*算法慢。

三、路径跟踪 

路径生成了，需要跟踪这个路径。常用以下两种方法：

• 纯追踪：简单地朝着下一个目标点移动。
• PID控制器：使用比例-积分-微分控制器来平滑地引导实体沿着路径移动。

四、代码实现 

1、自定义结构体

struct Node {
    int x, y;
    double g, h, f;
    Node* parent;
};

在这段 C++ 代码中，Node 是一个自定义的结构体类型。

• x 和 y ：可能用于表示节点的坐标信息。
• g、h 和 f ：在路径规划算法，g 表示从起始节点到当前节点的实际代价，h 表示从当前节点到目标节点的估计代价，f 则是 g 和 h 的总和，用于决策节点的优先级或选择顺序。
• parent ：这是一个指向 Node 类型的指针，可能用于构建节点之间的关系。

 2、启发式值计算

// 启发式函数，使用曼哈顿距离
double heuristic(Node* a, Node* b) {
    return std::abs(a->x - b->x) + std::abs(a->y - b->y);
}

使用曼哈顿距离作为启发式函数。曼哈顿距离是计算两点在水平和垂直方向上距离之和，即 std::abs(a->x - b->x) + std::abs(a->y - b->y) 。路径搜索算法（如 A* 算法），启发式函数用于估计从一个节点到目标节点的距离或成本。通过提供一个合理的估计，可以帮助算法更有效地搜索解空间。

3、距离计算 

double distance(const Node& a, const Node& b) {
    return std::abs(a.x - b.x) + std::abs(a.y - b.y);
}

distance 的函数，其功能是计算两个 Node 对象 a 和 b 之间的距离。具体的计算方式是使用曼哈顿距离，即分别计算两个节点的 x 坐标和 y 坐标的差值的绝对值，然后将这两个差值的绝对值相加。distance 和 heuristic 在功能上是相似的，都是通过计算两个 Node 对象坐标差值的绝对值之和来得到一个值。

4、算法实现

// A*算法实现
Node* aStar(Node* start, Node* end, const std::vector<std::vector<char>>& grid) {
    std::priority_queue<Node*, std::vector<Node*>, Compare> openList;
    std::unordered_set<int> openSet;
    std::unordered_map<int, Node*> closedMap; // 使用 map 来跟踪 closedList

    start->g = 0;
    start->h = heuristic(*start, *end);
    start->f = start->g + start->h;

    openList.push(start);
    openSet.insert(start->index);

    while (!openList.empty()) {
        Node* current = openList.top();
        openList.pop();
        openSet.erase(current->index); // 从 openSet 中移除当前节点

        if (current == end) return current;

        closedMap[current->index] = current;

        for (Node* neighbor : getNeighbors(current, grid)) {
            int neighborIndex = neighbor->index;

            if (closedMap.find(neighborIndex) != closedMap.end()) continue; 
            double tentative_g = current->g + distance(*current, *neighbor);

            if (neighbor->g == 0 || tentative_g < neighbor->g) {
                neighbor->updateG(tentative_g);
                neighbor->h = heuristic(*neighbor, *end);
                neighbor->f = neighbor->g + neighbor->h;
                neighbor->parent = current;

                if (openSet.find(neighborIndex) == openSet.end()) {
                    openList.push(neighbor);
                    openSet.insert(neighborIndex);
                }
            }
        }
    }

    return nullptr;
}

std::vector<Node*> getNeighbors(const Node& node, const std::vector<std::vector<bool>>& grid) {
    std::vector<Node*> neighbors;
    int dx[] = {-1, 1, 0, 0};
    int dy[] = {0, 0, -1, 1};

    for (int i = 0; i < 4; ++i) {
        int nx = node.x + dx[i];
        int ny = node.y + dy[i];

        if (nx >= 0 && nx < grid.size() && ny >= 0 && ny < grid[0].size() && !grid[nx][ny]) {
            neighbors.push_back(new Node(nx, ny));
        }
    }

    return neighbors;
}

5、主函数调用

int main() {
    const int GRID_SIZE = 10;
    std::vector<std::vector<bool>> grid(GRID_SIZE, std::vector<bool>(GRID_SIZE, false));

    // 定义障碍物
    grid[5][5] = true;

    Node* start = new Node(0, 0);
    Node* end = new Node(9, 9);

    Node* path = aStar(start, end, grid);

    if (path) {
        Node* step = path;
        std::cout << "Path from " << start->x << ", " << start->y << " to " << end->x << ", " << end->y << ":\n";
        while (step) {
            std::cout << "(" << step->x << ", " << step->y << ") -> ";
            step = step->parent;
        }
        std::cout << "(end)" << std::endl;
    } else {
        std::cout << "No path found." << std::endl;
    }

    // 清理内存
    delete start;
    delete end;
    return 0;
}