机器人路径规划算法c++
时间: 2024-01-31 21:10:59 浏览: 37
机器人路径规划算法在C++中有多种实现方式,其中比较常用的算法包括A*算法、Dijkstra算法和RRT算法。下面是一个使用A*算法实现机器人路径规划的C++代码示例:
```cpp
#include <iostream>
#include <vector>
#include <queue>
#include <cmath>
using namespace std;
// 定义节点结构体
struct Node {
int x; // 节点的x坐标
int y; // 节点的y坐标
double g; // 从起点到该节点的实际代价
double h; // 从该节点到目标节点的估计代价
double f; // f = g + h,综合考虑实际代价和估计代价的总代价
Node* parent; // 父节点指针
Node(int x, int y) : x(x), y(y), g(0), h(0), f(0), parent(nullptr) {}
};
// 计算两个节点之间的欧几里得距离
double calcDistance(Node* node1, Node* node2) {
int dx = node1->x - node2->x;
int dy = node1->y - node2->y;
return sqrt(dx * dx + dy * dy);
}
// A*算法实现机器人路径规划
vector<Node*> AStar(Node* start, Node* goal, vector<vector<int>>& grid) {
// 定义开放列表和关闭列表
vector<Node*> openList;
vector<Node*> closedList;
// 将起点加入开放列表
openList.push_back(start);
// 开始搜索路径
while (!openList.empty()) {
// 从开放列表中选择f值最小的节点作为当前节点
Node* current = openList[0];
int currentIndex = 0;
for (int i = 1; i < openList.size(); i++) {
if (openList[i]->f < current->f) {
current = openList[i];
currentIndex = i;
}
}
// 将当前节点从开放列表中移除,并加入关闭列表
openList.erase(openList.begin() + currentIndex);
closedList.push_back(current);
// 判断是否到达目标节点
if (current == goal) {
// 构建路径
vector<Node*> path;
Node* node = current;
while (node != nullptr) {
path.push_back(node);
node = node->parent;
}
return path;
}
// 获取当前节点的相邻节点
vector<Node*> neighbors;
int dx[4] = {1, -1, 0, 0};
int dy[4] = {0, 0, 1, -1};
for (int i = 0; i < 4; i++) {
int nx = current->x + dx[i];
int ny = current->y + dy[i];
if (nx >= 0 && nx < grid.size() && ny >= 0 && ny < grid[0].size() && grid[nx][ny] == 0) {
Node* neighbor = new Node(nx, ny);
neighbors.push_back(neighbor);
}
}
// 遍历相邻节点
for (Node* neighbor : neighbors) {
// 判断相邻节点是否在关闭列表中
bool inClosedList = false;
for (Node* node : closedList) {
if (neighbor->x == node->x && neighbor->y == node->y) {
inClosedList = true;
break;
}
}
if (inClosedList) {
continue;
}
// 计算相邻节点的实际代价和估计代价
double g = current->g + calcDistance(current, neighbor);
double h = calcDistance(neighbor, goal);
double f = g + h;
// 判断相邻节点是否在开放列表中
bool inOpenList = false;
for (Node* node : openList) {
if (neighbor->x == node->x && neighbor->y == node->y) {
inOpenList = true;
break;
}
}
// 如果相邻节点不在开放列表中或者新的路径代价更小,则更新节点信息
if (!inOpenList || g < neighbor->g) {
neighbor->g = g;
neighbor->h = h;
neighbor->f = f;
neighbor->parent = current;
// 如果相邻节点不在开放列表中,则加入开放列表
if (!inOpenList) {
openList.push_back(neighbor);
}
}
}
}
// 如果开放列表为空,表示无法找到路径
return vector<Node*>();
}
int main() {
// 定义地图
vector<vector<int>> grid = {
{0, 0, 0, 0, 0},
{0, 1, 1, 0, 0},
{0, 0, 0, 0, 0},
{0, 0, 1, 1, 0},
{0, 0, 0, 0, 0}
};
// 定义起点和目标点
Node* start = new Node(0, 0);
Node* goal = new Node(4, 4);
// 使用A*算法进行路径规划
vector<Node*> path = AStar(start, goal, grid);
// 输出路径
if (!path.empty()) {
cout << "Path found:" << endl;
for (int i = path.size() - 1; i >= 0; i--) {
cout << "(" << path[i]->x << ", " << path[i]->y << ")" << endl;
}
} else {
cout << "Path not found." << endl;
}
// 释放内存
delete start;
delete goal;
for (Node* node : path) {
delete node;
}
return 0;
}
```