Hybrid Astar 算法剖析和实现

Posted 2022-05-23 穿越临界点

在学习资料满天飞的大环境下，知识变得非常零散，体系化的知识并不多，这就导致很多人每天都努力学习到感动自己，最终却收效甚微，甚至放弃学习。我的使命就是过滤掉大量的无效信息，将知识体系化，以短平快的方式直达问题本质，把大家从大海捞针的痛苦中解脱出来。

文章目录

• • 0 前言
  • 1 什么是节点拓展
  • 2 基于自行车模型拓展
  • • 2.1 连续拓展
    • 2.2 离散拓展
    • 2.3 中间节点
  • 3 总结

0 前言

上两篇对Hybrid Astar算法的主体搜索流程做了剖析和实现，本节详细介绍节点拓展。

1 什么是节点拓展

节点拓展就是基于当前节点状态空间向量推导下一个节点状态空间向量。

关于状态空间的介绍可以参看这篇文章：Hybrid Astar 算法剖析和实现（二）_穿越临界点的博客-CSDN博客

2 基于自行车模型拓展

2.1 连续拓展

设当前节点 $Nod{e}_{current}$ 中记录着车辆当前的位姿 $[x_0,y_0,{\theta }_0]$ （状态量）。求在控制量$[v,\varphi ]$ 的控制下，$t$时刻后车辆的位姿。

简易自行车模型如下式，其中 $L_w$ 代表wheel base，$\varphi$ 为前轮转角。

$$\frac{d}{dt}\left[\begin{array}{c}x(t)\\ y(t)\\ \theta (t)\end{array}\right]=\left[\begin{array}{c}v(t)\cdot cos\theta (t)\\ y(t)\cdot sin\theta (t)\\ v(t)\cdot tan\varphi (t)/L_w\end{array}\right],\forall t.\text{\hspace{1em}(1)}$$

设当前时刻 $t=0$，则有，
$$\{\begin{array}{l}x(0)=x_0\\ y(0)=y_0\\ \theta (0)={\theta }_0\end{array}\text{\hspace{1em}(2)}$$
$t$ 时刻后有，
$$\{\begin{array}{ll}x(t)& =x(0)+{\int }_{\tau =0}^{t}v(\tau )\cdot cos\theta (\tau )\cdot d\tau \\ & =x_0+{\int }_{\tau =0}^{t}v(\tau )\cdot cos\theta (\tau )\cdot d\tau \\ y(t)& =y(0)+{\int }_{\tau =0}^{t}v(\tau )\cdot sin\theta (\tau )\cdot d\tau \\ & =y_0+{\int }_{\tau =0}^{t}v(\tau )\cdot sin\theta (\tau )\cdot d\tau \\ \theta (t)& =\theta (0)+{\int }_{\tau =0}^{t}v(\tau )\cdot tan\varphi (\tau )/L_w\cdot d\tau \\ & 以上是关于Hybrid\; Astar\; 算法剖析和实现的主要内容，如果未能解决你的问题，请参考以下文章\end{array}$$

Hybrid Astar 算法剖析和实现

Hybrid Astar 算法剖析和实现

Hybrid Astar 算法剖析和实现

Hybrid Astar 算法剖析和实现

Hybrid Astar 算法剖析和实现

Hybrid Astar 算法剖析和实现