买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

龙图腾网恭喜江苏崇创嘉航智能科技有限公司申请的专利一种基于图优化算法的地下矿井掘进机导航定位方法获国家发明授权专利权，本发明授权专利权由国家知识产权局授予，授权公告号为：CN115655268B 。

龙图腾网通过国家知识产权局官网在2025-05-16发布的发明授权授权公告中获悉：该发明授权的专利申请号/专利号为：202211354439.5，技术领域涉及：G01C21/16；该发明授权一种基于图优化算法的地下矿井掘进机导航定位方法是由杨功流;蔡庆中;牛浩飞;彭侠夫;张江源设计研发完成，并于2022-11-01向国家知识产权局提交的专利申请。

本一种基于图优化算法的地下矿井掘进机导航定位方法在说明书摘要公布了：本发明公开了一种基于图优化算法的地下矿井掘进机导航定位方法，本定位方法包括以下步骤：S1、对惯性测量单元零偏进行标定，计算惯性测量单元与超宽带测距传感器之间的相对位置，记录标定参数并补偿。本发明基于因子图模型框架下的导航建模与求解能够处理观测信息的动态增删，不同的观测约束被编码为因子的形式添加在因子图当中与状态变量相连，系统更加灵活可靠，行进中自动抛下UWB锚点形成自动组网，降低了矿井作业的危险性，采用双UWB测距与航位推算的紧耦合，能够约束并校准航向角，在矿井下煤尘浓度极高、特征纹理不明显，超宽带具有极强穿透能力，不受该环境的影响且误差不随时间发散，解决了地下环境下传统方法精度低的问题。

本发明授权一种基于图优化算法的地下矿井掘进机导航定位方法在权利要求书中公布了：1.一种基于图优化算法的地下矿井掘进机导航定位方法，其特征在于：本定位方法包括以下步骤：S1、对惯性测量单元零偏进行标定，计算惯性测量单元与超宽带测距传感器之间的相对位置，记录标定参数并补偿；S2、在掘进机行进时，利用车体携带的惯性测量单元与轮速传感器以航位推算算法进行掘进机初步导航定位，通过轮速传感器获得车辆的速度观测，通过惯性测量单元中的陀螺仪获得角速率信息，根据速度与角速率信息可实时推算自身的位置信息；S3、掘进机在行进中每相隔一定距离安放一对超宽带测距锚点在道路两侧，作为锚点的位置坐标，分别记为X1t，Y1t、X2t，Y2t，每个超宽带测距锚点可与其他超宽带测距锚点和掘进机上的超宽带接收机进行测距获得两点之间的相对距离信息；S4、构建因子图模型，因子图模型包括有状态节点和因子，对所建立的因子图模型进行优化求解，以航位推算结果作为优化初值，采用LM算法进行非线性迭代优化，直至全局误差函数不再下降，得到优化后的导航参数X,Y,A；S5、实时解算，重复所述步骤S4，并不断更新当前时刻的导航参数，进而实现矿井下的掘进机导航定位的功能；本方法步骤S1中在起点处利用所述惯性测量单元获取初始航向角A0，并记录在二维坐标系下的起始坐标，将起点作为原点，以X轴作为初始的行进方向，记录起点坐标为X0，Y0，A0；其中X0，Y0为横、纵坐标值，单位米，A0为航向角，单位度；航位推算模型状态量为X,Y,A，模型为：Xt＝Xt-1+VtTcosAYt＝Yt-1+VtTsinA其中，Xt为t时刻横坐标值，Yt为t时刻纵坐标值，A为t时刻的航向角，T为轮速计采样间隔，Vt为t时刻的速度观测值；所述超宽带测距传感器测距信息，观测方程为： 其中，D1描述了在t时刻下掘进机的位置与一组锚点中的第一个锚点之间的距离方程，D2描述了在t时刻下掘进机的位置与一组锚点中的第二个锚点之间的距离方程，用于构建掘进机当前的位置与锚点之间的位置所计算出的理论距离与实际超宽带所测量的距离之间的误差；所述因子图模型对应的数学形式为多个残差方程，采用非线性优化算法对导航参数进行全局优化，具体模型如下： 其中，d1为掘进机上的超宽带测距传感器测量的与第一个锚点之间的距离测量值，d2为掘进机上的超宽带测距传感器测量的与第二个锚点之间的距离测量值，为t时刻掘进机与所放置的一对锚点中的第一个锚点之间的约束残差方程，为t时刻掘进机与所放置的一对锚点中的第二个锚点之间的约束残差方程，代表t时刻下掘进机当前的位置与锚点之间的位置所计算出的理论距离与实际超宽带所测量的距离之间的误差，该误差用于修正掘进机当前时刻的位置；所述残差方程进行求导，用于指导图优化的方向，求导获得的雅可比矩阵为： 基于上述公式，进行非线性优化，以航位推算坐标X,Y,A作为优化初值，以雅可比矩阵作为梯度，将所有时刻产生的残差项求和得到整个因子图模型的系统残差方程，具体模型如下： 其中，Σi为超宽带基站测量的相对距离的协方差；为i时刻掘进机与所放置的一对锚点中的第一个锚点之间的约束残差方程，为i时刻掘进机与所放置的一对锚点中的第二个锚点之间的约束残差方程；在上述所计算出的残差函数ex基础上，通过迭代优化使残差之和下降至极小值，完成非线性优化过程，根据公式计算状态更新量Δx：ex+Δx≈ex+JxTΔx 其中，JxT为上述步骤中所计算的雅可比矩阵的转置，Δx为每次优化迭代时对状态变量的更新量，用于校准带有误差的掘进机状态；对状态量X,Y,A不断更新：X＝X+Δx，基于优化结果中所获得的当前时刻的位置变量Xt，Yt与上一时刻的位置变量Xt-1，Yt-1更新航向角：

如需购买、转让、实施、许可或投资类似专利技术，可联系本专利的申请人或专利权人江苏崇创嘉航智能科技有限公司，其通讯地址为：226000 江苏省南通市崇川区永兴街道深南路199号天安数码城8幢1603室；或者联系龙图腾网官方客服，联系龙图腾网可拨打电话0551-65771310或微信搜索“龙图腾网”。