Hybrid Astar 算法剖析和实现
Posted 穿越临界点
tags:
篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了Hybrid Astar 算法剖析和实现相关的知识,希望对你有一定的参考价值。
在学习资料满天飞的大环境下,知识变得非常零散,体系化的知识并不多,这就导致很多人每天都努力学习到感动自己,最终却收效甚微,甚至放弃学习。我的使命就是过滤掉大量的无效信息,将知识体系化,以短平快的方式直达问题本质,把大家从大海捞针的痛苦中解脱出来。
文章目录
0 前言
上两篇对Hybrid Astar算法的主体搜索流程做了剖析和实现,本节详细介绍节点拓展。
1 什么是节点拓展
节点拓展就是基于当前节点状态空间向量推导下一个节点状态空间向量。
关于状态空间的介绍可以参看这篇文章:Hybrid Astar 算法剖析和实现(二)_穿越临界点的博客-CSDN博客
2 基于自行车模型拓展
2.1 连续拓展
设当前节点 N o d e c u r r e n t Node_current Nodecurrent 中记录着车辆当前的位姿 [ x 0 , y 0 , θ 0 ] [x_0,y_0,\\theta_0] [x0,y0,θ0] (状态量)。求在控制量 [ v , ϕ ] [v,\\phi] [v,ϕ] 的控制下, t t t时刻后车辆的位姿。
简易自行车模型如下式,其中 L w L_w Lw 代表wheel base, ϕ \\phi ϕ 为前轮转角。
d d t [ x ( t ) y ( t ) θ ( t ) ] = [ v ( t ) ⋅ c o s θ ( t ) v ( t ) ⋅ s i n θ ( t ) v ( t ) ⋅ t a n ϕ ( t ) / L w ] , ∀ t . (1) \\fracddt \\beginbmatrix x(t) \\\\ y(t) \\\\ \\theta(t) \\endbmatrix = \\beginbmatrix v(t)\\cdot cos\\theta(t) \\\\ v(t) \\cdot sin\\theta(t) \\\\ v(t) \\cdot tan\\phi(t)/L_w \\endbmatrix ,\\forall t. \\tag1 dtd⎣⎡x(t)y(t)θ(t)⎦⎤=⎣⎡v(t)⋅cosθ(t)v(t)⋅sinθ(t)v(t)⋅tanϕ(t)/Lw⎦⎤,∀t.(1)
设当前时刻
t
=
0
t=0
t=0,则有,
x
(
0
)
=
x
0
y
(
0
)
=
y
0
θ
(
0
)
=
θ
0
(2)
\\begincases x(0)=x_0\\\\ y(0) = y_0 \\\\ \\theta(0) = \\theta_0 \\endcases \\tag2
⎩⎪⎨⎪⎧x(0)=x0y(0)=y0θ(0)=θ0(2)
t
t
t 时刻后有,
x
(
t
)
=
x
(
0
)
+
∫
τ
=
0
t
v
(
τ
)
⋅
c
o
s
θ
(
τ
)
⋅
d
τ
=
x
0
+
∫
τ
=
0
t
v
(
τ
)
⋅
c
o
s
θ
(
τ
)
⋅
d
τ
y
(
t
)
=
y
(
0
)
+
∫
τ
=
0
t
v
(
τ
)
⋅
s
i
n
θ
(
τ
)
⋅
d
τ
=
y
0
+
∫
τ
=
0
t
v
(
τ
)
⋅
s
i
n
θ
(
τ
)
⋅
d
τ
θ
(
t
)
=
θ
(
0
)
+
∫
τ
=
0
t
v
(
τ
)
⋅
t
a
n
ϕ
(
τ
)
/
L
w
⋅
d
τ
=
θ
0
+
∫
τ
=
0
t
v
(
τ
)
⋅
t
a
n
ϕ
(
τ
)
/
L
w
⋅
d
τ
(3)
\\begincases x(t)&=x(0)+\\int_\\tau=0^t v(\\tau)\\cdot cos\\theta(\\tau) \\cdot d\\tau \\\\[2ex] &=x_0+\\int_\\tau=0^t v(\\tau)\\cdot cos\\theta(\\tau) \\cdot d\\tau\\\\[3ex] y(t)&=y(0)+\\int_\\tau=0^t v(\\tau)\\cdot sin\\theta(\\tau)\\cdot d\\tau \\\\[2ex] &=y_0+\\int_\\tau=0^t v(\\tau)\\cdot sin\\theta(\\tau)\\cdot d\\tau \\\\[3ex] \\theta(t) &= \\theta(0) +\\int_\\tau=0^t v(\\tau) \\cdot tan\\phi(\\tau)/L_w \\cdot d\\tau \\\\[2ex] &= \\theta_0 +\\int_\\tau=0^t v(\\tau) \\cdot tan\\phi(\\tau)/L_w \\cdot d\\tau \\endcases\\tag3
⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧x(t)y(t)θ(t)=x(0)+∫τ=0tv(τ)⋅cosθ(τ)⋅dτ=x0+∫τ=0tv(τ)⋅cosθ(τ)⋅dτ=y(0)+∫τ=0tv(τ)⋅sinθ(τ)⋅dτ=y0+∫τ=0tv(τ)⋅sinθ(τ)⋅dτ=θ(0)+∫τ=0tv(τ)⋅tanϕ(τ)/Lw⋅dτ以上是关于Hybrid Astar 算法剖析和实现的主要内容,如果未能解决你的问题,请参考以下文章