From f6474761b244bc6bf05e5838217b578e7166230c Mon Sep 17 00:00:00 2001 From: wangzheng Date: Thu, 10 Oct 2024 11:37:53 +0800 Subject: [PATCH] =?UTF-8?q?=E5=9F=BA=E4=BA=8E=E6=90=9C=E7=B4=A2=E7=9A=84?= =?UTF-8?q?=E8=B7=AF=E5=BE=84=E8=A7=84=E5=88=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ...57\345\276\204\350\247\204\345\210\222.md" | 62 +++++++++++++++++++ ...00\350\277\233\347\255\226\347\225\245.md" | 13 ++++ ...32\347\232\204\350\267\235\347\246\273.md" | 13 ++++ ...72\346\240\221\345\244\204\347\220\206.md" | 47 ++++++++++++++ 4 files changed, 135 insertions(+) create mode 100644 "docs/blog/posts/\345\237\272\344\272\216\346\220\234\347\264\242\347\232\204\350\267\257\345\276\204\350\247\204\345\210\222.md" create mode 100644 "docs/blog/tmp/\346\262\277\350\267\257\346\277\200\350\277\233\347\255\226\347\225\245.md" create mode 100644 "docs/blog/tmp/\346\262\277\350\276\271\345\234\250\345\205\201\350\256\270\350\277\207\347\252\204\351\201\223\347\232\204\346\203\205\345\206\265\344\270\213\344\277\235\346\214\201\345\222\214\351\232\234\347\242\215\347\211\251\344\270\200\345\256\232\347\232\204\350\267\235\347\246\273.md" create mode 100644 "docs/blog/tmp/\350\241\214\344\270\272\346\240\221\345\244\204\347\220\206.md" diff --git "a/docs/blog/posts/\345\237\272\344\272\216\346\220\234\347\264\242\347\232\204\350\267\257\345\276\204\350\247\204\345\210\222.md" "b/docs/blog/posts/\345\237\272\344\272\216\346\220\234\347\264\242\347\232\204\350\267\257\345\276\204\350\247\204\345\210\222.md" new file mode 100644 index 0000000..3e8458f --- /dev/null +++ "b/docs/blog/posts/\345\237\272\344\272\216\346\220\234\347\264\242\347\232\204\350\267\257\345\276\204\350\247\204\345\210\222.md" @@ -0,0 +1,62 @@ +--- +title: 基于搜索的路径规划 +date: + created: 2024-10-10 + updated: 2024-10-10 +categories: + - 机器人 +--- + +## 1. 图搜索基础 + +### 1.1 配置空间 + +一个n维空间，其中每一个点代表一个机器人的位姿 + + + +>在配置空间中规划 +![在配置空间中规划](https://picgo-1257309505.cos.ap-guangzhou.myqcloud.com/20241010110244.png) + +### 1.2 图搜索 + +1. 构建graph + - 栅格地图： 天然就是一个graph + - PRM: 随机采样点，然后连接成图 + +2. 图搜索主循环 + - 维护一个容器存储所有待访问的节点 + - 容器初始节点为start + - 循环 + - pop + - expansion + - push + - 结束循环 + +### 1.3 启发式搜索 + +存储节点，根据启发式函数排序 + + - 贪心搜索： $f(n) = h(n)$ + - A*搜索： $f(n) = g(n) + h(n)$ + - Dijkstra： $f(n) = g(n)$ + +启发式函数：$h(n)$ 估计从n到goal的代价 + - 曼哈顿距离 + - 欧几里得距离 + - 切比雪夫距离 + + +## 2. Dijkstra和A*算法 + +### 2.1 Dijkstra算法 + +$g(n)$: 从start到n的最少代价 + +![](https://picgo-1257309505.cos.ap-guangzhou.myqcloud.com/20241010112711.png) + +### 2.2 A*算法 + +![](https://picgo-1257309505.cos.ap-guangzhou.myqcloud.com/20241010113458.png) + +!!! note Admissible启发式函数：$h(n) \leq h^*(n)$ \ No newline at end of file diff --git "a/docs/blog/tmp/\346\262\277\350\267\257\346\277\200\350\277\233\347\255\226\347\225\245.md" "b/docs/blog/tmp/\346\262\277\350\267\257\346\277\200\350\277\233\347\255\226\347\225\245.md" new file mode 100644 index 0000000..6315670 --- /dev/null +++ "b/docs/blog/tmp/\346\262\277\350\267\257\346\277\200\350\277\233\347\255\226\347\225\245.md" @@ -0,0 +1,13 @@ +沿路时需要贴近+减少腾挪 + +## 条件： +- 原始参考线足够直: simplify后点数为2 +- 机器人右侧是路，且路的边缘足够直： PCA拟合直线 +- 原始参考线和路的边缘几乎平行 +- 机器人与路的边缘几乎平行 + +## 规划 +- 将路的边缘进行平移，以此为参考线进行纯轨迹跟踪 + + + diff --git "a/docs/blog/tmp/\346\262\277\350\276\271\345\234\250\345\205\201\350\256\270\350\277\207\347\252\204\351\201\223\347\232\204\346\203\205\345\206\265\344\270\213\344\277\235\346\214\201\345\222\214\351\232\234\347\242\215\347\211\251\344\270\200\345\256\232\347\232\204\350\267\235\347\246\273.md" "b/docs/blog/tmp/\346\262\277\350\276\271\345\234\250\345\205\201\350\256\270\350\277\207\347\252\204\351\201\223\347\232\204\346\203\205\345\206\265\344\270\213\344\277\235\346\214\201\345\222\214\351\232\234\347\242\215\347\211\251\344\270\200\345\256\232\347\232\204\350\267\235\347\246\273.md" new file mode 100644 index 0000000..0b2f1b8 --- /dev/null +++ "b/docs/blog/tmp/\346\262\277\350\276\271\345\234\250\345\205\201\350\256\270\350\277\207\347\252\204\351\201\223\347\232\204\346\203\205\345\206\265\344\270\213\344\277\235\346\214\201\345\222\214\351\232\234\347\242\215\347\211\251\344\270\200\345\256\232\347\232\204\350\267\235\347\246\273.md" @@ -0,0 +1,13 @@ +# 沿边过窄道 + +## 机器人基本参数 +机器人现状近似长方形，长0.6m，宽0.4m + +## 环境表示 +障碍物由多边形表示。 + +## 目前方案 + +对障碍物进行膨胀半车宽(0.2m)+额定沿边距离(0.08m)，得到参考线，机器人沿着参考线进行行驶，就可以实现沿边的过程同时基本保持和障碍物距离0.08m + +缺点: 由于是对所有障碍物膨胀了0.28m，因此假设两个障碍物直接的最小距离为0.45m，即小于半车宽，理论上机器人应该从障碍物中间穿过，但是对两个障碍物分别膨胀0.28m后，膨胀后的参考线是绕过这两个障碍物 \ No newline at end of file diff --git "a/docs/blog/tmp/\350\241\214\344\270\272\346\240\221\345\244\204\347\220\206.md" "b/docs/blog/tmp/\350\241\214\344\270\272\346\240\221\345\244\204\347\220\206.md" new file mode 100644 index 0000000..ea0df7f --- /dev/null +++ "b/docs/blog/tmp/\350\241\214\344\270\272\346\240\221\345\244\204\347\220\206.md" @@ -0,0 +1,47 @@ +行为树有Sequence Fallback RetryUntilSuccessful ReactiveSequence等控制节点。 + +我还有如下动作节点： +BreakCheck +BreakHandle +StartCutter +Plan +Escape +ForceFinish +ClosureCheck +FinishCheck +CalcNavigationTarget +Navigation +ArriveTargetCheck + +我想实现的逻辑如下： +``` cpp +while (true) { + if (BreakCheck) { + BreakHandle; + StartCutter; + } + if (!Plan) { + if (Escape) { + StartCutter; + } else { + return SUCCESS; + } + } + if (ClosureCheck) { + if (FinishCheck) { + return SUCCESS; + } else { + for (int i = 0; i < 3; ++ i) { + CalcNavigationTarget; + if (Navigation) { + break; + } + } + if (!ArriveTargetCheck) { + return SUCCESS; + } + } + } + return RUNNING; +} +``` \ No newline at end of file