目录
1、PRM(Probabilistic Roadmap Method)
大佬博主博客：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法_自动驾驶驾驶动态规划算法-CSDN博客
一、基于图搜索的global planner
1、Dijkstra
参考文献：
[1]https://ieeexplore.ieee.org/document/736775
[2]https://ieeexplore.ieee.org/document/1690266
#include<cstdio>
#include<cstring>
#include<algorithm>
#include<iostream>
#define Inf 0x3f3f3f3f
using namespace std;
int map[1005][1005];
int vis[1005], dis[1005];
int n, m;
void Init()
{
memset(map, Inf, sizeof(map));
for (int i = 1; i <= n; i++)
{
map[i][i] = 0;
}
}
void Getmap()
{
int u, v, w;
for (int t = 1; t <= m; t++)
{
scanf_s("%d%d%d", &u, &v, &w);
if (map[u][v] > w)
{
map[u][v] = w;
map[v][u] = w;
}
}
}
void Dijkstra(int u)
{
memset(vis, 0, sizeof(vis));
for (int t = 1; t <= n; t++)
{
dis[t] = map[u][t];
}
vis[u] = 1;
for (int t = 1; t < n; t++)
{
int minn = Inf, temp;
for (int i = 1; i <= n; i++)
{
if (!vis[i] && dis[i] < minn)
{
minn = dis[i];
temp = i;
}
}
vis[temp] = 1;
for (int i = 1; i <= n; i++)
{
if (map[temp][i] + dis[temp] < dis[i])
{
dis[i] = map[temp][i] + dis[temp];
}
}
}
}
int main()
{
/*
input；
5 4
1 2 3
2 3 4
3 4 1
4 1 8
2 4 2*/
scanf_s("%d%d", &m, &n);//5,4
Init();
Getmap();
Dijkstra(n);//4
printf("%d\n", dis[1]);
return 0;
} 
2、A* 家族
2.1A*
参考文献：[1]https://www.roboticsproceedings.org/rss04/p28.pdf
#include <stdio.h>
#include <vector>
using namespace std;
#define ROWS 10
#define COLS 10
#define ZXDJ 10
#define XXDJ 14
enum direct { p_up, p_down, p_left, p_right, p_lup, p_ldown, p_rup, p_rdown };
struct MyPoint {
int row; //y
int col; //x
int f, g, h; //用来量化评估的
void getF() { f = g + h; }
};
//树节点
struct treeNode {
MyPoint pos;
treeNode* pParent;
vector<treeNode*> child;
};
treeNode* createNode(MyPoint pos) {
treeNode* pNew = new treeNode;
//memset(pNew, 0, sizeof treeNode);//清空
*pNew = { 0 };
pNew->pos.row = pos.row;
pNew->pos.col = pos.col;
return pNew;
}
int getH(MyPoint pos, MyPoint endPos);
int main() {
//描述地图
int map[ROWS][COLS] = {
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
{ 0, 0, 1, 1, 1, 0, 0, 0, 0, 0 },
{ 0, 1, 0, 1, 1, 0, 0, 0, 0, 0 },
{ 0, 1, 1, 0, 1, 0, 0, 0, 0, 0 },
{ 0, 0, 1, 1, 1, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 },
{ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0 }
};
bool pathMap[ROWS][COLS] = { 0 };//全部都是false 没有走过
//起点和终点
MyPoint begPos = { 1, 1 };
MyPoint endPos = { 6, 7 };
pathMap[begPos.row][begPos.col] = true;//起点标记走过
treeNode* pRoot = createNode(begPos);//起点成为树根
//准备一个数组用来存储并获取f最小的点
vector<treeNode*> buff;
vector<treeNode*>::iterator it;
vector<treeNode*>::iterator itMin;
treeNode* pCurrent = pRoot;//当前点
bool isFindEnd = false;
//寻路
while (1) {
//当前点发散出8个点
for (int i = 0; i < 8; i++) {
treeNode* pTemp = createNode(pCurrent->pos);
switch (i)
{
case p_up:
pTemp->pos.row--;
pTemp->pos.g += ZXDJ;
break;
case p_down:
pTemp->pos.row++;
pTemp->pos.g += ZXDJ;
break;
case p_left:
pTemp->pos.col--;
pTemp->pos.g += ZXDJ;
break;
case p_right:
pTemp->pos.col++;
pTemp->pos.g += ZXDJ;
break;
case p_lup:
pTemp->pos.row--;
pTemp->pos.col--;
pTemp->pos.g += XXDJ;
break;
case p_ldown:
pTemp->pos.row++;
pTemp->pos.col--;
pTemp->pos.g += XXDJ;
break;
case p_rup:
pTemp->pos.row--;
pTemp->pos.col++;
pTemp->pos.g += XXDJ;
break;
case p_rdown:
pTemp->pos.row++;
pTemp->pos.col++;
pTemp->pos.g += XXDJ;
break;
}//end of switch
//能走的才考虑 已经走过的应该不考虑
if (pathMap[pTemp->pos.row][pTemp->pos.col] == false &&
map[pTemp->pos.row][pTemp->pos.col] == 0 &&
pTemp->pos.row >= 0 && pTemp->pos.row < ROWS &&
pTemp->pos.col >= 0 && pTemp->pos.col < COLS) {
//计算h值
pTemp->pos.h = getH(pTemp->pos, endPos);
//计算f值
pTemp->pos.getF();
//入树
pCurrent->child.push_back(pTemp);
pTemp->pParent = pCurrent;
//数组存储
buff.push_back(pTemp);
}
else {//不能走
//delete pTemp;//释放pTemp
}
}//end of for
//找出buff中 f最小的节点
itMin = buff.begin();//假设第一个的f值最小
//从第一个找到最后一个
for (it = buff.begin(); it != buff.end(); it++) {
itMin = (((*itMin)->pos.f < (*it)->pos.f) ? itMin : it);
}
//pCurrent指向它
pCurrent = *itMin;
pathMap[pCurrent->pos.row][pCurrent->pos.col] = true;//标记走过
printf("current(%d,%d)\n", pCurrent->pos.row, pCurrent->pos.col);
//buff中删掉它
buff.erase(itMin);
//是否到达终点
if (pCurrent->pos.row == endPos.row &&
pCurrent->pos.col == endPos.col) {
isFindEnd = true;
break;
}
//是否buff空了
if (buff.size() == 0) break;
}//end of while
if (isFindEnd) {
printf("找到终点了!\n");
while (pCurrent) {
printf("(%d,%d)", pCurrent->pos.row, pCurrent->pos.col);
pCurrent = pCurrent->pParent;
}
printf("\n");
}
while (1);
return 0;
}
int getH(MyPoint pos, MyPoint endPos) {
int x = ((pos.col > endPos.col) ? (pos.col - endPos.col) : (endPos.col - pos.col));
int y = ((pos.row > endPos.row) ? (pos.row - endPos.row) : (endPos.row - pos.row));
return (x + y) * ZXDJ;
} 
2.2动态A*(D*)
参考文献：https://ieeexplore.ieee.org/document/351061
#pragma once
#include<vector>
#include<queue>
#include<memory>
#include<algorithm>
#include<iostream>
namespace DStar {
enum State
{
New,
Open,
Closed
};
struct DNode {
explicit DNode(int X, int Y, DNode* Next = nullptr) : x(X), y(Y), next(Next) {}
DNode() = default;
void setObstacle()
{
is_obstacle = true;
}
int x = 0;
int y = 0;
int H = 0; // distance to target
int K = 0; //所有时刻最小的H！
State state = New;
DNode* next = nullptr;
bool is_obstacle = false;
};
struct MyCompare
{
bool operator()(DNode* lhs, DNode* rhs) const {
return lhs->K > rhs->K;
}
};
class DStar {
public:
DStar(const std::vector<std::vector<int>> map) : Map(map), shortestPaths(map.size(), std::vector<DNode*>(map.front().size(), nullptr)) {}
void find_first_path(const std::pair<int, int>& start, const std::pair<int, int>& target, const std::vector<std::pair<int, int>>& obstacles);
~DStar()
{
for (int i = 0; i < shortestPaths.size(); ++i)
{
for (int j = 0; j < shortestPaths.front().size(); ++j)
{
if (shortestPaths[i][j])
delete shortestPaths[i][j];
}
}
}
private:
int cost(DNode* lhs, DNode* rhs) const;
void insert(DNode* node, int Cost);
//核心函数
int process_state();
int modify(DNode* node);
void setObstacles(const std::vector<std::pair<int, int>>& obstacles);
private:
const static int _cost_low = 10;
const static int _cost_high = 14;
const static int _cost_max = 65536;
std::priority_queue<DNode*, std::vector<DNode*>, MyCompare> openlist;
std::vector<std::vector<int>> Map;
std::vector<std::vector<DNode*>> shortestPaths; //保存每个节点到终点的最短路径信息
};
int DStar::cost(DNode* lhs, DNode* rhs) const
{
if (lhs->is_obstacle || rhs->is_obstacle)
return _cost_max;
if (lhs->x != rhs->x && lhs->y != rhs->y)
return _cost_high;
else
return _cost_low;
}
void DStar::insert(DNode* node, int new_H)
{
new_H = (new_H >= _cost_max) ? _cost_max : new_H;
if (node->state == New)
{
node->H = new_H;
node->K = new_H;
node->state = Open;
shortestPaths[node->x][node->y] = node;
openlist.push(node);
}
else if (node->state == Open) //节点已经在openlist中 那么只需要改变对应openlist位置的节点的H和K即可
{
node->H = new_H;
node->K = std::min(node->K, new_H);
}
else
{
node->K = std::min(node->H, new_H);
node->H = new_H;
node->state = Open;
openlist.push(node);
}
}
int DStar::process_state()
{
DNode* cur_min = openlist.top();
if (!cur_min)
return -1;
openlist.pop();
cur_min->state = Closed;
int K_old = cur_min->K;
if (K_old < cur_min->H)
{
for (int i = cur_min->x - 1; i <= cur_min->x + 1; ++i)
{
for (int j = cur_min->y - 1; j <= cur_min->y + 1; ++j)
{
if (i >= 0 && j >= 0 && i < Map.size() && j < Map.front().size() && shortestPaths[i][j])
{
if (i == cur_min->x && j == cur_min->y)
continue;
if (shortestPaths[i][j]->H <= K_old && cur_min->H > shortestPaths[i][j]->H + cost(cur_min, shortestPaths[i][j]))
{
cur_min->H = shortestPaths[i][j]->H + cost(cur_min, shortestPaths[i][j]);
cur_min->next = shortestPaths[i][j];
}
}
}
}
}
if (K_old == cur_min->H)
{
for (int i = cur_min->x - 1; i <= cur_min->x + 1; ++i)
{
for (int j = cur_min->y - 1; j <= cur_min->y + 1; ++j)
{
if (i >= 0 && j >= 0 && i < Map.size() && j < Map.front().size())
{
if (i == cur_min->x && j == cur_min->y)
continue;
if (!shortestPaths[i][j] && !Map[i][j]) //表示当前节点为New
{
DNode* neighbor = new DNode(i, j, cur_min);
insert(neighbor, cur_min->H + cost(neighbor, cur_min));
}
else if (shortestPaths[i][j] && !shortestPaths[i][j]->is_obstacle)
{
if ((shortestPaths[i][j]->next == cur_min && shortestPaths[i][j]->H != cur_min->H + cost(shortestPaths[i][j], cur_min))
|| (shortestPaths[i][j]->next != cur_min && shortestPaths[i][j]->H > cur_min->H + cost(shortestPaths[i][j], cur_min)))
{
insert(shortestPaths[i][j], cur_min->H + cost(shortestPaths[i][j], cur_min));
shortestPaths[i][j]->next = cur_min;
}
}
}
}
}
}
else
{
//传递raise状态
for (int i = cur_min->x - 1; i <= cur_min->x + 1; ++i)
{
for (int j = cur_min->y - 1; j <= cur_min->y + 1; ++j)
{
if (i == cur_min->x && j == cur_min->y)
continue;
if (i >= 0 && j >= 0 && i < Map.size() && j < Map.front().size())
{
if (!shortestPaths[i][j] && !Map[i][j]) //表示当前节点为New
{
DNode* neighbor = new DNode(i, j, cur_min);
insert(neighbor, cur_min->H + cost(neighbor, cur_min));
}
else if (shortestPaths[i][j] && !shortestPaths[i][j]->is_obstacle)
{
if (shortestPaths[i][j]->next == cur_min && shortestPaths[i][j]->H != cur_min->H + cost(cur_min, shortestPaths[i][j]))
{
insert(shortestPaths[i][j], cur_min->H + cost(cur_min, shortestPaths[i][j]));
}
else
{
if (shortestPaths[i][j]->next != cur_min)
{
int temp = (shortestPaths[i][j]->H + cost(shortestPaths[i][j], cur_min) >= _cost_max) ? _cost_max : shortestPaths[i][j]->H + cost(shortestPaths[i][j], cur_min);
if (cur_min->H < temp)
insert(cur_min, cur_min->H);
else if (shortestPaths[i][j]->next != cur_min && cur_min->H > shortestPaths[i][j]->H + cost(shortestPaths[i][j], cur_min)
&& shortestPaths[i][j]->state == Closed && shortestPaths[i][j]->K > K_old)
{
insert(shortestPaths[i][j], shortestPaths[i][j]->H);
}
}
}
}
}
}
}
}
return openlist.top()->K;
}
void DStar::setObstacles(const std::vector<std::pair<int, int>>& obstacles)
{
if (obstacles.empty())
return;
for (const auto& rhs : obstacles)
{
if (shortestPaths[rhs.first][rhs.second])
{
shortestPaths[rhs.first][rhs.second]->is_obstacle = true;
shortestPaths[rhs.first][rhs.second]->H = _cost_max;
}
Map[rhs.first][rhs.second] = '#';
}
}
int DStar::modify(DNode* node)
{
DNode* next = node->next;
openlist.push(next);
int k_old;
do {
k_old = process_state();
if (k_old == -1)
break;
} while (node->H == _cost_max || k_old < node->H);
return k_old;
}
void DStar::find_first_path(const std::pair<int, int>& start, const std::pair<int, int>& target,
const std::vector<std::pair<int, int>>& obstacles)
{
DNode* end = new DNode(target.first, target.second);
shortestPaths[target.first][target.second] = end;
openlist.push(end);
while (true)
{
process_state();
if (shortestPaths[start.first][start.second] && shortestPaths[start.first][start.second]->state == Closed)
break;
}
setObstacles(obstacles);
DNode* begin = shortestPaths[start.first][start.second];
int i;
while (begin)
{
Map[begin->x][begin->y] = '*';
if (begin->next && begin->next->is_obstacle)
{
i = modify(begin);
if (i == -1)
{
break;
}
}
begin = begin->next;
}
if (i == -1)
{
std::cout << "No New Path To Target" << std::endl;
return;
}
else
{
for (const auto& i : Map)
{
for (const auto& j : i)
{
if (j > 1)
std::cout << char(j) << " ";
else
std::cout << j << " ";
}
std::cout << std::endl;
}
}
}
}
#include <time.h>
#include"DStar.h"
int main()
{
std::vector<std::vector<int>> map2(25, std::vector<int>(25));
for (int j = 3; j < 8; ++j)
map2[6][j] = 1;
for (int j = 4; j < 7; ++j)
map2[7][j] = 1;
clock_t start = clock();
DStar::DStar d(map2);
d.find_first_path({ 3, 2 }, { 23, 16 }, { {12, 4}, {12, 5}, {12, 6}, {12, 7}, {12, 8}, {12, 9}, {12, 10}, {12, 11}, {11, 7}, {13, 3}, {10, 6}, {14, 5}, {6, 2} });
clock_t ends = clock();
std::cout << "Running Time : " << (double)(ends - start) / CLOCKS_PER_SEC * 1000 << "ms" << std::endl;
return 0;
}原文链接：基于C++的D*算法详解_dstar算法-CSDN博客
2.3 Field D∗
参考文献：https://www.ri.cmu.edu/pub_files/pub4/ferguson_david_2006_3/ferguson_david_2006_3.pdf
原文链接：Field Ｄ*路径规划算法_field d*-CSDN博客
2.4Theta*
参考文献：http://idm-lab.org/bib/abstracts/papers/aaai07a.pdf
扩展：Lazy Theta*
2.5ARA*和AD*
参考文献：https://www.sciencedirect.com/science/article/pii/S000437020800060X
2.6A*结合voronoi图
参考文献：https://ieeexplore.ieee.org/document/4621302
2.7Hybrid A*
参考文献：http://robots.stanford.edu/papers/junior08.pdf
3、State Lattice
参考文献：https://www.cs.cmu.edu/~alonzo/pubs/papers/TrajGenSubmitFinalCompressed.pdf
二、基于采样的global planner
1、PRM(Probabilistic Roadmap Method)
2、RRT
参考文献：Randomized kinodynamic planning | IEEE Conference Publication | IEEE Xplore
3、RRT*
三、基于插值的曲线planner
1、Lines and Circles
2、 Clothoid Curves
3、多项式差值曲线（Polynomial Curves）
参考文献：https://www.ri.cmu.edu/pub_files/2012/5/ICRA12_xuwd_Final.pdf
4. 贝塞尔曲线（Bézier Curves）
5. 样条曲线（Spline Curves）
参考文献： Pythagorean—Hodograph Curves
四、基于数值优化的planner（最优化理论）
需要构建优化模型，设置目标函数，并考虑各种约束，比如运动学、动力学、障碍物、舒适性、安全性等等。
五、总结
最常用的是基于插值的曲线路径生成，其次是基于图搜索的路径规划，其中最长用的是lattice planner。
从控制的角度来看，必须考虑多种约束，从车辆运动学限制到乘客舒适度。最近的发展旨在在规划阶段考虑这些限制因素，从而实现平滑和可实现的轨迹，减少控制阶段的限制。
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
