一种基于因子图的异构数据钻机引孔定位方法和系统与流程
本发明设计钻机,特别涉及一种基于因子图的异构数据钻机引孔定位方法和系统。
背景技术:
1、目前传统的钻机引孔定位方法主要采用扩展卡尔曼滤波(ekf)的方式实现定位,然而传统的扩展卡尔曼滤波(ekf)引孔定位方案存在如下技术问题:1、扩展卡尔曼滤波(ekf)器是个前向的递推估计器,因此当某个时刻状态被更新时,后续的递推器无法对早期误差进行修正,因而容易造成误差累积;2、传统的扩展卡尔曼滤波(ekf)器依赖一阶泰勒展开进行线性化,线性化误差在大角度旋转或远距离观测时会显著累积;3、传统扩展卡尔曼滤波(ekf)器要求所有观测值必须通过单一的观测方程纳入滤波,因此在处理异构数据时需要反复更改观测方程的维度和反复重构雅可比矩阵,从而使得观测方程的稳定性较差。
技术实现思路
1、本发明其中一个发明目的在于提供一种基于因子图的异构数据钻机引孔定位方法和系统,所述方法和系统利用因子图对不同类型传感器的异构数据的每次观测数据都建模为独立的因子,不同异构数据的因子之间通过状态节点进行联结构建因子图,因此当需要新的异构数据观测时,本发明仅需要在所述状态节点的因子图中增加新的误差项,无需全局调整算法结构,可以实现多源异构数据的任意组合进行精准的定位,同时大幅降低引孔定位过程中数据方程计算量,从而提高引孔定位控制的稳定性。
2、本发明其中一个发明目的在于提供一种基于因子图的异构数据钻机引孔定位方法和系统,所述方法和系统构建了以钻机变幅运动编码因子作为本发明因子图中的必须配置的强约束因子节点,其中所述钻机变幅运动编码因子将根据钻机关节中对应液压缸驱动并配有高精度角度编码器的编码数据构建,而所述钻机变幅运动编码因子不受光照、天气、电磁干扰、遮挡等外部因素影响,因此本发明的钻机引孔过程中即使其他因子节点全部失效时,所述钻机引孔仍然可以强约束地执行对应引孔定位动作,从而提高了钻机引孔定位的鲁棒性和安全性。
3、本发明其中一个发明目的在于提供一种基于因子图的异构数据钻机引孔定位方法和系统,所述方法和系统利用所述因子图在新的异构观测数据加入时对整个滑动窗口内的所有状态进行重新批量优化,因此本发明可以实现早期的一个粗差观测可以被后期的一组精确观测联合纠正,并且本发明针对所述因子图采用迭代非线性最小二乘求解器,在每次迭代中重新线性化,精度远高于ekf的单次线性化,可以大幅降低后续的累积误差。
4、为了实现至少一个上述发明目的,本发明进一步提供一种基于因子图的异构数据钻机引孔定位方法,所述方法包括:
5、获取不同传感器针对钻机检测的空间位置数据、姿态数据、速度数据和钻机传感器的零偏数据的异构数据,用于构建所述钻机状态变量的关键帧,根据所述状态变量的关键帧构建因子图节点;
6、对引孔动作进行增量因子图节点构建,并针对钻机引孔动作结果分别进行所述异构数据的有效性验证,根据所述有效性验证结果构建对应异构数据的因子图边;
7、以钻机变幅运动编码数据作为核心状态数据,构建所述钻机变幅运动编码数据强制性约束的因子图边,计算因子图节点每个类型异构数据的残差和协方差;
8、根据所述每个类型异构数据残差构建全局残差向量,以最小化全局残差向量的马氏距离平方和作为目标函数,并配置钻机动作后的对应坐标方向置信区间半径,根据所述对应坐标方向置信区间半径判定当前引孔定位是否成功。
9、根据本发明其中一个较佳实施例,所述关键帧构建包括:构建三维空间坐标系,并获取所述钻机的gnss绝对位置观测数据、imu惯性观测数据、视觉重投影观测数据、lidar点云观测数据和钻机变幅机构运动观测数据,根据上述对应观测数据得到对应的空间位置数据、姿态数据、速度数据和钻机传感器的零偏数据作为对应的状态变量;以同一时间序列下所有传感器类型状态变量集合作为所述钻机关键帧。
10、根据本发明另一个较佳实施例,所述增量因子图节点构建方法包括:当操作人员以一定速度操控变幅机构向引孔位置移动时,执行增量因子图节点构建:对imu惯性单元以特定频率进行预积分计算,当检测到imu惯性单元存在如下条件中:钻头位置移动量大于设定阈值、姿态变化量大于设定阈值或距离上一关键帧的时间间隔大于设定阈值中的至少满足一种条件时,则在当前时间序列创建新的状态变量的因子节点,并将因子节点加入到因子图窗口中。
11、根据本发明另一个较佳实施例,所述异构数据的有效性验证方法包括:对于每个新增的因子节点,判断对应的gnss绝对位置观测数据是否处于固定解状态,若是,则添加gnss因子图边;根据imu惯性单元的预积分添加imu预积分因子图边;判断视觉跟踪是否有效,计算视觉重投影观测数据的特征点数量是否大于设定阈值,若是,则添加视觉重投影因子图边;判断lidar帧间配准有效,计算lidar点云匹配点数量是否大于预设阈值,若是,则添加点云匹配因子图边;其中根据钻机变幅运动编码数据强制添加钻机变幅运动因子图边。
12、根据本发明另一个较佳实施例,不同异构数据的因子图边构建方法包括:将所述imu预积分因子图边构建为二元边,所述二元边连接新节点和上一节点,用于传递运动约束;将所述gnss因子图边构建为一元边,所述一元边仅连接新节点自身,用于更新当前位置数据;将所述视觉重投影因子图边构建为三元边,所述三元边连接当前时序下关键帧和地图中三维坐标点,用于构建三维空间关系;将所述点云匹配因子图边构建为二元边,用于连接新节点和上一节点;钻机变幅运动因子图边构建为一元边,用于更新钻机自身运动状态。
13、根据本发明另一个较佳实施例,所述钻机变幅运动因子节点计算方法包括:获取钻机所有旋转关节的编码器实测值,根据d-h参数法建立所述编码器实测值正向运动学模型,利用所述正向运动学模型输出钻头安装点相对于钻机底座的平移向量和旋转矩阵,利用包括参考传感器进行机械推算得到所述钻机在三维坐标系下推算绝对位置,根据所述推算绝对位置和实测绝对位置进行残差计算。
14、根据本发明另一个较佳实施例,所述钻机变幅运动因子节点计算方法包括:计算得到所述推算绝对位置和实测绝对位置的残差后,计算如下误差源的协方差:计算底座位姿的协方差、每个关节角度编码器的噪声协方差、d-h运动学模型中包括连杆长度和扭转角几何参数的标定不确定性协方差,计算每个误差源到终端误差的雅可比矩阵,对多个误差源的协方差进行一阶线性传递合成后得到所述残差的协方差。
15、根据本发明另一个较佳实施例,所述置信区间半径计算方法包括:对钻机位置进行lm迭代优化,并从最后一次迭代优化的结果中提取钻机位置的协方差矩阵,根据所述协方差矩阵获取对应方向上位置方差值,并计算每个方向上95%置信区间的每个钻机位置,根据设置对应方向上的所述置信区间半径作为阈值;并选择水平的两个方向中最大置信区间值和水平置信区间半径对比,若小于等于所述水平置信区间半径,且高程方向上置信区间小于等于对应高程置信区间半径,则当前引孔定位动作判定通过。
16、为了实现至少一个上述发明目的,本发明进一步提供一种基于因子图的异构数据钻机引孔定位系统,所述系统执行上述一种基于因子图的异构数据钻机引孔定位方法。
17、本发明进一步提供一种计算机可读存储介质,所述计算机可读存储介质存储有计算机程序,所述计算机程序被处理器执行以实现上述一种基于因子图的异构数据钻机引孔定位方法。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com