开云优惠体育网页版入口

专利名称：一种基于分层式扩展卡尔曼滤波的无人机群协同定位方法及系统

专利类型：发明专利

专利申请号：CN202310798488.6

专利申请（专利权）人：哈尔滨工业大学(威海)
权利人地址：山东省威海市文化西路2号

专利发明（设计）人：罗清华,杨可欣,贾广乐,焉晓贞,王晨旭,周志权

专利摘要：本发明一种基于分层式扩展卡尔曼滤波的无人机群协同定位方法及系统，涉及无人机定位技术领域，为解决现有的卡尔曼滤波针对大型无人机群协同定位，存在扩展性和灵活性低，且算法复杂度高的问题。包括如下步骤：S1、对目标从无人机i在t?1时刻的状态值#imgabs0#和协方差矩阵#imgabs1#进行初始化；S2、构建状态方程，计算状态估计值；S3、计算目标从无人机i与主无人机层的主无人机j之间的量测值#imgabs2#S4、计算误差协方差矩阵估计#imgabs3#S5、计算目标从无人机i的系统增益矩阵#imgabs4#S6、对状态估计值及误差协方差矩阵进行更新。本发明计算量小、实时性高、收敛速度快。融合了多个传感器的信息，提高了系统的稳定性。

主权利要求：
1.一种基于分层式扩展卡尔曼滤波的无人机群协同定位方法，其特征在于，包括如下步骤：S1、对目标从无人机i在t?1时刻的状态值 和协方差矩阵 进行初始化；
S2、构建状态方程，计算状态估计值；
S3、计算目标从无人机i与主无人机层的主无人机j之间的量测值S4、计算误差协方差矩阵估计
S5、计算目标从无人机i的系统增益矩阵
S6、对状态估计值及误差协方差矩阵进行更新；
S2中构建状态方程为：
ij
其中， 为目标从无人机i在t时刻的估计值，Q 为系统噪声，且为零均值高斯白噪声，F为系统的状态转移矩阵，在MATLAB仿真中获。篠3中计算目标从无人机i与主无人机层的主无人机j之间的量测值 的公式为：ij
其中V 为量测噪声，且为零均值高斯白噪声， 为系统观测方程的雅可比矩阵，为：式中 为目标从无人机i与主无人机j之间的量测方程；
S4中计算误差协方差矩阵估计 的公式为：S5中计算目标从无人机i的系统增益矩阵 的计算公式为：其中Ni表示目标从无人机i的可通信邻居主无人机的集合。
2.根据权利要求1所述的基于分层式扩展卡尔曼滤波的无人机群协同定位方法，其特征在于，S1中目标从无人机的状态值包括：目标从无人机的位置、速度和加速度，即TX＝[sxsyszvxvyvzaxayaz]其中，sx、sy和sz分别表示目标从无人机沿x轴方向、y轴方向和z轴方向的位置信息，vx、vy、vz分别表示目标从无人机沿x轴方向、y轴方向和z轴方向的速度信息，ax、ay、az分别表示目标从无人机沿x轴方向、y轴方向和z轴方向的加速度信息。
3.根据权利要求1所述的基于分层式扩展卡尔曼滤波的无人机群协同定位方法，其特征在于，S6对状态估计值进行更新的计算公式为：其中 为根据当前目标从无人机i状态估计值 得到的目标从无人机i与主无人机j之间的量测信息估计值：对误差协方差矩阵进行更新的计算公式为：
4.一种基于分层式扩展卡尔曼滤波的无人机群协同定位系统，其特征在于，该系统具有与上述权利要求1～3任一项所述方法的步骤对应的程序模块，运行时执行上述的基于分层式扩展卡尔曼滤波的无人机群协同定位方法中的步骤。
5.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1～3中任一项所述的基于分层式扩展卡尔曼滤波的无人机群协同定位方法的步骤。 说明书 : 一种基于分层式扩展卡尔曼滤波的无人机群协同定位方法及
系统技术领域[0001] 本发明涉及无人机定位技术领域，具体而言，涉及一种基于分层式扩展卡尔曼滤波的无人机群协同定位方法及系统。背景技术[0002] 无人机(UnmannedAerialVehicle，UAV)作为无人系统的重要组成部分，近些年来受到广泛关注，而无人机群是未来的发展方向之一。在无人机集群飞行的过程中，无人机相互之间的位置非常重要，但是仅依靠传统的机载导航设备可能会存在误差漂移，而且整个无人机群的成本将会大大增加，这对于无人机群的定位精度和系统的可靠性提出了很高的要求。目前无人机群协同定位算法中常用的信息融合手段为卡尔曼滤波。但针对大型无人机群协同定位问题，目前的算法大都存在扩展性和灵活性低，且算法复杂度高的问题。并且对于大型的无人机群，如果只有一个主无人机，那么系统主无人机失效时的产生的代价将会非常大，系统的可靠性非常低。因此对于大型的无人机群，对系统的可靠性提出了更高的要求。发明内容[0003] 本发明要解决的技术问题是：[0004] 现有的卡尔曼滤波针对大型无人机群协同定位，存在扩展性和灵活性低，且算法复杂度高的问题。[0005] 本发明为解决上述技术问题所采用的技术方案：[0006] 本发明提供了一种基于分层式扩展卡尔曼滤波的无人机群协同定位方法，包括如下步骤：[0007] S1、对目标从无人机i在t?1时刻的状态值 和协方差矩阵 进行初始化；[0008] S2、构建状态方程，计算状态估计值；[0009] S3、计算目标从无人机i与主无人机层的主无人机j之间的量测值[0010] S4、计算误差协方差矩阵估计[0011] S5、计算目标从无人机i的系统增益矩阵[0012] S6、对状态估计值及误差协方差矩阵进行更新。[0013] 进一步地，S1中所述无人机的状态值包括：无人机的位置、速度、加速度，即[0014] x＝[xyzvxvyvzaxayaz]T[0015] 其中，x、y和z分别表示无人机沿x轴方向、y轴方向和z轴方向的位置信息，分别表示无人机沿x轴方向、y轴方向和z轴方向的速度信息， 分别表示无人机沿x轴方向、y轴方向和z轴方向的加速度信息。[0016] 进一步地，S2中构建状态方程为：[0017][0018] 其中， 为从无人机i在t时刻的估计值，Qij为系统噪声，且为零均值高斯白噪声，F为系统的状态转移矩阵，在MATLAB仿真中获。篬0019][0020] 进一步地，计算目标从无人机i与主无人机层的主无人机j之间的量测值 的公式为：[0021]ij[0022] 其中V 为量测噪声，且为零均值高斯白噪声， 为系统观测方程的雅可比矩阵，为：[0023][0024] 式中 为目标从无人机i与主无人机j之间的量测方程。[0025] 进一步地，S4中计算误差协方差矩阵估计 的公式为：[0026][0027] 进一步地，S5中计算目标从无人机i的系统增益矩阵 的计算公式为：[0028][0029] 其中Ni表示目标从无人机i的可通信邻居主无人机的集合。[0030] 进一步地，S6对状态估计值进行更新的计算公式为：[0031][0032] 其中 为根据当前目标从无人机i状态估计值 得到的目标从无人机i与主无人机j之间的量测信息估计值：[0033][0034] 对误差协方差矩阵进行更新的计算公式为：[0035][0036] 一种基于分层式扩展卡尔曼滤波的无人机群协同定位系统，该系统具有与上述技术方案中任一项的步骤对应的程序模块，运行时执行上述的基于分层式扩展卡尔曼滤波的无人机群协同定位方法中的步骤。[0037] 一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现上述技术方案中任一项所述的基于分层式扩展卡尔曼滤波的无人机群协同定位方法的步骤。[0038] 相较于现有技术，本发明的有益效果是：[0039] 本发明一种基于分层式扩展卡尔曼滤波的无人机群协同定位方法及系统，为了兼顾定位精度、定位效率及系统的扩展性，根据定位精度将无人机群分为高精度的主无人机层和低精度的从无人机层，在传统的扩展卡尔曼滤波算法的基础上，融合了主无人机的高精度信息，对低精度的从无人机进行了修正。[0040] 本发明可利用无人机上自身传感器及与其他无人机的相对信息，快速计算无人机的位置、速度及加速度信息，将无人机自身的低精度信息进行提高。[0041] 本发明计算量小、实时性高、收敛速度快。融合了多个传感器的信息，提高了系统的稳定性，并且能够降低系统对传感器测量精度的要求，降低了系统的成本，并且在同等传感器精度情况下，能够提供更高精度的无人机位置信息。附图说明[0042] 图1为本发明实施例中一种基于分层式扩展卡尔曼滤波的无人机群协同定位方法的流程图；[0043] 图2为本发明实施例中的分层式无人机群飞行轨迹仿真图；[0044] 图3为本发明实施例中的各算法误差对比图；[0045] 图4为本发明实施例中的不同相对测距精度下从无人机的均方根位置误差图；[0046] 图5为本发明实施例中的均方根位置误差与相对测距精度之间的关系图；[0047] 图6为本发明实施例中的不同相对测角精度下从无人机的均方根位置误差图；[0048] 图7为本发明实施例中的均方根位置误差与相对测角精度之间的关系图。具体实施方式[0049] 在本发明的描述中，应当说明的是，在本发明的实施例中所提到的术语“第一”、“第二”、“第三”仅用于描述目的，并不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”、“第三”的特征可以明示或者隐含地包括一个或者多个该特征。[0050] 为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。[0051] 如图1所示，本发明提供一种基于分层式扩展卡尔曼滤波的无人机群协同定位方法，假设系统中主无人机层有n架主无人机，从无人机层有m架从无人机。包括如下步骤：[0052] S1、对目标从无人机i在t?1时刻的状态值 和协方差矩阵 进行初始化；[0053] S2、构建状态方程，计算状态估计值；[0054] S3、计算目标从无人机i与主无人机层的主无人机j之间的量测值[0055] S4、计算误差协方差矩阵估计[0056] S5、计算目标从无人机i的系统增益矩阵[0057] S6、对状态估计值及误差协方差矩阵进行更新。[0058] S1中所述无人机的状态值包括：无人机的位置、速度、加速度，即[0059] x＝[xyzvxvyvzaxayaz]T[0060] 其中，x、y和z分别表示无人机沿x轴方向、y轴方向和z轴方向的位置信息，分别表示无人机沿x轴方向、y轴方向和z轴方向的速度信息， 分别表示无人机沿x轴方向、y轴方向和z轴方向的加速度信息。[0061] S2中构建状态方程为：[0062][0063] 其中， 为从无人机i在t时刻的估计值，Qij为系统噪声，且为零均值高斯白噪声，F为系统的状态转移矩阵，在MATLAB仿真中获。篬0064][0065] 计算目标从无人机i与主无人机层的主无人机j之间的量测值 的公式为：[0066]ij[0067] 其中V 为量测噪声，且为零均值高斯白噪声， 为系统观测方程的雅可比矩阵，为：[0068][0069] 式中 为目标从无人机i与主无人机j之间的量测方程。此项是由传感器获得。[0070] S4中计算误差协方差矩阵估计 的公式为：[0071][0072] S5中计算目标从无人机i的系统增益矩阵 的计算公式为：[0073][0074] 其中Ni表示目标从无人机i的可通信邻居主无人机的集合。[0075] S6对状态估计值进行更新的计算公式为：[0076][0077] 其中 为根据当前目标从无人机i状态估计值 得到的目标从无人机i与主无人机j之间的量测信息估计值：[0078][0079] 对误差协方差矩阵进行更新的计算公式为：[0080][0081] 实施例1[0082] 设计了一主多从式无人机系统的协同定位仿真实验，算法的基本参数设置为：设置总仿真步长为100步，场景中共有20架无人机，为了满足系统可观性，设计如图2所示的轨迹图，其中虚线表示主无人机的飞行轨迹，实线代表从无人机的飞行轨迹。[0083] (1)算法定位精度仿真分析[0084] 在正常环境下，假设所有的无人机正常运行。设置所有主无人机与从无人机之间的距离观测噪声标准差相等，均为Rd＝10m，俯仰角和方位角的观测噪声标准差均为Rangle＝0.1°。在上述环境下，使用传统的扩展卡尔曼滤波(ExtendedKalmanFilter，EKF)、无迹卡尔曼滤波(UnscentedKalmanFilter，UKF)、容积卡尔曼滤波(CubatureKalmanFilter，CKF)协同定位算法与本发明所提出的基于分层式扩展卡尔曼滤波的无人机群协同定位方法(HEKF)对从无人机进行定位，得到如图3所示的算法误差对比图。[0085] 根据图3可以看出，本发明的分层式协同定位方法(HEKF)的定位精度相比于其他三种算法均有所提高，在定位开始时就已经进入了收敛状态，而其他三种算法在40s左右才进入收敛状态。[0086] 为了更好的显示算法的定位误差大小及波动性，计算几种算法定位的均方根定位误差得到如表1所示的结果。[0087] 表1[0088][0089] 根据表1所示的定位误差可知，HEKF协同定位算法定位精度高于其他三种传统的主从式协同定位算法，相比于EKF协同定位算法定位精度提高了43.997％，相比于UKF协同定位算法定位精度提高了35.968％，相比于CKF协同定位算法定位精度提高了23.985％。由此可见，本发明提出的HEKF协同定位算法定位精度高于传统的协同定位算法。这是由于HEKF协同定位算法融合了多个主无人机的信息，提高了系统的定位精度。[0090] 将相对测距精度和相对测角精度对算法性能的影响进行分析。首先对不同相对测距精度下的算法定位性能进行分析。在测距精度分别为5m和20m情况下，将传统的协同定位算法和本发明提出的HEKF协同定位算法进行仿真对比，得到图4所示的误差结果。[0091] 在图4中可以看出，对于从无人机来说，提高测距的精度可以减少定位误差，与传统协同定位算法相比，HEKF协同定位算法的定位误差得到了改善。具体误差结果如表2所示。[0092] 表2[0093][0094] 从表2中可以看出，在测距精度为5m时，与传统的EKF、UKF和CKF协同定位算法相比，所提出的HEKF协同定位算法的平均定位精度分别提高了45.654％、37.199％、23.879％；在测距精度为20m时，与传统的EKF、UKF和CKF协同定位算法相比，本发明的HEKF协同定位算法的平均定位精度分别提高了47.072％、38.231％、26.733％。而且当相对测距误差Rd和绝对测距误差Vd为5m时，各个算法的定位精度均高于测距误差为20m时，说明测距误差越。ㄎ痪仍礁。[0095] 将主无人机与从无人机之间的相对测距误差Rd从5m逐渐变化到20m，得到如图5所示的定位误差和相对测距精度之间的关系图。[0096] 从图5可以看出，随着测距精度的降低，目标从无人机的平均定位精度也逐渐降低，并且用多项式对四种算法的均方根位置误差进行拟合，得到图5中的拟合函数。由此可以看出，HEKF协同定位算法均方根位置误差的拟合曲线的二次项系数最。馑得魉孀挪饩辔蟛畹脑黾，其均方根位置误差增加幅度比其他三种算法小。[0097] 将不同测角精度对算法性能的影响进行分析。在测角误差分别为0.1°和1.5°情况下，将传统的协同定位算法和本发明的HEKF协同定位方法进行仿真对比，得到图6所示的误差结果。[0098] 从图6中可以看出，角度测量精度的不同会影响算法的定位精度，提高角度测量的精度可以有效减少定位误差。相比于传统的协同定位算法，本发明的HEKF协同定位方法的定位精度较高。如表3所示，对四种算法进行定量分析：[0099] 表3[0100][0101] 从表3中可以看出，在测角精度为0.1°时，与传统的EKF、UKF和CKF协同定位算法相比，本发明的HEKF协同定位方法的平均定位精度分别提高了46.259％、37.228％、23.553％；在测角精度为1.5°时，与传统的EKF、UKF和CKF协同定位算法相比，本发明的HEKF协同定位方法的平均定位精度分别提高了48.513％、40.480％、32.217％。而且当测角误差Rangle＝0.1°时，各个算法的定位精度均高于测角误差为1.5°时，说明测角误差越。ㄎ痪仍礁。[0102] 将测角误差Rangle从0.1°逐渐变化到1.5°，得到如图7所示的算法均方根位置误差和测角精度之间的关系图。[0103] 从图7可以看出中，随着角度测量的误差从0.1°逐渐变化到1.5°的过程中，目标从无人机i的均方根位置误差也逐渐增大。同样使用多项式对四种算法的均方根位置误差进行拟合，得到图7中的拟合函数。可以看出，本发明的HEKF协同定位算法均方根位置误差的拟合曲线的二次项系数最。得魉孀沤嵌炔饬课蟛畹脑黾，其均方根位置误差增加幅度比其他三种算法小。[0104] 通过上述对不同测距和测角精度情况下算法定位精度的分析可知，与其他算法相比，本发明的HEKF协同定位方法在定位精度上得到了改善，并且能以更低的测量精度达到等效的定位精度，即降低了对测距和测角精度的要求。[0105] (2)算法效率分析[0106] 对基于EKF、UKF、CKF的协同定位算法及本发明的HEKF协同定位算法的平均耗时计算结果如表4所示。[0107] 表4[0108][0109] 从表4中可以看出，EKF协同定位方法的定位耗时最短，其次即为本发明的HEKF滤波的协同定位算法，而UKF和CKF的协同定位算法的定位耗时均高于本发明的HEKF协同定位算法。这是由于相较于EKF协同定位算法，分层式协同定位算法融合了多个主无人机的信息，故增加了计算复杂度，但是相较于UKF和CKF协同定位算法，HEKF协同定位算法中并不需要进行容积点的计算，所以其定位耗时明显少于UKF和CKF协同定位算法。[0110] 虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本发明领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

专利地区：山东

专利申请日期：2023-06-30

专利公开日期：2024-11-29

专利公告号：CN116907501B