开云优惠体育网页版入口

专利名称：一种用于井工矿环境自动驾驶参数调整的定位方法和系统

专利类型：发明专利

专利申请号：CN202411146722.8

专利申请（专利权）人：理工雷科智途(泰安)汽车开云优惠体育网页版入口有限公司,理工雷科智途(北京)开云优惠体育网页版入口有限公司
权利人地址：山东省泰安市高新区国家级高新技术创业服务中心

专利发明（设计）人：黄琰,孙静,冯冲,申永刚

专利摘要：本发明提出了一种用于井工矿环境自动驾驶参数调整的定位方法和系统，属于自动驾驶车辆定位技术领域，该方法包括：将获取的目标车辆的激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步；构建井工矿环境的点云地图；加载井工矿环境的点云地图，将同步后的数据均作为预测值，激光雷达数据作为观测值进行卡尔曼滤波融合得到初始定位数据；对井工矿环境中的走廊和岔路口分别特征提。萏崛〗峁髡ǘ瞬ǖ牟问涑鲎钪斩ㄎ皇。基于该方法，还提出了一种用于井工矿环境自动驾驶参数调整的定位系统。本发明提高了自动驾驶的安全性和可靠性，使得自动驾驶车辆能够在复杂的井下环境中安全、准确地行驶。

主权利要求：
1.一种用于井工矿环境自动驾驶参数调整的定位方法，其特征在于，包括以下步骤：获取目标车辆的激光雷达数据、IMU数据和里程计数据，将所述激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步；
采用激光雷达扫描井工矿环境，在SLAM构图环境中加入IMU数据和里程计数据，构建井工矿环境的点云地图；
加载井工矿环境的点云地图，将同步后的IMU数据、里程计数据和超宽带定位数据均作为预测值，激光雷达数据作为观测值进行卡尔曼滤波融合得到初始定位数据；
对井工矿环境中的走廊和岔路口分别特征提。萏卣魈崛〉慕峁髡ǘ瞬ǖ牟问，输出最终定位数据；
对井工矿环境中的走廊特征提取的过程包括：读入墙体点云数据，将所述墙体点云数据地面投影分块，生成点云分布矩阵；然后将点云分布矩阵转化为灰度图，并进行二值化处理；最后，基于二值图利用霍夫变换进行直线检测完成井工矿环境中的走廊特征提。
对井工矿环境中的岔路口特征提取的过程包括：通过计算获取的目标点云两个激光点的欧式距离，判断获取的目标点云两个激光点是否属于同一簇；如果位于同一簇，则提取簇的质心、簇的尺寸、簇的主方向；所述目标点云为墙体点云数据或者岔路口点云数据；通过计算不同簇之间的距离，以及与其他簇的主方向的夹角判断是否存在岔路口；
根据特征提取的结果调整卡尔曼滤波的参数，输出最终定位数据的过程包括：根据井工矿环境中的走廊特征和岔路口特征，调整卡尔曼滤波的预测值和过程噪声的权重，输出更精确的最终定位数据；
若环境特征为走廊，则降低预测噪声提高过程噪声，即更相信odom/IMU的数据；反之，则提高预测噪声，降低过程噪声，即更相信激光雷达匹配得到的位姿。
2.根据权利要求1所述的一种用于井工矿环境自动驾驶参数调整的定位方法，其特征在于，将所述激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步的过程包括：标定激光雷达坐标系与车体坐标系；以及标定IMU坐标系和车体坐标系实现激光雷达数据、IMU数据的空间同步；
获取目标车辆的超宽带定位数据，以及解析轮速计数据得到里程计数据，将所述激光雷达数据、IMU数据、超宽带定位数据和里程计数据进行时间同步。
3.根据权利要求1所述的一种用于井工矿环境自动驾驶参数调整的定位方法，其特征在于，在构建井工矿环境的点云地图的过程中：对点云数据首先进行预处理，然后进行预处理后点云数据的帧间配准和全局优化，生成PCD格式的点云地图。
4.根据权利要求3所述的一种用于井工矿环境自动驾驶参数调整的定位方法，其特征在于，对点云数据进行预处理的过程包括：对点云数据进行去噪、下采样以及滤除掉地面点云。
5.根据权利要求1所述的一种用于井工矿环境自动驾驶参数调整的定位方法，其特征在于，将IMU数据、里程计数据和超宽带定位数据作为预测值的具体过程为：根据IMU三轴加速度计算在预设时间内，IMU在三轴上的移动距离作为IMU因子；
计算出车轮里程计的速度和加速度，在预设时间内积分得到里程计数据所需要的距离和角度信息，作为里程计因子；
将宽带定位数据的定位信息作为uwb优化因子；
将IMU因子、里程计因子和uwb优化因子作为预测值。
6.根据权利要求5所述的一种用于井工矿环境自动驾驶参数调整的定位方法，其特征在于，计算出车轮里程计的速度和加速度，在预设时间内积分得到里程计数据所需要的距离和角度信息，作为里程计因子的过程包括：确定以后轮轴中点为中心的里程计模型，左侧车轮速度设为 ,右侧车轮速度设为 ，里程计模型速度为 ，角速度为 ；
；（1）
；（2）
其中，是转弯半径； 是轮子距离底盘中心的位置；
由公式（1）和公式（2）可得：
；（3）
（4）。
7.一种用于井工矿环境自动驾驶参数调整的定位系统，用于执行权利要求1至6任意一项所述的一种用于井工矿环境自动驾驶参数调整的定位方法，其特征在于，包括预处理模块、构建地图模块、第一定位模块和第二定位模块；
所述预处理模块用于获取目标车辆的激光雷达数据、IMU数据和里程计数据，将所述激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步；
所述构建地图模块用于采用激光雷达扫描井工矿环境，在SLAM构图环境中加入IMU数据和里程计数据，构建井工矿环境的点云地图；
所述第一定位模块用于加载井工矿环境的点云地图，将同步后的IMU数据、里程计数据和超宽带定位数据均作为预测值，激光雷达数据作为观测值进行卡尔曼滤波融合得到初始定位数据；
所述第二定位模块用于对井工矿环境中的走廊和岔路口分别特征提。萏卣魈崛〉慕峁髡ǘ瞬ǖ牟问，输出最终定位数据。 说明书 : 一种用于井工矿环境自动驾驶参数调整的定位方法和系统技术领域[0001] 本发明属于自动驾驶车辆定位技术领域，特别涉及一种用于井工矿环境自动驾驶参数调整的定位方法和系统。背景技术[0002] 在矿山环境中，自动驾驶技术的应用可以大大提高矿山作业的效率和安全性。然而，矿山环境复杂，包括长走廊、岔路口等各种复杂的地形，这对自动驾驶技术提出了更高的要求。同时，定位技术也是自动驾驶技术的重要组成部分，它决定了自动驾驶车辆在环境中的位置和姿态，从而影响到自动驾驶的安全性和准确性。[0003] 在现有的技术中，通常采用激光SLAM（SIMUltaneousLocalizationandMapping）进行建图和定位。激光SLAM是一种通过激光雷达获取环境信息，并同时进行定位和地图构建的技术。其次，还会使用Odom（里程计）、IMU（惯性测量单元）和Lidar（激光雷达）的三者卡尔曼融合来提高定位的精度。此外，视觉加激光的融合也是井下主流的定位技术。但是现有技术在实际应用中还存在一些问题，首先，井下环境复杂，包括长走廊、岔路口等各种复杂的地形，这需要自动驾驶车辆能够根据不同的环境对卡尔曼的参数进行适配，以保证定位的精度和稳定性。其次，现有的定位技术在面对复杂的井下环境时，可能会出现定位误差较大或者定位中断的问题，且视觉定位容易受到光照条件的限制，这会影响自动驾驶的安全性和可靠性。因此，如何在井工矿环境下实现自动驾驶的自适应参数调整，提高定位的精度和稳定性，是当前亟待解决的问题。发明内容[0004] 本发明提出了一种用于井工矿环境自动驾驶参数调整的定位方法和系统。本发明能够实现在井工矿环境下，无人驾驶能够根据不同的环境对卡尔曼的参数进行适配，提高了定位的精度和稳定性。[0005] 为实现上述目的，本发明采用以下技术方案：[0006] 一种用于井工矿环境自动驾驶参数调整的定位方法，包括以下步骤：[0007] 获取目标车辆的激光雷达数据、IMU数据和里程计数据，将所述激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步；[0008] 采用激光雷达扫描井工矿环境，在SLAM构图环境中加入IMU数据和里程计数据，构建井工矿环境的点云地图；[0009] 加载井工矿环境的点云地图，将同步后的IMU数据、里程计数据和超宽带定位数据均作为预测值，激光雷达数据作为观测值进行卡尔曼滤波融合得到初始定位数据；[0010] 对井工矿环境中的走廊和岔路口分别特征提。萏卣魈崛〉慕峁髡ǘ瞬ǖ牟问，输出最终定位数据。[0011] 进一步的，将所述激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步的过程包括：[0012] 标定激光雷达坐标系与车体坐标系；以及标定IMU坐标系和车体坐标系实现激光雷达数据、IMU数据的空间同步；[0013] 获取目标车辆的超宽带定位数据，以及解析轮速计数据得到里程计数据，将所述激光雷达数据、IMU数据、超宽带定位数据和里程计数据进行时间同步。[0014] 进一步的，在构建井工矿环境的点云地图的过程中：对点云数据首先进行预处理，然后进行预处理后点云数据的帧间配准和全局优化，生成PCD格式的点云地图。[0015] 进一步的，对点云数据进行预处理的过程包括：对点云数据进行去噪、下采样以及滤除掉地面点云。[0016] 进一步的，将IMU数据、里程计数据和超宽带定位数据作为预测值的具体过程为：[0017] 根据IMU三轴加速度计算在预设时间内，IMU在三轴上的移动距离作为IMU因子；[0018] 计算出车轮里程计的速度和加速度，在预设时间内积分得到里程计数据所需要的距离和角度信息，作为里程计因子；[0019] 将宽带定位数据的定位信息作为uwb优化因子；[0020] 将IMU因子、里程计因子和uwb优化因子作为预测值。[0021] 进一步的，计算出车轮里程计的速度和加速度，在预设时间内积分得到里程计数据所需要的距离和角度信息，作为里程计因子的过程包括：[0022] 确定以后轮轴中点为中心的里程计模型，左侧车轮速度设为 ,右侧车轮速度设为 ，里程计模型速度为 ，角速度为 ；[0023] ；（1）[0024] ；（2）[0025] 其中，是转弯半径； 是轮子距离底盘中心的位置；[0026] 由公式（1）和公式（2）可得：[0027] ；（3）[0028] （4）。[0029] 进一步的，对井工矿环境中的走廊特征提取的过程包括：读入墙体点云数据，将所述墙体点云数据地面投影分块，生成点云分布矩阵；然后将点云分布矩阵转化为灰度图，并进行二值化处理；最后，基于二值图利用霍夫变换进行直线检测完成井工矿环境中的走廊特征提取。[0030] 进一步的，对井工矿环境中的岔路口特征提取的过程包括：[0031] 通过计算获取的目标点云两个激光点的欧式距离，判断获取的目标点云两个激光点是否属于同一簇；如果位于同一簇，则提取簇的质心、簇的尺寸、簇的主方向；所述目标点云为墙体点云数据或者岔路口；[0032] 通过计算不同簇之间的距离，以及与其他簇的主方向的夹角判断是否存在岔路口。[0033] 进一步的，根据特征提取的结果调整卡尔曼滤波的参数，输出最终定位数据的过程包括：[0034] 根据井工矿环境中的走廊特征和岔路口特征，调整卡尔曼滤波的预测值和过程噪声的权重，输出更精确的最终定位数据。[0035] 本发明还提出了一种用于井工矿环境自动驾驶参数调整的定位系统，包括预处理模块、构建地图模块、第一定位模块和第二定位模块；[0036] 所述预处理模块用于获取目标车辆的激光雷达数据、IMU数据和里程计数据，将所述激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步；[0037] 所述构建地图模块用于采用激光雷达扫描井工矿环境，在SLAM构图环境中加入IMU数据和里程计数据，构建井工矿环境的点云地图；[0038] 所述第一定位模块用于加载井工矿环境的点云地图，将同步后的IMU数据、里程计数据和超宽带定位数据均作为预测值，激光雷达数据作为观测值进行卡尔曼滤波融合得到初始定位数据；[0039] 所述第二定位模块用于对井工矿环境中的走廊和岔路口分别特征提。萏卣魈崛〉慕峁髡ǘ瞬ǖ牟问，输出最终定位数据。[0040] 发明内容中提供的效果仅仅是实施例的效果，而不是发明所有的全部效果，上述技术方案中的一个技术方案具有如下优点或有益效果：[0041] 本发明提出了一种用于井工矿环境自动驾驶参数调整的定位方法和系统，属于自动驾驶车辆定位技术领域，该方法包括以下步骤：获取目标车辆的激光雷达数据、IMU数据和里程计数据，将所述激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步；采用激光雷达扫描井工矿环境，在SLAM构图环境中加入IMU数据和里程计数据，构建井工矿环境的点云地图；加载井工矿环境的点云地图，将同步后的IMU数据、里程计数据和超宽带定位数据均作为预测值，激光雷达数据作为观测值进行卡尔曼滤波融合得到初始定位数据；[0042] 对井工矿环境中的走廊和岔路口分别特征提。萏卣魈崛〉慕峁髡ǘ瞬ǖ牟问，输出最终定位数据。基于一种用于井工矿环境自动驾驶参数调整的定位方法，还提出了一种用于井工矿环境自动驾驶参数调整的定位系统。本发明能够实现在井工矿环境下，无人驾驶能够根据不同的环境对卡尔曼的参数进行适配，提高了定位的精度和稳定性，从而提高了自动驾驶的安全性和可靠性，使得自动驾驶车辆能够在复杂的井下环境中安全、准确地行驶。[0043] 本发明提出的一种用于井工矿环境自动驾驶参数调整的定位方法和系统，也可以应用于其他复杂环境，具有较强的通用性和实用性。附图说明[0044] 图1为本发明实施例1提出的一种用于井工矿环境自动驾驶参数调整的定位方法总流程图；[0045] 图2为本发明实施例1提出的数据预处理和构建井工矿环境的点云地图的示意图；[0046] 图3为本发明实施例1提出的一种用于井工矿环境自动驾驶参数调整的定位方法中数据预处理的示意图；[0047] 图4为本发明实施例1提出的走廊示意图；[0048] 图5为本发明实施例2提出的岔路口示意图；[0049] 图6为本发明实施例1提出的一种用于井工矿环境自动驾驶参数调整的定位系统示意图。具体实施方式[0050] 为能清楚说明本方案的技术特点，下面通过具体实施方式，并结合其附图，对本发明进行详细阐述。下文的公开提供了许多不同的实施例或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。此外，本发明可以在不同例子中重复参考数字和/或字母。这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施例和/或设置之间的关系。应当注意，在附图中所图示的部件不一定按比例绘制。本发明省略了对公知组件和处理技术及工艺的描述以避免不必要地限制本发明。[0051] 实施例1[0052] 本发明实施例1提出了一种用于井工矿环境自动驾驶参数调整的定位方法，用于解决现有技术中井工矿环境下自动驾驶定位存在的技术问题，图1为本发明实施例1提出的一种用于井工矿环境自动驾驶参数调整的定位方法总流程图。[0053] 在步骤S1中，获取目标车辆的激光雷达数据、IMU数据和里程计数据，将激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步。[0054] 图3为本发明实施例1提出的一种用于井工矿环境自动驾驶参数调整的定位方法中数据预处理的示意图；[0055] 具体的过程包括：标定激光雷达坐标系与车体坐标系；以及标定IMU坐标系和车体坐标系实现激光雷达数据、IMU数据的空间同步；获取目标车辆的超宽带定位数据，以及解析轮速计数据得到里程计数据，将激光雷达数据、IMU数据、超宽带定位数据和里程计数据进行时间同步。[0056] 本申请中，标定激光雷达坐标系与车体坐标系，得到二者的位姿关系，构建后轮轮速计差速模型，解析轮速计数据，得到里程计数据；[0057] 标定IMU坐标系与车体坐标系，通过里程计获取无人驾驶车辆的速度和航向角信息，通过IMU获取无人驾驶车辆的姿态角信息，将获取的速度、航向角信息和姿态角信息进行时间同步；[0058] 同时解析宽带定位数据的定位信息。[0059] 对所有传感器输出的数据进行时间同步处理，即对数据打时间戳。[0060] 在步骤S2中，采用激光雷达扫描井工矿环境，在SLAM构图环境中加入IMU数据和里程计数据，构建井工矿环境的点云地图。[0061] 构建井工矿环境的点云地图的过程中：对点云数据首先进行预处理，然后进行预处理后点云数据的帧间配准和全局优化，生成PCD格式的点云地图。[0062] 在本申请中，自动驾驶车辆搭载IMU、激光雷达对井下环境进行扫描，录制保存传感器的话题数据（bag包）根据激光雷达、IMU、odom用cartographer算法进行适配建图，对点云数据进行体素降采样、去噪等预处理,进行点云数据的帧间配准和全局优化，生成PCD格式的点云地图。[0063] 对点云数据进行去噪的过程包括：激光雷达的测距精度会随着距离的不同而不同，0.3?1m时，测距精度为±0.7cm，1?50m时，测距精度为2cm左右，超过50m后，测距精度下降到5cm左右。同时，过近的点由于收到反射影响，也会存在一定的误差，因此，在点云数据使用之前，需要剔除过近以及过远的点。[0064] 下采样的过程包括：激光雷达每秒钟都会返回上百万个点，如果对每个点都进行处理，会耗费大量的算力，因此，需要对返回的激光点进行下采样处理，即对无人车周围的三位空间进行划分，划分为边长为设定值的立方体，此时，投射在立方体内的所有点用立方体中心的坐标来代替，这样，就大大减少了数据量，提高了运算效率；[0065] 滤除掉地面点云的过程包括：地面点云滤除，无人驾驶在隧道中行驶，由于地面存在积水等因素，需要剔除地面点云，消除地面对于激光点云的影响。本地面消除算法首先将点云三维空间降到二维平面来看，计算每一个点到车辆x正方向的平面夹角，根据夹角的情况来判别地面点与非地面点，从而剔除地面点，保留非地面点。[0066] 图2为本发明实施例1提出的数据预处理和构建井工矿环境的点云地图的示意图；将激光雷达数据LIDAR、IMU数据和odom数据分别进行时间同步和空间同步，采用激光雷达扫描井工矿环境，在SLAM构图环境中加入IMU数据和里程计数据，构建井工矿环境的点云地图。[0067] 在步骤S3中，加载井工矿环境的点云地图，将同步后的IMU数据、里程计数据和超宽带定位数据均作为预测值，激光雷达数据作为观测值进行卡尔曼滤波融合得到初始定位数据；[0068] 将IMU数据、里程计数据和超宽带定位数据作为预测值的具体过程为：根据IMU三轴加速度计算在预设时间内，IMU在三轴上的移动距离作为IMU因子；[0069] 计算出车轮里程计的速度和加速度，在预设时间内积分得到里程计数据所需要的距离和角度信息，作为里程计因子；将宽带定位数据的定位信息作为uwb优化因子；将IMU因子、里程计因子和uwb优化因子作为预测值。[0070] 本申请中计算出车轮里程计的速度和加速度，在预设时间内积分得到里程计数据所需要的距离和角度信息，作为里程计因子的过程包括：确定以后轮轴中点为中心的里程计模型，左侧车轮速度设为 ,右侧车轮速度设为 ，里程计模型速度为 ，角速度为 ；[0071] ；（1）[0072] ；（2）[0073] 其中，是转弯半径； 是轮子距离底盘中心的位置；[0074] 由公式（1）和公式（2）可得：[0075] ；（3）[0076] （4）。[0077] 通过上述公式，可以推算得到车轮里程计的速度和角速度，然后再通过delta_t内时间累积积分，得到里程计数据所要的距离和角度信息，作为里程计因子。[0078] 将IMU、odom、uwb三个传感器的数据作为预测值（测量噪声），lidar作为观测值（观测噪声）进行无迹卡尔曼滤波（UKF）融合处理，计算输出位姿。[0079] 卡尔曼滤波的过程包括：[0080] 第一、定义状态向量和观测值[0081] ①、定义状态向量和观测值[0082] 状态向量 ：[0083] ；其中，和 是位置坐标；是姿态角；[0084] ②、控制输入[0085] ；其中， 是位移； 是转角；[0086] ③、观测值[0087] 激光雷达扫描匹配：可以提供相对位姿的观测。[0088] 里程计/IMU：提供运动模型的预测数据[0089] 第二、初始化[0090] 初始化状态估计 和协方差矩阵 ；[0091] 初始化过程噪声协方差Q和观测噪声协方差R。[0092] 生成sigma点，对状态向量 进行变换，生成一组sigma点。[0093] 预测步骤，对每个sigma点进行预测；使用运动模型（IMU/里程计/UWB）对每个sigma点进行状态更新，得到预测的sigma点；计算预测均值和方差。[0094] 更新步骤，使用激光雷达扫描结果作为观测值，生成观测sigma点；对预测的sigma点应用观测模型，得到观测sigma点。计算均值和协方差；计算状态和观测之间的交叉协方差：计算卡尔曼增益：更新状态估计和协方差矩阵。[0095] 在每次迭代中，更新预测步骤和更新步骤，持续融合里程计/IMU数据/uwb和激光雷达数据，不断提高位姿估计的精度。[0096] 在步骤S4中，对井工矿环境中的走廊和岔路口分别特征提。萏卣魈崛〉慕峁髡ǘ瞬ǖ牟问，输出最终定位数据。[0097] 利用点云中的垂直面特征检测左右两侧的墙面。长走廊环境中，墙面通常是平行且连续的，而在岔路口环境中，墙面可能出现分叉或中断。[0098] 图4为本发明实施例1提出的走廊示意图；对井工矿环境中的走廊特征提取的过程包括：读入墙体点云数据，将所述墙体点云数据地面投影分块，生成点云分布矩阵；然后将点云分布矩阵转化为灰度图，并进行二值化处理；最后，基于二值图利用霍夫变换进行直线检测完成井工矿环境中的走廊特征提取。[0099] 读入墙体点云数据，存储为Ｎ行两列的矩阵Ａ，一二列存储点云Ｘ，Ｙ坐标，Ｎ表示点云数量；[0100] 利用矩阵Ａ中存储的N个点的Ｘ，Ｙ坐标，计算Ｘ和Ｙ方向的最大值和最小值，并确定分块个数；[0101] 统计各分块区域的点云数量，根据各分块区域的点云数量，生成点云分布矩阵，获得点云分布图；[0102] 将点云分布矩阵转化为灰度图像，为获取更加简洁的点云分布描述，设定阈值对灰度图像进行二值化处理，对于高于阈值的区域存储其值为１，其余存储为0，生成点云分布二值图；[0103] 对二值图利用霍夫变换算法检测直线，霍夫变换峰值点阈值的选择尤为重要，只有大于该阈值的点才认为可表示直线，否则为岔路口。霍夫矩阵Ｈ是以峰值点为元素组成的矩阵，峰值点阈值默认取该矩阵内最大元素的1/2，即0.5Hmax,以上，得到走廊特征。[0104] 在岔路口环境中，通常会有多个开口，即走廊的延伸方向，检测这些开口可以帮助识别岔路口。[0105] 图5为本发明实施例2提出的岔路口示意图；对井工矿环境中的岔路口特征提取的过程包括：通过计算获取的目标点云两个激光点的欧式距离，判断获取的目标点云两个激光点是否属于同一簇；如果位于同一簇，则提取簇的质心、簇的尺寸、簇的主方向；所述目标点云为墙体点云数据或者岔路口点云数据；通过计算不同簇之间的距离，以及与其他簇的主方向的夹角判断是否存在岔路口。[0106] 使用欧式聚类算法对墙体点云数据或者岔路口进行聚类，识别多个开口区域。[0107] 确定两个激光点是否属于同一簇，点 和点 之间的欧式距离为：[0108][0109] 聚类参数：距离阈值?：设定一个最大距离阈值，两个点的距离小于?时被视为同一簇。最小簇大小 ：每个簇的最小点数。[0110] 提取簇的质心的过程为：[0111] 质心C计算公式为：[0112] ；[0113] 提取簇的宽度 、高度 和深度 分别为：[0114] ；[0115] ；[0116] ；[0117] 通过主成分分析（PCA）来计算簇的主方向，PCA计算主成分方向矩阵V和特征值λ；[0118] ;[0119][0120] 其中，Cov(X)是协方差矩阵，Λ是特征值对角矩阵；V是特征向量矩阵。[0121] 岔路口通常具有多个分支，可以通过以下标准进行检测：簇间距离：检测不同簇之间的距离，以识别是否存在交叉点。[0122] 设簇 和 之间的距离为：[0123][0124] 与其他簇的主方向的夹角，夹角θ计算公式为：[0125] ；[0126] 其中， 和 是簇 和簇 的主方向向量。[0127] 根据距离和角度变化来判断是否存在岔路口。如果有N（N取4）个簇在一个小距离内并且它们的主方向有显著差异，则可能表示岔路口。[0128] 若环境特征为走廊，则降低预测噪声提高过程噪声，即更相信odom/IMU的数据；反之，则提高预测噪声，降低过程噪声，即更相信激光雷达匹配得到的位姿。[0129] 本发明实施例1提出的一种用于井工矿环境自动驾驶参数调整的定位方法，能够实现在井工矿环境下，无人驾驶能够根据不同的环境对卡尔曼的参数进行适配，提高了定位的精度和稳定性，从而提高了自动驾驶的安全性和可靠性，使得自动驾驶车辆能够在复杂的井下环境中安全、准确地行驶。[0130] 本发明实施例1提出的一种用于井工矿环境自动驾驶参数调整的定位方法，也可以应用于其他复杂环境，具有较强的通用性和实用性。[0131] 实施例2[0132] 基于本发明实施例1提出的一种用于井工矿环境自动驾驶参数调整的定位方法，本发明实施例2还提出了一种用于井工矿环境自动驾驶参数调整的定位系统，图6为本发明实施例1提出的一种用于井工矿环境自动驾驶参数调整的定位系统示意图，包括预处理模块、构建地图模块、第一定位模块和第二定位模块；[0133] 预处理模块用于获取目标车辆的激光雷达数据、IMU数据和里程计数据，将所述激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步；[0134] 构建地图模块用于采用激光雷达扫描井工矿环境，在SLAM构图环境中加入IMU数据和里程计数据，构建井工矿环境的点云地图；[0135] 第一定位模块用于加载井工矿环境的点云地图，将同步后的IMU数据、里程计数据和超宽带定位数据均作为预测值，激光雷达数据作为观测值进行卡尔曼滤波融合得到初始定位数据；[0136] 第二定位模块用于对井工矿环境中的走廊和岔路口分别特征提。萏卣魈崛〉慕峁髡ǘ瞬ǖ牟问，输出最终定位数据。[0137] 预处理模块中，将激光雷达数据、IMU数据和里程计数据分别进行时间同步和空间同步的过程包括：标定激光雷达坐标系与车体坐标系；以及标定IMU坐标系和车体坐标系实现激光雷达数据、IMU数据的空间同步；获取目标车辆的超宽带定位数据，以及解析轮速计数据得到里程计数据，将激光雷达数据、IMU数据、超宽带定位数据和里程计数据进行时间同步。[0138] 构建地图模块中，对点云数据首先进行预处理，然后进行预处理后点云数据的帧间配准和全局优化，生成PCD格式的点云地图。对点云数据进行预处理的过程包括：对点云数据进行去噪、下采样以及滤除掉地面点云。[0139] 第一定位模块中，将IMU数据、里程计数据和超宽带定位数据作为预测值的具体过程为：根据IMU三轴加速度计算在预设时间内，IMU在三轴上的移动距离作为IMU因子；计算出车轮里程计的速度和加速度，在预设时间内积分得到里程计数据所需要的距离和角度信息，作为里程计因子；将宽带定位数据的定位信息作为uwb优化因子；将IMU因子、里程计因子和uwb优化因子作为预测值。[0140] 计算出车轮里程计的速度和加速度，在预设时间内积分得到里程计数据所需要的距离和角度信息，作为里程计因子的过程包括：[0141] 确定以后轮轴中点为中心的里程计模型，左侧车轮速度设为 ,右侧车轮速度设为 ，里程计模型速度为 ，角速度为 ；[0142] ；（1）[0143] ；（2）[0144] 其中，是转弯半径； 是轮子距离底盘中心的位置；[0145] 由公式（1）和公式（2）可得：[0146] ；（3）[0147] （4）。[0148] 第二定位模块中，对井工矿环境中的走廊特征提取的过程包括：读入墙体点云数据，将所述墙体点云数据地面投影分块，生成点云分布矩阵；然后将点云分布矩阵转化为灰度图，并进行二值化处理；最后，基于二值图利用霍夫变换进行直线检测完成井工矿环境中的走廊特征提取。[0149] 对井工矿环境中的岔路口特征提取的过程包括：通过计算获取的目标点云两个激光点的欧式距离，判断获取的目标点云两个激光点是否属于同一簇；如果位于同一簇，则提取簇的质心、簇的尺寸、簇的主方向；所述目标点云为墙体点云数据或者岔路口；通过计算不同簇之间的距离，以及与其他簇的主方向的夹角判断是否存在岔路口。[0150] 根据特征提取的结果调整卡尔曼滤波的参数，输出最终定位数据的过程包括：根据井工矿环境中的走廊特征和岔路口特征，调整卡尔曼滤波的预测值和过程噪声的权重，输出更精确的最终定位数据。[0151] 本发明实施例2提出的一种用于井工矿环境自动驾驶参数调整的定位系统，能够实现在井工矿环境下，无人驾驶能够根据不同的环境对卡尔曼的参数进行适配，提高了定位的精度和稳定性，从而提高了自动驾驶的安全性和可靠性，使得自动驾驶车辆能够在复杂的井下环境中安全、准确地行驶。[0152] 本发明实施例2提出的一种用于井工矿环境自动驾驶参数调整的定位系统，也可以应用于其他复杂环境，具有较强的通用性和实用性。[0153] 本申请实施例2提供的一种用于井工矿环境自动驾驶参数调整的定位系统中相关部分的说明可以参见本申请实施例1提供的一种用于井工矿环境自动驾驶参数调整的定位方法中对应部分的详细说明，在此不再赘述。[0154] 需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。另外，本申请实施例提供的上述技术方案中与现有技术中对应技术方案实现原理一致的部分并未详细说明，以免过多赘述。[0155] 上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制。对于所属领域的技术人员来说，在上述说明的基础上还可以做出其它不同形式的修改或变形。这里无需也无法对所有的实施方式予以穷举。在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

专利地区：山东

专利申请日期：2024-08-21

专利公开日期：2024-11-19

专利公告号：CN118670399B