1.本发明涉及无人机技术领域，尤其涉及一种多无人机协同目标检测与跟踪装置和方法。
背景技术：
2.目标跟踪是无人机研究中的一项关键技术。无人机平台通常使用机载相机采集图像，根据图像信息对目标进行识别和定位，进而引导无人机实现对目标的跟踪。然而，由于无人机应用场景的复杂性，且无人机与目标之间存在相对运动，在跟踪过程中可能会出现丢失目标的情况，从而导致任务失败。
3.现有的多无人机协同目标检测与跟踪方法仅对跟踪目标的三维位置进行估计，但未考虑最小化多架无人机联合估计跟踪目标三维位置的不确定度。而且现有方法通常未考虑在协同目标检测与跟踪的过程中编队避免碰撞的问题，因此无法保证多无人机协同检测与跟踪运动规划在实际应用过程中的安全性。
4.现有的基于深度学习的多无人机协同目标检测与跟踪算法主要有以下两个缺点：1)每架无人机上的运动规划器没有为目标三维位置估计的不确定度最小化提供便利；2)多架无人机使用模型预测控制器mpc(model predictive control)生成的局部运动规划随后利用势场进行修改，以避免碰撞。这将进一步导致生成次优轨迹，并且使无人机容易陷入局部最小值。
技术实现要素：
5.鉴于上述的分析，本发明实施例旨在提供一种多无人机协同目标检测与跟踪装置和方法，用以解决现有方法未考虑在协同目标检测与跟踪的过程中编队避免碰撞并且现有次优轨迹使无人机容易陷入局部最小值的问题。
6.一方面，本发明实施例提供了一种多无人机协同目标检测与跟踪装置，包括：自位姿估计模块，用于通过第一卡尔曼滤波器ekf根据当前时刻和先前时刻的当前无人机位姿预测后续时刻的当前无人机位姿；协同检测与跟踪模块，用于通过融合当前无人机的目标检测测量值和来自于其他无人机的目标检测测量值获取后续时刻的跟踪目标位姿并基于所述后续时刻的当前无人机位姿校正当前无人机偏差；基于模型预测控制mpc的编队控制模块，用于根据所述跟踪目标位姿、其它无人机位姿障碍物位置和其他无人机轨迹生成当前无人机的路径点指令；以及控制模块，用于根据当前无人机的路径点指令调整所述当前无人机的位置和偏航角，以生成所述当前时刻的当前无人机位姿。
7.上述技术方案的有益效果如下：利用本发明的多无人机协同目标检测与跟踪装置，相机视场覆盖面积更大，成像视角更多，目标的丢失率降低；由于融合了多架无人机的目标定位数据，提高了对目标的定位精度和跟踪效果；整个系统的冗余性提高，随着机间协同作用的增强，单架无人机故障对任务的影响降低。
8.基于上述装置的进一步改进，所述协同检测与跟踪模块包括基于dcnn的目标检测
器和基于ekf的目标跟踪器，其中，所述基于dcnn的目标检测器，用于基于先前时刻t-1的感兴趣区域和当前时刻t的输入图像进行目标检测，以获取所述当前无人机的目标检测测量值；所述基于ekf的目标跟踪器，用于利用第二扩展卡尔曼滤波器基于所述当前无人机的目标检测测量值和来自于其他无人机的目标检测测量值预测所述后续时刻的跟踪目标位姿。
9.基于上述装置的进一步改进，所述协同检测与跟踪模块还用于基于所述当前时刻t的输入图像和先前存储的跟踪目标位姿预测所述当前时刻t的输入图像的感兴趣区域roi，其中，所述感兴趣区域roi引导所述当前无人机调整偏航角以确保跟踪目标在所述当前无人机的视场内；以及所述感兴趣区域roi还用于对所述当前无人机位置进行偏差校正。
10.基于上述装置的进一步改进，所述基于模型预测控制mpc的编队控制模块还用于对以下第k架无人机的测量不确定度协方差矩阵的迹进行最小化以确保每架无人机减小测量不确定度：
[0011][0012]
其中，是第k架无人机对于所述跟踪目标的相对位置函数，k∈[1,
…
,k]，为第k架无人机在t时刻在相机坐标系下对跟踪目标p的位置的观测值的平均距离，当时，第k架无人机会与跟踪目标或其它无人机发生碰撞，对上式的最小化等价于最小化小化其中为第k架无人机的三维位置，为第k架无人机的期望三维位置，i3为3
×
3的单位阵，c1、c2、c3均为常数，λ＝c1 c2 c3。
[0013]
基于上述装置的进一步改进，所述基于模型预测控制mpc的编队控制模块还用于通过以下配置来最小化多无人机编队的联合不确定度：相对跟踪目标的位置，多架无人机之间的角度为2π/k；最小化每架无人机的测量不确定度，其中，所述多无人机编队的联合不确定度与每架无人机独立的测量不确定度协方差矩阵有关。
[0014]
基于上述装置的进一步改进，基于模型预测控制mpc的编队控制模块还用于设置每架无人机上的模型预测控制mpc的目标以避免当前无人机与其他无人机或所述跟踪目标发生碰撞包括：最小化所述跟踪目标到期望安全表面的距离，其中，所述期望安全表面以所述跟踪目标为中心并具有期望半径和期望高度；最小化无人机的速度与联合估计的跟踪目标速度之间的偏差；以及最小化无人机控制力矩。
[0015]
基于上述装置的进一步改进，所述基于模型预测控制mpc的编队控制模块还用于构造以下目标函数jact：
[0016][0017]
其中，优化所述目标
函数j
act
中的第一部分最小化输入控制信号，第二部分确保期望最终状态与最终无人机状态之间的距离最小化，we和为自定义的对角半正定权重矩阵；基于模型预测控制mpc的约束条件：无人机之间的角度保持期望的2π/k的非凸约束；与其它无人机及环境中的障碍物保持最小距离以防止碰撞的非凸约束，其中，将非凸约束以排斥势场力的形式融入到所述模型预测控制mpc中，使得通过以下公式计算凸模型预测控制mpc：
[0018][0019][0020][0021][0022]
其中，表示第k架无人机的速度，表示第k架无人机的输入加速度，n表示时间步，为目标函数j
act
最小时的最优解，argmin函数表示使目标函数j
act
取最小值时的变量值，g为一个很小的常量动态矩阵，第k架无人机的三维运动部分取决于加速度部分取决于总外部控制输入动态矩阵a和控制转移矩阵b定义如下：
[0023][0024]
其中δt为采样时间。
[0025]
基于上述装置的进一步改进，通过以下公式计算作用于第k架无人机的总外部控制输入
[0026][0027]
其中，f
max
为最大势场幅度，通过以下公式计算第k架无人机相对于其它无人机的总体主动跟踪外部控制输入：
[0028][0029]
其中，在与接近所述跟踪目标的方向垂直的方向上作用的势场力，通过以下公式计算所述第k架无人机与所述无人机集群中的第j架无
人机之间的势场力：
[0030][0031]
其中r
des
为无人机与跟踪目标之间的期望距离，c为小的正常数，如果未达到期望角度差，确保力的大小不为0，绝对角度差角度差，确保力的大小不为0，绝对角度差作为参数用于计算势场的幅度，为第k架无人机与世界坐标系下的跟踪目标之间的角度，为与相邻第j架无人机之间的角度；通过以下公式计算避障总外部控制输入
[0032][0033]
其中，为所述总体动态避障外部控制输入，为总体静态避障外部控制输入和为当接近跟踪目标的路线上存在障碍物时对无人机角度进行惩罚的力；通过以下公式计算所述总体动态避障外部控制输入
[0034][0035]
其中，f
k,j
(d)为第k架无人机与所述无人机集群中的第j架无人机之间的势场力，参数d
dyn
(n)为在动态避障的情况下第k架无人机与所述无人机集群中第j架无人机之间的欧几里得距离，通过以下公式计算对第m个静态障碍物的总体静态避障外部控制输入：
[0036][0037]
其中，f
k,m
为第k架无人机与第m个障碍物之间的势场力，d
sta
(n)为第k架无人机与第m个障碍物之间的欧几里得距离第m个障碍物之间的欧几里得距离β
k,m
为势场力作用的角度，
[0038]
另一方面，本发明实施例提供了一种多无人机协同目标检测与跟踪方法，包括：通过第一扩展卡尔曼滤波器ekf根据当前时刻和先前时刻的当前无人机位姿预测后续时刻的当前无人机位姿；通过融合当前无人机的目标检测测量值和来自于其他无人机的目标检测测量值获取后续时刻的跟踪目标位姿并基于所述后续时刻的当前无人机位姿校正当前无人机偏差；根据所述跟踪目标位姿、其它无人机位姿障碍物位置和其他无人机轨迹生成当前无人机的路径点指令；以及根据当前无人机的路径点指令调整所述当前无人机的位置和偏航角，以生成所述当前时刻的当前无人机位姿。
[0039]
基于上述方法的进一步改进，通过融合所述后续时刻的当前无人机位姿和来自于其他无人机的目标检测测量值获取跟踪目标位姿包括：基于先前时刻t-1的感兴趣区域
和当前时刻t的输入图像进行目标检测，以获取所述当前无人机的目标检测测量值；利用第二扩展卡尔曼滤波器基于所述当前无人机的目标检测测量值和来自于其他无人机的目标检测测量值预测所述后续时刻的跟踪目标位姿。
[0040]
本发明基于模型预测控制器提出一种去中心化编队控制模块，该控制器在多无人机协同检测与跟踪目标的同时主动最小化被跟踪目标三维位置估计的联合不确定度。与现有技术相比，本发明至少可实现如下有益效果之一：
[0041]
1、利用本发明的多无人机协同检测与跟踪装置和方法，相机视场覆盖面积更大，成像视角更多，目标的丢失率降低；由于融合了多架无人机的目标定位数据，提高了对目标的定位精度和跟踪效果；整个系统的冗余性提高，随着机间协同作用的增强，单架无人机故障对任务的影响降低。
[0042]
2、为了在多无人机协同检测与跟踪目标的过程中，无人机能够主动计算最优的局部运动规划，获取最优视角配置，最小化被跟踪目标三维位置估计的联合不确定度。本发明提出一种局部凸模型预测控制器，将联合联合不确定度最小化解耦为：1)与被跟踪目标保持阈值距离的凸二次多项式；2)约束了无人机相对于被跟踪目标的角度配置。
[0043]
3、为了保证多无人机协同检测与跟踪运动规划的安全性，本发明分别相对于其它无人机、跟踪目标、以及静态障碍物增加了防止碰撞的局部凸约束。防止碰撞的约束以及角度配置的约束本质上是非凸的。在本发明中，将上述约束转换为外部控制输入动态嵌入到模型预测控制器中以保留编队模型预测控制器的凸性。利用本发明提出的算法，多架无人机能够实现在存在静态和动态障碍物的情况下联合检测与跟踪移动目标。
[0044]
本发明中，上述各技术方案之间还可以相互组合，以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述，并且，部分优点可从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。
附图说明
[0045]
附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。
[0046]
图1为根据本发明实施例的多无人机协同目标检测与跟踪装置的框图；
[0047]
图2为根据本发明实施例的多无人机协同目标检测与跟踪系统的框图；
[0048]
图3为根据本发明实施例的协同检测与跟踪模块的框图；
[0049]
图4为根据本发明实施例的多无人机协同目标检测与跟踪的示图；
[0050]
图5为根据本发明实施例的协同检测与跟踪模块的示意图；
[0051]
图6a、图6b和图6c为根据本发明实施例的多无人机协同目标跟踪角度配置的示图，其中，图6a和图6b中的配置没有最小化联合不确定度，以及图6c中的配置最小化了联合不确定度并确保了对跟踪目标的最大视场覆盖；
[0052]
图7为根据本发明实施例的多无人机协同目标检测与跟踪方法的流程图。
具体实施方式
[0053]
下面结合附图来具体描述本发明的优选实施例，其中，附图构成本技术一部分，并
与本发明的实施例一起用于阐释本发明的原理，并非用于限定本发明的范围。
[0054]
参考图1，本发明的一个具体实施例，公开了一种多无人机协同目标检测与跟踪装置，包括：自位姿估计模块102，用于通过第一扩展卡尔曼滤波器ekf根据当前时刻和先前时刻的当前无人机位姿预测后续时刻的当前无人机位姿；协同检测与跟踪模块104，用于通过融合当前无人机的目标检测测量值和来自于其他无人机的目标检测测量值获取后续时刻的跟踪目标位姿并基于后续时刻的当前无人机位姿校正当前无人机偏差；基于模型预测控制mpc的编队控制模块106，用于根据跟踪目标位姿、其它无人机位姿障碍物位置和其他无人机轨迹生成当前无人机的路径点指令；以及控制模块108，用于根据当前无人机的路径点指令调整当前无人机的位置和偏航角，以生成当前时刻的当前无人机位姿。
[0055]
与现有技术相比，本实施例提供的多无人机协同目标检测与跟踪装置，相机视场覆盖面积更大，成像视角更多，目标的丢失率降低；由于融合了多架无人机的目标定位数据，提高了对目标的定位精度和跟踪效果；整个系统的冗余性提高，随着机间协同作用的增强，单架无人机故障对任务的影响降低。
[0056]
下文中，参考图1，对根据本发明实施例的多无人机协同目标检测与跟踪装置进行详细说明。多无人机协同目标检测与跟踪装置包括：自位姿估计模块102、协同检测与跟踪模块104、基于模型预测控制mpc的编队控制模块106和控制模块108。
[0057]
自位姿估计模块102用于通过第一扩展卡尔曼滤波器ekf根据当前时刻和先前时刻的当前无人机位姿预测后续时刻的当前无人机位姿。
[0058]
具体地，第k(k∈k)架无人机在t时刻在世界坐标系下的位姿可表示如下：
[0059][0060]
其中，表示第k架无人机在t时刻在世界坐标系下的三维位置坐标，表示第k架无人机在t时刻在世界坐标系下的方位角，分别代表第k架无人机在t时刻在世界坐标系下与z、y、x轴的夹角。
[0061]
协同检测与跟踪模块104，用于通过融合当前无人机的目标检测测量值和来自于其他无人机的目标检测测量值获取后续时刻的跟踪目标位姿并基于后续时刻的当前无人机位姿校正当前无人机偏差。协同检测与跟踪模块包括基于dcnn的目标检测器和基于ekf的目标跟踪器，其中，基于dcnn的目标检测器，用于基于先前时刻t-1的感兴趣区域和当前时刻t的输入图像进行目标检测，以获取当前无人机的目标检测测量值；基于ekf的目标跟踪器用于利用第二扩展卡尔曼滤波器基于当前无人机的目标检测测量值和来自于其他无人机的目标检测测量值预测后续时刻的跟踪目标位姿。
[0062]
协同检测与跟踪模块还用于基于当前时刻t的输入图像和先前存储的跟踪目标位姿预测当前时刻t的输入图像的感兴趣区域roi，其中，感兴趣区域roi引导当前无人机调整偏航角以确保跟踪目标在当前无人机的视场内；以及感兴趣区域roi还用于对当前无人机位置进行偏差校正。
[0063]
基于模型预测控制mpc的编队控制模块106用于根据跟踪目标位姿、其它无人机位姿障碍物位置和其他无人机轨迹生成当前无人机的路径点指令。
[0064]
基于模型预测控制mpc的编队控制模块还用于对以下第k架无人机的测量不确定度协方差矩阵的迹进行最小化以确保每架无人机减小测量不确定度：
[0065][0066]
其中，是第k架无人机对于所述跟踪目标的相对位置函数，k∈[1,
…
,k]，为第k架无人机在t时刻在相机坐标系下对跟踪目标p的位置的观测值的平均距离，当时，第k架无人机会与跟踪目标或其它无人机发生碰撞，对上式的最小化等价于最小化小化其中为第k架无人机的三维位置，为第k架无人机的期望三维位置，i3为3
×
3的单位阵，c1、c2、c3均为常数，λ＝c1 c2 c3。
[0067]
基于模型预测控制mpc的编队控制模块还用于通过以下配置来最小化多无人机编队的联合不确定度：相对跟踪目标的位置，多架无人机之间的角度为2π/k；最小化每架无人机的测量不确定度，其中，多无人机编队的联合不确定度与每架无人机独立的测量不确定度协方差矩阵有关。
[0068]
基于模型预测控制mpc的编队控制模块还用于设置每架无人机上的模型预测控制mpc的目标以避免当前无人机与其他无人机或跟踪目标发生碰撞包括：最小化跟踪目标到期望安全表面的距离，其中，期望安全表面以跟踪目标为中心并具有期望半径和期望高度；最小化无人机的速度与联合估计的跟踪目标速度之间的偏差；以及最小化无人机控制力矩。
[0069]
基于模型预测控制mpc的编队控制模块还用于构造以下目标函数j
act
：
[0070][0071]
其中，优化目标函数j
act
中的第一部分最小化输入控制信号，第二部分确保期望最终状态与最终无人机状态之间的距离最小化，we和为自定义的对角半正定权重矩阵；
[0072]
基于模型预测控制mpc的约束条件：无人机之间的角度保持期望的2π/k的非凸约束；与其它无人机及环境中的障碍物保持最小距离以防止碰撞的非凸约束，其中，将非凸约束以排斥势场力的形式融入到模型预测控制mpc中，使得通过以下公式计算凸模型预测控制mpc：
[0073][0074][0075][0076]
[0077]
其中，表示第k架无人机的速度，表示第k架无人机的输入加速度，n表示时间步，为目标函数j
act
最小时的最优解，第k架无人机的三维运动部分取决于加速度机的三维运动部分取决于加速度部分取决于总外部控制输入g为非常小的常数，以防止的值为零，动态矩阵a和控制转移矩阵b定义如下：
[0078][0079]
其中δt为采样时间。
[0080]
通过以下公式计算作用于第k架无人机的总外部控制输入
[0081][0082]
其中，f
max
为最大势场幅度，
[0083]
通过以下公式计算第k架无人机相对于其它无人机的总体主动跟踪外部控制输入：
[0084][0085]
其中，在与接近跟踪目标的方向垂直的方向上作用的势场力，通过以下公式计算第k架无人机与无人机集群中的第j架无人机之间的势场力：
[0086][0087]
其中r
des
为无人机与跟踪目标之间的期望距离，c为小的正常数，如果未达到期望角度差，确保力的大小不为0，绝对角度差角度差，确保力的大小不为0，绝对角度差作为参数用于计算势场的幅度，为第k架无人机与世界坐标系下的跟踪目标之间的角度，为与相邻第j架无人机之间的角度；
[0088]
通过以下公式计算避障总外部控制输入
[0089][0090]
其中，为总体动态避障外部控制输入，为总体静态避障外部控制输入和为当接近跟踪目标的路线上存在障碍物时对无人机角度进行惩罚的力；
[0091]
通过以下公式计算总体动态避障外部控制输入
[0092][0093]
其中，f
k,j
(d)为第k架无人机与所述无人机集群中的第j架无人机之间的势场力，参数d
dyn
(n)为在动态避障的情况下第k架无人机与所述无人机集群中第j架无人机之间的欧几里得距离，
[0094]
通过以下公式计算对第m个静态障碍物的总体静态避障外部控制输入：
[0095][0096]
其中，其中，f
k,m
为第k架无人机与第m个障碍物之间的势场力，d
sta
(n)为第k架无人机与第m个障碍物之间的欧几里得距离机与第m个障碍物之间的欧几里得距离β
k,m
为势场力作用的角度，
[0097]
控制模块108用于根据当前无人机的路径点指令调整当前无人机的位置和偏航角，以生成当前时刻的当前无人机位姿。
[0098]
参考图7，本发明的另一个具体实施例，公开了一种多无人机协同目标检测与跟踪方法，包括：在步骤s701中，通过第一扩展卡尔曼滤波器ekf根据当前时刻和先前时刻的当前无人机位姿预测后续时刻的当前无人机位姿；在步骤s702中，通过融合当前无人机的目标检测测量值和来自于其他无人机的目标检测测量值获取后续时刻的跟踪目标位姿并基于后续时刻的当前无人机位姿校正当前无人机偏差；在步骤s703中，根据跟踪目标位姿、其它无人机位姿障碍物位置和其他无人机轨迹生成当前无人机的路径点指令；以及在步骤s704中，根据当前无人机的路径点指令调整当前无人机的位置和偏航角，以生成当前时刻的当前无人机位姿。
[0099]
具体地，通过融合后续时刻的当前无人机位姿和来自于其他无人机的目标检测测量值获取跟踪目标位姿包括：基于先前时刻t-1的感兴趣区域和当前时刻t的输入图像进行目标检测，以获取当前无人机的目标检测测量值；利用第二扩展卡尔曼滤波器基于当前无人机的目标检测测量值和来自于其他无人机的目标检测测量值预测后续时刻的跟踪目标位姿。
[0100]
下文中，参考图2至图6c，以具体实例的方式对根据本发明实施例的多无人机协同目标检测与跟踪装置进行详细说明。
[0101]
多无人机协同目标检测与跟踪系统如图2所示，其中协同检测与跟踪模块(如图3所示)、去中心化的基于mpc的编队控制模块为本项目研究的重点。在协同检测与跟踪模块中，每架无人机利用ekf(extended kalman filter，即扩展卡尔曼滤波器)融合自身以及从其它无人机获取的目标检测测量值。每架无人机估计目标状态用于在未来的图像帧中预测
感兴趣区域(roi，region of interest)。roi引导每架无人机调整偏航角以确保跟踪目标在无人机的视场内。利用对目标的状态估计驱动去中心化的基于mpc的编队控制模块。在每个时间步，每架无人机的控制器根据估计的跟踪目标状态、与其它无人机通讯得出的运动规划、障碍物位置生成自身路径点位置及速度。生成的路径点位置引导每架无人机的底层位置和偏航控制器，实现多无人机持续协同目标跟踪。
[0102]
具体实现过程如下：
[0103]
(1)基本定义与问题陈述
[0104]
多无人机对目标p进行协同检测与跟踪如图4所示。
[0105]
第k(k∈k)架无人机在t时刻在世界坐标系下的位姿可表示如下：
[0106][0107]
其中，表示第k架无人机在t时刻在世界坐标系下的三维位置坐标，表示第k架无人机在t时刻在世界坐标系下的方位角，具体的，分别代表第k架无人机在t时刻在世界坐标系下与z、y、x轴的夹角。
[0108]
第k架无人机在t时刻的位姿不确定性协方差矩阵定义为
[0109]
第k架无人机在t时刻在相机坐标系下对目标p的位置的观测值由球坐标系下的平均距离方位角倾斜角构成，如图5所示，对应的测量噪声定义为在相机坐标系下，上述测量在笛卡尔坐标系下分别定义为融合估计的目标p的位置和不确定度协方差在世界坐标系下分别定义为和上述估计值通过融合第k架无人机的测量值与接收到的其余所有无人机的测量值进行计算。
[0110]
多无人机在具有m个已知静态障碍物和k-1个相邻无人机(动态障碍物)的环境下作业。多无人机协同跟踪的编队控制算法的目标包括：1)最小化多无人机融合估计的测量不确定度协方差2)避开m k-1个障碍物。
[0111]
(2)基于dcnn的协同目标检测与跟踪
[0112]
协同目标检测与跟踪算法基于扩展卡尔曼滤波(ekf)，输入为在前一时间步t-1时刻估计跟踪目标的3d位置该估计的不确定度协方差矩阵感兴趣区域在t时刻的输入图像及时间t。
[0113]
基于dcnn(deep convolutional neural networks，称为深度卷积神经网络)的目标检测器利用前一时间步t-1时刻的感兴趣区域以及t时刻的输入图像进行目标检测。对目标的测量在世界坐标系下分别定义为均值和噪声协方差矩阵原始图像目标检测测量结果由目标框、置信度分数以及噪声协方差矩阵组成。原始目标检测测量结果先变换到相机坐标系，然后变换到世界坐标系中。上述变换包含了原始目标检测测量的噪声协方差和无人机自位姿估计不确定度协方差。原始目标检测测量在2d图像平面中，最终的目标检测测量在3d世界坐标系中计算。因此，需要对跟踪目标的高度进行假设。假设
跟踪目标的高度服从正态分布在无人机之间传输数据，包括世界坐标系下的无人机的自位姿估计以及目标检测测量结果。利用ekf对目标检测测量结果进行预测。由于所有无人机的测量结果均在世界坐标系下，可通过对每架无人机的测量结果进行ekf更新实现测量结果的融合。主动选择roi以确保后续的检测均在图像中信息最丰富的区域进行。roi的计算方法如下：利用当前预测的目标检测测量结果(和)，通过ekf(扩展卡尔曼滤波)预测模型预测下一时间步t 1时刻的目标状态对t时刻的感兴趣区域进行计算。最后对每架无人机的自位姿估计进行偏差校正。每架无人机的自位姿估计通过利用gps、气压计、imu传感器获取，通常存在时变偏差。每架无人机的自位姿估计的偏差会导致在世界坐标系下融合目标检测结果的误匹配。在协同目标检测与跟踪算法中，对目标进行跟踪并补偿每架无人机自位姿估计的偏差。利用每架无人机自身的目标检测测量结果与融合后的跟踪估计结果之间的差异来更新偏差。
[0114]
(3)测量模型与联合不确定度最小化
[0115]
①
测量模型与假设
[0116]
目标检测测量噪声方差包括：距离噪声方差方位角噪声方差倾斜角方差将目标检测测量噪声从球坐标系转换到笛卡尔坐标系中。对于无人机的观测模型可以做出如下假设：利用偏航控制器引导各无人机的偏航角朝向跟踪目标，确保每架无人机的相机始终朝向跟踪目标，因此可以假设方位角由于多无人机协同跟踪目标的编队控制要求各无人机与目标保持期望的高度和距离，因此可以假设倾斜角经假设后，测量噪声不确定度协方差矩阵中的元素可表示如下：
[0117][0118][0119]
其中c
x
,cy,cz为通过仅包含c1,c2,c3的指数函数计算得到的常数项。
[0120]
②
最小化多无人机编队的联合不确定度
[0121]
最小化多无人机协同跟踪目标的联合状态估计不确定度需要满足如下条件：
[0122]
a)相对估计目标的位置，多架无人机之间的角度为2π/k
[0123]
对于k(≥3)个独立测量，能够令联合不确定度最小的传感器之间的角度为2π/k。图6a和图6b中的配置没有最小化联合不确定度图6c中的配置最小化了联合不确定度并确保了对跟踪目标的最大视场覆盖。
[0124]
b)最小化每架无人机的测量不确定度
[0125]
角度约束条件最小化了联合不确定度。然而，联合不确定度还与每架无人机独立的测量不确定度有关。是第k架无人机对于跟踪目标的相对位置函数。对于整个无人机编队而言，在不改变角度配置的情况下每架无人机的位置仍然可控。
因此，完整的联合不确定度最小化需要最小化每架无人机的测量不确定度的迹。
[0126]
(4)去中心化模型预测控制
[0127]
第k架无人机的测量不确定度协方差矩阵的迹(矩阵的迹指矩阵主对角线所有元素的和)可表示如下：
[0128][0129]
对上式进行最小化确保每架无人机减小测量不确定度。当时，无人机会与跟踪目标或其它无人机发生碰撞。因此，我们需要限制无人机到达一个期望安全表面。期望安全表面以跟踪目标为中心，半径为d
des
，高度为h
des
。对上式的最小化可等价于最小化其中为第k架无人机的三维位置，为第k架无人机的期望三维位置，i3为3
×
3的单位阵。在每架无人机上的模型预测控制的目标为最小化跟踪目标到安全表面的距离；无人机的速度与联合估计的跟踪目标速度之间的差距；无人机控制力矩。因此，主动跟踪模型预测控制(mpc)的优化目标函数可计算如下：
[0130][0131]
其中，优化目标j
act
中的第一部分最小化输入控制信号，第二部分确保期望最终状态中的第一部分最小化输入控制信号，第二部分确保期望最终状态与最终无人机状态之间的距离最小化。另外，主动mpc还需要满足以下约束：无人机之间的角度保持期望的2π/k的非凸约束；与其它障碍物之间保持至少d
min
距离的非凸约束。非凸约束以排斥势场力的形式融入到mpc中。凸mpc的计算可由如下公式定义：
[0132][0133][0134][0135][0136]
其中，表示第k架无人机的速度，表示第k架无人机的输入加速度，n表示时间步。第k架无人机的三维运动部分取决于加速度步。第k架无人机的三维运动部分取决于加速度部分取决于外部控制输入上式中的动态矩阵a和控制转移矩阵b定义如下：
[0137][0138]
其中δt为采样时间。
[0139]
(5)外部控制输入计算
[0140]
将无人机之间的角度配置、与其它无人机及环境中的障碍物保持最小距离以防止碰撞等非凸约束利用排斥势场转换成外部控制输入排斥势场函数表示为余切场函数。余切场力的大小表示为双曲函数f
k,j
(d)。参数d为欧几里得距离或第k架无人机与第j个实体(其它无人机或障碍物)之间的绝对角度差。d
max
表示势场的影响区域，d
min
表示在这个距离以内势场值趋于无穷大。
[0141]
①
主动跟踪输入
[0142]
为了满足多无人机之间的角度配置约束，需要计算第k架无人机与世界坐标系下的跟踪目标之间的角度与相邻第j架无人机之间的角度绝对角度差作为参数用于计算势场的幅度。第k架无人机与集群中的第j架无人机之间的势场力可计算如下：
[0143][0144]
其中r
des
为无人机与目标之间的期望距离，c为一个小的正常数，如果未达到期望角度差，可确保力的大小不为0。
[0145]
在与接近跟踪目标的方向垂直的方向上作用，上作用，第k架无人机相对于其它无人机的总体主动跟踪外部控制输入可计算如下：
[0146][0147]
对于主动跟踪控制输入，d
max
＝2π/k。
[0148]
②
避障输入
[0149]
a)动态避障
[0150]
集群中的其它无人机对于第k架无人机而言均可视为动态障碍物。f
k,j
(d)中的参数d在动态避障的情况下为第k架无人机与集群中第j架无人机之间的欧几里得距离，定义为总体动态避障外部输入可计算如下：
[0151][0152]
其中，
[0153]
b)静态避障
[0154]
对m个静态障碍物的外部控制输入可计算如下：
[0155]
[0156]
其中如果静态障碍物之间距离很近，无人机可能陷入场局部最小。为了避免上述情况，计算当接近跟踪目标的路线上存在障碍物时对无人机角度进行惩罚的力
[0157]
综上所述，避障总外部控制输入可计算如下：
[0158][0159]
因此，作用于第k架无人机的总外部控制输入可表示如下：
[0160][0161]
其中，f
max
为最大势场幅度。
[0162]
c)避障保证
[0163]
为了确保稳定的外部控制输入和保障避障，需考虑以下因素：最大跟踪误差e
max
，无人机速度最大幅度无人机的自位姿估计不确定度集群中其它无人机的自位姿估计不确定度无人机之间通讯或丢包。用于避障的外部控制输入的d
min
和d
max
可定义如下：
[0164][0165]
其中，是的最大奇异值，是的最大奇异值，随无人机之间通信延迟成比例地增大。
[0166]
与其他同类方法相比，本本技术实施例的特色如下：
①
相机视场覆盖面积更大，成像视角更多，目标丢失率减低；
②
融合多无人机的目标定位数据，提高目标定位精度和跟踪效果；
③
系统冗余性提高，随着机间协同作用的增强，单台无人机故障对任务的影响降低。
[0167]
本发明基于模型预测控制器提出一种去中心化编队控制器，该控制器在多无人机协同检测与跟踪目标的同时主动最小化被跟踪目标三维位置估计的联合不确定度。本方案的主要特色如下：
[0168]
1、为了在多无人机协同检测与跟踪目标的过程中，无人机能够主动计算最优的局部运动规划，获取最优视角配置，最小化被跟踪目标三维位置估计的联合不确定度。本发明提出一种局部凸模型预测控制器，将联合联合不确定度最小化解耦为：(1)与被跟踪目标保持阈值距离的凸二次多项式；(2)约束了无人机相对于被跟踪目标的角度配置。利用本发明的多无人机协同检测与跟踪算法，相机视场覆盖面积更大，成像视角更多，目标的丢失率降低；由于融合了多架无人机的目标定位数据，提高了对目标的定位精度和跟踪效果；整个系统的冗余性提高，随着机间协同作用的增强，单架无人机故障对任务的影响降低。
[0169]
2、为了保证多无人机协同检测与跟踪运动规划的安全性，本发明分别相对于其它无人机、跟踪目标、以及静态障碍物增加了防止碰撞的局部凸约束。防止碰撞的约束以及角度配置的约束本质上是非凸的。在本发明中，将上述约束转换为外部控制输入动态嵌入到
模型预测控制器中以保留编队模型预测控制器的凸性。利用本发明提出的算法，多架无人机能够实现在存在静态和动态障碍物的情况下联合检测与跟踪移动目标。
[0170]
本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。
[0171]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。