一种加权不确定性无人机集群协同导航方法

1.本发明涉及协同导航技术领域，特别涉及一种加权不确定性无人机集群协同导航方法。


背景技术：

2.近年来，随着无人机技术的快速发展，在军事作战、地震救灾和农林植保等领域获得了广泛应用。单架无人机由于受自身动力、载荷和功能有限等方面的约束，难以单独执行复杂任务。而由一定数量无人机组成的集群系统，可在低成本基础上进一步提高整体载荷和感知能力，最大限度地发挥无人机的优势。
3.由卫星导航等无线电方式提供的精确位置信息是无人机集群进行分布式控制和编队的基础，但当无人机集群穿行于城市隧道、洞穴等卫星导航信号失效的环境时，无人机需依靠惯性导航设备获取导航信息。然而受制于惯性器件各类误差，惯性导航系统的位置误差随时间发散，导致无人机集群无法顺利遂行任务。协同导航技术通过无人机间精确测距测向和信息共享，采用特定算法实现个体位置信息的协同优化，能够提升独立个体的定位与导航性能，是集群完成复杂任务组织和精确协同配合的基础。
4.卫星导航拒止条件下的协同导航技术一般分为主从式和并行式两类，主从式一般需要集群中有携带高精度惯性导航系统的主节点无人机提供导航参考信息，实现对集群中其余无人机位置的校准。但对于主节点依赖较高，导致集群系统鲁棒性差。而并行式方法中不依赖特定无人机的定位精度，存在个别无人机故障或通信失败时仍可实现相互校正，保证集群中其余个体的准确定位。传统并行式协同导航算法直接将导航信息进行等权重融合，未考虑各无人机位置误差大小受到如惯性器件质量、无人机机动状态和丢失卫星导航信号时间等多种因素影响，导致协同导航精度提升效果有限。


技术实现要素：

5.本发明的目的是提供一种解决在卫星导航信号丢失条件下无人机惯性导航系统误差随时间累计的问题的加权不确定性无人机集群协同导航方法。
6.为了解决上述问题，本发明提出一种基于加权置信传播的协同导航算法，在构建协同导航贝叶斯因子图模型的基础上，利用改进的置信传播算法完成因子图模型中的信息传递与融合，实现多个无人机的协同导航。
7.为此，本发明的具体技术方案如下：
8.一种加权不确定性无人机集群协同导航方法，其具体步骤如下：
9.1、一种加权不确定性无人机集群协同导航方法，其特征在于，步骤如下：
10.s1、各无人机根据自身惯性测量单元输出的位移量、角速率信息和加速度信息，获得其当前时刻的信度信息，包括无人机当前时刻计算位置和位置不确定性对角阵；
11.s2、各无人机接收其它无人机广播的信度信息，并将其它无人机的信度信息转换为粒子集形式；
12.对于任一架无人机i：
13.s201、无人机i接收其它无人机j广播的信度信息；
14.s202、将无人机j的当前时刻计算位置和位置不确定性对角阵作为正态分布中的参数，随机抽取m个粒子，构成计算位置粒子集其中，p为粒子的计算位置，w为粒子对应的权值，下标k表示粒子集内部的粒子编号；
15.s203、将无人机j到无人机i之间相对距离量测值和无人机j到无人机i之间相对距离量测的方差为作为正态分布中的参数，随机抽取m个粒子，构成距离粒子集其中，下标k表示粒子集内部的粒子编号，通过无人机携带的无线电测距设备测得，为无线电测距设备相关参数；
16.s204、按均匀分布(0,2π]随机抽取m个粒子，构成用于描述无人机j与无人机i之间相对角度的角度粒子集其中，下标k表示粒子集内部的粒子编号；
17.s205、基于步骤s202～步骤s204，无人机i接收到其余每架无人机j的信度信息的粒子集根据公式：计算得到；
18.s3、各无人机对其所接收到的信度信息的粒子集，求解出每个信度信息的权值，并进行归一化处理；
19.s301、无人机i根据接收的信度信息的粒子集计算其与各无人机j之间的位置信息的权值w
ij
(t)：
[0020][0021]
式中，c为与惯性器件精度有关的参数，ji{p(t)}为无人机i的综合位置不确定性，其由无人机i的北向的位置不确定性矩阵e{δpn(t)}和东向的位置不确定性矩阵e{δpe(t)}计算得到，计算公式为：计算得到；jj{p(t)}为无人机j的综合位置不确定性，其由无人机j的北向的位置不确定性矩阵e{δpn(t)}和东向的位置不确定性矩阵e{δpe(t)}计算得到，计算公式为：计算得到；
[0022]
s302、将全部信度信息的权值w
ij
(t)按照公式：进行归一化计算，得到归一化权值v
ij
(t)；其中，n为构成无人机集群的无人机数量。
[0023]
s4、计算修正后的当前时刻计算位置；
[0024]
s401、将无人机i接收到的信度信息接收信度信息的粒子集分别转化为概率密度函数
[0025][0026]
式中，为高斯核，σ为测距信息的标准差，与分别为信度信息的粒子集中的粒子的位置和粒子的权值；
[0027]
s402、将无人机i当前时刻计算位置转化为概率密度函数
[0028][0029]
式中，与分别为无人机i当前时刻计算位置的粒子集内的粒子的位置和粒子的权值；
[0030]
s403、将步骤s2得到的全部信度信息粒子集汇总并从中均匀采样，随机抽取500个粒子，得到代表无人机i位置分布的粒子集其中，为粒子的位置，为粒子的新权值，其计算公式为：
[0031][0032]
式中，与分别为通过核密度估计得到的计算位置信息和接收信度信息的概率密度函数，表示无人机i接收到的全部信度信息粒子集的总和，表示无人机i所能接收到无人机j信度信息的集合；
[0033]
s404、无人机i根据公式：求得位置的最小均方误差，即经过修正后的无人机i的当前时刻位置。
[0034]
进一步地，步骤s1中，无人机的当前时刻的计算位置计算公式为：
[0035][0036]
式中，下标i表示无人机编号，表示无人机上一时刻位置，f(ω)为惯性导航系统输出的位移量，为无人机当前时刻计算位置。
[0037]
进一步地，无人机位置不确定性对角阵u
(t)
的计算公式为：
[0038]u(t)
＝diag{e{δpn(t)}e{δpe(t)}}，
[0039]
式中，e{δpn(t)}为无人机的北向的位置不确定性矩阵e{δpn(t)}，e{δpe(t)}为无人机的东向的位置不确定性矩阵，二者通过无人机的惯性测量单元输出的角速率信息和加速度信息计算得到；其中，
[0040]
[0041][0042]
式中，δk
gx
为载体坐标系下x轴方向上的陀螺标度因数误差，δk
gy
为载体坐标系下y轴方向上的陀螺标度因数误差，δk
gz
为载体坐标系下z轴方向上的陀螺标度因数误差；δk
ay
为载体坐标系下y轴方向上的加速度计的度因数误差，δk
ax
为载体坐标系下x轴方向上的加速度计的度因数误差；ω
x
为载体坐标系下x轴方向上的陀螺输出角速率，ωy为载体坐标系下y轴方向上的陀螺输出角速率，ωz为载体坐标系下z轴方向上的陀螺输出角速率；ε
x
为载体坐标系下x轴方向上的陀螺零偏值，εy为载体坐标系下y轴方向上的陀螺零偏值，εz为载体坐标系下z轴方向上的陀螺零偏值；
▽
x
为载体坐标系下x轴方向上的加速度计零偏值，
▽y为载体坐标系下y轴方向上的加速度计零偏值；为载体坐标系到导航坐标系的转换矩阵；g为重力加速度，fn为加速度计输出的加速度在北向的分量，fe为加速度计输出的加速度在东向的分量，fy为载体坐标系下y轴方向上的加速度计输出的加速度，f
x
为载体坐标系下x轴方向上的加速度计输出的加速度；t为卫星导航信号丢失时间；载体具体指无人机i。
[0043]
4、根据权利要求1所述的加权不确定性无人机集群协同导航方法，其特征在于，m≥500。
[0044]
与现有技术相比，该加权不确定性无人机集群协同导航方法结合惯性测量单元的动态误差统计特性来表征无人机动态位置误差与相关影响因素，可提升协同导航算法在无人机动态情况下的鲁棒性；同时，针对各无人机位置误差实时变化的特点，将无人机协同导航过程中所接收的位置信息进行量化，并构建自适应权值调节函数，实现对无人机集群协同导航中各无人机位置信息的有效融合和修正。
附图说明
[0045]
图1为本发明的加权不确定性无人机集群协同导航方法的流程图；
[0046]
图2为本发明的实施例中五架无人机的具体飞行轨迹与惯性导航仿真输出的路径的对比图；
[0047]
图3为本发明的实施例中根据50次仿真试验得到的采用本发明的协同导航方法、基于信度传播的协同导航方法、以及惯性导航自身输出数据得到的平均位置误差随时间变化的曲线图。
具体实施方式
[0048]
下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。
[0049]
如图1所示，该加权不确定性无人机集群协同导航方法的具体实施步骤如下：
[0050]
s1、各无人机根据自身惯性测量单元输出的位移量、角速率信息和加速度信息，获得其当前时刻的信度信息，包括无人机当前时刻计算位置和位置不确定性对角阵u
(t)
；
[0051]
具体地，该步骤s1的具体过程为：
[0052]
s101、无人机根据惯性导航系统输出的位移量得到当前时刻的计算位置：
[0053]
[0054]
式中，下标i表示无人机编号，表示无人机上一时刻位置，f(ω)为惯性导航系统输出的位移量，为无人机当前时刻计算位置；
[0055]
s102、无人机根据其惯性测量单元输出的角速率信息和加速度信息，计算其北向的位置不确定性矩阵e{δpn(t)}和东向的位置不确定性矩阵e{δpe(t)}，包括：
[0056]
(1)北向的位置不确定性矩阵e{δpn(t)}：
[0057][0058]
(2)东向的位置不确定性矩阵e{δpe(t)}：
[0059][0060]
式中，δk
gx
为载体坐标系下x轴方向上的陀螺标度因数误差，δk
gy
为载体坐标系下y轴方向上的陀螺标度因数误差，δk
gz
为载体坐标系下z轴方向上的陀螺标度因数误差；δk
ay
为载体坐标系下y轴方向上的加速度计的度因数误差，δk
ax
为载体坐标系下x轴方向上的加速度计的度因数误差；ω
x
为载体坐标系下x轴方向上的陀螺输出角速率，ωy为载体坐标系下y轴方向上的陀螺输出角速率，ωz为载体坐标系下z轴方向上的陀螺输出角速率；ε
x
为载体坐标系下x轴方向上的陀螺零偏值，εy为载体坐标系下y轴方向上的陀螺零偏值，εz为载体坐标系下z轴方向上的陀螺零偏值；
▽
x
为载体坐标系下x轴方向上的加速度计零偏值，
▽y为载体坐标系下y轴方向上的加速度计零偏值；为载体坐标系到导航坐标系的转换矩阵；g为重力加速度，fn为加速度计输出的加速度在北向的分量，fe为加速度计输出的加速度在东向的分量，fy为载体坐标系下y轴方向上的加速度计输出的加速度，f
x
为载体坐标系下x轴方向上的加速度计输出的加速度；t为卫星导航信号丢失时间；载体具体指无人机i；
[0061]
s103、无人机根据由步骤s102得到的北向的位置不确定性矩阵e{δpn(t)}和东向的位置不确定性矩阵e{δpe(t)}，计算得到其位置不确定性对角阵u
(t)
：u
(t)
＝diag{e{δpn(t)}e{δpe(t)}}；
[0062]
s2、各无人机接收其它无人机广播的信度信息，并将其它无人机的信度信息转换为粒子集形式；
[0063]
具体地，对于任一架无人机i来说，该步骤s2具体过程包括：
[0064]
s201、无人机接收其它无人机j广播的信度信息，即其余每架无人机广播的当前时刻计算位置和位置不确定性对角阵
[0065]
s202、将无人机j的当前时刻计算位置和位置不确定性对角阵作为正态分布中的参数，随机抽取500个粒子，构成计算位置粒子集其中，p为粒子的计算位置，w为粒子对应的权值，下标k表示粒子集内部的粒子编号；
[0066]
s203、将无人机j到无人机i之间相对距离量测值和无人机j到无人机i之间相对距离量测的方差为作为正态分布中的参数，随机抽取500个粒子，构成距离粒子集其中，下标k表示粒子集内部的粒子编号，通过无人机携带的无
线电测距设备测得，为无线电测距设备相关参数；
[0067]
s204、按均匀分布(0,2π]随机抽取500个粒子，构成用于描述无人机j与无人机i之间相对角度的角度粒子集其中，下标k表示粒子集内部的粒子编号；
[0068]
s205、基于步骤s202～步骤s204，无人机i接收到其余每架无人机j的信度信息的粒子集根据公式：计算得到。
[0069]
s3、各无人机对其所接收到的信度信息的粒子集求解出每个信度信息的权值w
ij
(t)，并进行归一化处理；
[0070]
具体地，该步骤s3具体过程包括：
[0071]
s301、无人机i根据接收的信度信息的粒子集计算其与各无人机j之间的位置信息的权值w
ij
(t)：
[0072][0073]
式中，c为与惯性器件精度有关的参数，ji{p(t)}为无人机i的综合位置不确定性，其由无人机i的北向的位置不确定性矩阵e{δpn(t)}和东向的位置不确定性矩阵e{δpe(t)}计算得到，计算公式为：计算得到；jj{p(t)}为无人机j的综合位置不确定性，其由无人机j的北向的位置不确定性矩阵e{δpn(t)}和东向的位置不确定性矩阵e{δpe(t)}计算得到，计算公式为：计算得到；
[0074]
s302、将全部信度信息的权值w
ij
(t)按照公式：进行归一化计算，得到归一化权值v
ij
(t)；其中，n为构成无人机集群的无人机数量；
[0075]
s4、计算修正后的当前时刻计算位置；
[0076]
具体地，该步骤s4具体过程包括：
[0077]
s401、将无人机i接收到的信度信息接收信度信息的粒子集分别转化为概率密度函数
[0078][0079]
式中，为高斯核，σ为测距信息的标准差，与分别为信度信息的粒子集中的粒子的位置和粒子的权值；
[0080]
s402、将无人机i当前时刻计算位置转化为概率密度函数
[0081][0082]
式中，与分别为无人机i当前时刻计算位置的粒子集内的粒子的位置和粒子的权值；
[0083]
s403、将步骤s2得到的全部信度信息粒子集汇总并从中均匀采样，随机抽取500个粒子，得到代表无人机i位置分布的粒子集其中，为粒子的位置，为粒子的新权值，其计算公式为：
[0084][0085]
式中，与分别为通过核密度估计得到的计算位置信息和接收信度信息的概率密度函数，表示无人机i接收到的全部信度信息粒子集的总和，表示无人机i所能接收到无人机j信度信息的集合；
[0086]
s404、无人机i根据公式：求得位置的最小均方误差，即经过修正后的无人机i的当前时刻位置。
[0087]
将该修正后的替换无人机i的当前时刻计算位置完成无人机位置状态的融合更新。
[0088]
为验证本技术的方法的有效性，将本技术采用的无人机集群协同导航方法与广泛使用的基于信度传播的协同导航方法进行了仿真对比验证。
[0089]
具体地，仿真条件设置为：假设五架无人机在同一高度飞行，初始速度均为2m/s；其中，无人机1号、无人机2号和无人机5号采用匀速直线飞行模式，无人机3号采用先加速后减速的飞行模式，无人机4号采用进行转弯机动的飞行模式；在运动过程中，无人机每1秒进行一次相互测距及导航信息交互；仿真时间100s，陀螺的零偏值和噪声分别为1
°
/h和0.1
°
/h，加速度计的零偏值和噪声分别为1mg和0.1mg，标度因数误差为100ppm，相对测距标准差为0.2m；
[0090]
如图2所示为五架无人机的具体飞行轨迹与惯性导航仿真输出的路径的对比图。图中实线线条为五架无人机飞行轨迹的真实值，虚线线条为惯性导航仿真输出的误差路径；从每架无人机的实际飞行轨迹与误差路径的对比可以看出，直接采用惯性导航仿真输出的路径与实际路径具有巨大差异。
[0091]
如图3所示为根据50次仿真试验得到的采用本发明的协同导航方法、基于信度传播的协同导航方法、以及惯性导航自身输出数据得到的平均位置误差随时间变化的曲线图。如图3中可以明显看出，采用本发明的协同导航方法、基于信度传播的协同导航方法、以及惯性导航自身输出数据得到的平均位置误差逐渐增大，试验结果表明，惯性导航自身位置误差为52米，而传统基于信度传播的协同导航方法的位置误差为33米，而本技术的位置误差为18米，综上，本技术的方法确实实现了进一步提升卫星导航信号拒止条件下的导航
精度的目的。