一种多2D激光雷达融合建图与定位方法及系统技术方案

本发明专利技术涉及即时定位与建图导航领域，具体涉及一种多2D激光雷达融合建图与定位方法及系统，旋转雷达，使得雷达扫描到墙角位置，记录存储每个雷达旋转前后扫描的激光帧数据，处理扫描墙角，找出角点位姿，将雷达数据帧变换到同一坐标系下，设置合适的分辨率进行滤波，对雷达数据进行时间戳处理，进行匹配建图，每隔一段距离或一定时间记录一次雷达数据帧，存储激光点数据及当前位姿作为站点，每隔一段距离搜索一次附近站点，搜索最近站点进行一次激光数据匹配，根据匹配度p判断是否需要进行位姿修正；本发明专利技术提供的技术方案能够有效克服现有技术所存在的无法对多2D激光雷达进行较好融合，并精简进行融合定位建图的缺陷。

A multi-2d lidar fusion mapping and positioning method and system

【技术实现步骤摘要】
一种多2D激光雷达融合建图与定位方法及系统
本专利技术涉及即时定位与建图导航领域，具体涉及一种多2D激光雷达融合建图与定位方法及系统。
技术介绍
目前SLAM(即时定位与建图技术)在移动机器人行业的运用越来越广泛，运用的传感器主要有惯导、激光、相机等。激光具有稳定性高、精度高等优点，但其成本相对较高；相机的成本低，但是使用稳定性无法保证。运用的定位建图方式主要有粒子滤波和激光匹配，随着场景的增大，定位累计误差会增大。目前，针对累计误差消除的后端优化方式有很多，通常都是一些集成库，如g2o(图优化库)、ceres-solver(非线性优化库)。但是，这种优化方式很繁琐并耗时，对处理器也有一定要求。2D激光雷达的选型根据成本主要有360度扫描激光、270度扫描激光以及180度扫描激光。但是，由于使用场景的因素，往往安装在设备上不能达到全角度扫描，导致定位精度有所下降。目前，没有一种很好的多雷达融合方案。虽然目前算法中后端优化方法很好，但是涉及的配置参数过多，计算相对复杂，不够精简，往往参数没有调整好会导致定位精度不够。所以，目前需要一种针对多2D激光雷达的精简的融合定位建图算法。
技术实现思路
(一)解决的技术问题针对现有技术所存在的上述缺点，本专利技术提供了一种多2D激光雷达融合建图与定位方法及系统，能够有效克服现有技术所存在的无法对多2D激光雷达进行较好融合，并精简进行融合定位建图的缺陷。(二)技术方案为实现以上目的，本专利技术通过以下技术方案予以实现：一种多2D激光雷达融合建图与定位方法，包括以下步骤：S1、旋转雷达，使得雷达扫描到墙角位置，记录存储每个雷达旋转前后扫描的激光帧数据lasern(Scant1，Scant2)；S2、处理扫描墙角，找出角点位姿；S3、将雷达数据帧变换到同一坐标系下，设置合适的分辨率进行滤波；S4、对雷达数据进行时间戳处理；S5、进行匹配建图；S6、每隔一段距离或一定时间记录一次雷达数据帧，存储激光点数据及当前位姿作为站点；S7、每隔一段距离搜索一次附近站点，搜索最近站点进行一次激光数据匹配，根据匹配度p判断是否需要进行位姿修正；S8、将位姿或修正后的位姿进行融合，并更新地图和位姿。优选地，所述处理扫描墙角，找出角点位姿的方法包括以下步骤：S1、选取两个墙面的直线激光点，裁剪激光扇面范围；S2、运用最小二乘算法拟合直线，得到两条直线方程为求出交点坐标(x，y)；S3、设激光位置为(0，0)，则角度为θ＝arctan(y，x)，每个激光雷达在t时刻求出一个角点位置Xn＝[xyθ]T；S4、设两个雷达之间固定相对位姿变换矩阵为T1(R(θ)，t)，t1时刻第一雷达相对墙角的位姿为X1，t2时刻第二雷达相对于墙角的位置为X2，则通过X2＝T1*T2*X1求出T1，其中每个雷达t1时刻到t2时刻的位姿变换T2通过激光帧变换计算。优选地，所述裁剪激光扇面范围时，通过旋转使得激光雷达扫描到墙角墙面。优选地，所述设置合适的分辨率进行滤波采用体素滤波方式。优选地，所述对雷达数据进行时间戳处理包括以下步骤：在开始时刻t1到结束时刻t2内时间间隔△t下获取几个雷达数据点总数为n，以结束时刻t2为基准，采用下式对每个雷达点进行时间戳分配：TimePoint[i]＝(Point[t2]-[△t]/n*i)。优选地，所述匹配建图的匹配方式及栅格概率地图的构建方式基于hector算法，并在hector算法中融合用于匹配角度初值的Imu数据。优选地，所述匹配度p大于0.5时，则进行一次位姿修正，所述位姿修正包括以下步骤：S1、设当前位姿为X1，站点位姿为X2，通过当前位姿X1求出位姿变换T1，通过两个位姿点的激光帧匹配得到位姿变换T2，则产生的误差方程为e(x，y，θ)＝T1-Ω*T2，其中Ω＝p*k；S2、使用非线性最小二乘法进行优化，根据匹配度p分配信任度，得到优化后的位姿X3，其中理想情况误差e＝0，参数k可调；S3、将当前位姿X1的站点从内存中删除，将位姿X3的站点存储到站点位姿管理器中，并将位姿X3存储到位姿融合器中。优选地，所述将位姿或修正后的位姿进行融合包括以下步骤：S1、采用时间序列方法在位姿融合器中对位姿或修正后的位姿进行融合；S2、在位姿融合器中取时间点最靠近的位姿进行平移和旋转，获取位姿；S3、实时从位姿融合器中获取到一定输出频率的位姿数据，用于移动机器人的控制输入。优选地，所述位姿融合器的位姿作为匹配建图的初始值输入。一种多2D激光雷达融合建图与定位系统，包括前端处理单元和后端优化单元，所述前端处理单元包括用于对多雷达数据的相对位姿进行标定的多雷达标定模块，用于对多雷达数据进行滤波和时间戳处理多雷达数据融合模块，用于进行匹配建图的数据匹配计算模块；所述后端优化单元包括用于优化位姿对应站点并进行站点数据处理的站点位姿激光帧存储模块，用于对位姿进行优化的位姿优化计算模块，用于对位姿进行融合的位姿融合模块。(三)有益效果与现有技术相比，本专利技术所提供的一种多2D激光雷达融合建图与定位方法及系统，可以构建出良好的2D概率栅格地图，输出相对精度较高的位姿；可以使用单一360度雷达，也可选用多个小角度雷达安装在不同方向，通过融合算法实现360度扫描的目的，从设备上有效摆脱了使用场景的限制；同时加上站点位姿优化这种算法，能够简单有效对位姿进行矫正，避免了繁琐的计算，有效消除建图定位过程中产生的累计误差，适用于不同场景下的室内定位。附图说明为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1为本专利技术流程示意图；图2为本专利技术系统示意图；图3为本专利技术找出角点位姿的示意图；图4为本专利技术基于hector算法匹配建图位姿优化前后的效果图。具体实施方式为使本专利技术实施例的目的、技术方案和优点更加清楚，下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述。显然，所描述的实施例是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本专利技术保护的范围。一种多2D激光雷达融合建图与定位方法，如图1、图3所示，包括以下步骤：S1、旋转雷达，使得雷达扫描到墙角位置，记录存储每个雷达旋转前后扫描的激光帧数据lasern(Scant1，Scant2)；S2、处理扫描墙角，找出角点位姿；S3、将雷达数据帧变换到同一坐标系下，设置合适本文档来自技高网...

【技术保护点】
1.一种多2D激光雷达融合建图与定位方法，其特征在于：包括以下步骤：/nS1、旋转雷达，使得雷达扫描到墙角位置，记录存储每个雷达旋转前后扫描的激光帧数据laser

【技术特征摘要】
1.一种多2D激光雷达融合建图与定位方法，其特征在于：包括以下步骤：
S1、旋转雷达，使得雷达扫描到墙角位置，记录存储每个雷达旋转前后扫描的激光帧数据lasern(Scant1，Scant2)；
S2、处理扫描墙角，找出角点位姿；
S3、将雷达数据帧变换到同一坐标系下，设置合适的分辨率进行滤波；
S4、对雷达数据进行时间戳处理；
S5、进行匹配建图；
S6、每隔一段距离或一定时间记录一次雷达数据帧，存储激光点数据及当前位姿作为站点；
S7、每隔一段距离搜索一次附近站点，搜索最近站点进行一次激光数据匹配，根据匹配度p判断是否需要进行位姿修正；
S8、将位姿或修正后的位姿进行融合，并更新地图和位姿。


2.根据权利要求1所述的多2D激光雷达融合建图与定位方法，其特征在于：所述处理扫描墙角，找出角点位姿的方法包括以下步骤：
S1、选取两个墙面的直线激光点，裁剪激光扇面范围；
S2、运用最小二乘算法拟合直线，得到两条直线方程为



求出交点坐标(x，y)；
S3、设激光位置为(0，0)，则角度为θ＝arctan(y，x)，每个激光雷达在t时刻求出一个角点位置Xn＝[xyθ]T；
S4、设两个雷达之间固定相对位姿变换矩阵为T1(R(θ)，t)，t1时刻第一雷达相对墙角的位姿为X1，t2时刻第二雷达相对于墙角的位置为X2，则通过X2＝T1*T2*X1求出T1，其中每个雷达t1时刻到t2时刻的位姿变换T2通过激光帧变换计算。


3.根据权利要求2所述的多2D激光雷达融合建图与定位方法，其特征在于：所述裁剪激光扇面范围时，通过旋转使得激光雷达扫描到墙角墙面。


4.根据权利要求1所述的多2D激光雷达融合建图与定位方法，其特征在于：所述设置合适的分辨率进行滤波采用体素滤波方式。


5.根据权利要求1所述的多2D激光雷达融合建图与定位方法，其特征在于：所述对雷达数据进行时间戳处理包括以下步骤：在开始时刻t1到结束时刻t2内时间间隔△t下获取几个雷达数据点总数为n，以结束时刻t2为基准，采用下式对...

【专利技术属性】
技术研发人员：姜跃君，张启富，
申请(专利权)人：安徽意欧斯物流机器人有限公司，
类型：发明
国别省市：安徽;34