基于分布节点信息区块的无人机集群协同导航系统及方法与流程

本发明涉及一种基于分布节点信息区块的无人机集群协同导航系统及方法，属于定位与导航技术领域。

背景技术：

近年来，飞行器集群飞行越来越受到国际上的关注，所谓飞行器集群飞行，即多架飞行器为适应任务要求而进行的某种队形排列和任务分配的组织模式。飞行器编队飞行能够扩大飞行器搜索侦查范围，具有可执行多重任务，可靠性高，整体效率高，减少飞行阻力等优点。飞行器协同集群飞行模式相比单架无人机具有更多优势以及更广阔的应用前景。采用多无人机协同的作战方式，进一步提高了无人机自主生存能力和整体作战效能，通过集群无人机之间的信息共享，将实战中协调任务、协同搜索、侦查与攻击等协同打击效能得以充分发挥。

因此，高精度的相对导航系统为提高集群整体作战效能提供了支撑和保障，是实现无人机集群飞行的关键核心技术。目前，用于相对导航系统的传感器主要有惯性测量器件和卫星导航系统。由于惯性测量单元存在常值漂移，而卫星导航系统信号易受外界干扰，可靠性低，无法满足集群无人机相对导航系统对精度的要求。协同导航技术利用多飞行器间的传感器信息，实现多飞行器间的信息协同，结合数据融合的方法以提高多飞行器间的定位精度，弥补了导航传感器的误差，并且对可能出现的故障进行识别、隔离和恢复。

技术实现要素：

为了实时显示多无人机集群飞行时，各无人机飞行航迹以及无人机之间的距离变化情况；并且验证通过相对距离信息解算精度较低的待定位无人机的定位精度方法的有效性。本发明提出了一种基于分布节点信息区块的无人机集群协同导航系统及方法，该方法通过距离测量单元，卫星接收机单元，数据传输单元以及导航计算机来实时解算参与集群飞行的待定位无人机位置，并实时显示各集群无人机的飞行航迹，以及待定位无人机的位置数据等。本发明可以有效验证了基于相对距离信息的集群无人机协同导航算法的有效性，提高协同导航算法在工程上的应用能力。

本发明为解决其技术问题采用如下技术方案：

一种基于分布节点信息区块的无人机集群协同导航系统，包括顺序连接的传感器模块、协同导航系统启动模块，基于相对距离信息的无人机位置解算模块和实时显示模块；所述传感器模块包括距离测量单元、卫星接收机单元、数据传输单元和数据计算单元，其中：距离测量单元、卫星接收机单元分别和数据计算单元连接，数据计算单元和数据传输单元连接；所述协同导航系统启动模块包括无人机飞行控制单元、导航系统节点启动单元和导航系统节点数据实时采集分类存储单元，其中，无人机飞行控制单元、导航系统节点启动单元分别与导航系统节点数据实时采集分类存储单元连接；基于相对距离信息的无人机位置解算模块包括顺序连接的坐标系转换单元，相对距离方程建立单元和位置协同解算单元；实时显示模块包括集群飞行器航迹显示单元、待定位飞行器位置解算结果显示单元、传感器测量数据显示单元、位置解算误差显示单元，其中，各显示单元模块相互独立显示导航系统采集的数据。

基于分布节点信息区块的无人机集群协同导航系统的导航方法，包括如下步骤：

(1)通过传感器模块搭建集群无人机协同导航系统；

(2)集群无人机协同导航系统搭建完成后，通过协同导航系统启动模块远程发送启动命令来启动无人机协同导航系统，其中无人机飞行控制单元控制集群无人机飞行航迹，导航系统节点启动单元启动各导航传感器，导航系统节点数据实时采集分类存储单元采集存储无人机集群系统飞行传感器数据；

(3)基于相对距离信息的无人机位置解算模块通过获取各无人机导航系统传感器数据，以此来解算待定位无人机的位置数据；

(4)待定位无人机位置解算完毕后，实时显示模块中集群飞行器航迹显示单元、待定位飞行器位置解算结果显示单元、传感器测量数据显示单元、位置解算误差显示单元实时显示解算结果。

步骤(1)中所述传感器模块搭建集群无人机协同导航系统方法如下：

步骤(3-1)，确定集群无人机协同导航系统仿真所需集群无人机数量n；

步骤(3-2)，判断协同导航系统仿真所需飞行器数量n是否大于5，若n＞＝5则继续执行下面步骤，若n＜5，则该协同导航系统无法使用，需继续等待直至集群无人机数量符合要求；

步骤(3-3)，对上述的n个协同导航系统节点进行编号，编号为i，其中1≤i≤n，配置n个协同导航系统节点，通过每个导航系统节点的数据来模拟单架飞行器机载导航数据；

步骤(3-4)，令i＝1，配置第一个系统节点；

步骤(3-5)，配置距离测量单元，将距离测量单元和数据计算单元串接，距离测量单元测量该系统节点至其他所有系统节点的距离数据，该数据包括该系统节点编号m，距离测量时间td，其余系统节点编号i，其中1≤i≤n,i≠m，以及相对应的与其余系统节点的距离数据di，其中1≤i≤n,i≠m和数据有效标志位pd；

步骤(3-6)，配置卫星接收机单元，将卫星接收机单元和数据计算单元串接，卫星接收机单元测量该节点所处位置数据，该数据包括测量时间tg、节点所处位置的经度数据em、纬度数据nm，高度数据hm，以及数据有效标志位pg；

步骤(3-7)，配置数据计算单元，将接受到的节点间距离测量信息以及节点的卫星接收机数据进行组合和处理，读取接受到的距离测量单元数据的有效标志位pd，判断接受到的距离数据是否有效，如果数据有效则继续进行下一步骤，否则继续接受新的距离数据重新判断；

步骤(3-8)，根据数据计算单元接受到的有效的节点距离数据，数据计算单元将该数据进行切割和分块储存，将接受到的有效距离数据按照数据格式将系统自身节点编号m、测量时间数据td、系统节点编号数据i、以及和对应系统节点的距离数据di进行分割，分别存储在数据计算单元的存储器中；

步骤(3-9)，判断数据计算单元接受到的卫星接收机单元数据的有效标志位pg，如果数据有效则继续进行下一步骤，否则继续接受新的有效的卫星接收机单元数据；

步骤(3-10)，根据数据计算单元接受到的有效的卫星接收机单元数据，数据计算单元将卫星接收机单元数据进行切割和分块储存，将接收到的卫星接收机单元数据按照数据格式将数据测量时间tg、经度数据em、纬度数据nm、高度数据hm进行切割然后分块存储在数据计算单元的存储器中；

步骤(3-11)，数据计算单元将分块存储在存储器中的系统节点距离测量时间td、系统节点编号数据i，其中1≤i≤n,i≠m、对应系统节点距离数据di其中1≤i≤n,i≠m、卫星接收机测量时间tg、经度数据em、纬度数据nm、高度数据hm按照顺序连接在一起形成新的系统节点数据a，并且两两数据间添加数据标识位“-”，并且在新合成的节点数据前添加数据头即该系统节点编号m，每条数据末端添加数据结束符φ，数据计算单元最终形成的数据a如下所示：

m-td-1-d1-2-d2…-i-di…-n-dn-tg-em-nm-hm-φ

步骤(3-12)，将数据计算单元和数据传输单元串接，来将数据计算单元最终形成的系统节点数据a通过数据传输单元传输至导航计算机；

步骤(3-13)，比较i和n的大小关系，来判断参与仿真测试的所有系统节点是否都已经配置完成，如果i＜n则跳转至步骤(3-5)，并且令i＝i+1，如果i≥n，则所有的系统节点都已经配置完成了，继续执行下一步骤；

步骤(3-14)，将配置好的n个系统节点安装在n架无人机上，无人机机载电源与系统节点供电部分相连，为仿真系统节点供电；

步骤(3-15)，导航计算机通过无线传输的方式接受数据传输单元所传输的传感器数据并且同时向系统节点发送启动命令。

步骤(2)中所述的协同导航系统启动模块启动无人机协同导航系统包括如下步骤：

步骤(4-1)，协同导航系统启动模块中无人机飞行控制单元通过无人机机载无线电通信方式发送无人机飞行启动命令至n架无人机上，所有参与集群飞行的n架无人机同时按照既定的航线飞行；

步骤(4-2)，选择无人机集群系统中一架无人机作为待定位无人机，一架无人机作为主基准无人机，其余无人机作为副基准无人机；

步骤(4-3)，协同导航系统启动模块通过导航系统节点启动单元向各无人机上的系统节点发送启动命令，各系统节点同时开始工作，获取传感器测量数据；

步骤(4-4)，系统节点工作后，导航计算机会收到各系统节点发送来传感器测量数据，导航计算机根据接受到的数据头文件即接收到的系统节点的编号对不同系统节点发送来的数据进行区分，并且根据数据中的数据标识位特征对数据进行切割存储，最终分割完的数据包括：该无人机编号m、该无人机和系统中其他无人机的编号以及对应距离数据di、距离测量时间td、经度数据em、纬度数据nm、高度数据hm，卫星接收机数据测量时间tg，导航系统节点数据实时采集分类存储单元按照各无人机编号对接受到的数据分块存储，并且各分块存储器对上述切割后的数据进行分类保存。

步骤(3)的具体过程如下：

步骤(5-1)，根据导航计算机对系统节点数据的存储方式，通过系统节点编号，从导航计算机存储器中提取仿真系统各节点的经度数据、纬度数据和高度数据和集群无人机总数n；

步骤(5-2)，根据步骤(5-1)提取的经度数据、纬度数据和高度数据，通过坐标转换单元计算地球坐标系下的x轴坐标xi、y轴坐标yi、z轴坐标zi其表达式为：

xi＝(fi+hi)coslicosλi

yi＝(fi+hi)coslisinλi

zi＝[fi(1-e²)+hi]sinli

其中，fi为卯酉圈曲率半径，li为纬度，λi为经度，hi为高度，i＝1,2,3…n为系统节点编号，e为椭球偏心率，与地球长半径a，短半径b有关，e的计算公式为：

fi的计算表达式如下所示：

i＝1…n；

步骤(5-3)，根据步骤(4-2)中导航系统启动中确定的待定位无人机系统节点以及主基准无人机系统节点，设定待定位无人机系统节点在地球坐标系下x轴坐标为x、y轴坐标为y、z轴坐标为z，主基准无人机系统节点在地球坐标系下的x轴坐标为x0、y轴坐标为y0、z轴坐标为z0；

步骤(5-4)，从导航计算机上提取待定位系统节点和主基准系统节点距离为d0，和其他副基准仿真系统节点的距离di，其中i＝1…n-1，并且通过导航计算机计算待定位无人机与主副基准系统节点之间的距离差数据ri，其中i＝1…n-1，副基准无人机节点和主基准无人机节点距离为di，其中i＝1…n-1；

步骤(5-5)，根据步骤(5-3)设定的待定位无人机以及主副基准无人机的坐标，和步骤(5-4)获取的距离数据通过相对距离方程建立单元建立相对距离方程，如下所示：

α＝δ-2d0h-2βx

其中：

α＝[α0,α1…αn-1]

h＝[r1,r2…rn-1]^t

x＝[x,y,z]，为待定位目标飞行器的三维坐标，αi为距离测量误差系数，其中i＝1…n-1，d0为步骤(5-4)从导航计算机上提取的待定位系统节点和主基准系统节点距离，xi、yi、zi为步骤(5-2)计算的基准无人机地球坐标系下x轴坐标、y轴坐标、z轴坐标，其中i＝1…n-1；

步骤(5-6)，通过位置协同解算单元来解算上述的相对距离方程，位置协同解算算法通过两步最小二乘算法来进行求解，对上述的式(5)进行化简，如下所示：

α＝z-hx

其中，z＝δ-2d0h，h＝2β，

通过第一重最小二乘法得到：

x＝(h^th)^-1h^tz

并将相关参数代入得到：

x＝((2β)^t(2β))^-1(2β)^t(δ-2d0h)

步骤(5-7)，再进行第二重的最小二乘解算：将式(7)再代入到式(5)中，建立基于目标距离的方程如下公式所示：

并对上述等式进行化简，得到

α＝z′-h′d0

z′＝[i-h(h^th)^-1h^t]δ

h′＝2[i-h(h^th)^-1h^t]h

其中：i为单位矩阵；

步骤(5-8)通过最小二乘的方法求解出d0，并将其代入到式(8)中，得到最终待定位无人机系统节点的地球坐标系坐标：

步骤(5-9)，将计算的待定位无人机系统节点地球坐标系坐标(x,y,z)，x为待定位无人机节点地球坐标系x轴坐标，y为待定位无人机节点地球坐标系y轴坐标，z为待定位无人机节点地球坐标系z轴坐标，转换成经度、纬度和高度数据，转换成经度如下所示：

其中，λ′为计算的经度数据；

步骤(5-10)，通过迭代算法来将地球坐标系坐标转换成纬度l′和高度h′，设置纬度和高度计算迭代总次数为τ，初始迭代变量t＝1，初始l′＝0.1；

步骤(5-11)，根据步骤(5-8)的初始l′，计算高度h′和纬度l′的不断修正值，并且迭代次数自加1，表达式如下：

t＝t+1

其中，r为地球半径，e为地球偏心率；

步骤(5-12)，根据步骤(5-11)的迭代次数t的值，比较t与τ的大小关系，如果t≤τ，则执行步骤(5-11)，如果t＞τ，执行步骤(5-13)；

步骤(5-13)，获取最后的待定位无人机系统节点的经度λ′，纬度l′和高度h′数据；

步骤(5-14)，根据算法计算得到的待定位无人机系统节点的实时位置数据，通过导航计算机所接入的数据传输单元将实时计算的位置数据发送至待定位无人机系统；

步骤(5-15)，待定位无人机系统接收到导航计算机发送的实时的精确的位置数据后，通过无人机飞行控制单元引导无人机按照预先设置的无人机飞行航迹继续飞行。

步骤(4)所述实时显示模块实时显示待定位目标无人机的三维坐标结果、根据系统节点编号，提取导航计算机上各系统节点经度、纬度和高度数据并以三维航迹形式显示出来、以及各系统节点间的距离实时变化曲线和位置结果计算误差曲线。

本发明的有益效果如下：

1、本发明设计了一种由传感器模块、基于相对距离信息的无人机位置解算模块以及实时显示模块串接而成的协同导航系统；该协同导航系统可以通过集群无人机间距离数据解算出目标无人机的位置数据，并且实时显示了各集群无人机间的传感器数据和位置解算结果。

2、本发明设计了一种实时传感器数据分类处理方法，该方法可以有效对集群无人机众多传感器数据的有效性进行判断并进行分类提取和存储，有效提高了算法的解算效率。

3、本发明系统可以有效验证基于相对距离信息的集群无人机协同导航算法的有效性，提高了协同导航算法在工程上的应用能力。

4、本发明的基于相对距离信息的协同导航方法在实测环境下通过距离数据解算的目标飞行器位置精度较高，有利于工程上运用。

附图说明

图1为本发明系统方法的原理流程示意图。

图2为本发明系统各模块之间的组成单元及数据流程图。

图3为本发明的基于相对距离信息的协同导航算法实测数据位置解算结果图。

图4为本发明设计的集群无人机传感器数据分类提取存储方法数据格式和流向图。

具体实施方式

下面结合附图对本发明做更进一步的详细说明。

一种基于分布节点信息区块的无人机集群协同导航系统，包括顺序连接的传感器模块、协同导航系统启动模块，基于相对距离信息的无人机位置解算模块和实时显示模块；所述传感器模块包括距离测量单元、卫星接收机单元、数据传输单元和数据计算单元，其中：距离测量单元、卫星接收机单元分别和数据计算单元连接，数据计算单元和数据传输单元连接；所述协同导航系统启动模块包括无人机飞行控制单元、导航系统节点启动单元、导航系统节点数据实时采集分类存储单元，其中，无人机飞行控制单元、导航系统节点启动单元分别和导航系统节点数据实时采集分类存储单元连接；基于相对距离信息的无人机位置解算模块包括顺序连接的坐标系转换单元，相对距离方程建立单元和位置协同解算单元；实时显示模块包括集群飞行器航迹显示单元、待定位飞行器位置解算结果显示单元、传感器测量数据显示单元和位置解算误差显示单元，其中，各显示单元模块相互独立显示导航系统采集的数据。本专利提出的基于分布节点信息区块的无人机集群协同导航系统搭建流程及导航算法解算过程如图1所示，本文提出的基于分布节点信息区块的无人机集群协同导航系统功能模块功能如图2所示。

基于分布节点信息区块的无人机集群协同导航系统及导航方法，包括如下步骤：

(1)通过传感器模块搭建集群无人机协同导航系统；

(2)集群无人机协同导航系统搭建完成后，通过协同导航系统启动模块远程发送启动命令来启动无人机协同导航系统，其中包括无人机飞行控制单元控制集群无人机飞行航迹，导航系统节点启动单元启动各导航传感器，导航系统节点数据实时采集分类存储单元采集存储无人机集群系统飞行传感器数据；

(3)基于相对距离信息的无人机位置解算模块通过获取导航系统传感器数据，以此来解算待定位无人机的位置数据；

(4)待定位无人机位置解算完毕后，实时显示模块中集群飞行器航迹显示单元、待定位飞行器位置解算结果显示单元、传感器测量数据显示单元、位置解算误差显示单元实时显示解算结果。

传感器模块搭建方法如下所示：

步骤(3-1)，确定集群无人机协同导航系统仿真所需集群无人机数量n；

步骤(3-2)，判断协同导航系统仿真所需飞行器数量n是否大于5，若n＞＝5则继续执行下面步骤，若n＜5，则该协同导航系统无法使用，需继续等待直至集群无人机数量符合要求；

步骤(3-3)，对上述的n个协同导航系统节点进行编号，编号为i，其中1≤i≤n，配置n个协同导航系统节点，通过每个导航系统节点的数据来模拟单架飞行器机载导航数据；

步骤(3-4)，令i＝1，配置第一个系统节点；

步骤(3-5)，配置距离测量单元，将距离测量单元和数据计算单元串接，距离测量单元测量该系统节点至其他所有系统节点的距离数据，该数据包括该系统节点编号m，距离测量时间td，其余系统节点编号i，其中1≤i≤n,i≠m，以及相对应的与其余系统节点的距离数据di其中1≤i≤n,i≠m和数据有效标志位pd；

步骤(3-6)，配置卫星接收机单元，将卫星接收机单元和数据计算单元串接，卫星接收机单元测量该节点所处位置数据，该数据包括测量时间tg、节点所处位置的经度数据em、纬度数据nm，高度数据hm，以及数据有效标志位pg；

步骤(3-7)，配置数据计算单元，将接受到的节点间距离测量信息以及节点的卫星接收机数据进行组合和处理，读取接受到的距离测量单元数据的有效标志位pd，判断接受到的距离数据是否有效，如果数据有效则继续进行下一步骤，否则继续接受新的距离数据重新判断；

步骤(3-8)，根据数据计算单元接受到的有效的节点距离数据，数据计算单元将该数据进行切割和分块储存，将接受到的有效距离数据按照数据格式将系统自身节点编号m、测量时间数据td、系统节点编号数据i、以及和对应系统节点的距离数据di进行分割，分别存储在数据计算单元的存储器中；

步骤(3-9)，判断数据计算单元接受到的卫星接收机单元数据的有效标志位pg，如果数据有效则继续进行下一步骤，否则继续接受新的有效的卫星接收机单元数据；

步骤(3-10)，根据数据计算单元接受到的有效的卫星接收机单元数据，数据计算单元将卫星接收机单元数据进行切割和分块储存，将接收到的卫星接收机单元数据按照数据格式将数据测量时间tg、经度数据em、纬度数据nm、高度数据hm进行切割然后分块存储在数据计算单元的存储器中；

步骤(3-11)，数据计算单元将分块存储在存储器中的系统节点距离测量时间td、系统节点编号数据i其中1≤i≤n,i≠m、对应系统节点距离数据di其中1≤i≤n,i≠m、卫星接收机测量时间tg、经度数据em、纬度数据nm、高度数据hm按照顺序连接在一起形成新的系统节点数据a，并且两两数据间添加数据标识位“-”，并且在新合成的节点数据前添加数据头即该系统节点编号m，每条数据末端添加数据结束符φ，数据计算单元最终形成的数据a如下所示：

m-td-1-d1-2-d2…-i-di…-n-dn-tg-em-nm-hm-φ(1)

步骤(3-12)，将数据计算单元和数据传输单元串接，来将数据计算单元最终形成的系统节点数据a通过数据传输单元传输至导航计算机；

步骤(3-13)，比较i和n的大小关系，来判断参与仿真测试的所有系统节点是否都已经配置完成，如果i＜n则跳转至步骤(3-5)，并且令i＝i+1，如果i≥n，则所有的系统节点都已经配置完成了，继续执行下一步骤；

步骤(3-14)，将配置好的n个系统节点安装在n架无人机上，无人机机载电源与系统节点供电部分相连，为仿真系统节点供电；

步骤(3-15)，导航计算机通过无线传输的方式接受数据传输单元所传输的传感器数据并且同时向系统节点发送启动命令。

协同导航系统启动模块具体包括如下步骤：

步骤(4-1)，协同导航系统启动模块中无人机飞行控制单元通过无人机机载无线电通信方式发送无人机飞行启动命令至n架无人机上，所有参与集群飞行的n架无人机同时按照既定的航线飞行；

步骤(4-2)，选择无人机集群系统中一架无人机作为待定位无人机，一架无人机作为主基准无人机，其余无人机作为副基准无人机；

步骤(4-3)，协同导航系统启动模块通过导航系统节点启动单元向各无人机上的系统节点发送启动命令，各系统节点同时开始工作，获取传感器测量数据；

步骤(4-4)，系统节点工作后，导航计算机会收到各系统节点发送来传感器测量数据，导航计算机根据接受到的数据头文件即接收到的系统节点的编号对不同系统节点发送来的数据进行区分，并且根据数据中的数据标识位特征对数据进行切割存储，最终分割完的数据包括：该无人机编号m、该无人机和系统中其他无人机的编号以及对应距离数据di、距离测量时间td、经度数据em、纬度数据nm、高度数据hm，卫星接收机数据测量时间tg，导航系统节点数据实时采集分类存储单元按照各无人机编号对接受到的数据分块存储，并且各分块存储器对上述切割后的数据进行分类保存，传感器数据实时采集分类存储示意图如图4所示。

基于相对距离信息的无人机位置解算模块具体工作包括如下步骤：

步骤(5-1)，上述步骤4启动了协同导航系统后，获取了协同导航系统节点间的测量数据，根据导航计算机对系统节点数据的存储方式，通过系统节点编号，从导航计算机存储器中首先提取仿真系统各节点的经度数据、纬度数据和高度数据和集群无人机总数n；

步骤(5-2)，根据步骤(5-1)提取的经度数据、纬度数据和高度数据，通过坐标转换单元计算地球坐标系下的x轴坐标xi、y轴坐标yi、z轴坐标zi其表达式为：

xi＝(fi+hi)coslicosλi(2)

yi＝(fi+hi)coslisinλi(3)

zi＝[fi(1-e²)+hi]sinli(4)

其中，fi为卯酉圈曲率半径，其计算公式如下所示，li为纬度，λi为经度，hi为高度，i＝1,2,3…n为系统节点编号，e为椭球偏心率，与地球长半径a，短半径b有关，e的计算公式为：

fi的计算表达式如下所示：

i＝1…n；

步骤(5-3)，根据步骤(4-2)中导航系统启动中确定的待定位无人机系统节点以及主基准无人机系统节点，设定待定位无人机系统节点在地球坐标系下x轴坐标为x、y轴坐标为y、z轴坐标为z，主基准无人机系统节点在地球坐标系下的横坐标为x0、纵坐标为y0、竖坐标为z0；

步骤(5-4)，然后从导航计算机上提取待定位系统节点和主基准系统节点距离为d0，和其他副基准仿真系统节点的距离di，其中i＝1…n-1，并且通过导航计算机计算待定位无人机与主副基准系统节点之间的距离差数据ri，其中i＝1…n-1，副基准无人机节点和主基准无人机节点距离为di，其中i＝1…n-1；

步骤(5-5)，第二步是根据步骤(5-3)设定的待定位无人机以及主副基准无人机的坐标，和步骤(5-4)获取的距离数据通过相对距离方程建立单元建立相对距离方程，如下所示：

α＝δ-2d0h-2βx(5)

其中：

α＝[α0,α1…αn-1]

h＝[r1,r2…rn-1]^t

x＝[x,y,z]，为待定位目标飞行器的三维坐标，αi为距离测量误差系数，其中i＝1…n-1，d0为步骤(5-4)从导航计算机上提取的待定位系统节点和主基准系统节点距离，xi、yi、zi为步骤(5-2)计算的基准无人机地球坐标系下x轴坐标、y轴坐标、z轴坐标，其中i＝1…n-1；

步骤(5-6)，第三步通过位置协同解算单元来解算上述的相对距离方程，位置协同解算算法通过两步最小二乘算法来进行求解，对上述的式(5)进行化简，如下所示：

α＝z-hx(6)

其中，z＝δ-2d0h，h＝2β，

通过第一重最小二乘法可以得到：

x＝(h^th)^-1h^tz(7)

并将相关参数代入得到：

x＝((2β)^t(2β))^-1(2β)^t(δ-2d0h)(8)

步骤(5-7)为了提高位置协同解算算法的解算精度，再进行第二重的最小二乘解算：将式(7)再代入到式(5)中，建立基于目标距离的方程如下公式所示：

并对上述等式进行化简，得到

α＝z′-h′d0(10)

z′＝[i-h(h^th)^-1h^t]δ

h′＝2[i-h(h^th)^-1h^t]h

其中：i为单位矩阵；

步骤(5-8)通过最小二乘的方法求解出d0，并将其代入到式(8)中，得到最终待定位无人机系统节点的地球坐标系坐标：

步骤(5-9)，将计算的待定位无人机系统节点地球坐标系坐标(x,y,z)，x为待定位无人机节点地球坐标系x轴坐标，y为待定位无人机节点地球坐标系y轴坐标，z为待定位无人机节点地球坐标系z轴坐标，转换成经度、纬度和高度数据，转换成经度如下所示：

其中，λ′为计算的经度数据；

步骤(5-10)，通过迭代算法来将地球坐标系坐标转换成纬度l′和高度h′，设置纬度和高度计算迭代总次数为τ，初始迭代变量t＝1，初始l′＝0.1；

步骤(5-11)，根据步骤(5-8)的初始l′，计算高度h′和纬度l′的不断修正值，并且迭代次数自加1，表达式如下：

t＝t+1

其中，r为地球半径，e为地球偏心率；

步骤(5-12)，根据步骤(5-11)的迭代次数t的值，比较t与τ的大小关系，如果t≤τ，则执行步骤(5-11)，如果t＞τ，执行步骤(5-13)；

步骤(5-13)，获取最后的待定位无人机系统节点的经度λ′，纬度l′和高度h′数据，图3为在实测数据下本专利提出的基于相对距离信息的无人机位置解算算法和传统toa(timeofarrival)算法的解算误差对比。

步骤(5-14)，根据算法计算得到的待定位无人机系统节点的实时位置数据，通过导航计算机所接入的数据传输单元将实时计算的位置数据发送至待定位无人机系统；

步骤(5-15)，待定位无人机系统接收到导航计算机发送的实时的精确位置数据后，通过无人机飞行控制单元引导无人机按照预先设置的无人机飞行航迹继续飞行。

实时显示模块主要实时显示上述步骤计算的待定位目标无人机的三维坐标结果、根据系统节点编号，提取导航计算机上各系统节点经度、纬度和高度数据并以三维航迹形式显示出来、以及各系统节点间的距离实时变化曲线和位置结果计算误差曲线。