这项工作涉及在杂乱环境中配备视觉传感器的飞行器的移动目标追逐任务。 与无障碍或稀疏环境相比，追赶者应能够同时处理碰撞和遮挡，并提高飞行效率。 为了通过实时重规划应对这些挑战，我们引入了一种针对目标可见性的指标，并提出了一个级联的追逐计划者。 通过图搜索方法，我们首先生成确保安全并优化可见性的一系列追逐走廊和航路点。 在接下来的阶段中，走廊和航路点在二次编程中用作约束和目标，从中我们完成了动态可行的追赶轨迹。 所提出的算法在多个密集环境中进行了测试。 带有完整代码实现和GUI的模拟器AutoChaser可以在https://github.com/icsl-Jeon/traj_gen_ vis中找到。
由于定位[2]，[3]，跟踪[4]和控制[5]自主性的增强，MAV已被广泛用于基于视觉的任务，如摄像和监视。 特别是，先前研究（[1]，[8]，[9]）中提出的规划器通过实时适用性提高了自主跟随目标的性能。 尽管可以在无障碍或稀疏情况下合理使用这些方法，但是对于一般混乱的环境仍然存在问题。
追踪任务中存在多个障碍带来了两个主要困难：碰撞和遮挡，应同时处理。 除了安全方面的考虑外，在追赶者没有关于目标未来运动的完整信息的情况下，遮挡处理尤其重要。 在这种情况下，尽管可以从追赶者当前时间看到目标可见性，但无法保证其在未来时间范围内的可见性。此外，如果追逐者花费过多的代价（例如，行驶距离），以备将来能见度大时，会导致运动效率低下。
为了克服这些问题，追踪规划器需要在保证安全的前提下优化能见度和行驶效率。同样，快速计算对于有效响应目标运动的不确定性至关重要。在本文中，我们提出了一种在线追踪规划器，该规划器以综合方式处理密集障碍物的遮挡和碰撞。
图1 上：在复杂的城市中追逐任务。 目标沿着红色线在地面上移动。 连接目标和追踪者的方位向量在喷气彩色图比例尺中标记可见性得分（第IV-A，VI节）。 下：追踪从不同可见度权重
w
v
w_v
wv获得的轨迹（第IV-B节）。 在（b）中，将
w
v
w_v
wv设置为大于（a）的值。 可以根据行驶效率和可见性的相对重要性来调整追赶走廊（青色），同时确保安全。 如虚线框（A），（B）所示，对于不同的
w
v
w_v
wv获得了不同的追逐行为。 在
w
v
w_v
wv较低的情况下，尽管追赶者行进的距离较短，但在虚线框区域（A）中看不到目标。 为了在
t
4
t_4
t4之后不确定的目标运动（框区域（B））获得更高安全性的视点，需要进行更多
w
v
w_v
wv作用的策略。
我们的主要贡献是双重的。 首先，我们介绍了移动目标的可见性指标，该指标可以在一般环境中定义，而不受障碍物形状的限制。 通过简单的计算，该度量即可对可见性对目标未来运动不确定性的敏感性进行编码。 其次，提出了一种具有实时性能的级联跟踪策略。 我们没有采用具有多个目标的非凸优化，而是通过图形搜索方法为稀疏的路径点提供种子，以确保安全性和可见性。 基于离散视点骨架，以二次规划（QP）公式生成分段多项式，并附加了安全考虑。 规划器在确保整个追踪路径的安全性的同时调整了跟踪效率和可见性。 我们发现级联规划器可以在不牺牲在线计算速度的情况下确保轨迹的追踪质量。
研究[14]，[15]基于图像空间中的公式处理追逐任务中的遮挡和碰撞。 [14]利用二次规划（QP）来计算控制输入，以避免单个多面体障碍物的遮挡。 在[15]中使用了一个切换控制器，它由多个目标组成（标称跟踪，避免碰撞和遮挡处理）。 尽管上述作品中的公式不需要在3D空间中重建目标，但这些方法在单个障碍物或稀疏环境中均得到了验证。
与[14]，[15]相反，一组研究[6]，[7]，[16]，[17]使用笛卡尔空间公式。 这些作品借助可靠的地图绘制和定位功能，采用3d重建过程来处理更复杂的场景。考虑到遮挡，[17]中的作者引入了一个风险函数，该函数定义为目标逃离追赶者的传感器范围以评估可见性的最短距离。 尽管该算法针对多个障碍物进行了验证，但它必须计算障碍物的精确几何形状以进行遮挡剔除。 [16]采用安全走廊生成方法[11]来避免动态目标追赶的碰撞。 他们研究了飞行效率与避免碰撞之间的权衡。尽管安全飞行能避开障碍物，但仍未考虑目标的可见性，无法保证将目标保持在视线范围内。
最近，[6]，[7]提出了规划策略，该策略同时考虑了障碍环境中的遮挡和碰撞。 文献[6]基于封堵器的椭圆形假设，定义了考虑可见性和安全性的成本函数。 [7]在octomap框架（[18]）下提出了一个最能适应障碍物形状的优化问题。 为了测量目标的可见性，作者计算了二维流形上有符号距离场（SDF）的积分。
文献[6]，[7]都定义了非线性成本函数，并用梯度下降法对其进行了优化。 尽管这些方法保证了MAV的动态可行性并可以在线计算解决方案，但由于同时考虑了安全性和可见性的多目标成本函数的不凸性，因此它们受到局部最优的约束。如[13]中指出的那样，完全依靠非凸公式在更密集的环境中尤其无效。
在本小节中，我们提到了一些有关安全规划的最新研究，以达到静态目标并避免障碍。尽管它的任务与我们的任务不同，但值得注意的是[10]，[11]，[13]中开发的用于处理密集环境的方法。
该工作采用了级联规划器，在第一阶段播种一系列稀疏航路点或与安全走廊一起规划，随后进行连续轨迹完成。 让我们将第一个过程称为预规划。 离散优化（例如基于搜索的[19]或基于采样的方法[20]）用于预规划阶段。 它的输出可作为连续优化的良好初始猜测或约束，从而避免了质量较差的本地解决方案。 在具有安全性，动态可行性和优化稳定性的复杂环境中，验证了级联规划器在上述工作中的适用性。 受这些方法的启发，我们为自动追赶开发了一个预计划器，该预计划器对于更具挑战性的情况非常有效。
在我们的追逐环境中，涉及多个任意形状的障碍物。 假定环境被称为先验条件。 为了进行目标跟踪和状态估计，假定MAV（追赶者）配备了具有有限FOV的视觉传感器。 在本文中，假设这些过程是可靠的（[2]，[4]），我们将重点放在追赶运动策略上。 关于感兴趣的运动目标，假定其具有有限的加速度，并且最初位于MAV的FOV中。 目标的整个路径在追赶者之前无法获得。 取而代之的是，为追赶者提供目标在未来短时间内的运动，并以与[8]，[16]类似的方式在空中反复更新。
基于追赶者和目标的这些假设，我们旨在构建一个具有以下功能的追赶规划器。
1）保证整个跟踪路径的安全性。
2）以预定的时间步长保证目标的可见性。
3）优化了目标的可见性和总行驶距离。
4）跟踪路径的高阶导数的积分被最小化，以保持路径的平滑性和飞行效率。
5）响应更新的在线信息（即图2的左侧），实现了规划器的实时性能。
图2.拟设计的追逐策略的规划流程概述。这项工作的重点是运动规划器，它由两个阶段（即右侧的两个方框）组成，以响应信息的更新。
预先规划过程（绿色虚线框）生成安全可见的视点序列。 连接每个点的线还确保了与障碍物之间的安全距离，从而获得了一组走廊。 基于走廊和视点，使用凸优化（QP）计算平滑的追逐路径。
在开始之前，我们定义了几种表示法以便于讨论。
在本小节中，我们将解释为了最大化跟踪的鲁棒性而最大化
x
p
x_p
xp的可见性度量。 我们假设追踪器具有Sec III-A中所述的地图信息
χ
\chi
χ。 为简单起见，我们假设追踪器将
x
p
−
x
c
x_p -x_c
xp−xc作为其方位向量。
我们首先考虑
x
∈
R
3
x\in R^3
x∈R3的水平集函数
ϕ
(
x
)
\phi(x)
ϕ(x) 表示障碍物区域
χ
o
b
s
\chi_{obs}
χobs，其中对于
x
∈
χ
o
b
s
，
ϕ
(
x
)
≤
0
x\in\chi_{obs}，\phi(x)\le0
x∈χobs，ϕ(x)≤0，对于
x
∈
χ
f
r
e
e
，
ϕ
(
x
)
>
0
x\in\chi_{free}，\phi(x)>0
x∈χfree，ϕ(x)>0。选择
ϕ
(
x
)
\phi(x)
ϕ(x) 作为欧几里得距离场
E
D
F
（
x
;
χ
o
b
s
）
EDF（x; \chi_{obs}）
EDF（x;χobs），可以通过诸如[21]之类的方法有效地进行计算。 基于
ϕ
(
x
)
\phi(x)
ϕ(x) ，当从位置
x
x
x沿着方位向量
x
p
−
x
x_p-x
xp−x观测时，我们定义目标的可见性
x
p
x_p
xp，如下所示。
因为
ϕ
(
x
)
\phi(x)
ϕ(x)等效于
E
D
F
（
x
;
χ
o
b
s
）
EDF（x; \chi_{obs}）
EDF（x;χobs），式（1）是线段
L
（
x
;
x
p
）
L（x; x_p）
L（x;xp）与
χ
o
b
s
\chi_{obs}
χobs中的元素之间的最短距离，如图3所示。因为（1）是对应于
x
p
x_p
xp的遮挡区域和可见区域的水平集表示，因此可以用集合
{
x
∣
ϕ
(
x
;
x
p
)
>
0
}
\{x|\phi(x;x_p)>0\}
{x∣ϕ(x;xp)>0}来表示
χ
v
i
s
(
x
p
)
\chi_{vis}(x_p)
χvis(xp)和用
χ
o
c
c
(
x
p
)
\chi_{occ}(x_p)
χocc(xp)表示
{
x
∣
ϕ
(
x
;
x
p
)
≤
0
}
\{x|\phi(x;x_p)\le0\}
{x∣ϕ(x;xp)≤0}。可见性水平集（1）的概念在几篇文献中用于遮挡描述[23]，以计算环境的可见体积。 当我们专注于追赶任务时，我们将利用
ψ
（
x
;
x
p
）
\psi（x; x_p）
ψ（x;xp）来衡量追赶者位置
x
x
x能够保持目标
x
p
x_p
xp可见的可靠性。
在本文中，为了与octomap兼容，我们在3D栅格地图D上进行操作。 可见性（1）的定义适用于我们的场景，原因如下：首先，可以根据D来计算（1），D的表示采用比以前关于假定特殊形状的障碍物的追赶研究（例如 [6]中的椭圆形和[17]中的多边形）更通用的 χ \chi χ。 其次，（1）的计算对于在线应用来说很容易处理。 在我们的问题设置中，一旦获得 ϕ ( x ) \phi(x) ϕ(x)，就可以在毫秒级计算出（1）。第三，（1）提供了 x p x_p xp视点的鲁棒性的度量，因为 ψ （ x ; x p ） \psi（x; x_p） ψ（x;xp）是水平集合函数，编码关于 x x x或 x p x_p xp扰动的遮挡敏感性，如[22]中所述（如图3所示）。 这有助于我们针对目标的未知未来运动可靠地评估视点的质量。
利用
ϕ
(
x
)
\phi(x)
ϕ(x)和
ψ
（
x
;
x
p
）
\psi（x; x_p）
ψ（x;xp）分别作为目标安全性和可见性的量度，引入预规划器来播种对应于目标在短期视野
H
H
H的未来路径的可用信息的一系列路径点。 如第III-A部分所提，在当前位置
x
c
（
t
）
x_c（t）
xc（t）处的追踪器提供
x
p
（
τ
）
（
t
≤
τ
≤
t
+
H
）
x_p（\tau）（t≤\tau≤t+ H）
xp（τ）（t≤τ≤t+H）。 让我们用时间步长
Δ
t
=
H
N
\Delta t = \frac{H}{N}
Δt=NH划分时间范围，并令
σ
[
t
0
,
t
N
]
=
（
x
0
,
x
1
,
⋯
,
x
N
）
\sigma_{[t_0, t_N]} =（x0,x_1,\cdots,x_N）
σ[t0,tN]=（x0,x1,⋯,xN）表示N +1个航点的序列，其中
x
n
=
x
c
（
t
n
）
x_n = x_c（t_n）
xn=xc（tn）和
t
n
=
t
+
n
Δ
t
（
n
=
0
,
1
,
⋯
,
N
）
t_n = t + n_{\Delta t}（n = 0, 1,\cdots,N）
tn=t+nΔt（n=0,1,⋯,N）。
对于
x
p
,
n
=
x
p
（
t
n
）
x_{p,n} = x_p（t_n）
xp,n=xp（tn），我们现在定义两个路径点
x
n
−
1
x_{n-1}
xn−1和
x
n
x_n
xn之间的过渡可见性代价
c
v
（
x
n
−
1
;
,
x
n
）
c_v（x_{n-1};,x_n）
cv（xn−1;,xn）如下所示
等式（2）表示与
x
p
,
n
−
1
x_{p,n-1}
xp,n−1和
x
p
,
n
x_{p, n}
xp,n相对应的两个连续可见性场的线积分的几何平均值的逆，并且如果所有沿着
L
(
x
n
−
1
,
x
n
)
L(x_{n-1},x_n)
L(xn−1,xn)的点属于
χ
o
c
c
(
x
p
,
n
)
\chi_{occ}(x_{p,n})
χocc(xp,n)或
χ
o
c
c
(
x
p
,
n
−
1
)
\chi_{occ}(x_{p,n-1})
χocc(xp,n−1)，那么
c
v
(
x
n
−
1
,
x
n
)
→
∞
c_v(x_{n-1},x_n)\rightarrow\infin
cv(xn−1,xn)→∞
我们的目标是找到一条预规划路径
σ
[
t
0
,
t
N
]
\sigma_{[t_0, t_N]}
σ[t0,tN]，它是以下优化问题的解决方案。
这里，
r
s
a
f
e
是
与
障
碍
物
之
间
的
安
全
距
离
，
而
r_{safe}是与障碍物之间的安全距离，而
rsafe是与障碍物之间的安全距离，而d_{des}$是目标与跟踪器之间的期望相对距离。
d
m
a
x
d_{max}
dmax是航路点之间的最大允许距离。 对于跟踪距离极限
d
u
p
p
e
r
,
d
l
o
w
e
r
d_{upper}, d_{lower}
dupper,dlower服从
d
l
o
w
e
r
≤
∥
x
n
−
x
p
∥
≤
d
u
p
p
e
r
d_{lower}\leq\parallel x_n-x_p\parallel\leq d_{upper}
dlower≤∥xn−xp∥≤dupper，
Ω
(
x
p
,
n
)
\Omega(x_{p,n})
Ω(xp,n)是在框区域
B
(
x
p
,
n
,
2
d
u
p
p
e
r
)
B
(
x
p
,
n
,
2
d
l
o
w
e
r
)
B(x_{p,n},2d_{upper}) B(x_{p,n},2d_{lower})
B(xp,n,2dupper)B(xp,n,2dlower)中选择的一组离散点，相应时间步
t
n
t_n
tn处的视点将被选择（请参见图IV-A中的红点组）。 通过调整权重
w
v
w_v
wv和
w
d
w_d
wd，在满足（3）中的约束的同时，实现了追赶者的不同行为。 在这项工作中，我们有兴趣研究总行驶距离和可见度之间的折衷，因此将更详细地分析
w
v
w_v
wv的影响（请参见图I和图8中的下图）。
为了求解（3），我们现在构造一个有向图
G
（
V
,
E
）
G（V, E）
G（V,E），其中每个节点对应于
σ
[
t
0
,
t
N
]
\sigma[t_0,t_N]
σ[t0,tN]的候选航路点。 对于一个顶点集
V
=
⋃
n
=
0
N
+
1
V
n
)
V=\bigcup\limits_{n=0}^{N+1}V_n)
V=n=0⋃N+1Vn)
引入
x
d
u
m
m
y
x_{dummy}
xdummy来将图包装为虚拟目标点，因为在我们的案例中未预先定义特定的目标点。 我们按照算法1中的过程构造
E
E
E(算法1基于C ++中使用的运算符进行表示)。（3）的解是通过最短路径搜索方法在G上从
x
c
（
t
0
）
∈
V
0
x_c（t_0）\in V_0
xc（t0）∈V0到
x
d
u
m
m
y
∈
V
N
+
1
x_{dummy} \in V_{N + 1}
xdummy∈VN+1计算得出的。
基于从上一节获得的解
σ
[
t
0
,
t
N
]
\sigma_{[t_0,t_N]}
σ[t0,tN]，我们生成了一组跟随通道，用于计算连续的最佳轨迹，这将在下一部分中进行描述。 现在，让我们考虑以下线性插值KaTeX parse error: Undefined control sequence: \lamda at position 1: \̲l̲a̲m̲d̲a̲（\tau;\sigma_{[…：
由于我们的公式（3），
σ
[
t
0
,
t
N
]
\sigma_{[t_0,t_N]}
σ[t0,tN]满足
γ
（
τ
;
σ
[
t
0
;
t
N
]
）
，
t
0
≤
τ
≤
t
N
\gamma（\tau;\sigma_{[t_0; t_N]}），t_0≤\tau≤t_N
γ（τ;σ[t0;tN]），t0≤τ≤tN，
γ
（
t
n
;
σ
[
t
0
;
t
N
]
）
∈
χ
v
i
s
(
x
p
(
t
)
)
，
n
=
{
0
,
1
,
⋯
,
N
}
\gamma（t_n;\sigma_{[t_0; t_N]}）\in\chi_{vis}(x_p(t))，n =\{ 0,1,\cdots,N\}
γ（tn;σ[t0;tN]）∈χvis(xp(t))，n={0,1,⋯,N},作为安全和可见性约束，如
I
I
I
−
B
−
1
）
III-B-1）
III−B−1）和2）所述。 同样，在权重
w
v
w_v
wv足够大而
Δ
t
\Delta t
Δt足够小的情况下，我们观察到条件
γ
（
τ
;
σ
[
t
0
;
t
N
]
）
，
t
0
≤
τ
≤
t
N
\gamma（\tau;\sigma_{[t_0; t_N]}），t_0≤\tau≤t_N
γ（τ;σ[t0;tN]），t0≤τ≤tN 已满足而不是较长行驶距离（见图I右侧）。
现在，我们在
τ
\tau
τ处定义追赶走廊，如下所示：
参数
l
c
o
r
r
(
τ
)
∈
R
3
l_{corr}(\tau)\in \mathbb{R}^3
lcorr(τ)∈R3代表在
τ
\tau
τ时刻廊道的尺寸，该参数被选择来满足以下安全性必要条件：
(
l
c
o
r
r
(
τ
)
)
i
≤
ϕ
(
γ
(
τ
;
σ
t
0
,
t
N
)
)
3
,
i
=
1
,
2
,
3
(l_{corr}(\tau))_i\leq\frac{\phi(\gamma(\tau;\sigma_{t_0,t_N}))}{\sqrt{3}},i=1,2,3
(lcorr(τ))i≤3
ϕ(γ(τ;σt0,tN)),i=1,2,3。附带说明，尽管放松
l
c
o
r
r
l_{corr}
lcorr对于平滑路径生成的灵活性可能是有利的，但它可能会降低两点
（
x
n
−
1
,
x
n
）
（x_{n-1},x_n）
（xn−1,xn）之间的可见性。 图1的下部显示了
w
v
w_v
wv的两个不同值的追赶通道示例。
上一节解释了预规划器如何考虑安全性和可见性而产生一系列的视点和追逐走廊。 利用四旋翼MAV [12]的微分平坦度以及预规划器的
σ
t
0
,
t
N
\sigma_{t_0,t_N}
σt0,tN和
Q
（
τ
;
σ
t
0
,
t
N
）
Q（\tau; \sigma_{t_0,t_N}）
Q（τ;σt0,tN），我们描述了如何为空中追赶者
x
c
（
τ
）
（
t
0
≤
τ
≤
t
N
）
x_c（\tau）（t_0≤\tau≤t_N）
xc（τ）（t0≤τ≤tN）生成动态可行的路径。
追赶者的偏航角是确定的，以使MAV坐标系的x轴朝向目标。追踪器路径表示为以下分段多项式。
这里，
p
n
,
k
∈
R
3
p_{n,k}\in\mathbb{R}^3
pn,k∈R3是K阶多项式的系数。它们是从以下优化中获得的。
（8）式是航点加加速度平方和平方误差的积分的加权和。 （8）可以按照与[12]，[13]类似的方式转换为二次编程。 凸优化可以通过诸如内点法或活跃集之类的算法有效地解决。 这样，安全性，可见性和行进距离的考虑就隐含地作为框约束
Q
（
τ
;
σ
t
0
,
t
N
）
Q（\tau; \sigma_{t_0,t_N}）
Q（τ;σt0,tN）而不是将它们包括在非凸目标函数中，如[6]，[7] 做到了。
