本发明涉及即时定位与建图导航领域，具体涉及一种多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、将雷达数据帧变换到同一坐标系下，设置合适的分辨率进行滤波；
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激光雷达融合建图与定位系统，如图2所示，包括前端处理单元和后端优化单元，前端处理单元包括用于对多雷达数据的相对位姿进行标定的多雷达标定模块，用于对多雷达数据进行滤波和时间戳处理多雷达数据融合模块，用于进行匹配建图的数据匹配计算模块；
后端优化单元包括用于优化位姿对应站点并进行站点数据处理的站点位姿激光帧存储模块，用于对位姿进行优化的位姿优化计算模块，用于对位姿进行融合的位姿融合模块。
旋转雷达，使得雷达扫描到墙角位置，记录存储每个雷达旋转前后扫描的激光帧数据lasern(scant1，scant2)；处理扫描墙角，找出角点位姿；将雷达数据帧变换到同一坐标系下，设置合适的分辨率进行滤波；对雷达数据进行时间戳处理；进行匹配建图；每隔一段距离或一定时间记录一次雷达数据帧，存储激光点数据及当前位姿作为站点；每隔一段距离搜索一次附近站点，搜索最近站点进行一次激光数据匹配，根据匹配度p判断是否需要进行位姿修正；将位姿或修正后的位姿进行融合，并更新地图和位姿。
处理扫描墙角，找出角点位姿的方法包括以下步骤：
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数据。
hector算法是一种纯激光匹配算法，采用高斯牛顿法进行计算，受位姿初始值影响较大，在旋转过快时无法保证有效匹配，所以在此系统中融合imu数据，提供角度初始值用于hector算法的匹配，有效避免因旋转带来的位姿计算不准确。
匹配度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激光雷达融合建图与定位系统，包括前端处理单元和后端优化单元，前端处理单元包括用于对多雷达数据的相对位姿进行标定的多雷达标定模块，用于对多雷达数据进行滤波和时间戳处理多雷达数据融合模块，用于进行匹配建图的数据匹配计算模块；
后端优化单元包括用于优化位姿对应站点并进行站点数据处理的站点位姿激光帧存储模块，用于对位姿进行优化的位姿优化计算模块，用于对位姿进行融合的位姿融合模块。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不会使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
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为基准，采用下式对每个雷达点进行时间戳分配：
timepoint[i]＝(point[t2]-[△t]/n*i)。
6.根据权利要求1所述的多2d激光雷达融合建图与定位方法，其特征在于：所述匹配建图的匹配方式及栅格概率地图的构建方式基于hector算法，并在hector算法中融合用于匹配角度初值的imu数据。
7.根据权利要求1所述的多2d激光雷达融合建图与定位方法，其特征在于：所述匹配度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存储到位姿融合器中。
8.根据权利要求1所述的多2d激光雷达融合建图与定位方法，其特征在于：所述将位姿或修正后的位姿进行融合包括以下步骤：
s1、采用时间序列方法在位姿融合器中对位姿或修正后的位姿进行融合；
s2、在位姿融合器中取时间点最靠近的位姿进行平移和旋转，获取位姿；
s3、实时从位姿融合器中获取到一定输出频率的位姿数据，用于移动机器人的控制输入。
9.根据权利要求8所述的多2d激光雷达融合建图与定位方法，其特征在于：所述位姿融合器的位姿作为匹配建图的初始值输入。
10.一种多2d激光雷达融合建图与定位系统，其特征在于：包括前端处理单元和后端优化单元，所述前端处理单元包括用于对多雷达数据的相对位姿进行标定的多雷达标定模块，用于对多雷达数据进行滤波和时间戳处理多雷达数据融合模块，用于进行匹配建图的数据匹配计算模块；
所述后端优化单元包括用于优化位姿对应站点并进行站点数据处理的站点位姿激光帧存储模块，用于对位姿进行优化的位姿优化计算模块，用于对位姿进行融合的位姿融合模块。
技术总结