15、目标跟踪算法实现详解

目标跟踪算法实现详解

1. 跟踪算法概述

跟踪算法主要通过算法 3.1 进行总结。当解在边界内部时，采用带有 Armijo 规则的梯度下降法（算法 3.2）来解决最小化问题。梯度向量 $g_k$ 决定了梯度下降的搜索方向，而 Armijo 规则通过调整 $\alpha_{amj}$ 来确定在搜索方向上的移动距离。下面将详细介绍该跟踪算法在 MATLAB 和 Python 中的逐步实现。

2. 约束条件

2.1 最小转弯半径约束

在全局坐标系中给出的曲率约束（3.30），若转换到机体坐标系会变得简单很多。如图 3.18 所示，无人机的速度与机体 x 轴对齐，且 $v_B^y(0) = 0$。在机体坐标系中，曲率线变为 $u_B^y = \pm c_B^{cvt}$，其中：
[c_B^{cvt} = \frac{[v_B^x(0)]^2}{r_{min}}]
如果 $c_B^{cvt}$ 小于 $u_{ymax}$ 或 $u_{ymin}$，则相应的边界将被 $c_B^{cvt}$ 替换。在程序中，假设无人机沿机体 x 轴对称，因此 $u_{ymin} = -u_{ymax}$。

算法 3.1 给出了最优目标跟踪控制输入的具体步骤：
1. 初始化预测间隔和无人机/目标约束：$\Delta t$，$v_{max}$，$v_{min}$，$u_{xmax}$，$u_{xmin}$，$u_{ymax}$，$u_{ymin}$，$r_{min}$，$w_{max}$。
2. 设置无人机的初始位置/速度和目标位置：$x_a(0)$，$y_a(0)$，$v_x(0)$，$v_y(0)$，$x_t(0)$，$y