[0001] 本发明涉及导航的技术领域，尤其涉及一种复杂环境下自适应协同导航方法。

[0002] 多无人机协同导航利用共享测量和信息交换，可以实现平台间导航资源的共享，从而获得比各平台单体导航更优的性能。协同导航可以通过直接或间接相对观测进行导航共享，例如基于相对距离/角度测量的协同导航或基于无人机对公共路标点观测的协同导航。但是不同环境条件下，导航传感器的精度不同，如果不加区分的使用所有导航信息源进行协同导航，反而会造成导航精度下降。

[0003] 本专利中一辆或多辆在不易受GNSS信号破坏的地区飞行的无人机，为长机，通过广播其定位信息并通过相对传感(通过视觉和/或射频测距技术)进行协同，来支持僚机在具有挑战性的环境下的自主导航。无人机搭载着GNSS、视觉、磁力计、测距模块和惯性导航。其中基于GNSS/视觉的组合，将多天线GNSS姿态估计的概念扩展到多无人机场景，用于提高姿态估计精度。结合磁力计、惯性导航和测距模块，提高无人机导航的鲁棒性，在GNSS挑战环境下通过自适应调整导航滤波器使用的协同测量信息，保持定位精度。此外，无人机编队几何形状取决于当前的协同导航模式，例如无人机间距离的增加通常有利于基于协同的姿态估计，但增加了视觉的定位不确定性。广义精度因子(geDOP)能够预测可实现的定位精度，有助于规划无人机编队的几何形状。

[0041] 为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合说明书附图对本发明的具体实施方式做详细的说明，显然所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的保护的范围。

[0042] 在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是本发明还可以采用其他不同于在此描述的其它方式来实施，本领域技术人员可以在不违背本发明内涵的情况下做类似推广，因此本发明不受下面公开的具体实施例的限制。

[0043] 其次，此处所称的“一个实施例”或“实施例”是指可包含于本发明至少一个实现方式中的特定特征、结构或特性。在本说明书中不同地方出现的“在一个实施例中”并非均指同一个实施例，也不是单独的或选择性的与其他实施例互相排斥的实施例。

[0044] 本发明结合示意图进行详细描述，在详述本发明实施例时，为便于说明，表示器件结构的剖面图会不依一般比例作局部放大，而且所述示意图只是示例，其在此不应限制本发明保护的范围。此外，在实际制作中应包含长度、宽度及深度的三维空间尺寸。

[0045] 同时在本发明的描述中，需要说明的是，术语中的“上、下、内和外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一、第二或第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

[0046] 本发明中除非另有明确的规定和限定，术语“安装、相连、连接”应做广义理解，例如：可以是固定连接、可拆卸连接或一体式连接；同样可以是机械连接、电连接或直接连接，也可以通过中间媒介间接相连，也可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

[0047] 实施例1

[0048] 参照图1—2，为本发明的一个实施例，该实施例提供了一种复杂环境下自适应协同导航方法，包括：

[0049] S1：根据全球卫星导航系统接收机输出的精度因子，判断无人机当前的精度，分为长机和僚机；

[0050] 更进一步的，长机为无人机集群中在全球卫星导航系统信号良好地区飞行的无人机，僚机为在具有挑战性的环境下飞行的无人机。无人机集群协同导航通过长、僚机之间的相对观测进行导航共享，从而获得比各平台单体导航更优的性能。

[0051] S2：根据僚机的精度因子值选择使用全球卫星导航系统相关测量和无关测量进行协同导航滤波，融合导航信息，计算僚机导航信息误差；

[0052] 更进一步的，协同导航通过长机和僚机之间的相对测量，并使用滤波器估计僚机的导航状态。滤波器的状态量包括僚机位置误差、速度误差和姿态误差以及陀螺仪和加速度计的零偏；

[0053] 滤波器的观测量包括全球卫星导航系统和磁力计残差以及长僚机之间的相对测量，观测量表示为：

[0054] δy＝[δyGNSS δyM δy1 … δyJ]T

[0055] 其中，δyGNSS为全球卫星导航系统残差，δyM为磁力计残差，δyj,j＝1,2,…,J为僚机与第j个长机之间的协同测量信息。

[0056] 更进一步的，将协同导航时的长、僚机之间的相对观测分为全球卫星导航系统相关测量和无关测量两部分。

[0057] 相关测量为全球卫星导航系统视觉信息，利用长机的相对方向改进僚机姿态估计，视觉信息表示为：

[0058]

[0059] 其中， 是相机测量的从僚机到第j长机在载体坐标系b系下的向量， 是GNSS测量的从僚机到第j长机在导航坐标系n系下的向量， 为n系到b系的旋转矩阵，为通过旋转矩阵将GNSS测量的向量转到b系，相机测量信息和GNSS测量信息的差值。

[0060] 无关测量量用于全球卫星导航系统挑战的环境中，实现有界导航，无关测量量表示为：

[0061]

[0062] 其中， δyj,RF为测距模块估计长、僚机距离的误差，Az和El表示摄像机成像的无人机的方位角和水平角，l→z，表示从僚机到长机，z→l表示从长机到僚机，lc表示僚机的相机坐标系；

[0063] 无关测量量还包括：

[0064]

[0065]

[0066]

[0067]

[0068]

[0069] 其中， 为第j个长机的位置误差， 是第j个长机的姿态误差，δcam为相机精度，δRF为测距精度。

[0070] 更进一步的，当接收的全球卫星导航系统卫星数量减少时，或者当卫星结构较差时，全球卫星导航系统精度会下降；当信号接收良好时，无关测量对导航滤波器的校正效果十分微弱；

[0071] 通过基于精度因子的准则自适应地使用相关测量或无关测量，每个时间点的无人机基于当前的卫星实时估计精度因子，当精度因子低于阈值λ时，滤波器使用相关测量，当精度因子高于阈值λ时，滤波器使用无关测量。

[0072] S3：通过无人机当前的精度确定无人机集群几何结构。

[0073] 更进一步的，无人机集群几何结构，包括：

[0074] 根据三维环境的先验知识和当前全球卫星导航系统的几何位置，预测无人机的覆盖条件，以长机处于低精度条件下最小化广义精度因子为原则，确定最有利于保持导航精度的无人机集群几何结构。

[0075] 实施例2

[0076] 参照图3，为本发明的一个实施例，该实施例提供了一种复杂环境下自适应协同导航方法，为了证明本发明的有益效果，通过具体试验进行科学论证。

[0077] 本实施例通过Matlab进行仿真分析验证，仿真设置一个长机一个僚机，两架无人机等高面飞行，仿真条件如下：

[0078] 1、长机初始位置分别为东向10m、北向0m、天向50m，初始姿态角分别为横摇角φE°；＝0°、俯仰角φN＝0°、航向角φU＝45

[0079] 僚机初始位置分别为东向0m、北向10m、天向50m，初始姿态角分别为俯仰角φE＝0°、横滚角φN＝0°、航向角φU＝30°。

[0080] 2、陀螺仪测量三轴零偏为1°/h，加速度计测量三轴零偏为1mg。

[0081] 3、仿真时间1小时，在第2400秒时GNSS接收信号变差，仿真频率100Hz。

[0082] 仿真结果如图3所示，由图中可以看出，点线为不依靠协同导航的情况，僚机定位结果较差；虚线为仅依靠GNSS相关测量进行协同导航且不对编队进行规划的情况，僚机定位结果有了一定改善；实线为仅依靠GNSS相关测量进行协同导航但对编队进行规划的情况，僚机定位结果相较蓝色虚线有一点改善，但不明显；点划线为自适应地采用GNSS相关测量或GNSS无关测量进行协同导航但不进行编队规划的情况，僚机定位结果相较于仅依靠GNSS相关测量的协同导航有了较明显的改善；粗实线为自适应地采用GNSS相关测量或GNSS无关测量进行协同导航且进行编队规划的情况，僚机定位效果最好。由仿真试验验证了本专利提出的自适应协同导航及编队规划方法的有效性。

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