文章目录
0 摘要1 概览1.1 传统方法1.2 如何构建
E
S
D
F
ESDF
ESDF ？1.3
E
G
O
−
P
l
a
n
n
e
r
EGO-Planner
EGO−Planner 介绍1.3.1 方法步骤
2 相关工作2.1 基于梯度的运动规划2.2
E
S
D
F
ESDF
ESDF2.3 碰撞避免2.3.1 算法1：
C
h
e
c
k
A
n
d
A
d
d
O
b
s
t
a
c
l
e
I
n
f
o
CheckAndAddObstacleInfo
CheckAndAddObstacleInfo2.3.2 碰撞力估计小结
3 基于梯度的轨迹优化器3.1 建模3.1.1 光滑项惩罚3.1.2 碰撞项惩罚3.1.3 可行项惩罚
3.2 最优化解法（数值优化）
4 时间重新分配以及轨迹进一步优化4.1 具体步骤
5 实验结果5.1 算法框架5.2 实施详情
6 总结
0 摘要
基于梯度的规划器广泛用于四旋翼的局部路径规划，其中欧几里德距离场 (ESDF) 对于评估梯度大小和方向至关重要。然而，计算这样一个字段有很多冗余，因为轨迹优化过程只覆盖了 ESDF 更新范围的非常有限的子空间。在本文中，提出了一种基于无 ESDF 梯度的规划框架，显著减少了计算时间。主要改进是惩罚函数中的碰撞项是通过将碰撞轨迹与无碰撞引导路径进行比较来制定的。只有当轨迹碰到新的障碍物时，才会存储生成的障碍物信息，使规划器只提取必要的障碍物信息。然后，如果违反动态可行性，我们会延长时间分配，引入各向异性曲线拟合算法，在保持原始形状的同时调整轨迹的高阶导数。
1 概览
1.1 传统方法
传统方法一般是采用基于梯度的规划器，但是会依靠预先构建的
E
S
D
F
ESDF
ESDF 图来评估梯度的大小和方向，并使用数值优化来生成局部最优解。但是，就是构建
E
S
D
F
ESDF
ESDF 图很困难，
E
S
D
F
ESDF
ESDF 计算占用了执行本地规划的总时间的
70
%
70\%
70%，所以说，构建
E
S
D
F
ESDF
ESDF 成为了基于梯度的规划器的瓶颈。
1.2 如何构建
E
S
D
F
ESDF
ESDF ？
方法可以分为增量式的全部更新，以及批量的局部计算，然而这两种方法都没有关注轨迹本身。因此，过多的计算花费在计算对规划没有贡献的
E
S
D
F
ESDF
ESDF 值上。换句话说，当前基于
E
S
D
F
ESDF
ESDF 的方法不能单独和直接地为轨迹优化服务。如下图所示，对于一般的自主导航场景，期望无人机在本地避免碰撞，轨迹仅覆盖 ESDF 更新范围的有限空间。在实践中，虽然一些手工规则可以决定一个很小的 ESDF 范围，但它们缺乏理论上的合理性，并且仍然会导致不必要的计算
如图，优化过程中的轨迹只覆盖了 ESDF 更新范围的非常有限的空间
E
S
D
F
ESDF
ESDF 更新的空间就是被粉色墙面包围的空间 但是轨迹优化期间覆盖的空间（如上图紫色的部分） 所以轨迹仅覆盖
E
S
D
F
ESDF
ESDF 更新范围的有限空间，在实践中，虽然一些手工规则可以决定一个很小的 ESDF 范围，但它们缺乏理论上的合理性，并且仍然会导致不必要的计算
1.3
E
G
O
−
P
l
a
n
n
e
r
EGO-Planner
EGO−Planner 介绍
E
G
O
−
P
l
a
n
n
e
r
EGO-Planner
EGO−Planner 是一个无
E
S
D
F
ESDF
ESDF 的基于梯度的局部路径规划框架
E
G
O
−
P
l
a
n
n
e
r
EGO-Planner
EGO−Planner 由基于梯度的样条优化器和后细化程序组成
1.3.1 方法步骤
首先使用平滑度，碰撞以及动态可行性优化轨迹与预先计算的
E
S
D
F
ESDF
ESDF 的传统方法不同，通过将障碍物内的轨迹与引导的无碰撞轨迹进行比较对比，来对碰撞成本进行建模之后将力投射到碰撞轨迹上并生成估计的梯度以将轨迹包裹在障碍物之外在优化过程中，轨迹会在附近的障碍物之间反弹几次，最终终止于安全区域这样，我们只在必要时计算梯度，避免在与局部轨迹无关的区域计算
E
S
D
F
ESDF
ESDF如果生成的轨迹违反动态限制，这通常是由不合理的时间分配引起的，则激活细化过程在细化期间，当超出限制时重新分配轨迹时间。随着时间分配的扩大，生成了一种新的
B
B
B样条，它在平衡可行性和拟合精度的同时，拟合了之前的动态不可行为了提高鲁棒性，拟合精度采用各向异性建模，在轴向和径向上有不同的惩罚
2 相关工作
2.1 基于梯度的运动规划
基于梯度的运动规划是无人机局部轨迹生成的主流，这是一种无约束的非线性优化许多框架利用其丰富的梯度信息，直接优化配置空间的轨迹还有一种连续时间多项式的轨迹优化方法，但是，势函数积分导致了很大的计算代价还有就是老朋友
B
−
S
p
l
i
n
e
B-Spline
B−Spline的参数化，利用其凸包特性通过寻找无碰撞路径（通常可由采样的路径规划找出，比如
K
i
n
o
d
y
n
a
m
i
c
Kinodynamic
Kinodynamic
R
R
T
∗
RRT*
RRT∗），将无碰撞的初始路径作为前端，成功率显著提高当初始无碰撞路径的生成考虑动力学约束的同时，性能会进一步提高结合感知意识，鲁棒性提升明显
E
S
D
F
ESDF
ESDF 在评估与附近障碍物的梯度大小和方向的距离方面起着至关重要的作用
2.2
E
S
D
F
ESDF
ESDF
E
S
D
F
ESDF
ESDF 长期以来一直被用于从嘈杂的传感器数据构建对象，有学者提出了一种包络算法，将
E
S
D
F
ESDF
ESDF 构造的时间复杂度降低到
O
(
n
)
O(n)
O(n)；其中
n
n
n 表示为体素数。该算法不适用于
E
S
D
F
ESDF
ESDF 的增量构建，而四旋翼飞行过程中经常需要动态更新场。为了解决这个问题，
O
l
e
y
n
i
k
o
v
a
Oleynikova
Oleynikova 和
H
a
n
Han
Han 提出了增量
E
S
D
F
ESDF
ESDF 生成方法，即
V
o
x
b
l
o
x
Voxblox
Voxblox 和
F
I
E
S
T
A
FIESTA
FIESTA。尽管这些方法在动态更新情况下非常有效，但生成的
E
S
D
F
ESDF
ESDF 几乎总是包含可能根本不会在规划过程中使用的冗余信息。如上图所示，该轨迹仅扫过整个
E
S
D
F
ESDF
ESDF 更新范围的非常有限的子空间。因此，设计一种更智能、更轻量级的方法，而不是维护整个领域是有价值的。
2.3 碰撞避免
决策变量是
B
B
B 样条曲线的控制点
Q
Q
Q。每个
Q
Q
Q 都独立地拥有自己的环境信息。一开始，生成了一条满足约束但是不考虑障碍物的
B
B
B 样条曲线
Φ
Φ
Φ，接下来，对于在迭代中检测到的每个碰撞段，用一些算法（比如A*，RRT*等全局规划算法）生成一条无碰撞路径
Γ
Γ
Γ。对于发生碰撞的线段的每个控制点
Q
i
Q_i
Qi，都会生成一个在障碍物表面的定位点
p
i
j
p_{ij}
pij，且对应一个排斥方向向量
v
i
j
v_{ij}
vij，
v
i
j
v_{ij}
vij与向量
Q
i
Q_i
Qi
p
i
j
p_{ij}
pij同向，这里不应该是相等，因为这个
v
i
j
v_{ij}
vij只是一个单位方向向量，看下方图中的红色虚线与实线即可。每一对
p
,
v
{p, v}
p,v都对应一个特定的控制点
Q
i
Q_i
Qi，这点也能从图中看出来，如下图所示。
用
i
∈
N
+
i ∈ N+
i∈N+ 表示控制点的索引，
j
∈
N
j ∈ N
j∈N 表示 {
p
,
v
p, v
p,v} 对的索引。请注意，每个 {
p
,
v
p, v
p,v} 对只属于一个特定的控制点。为简洁起见，省略了下标
i
j
ij
ij 而不会引起歧义。则从
Q
i
Q_i
Qi 到第
j
j
j 个障碍物的障碍物距离定义为
p
,
v
{p, v}
p,v是怎么生成的？穿过障碍物的轨迹
Φ
Φ
Φ 为控制点
Q
Q
Q 生成几个
(
p
,
v
)
(p, v)
(p,v) 对
p
p
p 是障碍物表面上的点（前面说了
p
p
p是由控制点
Q
Q
Q生成的），
v
v
v 是从控制点
Q
Q
Q指向
p
p
p 的单位向量)垂直于切向量
R
i
R_i
Ri （这个切向量，仔细看，就是之前用
A
∗
A*
A∗ 生成的那条的不考虑碰撞的曲线的切线）的平面
Ψ
Ψ
Ψ 与
Γ
Γ
Γ （
Γ
Γ
Γ是那一条无碰撞路径）相交，形成一条线
l
l
l，从中确定
p
,
v
{p,v}
p,v 对。 距离场定义
d
i
j
=
(
Q
i
−
p
i
j
)
⋅
v
i
j
d_{ij} = (Q_i − p_{ij} ) · v_{ij}
dij=(Qi−pij)⋅vij 的切片可视化。颜色表示距离，箭头是等于
v
v
v 的相同梯度。
p
p
p 在平面上（障碍物表面上的点，如图
c
c
c ）。
2.3.1 算法1：
C
h
e
c
k
A
n
d
A
d
d
O
b
s
t
a
c
l
e
I
n
f
o
CheckAndAddObstacleInfo
CheckAndAddObstacleInfo
伪代码解析：
F
i
n
d
C
o
n
s
e
c
u
t
i
v
e
C
o
l
l
i
d
i
n
g
S
e
g
m
e
n
t
(
Q
)
：
FindConsecutiveCollidingSegment(Q)：
FindConsecutiveCollidingSegment(Q)： 找到与控制点
Q
Q
Q 发生碰撞的障碍物，并判断其是否存在
G
e
t
C
o
l
l
i
s
i
o
n
S
e
g
m
e
n
t
(
)
：
GetCollisionSegment()：
GetCollisionSegment()：提取出与控制点
Q
Q
Q 发生碰撞的障碍物
.
p
u
s
h
_
b
a
c
k
(
)
：
.push\_back()：
.push_back()：将障碍物添加入障碍物集合
S
S
S 中
P
a
t
h
S
e
a
r
c
h
(
)
：
PathSearch()：
PathSearch()：针对障碍物生成一个无碰撞的路径
F
i
n
d
_
p
_
v
_
P
a
i
r
s
(
Q
,
p
a
t
h
)
：
Find\_p\_v\_Pairs(Q, path)：
Find_p_v_Pairs(Q,path)：就是根据控制点与无碰撞路径确定匹配的
(
p
,
v
)
(p, v)
(p,v)对
Q
i
Q_i
Qi 到第
j
j
j 个障碍物的距离如下，需要注意单位向量
v
v
v 在第一次生成后就不会再次发生改变，所以
d
i
j
d_{ij}
dij 的值是分正负的，
d
i
j
=
(
Q
i
−
p
i
j
)
⋅
v
i
j
d_{ij} = (Q_i − p_{ij} ) · v_{ij}
dij=(Qi−pij)⋅vij通过以上的那个公式，我们就可以利用其来判断这个控制点
Q
i
Q_i
Qi因为之前说了，在老障碍物当中
v
i
j
v_{ij}
vij 与向量
Q
i
Q_i
Qi
p
i
j
p_{ij}
pij 同向，那么向量
Q
i
Q_i
Qi
p
i
j
p_{ij}
pij 就是
−
(
Q
i
−
p
i
j
)
-(Q_i - p_{ij})
−(Qi−pij)，是这个与
v
i
j
v_{ij}
vij 同向，而
(
Q
i
−
p
i
j
)
(Q_i - p_{ij})
(Qi−pij) 肯定就是与
v
i
j
v_{ij}
vij 反向了，如果不是新发现的障碍物，那么
d
i
j
=
(
Q
i
−
p
i
j
)
⋅
v
i
j
d_{ij} = (Q_i − p_{ij} ) · v_{ij}
dij=(Qi−pij)⋅vij 算出来肯定是
d
i
j
<
0
d_{ij} < 0
dij<0 （两者反向），而对于新发现的障碍物，肯定就是
d
i
j
=
(
Q
i
−
p
i
j
)
⋅
v
i
j
d_{ij} = (Q_i − p_{ij} ) · v_{ij}
dij=(Qi−pij)⋅vij，算出来为
d
i
j
>
0
d_{ij} > 0
dij>0
综上，为了防止轨迹被拉出当前障碍物前，为了避免在前几次迭代过程中轨迹从当前障碍物逃逸之前产生重复的
{
p
,
v
}
\{p, v\}
{p,v} 对，迭代过程中反复生成
{
p
,
v
}
\{p,v\}
{p,v} 对，判断是否为新障碍物的标准是：如果控制点
Q
i
Q_i
Qi 处于障碍物中时，并且对于当前得到的所有障碍物
j
j
j 满足
d
i
j
>
0
d_{ij} >0
dij>0 ，则该障碍物为新发现的障碍物，从而只计算影响轨迹的障碍物信息，减少运行时间。
2.3.2 碰撞力估计小结
为了将必要的*环境意识融入当地的规划中，需要明确地构建一个目标函数(设计基于梯度的轨迹优化器)，使轨道远离障碍。
E
S
D
F
ESDF
ESDF 提供了这种至关重要的碰撞信息，但代价是沉重的计算负担。此外，如图
2
2
2 所示，由于
E
S
D
F
ESDF
ESDF 反馈的错误信息不足，基于
E
S
D
F
ESDF
ESDF 的规划者很容易陷入局部最小值，无法逃脱障碍。为了避免这种情况，额外的前端总是需要提供一个无碰撞的初始轨迹。由于明确设计的斥力对于不同的任务和环境都是相当有效的，所以上述方法在提供避免碰撞的重要信息方面优于
E
S
D
F
ESDF
ESDF 。
3 基于梯度的轨迹优化器
3.1 建模
本文使用均匀
B
B
B 样条曲线
Φ
Φ
Φ 来表示轨迹，其阶数为
p
b
p_b
pb，均匀
B
B
B 样条曲线的每个节点有相同的时间间隔
{
t
1
,
t
2
,
.
.
.
,
t
M
}
\{t_1, t_2, ... , t_M\}
{t1,t2,...,tM}，每个节点与其父节点之间的时间间隔相同，为：
△
t
=
t
m
+
1
−
t
m
△_t = t_{m+1} - t_m
△t=tm+1−tm
B
B
B 样条曲线的第一个性质也即凸包性质表明，某段曲线只与
p
b
+
1
p_b+1
pb+1 个连续的控制点（
Q
∈
R
3
\mathrm{Q} \in \mathbb{R}^3
Q∈R3）有关，并且曲线被包含在这些点构成的凸包内，例如
B
B
B 样条的
(
t
i
,
t
i
+
1
)
(t_i, t_{i+1})
(ti,ti+1) 内的跨度位于由
{
Q
i
−
p
b
,
Q
i
−
p
b
+
1
,
.
.
.
,
Q
i
}
\{Q_i - p_b, Q_{i - p_b + 1}, ... , Q_i\}
{Qi−pb,Qi−pb+1,...,Qi} 形成的凸包内
B
B
B 样条曲线的第二个性质表面，
B
B
B 样条曲线的
k
k
k 阶导数仍然是
p
b
−
k
p_b−k
pb−k 阶
B
B
B 样条曲线，由于时间间隔
△
t
△_t
△t 是相同的，轨迹
Φ
Φ
Φ 的一阶（
V
i
V_i
Vi）、二阶（
A
i
A_i
Ai）、三阶（
J
i
J_i
Ji）导数的控制点分别为：
V
i
=
Q
i
+
1
−
Q
i
Δ
t
,
A
i
=
V
i
+
1
−
V
i
Δ
t
,
J
i
=
A
i
+
1
−
A
i
Δ
t
\mathbf{V}_i=\frac{\mathbf{Q}_{i+1}-\mathbf{Q}_i}{\Delta t}, \mathbf{A}_i=\frac{\mathbf{V}_{i+1}-\mathbf{V}_i}{\Delta t}, \mathbf{J}_i=\frac{\mathbf{A}_{i+1}-\mathbf{A}_i}{\Delta t}
Vi=ΔtQi+1−Qi,Ai=ΔtVi+1−Vi,Ji=ΔtAi+1−Ai其实大体上的框架还是
F
a
s
t
−
P
l
a
n
n
e
r
Fast-Planner
Fast−Planner的框架，
H
K
U
S
T
HKUST
HKUST还是牛的哇再根据
U
A
V
UAV
UAV 的微分平坦特性，需要降低要规划的变量，优化问题被重新定义为：
min
Q
J
=
λ
s
J
s
+
λ
c
J
c
+
λ
d
J
d
\min _{\mathrm{Q}} J=\lambda_s J_s+\lambda_c J_c+\lambda_d J_d
QminJ=λsJs+λcJc+λdJd
微分平坦特性，在做轨迹规划的过程中不可能对
U
A
V
UAV
UAV
12
12
12 维的全维度空间进行规划，这是非常复杂的，但可以找到一个平坦输出的空间，这个空间只有四个维度的变量，位置
x
,
y
,
z
x,y,z
x,y,z和偏航角
Ψ
Ψ
Ψ，剩下所有的状态都能用这四个变量和其有限阶导数的代数组合所表示，因此在做无人机轨迹规划的时候只需要对这四个变量进行规划就可以了。
是不是很熟悉？这其实就是在贝塞尔曲线那里的三个惩罚项函数
J
s
J_s
Js 为光滑项惩罚，
J
c
J_c
Jc 为碰撞项惩罚，
J
d
J_d
Jd 为动力学可行项惩罚，
λ
λ
λ 为惩罚项的权值
3.1.1 光滑项惩罚
在
B
−
S
p
l
i
n
e
B-Spline
B−Spline 轨迹优化当中被提出，可以看看我之前的
B
l
o
g
Blog
Blog ，有提到过。光滑性惩罚被公式化为轨迹参数(加速度、加加速度等)的平方导数上的时间积分。由于
B
B
B 样条曲线的凸包性质，只要最小化轨迹
Φ
Φ
Φ 的二阶和三阶控制点的平方和就能够有效地减小加速度和加加速度的平方和（类似
m
i
n
i
m
u
m
minimum
minimum
s
n
a
p
snap
snap ，这东西真是经典，哪里都用到了），公式如下：
J
s
=
∑
i
=
1
N
c
−
2
∥
A
i
∥
2
2
+
∑
i
=
1
N
c
−
3
∥
J
i
∥
2
2
J_s=\sum_{i=1}^{N_c-2}\left\|A_i\right\|_2^2+\sum_{i=1}^{N_c-3}\left\|J_i\right\|_2^2
Js=i=1∑Nc−2∥Ai∥22+i=1∑Nc−3∥Ji∥22
3.1.2 碰撞项惩罚
碰撞惩罚使控制点远离障碍物，这是通过采用安全间隙和惩罚控制点
d
i
j
<
s
f
d_{ij}< s_f
dij d i j d_{ij} dij 的减小而抑制其斜率，从而得到分段函数： j c ( i , j ) = { 0 , c i j ≤ 0 c i j 3 , 0 ≤ c i j ≤ s f 3 s f c i j 2 − 3 s f 2 c i j + s f 3 , c i j > s f j_c(i, j)= \begin{cases}0, & c_{i j} \leq 0 \\ c_{i j}^3, & 0 \leq c_{i j} \leq s_f \\ 3 s_f c_{i j}^2-3 s_f^2 c_{i j}+s_f^3, & c_{i j}>s_f\end{cases} jc(i,j)=⎩ ⎨ ⎧0,cij3,3sfcij2−3sf2cij+sf3,cij≤00≤cij≤sfcij>sf c i j = s f − d i j c_{i j}=s_f-d_{i j} cij=sf−dij对所有控制点的惩罚求和得到总的碰撞项惩罚： J c = ∑ i = 1 N c j c ( Q i ) J_c=\sum_{i=1}^{N_c} j_c\left(Q_i\right) Jc=i=1∑Ncjc(Qi)相比于传统用三线性插值的方法求碰撞项的梯度，直接计算二次连续可微惩罚函数对 Q i Q_i Qi 的导数来得到梯度： ∂ j c ∂ Q i = ∑ i = 1 N c ∑ j = 1 N p { 0 , c i j ≤ 0 − 3 c i j 2 , 0 ≤ c i j ≤ s f − 6 s f c i j + 3 s f 2 , c i j > s f \frac{\partial j_c}{\partial Q_i}=\sum_{i=1}^{N_c} \sum_{j=1}^{N_p} \begin{cases}0, & c_{i j} \leq 0 \\ -3 c_{i j}^2, & 0 \leq c_{i j} \leq s_f \\ -6 s_f c_{i j}+3 s_f^2, & c_{i j}>s_f\end{cases} ∂Qi∂jc=i=1∑Ncj=1∑Np⎩ ⎨ ⎧0,−3cij2,−6sfcij+3sf2,cij≤00≤cij≤sfcij>sf 3.1.3 可行项惩罚 通过限制轨迹在每一维上的高阶导数来保证其可行性。由于凸包的性质，对控制点的导数进行约束足以约束整个 B B B 样条。 F ( ) F() F() 为为每个维度（速度加速度 J e r k Jerk Jerk）的高阶导数构造的惩罚函数： J d = ∑ i = 1 N c − 1 ω v F ( V i ) + ∑ i = 1 N c − 2 ω a F ( A i ) + ∑ i = 1 N c − 3 ω j F ( J i ) J_d=\sum_{i=1}^{N_c-1} \omega_v F\left(V_i\right)+\sum_{i=1}^{N_c-2} \omega_a F\left(A_i\right)+\sum_{i=1}^{N_c-3} \omega_j F\left(J_i\right)\\ Jd=i=1∑Nc−1ωvF(Vi)+i=1∑Nc−2ωaF(Ai)+i=1∑Nc−3ωjF(Ji) F ( C ) = ∑ r = x , y , z f ( c r ) F(C)=\sum_{r=x, y, z} f\left(c_r\right) F(C)=r=x,y,z∑f(cr) f ( c r ) = { a 1 c r 2 + b 1 c r + c 1 , c r < − c j ( − λ c m − c r ) 3 , − c j < c r < − λ c m 0 , − λ c m ≤ c r ≤ λ c m ( c r − λ c m ) 3 , λ c m < c r < c j a 2 c r 2 + b 2 c r + c 2 , c r > c j f\left(c_r\right)= \begin{cases}a_1 c_r^2+b_1 c_r+c_1, & c_r<-c_j \\ \left(-\lambda c_m-c_r\right)^3, & -c_j f(cr)=⎩ ⎨ ⎧a1cr2+b1cr+c1,(−λcm−cr)3,0,(cr−λcm)3,a2cr2+b2cr+c2,cr<−cj−cj 可以看出问题建模中应用的惩罚函数全部是多项式和，这有利于降低求解最优化问题的复杂度。（轻量化算法） 3.2 最优化解法（数值优化） 目标函数： min Q J = λ s J s + λ c J c + λ d J d \min _{\mathrm{Q}} J=\lambda_s J_s+\lambda_c J_c+\lambda_d J_d QminJ=λsJs+λcJc+λdJd 会随着新障碍物的加入而不断改变，这就要求求解器能够快速重启并求解，并且目标函数主要由二次项组成，所以 H e s s i a n Hessian Hessian（黑塞矩阵）信息能够加快收敛速度。但得到精确的 H e s s i a n Hessian Hessian 消耗大量计算机资源。所以使用拟牛顿法（ q u a s i − N e w t o n quasi-Newton quasi−Newton m e t h o d s methods methods) 从梯度信息中来近似计算 H e s s i a n Hessian Hessian在对比了 B a r z i l a i − B o r w e i n Barzilai-Borwein Barzilai−Borwein m e t h o d method method、 t r u n c a t e d truncated truncated N e w t o n Newton Newton m e t h o d method method和 L − B F G S L-BFGS L−BFGS m e t h o d h methodh methodh后发现， L − B F G S L-BFGS L−BFGS表现最好，平衡了重启损失和逆 H e s s i a n Hessian Hessian 估计的准确性。 L − B F G S L-BFGS L−BFGS 从以前的目标函数评估近似 H e s s i a n Hessian Hessian，但需要一系列的迭代，以达到一个相对准确的估计 4 时间重新分配以及轨迹进一步优化 在优化之前进行准确的时间分配是不合理的，因为那时候还不知道关于最终轨迹的信息，所以，一个额外的时间重新分配程序对于确保动态可行性是至关重要的之前都是将轨迹参数化为非均匀 B B B 样条线，当某些线段超过导数限制时，迭代地加长节点跨度的子集但是，一个节点跨度 △ t n △t_{n} △tn 会影响多个控制点，从而导致在接近开始状态时调整节点跨度时上一个轨迹的高阶导数不连续所以从上节得到的安全轨迹 Φ s Φ_s Φs ，经过合理的时间重新分配，重新生成一条均匀的时间分配合理的 B B B 样条曲线 Φ f Φ_f Φf，然后使用各向异性曲线拟合的方法（ a n an an a n i s o t r o p i c anisotropic anisotropic c u r v e curve curve f i t t i n g fitting fitting m e t h o d method method）使 Φ f Φ_f Φf 在保持形状与 Φ s Φ_s Φs 几乎相同的导数形状的同时（因为 Φ s Φ_s Φs 是安全轨迹，所以形状必须差不多相同），能够自由地优化其控制点，以满足高阶导数的约束 4.1 具体步骤 首先像 F a s t − P l a n n e r Fast-Planner Fast−Planner 所做的那样，计算极限超标率（超过限制的最大的比例，下标 m m m 表示限制的最大值）： r e = max { ∣ V i , r / v m ∣ , ∣ A j , r / a m ∣ , ∣ J k , r / j m ∣ 3 , 1 } r_e=\max \left\{\left|\mathbf{V}_{i, r} / v_m\right|, \sqrt{\left|\mathbf{A}_{j, r} / a_m\right|}, \sqrt[3]{\left|\mathbf{J}_{k, r} / j_m\right|},1\right\} re=max{∣Vi,r/vm∣,∣Aj,r/am∣ ,3∣Jk,r/jm∣ ,1} r e r_e re 表明相对于 Φ s Φ_s Φs 来说， Φ f Φ_f Φf 需要分配多少时间 V i V_i Vi， A j A_j Aj 和 J k J_k Jk 分别是与 △ t △t △t 的一次、二次、和三次成反比，通过与时间的反比关系可以降低速度及其导数得到了 Φ f Φ_f Φf 的新时间间隔为： Δ t ′ = r e Δ t \Delta t^{\prime}=r_e \Delta t Δt′=reΔt通过求解一个如下的闭式的最小二乘问题，在速度及其导数的约束下初始生成时间跨度为 Δ t ′ \Delta t^{\prime} Δt′ 的轨迹 Φ f Φ_f Φf ，同时保持与 Φ s Φ_s Φs 相同的形状和控制点数，然后重新计算光滑项惩罚和可行性项惩罚得到新的目标函数： min Q J ′ = λ s J s + λ d J d + λ f J f \min _{\mathbf{Q}} J^{\prime}=\lambda_s J_s+\lambda_d J_d+\lambda_f J_f QminJ′=λsJs+λdJd+λfJf λ f \lambda_f λf 是适应度（拟合）项的权重， J f J_f Jf 被定义为从 Φ f ( α T ′ ) Φ_f(αT′) Φf(αT′) 到 Φ s ( α T ) Φ_s(αT) Φs(αT) 各向异性位移的积分（ α ∈ [ 0 ， 1 ] α∈[0，1] α∈[0，1]），其中 T T T 和 T ′ T′ T′ 为轨迹 Φ s Φ_s Φs 和 Φ f Φ_f Φf 的持续时间由于拟合的曲线 Φ s Φ_s Φs 已经是无碰撞的（刚刚说过了 Φ s Φ_s Φs 是安全轨迹），所以对于两条曲线，用带有低权重的轴向位移 d a d_a da 来放宽光滑调整限制，用高权重的径向位移 d r d_r dr 来防止碰撞，如下图所示， 使用球状度量（好像在老师的某篇文章中也提到过，具体的我忘了）来使在同一球体表面的位移产生相同的惩罚。(关于径向位移和轴向位移应该在具体算法中了解，目前我认为轴向位移为该点的切线方向，而径向方向为该切线的垂线方向，两者计算公式如下：) d a = ( Φ f − Φ s ) ⋅ Φ ˙ s ∥ Φ ˙ s ∥ d r = ∥ ( Φ f − Φ s ) × Φ ˙ s ∥ Φ ˙ s ∥ ∥ \begin{aligned} d_a &=\left(\Phi_f-\boldsymbol{\Phi}_s\right) \cdot \frac{\dot{\Phi}_s}{\left\|\dot{\Phi}_s\right\|}\\ d_r &=\left\|\left(\Phi_f-\boldsymbol{\Phi}_s\right) \times \frac{\dot{\Phi}_s}{\left\|\dot{\Phi}_s\right\|}\right\| \end{aligned} dadr=(Φf−Φs)⋅∥ ∥Φ˙s∥ ∥Φ˙s=∥ ∥(Φf−Φs)×∥ ∥Φ˙s∥ ∥Φ˙s∥ ∥ 用于度量 Φ f ( α T ′ ) Φ_f(αT′) Φf(αT′) 惩罚大小的椭圆体是一个以 Φ f ( α T ′ ) Φ_f(αT′) Φf(αT′) 为中心的椭圆，其半长轴长度为 a a a、其半短轴长度为 b b b。则轴向位移 d a d_a da 和径向位移 d r d_r dr 为： d a ( α T ′ ) = ( Φ f ( α T ′ ) − Φ s ( α T ) ) ⋅ Φ s ˙ ( α T ) ∥ Φ s ˙ ( α T ) ∥ d r ( α T ′ ) = ∥ ( Φ f ( α T ′ ) − Φ s ( α T ) ) × Φ s ˙ ( α T ) ∥ Φ s ˙ ( α T ) ∥ ∥ \begin{aligned} &d_a\left(\alpha T^{\prime}\right)=\left(\Phi_f\left(\alpha T^{\prime}\right)-\Phi_s(\alpha T)\right) \cdot \frac{\dot{\Phi_s}(\alpha T)}{\left\|\dot{\Phi_s}(\alpha T)\right\|} \\ &d_r\left(\alpha T^{\prime}\right)=\left\|\left(\Phi_f\left(\alpha T^{\prime}\right)-\Phi_s(\alpha T)\right) \times \frac{\dot{\Phi_s}(\alpha T)}{\left\|\dot{\Phi_s}(\alpha T)\right\|}\right\| \end{aligned} da(αT′)=(Φf(αT′)−Φs(αT))⋅∥ ∥Φs˙(αT)∥ ∥Φs˙(αT)dr(αT′)=∥ ∥(Φf(αT′)−Φs(αT))×∥ ∥Φs˙(αT)∥ ∥Φs˙(αT)∥ ∥ 则拟合项可以表示为： J f = ∫ 0 1 [ d a ( α T ′ ) 2 a 2 + d r ( α T ′ ) 2 b 2 ] d α J_f=\int_0^1\left[\frac{d_a\left(\alpha T^{\prime}\right)^2}{a^2}+\frac{d_r\left(\alpha T^{\prime}\right)^2}{b^2}\right] \mathrm{d} \alpha Jf=∫01[a2da(αT′)2+b2dr(αT′)2]dα 其中 a a a 和 b b b 分别是椭圆的半长轴和半短轴，径向位移对应的半短轴 b b b 使径向位移的惩罚权重增大以防止防止碰撞 上式拟合项被离散化为有限个数的点 Φ f ( α Δ t ′ ) Φ_f(αΔt′) Φf(αΔt′) 和 Φ s ( α Δ t ) Φ_s(αΔt) Φs(αΔt) 其中， K ∈ R \mathrm{K} \in \mathbb{R} K∈R， 0 ≤ k ≤ [ T / △ t ] 0 \leq k \leq [T/△t] 0≤k≤[T/△t] 5 实验结果 5.1 算法框架 代码解析 F i n d I n i t ( Q , G ) ： FindInit(Q, G)： FindInit(Q,G)：生成一条满足终端约束但不考虑障碍物的 B B B 样条曲线 Φ Φ Φ 对应的控制点，对应初始化步骤 I s C o l l i s i o n F r e e ( E , Q ) ： IsCollisionFree(E, Q)： IsCollisionFree(E,Q)：判断控制点是否在环境中是无碰撞的，有碰撞时输出 f a l s e false false，无碰撞时输出 t r u e true true C h e c k A n d A d d O b s t a c l e I n f o ( E , Q ) ： CheckAndAddObstacleInfo(E, Q)： CheckAndAddObstacleInfo(E,Q)：检测控制点所在障碍物，并添加障碍物信息( { p , v } \{p, v\} {p,v}对以及距离场) E v a l u a t e P e n a l t y ( Q ) ： EvaluatePenalty(Q)： EvaluatePenalty(Q)：根据问题建模构造控制点相应惩罚项 J J J 以及对应的梯度（这个惩罚项就是那三个惩罚，光滑项惩罚，碰撞项惩罚，可行项惩罚） O n e S t e p O p t i m i z e ( J , G ) ： OneStepOptimize(J, G)： OneStepOptimize(J,G)：求解惩罚项的最小化问题，即最优化求解，从而得到满足惩罚项的最小化的控制点位置，即完成第一步的轨迹优化 I s F e a s i b l e ( Q ) ： IsFeasible(Q)： IsFeasible(Q)：判断由控制点 Q Q Q 决定的轨迹是否可行(主要是速度以及其多阶导数否超过限制最大值) R e A l l o c a t e T i m e ( Q ) ： ReAllocateTime(Q)： ReAllocateTime(Q)：通过重新分配由控制点 Q Q Q 决定的轨迹的时间降低速度以及其多阶导数，使其满足各类速度约束 C u r v e F i t t i n g O p t i m i z e ( Q ) ： CurveFittingOptimize(Q)： CurveFittingOptimize(Q)：将之前的惩罚中碰撞项替换为曲线拟合项，求解惩罚项最优化问题。使其在满足新时间间隔的前提下，拟合由旧控制点构成的轨迹得到新轨迹，在继承旧轨迹的无碰撞特性的前提下实现约束下可行性 5.2 实施详情 设置B样条曲线的阶数 p b = 3 p_b=3 pb=3，控制点的个数 N c = 25 N_c = 25 Nc=25个左右，具体由规划预期距离（大约 7 m 7m 7m）和初始的邻近点间距（大约 0.3 m 0.3m 0.3m）决定。这些参数根据经验通过平衡了问题的复杂度和自由度而得到 因为根据 B B B 样条曲线的性质，一个控制点只影响周围的轨迹，所以算法的时间复杂度为 O ( N c ) O(N_c) O(Nc) L − B F G S L-BFGS L−BFGS 的复杂性在相同的相对公差上也是线性的( T h e The The c o m p l e x i t y complexity complexity o f of of L − B F G S L-BFGS L−BFGS i s is is a l s o also also l i n e a r linear linear o n on on t h e the the s a m e same same r e l a t i v e relative relative t o l e r a n c e tolerance tolerance) 在无碰撞路径搜索中，我们采用 A A A 星（ A ∗ A* A∗）算法进行轨迹优化，而它生成的轨迹 Γ Γ Γ 常常贴着障碍物。因此，我们可以直接在 A A A 星算法生成的轨迹 Γ Γ Γ 上选择定位点（ a n c h o r anchor anchor p o i n t point point） p p p 而不用搜索障碍物的表面(这里才真正解释出了一开始所说采用 A A A 星算法的作用)。对于图 3 b 3b 3b 中定义的向量 R i R_i Ri，通过均匀 b b b 样条参数化的性质，可以推导出： R i = Q i + 1 − Q i − 1 2 Δ t \mathbf{R}_i=\frac{\mathbf{Q}_{i+1}-\mathbf{Q}_{i-1}}{2 \Delta t} Ri=2ΔtQi+1−Qi−1 这里的 R i R_i Ri是下图中确定 { p , v } \{p,v\} {p,v} 对的关键 读到这里，我们再看看论文中的图 3 ： 3： 3： 第一步：根据生成一条满足终端约束但不考虑障碍物的 B B B 样条曲线 Φ Φ Φ，依靠 A A A 星算法生成的轨迹 Γ Γ Γ 第二步：根据上述的 R i R_i Ri 公式通过 B B B 样条曲线的控制点计算出向量 R i R_i Ri，再做出垂直于 R i R_i Ri 的平面 Ψ Ψ Ψ，平面 Ψ Ψ Ψ 与依靠 A A A 星算法生成的轨迹 Γ Γ Γ 相交于定位点（ a n c h o r anchor anchor p o i n t point point） p p p，连接对应的定位点（ a n c h o r anchor anchor p o i n t point point） p p p 与控制点 Q Q Q，才得到直线 l l l，而向量 v v v 是向量 l l l 对应的由起点控制点 Q Q Q 到终点定位点 p p p 对应的单位向量。( v v v 可能是生成以后不会再变化的)。到这里才生成了 { p , v } \{p,v\} {p,v} 对 Q ： Q： Q：关于 { p , v } \{p,v\} {p,v} 对过程中，为什么将定位点 p p p 定位到 A A A 星算法生成的贴着障碍物的轨迹上，而不直接定位在障碍物表面？ A ： A： A：首先 A ∗ A* A∗ 算法生成的轨迹肯定是安全无碰撞的轨迹，其次直接定位到障碍物表面的话，因为这是仿真，难免会产生误差，使得与现实世界所得结果不符，再者使用 A ∗ A* A∗ 算法生成的轨迹定位的算力比直接定位在障碍物表面的要低，所以不管是为了留有稳定的裕量还是为了降低计算的算力，还是选择使用 A ∗ A* A∗ 算法进行定位。 6 总结 该方法仍然存在一些缺陷，即 A ∗ A* A∗ 算法搜索引入的局部最小值和统一时间重新分配引入的保守轨迹。因此，我们将致力于执行拓扑规划，以逃避局部最小值，并重新制定问题，以生成接近最优的轨迹。规划器为静态环境设计，无需处理缓慢移动的障碍物（低于 0.5 m / s 0.5m/s 0.5m/s）。在未来，我们将通过移动对象检测和拓扑规划来研究动态环境导航。 参考阅读
发表评论