用于无人机蜂群协同导航的动态互观测在线建模方法与流程

本发明涉及用于无人机蜂群协同导航的动态互观测在线建模方法，属于无人机蜂群协同导航技术领域。

背景技术：

无人机蜂群是国内外近年来提出的新概念，即多架无人机为适应任务要求而进行的三维空间排列和任务分配的组织模式，它既包括编队飞行的队形产生、保持和重组，也包括飞行任务的组织，可以根据外部情况和任务需求进行动态调整。

传统组合导航系统模型主要是基于固定参考坐标系和固定性能的量测信息，而无人机蜂群在飞行过程中各成员的相对位置和定位性能处于不断变化过程中，各成员在蜂群协同导航中是作为被辅助的对象节点还是辅助的参考节点的角色也不断变化，传统组合导航模型无法适应无人机蜂群协同导航需求。

因此，研究基于移动参考坐标系并考虑成员间观测关系、自身定位性能和协同导航中角色变化的动态互观测模型和建模方法，将能够有效地实现协同导航过程中互观测信息的自适应模型描述，为无人机蜂群发挥自主协同优势提供支持。

技术实现要素：

本发明所要解决的技术问题是：提供用于无人机蜂群协同导航的动态互观测在线建模方法，在移动参考坐标系下考虑成员间观测关系、自身定位性能和协同导航中角色变化，建立动态互观测模型并进行优化，为有效实现协同导航提供准确依据。

本发明为解决上述技术问题采用以下技术方案：

用于无人机蜂群协同导航的动态互观测在线建模方法，包括如下步骤：

步骤1，对无人机蜂群中的每个成员进行编号并表示为1,2,…,n，按照当前时刻各成员机载卫星导航接收机接收到可用星数量，对成员进行第一级筛选，确定各成员在协同导航中的角色：设接收到可用星数量小于4的成员为对象成员，将对象成员编号集合记为a；设接收到可用星数量不小于4的成员为备选参考成员，将备选参考成员编号集合记为b；且

步骤2，获取对象成员i机载导航系统指示位置，并以该指示位置为原点，建立该对象成员当地东北天地理坐标系，i表示成员编号且i∈a；

步骤3，获取备选参考成员j机载导航系统指示位置及其定位误差协方差，并将备选参考成员j机载导航系统指示位置及其定位误差协方差均转换到步骤2建立的对象成员i当地东北天地理坐标系中，j表示成员编号且j∈b；

步骤4，按照每个对象成员与每个备选参考成员之间是否可以相互测距，对备选参考成员进行第二级筛选，确定各备选参考成员在协同导航中的角色：设与对象成员i可以相互测距的备选参考成员为对象成员i的可用参考成员，将对象成员i的可用参考成员编号集合记为ci，且

步骤5，计算对象成员与其可用参考成员的互观测矢量，并根据互观测矢量计算对象成员与其可用参考成员的矢量投影矩阵；

步骤6，计算对象成员与其可用参考成员的对象位置投影矩阵以及可用参考位置投影矩阵；

步骤7，利用步骤5获得的矢量投影矩阵和步骤6获得的对象位置投影矩阵，计算对象成员与其可用参考成员之间状态互观测矩阵；

步骤8，利用步骤5获得的矢量投影矩阵和步骤6获得的可用参考位置投影矩阵，计算对象成员与其可用参考成员之间噪声互观测矩阵；利用噪声互观测矩阵，计算对象成员与其可用参考成员之间互观测噪声协方差；

步骤9，利用步骤7获得的状态互观测矩阵，建立对象成员对其全部可用参考成员的互观测集合矩阵；

步骤10，利用步骤8获得的互观测噪声协方差，建立对象成员对其全部可用参考成员的互观测集合协方差；

步骤11，利用步骤5获得的互观测矢量，建立对象成员对其全部可用参考成员的互观测集合观测量；

步骤12，根据步骤9获得的互观测集合矩阵、步骤10获得的互观测集合协方差以及步骤11获得的互观测集合观测量，建立无人机蜂群协同导航的动态互观测模型，根据动态互观测模型进行对象成员加权最小二乘定位，得到对象成员位置的经度修正量、纬度修正量、高度修正量，并计算修正的经度、纬度、高度；

步骤13，利用步骤7获得的状态互观测矩阵和步骤8获得的互观测噪声协方差，计算对象成员位置估计协方差；

步骤14，利用步骤6获得的对象位置投影矩阵和步骤12得到的对象成员位置的经度修正量、纬度修正量、高度修正量，计算在线建模误差量；当在线建模误差量小于事先设置的动态互观测在线建模误差控制标准时，判定在线建模迭代收敛，即在线建模结束并转入步骤15，否则返回步骤5对互观测模型进行迭代修正；

步骤15，判断是否导航结束，如是则结束；否则返回步骤1进行下一时刻建模。

作为本发明的一种优选方案，步骤5所述互观测矢量，表达式为：

其中，为对象成员i与其可用参考成员k的互观测矢量，分别为在对象成员i当地东北天地理坐标系东、北、天向的分量，δλik、δlik、δhik分别为对象成员i与其可用参考成员k机载导航系统输出经度、纬度、高度之差，rn为地球参考椭球卯酉圈曲率半径，f为地球参考椭球扁率，li、hi分别为对象成员i机载导航系统输出的纬度、高度。

作为本发明的一种优选方案，步骤5所述矢量投影矩阵，表达式为：

其中，为对象成员i与其可用参考成员k的矢量投影矩阵，分别为在对象成员i当地东北天地理坐标系东、北、天向的分量，为对象成员i与其可用参考成员k的互观测矢量，dik为对象成员i与其可用参考成员k之间的距离计算值，

作为本发明的一种优选方案，步骤6所述对象位置投影矩阵，表达式为：

其中，为对象成员i与其可用参考成员k的对象位置投影矩阵，δλik、δlik分别为对象成员i与其可用参考成员k机载导航系统输出经度、纬度之差，li、hi分别为对象成员i机载导航系统输出的纬度、高度，rn为地球参考椭球卯酉圈曲率半径。

作为本发明的一种优选方案，步骤6所述可用参考位置投影矩阵，表达式为：

其中，为对象成员i与其可用参考成员k的可用参考位置投影矩阵，li、hi分别为对象成员i机载导航系统输出的纬度、高度，rn为地球参考椭球卯酉圈曲率半径。

作为本发明的一种优选方案，步骤7所述状态互观测矩阵，表达式为：

其中，为对象成员i与其可用参考成员k的状态互观测矩阵，为对象成员i与其可用参考成员k的矢量投影矩阵，为对象成员i与其可用参考成员k的对象位置投影矩阵。

作为本发明的一种优选方案，步骤8所述噪声互观测矩阵，表达式为：

其中，为对象成员i与其可用参考成员k的噪声互观测矩阵，为对象成员i与其可用参考成员k的矢量投影矩阵，为对象成员i与其可用参考成员k的可用参考位置投影矩阵。

作为本发明的一种优选方案，步骤8所述互观测噪声协方差，表达式为：

其中，为对象成员i与其可用参考成员k的互观测噪声协方差，为对象成员i与其可用参考成员k的噪声互观测矩阵，表示相对测距传感器的误差协方差，表示可用参考成员k定位误差协方差。

作为本发明的一种优选方案，步骤14所述在线建模误差量，表达式为：

其中，为对象成员i与其可用参考成员k的在线建模误差量，为对象成员i与其可用参考成员k的对象位置投影矩阵，分别为对象成员i位置的经度修正量、纬度修正量、高度修正量。

本发明采用以上技术方案与现有技术相比，具有以下技术效果：

1、本发明考虑了无人机蜂群飞行过程中各成员导航性能的动态变化，通过动态筛选确定各成员在协同导航中的角色，达到优选高定位性能成员辅助低定位性能成员的目的，避免固定角色模式下建模适应性差的问题。

2、本发明考虑了参考成员之间定位性能的差异，通过综合参考成员定位误差和测距传感器测量误差并引入加权迭代，提高了建模精度。

3、本发明灵活性强，适应不同规模的无人机蜂群和成员间不同相互位置关系和分布下的互观测条件。

附图说明

图1是本发明用于无人机蜂群协同导航的动态互观测在线建模方法的流程图。

图2是采用本发明方法构建的对象成员移动坐标系进行迭代建模的曲线图。

图3是采用本发明方法进行迭代建模的位置误差曲线图。

图4是采用本发明方法进行迭代建模的经度、纬度、高度误差曲线图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本发明提供用于无人机蜂群协同导航的动态互观测在线建模方法，为无人机蜂群协同导航提供有效支持，提高了协同导航建模的灵活性和精度，方案如图1所示，包括以下步骤：

(1)设无人机蜂群中的成员数量为n，对其成员进行编号并表示为1,2,…,n，n为所有成员数量，设置动态互观测在线建模误差控制标准ζ。

(2)按照当前时刻无人机蜂群各成员机载卫星导航接收机接收到可用星数量，对成员进行第一级筛选，确定各成员在协同导航中的角色：设接收到可用星数量小于4的成员为对象成员，将对象成员编号集合记为a；设接收到可用星数量不小于4的成员为备选参考成员，将备选参考成员编号集合记为b；且

(3)获取步骤(2)分类中对象成员机载导航系统指示位置，并以该指示位置为原点，建立该对象成员当地东北天地理坐标系；记对象成员i机载导航系统指示位置为(λi,li,hi)，对应建立的当地东北天坐标系表示为oixyz，其中λ表示经度，l表示纬度，h表示高度，其中i表示成员编号且i∈a。

(4)获取步骤(2)分类中备选参考成员机载导航系统指示位置及其定位误差协方差，并将其转换到步骤(3)建立的对象成员当地东北天地理坐标系中；记备选参考成员j机载导航系统指示位置为(λj,lj,hj)，其中j表示成员编号且j∈b。

(5)依次按照每个对象成员与每个备选参考成员之间是否可以相互测距，对备选参考成员进行第二级筛选，确定各备选参考成员在协同导航中的角色：设与对象成员i可以相互测距的备选参考成员为对象成员i的可用参考成员，将对象成员i的可用参考成员编号集合记为ci，且

(6)计算对象成员与其可用参考成员的互观测矢量，记对象成员i与其可用参考成员k的互观测矢量为其表达式为：

其中i、k表示成员编号且i∈a、k∈ci，δλik为对象成员i与其可用参考成员k机载导航系统输出经度之差，δlik为对象成员i与其可用参考成员k机载导航系统输出纬度之差，δhik为对象成员i与其可用参考成员k机载导航系统输出高度之差，rn为地球参考椭球卯酉圈曲率半径，为常数；f为地球参考椭球扁率，为常数；li为对象成员i机载导航系统输出的纬度，hi为对象成员i机载导航系统输出的高度。

(7)利用步骤(6)获得的对象成员与其可用参考成员的互观测矢量，计算矢量投影矩阵；记对象成员i与其可用参考成员k的矢量投影矩阵为其表达式为：

其中，dik为对象成员i与其可用参考成员k之间的距离计算值，其表达式为

(8)计算对象位置投影矩阵；记对象成员i与其可用参考成员k的对象位置投影矩阵为其表达式为：

(9)计算可用参考位置投影矩阵；记对象成员i与其可用参考成员k的可用参考位置投影矩阵为其表达式为：

(10)利用步骤(7)获得的矢量投影矩阵和步骤(8)获得的对象位置投影矩阵，计算对象成员与其可用参考成员之间状态互观测矩阵；记对象成员i与其可用参考成员k的状态互观测矩阵为其表达式为：

(11)利用步骤(7)获得的矢量投影矩阵和步骤(9)获得的可用参考位置投影矩阵，计算对象成员与其可用参考成员之间噪声互观测矩阵；记对象成员i与其可用参考成员k的噪声互观测矩阵为其表达式为：

(12)利用步骤(11)获得的噪声互观测矩阵，计算对象成员与其可用参考成员之间互观测噪声协方差，其表达式为：

其中表示相对测距传感器的误差协方差；表示可用参考成员k定位误差协方差。

(13)利用步骤(10)获得对象成员i与其可用参考成员k的状态互观测矩阵为建立无人机蜂群各成员的互观测集合矩阵；记对象成员i对其全部可用参考成员的互观测集合矩阵为其表达式为：

为所有满足k∈ci的作为行向量组成的矩阵。

(14)利用步骤(12)获得的对象成员与其可用参考成员之间互观测噪声协方差，建立无人机蜂群各成员的互观测集合协方差；记对象成员i对其全部可用参考成员的互观测集合协方差为其表达式为：

其中为所有满足k∈ci的作为对角线元素，且非对角线元素为0的矩阵。

(15)利用(6)获得的对象成员与其可用参考成员的互观测矢量，建立无人机蜂群各成员的互观测集合观测量；记对象成员i对其全部可用参考成员的互观测集合观测量为其表达式为：

其中，dik为对象成员i与其可用参考成员k之间的距离计算值，其表达式为为对象成员i与其可用参考成员k之间的距离测量值。

(16)利用步骤(13)获得的对象成员i对其全部可用参考成员的互观测集合矩阵步骤(14)获得的对象成员i对其全部可用参考成员的互观测集合协方差为步骤(15)获得的对象成员i对其全部可用参考成员的互观测集合观测量为组成无人机蜂群协同导航的动态互观测模型，进行对象成员加权最小二乘定位，得到对象成员i位置的经度修正量纬度修正量高度修正量

(17)利用步骤(16)得到的对象成员i的经度修正量纬度修正量高度修正量计算修正的经度、纬度、高度，其表达式为：

(18)利用步骤(10)获得的对象成员与其可用参考成员之间状态互观测矩阵，步骤(12)获得的对象成员与其可用参考成员之间互观测噪声协方差，计算对象成员位置估计协方差；记对象成员i位置估计协方差为σpi，其表达式为：

(19)利用步骤(8)获得的对象位置投影矩阵和步骤(16)得到的对象成员i的经度修正量纬度修正量高度修正量计算在线建模误差量，其表达式为：

(20)判断在线建模迭代是否收敛，如则判定为收敛，在线建模结束并转入步骤(21)；否则返回步骤(6)对互观测模型进行迭代修正。

(21)判断是否导航结束，如是则结束；否则返回步骤(2)进行下一时刻建模。

为了验证本发明所提出的用于动态观测关系条件的无人机蜂群协同导航方法的有效性，进行数字仿真分析。仿真中采用的无人机蜂群中无人机数量为8架，相对距离测量精度为0.1米。图1是本发明用于无人机蜂群协同导航的动态互观测建模方法的方案图；图2是采用本发明方法构建的对象成员移动坐标系进行迭代建模的曲线图；图3是采用本发明方法进行迭代建模的位置误差曲线图；图4是采用本发明方法进行迭代建模的经度、纬度、高度误差曲线图。

由图2可以看出，采用本发明所提出的用于无人机蜂群协同导航的互观测模型与在线建模方法后，无人机蜂群中对象成员的计算位置逐渐初始位置收敛接近于真实位置；由图3可以看出，本发明所提出的用于无人机蜂群协同导航的互观测模型与在线建模方法后对象成员的位置误差逐渐减小，最终计算得到的位置误差较初始误差降低4个数量级；由图3可以看出，本发明所提出的用于无人机蜂群协同导航的互观测模型与在线建模方法后经度、纬度、高度方向误差均逐渐减小。此外，采用本发明方法能够适应无人机蜂群在飞行过程中互观测关系和成员角色的不断变化，具有良好的应用价值。

以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。