发明内容
(一)解决的技术问题
针对现有技术所存在的上述缺点,本发明提供了一种多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=[x y θ]T;
S4、设两个雷达之间固定相对位姿变换矩阵为Tlaser1(R(θ),t),t1时刻第一雷达相对墙角的位姿为Xlaser1,t2时刻第二雷达相对于墙角的位置为Xlaser2,则通过Xlaser2=Tlaser1*Tlaser2*Xlaser1求出Tlaser1,其中每个雷达t1时刻到t2时刻的位姿变换Tlaser2通过激光帧变换计算。
优选地,所述裁剪激光扇面范围时,通过旋转使得激光雷达扫描到墙角墙面。
优选地,所述设置合适的分辨率进行滤波采用体素滤波方式。
优选地,所述对雷达数据进行时间戳处理包括以下步骤:在开始时刻t1到结束时刻t2内时间间隔△t下获取几个雷达数据点总数为n,以结束时刻t2为基准,采用下式对每个雷达点进行时间戳分配:
Time Point[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度扫描的目的,从设备上有效摆脱了使用场景的限制;同时加上站点位姿优化这种算法,能够简单有效对位姿进行矫正,避免了繁琐的计算,有效消除建图定位过程中产生的累计误差,适用于不同场景下的室内定位。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述。显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
一种多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=[x y θ]T;
S4、设两个雷达之间固定相对位姿变换矩阵为Tlaser1(R(θ),t),t1时刻第一雷达相对墙角的位姿为Xlaser1,t2时刻第二雷达相对于墙角的位置为Xlaser2,则通过Xlaser2=Tlaser1*Tlaser2*Xlaser1求出Tlaser1,其中每个雷达t1时刻到t2时刻的位姿变换Tlaser2通过激光帧变换计算。
裁剪激光扇面范围时,通过旋转使得激光雷达扫描到墙角墙面。
设置合适的分辨率进行滤波采用体素滤波方式。
对雷达数据进行时间戳处理包括以下步骤:在开始时刻t1到结束时刻t2内时间间隔△t下获取几个雷达数据点总数为n,以结束时刻t2为基准,采用下式对每个雷达点进行时间戳分配:
Time Point[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=[x y θ]T;
S4、设两个雷达之间固定相对位姿变换矩阵为Tlaser1(R(θ),t),t1时刻第一雷达相对墙角的位姿为Xlaser1,t2时刻第二雷达相对于墙角的位置为Xlaser2,则通过Xlaser2=Tlaser1*Tlaser2*Xlaser1求出Tlaser1,其中每个雷达t1时刻到t2时刻的位姿变换Tlaser2通过激光帧变换计算。
裁剪激光扇面范围时,通过旋转使得激光雷达扫描到墙角墙面。如果不能保证多个雷达同时扫描到,可以旋转一周保证多个雷达可以扫描到。如果旋转一次不能使某个雷达扫描到墙角墙面,那么可以多旋转几次,求取方法和上面一样,递推即可。
设置合适的分辨率进行滤波采用体素滤波方式。使用体素滤波方式,设置合适的分辨率进行滤波,因为变换到了同一坐标系下,所以扫描到相同环境的激光点大致在一定距离阈值之内,则在一定分辨率下,可以消除微小误差,得到融合后的雷达数据帧作为匹配的数据使用。
对雷达数据进行时间戳处理包括以下步骤:在开始时刻t1到结束时刻t2内时间间隔△t下获取几个雷达数据点总数为n,以结束时刻t2为基准,采用下式对每个雷达点进行时间戳分配:
Time Point[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激光雷达融合建图与定位系统,包括前端处理单元和后端优化单元,前端处理单元包括用于对多雷达数据的相对位姿进行标定的多雷达标定模块,用于对多雷达数据进行滤波和时间戳处理多雷达数据融合模块,用于进行匹配建图的数据匹配计算模块;
后端优化单元包括用于优化位姿对应站点并进行站点数据处理的站点位姿激光帧存储模块,用于对位姿进行优化的位姿优化计算模块,用于对位姿进行融合的位姿融合模块。
以上实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不会使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。