【技术实现步骤摘要】
本专利技术涉及集群导航领域，特别是一种uwb与惯组组合集群相对导航方法。
技术介绍
1、集群系统通常需要完成如相对编队、协同侦察等协同任务。目前，集群系统主要通过卫星导航来实现集群间的相对定位导航，通过利用卫星伪距解算各自的坐标，从而得到在wgs84坐标系下的定位信息。这样的定位方式成本低，存在绝对基准，系统规模可以无限拓展。但是也存在一定限制，一方面由于多径效应等导致定位精度低，另一方面也存在在强电磁干扰环境、室内环境获取不到导航信号等问题。
2、uwb超宽带是一种可以通过纳秒级非正弦波窄脉冲进行测距的技术，可以获得厘米级的测量精度，理论上能够获得相比卫星导航更高精度的相对导航数据，并且能够适应集群间距离与规模有限的特点，获取集群之间的相对位置关系。
3、因此，为解决目前集群采用卫星导航系统，但受限于绝对定位精度，不能满足高精度相对定位的要求、容易受干扰，且在无卫星导航系统环境下不能实现相对定位的问题，需要实现一种通过集群间高精度相对测距实现高精度稳定相对定位的解决方案。
技术实现思路
1、本专利技术解决的技术问题是：克服现有技术的不足，提供了一种uwb与惯组组合集群相对导航方法，提高集群系统完成协同任务的导航精度，且不依赖卫星导航系统。
2、本专利技术的技术解决方案是：一种uwb与惯组组合集群相对导航方法，包括：
3、获取集群中各节点的惯组数据，所述惯组数据包括加速度和角速度；根据惯组数据，得到各节点的惯组推算位置；同时获取集群各
4、利用各节点的uwb定位位置对各节点惯组数据中的角速度进行漂移修正，得到各节点修正后的角速度；
5、利用各节点修正后的角速度，对各节点的惯组推算位置进行更新，得到各节点的修正后惯组推算位置；
6、以集群中某个节点为主节点作为导航基准，将各节点的uwb定位位置和修正后惯组推算位置进行融合，获得最终的导航信息。
7、进一步，通过安装于集群各节点的惯组模块获取惯组数据，通过安装于各节点的uwb测距模块获取各节点的uwb测距信息；uwb测距模块与惯组模块采用统一时间基准，使采集的惯组数据和uwb测距信息之间时间同步。
8、进一步，所述利用各节点的uwb定位位置对各节点惯组数据中的角速度进行漂移修正，得到各节点修正后的角速度的步骤包括：
9、对uwb定位位置进行差分得到uwb定位速度；根据惯组数据得到惯组推算速度；
10、利用uwb定位速度和惯组推算速度，得到一段时间内的角速度误差量；
11、利用角速度误差量对该段时间的惯组数据中修正前的角速度进行修正，得到修正后的角速度。
12、进一步，所述角速度误差量，利用以下公式实现，
13、
14、其中，e为角速度误差量，分别代表t和t+δt时刻uwb定位速度，ab为惯组模块获取的机体系下加速度，c为当前的姿态旋转矩阵，g是重力加速度；
15、所述修正后的角速度，利用以下公式实现，
16、gbiased＝gorig-e/δt*w
17、其中，gbiased为修正后的角速度，gorig是修正前的角速度，w是修正权重。
18、进一步，所述利用各节点修正后的角速度，对各节点的惯组推算位置进行更新，得到各节点的修正后惯组推算位置的步骤包括：
19、利用修正后的角速度，得到更新后的姿态旋转矩阵；
20、将惯组模块获取的机体系下加速度，通过更新后的姿态旋转矩阵转换到地理坐标系下，得到修正后的地理坐标系下加速度；
21、对修正后的地理坐标系下加速度进行积分得到修正后惯组推算位置。
22、进一步，所述更新后的姿态旋转矩阵，利用以下公式实现，
23、
24、其中，ct+1为更新后的姿态旋转矩阵，ct为更新前的姿态旋转矩阵，分别是修正后的角速度；
25、所述修正后惯组推算位置，利用以下公式实现，
26、
27、其中，aw为将惯组模块获取的机体系下的加速度ab通过ct+1转换到地理坐标系下后，得到的修正后的地理坐标系下加速度，vt、vt+1分别代表修正前、修正后速度，pt，pt+1分别代表修正前、修正后惯组推算位置。
28、进一步，采用组合滤波的方式将各节点的uwb定位位置和修正后惯组推算位置进行融合，根据导航系统误差协方差大小确定融合系数。
29、进一步，组合滤波的滤波状态方程和量测方程利用如下公式实现：
30、
31、pout(k)是k时刻修正后惯组推算位置状态矩阵，f[pout(k)]是惯组状态转移函数，w(k)是惯组推算位置高斯系统噪声，γ(k)是w(k)的分布矩阵，u(k)为uwb定位位置量测矩阵，h[pout(k)]是uwb定位位置量测转移函数，e(k)是uwb定位位置高斯量测噪声。
32、进一步，根据uwb测距信息，利用最大似然法和最小二乘法，得到各节点的uwb定位位置。
33、本专利技术与现有技术相比的优点在于：
34、(1)相比于传统的卫导和/或惯导组合定位方法，本专利技术通过基于uwb测距和惯导组合的相对测距导航方法，结合集群间uwb主动测距信息与惯组自推算位置获得的导航信息后实现集群间高精度相对导航，降低对于卫星等外部全局定位系统的依赖，提升集群间相对导航精度；
35、(2)通过集群间uwb主动测距信息得到集群内各节点uwb定位位置，基于uwb定位位置对惯组数据进行修正，利用修正后的惯组数据再进行航姿推算，得到高精度的集群节点位置信息。
36、(3)采用组合滤波的方式，将修正后的惯组推算位置和uwb定位位置进行融合，实现集群间高精度相对导航。
本文档来自技高网...【技术保护点】
1.一种UWB与惯组组合集群相对导航方法，其特征在于：
2.根据权利要求1所述的UWB与惯组组合集群相对导航方法，其特征在于：通过安装于集群各节点的惯组模块获取惯组数据，通过安装于各节点的UWB测距模块获取各节点的UWB测距信息；UWB测距模块与惯组模块采用统一时间基准，使采集的惯组数据和UWB测距信息之间时间同步。
3.根据权利要求1所述的UWB与惯组组合集群相对导航方法，其特征在于：所述利用各节点的UWB定位位置对各节点惯组数据中的角速度进行漂移修正，得到各节点修正后的角速度的步骤包括：
4.根据权利要求3所述的UWB与惯组组合集群相对导航方法，其特征在于：所述角速度误差量，利用以下公式实现，
5.根据权利要求1所述的UWB与惯组组合集群相对导航方法，其特征在于：所述利用各节点修正后的角速度，对各节点的惯组推算位置进行更新，得到各节点的修正后惯组推算位置的步骤包括：
6.根据权利要求5所述的UWB与惯组组合集群相对导航方法，其特征在于：所述更新后的姿态旋转矩阵，利用以下公式实现，
7.根据权利要求1所述的
8.根据权利要求7所述的UWB与惯组组合集群相对导航方法，其特征在于：组合滤波的滤波状态方程和量测方程利用如下公式实现：
9.根据权利要求1所述的UWB与惯组组合集群相对导航方法，其特征在于：根据UWB测距信息，利用最大似然法和最小二乘法，得到各节点的UWB定位位置。
...【技术特征摘要】
1.一种uwb与惯组组合集群相对导航方法，其特征在于：
2.根据权利要求1所述的uwb与惯组组合集群相对导航方法，其特征在于：通过安装于集群各节点的惯组模块获取惯组数据，通过安装于各节点的uwb测距模块获取各节点的uwb测距信息；uwb测距模块与惯组模块采用统一时间基准，使采集的惯组数据和uwb测距信息之间时间同步。
3.根据权利要求1所述的uwb与惯组组合集群相对导航方法，其特征在于：所述利用各节点的uwb定位位置对各节点惯组数据中的角速度进行漂移修正，得到各节点修正后的角速度的步骤包括：
4.根据权利要求3所述的uwb与惯组组合集群相对导航方法，其特征在于：所述角速度误差量，利用以下公式实现，
5.根据权利要求1所述的uwb与惯组组合集群相对导航方法，其特征在于：...
【专利技术属性】
技术研发人员：胡阳修，陈光山，赵长春，何宁宇，彭一洋，孟海洋，
申请(专利权)人：上海航天控制技术研究所，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。