【计算机视觉】--路径规划

简介

OpenCV实现路径规划算法时，通常会使用图像/图形来表示环境，然后在该该图像/图形上进行路径搜索。

1、加载地图

imread函数加载地图图像，确保每个像素代表地图的一个单元。白色像素表示可通过区域，黑色像素表示障碍物或不可通过区域。

2、定义起点和目标

在地图图像上选择起点和目标位置。

3、创建数据结构

为路径规划算法创建必要的数据结构。我们需要定义一个节点（Node）类，该类包含节点的位置、代价值和父节点等信息。还需要创建一个开放集（Open Set）和一个关闭集（Closed Set），用于存储待探索和已探索的节点。

4、算法实现

	· 将起点添加到开放集中，并初始化起点的代价值和启发式值（通常使用欧几里得距离或曼哈顿距离）。
	· 循环执行以下步骤，直到找到目标节点或开放集为空：
		· 从开放集中选择具有最低总代价值的节点作为当前节点。
		· 如果当前节点是目标节点，则表示路径已找到，停止搜索。
		· 否则，将当前节点从开放集中移除，并将其添加到关闭集中。
		·生成当前节点的邻居节点（上、下、左、右、斜对角等方向）。
		·对于每个邻居节点，计算代价值和启发式值，并更新节点的代价值和父节点。
		· 如果邻居节点已在开放集或关闭集中，则比较新的代价值与已有的代价值，保留较小的代价值。
		· 如果邻居节点不在开放集中，则将其添加到开放集中。

5、生成路径

当搜索结束后，如果找到了目标节点，则可以通过回溯父节点的方式生成路径。从目标节点开始，依次跟踪父节点指针，回溯到起点节点，形成完整的路径。

6、可视化路径

绘图函数（如line函数）将生成的路径在地图上可视化。

C++实现

struct Node {
    cv::Point position;
    double cost;
    double heuristic;
    Node* parent;

    Node(cv::Point pos, double cost, double heuristic, Node* parent = nullptr)
        : position(pos), cost(cost), heuristic(heuristic), parent(parent) {}

    double getTotalCost() const { return cost + heuristic; }
};

bool isValidCell(const cv::Mat& map, int x, int y) {
    if (x >= 0 && x < map.rows && y >= 0 && y < map.cols) {
        return map.at<uchar>(x, y) != 0;
    }
    return false;
}

double computeHeuristic(const cv::Point& p1, const cv::Point& p2) {
    // 曼哈顿距离
    return std::abs(p1.x - p2.x) + std::abs(p1.y - p2.y);
}

std::vector<cv::Point> findPath(const cv::Mat& map, const cv::Point& start, const cv::Point& goal) {
    std::vector<cv::Point> path;
    std::vector<Node*> openSet;
    std::vector<Node*> closedSet;

    Node* startNode = new Node(start, 0.0, computeHeuristic(start, goal));
    openSet.push_back(startNode);

    while (!openSet.empty()) {
        Node* current = openSet[0];
        int currentIndex = 0;
        for (int i = 1; i < openSet.size(); ++i) {
            if (openSet[i]->getTotalCost() < current->getTotalCost()) {
                current = openSet[i];
                currentIndex = i;
            }
        }

        openSet.erase(openSet.begin() + currentIndex);

        closedSet.push_back(current);
        
        if (current->position == goal) {
            Node* node = current;
            while (node != nullptr) {
                path.push_back(node->position);
                node = node->parent;
            }

            for (Node* node : openSet) {
                delete node;
            }
            for (Node* node : closedSet) {
                delete node;
            }

            return path;
        }

        std::vector<cv::Point> offsets = {
            cv::Point(-1, 0), cv::Point(1, 0), cv::Point(0, -1), cv::Point(0, 1),
            cv::Point(-1, -1), cv::Point(-1, 1), cv::Point(1, -1), cv::Point(1, 1)
        };
        for (const cv::Point& offset : offsets) {
            int x = current->position.x + offset.x;
            int y = current->position.y + offset.y;

            if (isValidCell(map, x, y)) {
                double cost = current->cost + 1.0;  // Assuming constant cost for each movement
                double heuristic = computeHeuristic(cv::Point(x, y), goal);
                Node* successor = new Node(cv::Point(x, y), cost, heuristic, current);
                bool skip = false;

                for (const Node* closedNode : closedSet) {
                    if (successor->position == closedNode->position && successor->getTotalCost() >= closedNode->getTotalCost()) {
                        skip = true;
                        break;
                    }
                }

                if (skip) {
                    delete successor;
                    continue;
                }

                for (Node* openNode : openSet) {
                    if (successor->position == openNode->position && successor->getTotalCost() >= openNode->getTotalCost()) {
                        skip = true;
                        break;
                    }
                }

                if (skip) {
                    delete successor;
                    continue;
                }

                // Add successor to the open set
                openSet.push_back(successor);
            }
        }
    }

    for (Node* node : openSet) {
        delete node;
    }
    for (Node* node : closedSet) {
        delete node;
    }

    return path;  
}

int main() {
    cv::Mat map = cv::imread("map.png", cv::IMREAD_GRAYSCALE);
    if (map.empty()) {
        std::cout << "Failed to load map image!" << std::endl;
        return 1;
    }

    cv::Point start(10, 10);
    cv::Point goal(200, 200);

    std::vector<cv::Point> path = findPath(map, start, goal);

    // Visualize the path on the map
    cv::cvtColor(map, map, cv::COLOR_GRAY2BGR);
    for (int i = 1; i < path.size(); ++i) {
        cv::line(map, path[i - 1], path[i], cv::Scalar(0, 0, 255), 2);
    }
}

这是一个简单的路径规划算法实现，可以根据实际需求进行扩展和优化。路径规划是一个广泛的领域，有很多更复杂和高级的算法可以用于不同的应用场景。