赞
踩
基于梯度的轨迹优化 G T O GTO GTO在四旋翼飞行器的轨迹重新规划中得到了广泛的应用,但是它存在局部极小值,这不仅对安全是致命的,而且不利于航行的顺利进行。此文章提出了一种基于 G T O GTO GTO的再规划方法,系统地解决了这一问题。针对不可行的局部极小点,提出了一种路径引导优化 P G O PGO PGO方法,大大提高了重规划的成功率。并提出了一种拓扑路径搜索算法,用于捕获三维环境中不同的有用路径集合,每条路径引导一个独立的轨迹优化。它激活了对解决方案空间的更全面的探索,并输出了更优的重新规划的轨迹。
G T O GTO GTO方法,是一种主要的路径生成算法,并把路径生成看作是一个最小化目标函数的非线性优化问题,但是也有一些小问题,就是局部最小值
P G O PGO PGO方法,在优化的过程当中加入几何路径,由于路径有效地引导优化逃离不可行的局部最小值,才保证了规划的成功率
同时,拓扑路径搜索产生的多条不同路径与 P G O PGO PGO集成
可见性变形:两条起点终点相同的轨迹,两者之间无障碍物且连线无碰撞,就代表可见,但是形状可以不相同。
等价性检查: V D VD VD 的计算代价很高,应该计算一个可见性图,并在其中进行路径搜索,这比检验 U V D UVD UVD 的复杂性更高。
AddGuard(G, g)
:增加
g
u
a
r
d
guard
guard 节点到
G
G
G 中sample()
:采样节点VisibleGuard(G, Ps)
:得到
G
G
G 中能观测到
P
s
P_s
Ps 的所有节点.size()
:计算节点的的个数Path(g1, c, g2)
:将
c
o
n
n
e
c
t
o
r
connector
connector 与 两个
g
u
a
r
d
guard
guard 之间生成路径SharedNeighbors(G,g1,g2)
:从
G
G
G 当中提取能够与
g
1
、
g
2
g_1、g_2
g1、g2连接的连接点集Equivalent(p1,p2)
:判断拓扑连接是是否相同len(p)
:计算路径长度Replace(G,p,n)
:替换
c
o
n
n
e
c
t
o
r
connector
connector 点(
p
p
p换下
n
n
n)distinct
:bool型数据,
t
r
u
e
true
true 表示拓扑连接有差别,
f
a
l
s
e
false
false 表示拓扑连接无差别伪代码说明:在设计拓扑路径图中,我们引入了两种不同的图节点,即 g u a r d guard guard 守卫/警卫点和 c o n n e c t o r connector connector连接点,类似于 V i s i b i l i t y − P R M Visibility-PRM Visibility−PRM。 g u a r d guard guard 负责探索自由空间的不同部分,任何两个 g u a r d guard guard,任何两个 g u a r d guard guard 点 g 1 g_1 g1 和 g 2 g_2 g2 彼此之间都是不可见的,但是连接 g 1 g_1 g1 和 g 2 g_2 g2 的线段存在碰撞。
在主循环之前,在起始点和结束点创建两个 g u a r d guard guard 点。每当采样点对所有其他 g u a r d guard guard 都不可见时,就会在此点创建一个新的 g u a r d guard guard 点,如第 6 − 7 6-7 6−7行。为了形成路线图的路径,使用连接点 c o n n e c t o r connector connector 连接不同的 g u a r d guard guard如第 7 − 19 7 -19 7−19行。当采样点恰好对两个 g u a r d guard guard可见时,就会创建一个新 c o n n e c t o r connector connector 点,要么连接 g u a r d guard guard,以形成拓扑上不同的连接第 19 − 20 19 - 20 19−20行,要么替换现有 c o n n e c t o r connector connector 以创建更短的路径第 16 − 17 16-17 16−17行。设置时间限制 t m a x t_{max} tmax或采样次数限制 N m a x N_{max} Nmax来终止循环。
下两图给出了属于两个UVD类别的三个轨迹的示例。下图2描述了VD和UVD之间的关系。它们都定义了两条路径 τ 1 ( s ) τ_1(s) τ1(s)和 τ 2 ( s ) τ_2(s) τ2(s)之间的连续映射,其中 τ 1 ( s ) τ_1(s) τ1(s)上的点通过直线变换到 τ 2 ( s ) τ_2(s) τ2(s)上的点。主要区别在于,对于UVD,点 τ 1 ( s 1 ) τ_1(s_1) τ1(s1)被映射到 τ 2 ( s 2 ) τ_2(s_2) τ2(s2),其中 s 1 = s 2 s_1=s_2 s1=s2,而对于VD, s 1 s_1 s1不必等于 s 2 s_2 s2。在概念上,UVD不那么通用,它描述了VD类的子集。实际上,它捕获的不同路径的类别比VD略多,但用于等价检查的成本要低得多
为了检验UVD的关系,把 s ∈ [ 0 , 1 ] s∈[0,1] s∈[0,1]均匀离散为 s i = i / K , i ∈ [ 0 , K ] s_i = i / K, i ∈[0,K] si=i/K,i∈[0,K]并检查 τ 1 ( s i ) τ_1(s_i) τ1(si)和 τ 2 ( s i ) τ_2(s_i) τ2(si)的冲突,对于分段直线路径,使除了 τ ( s ) τ(s) τ(s)以外的任何 s s s都是两条直线的交点
第一阶段的优化目标函数为:综上这就是之前所说的目标函数。
f
p
1
=
λ
1
s
f
s
+
λ
1
g
f
g
f_{p 1}=\lambda_{1 s} f_s+\lambda_{1 g} f_g
fp1=λ1sfs+λ1gfg
这里的
f
s
f_s
fs是轨迹平滑性的惩罚函数,而
f
g
f_g
fg 是引导路径和
B
B
B样条轨迹之间的距离的惩罚代价,主要是利用模拟弹性力来构造惩罚函数,其中
Q
i
Q_i
Qi 为
B
B
B 样条轨迹的控制点,每个控制点
Q
i
Q_i
Qi 都被分配了一个引导路径上关联的点
G
i
G_i
Gi ,其由沿着引导路径进行均匀采样得到:
f
s
=
∑
i
=
p
b
−
1
N
−
p
b
+
1
∥
Q
i
+
1
−
2
Q
i
+
Q
i
−
1
∥
2
f_s=\sum_{i=p_b-1}^{N-p_b+1}\left\|\mathbf{Q}_{i+1}-2 \mathbf{Q}_i+\mathbf{Q}_{i-1}\right\|^2
fs=i=pb−1∑N−pb+1∥Qi+1−2Qi+Qi−1∥2
f
g
=
∑
i
=
p
b
N
−
p
b
∥
Q
i
−
G
i
∥
2
f_g=\sum_{i=p_b}^{N-p_b}\left\|\mathbf{Q}_i-\mathbf{G}_i\right\|^2
fg=i=pb∑N−pb∥Qi−Gi∥2
第二阶段的目标函数如下,其中
f
s
f_s
fs 还是轨迹平滑性惩罚成本函数,
f
c
f_c
fc 是在
E
S
D
F
ESDF
ESDF 上评估的碰撞成本函数,当轨迹接近障碍物时,
E
S
D
F
ESDF
ESDF的碰撞成本迅速增长。再利用惩罚函数
f
v
f_v
fv 与
f
a
f_a
fa 对 速度以及加速度的不可行程度进行量化,可以参考一下
m
i
n
i
m
u
m
minimum
minimum
s
n
a
p
snap
snap以及软硬约束的思想
f
p
2
=
λ
2
s
f
s
+
λ
2
c
f
c
+
λ
2
d
(
f
v
+
f
a
)
f_{p 2}=\lambda_{2 s} f_s+\lambda_{2 c} f_c+\lambda_{2 d}\left(f_v+f_a\right)
fp2=λ2sfs+λ2cfc+λ2d(fv+fa)
将轨迹表示为 B B B 样条,以实现更有效的成本评估,对于碰撞中的轨迹段,我们将其重新参数化为具有控制点 ( Q 0 , Q 1 , . . . , Q N ) {(Q_0, Q_1, ..., Q_N)} (Q0,Q1,...,QN)的 p b p_b pb次的均匀 B B B 样条(由于轨迹的边界状态约束,只有控制点的子集 ( Q p b , Q p b + 1 , ⋅ ⋅ ⋅ , Q N − p b ) {(Q_{pb},Q_{pb+1},···,Q_{N−pb})} (Qpb,Qpb+1,⋅⋅⋅,QN−pb)被优化。评价光滑度需要 Q p b − 2 、 Q p b − 1 、 Q N − p b + 1 和 Q N − p b + 2 {Q_{pb−2}、Q_{pb−1}、Q_{N−pb+1}和Q_{N−pb+2}} Qpb−2、Qpb−1、QN−pb+1和QN−pb+2。)
P G O PGO PGO 是由两个阶段组成的,第一段是产生一个中间的“热身轨迹”( w a r m u p warmup warmup t r a j e c t o r y trajectory trajectory)(如上图的那条绿色轨迹,原始的 B B B样条轨迹,以及使 B B B样条轨迹有效变形的外部信息,因为仅仅是应用 E S D F ESDF ESDF会是徒劳的)
在本文中,提出了一种用于自主四旋翼导航的鲁棒轨迹重新规划方法。它通过路径引导优化、拓扑路径搜索和并行轨迹优化来克服局部最小值。进行了广泛的基准比较和积极的自主飞行实验,以验证方法的鲁棒性。目前,拓扑路径搜索的性能令人满意,但其完整性没有详细分析。此外,对重新规划方法的理论最优性并不完全确定。
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。