Trajectory generation for quadrotor while tracking a moving target in cluttered environment

摘要

提出了一种四旋翼在相对模式下跟踪运动目标时的轨迹生成方法。与现有方法相比，考虑了安全飞行区域、车辆物理极限和平滑度等因素，保证了车辆的避障和真实跟踪飞行。摘要针对复杂环境下四旋翼的安全可行航路点，采用并行粒子群优化算法，在考虑目标预测状态和真实跟踪模式的基础上，结合安全性，计算出四旋翼的安全可行航路点。然后，采用多路径点约束的运动规划方法，并将其嵌入代价函数中，通过凸优化方法解决四旋翼的轨迹生成和鲁棒跟踪问题，以四旋翼安全飞行廊道的几何约束保证了运动规划的实时性。数值模拟实验验证了该方法的有效性。

1 文章主要贡献

本文研究了一种新的四旋翼跟踪机动目标并保持相对跟踪模式的轨迹生成方法。本文将重点研究四旋翼机动运动目标跟踪与轨迹生成问题，将其表述为一个明确考虑杂波环境下动态约束和安全约束的优化问题。采用一种几何安全飞行走廊，提供了一个可压缩跟踪轨迹的可行飞行空间。为了便于轨迹生成的研究，我们假设四旋翼对障碍物和环境信息有充分的了解，并且可以观察或预测目标的状态。 本工作的主要贡献可以总结如下：

提出了一种基于相对跟踪模式的四旋翼目标跟踪的两步寻迹方法。首先，以四旋翼的跟踪性能和飞行安全性为代价，在较短的运动视距内找到可行的航路点。然后，采用序列运动规划方法，有效地降低了四轴飞行器的控制成本和QP中与期望路径点的偏差，保证了四轴飞行器的平滑性和跟踪性能，同时考虑了安全区域和可行性约束。

我们将轨迹表示为轴解耦的分段多项式，这样可以减少计算时间，有效地生成飞行轨迹。

本文的其余部分组织如下:在第2节中，介绍了整个系统并制定了目标跟踪轨迹。第3节介绍了基于优化的轨迹生成方法。第4节展示了在混乱环境下的仿真。最后，我们在第5节讨论结果并得出一些结论。

2 前言

2.1 轨迹公式

四旋翼有一个六维构型空间。然而，四旋翼动力学的四个输入是差分平坦[13]，状态和输入可以写为四个精心选择的平坦输出及其导数的代数函数。 σ = [ x , y , z , ψ ] T \sigma=[x,y,z,\psi]^T σ=[x,y,z,ψ]T是平坦输出的合适选择， r = [ x , y , z ] T r=[x,y,z]^T r=[x,y,z]T是全局坐标系中质心的坐标， ψ \psi ψ是四旋翼的yaw角。微分平坦意味着控制机器人沿着平坦输出轨迹移动的控制输入是平坦输出的代数函数及其有限数量的导数。
分段多项式广泛应用于四轴飞行器的轨迹规划，通过适当选择度数和块数，可以用任意数量的连续导数[15]表示任意复杂的轨迹。四旋翼在每个笛卡尔维 x , y , z x,y,z x,y,z上的所有轨迹都可以表示为n阶多项式，我们用标准能量基q(t)表示四旋翼的轨迹为：


这里 c μ i ∈ R 3 c_{\mu i}\in R^3 cμi​∈R3是在每一维 μ ∈ x , y , z \mu \in x,y,z μ∈x,y,z 上第i条四旋翼轨迹的系数

2.2 实现结构

杂波环境下四旋翼跟踪运动目标需要控制、环境映射、状态估计和目标观测模块。这些研究结果为机载在线应用提供了充分的依据。对于实时应用，我们的方法如图2所示。首先利用运动目标和地图的预测状态计算出具有相对模式的可行航路点，利用几何寻径算法生成标称航路;其次，生成安全飞行走廊，为跟踪轨迹生成模块提供安全几何约束;最后，基于第3节的框架生成四旋翼的跟踪轨迹。

3 跟踪轨迹生成

在本节中，我们提出了一种基于优化的四旋翼在复杂环境下跟踪机动运动目标时的飞行轨迹生成方法。

3.1 标称路径点生成

当使用机载传感器估计运动目标的位姿时，由于遮挡或摄像机的有限视场跟踪损失，测量可能间歇性可用。由于在杂波环境下跟踪目标时会遇到很多障碍物，我们必须在间歇观测的同时对目标运动进行预测，以保证跟踪的持续性。运动目标的运动预测有许多新的方法，如BP神经网络、多项式曲线拟合和其他基于深度学习的方法。考虑到观测误差和间歇性测量噪声的影响，本文采用一种基于高斯过程回归的方法对目标在短运动水平内的运动进行了预测，该方法既能对任意一个黑箱函数进行建模，又能对其不确定性进行建模。假设观测噪声在目标定位过程中满足高斯分布，选择平方指数核函数 k ( x , x ′ ) k(x, x') k(x,x′)进行预测:

高斯噪声 ε ∼ ( 0 , σ ) \varepsilon \sim (0,\sigma) ε∼(0,σ)， P μ 0 ( t ) P_{\mu}^0(t) Pμ0​(t)是在某一时刻 t t t观测到的位置值。隐藏真实值 P μ t ( t ) P_{\mu}^t(t) Pμt​(t)表示为高斯过程(GP)先验。

其中GP(·)表示高斯过程，其均值为0,k为协方差函数。

由图3所示， θ \theta θ 表示目标与四旋翼之间的相对平面角，可以得到四旋翼在短运动视界内的航路点:

其中， θ \theta θ表示目标与四旋翼的相对平面角， P ( t f ) = [ P x ( t f ) , P y ( t f ) , P z ( t f ) ] P(t_f)=[P_x(t_f),P_y(t_f),P_z(t_f)] P(tf​)=[Px​(tf​),Py​(tf​),Pz​(tf​)]是运动目标在未来某一时刻 t f t_f tf​的预测位置， T ( t f ) = [ T x ( t f ) , T y ( t f ) , T z ( t f ) ] T(t_f)=[T_x(t_f),T_y(t_f),T_z(t_f)] T(tf​)=[Tx​(tf​),Ty​(tf​),Tz​(tf​)] 是四旋翼的标称航路点， t f = t c + Δ t t_f=t_c+\Delta t tf​=tc​+Δt 中， t c t_c tc​是当前时间， Δ t \Delta t Δt 是预测的时间， d h d_h dh​是目标和四旋翼之间期望水平距离， d v d_v dv​ 是相对于移动目标的期望垂直高度。作为通常的跟踪应用， d h d_h dh​和 d v d_v dv​都预设为恒定值。

3.2 可行路径点生成

由于存在不可行区，由式(6)计算出的解可能会遇到障碍，说明在这种情况下我们必须找到一个可行的解：

SFC代表第3.3部分建造的安全飞行走廊， V ⃗ f \vec V_f V f​是方向矢量 T ( t f ) − P ( t f ) T(t_f)-P(t_f) T(tf​)−P(tf​)。 W ( t f ) W(t_f) W(tf​) 是满足图4所示约束条件的可行路径点。

为了找到用于跟踪轨迹生成的可行路径点，
并行粒子群优化(PSO)算法是用来高效的获得满足Eq.7的可行解 W ( t ) W(t) W(t)。
在我们的实现中,用来计算在迭代中单个粒子的速度和位置的方程如下所示

v i t v^t_i vit​ 是粒子i的速度， α i t \alpha^t_i αit​ 是它在迭代步骤t处的位置。 α p i t \alpha^t_{pi} αpit​是粒子的最佳位置， α g t \alpha^t_g αgt​是在迭代步骤t时集群的最佳位置。 N p N_p Np​是集群大小， N c N_c Nc​是迭代步的最大值（ i ≤ i ≤ N p , 1 ≤ t ≤ N c i\leq i\leq N_p,1\leq t\leq N_c i≤i≤Np​,1≤t≤Nc​）。 r a n d 1 rand_1 rand1​和 r a n d 2 rand_2 rand2​是0和1之间的随机值； w w w是自惯性影响因子。 c 1 c_1 c1​和 c 2 c_2 c2​是社会影响因子。在我们的实现中，参数设置如下: N p = 20 , N c = 20 , w = 0.4 , c 1 = c 2 = 2 N_p=20,N_c=20,w=0.4,c_1=c_2=2 Np​=20,Nc​=20,w=0.4,c1​=c2​=2。如图5所示。

3.3 安全飞行走廊生成

在本模块中，利用几何最短路径查找算法找到从当前位置到下一个可行路径点的标称路径。对于路径中的每个节点，应用基于Kdtree的方法获取节点位置到最近障碍物的距离。得到如图6(a)所示的安全空间球和内接立方体。然后在轴向方向 μ ∈ x , y , z \mu \in x, y, z μ∈x,y,z上扩大每个立方体，使其达到最大无碰撞体积[16]，[17]，形成安全飞行走廊(SFC)，并进行过滤。我们发现在相邻的立方体和无人机中都包含的重叠部分(图6(b))一定能够通过。
如上所述，如图7所示，最终安全飞行走廊由多个闭合凸立方体 S F C i SFC_i SFCi​组成, i ∈ 1 , 2 , ⋯   , m i\in 1,2,\cdots ,m i∈1,2,⋯,m ，并且对于 i ∈ 1 , 2 , ⋯   , m − 1 i\in 1,2,\cdots ,m-1 i∈1,2,⋯,m−1：

杂乱环境下目标跟踪的平面图示。在(a)中，标称路径算法找到的路径为绿色网格。飞行走廊标示为黄色。其中红色虚线圆和黑色虚线方分别表示节点到最近障碍物的位置所得到的球体和内接该球体的立方体。绿色曲线表示目标轨迹，红色曲线表示四旋翼跟踪轨迹。黑星和红星分别表示目标在某一时刻的预测位置和四旋翼在某一时刻的路径点。在(b)中，由红色虚线组成的矩形O是无人机肯定会经过的重叠部分。

3.4 代价函数

为了实现上述行为，多目标函数被广泛用于平衡跟踪精度、轨迹平滑度和相对跟踪模式，多权重值应当基于应用场景来进行连续经验调参，来获得更好的结果，这对于实际应用和实验来说是不现实的。在该框架中，我们采用了基于多路径点约束优化的轨迹生成方法，并且将 x , y , z x,y,z x,y,z三个维度上
在运行时间 t ( t ∈ [ t i − 1 , t i ] ) t(t\in [t_{i-1},t_i]) t(t∈[ti−1​,ti​])时，将x、y、z三个维度的加加速度来拟合四轴飞行器的跟踪性能，实现四轴飞行器的平滑响应。

3.5 强制约束

对于分段跟踪轨迹的生成，在执行目标跟踪任务的同时，必须对轨迹的光滑性、安全性和动力学可行性进行约束。
1）路径点与连续性约束:
对于多项式轨迹，需要保证轨迹通过生成的可行路径点，并且在两个连续多项式连接点处的第k次(k = 0,1,2)导数处连续光滑，以避免四旋翼偏航角突变:

W ( t f ) W(t_f) W(tf​)是在3.2部分计算的可行路径点。
2）安全约束
为了保证飞行安全，整个轨迹的每一个维度都要被限制在立方体内。通过在第j段上增加边界限制来实现安全约束。

3）动态可行性约束:
在实际的目标跟踪飞行中，必须满足速度、推力和体型的物理限制。本文采用[21]中的方法，将这些限制解耦为轨迹速度和加速度的约束。在实践中，长方体约束体积用于考虑外部约束，如安全规程[20]。四转子跟踪时的速度和加速度分别约束为 [ v m a x , v m a x ] 和 [ a m a x , a m a x ] [v_{max},v_{max}]和[a_{max},a_{max}] [vmax​,vmax​]和[amax​,amax​]，使生成的轨迹动态可行:

3.6 求解跟踪轨迹生成问题

在上述路径点、动态约束和安全约束条件下，采用优化方法求解优化问题(11)，找到最优解。算法1描述了求解跟踪轨迹的算法。

只有有限数量的约束路径点用于增强四旋翼的通过性[8]，与[13]中的密集约束点方法相比，它节省了大量的计算能力。然后定义如下优化问题：

等式约束包括连续性约束和路径点约束，不等式包含动态约束和安全约束。注意Q是正半定的，这表明它是海森矩阵。所生成的优化问题是一个典型的凸二次程序，可以用一般现成的优化求解器如c++的OOQP或MATLAB的quadprog进行有效的求解，这些优化求解器均可用于实时应用。

实验结果

本节给出了仿真结果，以证明该框架的可行性和鲁棒性，特别是在复杂的三维环境下跟踪机动目标。采用Matlab提供的通用凸求解器Quadprog在配备Intel Core i5-8250U处理器和8G RAM的Intel NUC上求解原QP表示的目标跟踪问题。
首先，我们给出了在复杂环境下给定初始和目标状态下无人机轨迹生成的仿真结果。四旋翼的任务是在一个 15 m × 25 m × 5 m 15m\times 25m \times 5m 15m×25m×5m的空间飞行，包含几个障碍。多项式曲线的阶数n设为7。最大水平速度和加速度分别设为 3 m / s 3 m/s 3m/s和 2 m / s 2 2 m/s^2 2m/s2,最大 垂直速度和加速度设为 1 m / s 1 m/s 1m/s和 0.5 m / s 2 0.5 m/s^2 0.5m/s2。在本次仿真中，我们将初始和终点位置设为 p i n i t = [ 0 , 0 , 5 ] p_init=[0,0,5] pi​nit=[0,0,5]和 p t e r m = [ 8 , 20 , 2 ] p_{term}=[8,20,2] pterm​=[8,20,2]，速度和加速度均设为零。设定整个规划过程的总时间为 15 s 15 s 15s。合成轨迹如图8所示，

图8:杂乱环境下轨迹生成示意图。(a)和(b)中，灰色区域表示障碍物，蓝色立体区域为安全飞行走廊(SFC)，其目的是用安全空间包围所有轨迹。红色曲线为四旋翼的安全飞行轨迹。三维分辨率均设置为0.05m。

上述轨迹的速度和加速度分别如图9所示。

可见，生成的轨迹是无障碍的动态可行的，每个再规划周期的平均时间成本为 9.6 m s 9.6 ms 9.6ms。
接下来，跟踪仿真结果如图10所示。

图10 在杂乱环境下用相对位置模式跟踪地面运动目标的轨迹生成示意图。蓝色星形表示目标位置，红色星形为(7)计算出的可行航路点，黑色曲线表示四轴飞行器安全跟踪轨迹。
在本实验中，所期望的相对平面距离和角度设置为:

移动视界为 2.0 s 2.0 s 2.0s，更新频率为 1 h z 1hz 1hz。正如预期的那样，四旋翼安全并成功地跟踪运动目标，同时尽可能保持相对模式与高重新规划率。在图11中，相对模式设置为 d h = 0 , d v = 4 d_h = 0, d_v = 4 dh​=0,dv​=4，运动视界为 2.0 s 2.0 s 2.0s，更新频率为 1 H z 1 Hz 1Hz。可以看出，四旋翼可以有效地跟踪目标的参考位置并保持 d h = 0 d_h = 0 dh​=0。
本文提出的目标跟踪轨迹生成方法具有良好的性能通过使用高效的凸优化求解器。以上两次跟踪仿真每个周期的平均时间开销如下：

如表1所示，本文所提出的方法使得在实时应用场景下基于机载计算机生成跟踪轨迹变成可行的。与以往文献[5,10]相比，我们提出了一种在杂波环境下实现四旋翼相对模式目标跟踪的新方法。采用两步法提高了计算效率，避免了频繁的权值调整，提高了对环境的适应性。将安全约束和动态约束进行线性化，有效地解决了优化问题，满足了实际应用的要求。

总结

本工作在考虑复杂环境下动态约束和安全约束的情况下，提出了一种实时跟踪四旋翼机动目标的最优轨迹生成策略。仿真结果表明，在保持相对模式的情况下，可以成功地跟踪运动目标。这项工作带来了许多令人兴奋的研究机会。例如，在实际应用中，需要对目标进行高精度的状态预测来处理目标的视差问题。还需要进一步的研究如何提高动态障碍物环境下的跟踪鲁棒性。