课题进展11-纯方位跟踪轨迹文献阅读
课题进展11-文献阅读
《AUV纯方位跟踪轨迹优化方法》
1. 研究问题
在纯方位跟踪系统中，AUV只能通过被动声纳获取目标的方位角信息，而无法直接获得距离和速度信息。这种系统的弱可观测性使得目标运动要素（位置、速度、航向）的估计精度较低，且容易发散。
主要挑战：
可观测性弱：仅凭方位角序列，目标运动要素（位置、速度、航向）的估计精度低，且容易滤波发散；
目标存在悖论：提高目标运动要素估计精度 VS 尽快与目标汇合，这两个之间往往相互矛盾—为了精确定位需要AUV做特定机动，而快速追及目标又需要不同的机动策略。
2. 系统建模
- 目标运动模型：匀速直线运动（CV模型）
- AUV运动模型：可机动模型
AUV和目标的运动状态如下：
系统的状态向量为相对状态向量：
状态方程为
$A$表示状态转移矩阵，$T$为采样时间间隔，$w(k)$为过程噪声，$z(k)$为被动声纳测得的目标方位角，$\boldsymbol{\upsilon} ( k )$是测量噪声，$u(k)$代表的是由AUV自身机动引起的、对相对运动状态的影响项，可以理解为控制输入项或机动补偿项。
3. 整体方案架构
作者提出基于距离的分段轨迹优化方法，核心思想是将跟踪过程按距离（即目标运动要素收敛程度）分为两个阶段，采用不同策略。
3.1 阶段一：基于位置误差GDOP的轨迹优化
定义优化目标为：
其中$P(k∣k)$是EKF（扩展卡尔曼滤波）输出的位置状态协方差矩阵。
每个时刻选择使$C_{GDOP}$最小的AUV机动方式，即在各时刻实现最优定位精度。同时引入相对运动指标：
该指标反映AUV相对于目标的可观测能力强弱
3.2 阶段二：基于短期预测的机动策略
该阶段分为预测、评估、决策三步
（1）预测
预测就是假设AUV短期可能的状态。AUV在每个指令周期内，根据自身机动性能约束（最大转弯角速度，转弯半径、最大/最小速度），枚举短期内所有可能的状态集合（前进、左转、右转、加速等）。
（2）评估（收益函数构建）
对每个预测状态，从三个维度计算收益：
距离收益$C_1$：AUV越接近目标，收益越高
航向调整收益$C_2$：航向变化越小，收益越高
速度收益$C_3$：根据期望速度和最大速度的关系定义
（3）决策
综合三因子加权计算，选择综合收益最大的状态执行：
4. 仿真结果与分析
AUV速度为2m/s，目标速度分别为1m/s、1.5m/s、2m/s、2.5m/s、3m/s。
从图1-图3中可以看出，当目标速度为1m/s、1.5m/s、2m/s、2.5m/s时，AUV能够与目标汇合。当目标速度较小时，AUV能够自主改变航向等待目标的到来。但是当目标速度为3m/s时，AUV无法追踪上目标。
无法追踪上目标的原因：
目标比AUV移动的更快，即使AUV采取最优的机动策略，也无法缩小与目标之间的距离。
在纯方位跟踪系统中，AUV只能获得目标的方位角，无法直接获得距离信息，为了估计目标的位置和速度，AUV必须进行机动（转弯、变速），否则系统不可观测。
那为什么目标速度为2.5m/s时，AUV可以追上：
核心原理在于：AUV可以利用几何关系和路径规划，通过走“捷径”来弥补速度的不足。相当于你虽然跑的慢，但是抄近道，反而能比车更早到达前面的某个路口，在那里等着它汇合。