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