基于搜索的路径规划

Aglargil · Oct 11, 2024 · 12092ef · 12092ef
1 parent e3a4069
commit 12092ef
Show file tree

Hide file tree

Showing 6 changed files with 168 additions and 0 deletions.
diff --git a/docs/blog/posts/基于搜索的路径规划.md b/docs/blog/posts/基于搜索的路径规划.md
@@ -0,0 +1,75 @@
+---
+title: 基于搜索的路径规划
+date:
+  created: 2024-10-10
+  updated: 2024-10-10
+categories:
+  - 机器人
+---
+
+## 1. 图搜索基础
+
+### 1.1 配置空间
+
+一个n维空间，其中每一个点代表一个机器人的位姿
+
+<!-- more -->
+
+>在配置空间中规划
+![在配置空间中规划](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的代价
+
+   - 曼哈顿距离 L1 norm
+   - 欧几里得距离 L2 norm
+   - 切比雪夫距离 L∞ norm
+
+
+## 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)$"
+
+
+    ![](https://picgo-1257309505.cos.ap-guangzhou.myqcloud.com/20241010115350.png)
+
+### 2.3 Weighted A*
+
+$f(n) = g(n) + \epsilon \cdot h(n), \epsilon > 1$
+
+- $\epsilon = 1$ 时，等价于A*
+- $\epsilon > 1$ 时，更倾向于贪心搜索
diff --git a/docs/blog/tmp/沿路激进策略.md b/docs/blog/tmp/沿路激进策略.md
@@ -0,0 +1,13 @@
+沿路时需要贴近+减少腾挪
+
+## 条件：
+- 原始参考线足够直: simplify后点数为2
+- 机器人右侧是路，且路的边缘足够直： PCA拟合直线
+- 原始参考线和路的边缘几乎平行
+- 机器人与路的边缘几乎平行
+
+## 规划
+- 将路的边缘进行平移，以此为参考线进行纯轨迹跟踪
+
+
+
diff --git a/docs/blog/tmp/沿边在允许过窄道的情况下保持和障碍物一定的距离.md b/docs/blog/tmp/沿边在允许过窄道的情况下保持和障碍物一定的距离.md
@@ -0,0 +1,13 @@
+# 沿边过窄道
+
+## 机器人基本参数
+机器人现状近似长方形，长0.6m，宽0.4m
+
+## 环境表示
+障碍物由多边形表示。
+
+## 目前方案
+
+对障碍物进行膨胀半车宽(0.2m)+额定沿边距离(0.08m)，得到参考线，机器人沿着参考线进行行驶，就可以实现沿边的过程同时基本保持和障碍物距离0.08m
+
+缺点: 由于是对所有障碍物膨胀了0.28m，因此假设两个障碍物直接的最小距离为0.45m，即小于半车宽，理论上机器人应该从障碍物中间穿过，但是对两个障碍物分别膨胀0.28m后，膨胀后的参考线是绕过这两个障碍物
diff --git a/docs/blog/tmp/行为树处理.md b/docs/blog/tmp/行为树处理.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;
+}
+```
diff --git a/docs/javascripts/katex.js b/docs/javascripts/katex.js
@@ -0,0 +1,10 @@
+document$.subscribe(({ body }) => { 
+    renderMathInElement(body, {
+      delimiters: [
+        { left: "$$",  right: "$$",  display: true },
+        { left: "$",   right: "$",   display: false },
+        { left: "\\(", right: "\\)", display: false },
+        { left: "\\[", right: "\\]", display: true }
+      ],
+    })
+  })
diff --git a/mkdocs.yml b/mkdocs.yml
@@ -70,5 +70,15 @@ markdown_extensions:
   - def_list
   - pymdownx.tasklist:
       custom_checkbox: true
+  - pymdownx.arithmatex:
+      generic: true
+
+extra_javascript:
+  - javascripts/katex.js
+  - https://unpkg.com/katex@0/dist/katex.min.js
+  - https://unpkg.com/katex@0/dist/contrib/auto-render.min.js
+
+extra_css:
+  - https://unpkg.com/katex@0/dist/katex.min.css
 
 copyright: 版权所有