CN103499809A - 一种纯方位双机协同目标跟踪定位路径规划方法 - Google Patents

Abstract

本发明公开了一种纯方位双机协同目标跟踪定位路径规划方法，针对纯方位双机协同三维目标跟踪定位路径规划问题提出基于扩展卡尔曼滤波器（EKF）和定位精度几何分布（GDOP）的优化算法。在建立双机协同被动跟踪EKF的基础上分别建立交互信息与协方差控制指标函数。为了克服基于EKF指标函数对目标模型的过度依赖，从目标定位角度建立了基于GDOP的指标函数。综合基于EKF和GDOP的指标函数，考虑约束条件建立了标准形式的双机协同路径规划模型，并给出了具体的模型求解步骤。所提方法实现了传感器观测、优化指标建立及路径规划解算的闭环，有效提高双机对非机动和机动辐射源跟踪定位能力，最终提高双机协同无源跟踪定位的精度。

Description

一种纯方位双机协同目标跟踪定位路径规划方法

技术领域

本发明属于信息技术领域，涉及一种纯方位双机协同目标跟踪定位路径规划方法。

背景技术

电子对抗、隐身飞机、反辐射导弹等雷达对抗手段的快速发展使得以主动雷达为代表的有源定位系统受到的威胁越来越严重。相对于有源定位系统，无源观测系统由于不向外辐射电磁信号而是通过获取目标辐射的可见光、红外或电磁信号对目标进行跟踪定位，因此具有隐蔽性好、生存能力强等优点。

无源观测系统只能报告目标的到达方位和到达时间，特别是在复杂电磁环境条件下，到达方位几乎成为唯一可靠的辐射源信号参数。纯方位目标单站目标跟踪与定位理论研究表明，跟踪定位的效果与观测站的运动轨迹密切相关。通常情况下为了获得较高的跟踪定位精度需要观测站以高于目标机动阶次的加速度进行机动。纯方位多站目标跟踪与定位理论研究也发现，对于固定的观测站而言，多站与目标之间形成的几何构型对跟踪定位的精度影响显著，扩展到运动的观测站，则需要对观测站的运动路径进行合理规划，才能获得高的跟踪定位精度。

双机协同是空战中常用的编队方式，双机组成的纯方位无源跟踪定位系统由于设备相对简单、技术相对成熟而应用广泛。目前关于双机协同纯方位无源跟踪定位系统跟踪定位精度分析的研究较多，而运用跟踪定位精度分析结论指导双机飞行路径规划的研究则鲜见报道。这实际上只解决了信息的获取问题而忽视了信息的运用问题，如果能够将跟踪定位精度分析结论指导双机飞行路径规划，形成信息获取与运用的闭环系统，无疑对提高双机协同无源跟踪定位效果具有重要的作用。

发明内容

为了克服现有技术中的缺陷，本发明提供一种纯方位双机协同目标跟踪定位路径规划方法，用以解决运动辐射源的跟踪定位问题，重点研究了利用扩展卡尔曼滤波器（ExtendedKalmanFilter，EKF）与目标定位精度的几何分布（GeometricalDilutionofPrecision，GDOP）建立优化指标函数对双机飞行路径规划以获得高精度的目标跟踪定位效果。其技术方案如下：

一种纯方位双机协同目标跟踪定位路径规划方法，包括观测过程和集中式处理过程：

所述观测过程主要完成双机对目标方位角和俯仰角的探测，给出观测向量同时提供双机的运动状态

其中x_i，y_i，z_i表示位置信息，

表示速度信息，符号“T”表示矩阵的转置运算，具体为：

双机组成的仅有角度的量测方程为

式中k＝0，1，2，…，N为离散化的时间序列，x_t，y_t，z_t表示目标的位置信息，W(k)为零均值高斯白噪声，其误差协方差阵为

为对角阵标号；

双机的运动方程给出如下

X_i(k)＝ΦX_i(k-1)+GU_i(k-1)(i＝1，2)     (2)

式中

为状态转移矩阵，

为控制矩阵，U_i=[a_ix，a_iy，a_iz]^T为载机i的控制向量，表示沿三个坐标轴方向的加速度，T为表示采样周期，I₃为3×3单位矩阵，O₃为3×3零矩阵；

所述集中式处理过程根据观测过程提供的信息主要完成指标函数的建立及路径规划的解算输出双机飞行控制策略及目标跟踪定位结果，采用基于EKF设计交互信息指标函数J_I与协方差控制指标函数J_C，利用指标函数J_I和J_C进行双机飞行路径的寻优；或者直接利用观测信息基于目标定位精度几何分布分析理论建立GDOP指标函数J_G进行双机飞行路径寻优。

进一步优选，所述集中式处理过程具体包括以下步骤：

步骤1建立基于EKF的路径规划指标函数；

从目标跟踪的角度，利用EKF流程中包含的新息更新关系建立交互信息指标函数与协方差控制指标函数；

步骤2建立基于GDOP的双机路径规划指标函数；

从目标定位的角度，利用双机协同目标定位的GDOP理论建立GDOP指标函数；

步骤3建立标准形式的双机协同路径规划模型；

根据约束条件和指标函数建立标准形式的双机协同路径规划模型；

步骤4纯方位双机协同路径规划算法。

进一步优选，所述的基于EKF的路径规划指标函数建立过程如下：

目标动态模型为

X(k)＝ΦX(k-1)+GV(k-1)     (3)

式中

为目标状态，x_t，y_t，z_t代表目标位置分量，为目标速度分量，矩阵Φ与G的定义与式（2）相同，V(k)为与W(k)相互独立的零均值高斯白噪声，其协方差阵为Q＝qI₃，q为过程噪声强度；

由状态方程式（3）和量测方程式（1）即可得到目标状态估计的EKF：

$\hat{X}\left(k\right|k-1)=\mathrm{\Φ}\hat{X}(k-1)---\left(4\right)$

P(k|k-1)＝ΦP(k-1)Φ^T+GQ(k)G^T     (5)

K(k)＝P(k|k-1)H^T(k)[H(k)P(k|k-1)H^T(k)+R(k)]^-1     (6)

$\hat{X}\left(k\right)=\hat{X}\left(k\right|k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\hat{X}\left(k\right|k-1)]---\left(7\right)$

P(k)＝[I₆-K(k)H(k)]P(k|k-1)     (8)

上式中

为目标状态的一步预测，P(k|k-1)为目标状态估计的一步预测误差协方差阵，为目标状态估计值，P(k)为目标状态估计的误差协方差阵，I₆为6×6单位矩阵，H(k)为量测方程在目标状态一步预测值的泰勒展开线性项，即

$H\left(X^{-}\right)=\frac{\∂h}{\∂X}{|}_{x=x}=\begin{array}{}\end{array}\left[\begin{array}{cccccc}-\frac{y_t^{-}-y_1}{{\left(r_{h1}^{-}\right)}^2}& \frac{x_t^{-}-x_1}{{\left(r_{h1}^{-}\right)}^2}& 0& 0& 0& 0\\ -\frac{(x_t^{-}-x_1)(z_t^{-}-z_1)}{r_{h1}^{-}{\left(r_1^{-}\right)}^2}& -\frac{(y_t^{-}-y_1)(z_t^{-}-z_1)}{r_{h1}^{-}{\left(r_1^{-}\right)}^2}& \frac{r_{h1}^{-}}{{\left(r_1^{-}\right)}^2}& 0& 0& 0\\ -\frac{y_t^{-}-y_2}{{\left(r_{h2}^{-}\right)}^2}& \frac{x_t^{-}-x_2}{{\left(r_{h2}^{-}\right)}^2}& 0& 0& 0& 0\\ -\frac{(x_t^{-}-x_2)(z_t^{-}-z_2)}{r_{h2}^{-}{\left(r_2^{-}\right)}^2}& -\frac{(y_t^{-}-y_2)(z_t^{-}-z_2)}{r_{h2}^{-}{\left(r_2^{-}\right)}^2}& \frac{r_{h2}^{-}}{{\left(r_2^{-}\right)}^2}& 0& 0& 0\end{array}\right]$

式（5）为目标状态一步预测误差协方差阵，可以将其看作在获取量测前目标的先验信息，（8）式为目标状态估计误差协方差阵，可以将其看作获取量测后目标的后验信息。先验信息与后验信息的差别称为交互信息，表示双机运动之后获取目标信息量的变化，建立交互信息指标函数：

$J_I={\log }_2\left(\frac{\det \left(P\left(k\right|k-1)\right)}{\det \left(P\left(k\right)\right)}\right)---\left(9\right)$

式中标号“det”为求行列式运算，“log”为取对数运算。

利用矩阵求逆定理将（8）式写成

P^-1(k)＝P^-1(k|k-1)+H^T(k)R^-1(k)H(k)     (10)

H^T(k)R^-1(k)H(k)同样反映了传感器在获得量测前后目标状态估计的误差协方差的不同，其实质也是一种信息收益，建立协方差指标函数：

J_C＝^tr[H^T(k)R^-1(k)H(k)]     (11)

式中标号“tr”表示矩阵求迹运算。

进一步优选，基于GDOP的路径规划指标函数建立过程如下：

首先给出纯方位双机协同目标定位模型：

根据定位模型可得纯方位双机协同目标定位的GDOP指标函数：

$J_G=\mathrm{GDOP}=\sqrt{{\mathrm{\σ}}_{x_t}^2+{\mathrm{\σ}}_{y_t}^2+{\mathrm{\σ}}_{z_t}^2}---\left(15\right)$

式中

其中

为双机自身位置误差的方差，

与（1）式中含义相同，

为对a关于b求偏导数。

进一步优选，标准形式的双机协同路径规划模型如下：

$\left\{\begin{array}{c}\max J_1\mathrm{or}\max J_C\mathrm{or}\min J_G\\ s.t.X\left(k\right)=\mathrm{\ΦX}(k-1)+\mathrm{GV}(k-1)\\ X_i\left(k\right)={\mathrm{\ΦX}}_i(k-1)+{\mathrm{GU}}_i(k-1)\\ U_i(k-1)\∈u(i=\mathrm{1,2})\end{array}\right.---\left(19\right)$

式中u为双机可用控制向量的集合。

进一步优选，所述纯方位双机协同路径规划算法包括以下步骤：

步骤1：初始化双机自身的位置误差方差

及被动传感器的测角误差方差

步骤2：利用双机被动传感器测得目标俯仰角与偏航角θ₁、θ₂、

步骤3：根据式（3）对目标状态进行一步预测

步骤4：假设双机的机动能力相同，以双机的最大加速度为左右边界给出均匀分布的制向量集u，在每一个决策步进行下列循环：

在控制向量集u中随机选取双机的控制向量U_i(temp)；

若是指标函数J_I与J_C则进行EKF步骤；若是指标函数J_G，则利用式（2）计算双机态；

利用（9）、（11）和（15）式计算指标值，存储每一控制向量所对应的指标值；

步骤5：选取满足指标函数的控制向量作为最终的控制向量U_i。

本发明的有益效果：

本发明充分利用了双机协同目标跟踪定位的结果，形成了传感器观测、优化指标建立及路径规划解算的闭环回路，通过建立基于EKF的交互信息及协防差控制指标函数于与基于GDOP的优化指标函数，给出了纯方位双机协同的路径优化模型，并给出了模型的数值求解算法，通过优化的双机飞行路径最终提高双机协同无源跟踪定位的精度。

附图说明

图1是双机协同路径规划方法总体框架；

图2是目标无机动时利用指标函数J_I、J_C、J_G规划的双机飞行路径；

图3是目标无机动时位置跟踪误差对比，图3a是X方向的跟踪误差，图3b是Y方向的跟踪误差,图3c是Z方向的跟踪误差；

图4是目标无机动时几何定位精度变化对比；

图5是目标机动时利用指标函数J_I、J_C、J_G规划的双机飞行路径；

图6是目标机动时位置跟踪误差对比图，6a是X方向的跟踪误差，图6b是Y方向的跟踪误差,图6c是Z方向的跟踪误差；

图7是目标机动时几何定位精度变化对比。

具体实施方式

下面结合附图和具体实施方式对本发明的技术方案作进一步详细地说明。

一种纯方位双机协同目标跟踪定位路径规划方法，包括以下步骤：

步骤1初始化双机及目标参数：

双机与目标的初始位置设置为：F₁(0,10km,0)、F₂(10km,0,0)、Target(100km,100km,2km)，双机与目标的初始速度为200m/s，双机在三个坐标轴方向的最大加速度为20m/s²，目标在三个坐标轴方向的最大加速度为40m/s²，目状态方程的过程噪声强度q＝0.05，观测方程的噪声强度

双机在三个坐标轴方向的位置误差方差均为100m，采样步长为1s。

步骤2计算双机被动传感器测得目标俯仰角与偏航角θ₁(k)、

θ₂(k)、

步骤3根据式（3）对目标状态进行一步预测

步骤4分别计算基于EKF与基于GDOP的指标函数。

在控制向量集u中随机选取双机控制向量，若是指标函数J_I与J_C则进行EKF步骤；若是指标函数J_G，则利用式（2）计算双机状态；利用（9）、（11）和（15）式计算指标值，存储每一控制向量所对应的指标值。

步骤5选取满足指标函数的控制向量作为最终的控制向量U_i。

图2为目标无机动时分别利用指标函数J_I、J_C、J_G进行规划的典型的双机飞行路径。图中“△”表示路径起点，“o”表示路径终点，F1、F2、Target分别表示载机1、载机2和目标。图2双机飞行轨迹显示利用J_I规划的双机飞行路径需要载机在飞行过程中频繁机动，这实际上反映了仅有角度测量探测系统可观测性的要求，但是这样将会消耗较多的能量；利用J_C规划的双机飞行轨迹虽然机动频率较低，但是双机在一段时间后与目标形成了尾追关系，这需要机载被动传感器具备后视能力；利用J_G规划的双机飞行轨迹比较平滑，且始终与目标保持相向飞行关系，被动传感器只需具备前视能力即可。

图3和图4分别是目标无机动时分别利用J_I、J_C、J_G进行规划得到的目标在三个坐标方向的位置跟踪误差曲线及目标GDOP变化曲线。目标不机动时，利用J_I、J_C、J_G三种指标函数规划的飞行路径能够实现双机对目标的跟踪，且跟踪效果总体相差不大。GDOP曲线表明，J_I、J_G规划的飞行路径能够实现目标定位误差的稳定降下降，而J_C规划的双机飞行路径由于存在飞行轨迹的交叉会导致定位误差异常增大。

图5为目标在z方向上做最大加速度为40m/s²机动时，分别利用指标函数J_I、J_C、J_G进行规划的典型的双机飞行路径。对比目标无机动时双机飞行路径可以发现，J_I、J_C对目标机动十分敏感，得到的双机飞行路径与目标无机动时相比变化较大。通过后面的算法性能分析可以发现，当目标机动后，基于EKF的路径规划算法已经失效。基于GDOP的双机路径规划算法对目标的机动适应性则较强，目标机动与无机动时的飞行路径变化不大。

图6和图7是目标机动时分别利用J_I、J_C、J_G进行规划得到的目标在三个坐标方向的位置跟踪误差曲线及目标GDOP变化曲线。从图6的目标跟踪误差曲线可以看出，由于目标在z轴方向出现了EKF事先无法描述的机动，使得基于J_I、J_C的双机路径规划算法失效，最终导致对目标z方向上的跟踪失败。相反，基于GDOP规划算法，由于能够避免对目标运动模型的依赖，直接从观测中获得信息对双机飞行路径进行规划，因而在目标机动时依然能够实现对目标的稳定跟踪。图7所示J_I、J_C所对应的GDOP变化曲线在后期都会增大，尤其J_C对应的GDOP值在后期发生了与目标无机动时相同的异常变化，且变化幅度更大。J_G对应的GDOP值稳定降低，反映了基于GDOP的双机路径规划算法较基于EKF的双机路径规划算法在应对机动目标时的有效性。

综上所述，所提双机协同被动探测路径规划方法能够实现对目标的有效跟踪定位，其中基于EKF的双机路径规划算法对目标模型的依赖性较高，规划过程中如果目标不做机动，则利用交互信息指标或协方差指标均能实现对目标的稳定跟踪，且跟踪效果基本相同；如果目标出现事先建立的模型无法描述的机动，则利用信息偏差指标与协方差指标均无法实现对目标的跟踪。基于GDOP的双机路径规划算法在目标无机动时跟踪定位效果较基于EKF的双机路径规划算法稍差，但在目标机动时依然能够实现对目标的稳定跟踪，且定位精度较高，表现出良好的适应性。

以上所述，仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。

Claims (6)

1.一种纯方位双机协同目标跟踪定位路径规划方法，其特征在于，包括观测过程和集中式处理过程：

所述观测过程主要完成双机对目标方位角和俯仰角的探测，给出观测向量

同时提供双机的运动状态

，其中x_i，y_i，z_i表示位置信息，表示速度信息，符号“T”表示矩阵的转置运算，具体为：

双机组成的仅有角度的量测方程为

(1)

式中k＝0，1，2，…，N为离散化的时间序列，x_t，y_t，z_t表示目标的位置信息，W(k)为零均值高斯白噪声，其误差协方差阵为

为对角阵标号；

双机的运动方程给出如下

X_i(k)＝ΦX_i(k-1)+GU_i(k-1)(i＝1，2)     (2)

式中

为状态转移矩阵，为控制矩阵，U_i=[a_ix，a_iy，a_iz]^T为载机i的控制向量，表示沿三个坐标轴方向的加速度，T为表示采样周期，I₃为3×3单位矩阵，O₃为3×3零矩阵；

所述集中式处理过程根据观测过程提供的信息主要完成指标函数的建立及路径规划的解算输出双机飞行控制策略及目标跟踪定位结果，采用基于EKF设计交互信息指标函数J_I与协方差控制指标函数J_C，利用指标函数J_I和J_C进行双机飞行路径的寻优；或者直接利用观测信息基于目标定位精度几何分布分析理论建立GDOP指标函数J_G进行双机飞行路径寻优。

2.根据权利要求1所述的纯方位双机协同目标跟踪定位路径规划方法，其特征在于，所述集中式处理过程具体包括以下步骤：

步骤1建立基于EKF的路径规划指标函数；

从目标跟踪的角度，利用EKF流程中包含的新息更新关系建立交互信息指标函数与协方差控制指标函数；

步骤2建立基于GDOP的双机路径规划指标函数；

从目标定位的角度，利用双机协同目标定位的GDOP理论建立GDOP指标函数；

步骤3建立标准形式的双机协同路径规划模型；

根据约束条件和指标函数建立标准形式的双机协同路径规划模型；

步骤4纯方位双机协同路径规划算法。

3.根据权利要求2所述的纯方位双机协同目标跟踪定位路径规划方法，其特征在于，所述的基于EKF的路径规划指标函数建立过程如下：

目标动态模型为

X(k)＝ΦX(k-1)+GV(k-1)     (3)

式中为目标状态，x_t，y_t，z_t代表目标位置分量，

为目标速度分量，矩阵Φ与G的定义与式（2）相同，V(k)为与W(k)相互独立的零均值高斯白噪声，其协方差阵为Q＝qI₃，q为过程噪声强度；

由状态方程式（3）和量测方程式（1）即可得到目标状态估计的EKF：

$\hat{X}\left(k\right|k-1)=\mathrm{\Φ}\hat{X}(k-1)---\left(4\right)$

P(k|k-1)＝ΦP(k-1)Φ^T+GQ(k)G^T                    (5)

K(k)＝P(k|k-1)H^T(k)[H(k)P(k|k-1)H^T(k)+R(k)]^-1     (6)

$\hat{X}\left(k\right)=\hat{X}\left(k\right|k-1)+K\left(k\right)[Z\left(k\right)-H\left(k\right)\hat{X}\left(k\right|k-1)]---\left(7\right)$

P(k)＝[I₆-K(k)H(k)]P(k|k-1)     (8)

上式中为目标状态的一步预测，P(k|k-1)为目标状态估计的一步预测误差协方差阵，

为目标状态估计值，P(k)为目标状态估计的误差协方差阵，I₆为6×6单位矩阵，H(k)为量测方程在目标状态一步预测值

的泰勒展开线性项，即

$H\left(X^{-}\right)=\frac{\∂h}{\∂X}{|}_{x=x}=\begin{array}{}\end{array}\left[\begin{array}{cccccc}-\frac{y_t^{-}-y_1}{{\left(r_{h1}^{-}\right)}^2}& \frac{x_t^{-}-x_1}{{\left(r_{h1}^{-}\right)}^2}& 0& 0& 0& 0\\ -\frac{(x_t^{-}-x_1)(z_t^{-}-z_1)}{r_{h1}^{-}{\left(r_1^{-}\right)}^2}& -\frac{(y_t^{-}-y_1)(z_t^{-}-z_1)}{r_{h1}^{-}{\left(r_1^{-}\right)}^2}& \frac{r_{h1}^{-}}{{\left(r_1^{-}\right)}^2}& 0& 0& 0\\ -\frac{y_t^{-}-y_2}{{\left(r_{h2}^{-}\right)}^2}& \frac{x_t^{-}-x_2}{{\left(r_{h2}^{-}\right)}^2}& 0& 0& 0& 0\\ -\frac{(x_t^{-}-x_2)(z_t^{-}-z_2)}{r_{h2}^{-}{\left(r_2^{-}\right)}^2}& -\frac{(y_t^{-}-y_2)(z_t^{-}-z_2)}{r_{h2}^{-}{\left(r_2^{-}\right)}^2}& \frac{r_{h2}^{-}}{{\left(r_2^{-}\right)}^2}& 0& 0& 0\end{array}\right]$

式中

式（5）为目标状态一步预测误差协方差阵，将其看作在获取量测前目标的先验信息，（8）式为目标状态估计误差协方差阵，将其看作获取量测后目标的后验信息，先验信息与后验信息的差别称为交互信息，表示双机运动之后获取目标信息量的变化，建立交互信息指标函数：

$J_I={\log }_2\left(\frac{\det \left(P\left(k\right|k-1)\right)}{\det \left(P\left(k\right)\right)}\right)---\left(9\right)$

式中标号“det”为求行列式运算，“log”为取对数运算；

利用矩阵求逆定理将（8）式写成

P^-1(k)＝P^-1(k|k-1)+H^T(k)R^-1(k)H(k)     (10)

H^T(k)R^-1(k)H(k)同样反映了传感器在获得量测前后目标状态估计的误差协方差的不同，其实质也是一种信息收益，建立协方差指标函数：

J_C＝tr[H^T(k)R^-1(k)H(k)]     (11)

式中标号“tr”表示矩阵求迹运算。

4.根据权利要求2所述的纯方位双机协同目标跟踪定位路径规划方法，其特征在于，基于GDOP的路径规划指标函数建立过程如下：

首先给出纯方位双机协同目标定位模型：

根据定位模型可得纯方位双机协同目标定位的GDOP指标函数：

$J_G=\mathrm{GDOP}=\sqrt{{\mathrm{\σ}}_{x_t}^2+{\mathrm{\σ}}_{y_t}^2+{\mathrm{\σ}}_{z_t}^2}---\left(15\right)$

式中

其中

为双机自身位置误差的方差，

与（1）式中含义相同，

为对a关于b求偏导数。

5.根据权利要求2所述的纯方位双机协同目标跟踪定位路径规划方法，其特征在于，标准形式的双机协同路径规划模型如下：

$\left\{\begin{array}{c}\max J_1\mathrm{or}\max J_C\mathrm{or}\min J_G\\ s.t.X\left(k\right)=\mathrm{\ΦX}(k-1)+\mathrm{GV}(k-1)\\ X_i\left(k\right)={\mathrm{\ΦX}}_i(k-1)+{\mathrm{GU}}_i(k-1)\\ U_i(k-1)\∈u(i=\mathrm{1,2})\end{array}\right.---\left(19\right)$

式中u为双机可用控制向量的集合。

6.根据权利要求2所述的纯方位双机协同目标跟踪定位路径规划方法，其特征在于，所述纯方位双机协同路径规划算法包括以下步骤：

步骤1：初始化双机自身的位置误差方差

及被动传感器的测角误差方差

步骤2：利用双机被动传感器测得目标俯仰角与偏航角θ₁、

θ₂、

步骤3：根据式（3）对目标状态进行一步预测

步骤4：假设双机的机动能力相同，以双机的最大加速度为左右边界给出均匀分布的控制向量集u，在每一个决策步进行下列循环：

在控制向量集u中随机选取双机的控制向量U_i(temp)；

若是指标函数J_I与J_C则进行EKF步骤；若是指标函数J_G，则利用式（2）计算双机状态；

利用（9）、（11）和（15）式计算指标值，存储每一控制向量所对应的指标值；

步骤5：选取满足指标函数的控制向量作为最终的控制向量U_i。

Images