99热这里：点燃激情，释放无限可能

路径规划是机器人学ã€�自动驾驶ã€�仓储物æµ�ã€�æ— äººæœºå¯¼èˆªç­‰é¢†åŸŸçš„æ ¸å¿ƒæŠ€æœ¯æ ¸å¿ƒç›®æ ‡æ˜¯ä¸ºç§»åŠ¨ä¸»ä½“æœºå™¨äººã€�车辆ã€�æ— äººæœºç­‰å¯»æ‰¾ä¸€æ�¡ä»�èµ·ç‚¹åˆ°ç›®æ ‡ç‚¹æ»¡è¶³æ— ç¢°æ’�ã€�最优性最短è·�离 / 时间 / 能耗ã€�å®�时性等约æ�Ÿæ�¡ä»¶çš„å�¯è¡Œè·¯å¾„。éš�ç�€äººå·¥æ™ºèƒ½ä¸�计算能力的æ��å�‡è·¯å¾„规划算法已ä»�ä¼ ç»Ÿçš„å›¾æ�œç´¢æ¼”进到è��å�ˆé‡‡æ ·ã€�智能优化ä¸�æ·±åº¦å­¦ä¹ çš„å¤šå…ƒåŒ–æ–¹å�‘。本文è�šç„¦C 语言å®�战工业级开å�‘首选兼顾å®�时性ä¸�å�¯ç§»æ¤�性ä»�å¼€å�‘视角系统梳ç�†æ ¸å¿ƒç®—法ã€�å®�ç�°æ­¥éª¤ä¸�工程优化技巧é�¿å¼€å¤�æ�‚å…¬å¼�侧é‡�å®�æ“�è�½åœ°æ‰€æœ‰ä»£ç �å�‡å�¯ç›´æ�¥å¤�制编译è¿�行适é…� ROS / 嵌入å¼�等工程场景助力开å�‘者快速上手路径规划算法的 C å¼€å�‘。一ã€�路径规划算法开å�‘基础认知C å¼€å�‘适é…�版

1 æ ¸å¿ƒå®šä¹‰ä¸�å¼€å�‘å‰�æ��路径规划算法开å�‘的本质是将 “ç�°å®�ç�¯å¢ƒâ€� 抽象为 “å�¯è®¡ç®—模å�‹â€�通过算法策略æ�œç´¢æœ€ä¼˜è·¯å¾„å†�æ˜ å°„å›�ç�°å®�场景执行。C å¼€å�‘需é¢�外关注内存å� 用ã€�è¿�行效ç�‡å¼€å�‘å‰�需æ˜�ç¡®ä¸‰ä¸ªæ ¸å¿ƒå‰�æ�� 一个 C 适é…�è¦�点ç�¯å¢ƒå»ºæ¨¡å°†ç�°å®�空间抽象为计算机å�¯å¤„ç�†çš„模å�‹å¸¸ç”¨ä¸¤ç§�æ–¹å¼� —— æ …æ ¼åœ°å›¾é€‚å�ˆäºŒç»´ç®€å�•åœºæ™¯å¦‚室内 AGVã€�点云 / è¿�续空间适å�ˆä¸‰ç»´å¤�æ�‚åœºæ™¯å¦‚æ— äººæœºã€�自动驾驶。C å¼€å�‘建议用 vector 容器存储地图数æ�®é�¿å…�动æ€�数组内存泄æ¼�。约æ�Ÿæ�¡ä»¶æ˜�确路径需满足的é™�制分为硬约æ�Ÿæ— 碰æ’�ã€�è¿�动学约æ�Ÿå¦‚机器人转弯å�Šå¾„和软约æ�Ÿæœ€ä¼˜æ€§ã€�å¹³æ»‘æ€§å¦‚è·¯å¾„æ— å‰§çƒˆè½¬æŠ˜ã€‚C å¼€å�‘å�¯é€šè¿‡ç»“æ�„体å°�装约æ�Ÿå�‚æ•°æ��å�‡ä»£ç �å¤�ç”¨æ€§ã€‚æ€§èƒ½éœ€æ±‚æ ¹æ�®åœºæ™¯ç¡®å®šå®�时性如自动驾驶需毫秒级å“�应ã€�最优性优先级如物æµ� AGV 优先最短è·�离急救机器人优先最短时间。C å¼€å�‘å�¯é€šè¿‡æŒ‡é’ˆã€�引用å‡�å°‘æ•°æ�®æ‹·è´�æ��å�‡è¿�行效ç�‡ã€‚C 适é…�è¦�点优先使用 STL 容器vectorã€�priority_queue 等简化开å�‘é�¿å…�使用全局å�˜é‡�通过类å°�装算法逻辑便äº�å��续工程集æˆ�如对æ�¥ ROS 节点ã€�嵌入å¼�底盘æ�§åˆ¶ã€‚

2 算法分类C å¼€å�‘é‡�点区分路径规划算法ç§�ç±»ç¹�多按 “规划范围â€� å’Œ â€œæ ¸å¿ƒæ€�想â€� å�¯åˆ†ä¸ºå››å¤§ç±»C å¼€å�‘è€…éœ€æ ¹æ�®åœºæ™¯é€‰å�‹é‡�点关注算法的时间å¤�æ�‚度ä¸�空间å¤�æ�‚度é�¿å…�盲目开å�‘ç®—æ³•ç±»åˆ«æ ¸å¿ƒæ€�想代表算法适用场景C å¼€å�‘éš¾åº¦ä¼ ç»Ÿå›¾æ�œç´¢ç®—法将ç�¯å¢ƒç¦»æ•£ä¸ºå›¾èŠ‚点é��å�†èŠ‚点寻找路径Dijkstraã€�A*ã€�JPS二维é�™æ€�场景室内 AGVã€�游æˆ�寻路ä½�优先æ�Œæ�¡ A*基äº�é‡‡æ ·çš„ç®—æ³•éš�æœºé‡‡æ ·æ�„建路径é�¿å…�显å¼�建模障ç¢�RRTã€�RRT*ã€�PRM高维 / å¤�æ�‚åœºæ™¯æ— äººæœºã€�机械臂中é‡�点æ�Œæ�¡ RRT*智能优化算法模拟自然ç�°è±¡æ±‚解多约æ�Ÿæœ€ä¼˜è§£é�—ä¼ ç®—æ³•ã€�èš�ç¾¤ç®—æ³•å¤šç›®æ ‡ä¼˜åŒ–èƒ½è€— è·�离约æ�Ÿä¸­éœ€æ�Œæ�¡å¤šçº¿ç¨‹ä¼˜åŒ–è��å�ˆå�‹ç®—法结å�ˆä¼ 统算法ä¸�æœºå™¨å­¦ä¹ é€‚é…�动æ€�ç�¯å¢ƒA*DWAã€�RRT*RL动æ€� / 未知场景自动驾驶ã€�户外机器人高需工程化集æˆ�能力二ã€�æ ¸å¿ƒè·¯å¾„è§„åˆ’ç®—æ³• C å®�战å�¯ç›´æ�¥ç¼–译è¿�行本节è�šç„¦ C å¼€å�‘中最常用的 3 类算法A*ã€�RRT*ã€�DWAä»�å�Ÿç�†ç®€åŒ–ã€�æ ¸å¿ƒæ­¥éª¤ã€�完整代ç �å®�ç�°ä¸‰ä¸ªç»´åº¦å±•å¼€ä»£ç �å�‡åŒ…å�«æ³¨é‡Šã€�测试用例适é…� VS/Qt/Clion 等编译器新手å�¯ç›´æ�¥å¤�用修改适é…�场景。å‰�置准备C11 å�Šä»¥ä¸Šæ ‡å‡†æ”¯æŒ� STL 容器ã€�lambda 表达å¼�æ— éœ€é¢�外ä¾�赖库å�¯è§†åŒ–å�¯å�¯é€‰æ·»åŠ OpenCV代ç �中给出å�¯é€‰æ–¹æ¡ˆã€‚

1 A * 算法最常用��场景首选

2.

1 æ ¸å¿ƒå�Ÿç�†C å¼€å�‘æ��简ç�†è§£A * 算法是 Dijkstra ç®—æ³•çš„æ”¹è¿›ç‰ˆæ ¸å¿ƒä¼˜åŠ¿æ˜¯å¼•å…¥å�¯å�‘å¼�函数 h (n)平衡 “å®�际代价â€� ä¸� “预估代价â€�大幅æ��å�‡æ�œç´¢æ•ˆç�‡ä¿�è¯�全局最优是 C 路径规划开å�‘的入门首选时间å¤�æ�‚度ä½�ã€�å®�ç�°ç®€å�•ã€‚其评价函数为f (n) g (n) h (n)g (n)ä»�起点到当å‰�节点 n çš„å®�é™…ç§»åŠ¨ä»£ä»·å¦‚æ …æ ¼åœ°å›¾ä¸­ä¸Šä¸‹å·¦å�³ç§»åŠ¨ä»£ä»·ä¸º 1对角线为√2h (n)ä»�当å‰�节点 n åˆ°ç›®æ ‡èŠ‚ç‚¹çš„é¢„ä¼°ä»£ä»·å¸¸ç”¨æ›¼å“ˆé¡¿è·�离ã€�æ¬§å‡ é‡Œå¾—è·�ç¦»æ …æ ¼åœºæ™¯ä¼˜å…ˆé€‰æ›¼å“ˆé¡¿è·�离计算更快f (n)节点 n 的总代价C 中用优先队列priority_queue自动æ�’åº�优先扩展 f (n) 最å°�的节点。

2.

2 C 完整å®�ç�°æ …æ ¼åœ°å›¾å�¯ç›´æ�¥è¿�è¡Œiostreamvectorqueuecmathalgorithmusing namespace std;//

定义节点结æ�„体å°�装节点信æ�¯C å°�装性优势struct Node {int x, y; // 节点å��æ ‡æ …æ ¼åœ°å›¾ç´¢å¼•int g; // 起点到当å‰�节点的å®�际代价int h; // 当å‰�èŠ‚ç‚¹åˆ°ç›®æ ‡èŠ‚ç‚¹çš„é¢„ä¼°ä»£ä»·int f; // 总代价 f g hNode* parent; // 父节点指针用äº�路径å›�溯// æ�„é€ å‡½æ•°Node (int x_, int y_, int g_, int h_, Node* parent_): x (x_), y (y_), g (g_), h (h_), f (g_h_), parent (parent_) {}};//

优先队列æ�’åº�规则å°�é¡¶å †æŒ‰ f 值ä»�å°�到大æ�’åº�struct CompareNode {bool operator ()(Node* a, Node* b) {return a-f b-f; // ä¼˜å…ˆé˜Ÿåˆ—é»˜è®¤å¤§é¡¶å †æ­¤å¤„å��转å®�ç�°å°�é¡¶å †}};//

å�¯å�‘函数曼哈顿è·�ç¦»æ …æ ¼å››å�‘移动首选计算高效int heuristic (int x1, int y1, int x2, int y

{return abs (x1 - x

abs (y1 - y

;}//

碰�检测判断节点是�在地图内�是�为障�物bool isCollision (int x, int y, const vectorint grid) {// 地图边界检测if (0 || x grid [0].size 0 || y grid.size ()) {return true;}// 障�物检测1 表示障�0 表示�通行return grid [y][x] 1;}//

Aç®—æ³•æ ¸å¿ƒå®�ç�°æ …æ ¼åœ°å›¾ç‰ˆ int, int AStar (constintint, int start,int, int goal) {// åˆ�始化优先队列开放集ã€�已访问节点集å�ˆå…³é—­é›†Node,Node*, CompareNode openSet;bool closedSet(grid.size(), vectorbool(grid[0].size(), false));// 起点节点åˆ�始化Node* startNode new Node (start.first, start.second, 0,heuristic (start.first, start.second, goal.first, goal.second),nullptr);openSet.push (startNode);// 节点相邻方å�‘å››å�‘移动上下左å�³å�¯æ‰©å±•ä¸ºå…«å�‘int dirs [4][2] ;while (!openSet.empty ()) {// å�–出 f 值最å°�的节点当å‰�最优节点Node* currentNode openSet.top ();openSet.pop ();// åˆ°è¾¾ç›®æ ‡èŠ‚ç‚¹å›�溯路径if (currentNode-x goal.first currentNode-y goal.second) {int, int path;Node* temp currentNode;while (temp ! nullptr) {path.emplace_back (temp-x, temp-y);temp temp-parent;}// å��转路径ä»�èµ·ç‚¹åˆ°ç›®æ ‡reverse (path.begin (), path.end ());// 释放内存C 关键é�¿å…�内存泄æ¼�while (!openSet.empty ()) {delete openSet.top ();openSet.pop ();}delete currentNode;return path;}// æ ‡è®°å½“å‰�èŠ‚ç‚¹ä¸ºå·²è®¿é—®åŠ å…¥å…³é—­é›†if (closedSet [currentNode-y][currentNode-x]) {delete currentNode;continue;}closedSet [currentNode-y][currentNode-x] true;// 扩展相邻节点for (auto dir : dirs) {int newX currentNode-x dir [0];int newY currentNode-y dir [1];// 碰æ’�检测跳过障ç¢�物ã€�边界外节点ã€�已访问节点if (isCollision (newX, newY, grid) || closedSet [newY][newX]) {continue;}// 计算新节点的 gã€�hã€�f 值四å�‘移动g 值 1int newG currentNode-g 1;int newH heuristic (newX, newY, goal.first, goal.second);Node* newNode new Node (newX, newY, newG, newH, currentNode);// å°†æ–°èŠ‚ç‚¹åŠ å…¥å¼€æ”¾é›†openSet.push (newNode);}}// 未找到å�¯è¡Œè·¯å¾„释放内存while (!openSet.empty ()) {delete openSet.top ();openSet.pop ();}delete startNode;return {}; // è¿”å›�空路径}//

测试代ç �主函数å�¯ç›´æ�¥ç¼–译è¿�è¡Œint main () {// æ�„å»ºæ …æ ¼åœ°å›¾10x101 为障ç¢�0 为å�¯é€šè¡Œint grid \n 路径长度 path.size () endl;} else { 未找到 endl;}return 0;}// 编译命令gg AStar.cpp -o AStar -stdc11// è¿�è¡Œ./AStarLinux/Mac 或 AStar.exeWindowsplaintext####

2.

3 C开�优化技巧工业级适�

内存优化é�¿å…�频ç¹�new/delete节点å�¯ä½¿ç”¨å¯¹è±¡æ± 模å¼�预先分é…�节点内存å‡�少内存ç¢�片适å�ˆåµŒå…¥å¼�场景

效ç�‡ä¼˜åŒ–å�¯å�‘函数å�¯æ ¹æ�®åœºæ™¯è°ƒæ•´å…«å�‘移动用切比雪夫è·�离优先队列å�¯ç»“å�ˆunordered_mapå®�ç�°èŠ‚点å�»é‡�å‡�少冗余计算

工程适�将A*算法�装为类对外�供��如设置地图�起点终点���函数��便��续对�ROS节点�底盘�制逻辑

å�¯è§†åŒ–å�¯é€‰è‹¥éœ€å�¯è§†åŒ–路径å�¯æ·»åŠ OpenCVä¾�赖用lineå‡½æ•°ç»˜åˆ¶æ …æ ¼åœ°å›¾ä¸�路径代ç �中å�¯æ·»åŠ 注释部分的OpenCV代ç �。 ###

2 RRT*算法高维/��场景首选 ####

2.

1 æ ¸å¿ƒå�Ÿç�†Cå¼€å�‘æ��简ç�†è§£ RRT快速éš�æœºæ ‘ç®—æ³•é€šè¿‡éš�æœºé‡‡æ ·ç”Ÿæˆ�æ ‘çŠ¶ç»“æ�„快速æ�¢ç´¢æœªçŸ¥ç�¯å¢ƒä½†è·¯å¾„é��最优RRT*是其改进版å¢�åŠ **父节点é‡�选**å’Œ**路径é‡�è¿�**两个步骤å®�ç�°è·¯å¾„æ¸�进最优。 Cå¼€å�‘中RRT*适å�ˆä¸‰ç»´åœºæ™¯å¦‚æ— äººæœºå¯¼èˆªã€�高维场景如机械臂关节空间规划é‡�点关注**éš�机数生æˆ�效ç�‡ã€�邻域æ�œç´¢ä¼˜åŒ–**é�¿å…�é‡‡æ ·å†—ä½™ã€‚ ####

2.

2 C完整��二维�续空间简化版��行 cpp #include vectorrandomcmathalgorithm using namespace std; //

定义节点结æ�„体è¿�续空间å��æ ‡ struct Node { double x, y; // è¿�续空间å��æ ‡ double cost; // ä»�起点到当å‰�节点的代价 Node* parent; // 父节点指针用äº�路径å›�溯 // æ�„é€ å‡½æ•° Node(double x_, double y_) : x(x_), y(y_), cost(

0.

, parent(nullptr) {} }; //

è®¡ç®—ä¸¤ä¸ªèŠ‚ç‚¹ä¹‹é—´çš„æ¬§å‡ é‡Œå¾—è·�离 double distance(Node* a, Node* b) { return sqrt(pow(a-x - b-x,

pow(a-y - b-y,

); } //

碰æ’�检测简化障ç¢�物为圆形å�¯æ ¹æ�®å®�际场景修改为矩形/多边形 // obstacleséšœç¢�物列表æ¯�ä¸ªå…ƒç´ ä¸ºx,y,å�Šå¾„ bool isCollision(Node*tupledouble, double, double obstacles) { for (auto obs : obstacles) { double0(obs); double oy1(obs); double orad2(obs); if (distance(node, new Node(ox orad) { return true; // 碰æ’� } } return false; // æ— ç¢°æ’� } //

éš�æœºé‡‡æ ·èŠ‚ç‚¹C11éš�机数生æˆ�比rand()更高效 Node* randomSample(double minX, double maxX, double minY, double maxY) { static random_device rd; static mt19937 gen(rd()); uniform distX(minX, maxX); uniform_real_distribution distY(minY, maxY); return new Node(distX(gen), distY(gen)); } //

æ‰¾åˆ°æ ‘ä¸­è·�ç¦»é‡‡æ ·ç‚¹æœ€è¿‘çš„èŠ‚ç‚¹ Node* findNearestNode(constNode* tree, Node* sample) { Node* nearest tree[0]; double minDist distance(nearest, sample); for (Node* node : tree) { double dist distance(node, sample); if ( minDist) { minDist dist; nearest node; } } return nearest; } //

ä»�最近节点å�‘é‡‡æ ·ç‚¹æ‰©å±•æ–°èŠ‚ç‚¹æ�§åˆ¶æ­¥é•¿é�¿å…�扩展过快 Node* extendNode(Node* nearest, Node* sample, double stepSize) { double dist distance(nearest, sample); if ( stepSize) { return sample; // é‡‡æ ·ç‚¹è·�离过近直æ�¥è¿”å›�é‡‡æ ·ç‚¹ } // 计算扩展方å�‘按步长生æˆ�新节点 double dx (sample-x - nearest-x) / dist * stepSize; double dy (sample-y - nearest-y) / dist * stepSize; Node* newNode new Node(nearest-x dx, nearest-y dy); newNode-parent nearest; newNode-cost nearest-cost stepSize; delete sample; // é‡Šæ”¾é‡‡æ ·ç‚¹å†…å­˜ return newNode; } //

RRT*ç®—æ³•æ ¸å¿ƒå®�ç�°äºŒç»´è¿�续空间 double, double RRTStar(Node* start, Node* goal,tupledouble, double, double obstacles, double minX, double maxX, double minY, double maxY, int maxIter 1500, double stepSize

5, double goalRadius

1.

{ Node* tree; // éš�æœºæ ‘å­˜å‚¨æ‰€æœ‰èŠ‚ç‚¹ tree.push_back(start); // èµ·ç‚¹ä½œä¸ºæ ‘æ ¹åŠ å…¥æ ‘ä¸­ bool goalReached false; for (int i 0; i maxIter; i) { //

éš�æœºé‡‡æ ·10%概ç�‡ç›´æ�¥é‡‡æ ·ç›®æ ‡ç‚¹åŠ 速收敛 Node* sample; if (rand() %

{ sample new Node(goal-x, goal-y); } else { sample randomSample(minX, maxX, minY, maxY); } //

找到最近节点 Node* nearest findNearestNode(tree, sample); //

扩展新节点 Node* newNode extendNode(nearest, sample, stepSize); //

碰�检测新节点��行则跳过 if (isCollision(newNode, obstacles)) { delete newNode; continue; } //

父节点�选在新节点邻域内寻找代价更�的父节点 double neighborRadius

0; // 邻域�径 Node* neighbors; for (Node* node : tree) { if (distance(node, neighborRadius) { neighbors.push_back(node); } } if (!neighbors.empty()) { Node* bestParent nearest; double minCost newNode-cost; for (Node* neighbor : neighbors) { // 计算通过邻居节点到新节点的代价 double tentativeCost neighbor-cost distance(neighbor, newNode); if ( minCost !isCollision(newNode, obstacles)) { minCost tentativeCost; bestParent neighbor; } } // 更新新节点的父节点和代价 newNode-parent bestParent; newNode-cost minCost; } //

路径��更新邻域内节点的父节点若通过新节点代价更� for (Node* neighbor : neighbors) { double tentativeCost newNode-cost distance(newNode, neighbor); if (tentative neighbor-cost !isCollision(neighbor, obstacles)) { neighbor-parent newNode; neighbor-cost tentativeCost; } } //

å°†æ–°èŠ‚ç‚¹åŠ å…¥æ ‘ä¸­ tree.push_back(newNode); //

检查是å�¦åˆ°è¾¾ç›®æ ‡åŒºåŸŸ if (distance(new goalRadius) { goal-parent newNode; tree.push_back(goal); goalReached true; break; } } // 路径å›�溯 vectordouble, double path; if (goalReached) { Node* temp goal; while (temp ! nullptr) { path.emplace_back(temp-x, temp-y); temp temp-parent; } reverse(path.begin(), path.end()); // å��è½¬è·¯å¾„èµ·ç‚¹åˆ°ç›®æ ‡ } // 释放内存C关键é�¿å…�内存泄æ¼� for (Node* node : tree) { if (node ! start node ! goal) { delete node; } } return path; } //

测试代ç �主函数å�¯ç›´æ�¥ç¼–译è¿�è¡Œ int main() { // åˆ�始化起点ã€�ç›®æ ‡ç‚¹è¿�续空间范围0~20 Node* start new Node(

0,

1.

; Node* goal new Node(

1

0,

18.

; // 障�物列表[(x, y, �径)]圆形障�物 double, double, double obstacles { {5, 5, 1}, {10, 10,

5}, {15, 5, 1}, {5, 15, 1}, {15, 15, 1}, {10, 5, 1} }; // �行RRT*算法 vectordouble, double path RRTStar( start, goal, obstacles,

0,

2

0,

0,

2

0, // �境范围minX, maxX, minY, maxY 1500,

5,

0 // 最大迭代次数ã€�步长ã€�ç›®æ ‡å�Šå¾„ ); // 打å�°ç»“æ�œ if (!path.empty()) { 找到å�¯è¡Œè·¯å¾„å��æ ‡åº�列x,y endl; for (size_t i 0; i path.size(); i) { cout path[i]. path[i ); if (i ! path.size() -

→ ; } } cout \n路径长度 path.size() - endl; } else { cout 未找到å�¯è¡Œè·¯å¾„ endl; } // é‡Šæ”¾èµ·ç‚¹å’Œç›®æ ‡èŠ‚ç‚¹å†…å­˜ delete start; delete goal; return 0; } // 编译命令gg RRTStar.cpp -o RRTStar -stdc11 // è¿�è¡Œ./RRTStarLinux/Mac 或 RRTStar.exeWindows

3 DWA 算法局部动��障首选

2.

1 æ ¸å¿ƒå�Ÿç�†C å¼€å�‘æ��简ç�†è§£DWA动æ€�窗å�£æ³•æ˜¯å±€éƒ¨è·¯å¾„è§„åˆ’ç®—æ³•æ ¸å¿ƒæ€�想是 “在速度空间线速度ã€�è§’é€Ÿåº¦ä¸­é‡‡æ ·å¤šä¸ªå€™é€‰é€Ÿåº¦é¢„æµ‹æ¯�个速度对应的轨迹通过评价函数筛选最优轨迹â€�适å�ˆåŠ¨æ€�ç�¯å¢ƒå¦‚自动驾驶é�¿éšœã€�移动机器人å®�时调整路径。C å¼€å�‘中DWA é‡�点关注轨迹预测效ç�‡ã€�评价函数æ�ƒé‡�调整å�¯ç»“å�ˆ A * 全局规划使用全局定方å�‘局部é�¿éšœç¢�å®�时性优äº�é‡‡æ ·ç±»ç®—æ³•ã€‚

2.

2 C 完整��简化版适�移动机器人iostreamvectorcmath algorithm using namespace std; //

定义机器人状æ€�结æ�„体 struct RobotState { double x; // xå��æ ‡ double y; // yå��æ ‡ double theta; // 航å�‘角弧度 double v; // 线速度 double w; // 角速度 // æ�„é€ å‡½æ•° RobotState(double x_, double y_, double theta_, double v_, double w_) : x(x_), y(y_), theta(theta_), v(v_), w(w_) {} }; //

定义轨迹结æ�„体存储预测轨迹的状æ€�和评价分数 struct Trajectory { RobotState states; // 轨迹上的所有状æ€� double score; // 评价分数分数越高轨迹越优 // æ�„é€ å‡½æ•° TraRobotState states_, double score_) : states(states_), score(score_) {} }; //

è½¨è¿¹é¢„æµ‹æ ¹æ�®å½“å‰�é€Ÿåº¦å’Œé‡‡æ ·é€Ÿåº¦é¢„æµ‹æœªæ�¥ä¸€æ®µæ—¶é—´çš„轨迹RobotState predictTrajectory(RobotState current, double v, double w, double dt

1, int predictTime

{ vectorRobotState trajectory; trajectory.push_back(current); for (int i 0; i predictTime; i) { // �动学模�差分驱动机器人 double x current.x v * cos(current.theta) * dt; double y current.y v * sin(current.theta) * dt; double theta current.theta w * dt; // 更新当�状� current RobotState(x, y, theta, v, w); trajectory.push_back(current); } return trajectory; } //

碰æ’�检测简化障ç¢�物为矩形å�¯æ ¹æ�®å®�际场景修改 bool isTraRobotState trajectory, double, double, double, double obstacles) { for (auto state : trajectory) { for (auto obs : obstacles) { double ox0(obs); // éšœç¢�物左上角x double oy11(obs); // éšœç¢�物左上角y double ox2 2(obs); // éšœç¢�物å�³ä¸‹è§’x double oy2 get3(obs); // éšœç¢�物å�³ä¸‹è§’y // 判断机器人中心是å�¦åœ¨éšœç¢�物内 if (state.x ox1 ox2 state.y oy1 oy

{ return true; // 碰æ’� } } } return false; // æ— ç¢°æ’� } //

评价函数筛选最优轨迹综å�ˆä¸‰ä¸ªæŒ‡æ ‡ç›®æ ‡æ–¹å�‘ã€�速度ã€�æ— ç¢°æ’� double evaluateTrajectory(constRobotState trajectory, RobotState goal, double vMax

0, double wMax

0.

{ //

ç›®æ ‡æ–¹å�‘ä»£ä»·è½¨è¿¹ç»ˆç‚¹åˆ°ç›®æ ‡ç‚¹çš„è·�离越å°�越好 RobotState end trajectory.back(); double goalDist sqrt(pow(end.x - goal.x,

pow(end.y - goal.y,

); double goalScore

0 / (

0 goalDist); // 归一化范围[0,1] //

速度代价线速度越�近最大速度分数越高 double vScore end.v / vMax; //

平滑代价角速度越�轨迹越平滑分数越高 double wScore

0 / (

0 fabs(end.w)); // æ�ƒé‡�调整å�¯æ ¹æ�®åœºæ™¯ä¿®æ”¹æ€»å’Œä¸º1 double weightGoal

5; double weightV

3; double weightW

2; return weightGoal * goalScore weightV * vScore weightW * wScore; } //

DWAç®—æ³•æ ¸å¿ƒå®�ç�° Trajectory DWA(RobotState current, RobotState goal, constdouble, double, double, double obstacles, double vMin

0, double vMax

0, double wMin -

5, double wMax

5, double dv

1, double dw

0.

{ Trajectory validTrajectories; // å­˜å‚¨æ‰€æœ‰æœ‰æ•ˆè½¨è¿¹æ— ç¢°æ’� //

é€Ÿåº¦ç©ºé—´é‡‡æ ·çº¿é€Ÿåº¦vã€�角速度w for (double v vMin vMax; v dv) { for (double w wMax; w dw) { //

预测轨迹 RobotState trajectory predictTrajectory(current, v, w); //

碰�检测跳过碰�轨迹 if (isTrajectoryCollision(trajectory, obstacles)) { continue; } //

è¯„ä»·è½¨è¿¹åŠ å…¥æœ‰æ•ˆè½¨è¿¹åˆ—è¡¨ double score evaluateTrajectory(trajectory, goal, vMax, wMax); validTrajectories.emplace_back(trajectory, score); } } //

筛选最优轨迹分数最高 if (validTrajectories.empty()) { // æ— æœ‰æ•ˆè½¨è¿¹è¿”å›�当å‰�状æ€�å�œæ­¢ä¸�动 vectorRobotState emptyTraj; emptyTraj.push_back(current); return Trajectory(emptyTraj,

0.

; } // 按分数�����第一个最优 sort(validTrajectories.begin(), validTrajectories.end(), [](Trajectory a, Trajectory b) { return a.score b.score; }); return validTrajectories[0]; } //

测试代ç �主函数å�¯ç›´æ�¥ç¼–译è¿�è¡Œ int main() { // åˆ�始化机器人当å‰�状æ€�x,y,theta,v,w RobotState current(

0,

0,

0,

0,

0.

; // åˆ�å§‹åŒ–ç›®æ ‡çŠ¶æ€�x,y,thetaå�¯ä»»æ„�v,wæ— æ„�义 RobotState goal(

0,

0,

0,

0,

0.

; // 障�物列表[(左上角x, 左上角y, �下角x, �下角y)]矩形障�物 double, double, double, double obstacles { {

0,

0,

0,

0}, {

0,

0,

0,

0} }; // �行DWA算法��最优轨迹 Trajectory bestTraj DWA(current, goal, obstacles); // 打�最优轨迹结� DWA算法最优轨迹x,y,theta,v,w endl; for (size_t i bestTraj.states.size(); i) { RobotState state bestTraj.states[i]; 第 步 , , state.theta , ) endl; } 最优轨迹评价分数 endl; return 0; } // 编译命令gg DWA.cpp -o DWA -stdc11 // �行./DWALinux/Mac 或 DWA.exeWindows三�C 路径规划算法工程化优化工业级适�

1 效ç�‡ä¼˜åŒ–æ ¸å¿ƒé‡�点内存优化é�¿å…�频ç¹� new/deleteä½¿ç”¨å¯¹è±¡æ± ã€�智能指针shared_ptr/unique_ptr管ç�†èŠ‚点内存å‡�少内存ç¢�ç‰‡æ …æ ¼åœ°å›¾ç”¨vectorvectorint存储é�¿å…�动æ€�数组计算优化å�¯å�‘函数ã€�è·�离计算等热点代ç �用 inline 关键字å‡�少函数调用开销优先使用基本数æ�®ç±»å�‹int/doubleé�¿å…�å¤�æ�‚结æ�„体拷è´�æ�œç´¢ä¼˜åŒ–A*算法用 priority_queueunordered_map å�Œé‡�结æ�„å®�ç�°èŠ‚点快速å�»é‡�RRT*算法的邻域æ�œç´¢ç”¨ KD æ ‘æ›¿ä»£çº¿æ€§æ�œç´¢é™�ä½�时间å¤�æ�‚度。

2 工程适é…�优化æ�¥å�£å°�装将æ¯�ç§�算法å°�装为独立的类对外æ��供统一æ�¥å�£å¦‚ setMapã€�setStartGoalã€�planPath便äº�å��续替æ�¢ç®—法ã€�集æˆ�到工程中多线程优化将路径规划ä¸�地图更新ã€�æ�§åˆ¶æŒ‡ä»¤ä¸‹å�‘分离用多线程并行执行C11 threadæ��å�‡å®�时性注æ„�线程安全用 mutex é”�ä¿�护共享数æ�®å¼‚常处ç�†æ·»åŠ 边界检测ã€�å�‚æ•°å�ˆæ³•æ€§æ ¡éªŒå¦‚速度ä¸�超过最大值ã€�å��æ ‡åœ¨åœ°å›¾å†…é�¿å…�程åº�崩溃用 try-catch æ�•è�·å¼‚常æ��å�‡ç¨‹åº�å�¥å£®æ€§åµŒå…¥å¼�适é…�精简代ç �移除冗余计算使用é�™æ€�编译å‡�å°‘ä¾�赖库优化内存å� 用é�¿å…�大容器一次性分é…�过多内存。

3 调试ä¸�å�¯è§†åŒ–æŠ€å·§è°ƒè¯•æŠ€å·§åœ¨å…³é”®æ­¥éª¤æ·»åŠ æ—¥å¿—è¾“å‡ºcout 或 printf打å�°èŠ‚点å��æ ‡ã€�代价ã€�轨迹分数等信æ�¯ä½¿ç”¨ GDB 调试Linux或 VS 调试器Windowsæ�’查内存泄æ¼�ã€�逻辑错误å�¯è§†åŒ–技巧结å�ˆ OpenCV 绘制地图ã€�起点ã€�终点ã€�éšœç¢�物和路径直观观察算法è¿�行结æ�œå¯¹æ�¥ ROS çš„ rviz å�¯è§†åŒ–工具适å�ˆæœºå™¨äººå¯¼èˆªåœºæ™¯ã€‚å››ã€�常è§�问题ä¸� C æ�’å�‘指å�—内存泄æ¼�这是 C å¼€å�‘最常è§�的问题节点ã€�轨迹等动æ€�分é…�的内存必须在使用å��释放如 A * 算法中开放集ã€�æ ‘èŠ‚ç‚¹çš„é‡Šæ”¾å»ºè®®ä½¿ç”¨æ™ºèƒ½æŒ‡é’ˆæ›¿ä»£è£¸æŒ‡é’ˆè¿�行效ç�‡ä½�检查是å�¦æœ‰é¢‘ç¹�的结æ�„体拷è´�用引用 / æŒ‡é’ˆä¼ é€’ã€�线性æ�œç´¢æ›¿æ�¢ä¸º KD æ ‘ / 哈希表ã€�冗余计算如é‡�å¤�计算è·�离路径ä¸�平滑A算法å�¯æ·»åŠ è´�å¡�尔曲线平滑代ç �DWA 算法调整评价函数中角速度的æ�ƒé‡�RRT算法å¢�åŠ è¿­ä»£æ¬¡æ•°ç¼–è¯‘æŠ¥é”™ç¡®ä¿�编译器支æŒ� C11 å�Šä»¥ä¸Šæ ‡å‡†æ·»åŠ - stdc11 编译选项检查 STL 容器的使用是å�¦æ­£ç¡®å¦‚ vector 的索引范围动æ€�ç�¯å¢ƒé€‚é…�差结å�ˆ A全局规划 DWA 局部规划A负责全局路径引导DWA è´Ÿè´£å®�æ—¶é�¿éšœå®šæœŸæ›´æ–°åœ°å›¾ä¿¡æ�¯å®�ç�°å¢�é‡�é‡�规划。五ã€�总结对äº� C å¼€å�‘者而言路径规划算法开å�‘çš„æ ¸å¿ƒæ˜¯ “选å�‹å�ˆç�†ã€�效ç�‡ä¼˜å…ˆã€�内存安全â€�é�™æ€�场景首选 A算法简å�•é«˜æ•ˆé«˜ç»´ / å¤�æ�‚场景首选 RRT算法扩展性强动æ€�é�¿éšœé¦–选 DWA 算法å®�时性好。

探索仙作直播的奇妙世界-探索仙作直播的奇妙世界应用