CN115655268B 一种基于图优化算法的地下矿井掘进机导航定位方法 （江苏崇创嘉航智能科技有限公司）

一种基于图优化算法的地下矿井掘进机导本发明公开了一种基于图优化算法的地下性测量单元与超宽带测距传感器之间的相对位型框架下的导航建模与求解能够处理观测信息灵活可靠，行进中自动抛下UWB锚点形成自动组2S1、对惯性测量单元零偏进行标定，计算惯性测量单元S2、在掘进机行进时，利用车体携带的惯性测量S3、掘进机在行进中每相隔一定距离安放一对超Y1t2t本方法步骤S1中在起点处利用所述惯性测量单元获取初始航向角A0，t为t时刻的速度观测值；其中，D1描述了在t时刻下掘进机的位置与一组锚点中D2描述了在t时刻下掘进机的位置与一组锚点中的第二个锚点之间的距离方程，用于构建掘进机当前的位置与锚点之间的位置所计算出的理论距离与实际超宽带所测量的距离之所述因子图模型对应的数学形式为多个残差方程，采用非线性优化算3其中，d1为掘进机上的超宽带测距传感器测量的与第一个锚点之间为掘进机上的超宽带测距传感器测量的与第二个锚点之间的距离测量值，为t时刻掘进机与所放置的一对锚点中的第一个锚点之间的约束残差方程，为t时刻掘进机与所放置的一对锚点中的第二个锚点之间的约束残差方程，代表t时刻下掘进机当前的位置与锚点点中的第一个锚点之间的约束残差方程，为i时刻掘进机与所放置的一对锚点中的第二在上述所计算出的残差函数e(x)基础上，通过2.根据权利要求1所述的基于图优化算法的地下矿井掘进机导航定位方法，其特征在3.根据权利要求1所述的基于图优化算法的地下矿井掘进机导航定位方法，其特征在44.根据权利要求1所述的基于图优化算法的地下矿井掘进机导航定位方法，其特征在5电导航的方法则需要人工铺设基站与参考点，在矿井中加入了人工作业环境增大了风险[0006]一种基于图优化算法的地下矿井掘进机导航定位方法，本定位方法包括以下步距锚点和掘进机上的超宽带接收机进行测距获得两点6[0023]其中，D1描述了在t时刻下掘进机的位置与一组锚点中的第一个锚点之间的距离构建掘进机当前的位置与锚点之间的位置所计算出的理论距离与实际超宽带所测量的距[0027]其中，d1为掘进机上的超宽带测距传感器测量的与第一个锚点之间的距离测量锚点中的第二个锚点之间的约束残差方程，代表t时刻下掘进机当前的位置与锚点之间的7[0033]在上述所计算出的残差函数e(x)基础上，通过迭代优化使残差之和下降至极小T为上述步骤中所计算的雅可比矩阵的转置，Δx为每次优化迭代时对[0039]作为本发明的进一步改进，本方法步骤S4中每次对状态更新将会使得残差函数e了更多的信息量，由于图优化算法基于梯度下降的原理在一个线性化点进行了多次迭代，[0043]2、本发明基于因子图模型框架下的导航建模与求解能够处理观测信息的动态增8[0047]图2为本发明一种基于图优化算法的地下矿井掘进机导航定位方法的系统总体示[0048]图3为本发明一种基于图优化算法的地下矿井掘进机导航定位方法的图优化算法[0049]图4为本发明一种基于图优化算法的地下矿井掘进机导航定位方法的因子图建模[0055]掘进机尾部装有十组超宽带测距传感器(UltraWideBand)可独立工作的锚球，在行进过程中每当掘进机转弯或运行距离超过了超宽带测距传感器(UltraWideBand)有9[0064]其中，D1描述了在t时刻下掘进机的位置与一组锚点中的第一个锚点之间的距离构建掘进机当前的位置与锚点之间的位置所计算出的理论距离与实际超宽带所测量的距[0069]其中，d1为掘进机上的超宽带测距传感器测量的与第一个锚点之间的距离测量锚点中的第二个锚点之间的约束残差方程，代表t时刻下掘进机当前的位置与锚点之间的态更新量Δx：T为上述步骤中所计算的雅可比矩阵的转置，Δ