CN115267869A - Gps拒止条件下的无人机群psd测距相对定位方法 - Google Patents

Abstract

本发明公开了一种GPS拒止条件下的无人机群PSD测距相对定位方法，针对主从式的无人机编队结构和位置敏感检测器(PSD)工作特性，实现GPS拒止条件下的无人机群PSD测距相对定位。PSD具有灵敏度高、分辨率高，实时性强和抗干扰性好等优点，适用于多种位置检测系统。本发明通过两自由度转台测量从机相对主机的角度，采用PSD三角测距获得主机与从机间的距离，并通过三角回环优化提高测距精度，实现了从机相对于主机的相对定位与从机在机群中的定位，有助于无人机群在GPS拒止条件下的实时性导航定位。

Description

GPS拒止条件下的无人机群PSD测距相对定位方法

技术领域

本发明涉及无人机蜂群协同导航定位技术领域，更具体的说是涉及一种GPS拒止条件下的无人机群PSD测距相对定位方法。

背景技术

近年来，无人机的应用价值愈发凸显、产业规模持续扩大、市场潜力不断释放，相关技术研究受到了越来越多国家的重视。其中，“无人机蜂群”的技术更是各方研究关注的一大焦点。

传统的单无人机作业时受到体型、成本和续航得限制，导致功能单一，稳定性无法保证，执行任务效率低、失败率高。相比之下，无人机群作业优势突出，一方面集群中可通过协同优化任务分配，携带不同的设备，在降低成本的同时提高任务效率；另一方面，集群作业大大提高了系统的准确性、全面性和可靠性，基于此可看出无人机蜂群拥有更多的应用价值和空间，当前在军事和民用领域都已有广泛应用。

现阶段，随着科学技术的发展，无人机集群智能已逐渐成为保障国防安全的重要手段之一，而集群的定位导航又是保证无人机群能按照指定路线飞行，顺利完成任务的重要技术。对于主从式无人机群，为降低导航成本优化任务分配，主机携带导航设备精度较高，从机导航设备简陋，在实际应用中的位置信息主要由全球定位系统(GPS)提供，但GPS属于非自主定位系统，信号易受到干扰和屏蔽，在GPS拒止的情况下从机无法对自身进行定位，此时就需要利用主机对从机进行相对定位，从而保证整个机群编队能够安全稳定飞行。

目前，国内外学者在自主相对定位领域已取得了丰富成果，并提出了一系列方法，如激光脉冲测距定位、UWB测距定位、视觉测距定位、超声波测距定位以及无线电测距定位等，被广泛应用到各个领域。然而，上述方法都存在一定的局限，无法很好地满足无人机蜂群内的相对定位需求，如UWB测距定位成本高昂，稳定性差，还可能干扰其他无线通讯；视觉测距定位图像计算任务繁重，实时性不足且受外界光环境影响大；超声波测距定位采集速度慢，适用范围较小；无线电测距定位易被干扰，可靠性差。这些方法用于无人机群中主机对从机的相对定位时局限较多，因此就需要一种模式简单、精度可靠、响应速度快的全自主机间测距相对定位方法来保证无人机蜂群在GPS拒止环境下的正常运作。

因此，如何提高无人机集群全自主机间距相对定位的精度和响应速度，并降低定位复杂度是本领域技术人员亟需解决的问题。

发明内容

有鉴于此，本发明提供了一种GPS拒止条件下的无人机群PSD测距相对定位方法，提高了主从式无人机群中在GPS拒止条件下，从机的定位精度和定位响应速度。

为了实现上述目的，本发明采用如下技术方案：

一种GPS拒止条件下无人机群PSD测距相对定位方法，包括以下步骤：

步骤1：无人机群中主机数量大于或等于三架，每架主机搜索各自对应的目标从机，向目标从机发送若干束激光信号；主机在水平和竖直两平面内自由旋转两自由度转台，通过组合在两自由度转台上的激光器在每个旋转角度发射激光信号搜寻目标从机；

步骤2：目标从机接收主机发来的每束激光信号，通过灰度质心法获取激光信号的激光光斑质心对应的差分输出电压；

步骤3：根据差分输出电压，并结合PSD输出电压与激光光斑质心成像点坐标之间的线性关系，计算获得目标从机的PSD成像点在PSD上的位置坐标，即激光光斑质心在PSD上的位置坐标；

步骤4：根据目标从机的两点位置坐标进行三角测距；

步骤5：任选三架主机，根据三架主机之间的距离利用三角回环优化，减小主机的测距误差，返回所述步骤1从而获得目标从机精确测距结果；

步骤6：获取主机坐标，并结合主机坐标和机群飞行动向构建群体坐标系，根据目标从机的位置坐标和精确测距结果计算目标从机在所述群体坐标系中的相对位置坐标，实现相对定位。

优选的，所述步骤2的具体过程包括：

步骤21：目标从机PSD接收激光信号，计算激光光斑图像中的所有像素的灰度值之和，表达式为：

其中，G_all表示所有像素的灰度值之和；(i,j)分别对应各光斑图像中个像素点坐标；I(i,j)表示对应像素点的灰度值；

步骤22：分别计算激光光斑图像中每个像素与其对应的x坐标的乘积之和、y坐标的乘积之和，表达式分别为：

其中，G_x为像素对应的x坐标的乘积之和；G_y为像素对应的y坐标的乘积之和；

步骤23：利用激光光斑图像中像素的x坐标的乘积之和、y坐标的乘积之和与所有像素的灰度值之和，获得激光光斑质心的坐标，表达式为：

其中，x、y分别为激光光斑质心的横坐标和纵坐标；

步骤24：将激光光斑质心的坐标转换成差分电压，获得差分输出电压，横坐标对应差分电压V_x+和V_x-，纵坐标对应差分电压V_y+和V_y-。

优选的，步骤3的具体实现过程为：

步骤31：根据差分输出电压和PSD工作原理，得到激光光斑质心成像点坐标理论电压，表达式为：

其中，V_x+和V_x-为激光光斑质心横坐标对应差分电压，V_y+和V_y-为激光光斑质心纵坐标对应差分电压；V_x为质心成像点横坐标理论电压；V_y为质心成像点纵坐标理论电压；

步骤32：根据PSD输出电压与质心成像点坐标的线性关系，获得当前光斑质点位置坐标，表达式为：

X＝a_xV_x+b_x (7)

Y＝a_yV_y+b_y (8)

其中，X、Y分别为质心成像点的横坐标输出电压线性拟合表达式和纵坐标输出电压线性拟合表达式；a_x、b_x、a_y和b_y为表达式系数，表示为：

其中，X_i，Y_i分别代表各位置处激光光斑质心的横坐标与纵坐标；V_xi是各横坐标对应的电压输出，与X_i一一对应；V_yi是各纵坐标对应的电压输出，与Y_i一一对应。

优选的，步骤4中主机的两自由度旋转台任意转动角度，只要当前目标从机接收到激光信号，则经过步骤2-步骤3均获得一个新的位置坐标，同一架目标从机的任意两点位置坐标与之间形成一个三角形，进行三角测距，获得当前目标从机与主机之间的测距结果。

优选的，三角测距的具体过程为：

步骤41：两点位置坐标之间的距离为：

其中，(x₁,y₁)和(x₂,y₂)分别为两点位置坐标；

步骤42：主机发射激光信号的位置点(即激光器)到两位置坐标中点的距离为主机与当前目标从机之间的距离，表达式为：

其中，L为测距结果，θ为前后两束激光信号对应的光路间的夹角，即转台转动角度。

优选的，步骤5中进行三角回环优化的具体过程为：

任意选取三架主机，任意两架主机之间的间距关系表示为：

其中，任意两架主机之间的间距分别为L₁+δ₁、L₂+δ₂和L₃+δ₃，δ₁、δ₂和δ₃均为误差值；θ₁、θ₂和θ₃分别为间距之间对应的夹角；

将公式(13)中的最后一个等式展开，获得：

L₁ ²+L₂ ²-L₃ ²+2L₁δ₁+2L₂δ₂-2L₃δ₃+δ₁ ²+δ₂ ²-δ₃ ²＝0 (14)

δ₁ ²、δ₂ ²、δ₃ ²是高阶小误差量；将其忽略以满足公式(13)求解条件，因此简化为：

L₁ ²+L₂ ²-L₃ ²+2L₁δ₁+2L₂δ₂-2L₃δ₃＝0 (15)

将式(13)和式(15)联立可得：

其中，C₁、C₂、C₃均为中间参数，表达式如下：

C₁＝tanθ₁ (17)

C₂＝1/sinθ₂ (18)

C₃＝L₁ ²-L₂ ²+L₃ ²+2C₁L₁L₂-2C₂L₁L₃ (19)

将δ₁、δ₂和δ₃的结果反代回L₁+δ₁、L₂+δ₂和L₃+δ₃，则减小主机间的测距误差。

优选的，步骤6的具体过程为：

步骤61：将主机位置作为坐标原点，以机群飞行方向为y轴正方向，沿地垂线指向天为中z轴正方向，机群右侧为x轴正方向，构成右手笛卡尔坐标系，作为群体坐标系；

步骤62：在无人机群的群体导航系下，目标从机相对于主机的相对位置坐标为：

其中，(x_uw,y_uw,z_uw)为群体导航系下的相对位置坐标；L为目标从机与主机的测量距离；α和β分别为目标从机的方位角和俯仰角，在搜寻目标从机过程中已通过主机所携带的两自由度转台获得；

步骤63：根据群体导航系下的主机坐标，以及主机与目标从机的相对位置坐标，将目标从机位置坐标转换到群体坐标系下：

其中，(x_u,y_u,z_u)为主机在群体导航系下的坐标；(x_w,y_w,z_w)为目标从机在群体坐标系下的相对位置坐标。

经由上述的技术方案可知，与现有技术相比，本发明公开提供了一种GPS拒止条件下无人机群PSD测距相对定位方法，通过主机监控从机判断当前机群的运动状态，利用PSD三角测距测量主机和从机之间的距离，再利用测角测距的方式获取从机的位置信息，最后通过测角测距方法对从机进行相对定位，并转换至无人机群的群体坐标系中。本发明针对主从式的无人机编队结构和位置敏感检测器(PSD)工作特性，实现在GPS拒止条件下的无人机群PSD测距相对定位。PSD具有灵敏度高、分辨率高，实时性强和抗干扰性好等优点，适用于多种位置检测系统。本发明通过两自由度转台测量从机相对主机的角度，采用PSD三角测距获得主机与从机间的距离，提高的定位响应速速，并通过三角回环优化提高测距精度，实现了从机相对于主机的相对定位与从机在机群中的定位，有助于无人机群在GPS拒止条件下的实时性导航定位，在一定程度上可推动无人机蜂群的进一步应用和发展。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。

图1附图为本发明提供的机间PSD测距相对定位流程示意图；

图2附图为本发明提供的整个无人机群导航系统框架示意图；

图3附图为本发明提供的主机对从机二维搜索过程示意图；

图4附图为本发明提供的二维PSD结构示意图；

图5附图为本发明提供的PSD三角测距原理示意图；

图6附图为本发明提供的三角回环优化示意图；

图7附图为本发明提供的无人机群的群体坐标系示意图；

图8附图为本发明提供的从机对于主机相对定位示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例公开了一种GPS拒止条件下无人机群PSD测距相对定位方法，通过主机监控从机判断当前机群的运动状态，利用PSD三角测距测量主机和从机之间的距离，再利用测角测距的方式获取从机的位置信息，最后通过测角测距方法对从机进行相对定位，并转换至无人机群的群体坐标系中。

实施例

无人机群整体导航系统框架如图1所示。为优化机群结构和任务分配，机群中主机携带导航设备精度较高，有GPS和高精度惯导，此外还携带测距传感器和两自由度转台，从机携带导航设备较简陋，只有GPS和MEMS(微机电系统)，整个无人机群在起飞前进行惯导在线自标定。当进入卫星拒止环境后，主机间共享导航信息并通过相互测角测距来约束和抑制误差发散，然后以主机定位信息为基准，通过测角测距的方式矫正从机的位置，以保证整个机群稳定飞行。

本发明PSD测距相对定位的实现流程如图2所示。本发明中主机和从机间测距相对定位方法的实现具体包括如下步骤：

S1：判断当前机群运行状态。

正常情况下，机群主要通过GPS来获得位置信息，GPS定位作为一种半自主式导航，卫星信号容易受到干扰和欺骗，在部分环境下不能有效接收和反馈信号，同时在机群运行过程中位置信息可能会突然出现未知的异常跳变，这些情况视为卫星拒止情况。整个机群的导航控制系统实时对当前机群运行状态进行监测，若无法接收和发送卫星信号，或是位置信息异常突变，就判断当前满足卫星拒止的条件，依次进行S2-4，否则认为机群运行状态正常，仍以GPS组合导航定位信息为基准。

S2：利用PSD三角测距测量主机和从机之间的距离。

S2.1：旋转主机的两自由度转台，搜寻目标从机。

在机群内，各主机携带两自由度转台、激光器和测距传感器(PSD)，从机携带测距传感器和伺服电机。主机的两自由度转台与激光器组合，使得激光器可以实现在水平和竖直两平面内的自由转动，从而对目标从机进行找寻。找寻过程如图3所示，通常机群都在同一平面内保持飞行，因此先进行水平面内的搜索，激光器发出激光的同时在水平面内转动至机群进入GPS拒止条件之前，通过GPS已获知的各从机的大概位置进行左右搜寻，同时检测从机PSD是否收到激光信号，若收到则认为已经找到目标从机，保持当前状态进行S2.2；否则可知目标从机与主机之间存在高度差，此时进行改变激光器的俯仰角后再进行搜寻，重复水平搜寻和竖直搜寻，直至找到目标从机即目标从机的PSD收到信号，进行S2.2。

S2.2：通过灰度质心法获取激光光斑的质心对应的差分输出电压。

先计算从机PSD上激光光斑图像中的所有像素的灰度值之和：

分别计算光斑图像中每个像素与其对应的x坐标的乘积之和与其对应y坐标的乘积之和：

然后通过做比获得光斑质心的坐标：

根据PSD的器件特性，实际使用时并不能直接输出光斑质心坐标，而是分别输出光斑质心成像点横坐标x对应的差分电压V_x+和V_x-，及纵坐标y对应的差分电压V_y+和V_y-，因此还需要进行之后的位置坐标确定。

S2.3：确定PSD成像点在PSD上的位置坐标。

由S2.2可以得到当前光斑质心成像点坐标对应的差分电压，如图4所示，PSD上每一位置处的横纵坐标均有与其对应的电压输出，这种关系是一一对应的。根据PSD器件的工作原理，可以得到当前质心成像点坐标横纵坐标对应的理论电压输出分别为：

再通过PSD输出电压与成像点坐标的线性关系获得当前光斑质心坐标：

X＝a_xV_x+b_x (7)

Y＝a_yV_y+b_y (8)

式(7)和式(8)分别是对成像点横纵坐标与对应输出电压的线性拟合表达式，其中系数a_x、b_x、a_y和b_y都通过最小二乘法获得，如下式所示：

S2.4：旋转转台，利用PSD两点位置信息进行三角测距；

由S2.3和S2.4可获得当前激光光斑质心在从机PSD上的位置坐标，此时使主机的两自由度转台绕任意转轴旋转一定角度，得到新的一个新的光斑位置，要确保转动后从机的PSD仍可接收到主机激光器的信号，如图5所示，重复S2.3与S2.4可获得新的光斑质心位置坐标。新的光斑点与先前的光斑点以及主机形成了一个三角形，以此来求解从机与主机间的距离。

PSD的尺寸往往只有几厘米，实际应用中远小于主机和从机之间的距离，因此PSD的长度完全可以忽略，近似将该三角形视作等腰三角形，两个光斑点的位置坐标分别为(x₁,y₁)和(x₂,y₂)，可以求得它们之间的距离为：

取两光斑点的中点，则激光器到该点的距离可视为等腰三角形底边的高，将其近似视为主机和从机之间的距离L。

S2.5：三角回环优化，减小测距误差。

根据S2.4所得两机间的距离L并不准确，存在一定误差δ，实际间距应为L+δ，应尽可能地使δ小，以获得更精确的定位。无人机编队飞行往往是以方队或直角三角编队方式行进，其中任一无人机往往能与其相邻两架无人机构成近似直角三角形，借助直角三角形的数学特性来对测距误差δ进行约束优化。

如图6所示，三架无人机两两之间的间距分别为L₁+δ₁、L₂+δ₂和L₃+δ₃，角度关系在图中已经给出，将δ₁、δ₂和δ₃看作未知变量，根据三角数学关系可得到如下关系：

上述方程组是无解的，对最后一个等式展开得

L₁ ²+L₂ ²-L₃ ²+2L₁δ₁+2L₂δ₂-2L₃δ₃+δ₁ ²+δ₂ ²-δ₃ ²＝0 (14)

其中δ₁ ²、δ₂ ²、δ₃ ²是高阶小误差量，可将其忽略，以满足方程组求解条件，因此上式可简化写为

L₁ ²+L₂ ²-L₃ ²+2L₁δ₁+2L₂δ₂-2L₃δ₃＝0 (15)

将式(15)与(13)前两个等式联立可解得

其中，C₁、C₂、C₃皆为中间参数，表达式如下：

C₁＝tanθ₁ (17)

C₂＝1/sinθ₂ (18)

C₃＝L₁ ²-L₂ ²+L₃ ²+2C₁L₁L₂-2C₂L₁L₃ (19)

将δ₁、δ₂和δ₃的结果反代回L₁+δ₁、L₂+δ₂和L₃+δ₃，即得到机间更准确的距离信息。

S3：利用测角测距的方式获取从机的位置信息。

S3.1：建立整个无人机群的群体坐标系。

为保证机群的稳定飞行，同时为方便确定从机在机群中的相对位置，为无人机群建立群体导航坐标系，如图7所示。

其中，以主机1为坐标原点，以整个机群的飞行方向为y轴正方向，z轴沿地垂线指向天，x轴指向机群右侧,构成右手笛卡尔坐标系。

S3.2：通过测角测距方法对从机进行相对定位，并转换至群体坐标系中。

设机群中某一主机u在群体导航系下的坐标为(x_u,y_u,z_u),如图8所示，通过S2确定某一从机w距离该主机距离为L，且相对方位角和俯仰角分别为α和β，由此可得到从机w相对于主机u的相对位置坐标(x_uw,y_uw,z_uw)为：

进而再在主机u的坐标基础上，将其转换到群体坐标系下：

即可得到从机在群体中的位置坐标(x_w,y_w,z_w)。

S4：重复上述S1-3进行从机相对定位，直至无人机群恢复正常飞行状态。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (7)

1.一种GPS拒止条件下无人机群PSD测距相对定位方法，其特征在于，包括以下步骤：

步骤1：无人机群中主机数量大于或等于三架，每架主机搜索各自对应的目标从机，向目标从机发送若干束激光信号；

步骤2：目标从机接收主机发送的每束激光信号，通过灰度质心法获取激光信号的激光光斑质心对应的差分输出电压；

步骤3：根据差分输出电压，并结合PSD输出电压与激光光斑质心成像点坐标之间的线性关系，计算获得目标从机的激光光斑质心在PSD上的位置坐标；

步骤4：根据目标从机的两点位置坐标进行三角测距；

步骤5：任选三架主机，根据三架主机之间的距离利用三角回环优化，减小主机的测距误差，返回所述步骤1从而获得目标从机精确测距结果；

步骤6：获取主机坐标，并结合主机坐标和机群飞行动向构建群体坐标系，根据目标从机的位置坐标和精确测距结果计算目标从机在所述群体坐标系中的相对位置坐标，实现相对定位。

2.根据权利要求1所述的GPS拒止条件下无人机群PSD测距相对定位方法，其特征在于，所述步骤2的具体过程包括：

步骤21：目标从机PSD接收激光信号，计算激光光斑图像中的所有像素的灰度值之和，表达式为：

其中，G_all表示所有像素的灰度值之和；(i,j)表示各激光光斑图像中像素点坐标；I(i,j)表示对应像素点的灰度值；

步骤22：分别计算激光光斑图像中每个像素与其对应的x坐标的乘积之和、y坐标的乘积之和，表达式分别为：

其中，G_x为像素对应的x坐标的乘积之和；G_y为像素对应的y坐标的乘积之和；

步骤23：利用激光光斑图像中像素的x坐标的乘积之和、y坐标的乘积之和与所有像素的灰度值之和，获得激光光斑质心的坐标，表达式为：

其中，x、y分别为激光光斑质心的横坐标和纵坐标；

步骤24：将激光光斑质心的坐标转换成差分电压，获得差分输出电压，横坐标对应差分电压V_x+和V_x-，纵坐标对应差分电压V_y+和V_y-。

3.根据权利要求1所述的GPS拒止条件下无人机群PSD测距相对定位方法，其特征在于，步骤3的具体实现过程为：

步骤31：根据差分输出电压和PSD工作原理，得到激光光斑质心成像点坐标理论电压，表达式为：

其中，V_x+和V_x-为激光光斑质心横坐标对应差分电压，V_y+和V_y-为激光光斑质心纵坐标对应差分电压；V_x为质心成像点横坐标理论电压；V_y为质心成像点纵坐标理论电压；

步骤32：根据PSD输出电压与质心成像点坐标的线性关系，获得当前光斑质点位置坐标，表达式为：

X＝a_xV_x+b_x (7)

Y＝a_yV_y+b_y (8)

其中，X、Y分别为质心成像点的横坐标输出电压线性拟合表达式和纵坐标输出电压线性拟合表达式；a_x、b_x、a_y和b_y为表达式系数，表示为：

其中，X_i，Y_i分别代表各位置处激光光斑质心的横坐标与纵坐标；V_xi是各横坐标对应的电压输出，与X_i一一对应；V_yi是各纵坐标对应的电压输出，与Y_i一一对应。

4.根据权利要求1所述的GPS拒止条件下无人机群PSD测距相对定位方法，其特征在于，步骤4中主机的两自由度旋转台任意转动角度，只要当前目标从机接收到激光信号，则经过步骤2-步骤3均获得一个新的位置坐标，同一架目标从机的任意两点位置坐标与之间形成一个三角形，进行三角测距，获得当前目标从机与主机之间的测距结果。

5.根据权利要求1所述的GPS拒止条件下无人机群PSD测距相对定位方法，其特征在于，三角测距的具体过程为：

步骤41：两点位置坐标之间的距离为：

其中，(x₁,y₁)和(x₂,y₂)分别为两点位置坐标；

步骤42：主机发射激光信号的位置点到两位置坐标中点的距离为主机与当前目标从机之间的距离，表达式为：

其中，L为测距结果，θ为相邻两束激光信号对应的光路间的夹角。

6.根据权利要求1所述的GPS拒止条件下无人机群PSD测距相对定位方法，其特征在于，步骤5中进行三角回环优化的具体过程为：

任意选取三架主机，任意两架主机之间的间距关系表示为：

其中，任意两架主机之间的间距分别为L₁+δ₁、L₂+δ₂和L₃+δ₃，δ₁、δ₂和δ₃均为误差值；θ₁、θ₂和θ₃分别为间距之间对应的夹角；

将公式(13)中的最后一个等式展开，获得：

L₁ ²+L₂ ²-L₃ ²+2L₁δ₁+2L₂δ₂-2L₃δ₃+δ₁ ²+δ₂ ²-δ₃ ²＝0 (14)

δ₁ ²、δ₂ ²、δ₃ ²是高阶小误差量；将其忽略以满足公式(13)求解条件，因此简化为：

L₁ ²+L₂ ²-L₃ ²+2L₁δ₁+2L₂δ₂-2L₃δ₃＝0 (15)

将式(13)和式(15)联立可得：

其中，C₁、C₂、C₃均为中间参数，表达式如下：

C₁＝tanθ₁ (17)

C₂＝1/sinθ₂ (18)

C₃＝L₁ ²-L₂ ²+L₃ ²+2C₁L₁L₂-2C₂L₁L₃ (19)

将δ₁、δ₂和δ₃的结果反代回L₁+δ₁、L₂+δ₂和L₃+δ₃，则减小主机间的测距误差。

7.根据权利要求1所述的GPS拒止条件下无人机群PSD测距相对定位方法，其特征在于，步骤6的具体过程为：

步骤61：将主机位置作为坐标原点，以机群飞行方向为y轴正方向，沿地垂线指向天为中z轴正方向，机群右侧为x轴正方向，构成右手笛卡尔坐标系，作为群体坐标系；

步骤62：在无人机群的群体导航系下，目标从机相对于主机的相对位置坐标为：

其中，(x_uw,y_uw,z_uw)为群体导航系下的相对位置坐标；L为目标从机与主机的测量距离；α和β分别为目标从机的方位角和俯仰角，在搜寻目标从机过程中通过主机所携带的两自由度转台获得；

步骤63：根据群体导航系下的主机坐标，以及主机与目标从机的相对位置坐标，将目标从机位置坐标转换到群体坐标系下：

其中，(x_u,y_u,z_u)为主机在群体导航系下的坐标；(x_w,y_w,z_w)为目标从机在群体坐标系下的相对位置坐标。

Images