本发明属于路径规划技术领域，涉及多无人机协同跟踪和雷达信息处理技术研究。
背景技术：
由于装载有机载雷达的无人机(uav)具有灵活性强、机动性高、代价低和低空突防等特点，因此无人机在监视、跟踪和救援等方面得到广泛应用。在对目标进行跟踪时，多无人机可以从不同视角对目标进行观测进而提高估计结果。然而由于实际环境复杂，提高无人机在威胁/障碍下的生存能力具有重要意义。总之，无人机作为一种目标探测、跟踪和打击的新手段，在民用和军用领域都有着广阔的发展空间。
对于分布式多无人机协同跟踪系统，各无人机通过不同视角观测目标，然后将量测信息输入到局部卡尔曼滤波器估计目标的状态，之后通过融合准则对各局部状态估计进行融合得到目标状态的全局估计。但随着无人机数量的增大，将导致维数灾难，而且选择跟踪精度为代价函数计算量大，从而导致实时更新多无人机的位置难以解决。文献"jointpassivesensorschedulingfortargettracking,"in201720thinternationalconferenceoninformationfusion(fusion),2017,pp.1-7.”中以费歇尔信息为代价函数，利用遍历的方法求解改优化问题，进而得到无人机的航迹。但该方法仅考虑了无威胁下的两架无人机协同情形，且计算复杂度高，不适合实际应用。在文献"基于改进a-star算法的无人机航迹规划算法研究[j].兵工学报,2008,29(07):788-792.”中建立了以路径代价和威胁代价的加权和为目标函数的数学模型，但该方法仅仅是在单个无人机在炮火威胁下的航迹规划，并没有研究在威胁下的多无人机协同路径规划算法。上述方法只针对单/两架无人机在一种威胁下的路径规划，不能用于在多种威胁下的多无人机路径规划。
技术实现要素：
本发明的目的是针对背景技术存在的缺陷，研究设计一种多约束下多无人机协同跟踪的路径规划方法，解决现有多无人机路径规划没有考虑威胁/障碍及无人机物理约束，从而无法实现多种约束下的多无人机对目标协同跟踪的问题。
本发明的解决方案是首先利用分布式融合准则将各无人机对目标的局部滤波估计进行融合，然后利用a最优准则建立包含融合后的目标估计误差协方差矩阵的代价函数，之后针对威胁/障碍约束，通过惩罚函数来修正代价函数并应用最速下降法快速求解此优化问题，最后通过约束无人机的最大转弯角以便于无人机的飞行。该方法有效解决了现有多无人机路径规划没有考虑威胁/障碍及无人机物理约束的问题，从而实现复杂环境下的多无人机对目标的协同跟踪。
本发明提出了一种多约束下多无人机协同跟踪的路径规划方法，该方法包括：
步骤1：多无人机跟踪系统参数初始化，
目标的
步骤2：计算分布式无人机滤波器的状态估计；
步骤2-1：利用扩展的卡尔曼滤波器估计目标的状态，计算目标的一步状态预测：
xk|k-1＝fxk-1|k-1(1)
其中，xk|k-1是滤波器在k时刻的一步状态预测，xk|k是在k时刻的卡尔曼滤波状态估计，f是目标状态转移矩阵，即，
ts表示采样周期；
步骤2-2：计算一步预测误差协方差矩阵；
pk|k-1是在k时刻的一步预测误差的协方差矩阵，pk-1k-1是在k时刻的卡尔曼滤波状态估计误差协方差矩阵，q是过程噪声v(k)的协方差矩阵，v(k)假设是零均值的高斯白噪声，即
步骤2-3：计算卡尔曼增益；
其中，kk是卡尔曼增益，pk|k-1表示在k时刻的一步预测误差的协方差矩阵，rk是量测噪声w(k)的协方差矩阵，w(k)是零均值的高斯白噪声，即
其中，σi是n架无人机在k时刻的角度量测噪声的标准差；假设不同无人机的量测噪声相互独立；hk是非线性量测h(x(k))的雅克比矩阵，即，
其中，
步骤2-4：确定目标状态估计
xk|k＝xk|k-1+kk(zk-h(xk|k-1))(8)
其中，xk|k是k时刻的卡尔曼滤波状态估计，xk|k-1表示滤波器在k时刻的一步状态预测，kk表示是卡尔曼增益，zk是n架无人机在k时刻的角度量测，即
z(k)＝h(x(k))+w(k)(9)
步骤2-5：计算状态估计误差协方差矩阵
pk|k＝(i-kkhk)pk|k-1(10)
其中，pk|k是在k时刻的卡尔曼滤波状态估计误差协方差矩阵，pk|k-1表示在k时刻的一步预测误差的协方差矩阵，i是4×4的单位阵；
重复步骤2-1至2-7进行每个无人机滤波器的递推滤波的计算；
步骤3：将步骤2中的局部滤波状态估计进行数据融合，并计算代价函数的值；
步骤3-1：由各局部滤波器的目标状态估计计算目标的全局状态估计
其中，
步骤3-2：计算代价函数的值，即计算融合后的状态估计误差协方差矩阵pfused的迹
其中，tr(·)是矩阵的求迹符号，j(πk)是融合后的状态估计误差协方差矩阵pfused的迹；
步骤3-3：计算修正的代价函数的值；
假设在位置c(i),i＝1,2,...,m处存在m个威胁/障碍，通过修正代价函数使uavs避开威胁/障碍，修正的目标函数为：
其中，ki,i＝1,2,...,m表示威胁强度，即与威胁相关的风险，ki越大，威胁c(i)越大，对uavs的航迹影响也越大；当uavs距离威胁/障碍很远时，jz≈j，此时威胁基本不对航迹产生影响，当距离威胁/障碍近时，jz会变大，进而影响uavs的移动，n表示无人机的架数；
步骤4：确定最优航迹；
步骤4-1：利用最速下降法求解各无人机的最优航迹；
假设无人机在k,k＝1,2…时刻的位置表示为
其中δk＝[δ1(k),δ2(k),...δn(k)]t，
步骤4-2：对航迹进行修正；
得到各无人机的航向后，进而计算无人机的转向角，这是因为无人机固有的物理约束，即无人机只能以小于或等于预定最大转向角
其中，θi(k)是无人机下一时刻的转弯角，当无人机的飞行方向
本发明的有益效果：本发明的方法是利用最速下降法得到了多种约束下的多无人机协同跟踪目标的最优航迹，首先利用分布式融合准则将各无人机对目标的局部滤波估计进行融合，然后利用a最优准则建立代价函数，之后针对威胁/障碍约束，通过惩罚函数来修正代价函数并应用最速下降法快速求解此优化问题，最后通过约束无人机的最大转弯角以便于无人机的飞行。从而解决了在威胁/障碍和物理约束下多无人机协同跟踪目标的问题，本发明的优点是实现了复杂环境下的多无人机对目标的协同跟踪，求解过程简单、计算复杂度低、精度高。本发明可以应用于交通监视、城市救援等领域。
附图说明
图1是本发明提供方法的流程框图。
图2是本发明具体实施方式采用的多无人机在某一时刻的协同飞行结构图。
图3是本发明具体实施方式采用的三架无人机在炮火威胁下跟踪固定目标的飞行路径结果图。(图3中的五角星代表目标的最终位置，圆代表无人机的最终位置)
图4是本发明具体实施方式采用的三架无人机在炮火威胁下跟踪移动目标的飞行路径结果图。(图4中的五角星代表目标的最终位置，圆代表无人机的最终位置)
图5是本发明具体实施方式采用的三架无人机跟踪移动目标的位置均方误差比较。
具体实施方式
本发明主要采用仿真实验的方法进行验证，所有步骤、结论都在matlab2010上验证正确。下面就具体实施方式对本发明作进一步的详细描述。
步骤一：对某一时刻多无人机跟踪目标的几何结构，初始化各系统参数。
步骤二：利用局部扩展卡尔曼滤波器计算各无人机对目标的状态估计。首先利用表达式(1)计算目标状态的一步状态预测，然后利用表达式(3)确定一步预测误差协方差矩阵，之后利用表达式(5)计算卡尔曼增益，进而利用表达式(8)计算目标状态的一步状态预测，再利用公式(7-9)得到各无人机对目标的状态估计，最后利用表达式(10)得到各无人机目标状态估计误差协方差矩阵。重复表达式(1)(3)(5)(8)(10)进行每架无人机递推滤波的计算。
步骤三：利用表达式(11-12)把步骤二中的各无人机局部滤波状态估计进行融合，得到目标的全局状态估计和全局状态估计误差协方差矩阵。
步骤四：首先利用表达式(13)得到代价函数的值，之后针对威胁/障碍约束，计算修正的代价函数值。
步骤五：首先利用表达式(15)计算各无人机下一时刻的最优位置，之后利用表达式(16)计算下一时刻的转弯角。若下一时刻的转弯角大于无人机最大偏转角，则按照表达式(17)各更新无人机下一时刻的最优位置。重复步骤二、三、四和五，就可实现威胁/障碍下的多无人机对目标的协同跟踪。
图3至图5分别三架无人机在威胁/障碍下跟踪固定目标的路径图、三架无人机在威胁/障碍下跟踪匀速移动目标的路径图和三架无人机跟踪移动目标的位置均方误差比较图。图3和图4对应的参数表分别为表1和表2。
表1
表2
通过本发明具体实施方式可以看出，本发明可以很好的实现复杂环境下多无人机对目标的协同跟踪。
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。