CN114838732B - 一种基于图优化的通信受限环境下协同导航方法 - Google Patents

Abstract

本发明公开了一种基于图优化的通信受限环境下协同导航方法，步骤为：S1、建立由智能体航位推算约束和UWB相对测距约束构成的传感器观测模型；S2、设定因子图模型的图顶点以及边的连接形式，并构建因子图模型；S3、基于各协同节点的因子图模型，采用LM算法对目标函数进行求解，以得到各节点在k时刻下状态变量的优化结果；该协同导航方法通过构建因子图模型，利用所建立的包含惯性航位推算因子图约束图优化模型实现陀螺零偏的实时所建立的模型可精确估计出陀螺零偏，利用节点间相对测距信息进行分布式协同导航与定位，有效抑制了复杂拒止环境下节点的导航误差发散，实现在锚点观测不连续不同步情况下，相比于连续同步观测的导航定位精度无明显变化。

Description

一种基于图优化的通信受限环境下协同导航方法

技术领域

本发明涉及多智能体协同导航与定位技术领域，特别涉及一种基于图优化的通信受限环境下协同导航方法。

背景技术

智能无人集群系统由无人机、无人车、机器人等多智能体通过彼此之间的信息交互构成，可以智能协同控制，完成单个无人系统无法实现的复杂工作，在军事和民用领域具有广阔应用前景。无人集群系统实现复杂的自主协同控制首要解决的一个问题便是节点间的时空统一问题，即系统中多智能体节点的相对与绝对定位与导航问题。

协同导航技术是一项旨在多智能体系统下提高个体定位与导航精度的关键技术。在多智能体系统中，通过系统中各个子节点之间的导航状态共享与相互观测的方式来为子节点提供更丰富的观测信息，各个节点相互协同，可实现在低成本条件下的导航定位精度提升。

然而，在复杂遮挡环境下，节点与基站之间的通信会受到影响，无法保证观测信号的全覆盖。这需要系统具备处理观测信息时有时无、动态变化的能力，同时利用相邻协同节点的相对观测来对导航状态进行约束，实现复杂遮挡环境下的多无人平台协同导航。在多智能体协同导航领域，现有的方法主要使用基于滤波的方法实现协同导航与定位。此类方法在处理异步、动态增删的观测信息时存在局限性，难以应用在复杂遮挡及通信受限环境下。图优化算法将观测与状态约束编码为图模型中可增删的因子和顶点，具有很强的灵活性，目前已经在即插即用的多源组合导航系统中体现了优势。因此，采用基于因子图模型的图优化算法在处理通信受限、观测信息动态变化的情况具有理论优势。

因此，为了克服复杂遮挡及通信受限环境下观测信息不连续、不同步对智能体导航定位造成的影响，进一步提高实现复杂遮挡环境下的多无人平台协同导航精度，需要采用基于图优化算法的分布式协同导航方法，解决传统滤波方法无法适应观测信息不同步的问题，提高复杂通信受限环境下集群综合导航精度。

发明内容

本发明的目的是提供一种针对复杂环境下，协同导航节点拓扑结构动态变化、锚点观测信号不足的问题，利用节点间相对测距信息进行分布式协同导航与定位的基于图优化的通信受限环境下协同导航方法。

为此，本发明技术方案如下：

一种基于图优化的通信受限环境下协同导航方法，其步骤如下：

S1、基于各智能体配备有惯性传感器、里程计和超宽带测距传感器，与智能体建立通信基站为超宽带基站，建立由智能体航位推算约束和UWB相对测距约束构成的传感器观测模型；

其中，在传感器观测模型中，智能体状态变量包括节点位置坐标(x_k,y_k)、航向角θ_k和天向陀螺仪零偏b_k；智能体航位推算约束根据智能体自身基于惯性传感器的航位推算信息建立而成，航位推算信息包括；UWB相对测距约束根据各智能体之间相对测距信息及智能体与锚点之间的相对测距信息建立而成；

S2、基于航位推算约束和UWB相对测距约束，设定因子图模型的图顶点以及边的连接形式，进而构建因子图模型；

S3、基于各协同节点的因子图模型，采用LM算法对目标函数： 进行求解，以得到各节点在k时刻下的位置坐标(包括x轴位置坐标和y轴位置坐标)、航向角、陀螺仪零偏的优化结果；其中，/>为航位推算的状态约束，/>为航位推算的时间约束，/>为UWB相对测距约束。

进一步地，在步骤S1中，智能体航位推算约束包括航位推算的时间约束和航位推算的状态约束，其构建步骤为：

1)设定无人机自身惯性传感器提供的天向陀螺仪k时刻的角速率为ω_k，里程计输出的k时刻的速率观测值为v_k，采样间隔时间为t_s，且设定陀螺零偏不随时间变化；则，智能体在k-1时刻与k时刻之间的状态递推方程为：

θ_k＝θ_k-1+(ω_k-1-b_k)t_s，

x_k＝x_k-1+v_k-1t_scosθ_k，

y_k＝y_k-1+v_k-1t_ssinθ_k，

式中，θ_k为智能体k时刻的航向角，θ_k-1为k-1时刻的航向角，ω_k-1为k-1时刻的角速率，x_k为k时刻的x轴位置坐标，x_k-1为k-1时刻的x轴位置坐标，v_k-1为k-1时刻的速率，y_k为k时刻的y轴位置坐标，y_k-1为k-1时刻的y轴位置坐标；b_k为k时刻的天向陀螺零偏b_k；

2)设待估计状态量为式中，/>为智能体待估计的k时刻x轴位置坐标，/>为智能体待估计的k时刻y轴位置坐标，/>为智能体待估计的k时刻航向角，/>为智能体待估计的k时刻天向陀螺零偏；

3)构建航位推算的时间约束，其表达式为：

式中，为k-1到k时刻之间关于x轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于y轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于航向角的时间约束残差，/>为k-1到k时刻之间关于天向陀螺零偏的时间约束残差，/>为智能体待估计的k-1时刻x轴位置坐标，/>为智能体待估计的k-1时刻y轴位置坐标，/>为智能体待估计的k-1时刻航向角，/>为智能体待估计的k-1时刻天向陀螺零偏；

4)构建航位推算的状态约束，其表达式为：

式中，为k时刻关于x轴位置坐标与航向角的状态约束残差，/>为k时刻关于x轴位置坐标与航向角的状态约束残差；/>为k时刻关于航向角的状态约束残差，θ₀为航向角的初值，i为历史某一时刻。

进一步地，在步骤S1中，UWB相对测距约束构建为：式中，/>为k时刻的UWB相对测距约束残差，d_k为k时刻智能体通过UWB相对测距获得的其与其它协同智能体之间的距离观测值、或其与锚点之间的距离观测值；(x_anc，y_anc)为其它协同智能体或锚点的坐标。

进一步地，对于任一智能体，步骤S2的具体实施步骤为：

S201、根据步骤S1构建的航位推算的时间约束、航位推算的状态约束和UWB相对测距约束分别推导出与之对应的雅可比矩阵；

S202、对任一智能体的因子图模型的架构方式进行设定：

1)设智能体系统优化变量为所有时刻的状态变量，其表达式为：各时刻下的待估计状态量作为因子图模型的图顶点，并按照时序排布；

2)设定每相邻时刻之间的状态变量通过航位推算因子进行连线；其中，航位推算因子由航位推算的状态约束：和和航位推算的时间约束：构成；智能体每两个相邻时刻下的x轴位置坐标之间、节点y轴位置坐标之间、航向角之间、以及天向陀螺仪零偏b_k之间均通过航位推算的时间约束进行连线；智能体同一时刻下的天向陀螺仪零偏与航向角之间通过航位推算的状态约束进行连线，同时航向角分别与节点x轴位置坐标、节点y轴位置坐标之间通过航位推算的状态约束进行连线；

3)对各状态变量添加总UWB因子：各状态变量均与当前状态对应的总UWB因子进行连线；

S203、基于步骤S202中对图顶点以及边的连接形式的设定，构建得到各协同节点相应的因子图模型。

进一步地，在步骤S3中，对任一智能体自k时刻至k+1时刻的状态变量优化结果求解方法为：

1)将k时刻下得到的状态变量优化值作为k+1时刻下状态变量初始值X＝[X₁，X₂，...，X_k]；将k时刻的状态变量/>代入航位推算的状态递推方程中，推算得到k+1时刻的状态变量X_k，并作为状态变量初始值，得到k+1时刻下状态变量初始值X＝[X₁，X₂，...，X_k，X_k+1]；

2)确定各时刻下状态变量受到的约束，包括智能体在每相邻时刻下的状态变量之间受到的航位推算约束以及智能体在各时刻受到的UWB相对测距约束；

3)基于k+1时刻下状态变量初始值X＝[X₁，X₂，...，X_k]，以及航位推算的时间约束对应的雅可比矩阵、航位推算的状态约束对应的雅可比矩阵和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数进行求解，得到k+1时刻下各状态变量优化值

进一步地，在步骤S3的UWB相对测距约束确定中，智能体之间通信关系基于“高通信精度节点向低精度节点进行信息传输”的原则进行，通信信息包括位置信息和相对距离观测值。

与现有技术相比，该基于图优化的通信受限环境下协同导航方法复杂遮挡环境下锚点观测不同步不连续的问题，针对复杂遮挡环境下锚点观测不同步不连续的问题，通过构建因子图模型，利用所建立的包含惯性航位推算因子图约束图优化模型实现陀螺零偏的实时所建立的模型可精确估计出陀螺零偏，使估计误差小于10％；同时利用节点间相对测距信息，进行分布式协同导航与定位且节点间的相互观测约束，有效抑制了复杂拒止环境下节点的导航误差发散，使得系统具备处理异步、动态增删观测信息的能力，在锚点观测不连续不同步情况下，相比于连续同步观测的导航定位精度无明显变化；综合实验结果表明，该方法使得短时通信拒止环境下单节点的位置精度保持能力提升5倍，集群在复杂通信受限环境的综合定位精度提升1倍。

附图说明

图1为本发明的基于图优化的通信受限环境协同导航方法的流程图；

图2(a)为本发明的航位推算时间约束因子图模型；

图2(b)为本发明的航位推算状态约束因子图模型；；

图3为本发明建立的系统中智能体导航定位的完整因子图模型；

图4为本发明建立的图优化算法结构图；

图5为本发明的用于仿真与实验分析研究的典型场景示意图；

图6为经过本发明实施例的基于图优化的协同导航方法得到的节点定位轨迹与传统航位推算算法下的节点定位轨迹对比图；

图7(a)为经过本发明实施例的基于图优化的协同导航方法得到的三个节点x轴坐标的位置估计误差与传统航位推算算法下的节点位置估计误差的对比图；

图7(b)为经过本发明实施例的基于图优化的协同导航方法得到的三个节点y轴坐标的位置估计误差与传统航位推算算法下的节点位置估计误差的对比图；

图8(a)为经过本发明实施例的基于图优化的协同导航方法得到的三个节点的航向角结果与航向角参考值的对比图；

图8(b)为经过本发明实施例的基于图优化的协同导航方法得到的三个节点的零偏结果与零偏参考值的对比图；

图9为经过本发明实施例的基于图优化的协同导航方法得到的三个节点在优化前与优化后的定位均方误差的对比图。

具体实施方式

下面结合附图及具体实施例对本发明做进一步的说明，但下述实施例绝非对本发明有任何限制。

如图5所示为一个典型的室内通信遮挡环境，具体来说，初始状态下，环境分为房间内部环境和房间外部环境，共计有三辆无人车，其中两辆无人车位于室内，一辆无人车位于室外；设置在外部的通信基站共计有三个，其中一个通信基站位于西侧，另外两个通讯基站间隔设置并位于室内南侧，且该两个通讯基站与房间距离一致；在房间的南侧墙壁上间隔开设有三个进出门，在房间西侧开设有一个进出门。

该三辆无人车和三个通讯基站共同构成本实施例的无人集群系统；其中，三个通信基站作为三个锚点，与相应的无人车建立实时通信；位于室外的无人车命名为节点1，其初始位置设定在室外西侧，其运动轨迹设定为先从室外初始位置行驶至室内，进而自室内西侧行驶至室内东侧，在整个运动过程中，该无人车能够与位于室内南侧的位于室内且靠近室内西侧的两个通信基站建立通信；位于室内北侧且靠近西侧的无人车命名为节点2，其运动轨迹设定为自室内北侧行驶至室内南侧，在整个运动过程中，该无人车能够与位于室外西侧的通讯基站建立通信；位于室内北侧且靠近东侧的无人车命名为节点3，其运动轨迹设定为自室内北侧朝南行驶一段距离后、再转弯朝向室内东侧行驶，在整个运动过程中，该无人车无法与任何通讯基站建立通信。

基于此，三辆无人车在实际行驶过程与锚点之间的通信建立不仅存在精度上的不同，同时每辆无人车无法保证实时的通信更新，因此，为了保证三辆无人车的精准导航，本申请提出了一种采用协同节点之间的相对量测代替无人车与锚点之间的非视距量测的方法。其中，由于各节点之间采用协同通信模式实现通信更新，因此，各节点也称为协同节点。

如图1所示，采用该基于图优化的通信受限环境下协同导航方法实现室、内外三辆无人车精准协同导航的具体实施步骤如下：

S1、基于各无人车配备有惯性传感器、里程计和超宽带测距传感器(即UWB测距传感器)，通信基站为超宽带基站，建立由无人车航位推算约束和UWB相对测距约束构成的传感器观测模型；其中，无人车航位推算约束根据无人车自身基于惯性传感器的航位推算信息建立而成；UWB相对测距约束根据各无人车之间相对测距信息及智能体与锚点之间的相对测距信息建立而成；

具体地，步骤S1的构建观测模型具体步骤为：

S101、建立无人车航位推算约束：

设定无人机自身惯性传感器提供的天向陀螺仪k时刻的角速率为ω_k，里程计输出的k时刻的速率观测值为v_k，采样间隔时间为t_s，且设定陀螺零偏不随时间变化；则，无人车在k-1时刻与k时刻之间的状态递推方程为：

θ_k＝θ_k-1+(ω_k-1-b_k)t_s，

x_k＝x_k-1+v_k-1t_scosθ_k，

y_k＝y_k-1+v_k-1t_ssinθ_k，

式中，θ_k为无人车k时刻的航向角，θ_k-1为k-1时刻的航向角，ω_k-1为k-1时刻的角速率，x_k为k时刻的x轴位置坐标，x_k-1为k-1时刻的x轴位置坐标，v_k-1为k-1时刻的速率，y_k为k时刻的y轴位置坐标，y_k-1为k-1时刻的y轴位置坐标；b_k为k时刻的天向陀螺零偏b_k；

S102、根据k-1时刻与k时刻之间的状态变化量构建残差约束，包括航位推算的时间约束和航位推算的状态约束；

具体地，时间约束基于两时刻之间状态增量建立，状态约束基于同一时刻下不同状态量之间满足的约束关系建立；基于此，

设待估计状态量为

式中，为无人车待估计的k时刻x轴位置坐标，/>为无人车待估计的k时刻y轴位置坐标，/>为无人车待估计的k时刻航向角，/>为无人车待估计的k时刻天向陀螺零偏；

进而，构建航位推算的时间约束，其表达式为：

式中，为k-1到k时刻之间关于x轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于y轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于航向角的时间约束残差，/>为k-1到k时刻之间关于天向陀螺零偏的时间约束残差，/>为无人车待估计的k-1时刻x轴位置坐标，/>为无人车待估计的k-1时刻y轴位置坐标，/>为无人车待估计的k-1时刻航向角，/>为无人车待估计的k-1时刻天向陀螺零偏；如图2(a)所示为航位推算的时间约束的构建方式示意图；

构建航位推算的状态约束，其表达式为：

式中，为k时刻关于x轴位置坐标与航向角的状态约束残差，/>为k时刻关于x轴位置坐标与航向角的状态约束残差；

其中，由于天向陀螺零偏b_k影响航向角的误差，因此以当前k时刻的航向角估计值与量测值建立残差，即可得到航向角状态约束的表达式为：

式中，为k时刻关于航向角的状态约束残差，θ₀为航向角的初值，i为历史某一时刻；如图2(b)所示为航位推算的状态约束的构建方式示意图；

其中，以带有*的物理量代表该变量参与优化，其余不带*的物理量作为已知量测值输入，不参与优化；

S103、基于超宽带测距传感器(简称为UWB)输出距离观测值，构建UWB相对测距约束，其表达式可为：

式中，为k时刻的UWB相对测距约束残差，dk为k时刻无人车通过UWB相对测距获得的其与协同节点(或其与锚点)之间的距离观测值；(x_anc，y_anc)为协同节点(或锚点)的坐标。

S2、基于步骤S1建立的航位推算约束和UWB相对测距约束，设定因子图模型的图顶点以及边的连接形式，进而构建因子图模型；

具体地，步骤S2的实施步骤如下：

S201、根据步骤S1构建的航位推算的时间约束，推导出其对应的雅可比矩阵形式：

根据步骤S1构建的航位推算的状态约束，推导出其对应的雅可比矩阵形式：

根据步骤S1构建的UWB相对测距约束，推导出其对应的雅可比矩阵形式：

S202、对因子图模型的架构方式进行设定：

对于系统中任一协同节点，状态变量包括节点位置坐标(x_k，y_k)、航向角θ_k和天向陀螺仪零偏b_k；因此，如图3所示，对于任一协同节点：

1)基于步骤S1中设定的待估计状态量设系统优化变量为所有时刻的状态变量，其表达式为：/>各时刻下的待估计状态量作为因子图模型的图顶点，并按照时序排布；

2)设定航位推算因子由航位推算的状态约束和航位推算的时间约束构成，

航位推算的状态约束为：

航位推算的时间约束为：

具体来说，对于各状态变量来说，两个相邻时刻的节点x轴位置坐标之间、节点y轴位置坐标之间、航向角之间、以及天向陀螺仪零偏b_k之间均通过航位推算的时间约束(即图3中的三角形标识)进行连线，表示该两个时刻的状态量之间存在航位推算的时间约束关系；而在同一时刻下，节点的天向陀螺仪零偏与航向角之间通过航位推算的状态约束进行连线，表示二者之间存在航位推算的状态约束关系(即图3中的方形标识)，而航向角分别与节点x轴位置坐标、节点y轴位置坐标之间通过航位推算的状态约束进行连线，表示其分别与节点x轴位置坐标、节点y轴位置坐标之间存在航位推算的状态约束的关系(即图3中的方形标识)；

3)对各状态变量添加总UWB因子；具体地，由于k时刻任一协同节点可能存在m个UWB相对测距约束，每个UWB相对测距约束为一个UWB因子，因此，设定在k时刻下任一协同节点对应有由m个UWB相对测距约束构成的总UWB因子，具体地，总UWB因子表示为：

S203、基于步骤S202中对图顶点以及边的连接形式的设定，构建得到各协同节点相应的因子图模型，即如图3所示。

S3、基于步骤S2构建的各协同节点的因子图模型，采用LM算法对目标函数：进行求解，以得到各节点在k时刻下的位置坐标(包括x轴位置坐标和y轴位置坐标)、航向角、陀螺仪零偏的优化结果；

如图4所示的系统中任一协同节点基于因子图模型图优化算法结构图；相应地，在该步骤S3中，以任一协同节点为例，对其获取待估计状态量的实施优化计算步骤进行具体描述如下：

S301、设定起始时刻(T＝1)状态变量的初值X₁，并将其代入至因子图模型中，得到T＝1时刻的优化结果

具体地，由于在初始时刻状态变量仅受相对测距因子约束，因此基于初值X₁和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数：进行求解，得到/>

S302、将步骤S301计算得到的添加到T＝2时刻下第一个状态的图顶点上(即沿图4中的黑色虚线路径)，作为历史时刻状态的初值X₁；将T＝1时刻的优化结果/>代入步骤S101的状态递推方程中，通过航位推算得到下一时刻(T＝2)的预测值X₂，该预测值X₂作为T＝2时刻的优化初始值；进而，通过将T＝2时刻下的初值X₁和X₂代入至因子图模型中，得到T＝2时刻的优化结果/>

具体地，由于在T＝2时刻下，初值X₁和X₂不仅各自受到相对测距因子约束，同时初值X₁和X₂之间还受到航位推算因子的约束，因此，基于初值X₁和X₂，以及航位推算的时间约束对应的雅可比矩阵、航位推算的状态约束对应的雅可比矩阵和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数：进行求解，得到优化后的/>

其中，在该步骤S302中，对于任一协同节点在某一时刻下的相对测距因子约束可能包括有该节点与其能够构建通信的锚点之间形成的约束、以及该节点与其接收通信的协同节点之间形成的约束；其中，

1)对于节点与其能够构建通信的锚点之间形成的UWB相对测距约束，其表达式为：其中，/>和/>为锚点的x轴位置坐标和y轴位置坐标；d_k为k时刻无人车通过UWB相对测距获得的其与锚点之间的距离观测值；

2)对于节点与其接收通信的协同节点之间形成的UWB相对测距约束，其表达式为：其中，/>和/>为另外协同节点的x轴位置坐标和y轴位置坐标；d_k为k时刻无人车通过UWB相对测距获得的其与另外协同节点之间的距离观测值；

其中，协同节点之间的通信关系基于“高通信精度节点向低精度节点进行信息传输”的原则，即通信精度高的节点向低于其通信精度的节点发送通信信息，通信信息包括位置信息(包括当前时刻的x轴位置坐标和y轴位置坐标)以及相对距离观测值；

S303、以此类推，在每个时刻重复步骤S302，以不断优化当前时刻(T＝k+1)下的状态变量

在本实施例中，如图5所示，协同节点1与两个通信基站之间能够建立通信，协同节点2与一个通信基站之间能够建立通信，该两个协同节点能够依靠锚点观测对自身零偏进行估计，而在不能与通信基站建立通信(处于拒止环境)时，各时刻的初值采用步骤S301的递推公式计算得到，因此依然可以依靠估计出的零偏结果对零偏进行补偿，提升在通信拒止环境下单节点的位置精度保持能力依靠锚点观测对自身零偏进行估计，进入室内拒止环境后依靠估计出的零偏结果对零偏进行补偿，提升在通信拒止环境下单节点的位置精度保持能力；

另外，根据各节点与各通信基站之间的通信关系可以确定，节点的通信精度关系为：节点1＞节点2＞节点3；因此，信息传输模式设定为：节点1向节点2和节点3发送其位置信息和相对距离观测值；节点2向节点3发送其位置信息和相对距离观测值，其接收节点1的位置信息和相对距离观测值作为一个UWB测距因子；节点3不发送任何信息，其分别接收的节点1和节点2的位置信息和相对距离观测值形成两个UWB测距因子；该协同节点之间的通信能够使低于其通信精度的各节点不断更新其UWB相对测距约束；该节点与其接收通信的协同节点之间形成的UWB相对测距约束在相对测距因子中的添加，能够实现通过在各协同节点之间建立相对导航约束，抑制复杂拒止环境下节点的导航误差发散；

如图5所示的三辆无人车节点为例，节点1从室外进入到室内，室外可同时接收到两个锚点的观测信息，室内由于墙壁遮挡，UWB信号时有时无，节点1能间歇接收到两个锚点其中的一个观测；节点1靠近墙壁行进，部分UWB观测信号可透过窗户到达无人车接收机，故节点1接收信号条件较好，可作为高精度节点；节点2距离锚点与节点1建立通信的锚点较远，但轨迹靠近西侧门，可接收到位于西侧的锚点的观测信息；节点3的信号接收情况最差，处于无锚点环境下；与此同时，依靠节点之间的协同，节点2可收到与节点1之间的相对测距信息，节点3可收到节点1和节点2的相对测距信息。

如图6所示为经过本发明实施例的基于图优化的协同导航方法得到的节点定位轨迹与传统航位推算算法下的节点定位轨迹对比图。从图中可以看出，与节点1构建通信的两个锚点坐标分别位于坐标(0,0)与(400,0)处，节点1靠近锚点一侧并保持水平直线运动，其在0～50s期间可同时收到2个锚点的通信信息；其在50s～100s无法接收到锚点的通信信息，其在100s～300s可以通过门/窗户间歇性交替接收到两个锚点中的其中一个通信信息，处于通信信息不同步的环境下。

另外，从图中可以确认，采用本申请的基于图优化的协同导航方法，节点1的行驶轨迹的参考值与优化后值完全重合，并与优化前值(即航位推算值)存在偏差；节点2的行驶轨迹的参考值、优化前值、优化后值几乎在一条线上；节点3的行驶轨迹的参考值与优化后值完全重合，并与优化前值存在偏差；可见三个节点导航定位误差均得到了有效校准；具体来说，节点1在观测信息时有时无、不同步的情况下仍能保持较高导航定位精度；节点2在仅接收到一个锚点观测情况下，利用节点1相对观测信息能够有效抑制误差发散；节点3在没有接收到锚节点观测的情况下，通过与节点1、节点2的协同，同样实现了位置误差校正，最终导航定位精度高于航位推算精度。

如图7(a)所示，自上而下分别为节点1、节点2和节点3的X轴定位误差曲线；如图7(b)所示，自上而下分别为节点1、节点2和节点3的Y轴定位误差曲线；从该两幅图中对比可知，节点1在观测信息时有时无且不同步的情况下，仍能保持定位误差小于0.2m；而节点2与节点3由于不能收到两个锚点的观测，定位精度低于节点1，但通过节点之间的协同，保持定位误差不发散，相比于单纯航位推算，导航定位精度获得了较大的提升。

如图8(a)所示，自上而下分别为节点1、节点2和节点3的航向角估计曲线；如图8(b)所示，自上而下分别为节点1、节点2和节点3的陀螺仪零偏估计曲线；从该两幅图中对比可知，三个节点的航向角均收敛至了真值附近；其中节点1依靠两个锚点有效估计出了陀螺仪零偏，估计结果1.0400e-3rad/s，真值为9.6914e-4rad/s，误差小于10％，与此同时，节点1在50s～100s的无锚点期间，通过零偏的补偿，航位推算没有产生明显的发散，导航精度得到了有效提高；节点2和节点3由于无法获得两个锚点的观测信息，零偏估计误差较大，但也能够收敛至真值附近上下波动。

如图8(a)所示，自上而下分别为节点1、节点2和节点3的航向角估计曲线；如图8(b)所示，自上而下分别为节点1、节点2和节点3的陀螺仪零偏估计曲线；从该两幅图中对比可知，三个节点的航向角均收敛至了真值附近；其中节点1依靠两个锚点有效估计出了陀螺仪零偏，估计结果1.0400e-3rad/s，真值为9.6914e-4rad/s，误差小于10％，与此同时，节点1在50s～100s的无锚点期间，通过零偏的补偿，航位推算没有产生明显的发散，导航精度得到了有效提高；节点2和节点3由于无法获得两个锚点的观测信息，零偏估计误差较大，但也能够收敛至真值附近上下波动。

如图9所示为节点1、节点2和节点3分别在优化前与优化后的定位均方误差的对比图；由图中可知，最终节点1的定位误差由1.5m提高到0.3m，相比单节点依靠惯性自主导航精度提高5倍；而节点3定位误差由1m提高到0.5m，综合定位精度提高1倍，充分证明了所提出方法的有效性。

综上所述，在复杂遮挡环境下，本申请的方法具备处理动态增删、时间异步的观测信息的能力，并可以对陀螺零偏进行实时估计与补偿，有效提高了多辆无人车系统在复杂遮挡环境下的各个协同节点导航定位精度。

本发明未详细公开的部分属于本领域的公知技术。尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化时显而易见的，一切利用本发明构思的发明创造均为保护之列。

Claims (6)

1.一种基于图优化的通信受限环境下协同导航方法，其特征在于，其步骤如下：

S1、基于各智能体配备有惯性传感器、里程计和超宽带测距传感器，与智能体建立通信基站为超宽带基站，建立由智能体航位推算约束和UWB相对测距约束构成的传感器观测模型；

其中，在传感器观测模型中，智能体状态变量包括节点位置坐标(x_k，y_k)、航向角θ_k和天向陀螺仪零偏b_k；智能体航位推算约束根据智能体自身基于惯性传感器的航位推算信息建立而成，航位推算信息包括；UWB相对测距约束根据各智能体之间相对测距信息及智能体与锚点之间的相对测距信息建立而成；

S2、基于航位推算约束和UWB相对测距约束，设定因子图模型的图顶点以及边的连接形式，进而构建因子图模型；

S3、基于各协同节点的因子图模型，采用LM算法对目标函数： 进行求解，以得到各节点在k时刻下的位置坐标(包括x轴位置坐标和y轴位置坐标)、航向角、陀螺仪零偏的优化结果；其中，/>为航位推算的状态约束，/>为航位推算的时间约束，/>为UWB相对测距约束。

2.根据权利要求1所述的基于图优化的通信受限环境下协同导航方法，其特征在于，在步骤S1中，智能体航位推算约束包括航位推算的时间约束和航位推算的状态约束，其构建步骤为：

1)设定无人机自身惯性传感器提供的天向陀螺仪k时刻的角速率为ω_k，里程计输出的k时刻的速率观测值为v_k，采样间隔时间为t_s，且设定陀螺零偏不随时间变化；则，智能体在k-1时刻与k时刻之间的状态递推方程为：

θ_k＝θ_k-1+(ω_k-1-b_k)t_s，

x_k＝x_k-1+v_k-1t_scosθ_k，

y_k＝y_k-1+v_k-1t_ssinθ_k，

式中，θ_k为智能体k时刻的航向角，θ_k-1为k-1时刻的航向角，ω_k-1为k-1时刻的角速率，x_k为k时刻的x轴位置坐标，x_k-1为k-1时刻的x轴位置坐标，v_k-1为k-1时刻的速率，y_k为k时刻的y轴位置坐标，y_k-1为k-1时刻的y轴位置坐标；b_k为k时刻的天向陀螺零偏b_k；

2)设待估计状态量为式中，/>为智能体待估计的k时刻x轴位置坐标，/>为智能体待估计的k时刻y轴位置坐标，/>为智能体待估计的k时刻航向角，为智能体待估计的k时刻天向陀螺零偏；

3)构建航位推算的时间约束，其表达式为：

式中，为k-1到k时刻之间关于x轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于y轴位置坐标的时间约束残差，/>为k-1到k时刻之间关于航向角的时间约束残差，/>为k-1到k时刻之间关于天向陀螺零偏的时间约束残差，/>为智能体待估计的k-1时刻x轴位置坐标，/>为智能体待估计的k-1时刻y轴位置坐标，/>为智能体待估计的k-1时刻航向角，/>为智能体待估计的k-1时刻天向陀螺零偏；

4)构建航位推算的状态约束，其表达式为：

式中，为k时刻关于x轴位置坐标与航向角的状态约束残差，/>为k时刻关于x轴位置坐标与航向角的状态约束残差；/>为k时刻关于航向角的状态约束残差，θ₀为航向角的初值，i为历史某一时刻。

3.根据权利要求1所述的基于图优化的通信受限环境下协同导航方法，其特征在于，在步骤S1中，UWB相对测距约束构建为：式中，为k时刻的UWB相对测距约束残差，dk为k时刻智能体通过UWB相对测距获得的其与其它协同智能体之间的距离观测值、或其与锚点之间的距离观测值；(x_anc，y_anc)为其它协同智能体或锚点的坐标。

4.根据权利要求1所述的基于图优化的通信受限环境下协同导航方法，其特征在于，对于任一智能体，步骤S2的具体实施步骤为：

S201、根据步骤S1构建的航位推算的时间约束、航位推算的状态约束和UWB相对测距约束分别推导出与之对应的雅可比矩阵；

S202、对任一智能体的因子图模型的架构方式进行设定：

1)设智能体系统优化变量为所有时刻的状态变量，其表达式为：各时刻下的待估计状态量作为因子图模型的图顶点，并按照时序排布；

2)设定每相邻时刻之间的状态变量通过航位推算因子进行连线；其中，航位推算因子由航位推算的状态约束：和和航位推算的时间约束：构成；智能体每两个相邻时刻下的x轴位置坐标之间、节点y轴位置坐标之间、航向角之间、以及天向陀螺仪零偏bk之间均通过航位推算的时间约束进行连线；智能体同一时刻下的天向陀螺仪零偏与航向角之间通过航位推算的状态约束进行连线，同时航向角分别与节点x轴位置坐标、节点y轴位置坐标之间通过航位推算的状态约束进行连线；

3)对各状态变量添加总UWB因子：各状态变量均与当前状态对应的总UWB因子进行连线；

S203、基于步骤S202中对图顶点以及边的连接形式的设定，构建得到各协同节点相应的因子图模型。

5.根据权利要求1所述的基于图优化的通信受限环境下协同导航方法，其特征在于，在步骤S3中，对任一智能体自k时刻至k+1时刻的状态变量优化结果求解方法为：

1)将k时刻下得到的状态变量优化值作为k+1时刻下状态变量初始值X＝[X₁，X₂，...，X_k]；将k时刻的状态变量/>代入航位推算的状态递推方程中，推算得到k+1时刻的状态变量X_k，并作为状态变量初始值，得到k+1时刻下状态变量初始值X＝[X₁，X₂，...，X_k，X_k+1]；

2)确定各时刻下状态变量受到的约束，包括智能体在每相邻时刻下的状态变量之间受到的航位推算约束以及智能体在各时刻受到的UWB相对测距约束；

3)基于k+1时刻下状态变量初始值X＝[X₁，X₂，...，X_k]，以及航位推算的时间约束对应的雅可比矩阵、航位推算的状态约束对应的雅可比矩阵和UWB相对测距约束对应的雅可比矩阵，采用LM算法对目标函数进行求解，得到k+1时刻下各状态变量优化值

6.根据权利要求5所述的基于图优化的通信受限环境下协同导航方法，其特征在于，在步骤S3的UWB相对测距约束确定中，智能体之间通信关系基于“高通信精度节点向低精度节点进行信息传输”的原则进行，通信信息包括位置信息和相对距离观测值。