基于预设线特征提取及匹配的激光雷达定位算法及存储介质制造技术

本发明专利技术涉及无人车、轮式机器人系统领域，公开了一种基于预设线特征提取及匹配的激光雷达定位算法及存储介质，基于激光点的蒙特卡洛定位算法上进行改进的算法，引入比激光点更高维的线特征，并通过特定的线特征匹配度计算方法实现预设地图特征与激光线特征进行匹配，并通过匹配结果进行车辆的定位与修正。比原自适应蒙特卡洛定位算法在定位精度、定位鲁棒性、定位收敛速度上均有较大提升，对室内环境下进行高精度定位具有重要的参考价值。下进行高精度定位具有重要的参考价值。下进行高精度定位具有重要的参考价值。

【技术实现步骤摘要】
基于预设线特征提取及匹配的激光雷达定位算法及存储介质


[0001]本专利技术涉及无人车、轮式机器人系统领域，特别是涉及基于预设线特征提取及匹配的激光雷达定位算法及存储介质。

技术介绍

[0002]随着无人驾驶技术以及轮式机器人运动导航技术的发展，我们的生活更加便利，定位技术作为整个过程的第一步，它的准确性是保证后续工作的基础。因此需要有一种高精度高鲁棒性的定位算法。对于室内定位问题，最为常用的算法是自适应蒙特卡洛定位(amcl)，amcl是基于粒子滤波器的定位算法。amcl使室内已知环境下的定位成为可能，但是依然存在以下问题：
[0003](1)定位初期严重依赖轮速里程计。因为amcl是基于粒子滤波的定位方法，在定位初期粒子还未收敛，所以初期的定位是不准确的，因此定位初期需要严重依赖轮速里程计，为使得粒子收敛，无人车/轮式机器人需要移动较长距离，而且如果初始位置不准确，那么整个定位效果是错误的；
[0004](2)定位鲁棒性差。使用amcl进行定位时，如果某时刻定位丢失，粒子会快速分散，粒子再次收敛困难；r/>[0005](本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于预设线特征提取及匹配的激光雷达定位算法，所述算法适用于搭载激光雷达的无人车或轮式机器人在室内已知环境下进行实时定位，其特征在于，包括：S1、进行全局环境地图的绘制；S2、将车辆所发出激光线与全局地图线进行匹配计算；S3、将匹配计算结果带入粒子滤波中，计算粒子置信度；S4、基于粒子滤波后得出的车辆位姿，进行车辆位姿估计值再修正。2.如权利要求1所述的基于预设线特征提取及匹配的激光雷达定位算法，其特征在于，所述S1步骤中“进行全局环境地图的绘制”至少包括以下步骤：S11、将搭载激光雷达的无人车或轮式机器人移动的环境进行详细测量，包括整体环境边界的尺寸、标志物与边界的相对位置关系，并将测量结果绘制到草图上；S12、将草图内容绘制并导出图片作为全局环境地图。3.如权利要求1所述的基于预设线特征提取及匹配的激光雷达定位算法，其特征在于，所述S2步骤中“将车辆所发出激光线与全局地图线进行匹配计算”至少包括以下步骤：S21、选定全局环境地图的原点，计算标志物边缘线的特征；S22、根据标志物边缘线的斜率，将待匹配边缘线分成斜率有穷及无穷两类分别存放到有穷集合vec_limit与无穷集合vec_limitless中；S23、将激光雷达产生的激光点云根据位置关系拟合成激光线；S24、获取激光线的起点与终点的极坐标，通过坐标变换获得全局环境地图下激光线的起点坐标(map_st_x、map_st_y)与终点坐标(map_ed_x、map_ed_y)；S25、遍历激光线，获取到激光线与所有标志物边缘线进行偏差计算，获取激光线与标志物边缘线的最小偏差。4.如权利要求3所述的基于预设线特征提取及匹配的激光雷达定位算法，其特征在于，所述S25步骤中“激光线与所有标志物边缘线进行偏差计算”，具体分为：S251、有穷集合vec_limit中的边缘线计算偏差：所述偏差包括角度偏差diff_angle与距离偏差diff_dist，根据公式：计算角度偏差diff_angle，其中，laser_k代表激光线的斜率，map_k代表边缘线的斜率；根据公式：计算激光线起点到边缘线的垂直距离dist_st与终点到边缘线的垂直距离dist_ed，其中，map_b代表边缘线的偏置；距离偏差diff_dist的计算方式根据激光线与边缘线是否交叉分为两种，当激光线与
边缘线发生交叉时，根据公式：计算激光线与边缘线之间的距离偏差diff_dist；判断距离偏差的均值是否小于0，如果小于0，则置为负；当激光线与边缘线不发生交叉时，根据公式：diff_dist＝0.5(dist_st|+|dist_ed) (4)；计算激光线与边缘线之间的距离偏差diff_dist；S252、无穷集合vec_limitless中的边缘线计算偏差：根据公式：计算角度偏差diff_angle，其中，laser_k代表激光线的斜率；距离偏差diff_dist的计算方式根据激光线与边缘线是否交叉分为两种，与S251步骤中距离偏差diff_dist的计算方式相同，根据公式：计算激光线起点到边缘线的垂直距离dist_st与终点到边缘线的垂直距离dist_ed，其中，m代表边缘线到y轴的距离；S253、经过S251步骤与S252步骤得到角度偏差diff_angle与距离偏差diff_dist，根据公式：计算得到综合偏差diff_add，其中，angle_weight代表角度权重，dist_weight代表距离权重，权重的赋值由实验获得且两个权重和为1，max_angle_diff与max_dist_diff代表最大角度偏差与最大距离偏差，由实验统计计算获得；S254、将本...

【专利技术属性】
技术研发人员：黄荣军，王泽兴，黄思德，邹广才，原诚寅，
申请(专利权)人：北京国家新能源汽车技术创新中心有限公司，
类型：发明
国别省市：