专利名称：一种大规模编队相对导航方法
技术领域：
本发明涉及相对导航领域。
背景技术：
相对导航是编队任务的一项关键技术，但目前多数的导航方法都需要复杂的导航 设备。多数导航算法都是针对同步测量信息的，对于测量信息间存在时间差(例如时分多 址方式)的导航方法目前还未研究。而且，所有的导航方法都是针对某一种具体情况而设 计的，能包容多个导航成员、适应多种状况的导航方法还未出现，为此，需要设计满足多任 务、能够适应不同条件的、简单可靠的相对导航模拟软件。
发明内容
本发明为了解决现有的相对导航技术不能处理存在时间差的情况，并且不能包容
多个导航成员、适应多种状况的问题，提供一种大规模编队相对导航方法。 —种大规模编队相对导航方法是基于下述系统实现的，所述系统由多个编队成员
组成，所述每个编队成员包括网络设备、导航设备、网络通信单元和核心算法单元，所述网
络设备由网控器及网络连接设备组成，所述核心算法单元由数据解释模块、算法调度模块、
初值估计模块和滤波算法模块组成，网络设备的数据输入/输出口与网络通信单元的网络
数据输入/输出口相连，导航设备的数据输出口与网络通信单元的导航数据输入口相连，
网络通信单元的数据输出口与数据解释模块的数据输入口相连，数据解释模块的数据输出
口与算法调度模块的数据输入口相连，算法调度模块的数据输出口同时与初值估计模块数
据输入口和滤波算法模块的数据输入口相连，初值估计模块数据输出口和滤波算法模块的
数据输出口同时与网络通信单元的数据输入口相连； 基于上述系统的大规模编队的相对导航方法的具体流程如下 步骤A、网络设备接收来自网络的数据，并将接收到的数据发送给网络通信单元，
网络通信单元将接收到的数据发送给数据解释模块； 步骤B、数据解释模块对接收到的数据按照数据存储顺序进行解码获得解码数据， 并将对应数据赋值给对应变量，数据解释模块将解码数据发送给算法调度模块；
所述解码数据包含指定的相对导航坐标系的坐标原点0和从编队成员中选出的n 个导航基站信息，所述导航基站信息为相对于坐标原点0的位置Os'和速度0v'，所述位 置0s'和速度0v'由其他导航设备给出，其中0《n《3; 步骤C、算法调度模块根据解码数据对应的节点号，判断出该组数据对应的编队成 员，将相应的数据传递给该编队成员，对所述编队成员进行导航，并且判断导航基站的数目 n，如果n > 2，执行步骤D，否则，即n《1 ，执行步骤E ;
步骤D、采用导航基站相对导航方法，具体步骤为 步骤Dl、在指定的坐标系中，将n个导航基站相对于坐标原点0的位置Os'作为 其他编队成员进行相对导航的基准；
步骤D2、判断本节点是否为导航基站，如果是，则执行步骤D8，否则，执行步骤D3 ;
步骤D3、采用相对导航判断接收到的解码数据是否为算法启动后接收到的第一组 数据或数据长时间丢失继而重新恢复后的第一组数据，如果是，则执行步骤D4，否则，执行 步骤D5 ; 步骤D4、初值估计模块根据导航基站的位置和本节点与导航基站的距离测量值， 按照几何关系估算本节点对应的编队成员相对于坐标原点0的位置0s和速度0v，然后执行 步骤D8 ; 步骤D5、利用上一采样周期内得到的该编队成员的最优估计结果0s'和Ov'，根 据该编队成员的运动规律和惯性测量元件的输出量对当前周期的位置Os和速度Ov进行预 测，获得预测结果； 步骤D6、本节点对应的编队成员分别与n个导航基站进行通信，并得到本编队成 员与n个导航基站的n个距离测量值； 步骤D7、滤波算法模块通过步骤D6中的n个距离测量值对步骤D5中的预测结果 进行修正，通过卡尔曼滤波得到该编队成员相对于坐标原点0的位置Os和速度Ov的最优 估计结果Os'和Ov'; 步骤D8、输出本节点对应编队成员相对于坐标原点0的位置0s'和速度Ov'，完 成当前周期该节点的导航； 步骤E、采用虚拟导航基站相对导航方法，具体过程为 步骤E1、放弃指定的坐标系，不再使用导航基站进行导航，从编队成员中任意选出 2个编队成员作为虚拟导航基站，选择其中一个虚拟导航基站作为坐标原点(O，O)，另一个 虚拟导航基站在横轴x轴上，位置为(Xs' ，O)，建立新坐标系； 步骤E2、判断本节点是否为虚拟导航基站，如果是，则执行步骤E8，否则，则执行 步骤E3 ; 步骤E3、判断接收到的解码数据是否为算法启动后接收到的第一组数据或数据长 时间丢失继而重新恢复后的第一组数据，如果是，则执行步骤E4，否则，执行步骤E5 ;
步骤E4、初值估计模块根据虚拟导航基站在虚拟坐标系中的位置和本节点与虚拟 导航基站的距离测量值，按照几何关系估算本节点对应的编队成员在虚拟坐标系中的位置 Xs和速度Xv，然后执行步骤E8 ; 步骤E5、利用上一采样周期内得到的该编队成员的最优估计结果Xs'和Xv'，根 据该编队成员的运动规律和惯性测量元件的输出量对当前周期的位置Xs和速度Xv进行预 测，获得预测结果； 步骤E6、本节点对应的编队成员分别与2个虚拟导航基站进行通信，并得到本编 队成员分别与2个虚拟导航基站的2个距离测量值； 步骤E7、滤波算法模块根据通过步骤E6获得的2个距离测量值对步骤E5中的预 测结果进行修正，通过卡尔曼滤波得到该编队成员在步骤E1中所述虚拟坐标系中的位置 Xs和速度Xv的最优估计结果Xs'和Xv'; 步骤E8、输出本节点对应的编队成员在虚拟坐标系中的位置Xs'和速度Xv'，完 成当前周期该节点的导航。 所述过程为一个周期的工作过程，下一周期的数据到来时，继续按上述步骤对相应节点进行导航。 本发明提出一种应用于大规模舰船和飞机编队的相对导航方法，由于编队成员间 需要通信，则可通过信号的传播时间获得成员间的相对距离，以此作为测量信息的相对导 航方法不仅可以降低编队成员的成本和重量，而且原理简单，易于实现，可以根据编队中遇 到的不同情况进行自动调节，实现多情况下的相对导航。
图1为每个编队成员的结构示意图。图2为本发明的大规模编队相对导航方法的 流程图。图3采用导航基站相对导航方法流程图。图4为采用虚拟导航基站相对导航方法 流程图。图5为本发明的相对导航方法中一个编队成员的界面显示示意图。
具体实施例方式
具体实施方式
一 结合图1、图2、图3和图4说明本实施方式，一种大规模编队相 对导航方法是基于下述系统实现的，所述系统由多个编队成员组成，所述每个编队成员包 括网络设备、导航设备、网络通信单元和核心算法单元，所述网络设备由网控器及网络连接 设备组成，所述核心算法单元由数据解释模块、算法调度模块、初值估计模块和滤波算法模 块组成，网络设备的数据输入/输出口与网络通信单元的网络数据输入/输出口相连，导航 设备的数据输出口与网络通信单元的导航数据输入口相连，网络通信单元的数据输出口与 数据解释模块的数据输入口相连，数据解释模块的数据输出口与算法调度模块的数据输入 口相连，算法调度模块的数据输出口同时与初值估计模块数据输入口和滤波算法模块的数 据输入口相连，初值估计模块数据输出口和滤波算法模块的数据输出口同时与网络通信单 元的数据输入口相连； 基于上述系统的大规模编队的相对导航方法的具体流程如下 步骤A、网络设备接收来自网络的数据，并将接收到的数据发送给网络通信单元，
网络通信单元将接收到的数据发送给数据解释模块； 步骤B、数据解释模块对接收到的数据按照数据存储顺序进行解码获得解码数据， 并将对应数据赋值给对应变量，数据解释模块将解码数据发送给算法调度模块；
所述解码数据包含指定的相对导航坐标系的坐标原点0和从编队成员中选出的n 个导航基站信息，所述导航基站信息为相对于坐标原点0的位置Os'和速度0v'，所述位 置0s'和速度0v'由其他导航设备给出，其中0《n《3; 步骤C、算法调度模块根据解码数据对应的节点号，判断出该组数据对应的编队成 员，将相应的数据传递给该编队成员，对所述编队成员进行导航，并且判断导航基站的数目 n，如果n > 2，执行步骤D，否则，即n《1 ，执行步骤E ;
步骤D、采用导航基站相对导航方法，具体步骤为 步骤Dl、在指定的坐标系中，将n个导航基站相对于坐标原点0的位置Os'作为 其他编队成员进行相对导航的基准； 步骤D2、判断本节点是否为导航基站，如果是，则执行步骤D8，否则，执行步骤D3 ;
步骤D3、采用相对导航判断接收到的解码数据是否为算法启动后接收到的第一组 数据或数据长时间丢失继而重新恢复后的第一组数据，如果是，则执行步骤D4，否则，执行步骤D5 ; 步骤D4、初值估计模块根据导航基站的位置和本节点与导航基站的距离测量值， 按照几何关系估算本节点对应的编队成员相对于坐标原点0的位置0s和速度0v，然后执行 步骤D8 ; 步骤D5、利用上一采样周期内得到的该编队成员的最优估计结果0s'和Ov'，根 据该编队成员的运动规律和惯性测量元件的输出量对当前周期的位置Os和速度Ov进行预 测，获得预测结果； 步骤D6、本节点对应的编队成员分别与n个导航基站进行通信，并得到本编队成 员与n个导航基站的n个距离测量值； 步骤D7、滤波算法模块通过步骤D6中的n个距离测量值对步骤D5中的预测结果 进行修正，通过卡尔曼滤波得到该编队成员相对于坐标原点0的位置Os和速度Ov的最优 估计结果Os'和Ov'; 步骤D8、输出本节点对应编队成员相对于坐标原点0的位置0s'和速度Ov'，完 成当前周期该节点的导航； 步骤E、采用虚拟导航基站相对导航方法，具体过程为 步骤E1、放弃指定的坐标系，不再使用导航基站进行导航，从编队成员中任意选出 2个编队成员作为虚拟导航基站，选择其中一个虚拟导航基站作为坐标原点(O，O)，另一个 虚拟导航基站在横轴x轴上，位置为(Xs' ，O)，建立新坐标系； 步骤E2、判断本节点是否为虚拟导航基站，如果是，则执行步骤E8，否则，则执行 步骤E3 ; 步骤E3、判断接收到的解码数据是否为算法启动后接收到的第一组数据或数据长 时间丢失继而重新恢复后的第一组数据，如果是，则执行步骤E4，否则，执行步骤E5 ;
步骤E4、初值估计模块根据虚拟导航基站在虚拟坐标系中的位置和本节点与虚拟 导航基站的距离测量值，按照几何关系估算本节点对应的编队成员在虚拟坐标系中的位置 Xs和速度Xv，然后执行步骤E8 ; 步骤E5、利用上一采样周期内得到的该编队成员的最优估计结果Xs'和Xv'，根 据该编队成员的运动规律和惯性测量元件的输出量对当前周期的位置Xs和速度Xv进行预 测，获得预测结果； 步骤E6、本节点对应的编队成员分别与2个虚拟导航基站进行通信，并得到本编 队成员分别与2个虚拟导航基站的2个距离测量值； 步骤E7、滤波算法模块根据通过步骤E6获得的2个距离测量值对步骤E5中的预 测结果进行修正，通过卡尔曼滤波得到该编队成员在步骤E1中所述虚拟坐标系中的位置 Xs和速度Xv的最优估计结果Xs'和Xv'; 步骤E8、输出本节点对应的编队成员在虚拟坐标系中的位置Xs'和速度Xv'，完 成当前周期该节点的导航。 本实施方式所述的虚拟导航基站可以任意选择，例如可以选择最靠前的两个节 点对应的两个编队成员，或者选择最靠后的两个节点对应的编队成员，或者为节点数为中 间的任意两个节点对应的编队成员。 本实施方式的方法中，当编队中存在一个或多个编队成员与导航基站的通信出现故障，如果该编队成员能够与两个以上导航基站通信，则采用能够通信的导航基站进行导 航，如果编队成员仅能与一个导航基站通信或与所有导航基站不能通信，则选择其他通信 状态良好且相对位置已经获得的编队成员作为虚拟导航基站进行导航，采用联邦滤波进行 相对导航 当编队成员与导航基站的通信出现故障时，本发明可以自动进行调节，适用不同 情况的相对导航。 在步骤D7或步骤E7中，编队成员在同一个采样周期内与不同导航基站的距离测 量时刻不同步，存在时差，因此采用滤波算法模块首先通过修改测量方程进行补偿，将存在 时差的距离测量值统一成同一时刻的距离测量值，然后再应用滤波函数进行待估计量的估 计。 本实施方式的网络设备与网络通信单元之间采用UDP的传输协议进行数据传递。
本实施方式中的数据来源既可以是实际编队的实测数据，也可以是消息模拟器产 生的模拟数据。 本实施方式的方法中，当更改相对导航坐标系的坐标原点O时，编队成员在状态 更新时，能够自动适应新的坐标原点的位置，进行相对导航。 本实施方式的方法中，当更改导航基站时，编队成员在状态更新时，能够自动适应 新的导航基站相对于坐标原点0的位置Os'和速度0v'，进行导航。 本实施方式中，编队成员的数目的最佳范围是20以内。本实施方式中，在导航过 程中，编队成员的数目可以在20以内变化，S卩编队成员数目可以增加或减少，算法会自动 调节以适应改变后的编队，不会影响导航。 本实施方式中的编队成员，可以是船只、飞机和汽车等装载有导航装置的设备。
具体实施方式
二 结合图l和图5说明本实施方式，本实施方式与具体实施方式
一 的不同之处在于，每个编队成员的结构还包括界面显示单元，所述界面显示单元用于显示 整个编队中各个编队成员之间的相对距离、估计位置和运动轨迹，及整个编队的拓扑结构。
权利要求
一种大规模编队相对导航方法是基于下述系统实现的，所述系统由多个编队成员组成，所述每个编队成员包括网络设备、导航设备、网络通信单元和核心算法单元，所述网络设备由网控器及网络连接设备组成，所述核心算法单元由数据解释模块、算法调度模块、初值估计模块和滤波算法模块组成，网络设备的数据输入/输出口与网络通信单元的网络数据输入/输出口相连，导航设备的数据输出口与网络通信单元的导航数据输入口相连，网络通信单元的数据输出口与数据解释模块的数据输入口相连，数据解释模块的数据输出口与算法调度模块的数据输入口相连，算法调度模块的数据输出口同时与初值估计模块数据输入口和滤波算法模块的数据输入口相连，初值估计模块数据输出口和滤波算法模块的数据输出口同时与网络通信单元的数据输入口相连；基于上述系统的大规模编队的相对导航方法的具体流程如下步骤A、网络设备接收来自网络的数据，并将接收到的数据发送给网络通信单元，网络通信单元将接收到的数据发送给数据解释模块；步骤B、数据解释模块对接收到的数据按照数据存储顺序进行解码获得解码数据，并将对应数据赋值给对应变量，数据解释模块将解码数据发送给算法调度模块；所述解码数据包含指定的相对导航坐标系的坐标原点O和从编队成员中选出的n个导航基站信息，所述导航基站信息为相对于坐标原点O的位置Os′和速度Ov′，所述位置Os′和速度Ov′由其他导航设备给出，其中0≤n≤3；步骤C、算法调度模块根据解码数据对应的节点号，判断出该组数据对应的编队成员，将相应的数据传递给该编队成员，对所述编队成员进行导航，并且判断导航基站的数目n，如果n≥2，执行步骤D，否则，即n≤1，执行步骤E；步骤D、采用导航基站相对导航方法，具体步骤为步骤D1、在指定的坐标系中，将n个导航基站相对于坐标原点O的位置Os′作为其他编队成员进行相对导航的基准；步骤D2、判断本节点是否为导航基站，如果是，则执行步骤D8，否则，执行步骤D3；步骤D3、采用相对导航判断接收到的解码数据是否为算法启动后接收到的第一组数据或数据长时间丢失继而重新恢复后的第一组数据，如果是，则执行步骤D4，否则，执行步骤D5；步骤D4、初值估计模块根据导航基站的位置和本节点与导航基站的距离测量值，按照几何关系估算本节点对应的编队成员相对于坐标原点O的位置Os和速度Ov，然后执行步骤D8；步骤D5、利用上一采样周期内得到的该编队成员的最优估计结果Os′和Ov′，根据该编队成员的运动规律和惯性测量元件的输出量对当前周期的位置Os和速度Ov进行预测，获得预测结果；步骤D6、本节点对应的编队成员分别与n个导航基站进行通信，并得到本编队成员与n个导航基站的n个距离测量值；步骤D7、滤波算法模块通过步骤D6中的n个距离测量值对步骤D5中的预测结果进行修正，通过卡尔曼滤波得到该编队成员相对于坐标原点O的位置Os和速度Ov的最优估计结果Os′和Ov′；步骤D8、输出本节点对应编队成员相对于坐标原点O的位置Os′和速度Ov′，完成当前周期该节点的导航；步骤E、采用虚拟导航基站相对导航方法，具体过程为步骤E1、放弃指定的坐标系，不再使用导航基站进行导航，从编队成员中任意选出2个编队成员作为虚拟导航基站，选择其中一个虚拟导航基站作为坐标原点(0，0)，另一个虚拟导航基站在横轴x轴上，位置为(Xs′，0)，建立新坐标系；步骤E2、判断本节点是否为虚拟导航基站，如果是，则执行步骤E8，否则，则执行步骤E3；步骤E3、判断接收到的解码数据是否为算法启动后接收到的第一组数据或数据长时间丢失继而重新恢复后的第一组数据，如果是，则执行步骤E4，否则，执行步骤E5；步骤E4、初值估计模块根据虚拟导航基站在虚拟坐标系中的位置和本节点与虚拟导航基站的距离测量值，按照几何关系估算本节点对应的编队成员在虚拟坐标系中的位置Xs和速度Xv，然后执行步骤E8；步骤E5、利用上一采样周期内得到的该编队成员的最优估计结果Xs′和Xv′，根据该编队成员的运动规律和惯性测量元件的输出量对当前周期的位置Xs和速度Xv进行预测，获得预测结果；步骤E6、本节点对应的编队成员分别与2个虚拟导航基站进行通信，并得到本编队成员分别与2个虚拟导航基站的2个距离测量值；步骤E7、滤波算法模块根据通过步骤E6获得的2个距离测量值对步骤E5中的预测结果进行修正，通过卡尔曼滤波得到该编队成员在步骤E1中所述虚拟坐标系中的位置Xs和速度Xv的最优估计结果Xs′和Xv′；步骤E8、输出本节点对应的编队成员在虚拟坐标系中的位置Xs′和速度Xv′，完成当前周期该节点的导航。
2. 根据权利要求1所述的一种大规模编队相对导航方法，其特征在于虚拟导航基站为最靠前的两个节点对应的编队成员，或者为最靠后的两个节点对应的编队成员，或者为节点数为中间的任意两个节点对应的编队成员。
3. 根据权利要求1所述的一种大规模编队相对导航方法，其特征在于每个编队成员的结构还包括界面显示单元，所述界面显示单元用于显示整个编队中各个编队成员之间的相对距离、估计位置和运动轨迹，及整个编队的拓扑结构。
4. 根据权利要求1所述的一种大规模编队相对导航方法，其特征在于步骤D7或步骤E7中编队成员在同一个采样周期内与不同导航基站的距离测量时刻不同步，存在时差，滤波算法模块首先通过修改测量方程进行补偿，将存在时差的距离测量值统一成同一时刻的距离测量值，然后在应用滤波函数进行待估计量的估计。
5. 根据权利要求1所述的一种大规模编队相对导航方法，其特征在于网络设备与网络通信单元之间采用UDP的传输协议进行数据传递。
6. 根据权利要求1所述的一种大规模编队相对导航方法，其特征在于编队成员的数目为20范围内的任何整数值，算法调度模块为20以内的编队成员分配数据。
7. 根据权利要求6所述的应用大规模编队相对导航方法的编队成员导航过程，其特征在于编队成员的数目在导航过程中在不超过20个范围内变化，编队成员数目可以增加或减少。
全文摘要
一种大规模编队相对导航方法，它涉及相对导航领域，解决了现有的相对导航技术不能适应多种状况的问题。具体步骤如下1、编队系统指定相对导航的坐标原点、从编队中选出n个成员作为导航基站，若导航基站数目少于2个，则选择2个虚拟导航基站；2、算法开始运行时，初值估计模块对该编队成员相对于坐标原点或虚拟坐标原点的位置和速度进行估计，将估计结果作为启动滤波算法的初始值；3、滤波算法模块对成员状态不断进行预测和修正，得到该编队成员相对于坐标原点或虚拟坐标原点的位置和速度；4、输出相对于坐标原点或虚拟坐标原点的位置和速度。本发明可以降低编队成员的成本和重量，原理简单，易于实现，适用于实现多情况下的相对导航。
文档编号G01C21/00GK101709973SQ20091031049
公开日2010年5月19日 申请日期2009年11月26日 优先权日2009年11月26日
发明者张世杰, 张锦绣, 曹喜滨, 邢艳军 申请人:哈尔滨工业大学