基于因子图权值自适应地下管网机器人多源融合定位方法

1.本发明属于地下机器人定位领域，更具体的说是基于因子图权值自适应地下管网机器人多源融合定位方法。


背景技术：

2.近年来，随着城市地上空间逐渐饱和，城市地下空间拓展已成为趋势。城市地下管廊是保证城市生活正常运转最重要的基础设施。
3.目前城市地下管廊系统运行管理大多仍停留在人工阶段、系统的自动化程度低、应变能力差。因此，地下管廊巡检机器人可以有效避免人工巡检的缺陷。然而，受地下环境、地理空间、排水情况、传感信号弱等因素限制，常规导航方式并不适用，地下环境中机器人的自主定位、导航决策已成为一个亟待解决的问题。
4.目前，针对地下管廊机器人的定位技术主要有以下方案：
5.1.基于极低频(elf)电磁波的定位技术。针对金属管壁及岩土环境对常规电磁信号的屏蔽问题，穿透性较强，对地下环境有较好的感知，极低频信号所需的发射功率较高，并且磁场信号衰减的较快，探测范围较小。
6.2.基于超宽带(uwb)的定位技术。
7.在地下管廊和地表联通区域以及可以安装uwb定位模块，基于指纹定位有以rss、csi的特征的定位方法。基于rss的指纹定位精度受非视距影响较大，以csi为特征的指纹定位在非视距情况下定位精度较好。
8.3.基于惯导的定位技术。
9.首先，mems-imu输出频率高、不受环境干扰的特征，具有辅助定位信号快速捕获的技术优势。由于机器人运行中，存在常规绝对定位初始化时间长，且无法实现定位结果高频输出的问题。
10.其次，机器人测姿系统的精度与可靠性主要依赖于mems-imu器件。惯性器件在机器人运行过程中易受温度、振动等因素影响，使得标定参数发生改变，导致惯性器件精度下降；辅助mems-imu测姿的各传感器在提供观测量时，由于机器人运行环境多变，存在失效或质量下降问题。因此，如何充分利用惯导信息提高绝对定位测姿系统实时性、连续性、高频输出问题亟待解决。
11.4.基于管网地图的定位技术。
12.地图匹配定位根据已知环境地图确定机器人位置。首先使用观测传感器对当前环境进行观测并生成局部地图，把局部地图和已知的全局环境地图进行特征匹配，根据匹配结果最终确定机器人在已知环境地图中的位置和方向。管网地图可以用于地图匹配或地图约束提高定位精度。
13.目前现有技术如下：
14.公开号：cn112577496a，名称：“一种基于自适应选权的多源融合定位方法”其方法步骤为：根据传感器初始数据获取初始节点；由初始节点生成初始位置变量节点和初始误
差变量节点；惯性导航因子对位置变量节点进行拓展，误差因子则对误差变量节点进行拓展，并且由误差变量节点对惯性导航因子进行纠正；按照不同权重将卫星导航因子和视觉里程计因子的输出结果与位置变量节点中的定位结果进行数据融合。本发明将自适应选权的因子图模型应用于多源融合导航定位，用较为简便的方式实现了对传感器因子自适应选权的过程，能够通过动态调整权重实现卫星定位信息的故障排除和重新接入，并且在一定程度上减小定位误差。
15.其采用的方案是，利用因子图的位置变量节点中的定位结果与卫星导航因子的定位结果之间的位置差值构建权函数完成自适应选权的方法。而我们采用的方案是，先根据传感器量测值与预测值得残差用动态权值函数进行计算，然后将各传感器每一时刻的权值用聚类算法进行处理，聚类中心点即为权值，可以有效提高定位的准确性。
16.公开号：cn111337020a，名称：“引入抗差估计的因子图融合定位方法”其包括：获取传感器量测信息；基于所述量测信息确定出由位置、速度、姿态以及偏差量构成的多维状态量，构建状态空间模型，从而建立组合导航系统模型；根据所述导航系统模型的预测残差向量构造检验统计量，判断是否有异常观测的存在；若存在，则引入指数衰减自适应因子自动调整观测噪声；若不存在，则构建因子图模型；定义所述量测信息为因子图的因子节点，定义所述导航系统模型的状态量信息为因子图的变量节点，构建基于因子图的多源信息融合的系统框架图，从而构建因子图模型；推算所述因子图模型，根据所述推算结果得到定位信息。以实现提高定位精确度的优点。
17.其采用的方案是，根据所述导航系统模型的预测残差向量构造检验统计量，判断是否有异常观测的存在。若存在，则引入指数衰减自适应因子自动调整观测噪声；若不存在，则构建因子图模型。异常观测通过统计量的置信度进行判断。而我们的专利中直接构建因子图模型，通过根据动态权值函数和聚类算法实现对异常观测的处理，隔离该时刻的传感器因子，使用其他传感器因子进行融合，在地下传感器信号易受干扰复杂环境中误差更小。
18.公开号：cn111780755a，名称：“一种基于因子图和可观测度分析的多源融合导航方法”，该方法包括以下步骤：构建基于惯导/辅助传感器组合导航模型的多源融合导航系统，得到以惯导为核心，卫星、视觉、里程计中两种或者两种以上为辅助传感器的组合导航鲁棒卡尔曼子滤波器；基于各组合导航鲁棒卡尔曼子滤波器的导航解算结果，度量各各组合导航鲁棒卡尔曼子滤波器状态变量的可观测度；采用增量式因子图架构，根据多源组合导航因子的可信度评价，在线选择最佳因子参与融合，并自动调节信息分配的权重，从而实现跨场景的多源融合导航。本发明能够实现多传感器的自适应融合与安全可靠导航定位，提高了惯性/卫星/视觉等多源导航信息融合的精度和可靠性。
19.其采用的方案是，构建惯性/卫星、惯性/视觉等组合导航的卡尔曼子滤波器，通过设计自适应鲁棒的卡尔曼滤波算法，优化滤波算法并减少不确定因素的影响。然后，基于动态时变系统的可观测度分析，在线度量惯性/卫星、惯性/视觉等组合导航因子的可信度。而我们的专利中没有使用卡尔曼滤波，通过根据动态权值函数和聚类算法进行定权，构建elf\uwb\ins\地图因子图模型，系统的复杂度更低。
20.现有的因子图算法，多源信息融合大多是根据初始的权值分配，然而地下环境中各子导航系统会受到环境和自身精度影响，产生异常观测值，因子图将异常值进行多源融
合，导致组合导航精度下降。本发明提出了种基于因子图权值自适应的地下管网机器人多源融合定位方法。每个传感器的权值根据动态权值函数进行实时调整，将观测值异常的传感器进行降权，甚至隔离。实现了组合导航信息的有效融合，增强了因子图算法的鲁棒性，从而提高了导航精度。
21.针对城市地下管廊机器人自主精准导航问题，本专利提出了一种基于因子图权值自适应的地下管网机器人多源融合定位方法，可以较为有效地解决地下环境定位精度差的问题。


技术实现要素：

22.为解决上述技术问题，本发明提出了基于因子图权值自适应地下管网机器人多源融合定位方法，可以较为有效地解决地下环境定位精度差的问题，融合了惯性导航定位、极低频定位、射频定位以及地下管网地图的组合定位方法，能够实现地下管网机器人高精度定位。
23.为实现上述目的，本发明采取的技术方案是：
24.本发明提供基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于，包括以下步骤：
25.s1：系统初始化，设置ins、uwb、elf、地图导航子系统的初始参数，确定滑窗大小s，建立ins\uwb\elf量测和误差模型,构建ins\uwb\elf因子节点与其代价函数,建立基于因子图权值自适应的多源融合定位模型；
26.s2：当有新的量测信息输入时，判断是否满足滑动窗口范围:当超出窗口范围时，则向前滑动窗或产生新的窗，并将上一窗内估计得到的状态量和观测量分布作为当前窗的先验估计；而当满足滑动窗口大小时，则直接进行s3；
27.s3：添加相应因子，根据不同传感器的量测信息结果对相应因子权值动态调整，隔离失效因子，进行增量更新处理，进一步对导航信息进行更新融合，迭代获取最优估计值；
28.s4：输出滑动窗内各状态节点的状态估计值，将各系统的协方差矩阵和均值向量的最终估计值作为下一个滑动窗内各参量的初始值，滑动窗向后移动k个节点；
29.s5：判断是否有新的量测信息输入，有则重复步骤s2，否则结束更新；
30.s6：根据上述得到的最优估计值序列，建立隐马尔可夫模型，根据观测代价和转换代价匹配地图数据，当一个最优估计值序列使用hmm匹配完成之后，使用维特比算法对匹配过程进行回溯，计算匹配结果，得到多源融合定位结果。
31.作为本发明进一步改进，所述步骤s1中，所述ins因子节点与其代价函数：
32.根据北东地载体坐标系，建立基于因子图多源融合定位模型，因子图中正方形表示因子节点，圆形表示变量节点；
33.设k时刻的状态变量为xk[0034][0035]
其中：p＝[x y z]
t
为载体位置向量；v＝[v
x v
y vz]
t
为载体速度向量；q0＝[q
1 q
2 q
3 q4]为载体姿态四元数向量；为加速度计随机游走项；为陀螺仪随机游走项；
[0036]
其代价函数为：
[0037][0038]
xk,x
k+1
为k,k+1时刻的状态变量，为ins偏差量测噪声协方差，h(xk,x
k+1
)为ins量测结果。
[0039]
作为本发明进一步改进，所述步骤s1中，所述uwb因子节点与其代价函数，具体为：
[0040]
建立uwb量测模型，构建uwb因子节点与其代价函数；
[0041]
将信道状态信息csi作为特征建立uwb指纹库，构建uwb因子节点；
[0042]
该定位方法分为两个阶段：离线阶段和在线阶段，在离线阶段，首先收集csi数据幅值，以建立csi原始指纹库，然后利用稀疏自动编码器sae将输入的csi数据压缩到特征层，输出特征层进一步连接到1d-cnn模型进行训练测试；
[0043]
csi＝[|csi
1,1,1
|,|csi
1,1,2
|,...|csi
1,1,q
|,...|csi
1,2,1
|,...|csi
m,n,q
|]
[0044]
其中m为是发射机天线数、n为接收机天线，q为信道数即子载波数；
[0045]
在在线阶段，地下机器人通过匹配接收到的csi量测值解码，根据回归模型获得定位结果，将uwb的量测坐标输入到因子图中，构建uwb因子节点；
[0046]
sae输入层为m*n*q个神经元，两个隐藏层分别为l个、q个神经元，三层的一维卷积神经网络1d-cnn包括卷积层、批量归一化层、激活层、池层、完全连接fc层和softmax输出层，激活函数为relu函数；
[0047]
其代价函数为：
[0048][0049]
其中n是验证样本的总数，y(x)是期望的输出，j表示网络中的层数，hj(x)是当x是输入时网络输出的激活向量。
[0050]
作为本发明进一步改进，所述步骤s1中，所述elf因子节点与其代价函数：
[0051]
建立elf量测模型，构建ins因子节点与其代价函数，通过传感器阵列，将磁信号转化为电压，然后通过粒子群拟牛顿混合算法进行非线性解算；
[0052]
其代价函数为：
[0053][0054]
其中，min是最小值函数，ui是电压观测值，ui是电压状态变量。
[0055]
作为本发明进一步改进，所述步骤s3中，所述elf因子节点与其代价函数：
[0056]
基于因子图的多源融合导航算法融合框架以ins因子为基础，随着时间更新，对ins进行预积分，并用滑窗进行优化，当导航系统接收到elf和uwb传感器的量测信息时，将对应的传感器因子节点添加到导航系统的因子图融合框架中，根据其对应的传感器量测方程以及其对应的代价函数对相关的变量节点进行更新；
[0057]
其代价函数为：
[0058][0059]
其中i表示该时刻各类导航系统的因子，argmin表示使目标函数取最小值时的变
量值，k(di)是动态权重函数，hi(xi)是不同导航系统的量测函数，zi是由各类导航系统得到的实际量测值。
[0060]
作为本发明进一步改进，所述步骤s3中,该方法基于残差的对因子图动态权值自适应调整步骤如下:
[0061][0062]
其中，k(di)是动态权重函数，δd2表示传感器的量测值和预测值的残差的平方，称为可信距离，参数θ为系数；
[0063]
将各传感器每一时刻的权值k(di)用hdbscan算法进行聚类处理，聚类中心点即为权值，每次新增的未聚类的离散点判定该时刻传感器因子节点失效，直接隔离该因子不使用该因子进行融合；
[0064]
将所有因子的权值k(di)进行归一化处理，后进行后续最优估计。
[0065]
作为本发明进一步改进，所述步骤s3中隔离失效因子如下：
[0066]
对传感器因子节点进行实时判定，若新增点未被聚类，隔离判定失效的传感器因子；对ins因子，连续t个时刻均未被聚类，则判定ins的累积误差较大，ins定位失效，隔离ins因子。
[0067]
作为本发明进一步改进，所述步骤s6中建立隐马尔可夫模型具体如下；
[0068]
对于每一个计算的最优估计值点,首先确定一组候选路段.如果发现计算的最优估计值点非常接近某个路段,就给这个路段指定一个较低的代价值.然后为马尔可夫链中连接每一对相邻顶点的边计算权重，最终,在马尔可夫链上找到最大似然路径；
[0069]cmatch
＝co+c
t
[0070]
其中，c
match
表示地图匹配的总代价，co表示地图匹配的观测代价，c
t
表示地图匹配的转换代价。
[0071]
作为本发明进一步改进，所述步骤s6中观测代价如下：
[0072]
(1)计算最优估计值点到匹配路段投影点的距离，距离越大，代价越大；
[0073]
(2)计算最优估计值点前进方向与路段的方向差，方向差越大，代价越大；
[0074]
(3)加权平均距离代价及方向代价的值作为最终观测代价；
[0075]
(4)如果最优估计值精度较低，相应地降低距离、方向代价；
[0076]
(5)最优估计值的速度较低时，方向代价需要降低。
[0077]
作为本发明进一步改进，所述步骤s6中转换代价如下：
[0078]
转换代价为上一个最优估计值观测点到下一个观测点的过程中，隐藏序列即匹配到路段上的点，从上一个点对应的某个路段到下一个点对应的某个路段的代价，路段距离越接近两观测点间的距离代价越小。
[0079]
与现有技术相比，本发明的有益效果为：
[0080]
1.本发明通过将极低频(elf)信号定位、惯性导航系统(ins)定位、超宽带(uwb)定位和地下管网地图先验信息用因子图进行融合，有效提高了在信号屏蔽性较强的地下环境中的定位精度。
[0081]
2.本发明引入基于残差的因子图权值自适应函数，通过对其可信度的判断，实现各个因子的即插即用和权值自适应调整。
[0082]
3.本发明改进了基于csi的uwb指纹定位网络，通过sae网络和1d-cnn网络训练识别，提高在uwb非视距情况下的定位精度。
附图说明
[0083]
图1地下管网机器人多源融合定位方法流程图；
[0084]
图2基于残差的动态权值自适应调整改进的因子图原理图；
[0085]
图3基于csi的uwb指纹定位网络。
具体实施方式
[0086]
下面结合附图与具体实施方式对本发明作进一步详细描述：
[0087]
本发明的总体流程如图1所示，基于残差的动态权值自适应调整改进的因子图原理图如图2所示，基于csi的uwb指纹定位网络如图3所示，本发明提供基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：包括以下步骤：
[0088]
本发明设计了基于因子图权值自适应的地下管网机器人多源融合定位方法，融合了惯性导航定位、极低频定位、射频定位以及地下管网地图的组合定位方法，能够实现地下管网机器人高精度定位。
[0089]
s1：系统初始化，设置ins、uwb、elf、地图导航子系统的初始参数，确定滑窗大小s，建立ins\uwb\elf量测和误差模型,构建ins\uwb\elf因子节点与其代价函数,建立基于因子图权值自适应的多源融合定位模型；
[0090]
s2：当有新的量测信息输入时，判断是否满足滑动窗口范围:当超出窗口范围时，则向前滑动窗或产生新的窗，并将上一窗内估计得到的状态量和观测量分布作为当前窗的先验估计；而当满足滑动窗口大小时，则直接进行s3；
[0091]
s3：添加相应因子，根据不同传感器的量测信息结果对相应因子权值动态调整，进行增量更新处理，进一步对导航信息进行更新融合，迭代获取最优估计值；
[0092]
s4：输出滑动窗内各状态节点的状态估计值，将各系统的协方差矩阵和均值向量的最终估计值作为下一个滑动窗内各参量的初始值，滑动窗向后移动k个节点；
[0093]
s5：判断是否有新的量测信息输入，有则重复步骤s2，否则结束更新；
[0094]
s6：根据上述得到的最优估计值序列，建立隐马尔可夫模型，用维特比算法进行地图匹配解算，得到多源融合定位结果。
[0095]
根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s1中，所述ins\uwb\elf因子节点与其代价函数：
[0096]
根据北东地载体坐标系，建立基于因子图多源融合定位模型，因子图中正方形表示因子节点，圆形表示变量节点；
[0097]
设k时刻的状态变量为xk[0098][0099]
其中：p＝[x y z]
t
为载体位置向量；v＝[v
x v
y vz]
t
为载体速度向量；q0＝[q
1 q
2 q
3 q4]为载体姿态四元数向量；为加速度计随机游走项；为陀螺仪随机游走项；
[0100]
其代价函数为：
[0101][0102]
为ins偏差量测噪声协方差，h(xk,x
k+1
)为ins量测结果；
[0103]
建立uwb量测模型，构建uwb因子节点与其代价函数；
[0104]
将信道状态信息csi作为特征建立uwb指纹库，构建uwb因子节点；
[0105]
该定位方法分为两个阶段：离线阶段和在线阶段，在离线阶段，首先收集csi数据幅值，以建立csi原始指纹库，然后利用稀疏自动编码器sae将输入的csi数据压缩到特征层，输出特征层进一步连接到1d-cnn模型进行训练测试；
[0106]
csi＝[|csi
1,1,1
|,|csi
1,1,2
|,...|csi
1,1,q
|,...|csi
1,2,1
,|...|csi
m,n,q
|]
[0107]
其中m为是发射机天线数、n为接收机天线，q为信道数即子载波数；
[0108]
在在线阶段，地下机器人通过匹配接收到的csi量测值解码，根据回归模型获得定位结果，将uwb的量测坐标输入到因子图中，构建uwb因子节点；
[0109]
sae输入层为m*n*q个神经元，两个隐藏层分别为l个、q个神经元，三层的一维卷积神经网络1d-cnn包括卷积层、批量归一化层、激活层、池层、完全连接fc层和softmax输出层，激活函数为relu函数；
[0110]
其代价函数为：
[0111][0112]
其中n是验证样本的总数，y(x)是期望的输出，j表示网络中的层数，hj(x)是当x是输入时网络输出的激活向量；
[0113]
建立elf量测模型，构建ins因子节点与其代价函数，通过传感器阵列，将磁信号转化为电压，然后通过粒子群拟牛顿混合算法进行非线性解算；
[0114]
其代价函数为：
[0115][0116]
根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s3中，因子图原理如下：
[0117]
基于因子图的多源融合导航算法融合框架以ins因子为基础，随着时间更新，对ins进行预积分，并用滑窗进行优化，当导航系统接收到elf和uwb传感器的量测信息时，将对应的传感器因子节点添加到导航系统的因子图融合框架中，根据其对应的传感器量测方程以及其对应的代价函数对相关的变量节点进行更新；
[0118]
其代价函数为：
[0119][0120]
其中hi(xi)是不同导航系统的量测函数，zi是由各类导航系统得到的实际量测值。
[0121]
根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s3中,该方法基于残差的对因子图动态权值自适应调整步骤如下:
[0122][0123]
其中，k(di)动态权重函数，δd2表示传感器的量测值和预测值的残差的平方，称为可信距离，参数θ为系数；
[0124]
将各传感器每一时刻的权值k(di)用hdbscan算法进行聚类处理，聚类中心点即为权值，每次新增的未聚类的离散点判定该该时刻传感器因子节点失效，直接隔离该因子不使用该因子进行融合，对ins因子，连续t个时刻均未被聚类，则判定ins的累积误差较大，ins定位失效，隔离ins因子；
[0125]
将所有因子的权值k(di)进行归一化处理，后进行后续最优估计。
[0126]
根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s6中，根据上述得到的最优估计值序列，建立隐马尔可夫模型，根据观测代价和转换代价匹配地图数据，当一个最优估计值序列使用hmm匹配完成之后，使用维特比算法对匹配过程进行回溯，计算匹配结果，得到多源融合定位结果；
[0127]
对于每一个计算的最优估计值点,首先确定一组候选路段.如果发现计算的最优估计值点非常接近某个路段,就给这个路段指定一个较低的代价值.然后为马尔可夫链中连接每一对相邻顶点的边计算权重，最终,在马尔可夫链上找到最大似然路径.
[0128]
观测代价如下：
[0129]
(1)计算最优估计值点到匹配路段投影点的距离，距离越大，代价越大；
[0130]
(2)计算最优估计值点前进方向与路段的方向差，方向差越大，代价越大；
[0131]
(3)加权平均距离代价及方向代价的值作为最终观测代价；
[0132]
(4)如果最优估计值精度较低，相应地降低距离、方向代价；
[0133]
(5)最优估计值的速度较低时，方向代价需要降低；
[0134]
转换代价是指上一个最优估计值观测点到下一个观测点的过程中，隐藏序列即匹配到路段上的点，从上一个点对应的某个路段到下一个点对应的某个路段的代价，路段距离越接近两观测点间的距离代价越小；
[0135]cmatch
＝co+c
t
[0136]
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

技术特征：
1.基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于，包括以下步骤：s1：系统初始化，设置ins、uwb、elf、地图导航子系统的初始参数，确定滑窗大小s，建立ins\uwb\elf量测和误差模型,构建ins\uwb\elf因子节点与其代价函数,建立基于因子图权值自适应的多源融合定位模型；s2：当有新的量测信息输入时，判断是否满足滑动窗口范围:当超出窗口范围时，则向前滑动窗或产生新的窗，并将上一窗内估计得到的状态量和观测量分布作为当前窗的先验估计；而当满足滑动窗口大小时，则直接进行s3；s3：添加相应因子，根据不同传感器的量测信息结果对相应因子权值动态调整，隔离失效因子，进行增量更新处理，进一步对导航信息进行更新融合，迭代获取最优估计值；s4：输出滑动窗内各状态节点的状态估计值，将各系统的协方差矩阵和均值向量的最终估计值作为下一个滑动窗内各参量的初始值，滑动窗向后移动k个节点；s5：判断是否有新的量测信息输入，有则重复步骤s2，否则结束更新；s6：根据上述得到的最优估计值序列，建立隐马尔可夫模型，根据观测代价和转换代价匹配地图数据，当一个最优估计值序列使用hmm匹配完成之后，使用维特比算法对匹配过程进行回溯，计算匹配结果，得到多源融合定位结果。2.根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s1中，所述ins因子节点与其代价函数：根据北东地载体坐标系，建立基于因子图多源融合定位模型，因子图中正方形表示因子节点，圆形表示变量节点；设k时刻的状态变量为x
k
其中：p＝[x y z]
t
为载体位置向量；v＝[v
x v
y v
z
]
t
为载体速度向量；q0＝[q
1 q
2 q
3 q4]为载体姿态四元数向量；为加速度计随机游走项；为陀螺仪随机游走项；其代价函数为：x
k
,x
k+1
为k,k+1时刻的状态变量，∑bias
(k,k+1)
为ins偏差量测噪声协方差，h(x
k
,x
k+1
)为ins量测结果。3.根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s1中，所述uwb因子节点与其代价函数，具体为：建立uwb量测模型，构建uwb因子节点与其代价函数；将信道状态信息csi作为特征建立uwb指纹库，构建uwb因子节点；该定位方法分为两个阶段：离线阶段和在线阶段，在离线阶段，首先收集csi数据幅值，以建立csi原始指纹库，然后利用稀疏自动编码器sae将输入的csi数据压缩到特征层，输出特征层进一步连接到1d-cnn模型进行训练测试；csi＝[|csi
1,1,1
|,|csi
1,1,2
|,...|csi
1,1,q
|,...|csi
1,2,1
|,...|csi
m,n,q
|]
其中m为是发射机天线数、n为接收机天线，q为信道数即子载波数；在在线阶段，地下机器人通过匹配接收到的csi量测值解码，根据回归模型获得定位结果，将uwb的量测坐标输入到因子图中，构建uwb因子节点；sae输入层为m*n*q个神经元，两个隐藏层分别为l个、q个神经元，三层的一维卷积神经网络1d-cnn包括卷积层、批量归一化层、激活层、池层、完全连接fc层和softmax输出层，激活函数为relu函数；其代价函数为：其中n是验证样本的总数，y(x)是期望的输出，j表示网络中的层数，h
j
(x)是当x是输入时网络输出的激活向量。4.根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s1中，所述elf因子节点与其代价函数：建立elf量测模型，构建ins因子节点与其代价函数，通过传感器阵列，将磁信号转化为电压，然后通过粒子群拟牛顿混合算法进行非线性解算；其代价函数为：其中，min是最小值函数，u
i
是电压观测值，u
i
是电压状态变量。5.根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s3中，所述elf因子节点与其代价函数：基于因子图的多源融合导航算法融合框架以ins因子为基础，随着时间更新，对ins进行预积分，并用滑窗进行优化，当导航系统接收到elf和uwb传感器的量测信息时，将对应的传感器因子节点添加到导航系统的因子图融合框架中，根据其对应的传感器量测方程以及其对应的代价函数对相关的变量节点进行更新；其代价函数为：其中i表示该时刻各类导航系统的因子，argmin表示使目标函数取最小值时的变量值，k(d
i
)是动态权重函数，h
i
(x
i
)是不同导航系统的量测函数，z
i
是由各类导航系统得到的实际量测值。6.根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s3中,该方法基于残差的对因子图动态权值自适应调整步骤如下:其中，k(d
i
)是动态权重函数，δd2表示传感器的量测值和预测值的残差的平方，称为可信距离，参数θ为系数；将各传感器每一时刻的权值k(d
i
)用hdbscan算法进行聚类处理，聚类中心点即为权值，每次新增的未聚类的离散点判定该时刻传感器因子节点失效，直接隔离该因子不使用该因
子进行融合；将所有因子的权值k(d
i
)进行归一化处理，后进行后续最优估计。7.根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s3中隔离失效因子如下：对传感器因子节点进行实时判定，若新增点未被聚类，隔离判定失效的传感器因子；对ins因子，连续t个时刻均未被聚类，则判定ins的累积误差较大，ins定位失效，隔离ins因子。8.根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s6中建立隐马尔可夫模型具体如下；对于每一个计算的最优估计值点,首先确定一组候选路段.如果发现计算的最优估计值点非常接近某个路段,就给这个路段指定一个较低的代价值.然后为马尔可夫链中连接每一对相邻顶点的边计算权重，最终,在马尔可夫链上找到最大似然路径；c
match
＝c
o
+c
t
其中，c
match
表示地图匹配的总代价，c
o
表示地图匹配的观测代价，c
t
表示地图匹配的转换代价。9.根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s6中观测代价如下：(1)计算最优估计值点到匹配路段投影点的距离，距离越大，代价越大；(2)计算最优估计值点前进方向与路段的方向差，方向差越大，代价越大；(3)加权平均距离代价及方向代价的值作为最终观测代价；(4)如果最优估计值精度较低，相应地降低距离、方向代价；(5)最优估计值的速度较低时，方向代价需要降低。10.根据权利要求1所述的基于因子图权值自适应地下管网机器人多源融合定位方法，其特征在于：所述步骤s6中转换代价如下：转换代价为上一个最优估计值观测点到下一个观测点的过程中，隐藏序列即匹配到路段上的点，从上一个点对应的某个路段到下一个点对应的某个路段的代价，路段距离越接近两观测点间的距离代价越小。

技术总结
基于因子图权值自适应地下管网机器人多源融合定位方法，对惯性导航定位、极低频定位、射频定位以及地下管网地图多源传感器进行组合定位方法。本方法首先根据INS\UWB\ELF量测和误差模型，构建INS\UWB\ELF因子节点与其代价函数，通过每一时刻的量测误差，对因子图中各传感器权值进行动态更新更新调整，并应用滑窗约束，降低导航先验信息的维度，根据初步融合的最优估计序列，并以此建立隐马尔可夫模型，进行地图匹配用维特比算法求解，得到多源融合定位结果，实现地下管网机器人在地下的高精度定位。精度定位。精度定位。


技术研发人员：阳媛 蔡杰 任近静 姚逸卿 李响 张巧 何昊霖 杜倩倩
受保护的技术使用者：东南大学
技术研发日：2023.04.24
技术公布日：2023/8/1