CN114355974B - 基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法 - Google Patents

Abstract

本发明公开了基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，该方法包括以下步骤：首先，基于合力计算球形簇集合每层无人机的加速度，并将计算所得结果作为卡尔曼滤波的初始值；然后，确定无人机在k时刻的状态矩阵、测量矩阵、状态转移矩阵、系统输入矩阵、系统的过程噪声矩阵、先验估计和先验估计的协方差矩阵；接着，计算卡尔曼增益、后验估计值和协方差矩阵；然后，将后验估计加速度带入虚拟力导向合力公式，动态调节虚拟力导向合力公式中的系数控制无人机移动到后验估计中最优加速度位置，并计算抖动范围均值；最后，不断计算更新抖动范围均值，使平均抖动范围随着时间趋于零，最终抖动消除。

Description

基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法

技术领域

本发明属于无人机集群编队控制系统技术领域，具体涉及基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法。

背景技术

无人机集群，指借鉴自然界的自组织机制，使具备有限自主能力的多架无人机在没有集中指挥控制的情况下，通过相互间信息通信产生整体效应，实现较高程度的自主协作。编队技术是无人机集群协同的基础，指多个无人机甚至成千上百的无人机联合运动时，无人机之间保持固定或变化的几何形状，并适应周边环境约束，通过机间协调完成复杂任务。无人机编队技术中的无人机通过局部感知和对拓扑结构中的相互作用决定运动状态，使多个无人机对象共同构成集合以形成相互的自主协调。在其任务区域内飞行过程中的信息相互交汇可实现高强度的协作对抗，防止友机敌机发生碰撞。从而在少量人员干预情况下完成预期的任务目标。

在无人机集群编队控制中，基于一种虚拟力导向的方法对每个无人机节点计算出引力和斥力综合的合力，根据合力控制移动无人机的位置及其运动状态。设置斥力来控制相邻无人机之间的安全距离，而利用引力来引导整个集群系统的行为，满足“近距排斥，远距吸引”的原则。能保证集群中各无人机不发生碰撞，用于复杂环境下的避障问题和路径规划问题。通过平衡斥力和引力来控制每个无人机状态，建立一个稳定无人机集群编队。对每个无人机节点计算出引力和斥力综合的合力，根据合力控制移动无人机的位置及其运动状态。当引力和斥力均衡时，每个无人机都应处于平衡状态。

在三维坐标下，无人机在引力与斥力表达均衡情况下，形成一个球形的簇集合，但可能存在簇内成员并非静止的情况，每架无人机在很小的球形坐标范围内位移抖动，范围幅度由外向内，导致平均抖动范围随着时间逐渐趋于一个常数，且位移抖动将一直存在，影响工作能力。这是目前基于虚拟力导向无人机集群编队控制方法需要解决的问题。

发明内容

本发明的目的在于提供一种基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，解决基于虚拟力导向无人机集群编队时产生的位移抖动问题。

为实现上述目的，本发明提供以下技术方案：基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，包括以下步骤：

S1将无人机编队节点放入三维坐标内形成一个球形的簇集合，在球形簇中根据半径进行层级划分，同一层内的无人机加速度相同且离中心节点越远加速度越大，每架无人机在时间t内抖动范围为Δ＝αt，其中α为加速度；确定无人机的抖动合集Δ＝{Δ₀,Δ₁,...Δ_i...,Δ_N}，N为无人机节点总数，计算平均抖动范围为

S2对无人机集群编队中的每个无人机节点计算出引力和斥力综合的合力得到虚拟力导向合力公式：

F_合＝γ×F_引+λ×F_斥，

其中，γ表示引力调节系数，λ表示斥力调节系数；

S3将无人机的运动速度和加速度作为初始值，建立卡尔曼滤波模型，对每层无人机加速度进行卡尔曼滤波得到球形簇中每层无人机的后验估计加速度，通过所述虚拟力导向合理公式动态选择引力调节系数和/或斥力调节系数来控制球形簇中每层无人机达到所述后验估计加速度值，并计算无人机平均抖动范围；不断利用卡尔曼滤波更新后验估计加速度并调整引力调节系数和/或斥力调节系数，直到平均抖动范围抖动消除。

进一步，所述S3包括以下步骤：

S31：基于合力计算球形簇集合每层无人机的加速度及速度，并将计算所得结果作为卡尔曼滤波的初始值；

S32：确定每层无人机在k时刻的状态矩阵、测量矩阵，并根据所述状态矩阵得到卡尔曼滤波的状态方程，根据所述状态方程确定先验估计和先验估计的协方差矩阵，并根据测量矩阵得到卡尔曼滤波测量方程；

S33：根据S32计算卡尔曼增益，确定后验估计值和更新误差协方差矩阵；

S34：将后验估计加速度带入虚拟力导向合力公式，动态调节虚拟力导向合力公式中的系数控制无人机移动到后验估计中最优加速度位置，并计算抖动范围均值；

S35：不断计算更新抖动范围均值，使平均抖动范围随着时间趋于零，最终抖动消除。

进一步，所述S1中当时间t趋近于无穷时，若ρ为常数，将观察到无人机抖动现象，并且抖动不会消除。

进一步，所述S32包括以下步骤：

S321：建立卡尔曼滤波模型，对每层无人机的加速度进行卡尔曼滤波；

设在第i层的无人机在k时刻的加速度为a_i(k)，速度为v_i(k)，得到k时刻的状态矩阵：

v₁(k)＝v₁(k-1)+a₁(k-1)×t+w_1,1(k-1)

a₁(k)＝u₁(k)+w_1,2(k-1)

v_M(k)＝v_M(k-1)+a_M(k-1)×t+w_M,1(k-1)

a_M(k)＝u_M(k)+w_M,2(k-1)

其中，u(k)为无人机k时刻产生的加速度，w(k)为其他因素产生的干扰加速度；

S322：测量第i层无人机在k时刻的加速度及速度作为测量方程的初始值，得到k时刻的测量矩阵：

z_1,1＝v₁(k)+r_1,1(k)

z_1,2＝a₁(k)+r_1,2(k)

z_M,1＝v_M(k)+r_M,1(k)

z_M,2＝a_M(k)+r_M,2(k)；

其中，r_i,1(k),r_i,2(k)为测量噪声；

S323：根据S321得到卡尔曼滤波状态方程：

X(k)＝A·X(k-1)+U_i(k)+W_i(k-1)；

其中，A为状态转移矩阵：U_i(k)为系统输入矩阵：/>W(k)为系统的过程噪声矩阵：/>设其符合正太分布p(w)～N(0,Q)；

S324：卡尔曼滤波模型状态建好后，确定先验估计和先验估计的协方差矩阵：

S325：以t为时间间隔，通过测量计算无人机节点运动的加速度及速度，将计算所得结果作为测量方程初始值；根据S22，得到卡尔曼滤波测量方程：

Z(k)＝H·X(k)+R(k)；

其中，H为测量方程的测量矩阵，是单位矩阵，为测量噪声，其符合正态分布，p(r)～N(0,R)。

进一步，所述S33具体为：

根据S32计算卡尔曼增益K'，确定后验估计值和更新误差协方差矩阵，获得相对精确的后验估计加速度及速度；所述卡尔曼增益K'具体为：

进一步，所述S35具体为：

将无人机调整到后验估计的加速度位置后，将打破在引力和斥力均衡条件下，位移抖动不停的状态，最终加速度将逐渐收敛减小，抖动范围随着时间增长逐渐减小，随着时间的增加，一直更新平均抖动范围判断最后是否满足/>使平均抖动范围/>实现抖动消除。

有益效果：

本发明提供了一种基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，利用卡尔曼滤波方法，通过预测和校正两个主要步骤完成对数据的处理，最终实现对无人机的加速度和速度的最优预测估计。预测的后验估计加速度的值相比未滤波前趋于平滑，逐渐收敛，限制了最大抖动范围。根据后验估计加速度，动态调整无人机坐标，在引力和斥力均衡条件下，移动距离将逐渐减小，使抖动范围随着时间逐渐减少最后趋于零，最终消除抖动。

附图说明

图1为本发明基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法流程图；

图2为本发明基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法无人机抖动范围示意图；

图3为本发明基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法无人机球形簇集合示意图。

具体实施方式

下面结合具体实施方式对本发明作进一步说明，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。

实施例1：如图1至图3所示，本实施例提供了一种基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，包括以下步骤：

S1将无人机编队节点放入三维坐标内形成一个球形的簇集合，在球形簇中根据半径进行层级划分，同一层内的无人机加速度相同且离中心节点越远加速度越大，每架无人机在时间t内抖动范围为Δ＝αt，其中α为加速度；确定无人机的抖动合集Δ＝{Δ₀,Δ₁,...Δ_i...,Δ_N}，N为无人机节点总数，计算平均抖动范围为

S2对无人机集群编队中的每个无人机节点计算出引力和斥力综合的合力得到虚拟力导向合力公式：

F_合＝γ×F_引+λ×F_斥，

其中，γ表示引力调节系数，λ表示斥力调节系数；

S3将无人机的运动速度和加速度作为初始值，建立卡尔曼滤波模型，对每层无人机加速度进行卡尔曼滤波得到球形簇中每层无人机的后验估计加速度，通过所述虚拟力导向合理公式动态选择引力调节系数和/或斥力调节系数来控制球形簇中每层无人机达到所述后验估计加速度值，并计算无人机平均抖动范围；不断利用卡尔曼滤波更新后验估计加速度并调整引力调节系数和/或斥力调节系数，直到平均抖动范围抖动消除。

进一步，所述S3包括以下步骤：

S31：基于合力计算球形簇集合每层无人机的加速度及速度，并将计算所得结果作为卡尔曼滤波的初始值；

S32：确定每层无人机在k时刻的状态矩阵、测量矩阵，并根据所述状态矩阵得到卡尔曼滤波的状态方程，根据所述状态方程确定先验估计和先验估计的协方差矩阵，并根据测量矩阵得到卡尔曼滤波测量方程；

S33：根据S32计算卡尔曼增益，确定后验估计值和更新误差协方差矩阵；

S34：将后验估计加速度带入虚拟力导向合力公式，动态调节虚拟力导向合力公式中的系数控制无人机移动到后验估计中最优加速度位置，并计算抖动范围均值；

S35：不断计算更新抖动范围均值，使平均抖动范围随着时间趋于零，最终抖动消除。

进一步，所述S1中当时间t趋近于无穷时，若ρ为常数，将观察到无人机抖动现象，并且抖动不会消除。

进一步，所述S32包括以下步骤：

S321：建立卡尔曼滤波模型，对每层无人机的加速度进行卡尔曼滤波；

设在第i层的无人机在k时刻的加速度为a_i(k)，速度为v_i(k)，得到k时刻的状态矩阵：

v₁(k)＝v₁(k-1)+a₁(k-1)×t+w_1,1(k-1)

a₁(k)＝u₁(k)+w_1,2(k-1)

v_M(k)＝v_M(k-1)+a_M(k-1)×t+w_M,1(k-1)

a_M(k)＝u_M(k)+w_M,2(k-1)

其中，u(k)为无人机k时刻产生的加速度，w(k)为其他因素产生的干扰加速度；

S322：测量第i层无人机在k时刻的加速度及速度作为测量方程的初始值，得到k时刻的测量矩阵：

z_1,1＝v₁(k)+r_1,1(k)

z_1,2＝a₁(k)+r_1,2(k)

z_M,1＝v_M(k)+r_M,1(k)

z_M,2＝a_M(k)+r_M,2(k)；

其中，r_i,1(k),r_i,2(k)为测量噪声；

S323：根据S321得到卡尔曼滤波状态方程：

X(k)＝A·X(k-1)+U_i(k)+W_i(k-1)；

其中，A为状态转移矩阵：U_i(k)为系统输入矩阵：/>W(k)为系统的过程噪声矩阵：/>设其符合正太分布p(w)～N(0,Q)；

S324：卡尔曼滤波模型状态建好后，确定先验估计和先验估计的协方差矩阵：

S325：以t为时间间隔，通过测量计算无人机节点运动的加速度及速度，将计算所得结果作为测量方程初始值；根据S22，得到卡尔曼滤波测量方程：

Z(k)＝H·X(k)+R(k)；

其中，H为测量方程的测量矩阵，是单位矩阵，为测量噪声，其符合正态分布，p(r)～N(0,R)。

进一步，所述S33具体为：

根据S32计算卡尔曼增益K'，确定后验估计值和更新误差协方差矩阵，获得相对精确的后验估计加速度及速度；所述卡尔曼增益K'具体为：

进一步，所述S35具体为：

将无人机调整到后验估计的加速度位置后，将打破在引力和斥力均衡条件下，位移抖动不停的状态，最终加速度将逐渐收敛减小，抖动范围随着时间增长逐渐减小，随着时间的增加，一直更新平均抖动范围判断最后是否满足/>使平均抖动范围/>实现抖动消除。

以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (6)

1.基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，其特征在于：包括以下步骤：

S1将无人机编队节点放入三维坐标内形成一个球形的簇集合，在球形簇中根据半径进行层级划分，同一层内的无人机加速度相同且离中心节点越远加速度越大，每架无人机在时间t内抖动范围为Δ＝αt，其中α为加速度；确定无人机的抖动合集Δ＝{Δ₀,Δ₁,...Δ_i...,Δ_N}，N为无人机节点总数，计算平均抖动范围为

S2对无人机集群编队中的每个无人机节点计算出引力和斥力综合的合力得到虚拟力导向合力公式：

F_合＝γ×F_引+λ×F_斥，

其中，γ表示引力调节系数，λ表示斥力调节系数；

S3将无人机的运动速度和加速度作为初始值，建立卡尔曼滤波模型，对每层无人机加速度进行卡尔曼滤波得到球形簇中每层无人机的后验估计加速度，通过所述虚拟力导向合力公式动态选择引力调节系数和/或斥力调节系数来控制球形簇中每层无人机达到所述后验估计加速度值，并计算无人机平均抖动范围；不断利用卡尔曼滤波更新后验估计加速度并调整引力调节系数和/或斥力调节系数，直到平均抖动范围抖动消除。

2.如权利要求1所述的基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，其特征在于，所述S3包括以下步骤：

S31：基于合力计算球形簇集合每层无人机的加速度及速度，并将计算所得结果作为卡尔曼滤波的初始值；

S32：确定每层无人机在k时刻的状态矩阵、测量矩阵，并根据所述状态矩阵得到卡尔曼滤波的状态方程，根据所述状态方程确定先验估计和先验估计的协方差矩阵，并根据测量矩阵得到卡尔曼滤波测量方程；

S33：根据S32计算卡尔曼增益，确定后验估计值和更新误差协方差矩阵；

S34：将后验估计加速度带入虚拟力导向合力公式，动态调节虚拟力导向合力公式中的系数控制无人机移动到后验估计中最优加速度位置，并计算抖动范围均值；

S35：不断计算更新抖动范围均值，使平均抖动范围随着时间趋于零，最终抖动消除。

3.如权利要求1所述的基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，其特征在于：所述S1中当时间t趋近于无穷时，若ρ为常数，将观察到无人机抖动现象，并且抖动不会消除。

4.如权利要求2所述的基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，其特征在于：所述S32包括以下步骤：

S321：建立卡尔曼滤波模型，对每层无人机的加速度进行卡尔曼滤波；

设在第i层的无人机在k时刻的加速度为a_i(k)，速度为v_i(k)，得到k时刻的状态矩阵：

其中，u(k)为无人机k时刻产生的加速度，w(k)为其他因素产生的干扰加速度；

S322：测量第i层无人机在k时刻的加速度及速度作为测量方程的初始值，得到k时刻的测量矩阵：

其中，r_i,1(k),r_i,2(k)为测量噪声；

S323：根据S321得到卡尔曼滤波状态方程：

X(k)＝A·X(k-1)+U_i(k)+W_i(k-1)；

其中，A为状态转移矩阵：U_i(k)为系统输入矩阵：/>W(k)为系统的过程噪声矩阵：/>设其符合正太分布p(w)～N(0,Q)；

S324：卡尔曼滤波模型状态建好后，确定先验估计和先验估计的协方差矩阵：

S325：以t为时间间隔，通过测量计算无人机节点运动的加速度及速度，将计算所得结果作为测量方程初始值；根据S22，得到卡尔曼滤波测量方程：

Z(k)＝H·X(k)+R(k)；

其中，H为测量方程的测量矩阵，是单位矩阵，为测量噪声，其符合正态分布，p(r)～N(0,R)。

5.如权利要求2所述的基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，其特征在于：所述S33具体为：

根据S32计算卡尔曼增益K'，确定后验估计值和更新误差协方差矩阵，获得相对精确的后验估计加速度及速度；所述卡尔曼增益K'具体为：

6.如权利要求2所述的基于卡尔曼滤波的虚拟力导向无人机编队抖动消除方法，其特征在于：所述S35具体为：

将无人机调整到后验估计的加速度位置后，将打破在引力和斥力均衡条件下，位移抖动不停的状态，最终加速度将逐渐收敛减小，抖动范围随着时间增长逐渐减小，随着时间的增加，一直更新平均抖动范围判断最后是否满足/>使平均抖动范围/>实现抖动消除。