发明内容
为此,本发明所要解决的技术问题在于克服现有技术中定位受GPS信号强弱和精度影响较大,以及定位受光照及环境纹理特征缺失所导致重定位失败的技术缺陷。
为解决上述技术问题,本发明提供了一种基于激光雷达的重定位方法,包括以下步骤:
S1、机器人遍历整个环境空间,通过机器人上的激光雷达传感器采集环境信息,构建完整的点云地图;
S2、机器人继续工作并记录当前时刻的点云数据和位姿信息,将当前时刻的激光点云数据和其位姿信息作为关键帧保存,直至机器人跑完S1中的点云地图,获得包含关键帧信息的点云数据库;
S3、当机器人定位丢失时,获取机器人当前时刻的点云数据,将机器人当前时刻的点云数据与点云数据库进行匹配搜索,获得搜索匹配对应的关键帧;
S4、根据搜索匹配的关键帧,计算机器人全局位姿信息以实现重定位。
作为优选的,所述S2中,所述关键帧的结构体还包括激光点云数据的scancontext描述。
作为优选的,所述S2中,机器人继续工作并记录当前时刻的点云数据和位姿信息,将当前时刻的激光点云数据和其位姿信息作为关键帧保存,包括:
获取当前时刻下机器人在地图坐标系下的位姿话题,提取当前关键帧;
计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换,所述相对变换包括机器人的空间位置变换和机器人的空间旋转角度变换;
当机器人的空间位置变换大于预设距离阈值或者当机器人的空间旋转角度变换大于预设角度阈值时,创建新的关键帧;
将创建的新的关键帧赋值给当前关键帧以更新当前关键帧;
保存更新后的当前的关键帧的位姿、当前时刻的点云数据并计算其scan context描述。
作为优选的,获取当前时刻下机器人在地图坐标系下的位姿话题,提取当前关键帧;计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换,所述相对变换包括机器人的空间位置变换和机器人的空间旋转角度变换,具体包括:
设机器人发布的里程计位姿为Ti,当前关键帧的位姿TKcurrent,设机器人初始里程计位姿为T0,令机器人初始关键帧位姿TK0=T0;
当机器人在运行定位的时候,计算其中,ΔT为i时刻的机器人里程计位姿态与当前最近的关键帧位姿的相对变换,ΔT包括空间位置变换ΔP和空间旋转角度变换ΔQ。
作为优选的,所述S3包括:
S31、将当前位姿的一帧点云数据用scan context表示;
S32、在保存好的地图关键帧中scan context中搜索出相近的候选关键帧,获得最近邻的多个候选关键帧;
S33、遍历候选关键帧,选择距离最近的帧作为匹配帧并返回此关键帧。
作为优选的,所述S31和S32之间还包括:
计算scan context的每一行的均值作为RingKey;
所述S32具体包括:用保存的第一关键帧的RingKey构建Kd-tree以搜索候选关键帧。
作为优选的,所述S33具体包括:
不断对任一候选关键帧的scan context进行循环右移操作,依次计算出候选关键帧与当前scan context的距离值,选择其中距离最近的一个候选关键帧作为匹配帧,并返回此关键帧。
作为优选的,所述S4包括:
获取搜索匹配的关键帧,将所述关键帧的位姿作为下一步点云匹配的初始位姿;
计算当前的激光点云和关键帧点云的FPFH,使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量;
将所述相对旋转变量作为迭代最近点算法的初值,计算获得实际的激光点云和关键帧点云的相对位姿变换;
将所述相对位姿变换乘以关键帧的点云,获得全局位姿;
将全局位姿传入定位节点,所述定位节点计算全局位姿的匹配误差;
将所述匹配误差与预设误差阈值比较,若匹配误差小于预设误差阈值,则重定位成功,否则,重定位失败。
本发明还公开了一种基于激光雷达的重定位***,包括:
建图模块,所述建图模块用于驱动机器人遍历整个环境空间,通过机器人上的激光雷达传感器采集环境信息,构建完整的点云地图;
点云数据库构建模块,所述点云数据库构建模块驱动机器人继续工作并记录当前时刻的点云数据和位姿信息,将当前时刻的激光点云数据和其位姿信息作为关键帧保存,直至机器人跑完建图模块获取的点云地图,获得包含关键帧信息的点云数据库;
搜索匹配模块,当机器人定位丢失时,所述搜索匹配模块获取机器人当前时刻的点云数据,将机器人当前时刻的点云数据与点云数据库进行匹配搜索,获得搜索匹配对应的关键帧;
重定位模块,所述重定位模块根据搜索匹配的关键帧,计算机器人全局位姿信息以实现重定位。
作为优选的,所述重定位模块具体执行以下操作:
获取搜索匹配的关键帧,将所述关键帧的位姿作为下一步点云匹配的初始位姿;
计算当前的激光点云和关键帧点云的FPFH,使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量;
将所述相对旋转变量作为迭代最近点算法的初值,计算获得实际的激光点云和关键帧点云的相对位姿变换;
将所述相对位姿变换乘以关键帧的点云,获得全局位姿;
将全局位姿传入定位节点,所述定位节点计算全局位姿的匹配误差;
将所述匹配误差与预设误差阈值比较,若匹配误差小于预设误差阈值,则重定位成功,否则,重定位失败。
本发明的上述技术方案相比现有技术具有以下优点:
1、本发明在定位模式下建立重定位地图,节省内存,简单高效。
2、本发明运用scan context进行点云的匹配搜索,准确快捷。
3、本发明基于截断最小二乘和FPFH特征计算匹配初值,精度更高。
4、本发明不依赖GPS信号,应用范围广。
5、本发明中的机器人不受所处环境的影响,在光照变化显著或者环境纹理特征缺失的情况下,不会出现误匹配甚至于匹配失败,重定位效果好。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明,以使本领域的技术人员可以更好地理解本发明并能予以实施,但所举实施例不作为对本发明的限定。
参照图1-n所示,本发明公开了一种基于激光雷达的重定位方法,包括以下步骤:
步骤一、机器人遍历整个环境空间,通过机器人上的激光雷达传感器采集环境信息,构建完整的点云地图。
步骤二、机器人继续工作并记录当前时刻的点云数据和位姿信息,将当前时刻的激光点云数据和其位姿信息作为关键帧保存,直至机器人跑完步骤一中的点云地图,获得包含关键帧信息的点云数据库。其中,所述关键帧的结构体还包括激光点云数据的scancontext(上下文扫描)描述。
机器人继续工作并记录当前时刻的点云数据和位姿信息,将当前时刻的激光点云数据和其位姿信息作为关键帧保存,包括:获取当前时刻下机器人在地图坐标系下的位姿话题,提取当前关键帧;计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换,所述相对变换包括机器人的空间位置变换和机器人的空间旋转角度变换;当机器人的空间位置变换大于预设距离阈值或者当机器人的空间旋转角度变换大于预设角度阈值时,创建新的关键帧;将创建的新的关键帧赋值给当前关键帧以更新当前关键帧;保存更新后的当前的关键帧的位姿、当前时刻的点云数据并计算其scan context描述。
具体的,设机器人发布的里程计位姿为Ti,当前关键帧的位姿TKcurrent,设机器人初始里程计位姿为T0,令机器人初始关键帧位姿TK0=T0;
当机器人在运行定位的时候,计算其中,ΔT为i时刻的机器人里程计位姿态与当前最近的关键帧位姿的相对变换,ΔT包括空间位置变换ΔP和空间旋转角度变换ΔQ。
步骤三、当机器人定位丢失时,获取机器人当前时刻的点云数据,将机器人当前时刻的点云数据与点云数据库进行匹配搜索,获得搜索匹配对应的关键帧,包括:
S31、将当前位姿的一帧点云数据用scan context表示;
S32、在保存好的地图关键帧中scan context中搜索出相近的候选关键帧,获得最近邻的多个候选关键帧;
S33、遍历候选关键帧,选择距离最近的帧作为匹配帧并返回此关键帧。
其中,所述S31和S32之间还包括:计算scan context的每一行的均值作为RingKey;所述S32具体包括:用保存的第一关键帧的RingKey构建Kd-tree以搜索候选关键帧。
所述S33具体包括:不断对任一候选关键帧的scan context进行循环右移操作,依次计算出候选关键帧与当前scan context的距离值,选择其中距离最近的一个候选关键帧作为匹配帧,并返回此关键帧。
步骤四、根据搜索匹配的关键帧,计算机器人全局位姿信息以实现重定位,包括:
S41、获取搜索匹配的关键帧,将所述关键帧的位姿作为下一步点云匹配的初始位姿;
S42、计算当前的激光点云和关键帧点云的FPFH,使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量;
S43、将所述相对旋转变量作为迭代最近点算法的初值,计算获得实际的激光点云和关键帧点云的相对位姿变换;
S44、将所述相对位姿变换乘以关键帧的点云,获得全局位姿;
S45、将全局位姿传入定位节点,所述定位节点计算全局位姿的匹配误差;
S46、将所述匹配误差与预设误差阈值比较,若匹配误差小于预设误差阈值,则重定位成功,否则,重定位失败。
本发明还公开了一种基于激光雷达的重定位***,包括建图模块、点云数据库构建模块、搜索匹配模块和重定位模块。
所述建图模块用于驱动机器人遍历整个环境空间,通过机器人上的激光雷达传感器采集环境信息,构建完整的点云地图。
所述点云数据库构建模块驱动机器人继续工作并记录当前时刻的点云数据和位姿信息,将当前时刻的激光点云数据和其位姿信息作为关键帧保存,直至机器人跑完建图模块获取的点云地图,获得包含关键帧信息的点云数据库。
当机器人定位丢失时,所述搜索匹配模块获取机器人当前时刻的点云数据,将机器人当前时刻的点云数据与点云数据库进行匹配搜索,获得搜索匹配对应的关键帧。
所述重定位模块根据搜索匹配的关键帧,计算机器人全局位姿信息以实现重定位,具体的:获取搜索匹配的关键帧,将所述关键帧的位姿作为下一步点云匹配的初始位姿;计算当前的激光点云和关键帧点云的FPFH,使用截断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量;将所述相对旋转变量作为迭代最近点算法的初值,计算获得实际的激光点云和关键帧点云的相对位姿变换;将所述相对位姿变换乘以关键帧的点云,获得全局位姿;将全局位姿传入定位节点,所述定位节点计算全局位姿的匹配误差;将所述匹配误差与预设误差阈值比较,若匹配误差小于预设误差阈值,则重定位成功,否则,重定位失败。
下面,结合具体实施例对本发明中的技术方案做进一步说明。
1.***组成
本发明***主要包括以下三个模块:1.构建重定位地图模块;2.scan context匹配搜索模块;3.全局位姿匹配计算模块。其***示意图如图2所示。
2.构建重定位地图
激光SLAM中点云地图是由一系列激光点云及其scan context描述和其在地图中所对应的位姿组成的。因此,本发明中定义的关键帧结构体包括激光点云数据与其scancontext描述和其位姿信息。
当在ROS(机器人操作***)环境下运行本方案定位节点时,会发布当前时刻下机器人在地图坐标系下的位姿话题。设机器人发布的里程计位姿为Ti,当前关键帧的位姿TKcurrent,设机器人初始里程计位姿为T0,令机器人初始关键帧位姿为TK0=T0。当机器人在运行定位的时候,计算其中ΔT为i时刻的机器人里程计位姿态与当前最近的关键帧位姿的相对变换。其中ΔT包括,空间位置变换ΔP和空间旋转角度变换ΔQ。
每当ΔP大于一定距离或者ΔQ大于一定角度时(角度和距离阈值可视地图大小确定),将创建一个新的关键帧,并把刚刚创建的关键帧赋值给TKcurrent,并保存当前的关键帧位姿TKj,以及当前时刻的点云数据并计算其scan context描述。其流程图见图3。
3.scan context搜索匹配
当重定位地图构建完毕后,重定位节点运行期间导入地图。地图中关键帧位姿及其点云是一一对应的。当定位丢失后,计算过程如下:
将当前位姿的一帧点云数据用scan context表示,并在保存好的地图关键帧中scan context中搜索出相近的候选关键帧。可选地,为了加快搜索速度,在存储关键帧数据scan context时,可以计算scan context的每一行的均值作为RingKey,然后依此RingKey值构造kd-tree结构。
通常上一步会得到最近邻的几个候选关键帧,又因scan context的数据表现形式与机器人的姿态有关,所以需要不断对任一候选关键帧的scan context进行循环右移操作,依次计算出与当前scan context的距离值,然后选择其中距离最小的一个关键帧,并返回此关键帧。其流程图见图4所示。
4.点云匹配计算全局位姿
主要思想:计算当前激光点云与检索到的关键帧之间的位姿相对变换,将计算得到的相对位姿变换乘上检索到的关键帧对应的地图位姿,得到的位姿即机器人在地图上的全局位姿。参照图5所示,其主要流程如下:
A.取出步骤3搜索匹配对应的关键帧,将此关键帧的位姿作为下一步点云匹配的初始位姿。
B.对关键帧的点云和当前的激光点云计算FPFH(点云特征直方图),使用截断最小二乘和半定松弛算法解出两帧点云的相对旋转变量。
C.以此作为初值放入ICP(Iterative Closest Point,迭代最近点)算法中,计算实际的两帧点云的相对位姿变换。
D.最后将计算后的位姿变换乘以关键帧的点云,得到的就是全局位姿。
E.将计算得到的全局位姿,传入定位节点,如果定位节点计算出此位姿匹配误差大,则说明重定位位姿不正确,则重定位失败,需要人工定位。
具体的,本发明的开发环境及数据格式如下:开发语言:C++;开发依赖开源软件库:ROS,PCL,Teaserplusplus,Eigen;数据存储格式:ros格式,txt格式。
其中一实施例具体如下:
1.在ROS(机器人操作***)环境下运行定位节点,计算通过Eigen库计算出平移变换量t和旋转变换量r。
2.当t>2米或者r>15度,保存当前的位姿,点云数据。
3.将当前位姿的一帧点云数据用scan context表示。
4、将当前时刻的位姿,点云数据组成一个关键帧,重复2,3,4步骤,直到完成重定位建图。
5、启动重定位节点,导入建好的重定位地图,等待定位程序发出重定位请求。
6、当前位姿的一帧点云数据用scan context表示,并在保存的地图关键帧scancontext中搜索出最近的关键帧。
7、对当前时刻的点云和找到的最近关键帧提取FPFH特征,用Teaserplusplus库将FPFH特征作为输入,计算两帧点云的相对旋转量。
8、将两帧点云和步骤7计算好的相对旋转量作为初值放入ICP计算,得到的位姿乘以步骤6返回的关键帧位姿,即为全局重定位位姿。如果定位节点位姿误差大于1.5,则需要手动重定位。
本领域内的技术人员应明白,本申请的实施例可提供为方法、***、或计算机程序产品。因此,本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然,上述实施例仅仅是为清楚地说明所作的举例,并非对实施方式的限定。对于所属领域的普通技术人员来说,在上述说明的基础上还可以做出其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。