移动传感器如何用估计感知连续时间最优控制跟踪目标？

规划轨迹的同时塑造估计不确定性

目标跟踪利用测量估计目标系统的状态，这些测量往往由移动传感器采集，例如搭载距离或方位传感器的飞行器。由于测量同时依赖传感器状态和目标状态，规划传感器路径时不能只考虑传感器自身动力学和运行限制，还必须考虑沿轨迹采集的测量如何影响对目标的估计。

论文构建了一个估计感知最优控制问题，在传感器非线性动力学、协方差动力学、固定的初始与末端传感器状态、状态不等式约束以及输入集合的约束下，最小化关于移动传感器状态、目标协方差和控制输入的运行代价。规划器由状态估计器提供的最新目标状态估计和协方差估计初始化，因此轨迹是在顾及估计质量的前提下选出的，而不是事后才考虑。

为何驱动规划的是协方差而非可观测性

已有目标跟踪工作使用多种指标量化传感器轨迹对目标状态估计的影响，包括可观测性格拉姆矩阵、关注目标末状态的变体以及费舍尔信息矩阵。本文使用状态估计误差协方差，它直接量化状态估计的不确定性，其吸引力在于卡尔曼滤波等常用估计器给出的协方差同时考虑了过程噪声和测量噪声。

该方法内嵌连续时间扩展卡尔曼滤波。滤波器传播目标估计的均值和协方差，卡尔曼增益依赖移动传感器状态，协方差微分方程通过测量雅可比把目标协方差与传感器位置耦合起来。在规划所用的零新息假设下，未来测量不可得，因此预测测量等于其期望值，均值动力学简化为从初始均值出发对目标漂移项的积分。

约束在离散点之间也保持成立

论文指出，已有目标跟踪方法的一个局限是只在离散时间约束移动传感器状态。禁入区等约束往往关乎安全，必须始终成立，但信息量大的感知常需要激进机动，使状态逼近约束边界，于是离散点之间可能出现违反。减少这类违反通常需要密集的时间离散，从而增大轨迹优化问题的规模和计算成本。

为此，论文把移动传感器上的约束转化为带边界条件的辅助连续时间非线性动力学，并在最优控制问题中加入累积约束违反项。增广状态堆叠了传感器状态、半向量化协方差、累积违反量以及运行代价的积分，所得问题经离散化和线性化后交给现有优化算法。论文报告称，即使时间离散较稀疏，该方法也能在数值精度内满足移动传感器的连续时间约束。

利用协方差对称性获得高效梯度

施加连续时间约束带来一个计算挑战：基于梯度的连续时间最优控制要对底层动力学方程求导，而在目标跟踪中该方程不仅引入移动传感器状态与目标协方差之间的非线性耦合，还必须保持协方差矩阵的对称结构。

论文给出一个高效公式，利用协方差矩阵的对称性对扩展卡尔曼滤波中的连续时间非线性协方差动力学求导。半向量化变换提取协方差的下三角元素（含对角线），把变量数降到 n_p = n_ξ(n_ξ + 1)/2，并得到最优控制问题中使用的协方差变化率函数。作者指出，连续时间扩展卡尔曼滤波协方差的高效求导公式，以及将其用于目标跟踪中的连续时间约束，此前受到的关注很少甚至没有。

两个算例在不同动力学下检验方法

论文用增广拉格朗日算法在两个算例上演示该方法。第一个是 LTE 发射机定位，固定末端时间 t_f = 10、N = 5，初始传感器状态 x_0 = (1, 1, 0, 0)，初始目标状态 ξ_0 = (0, 0, 0.001)，初始协方差 P_0 = diag(4, 4, 1×10^-4)，末端传感器状态 x_f = (0, 1, 0, 0)。约束参数包括位置边界 r_min_S = (-0.25, 0.75)、r_max_S = (1.25, 1.25)，速度边界 v_min_S = -0.5 和 v_max_S = 0.5，输入边界 u_min = (-0.2, -0.5) 和 u_max = (0.2, 0.5)；噪声协方差为 Q = diag(1×10^-6, 1×10^-6, 1.4193×10^-4) 和 R = 5。

第二个算例是地面车辆跟踪，其中无人机对地面车辆采集距离测量，需要估计该车辆的位置、速度和航向。目标采用独轮车模型以展示非线性目标动力学，目标状态包含位置、前向速度、与 x 轴的夹角和角速度，测量为传感器与目标之间距离的标量距离。参数为 t_f = 10、N = 5，初始传感器状态 x_0 = (0, 0, 0, 0)，初始目标状态 ξ_0 = (5, 5, 1, 0, 0.3)，初始协方差 P_0 = diag(10, 10, 0.5, 2, 0.1)，末端传感器状态 x_f = x_0，约束参数 r_max_S = 4、r_min_ST = 1、u_max = 2，噪声协方差 Q = diag(0.01, 0.01, 0.1, 0.1, 0.1) 和 R = 5。

协方差与能量的权重改变估计结果

结果比较了三种权重方案：仅协方差（ω_p = 1）、仅能量（ω_u = 1）和平衡（ω_p = ω_u = 0.5）。图 1 追踪了两个算例中误差协方差矩阵的 Schatten 2-范数 ||P(t)||_2 随时间的变化。随着协方差权重 ω_p 增大，协方差轨迹下方的面积减小，表明相对于 ω_p = 0 的最小能量解，估计性能有所改善。

图 2 给出了两个算例中扫描目标权重 ω_p 和 ω_u 时能量目标与协方差目标之间权衡的帕累托前沿。这些前沿把权衡关系显式呈现出来：两个目标并不一致，权重的选择决定了规划轨迹在估计质量与控制代价之间的落点。论文自身的表述是，该方法在满足移动传感器连续时间约束的同时实现了估计感知的目标跟踪，即使时间离散较稀疏。

证据覆盖到哪里，又止于哪里

连续时间约束满足和估计感知跟踪的证据来自两个算例的数值仿真，即 LTE 发射机定位和地面车辆跟踪，二者都在 N = 5 下用增广拉格朗日方法求解。论文的比较对象是只在离散点施加约束的现成求解器；连续时间满足的结论限定在数值精度内，且规划公式依赖零新息假设，因为路径规划时未来测量不可得。

该方法面向非线性传感器动力学、非线性目标动力学、非线性测量模型以及扩展卡尔曼滤波中的非线性协方差动力学，因此其适用范围是由这些模型以及所述约束和噪声参数所刻画的问题类别。论文没有报告硬件实验，所报告的结果是在所列初始条件、约束参数和噪声协方差下的仿真结果。

关键在于约束时机而非协方差权重

论文的独特重点在于施加约束的时机。协方差权重是可调的权衡，帕累托前沿表明增大 ω_p 会降低协方差轨迹下方的面积（相对于最小能量解），但该方法真正区别性的主张是：传感器状态和输入约束在数值精度内在连续时间成立，同时仍支持稀疏时间离散。

对判断这是否重要的读者而言，相关比较是在只在离散点施加约束的规划器与用累积违反项和辅助连续时间约束动力学增广问题的规划器之间。论文的证据在两个仿真场景和所述参数下支持后者；而 ω_p 和 ω_u 的选择则决定所得轨迹落在协方差—能量权衡上的哪个位置。

本文引用的文献