基于激光雷达的重定位方法及系统与流程

1.本发明涉及机器人定位技术领域，尤其是指一种基于激光雷达的重定位 方法及系统。


背景技术：

2.移动机器人在复杂环境下实行导航任务时，经常会出现其定位丢失的情 形，从而导致其导航任务的失败。目前解决机器人定位丢失的方案主要有如 下两种:第一、利用gps卫星信号的重定位，当机器人定位丢失时，利用gps 信号计算出其位置信息，从而实现机器人的重新定位；第二、基于图像特征 的视觉定位，当机器人定位丢失的时候，利用机器人当前状态下获取的图像 特征与建图时所保存的全局图像特征库进行匹配，当匹配成功时，即可获得 机器人的位姿。
3.上述两种方案都有其不足之处，第一种方案由于gps信号的强弱和精度 会受所处环境的影响，比如当机器人在桥梁下，隧道内，地下室及室内或处 于高大树木建筑附近，其gps信号精度都会受到不同程度的影响，因而基于 gps重定位的应用具有一定的局限性；而第二种基于图像视觉特征的方案同 样会受到其所处环境的影响，在光照变化显著或者环境纹理特征缺失的情况 下，会出现误匹配甚至匹配失败，都会导致重定位的失败。


技术实现要素：

4.为此，本发明所要解决的技术问题在于克服现有技术中定位受gps信号 强弱和精度影响较大，以及定位受光照及环境纹理特征缺失所导致重定位失 败的技术缺陷。
5.为解决上述技术问题，本发明提供了一种基于激光雷达的重定位方法， 包括以下步骤：
6.s1、机器人遍历整个环境空间，通过机器人上的激光雷达传感器采集环 境信息，构建完整的点云地图；
7.s2、机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时 刻的激光点云数据和其位姿信息作为关键帧保存，直至机器人跑完s1中的点 云地图，获得包含关键帧信息的点云数据库；
8.s3、当机器人定位丢失时，获取机器人当前时刻的点云数据，将机器人 当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹配对应的关键 帧；
9.s4、根据搜索匹配的关键帧，计算机器人全局位姿信息以实现重定位。
10.作为优选的，所述s2中，所述关键帧的结构体还包括激光点云数据的scancontext描述。
11.作为优选的，所述s2中，机器人继续工作并记录当前时刻的点云数据和 位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存，包括：
12.获取当前时刻下机器人在地图坐标系下的位姿话题，提取当前关键帧；
13.计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换，所述相 对变换
包括机器人的空间位置变换和机器人的空间旋转角度变换；
14.当机器人的空间位置变换大于预设距离阈值或者当机器人的空间旋转角 度变换大于预设角度阈值时，创建新的关键帧；
15.将创建的新的关键帧赋值给当前关键帧以更新当前关键帧；
16.保存更新后的当前的关键帧的位姿、当前时刻的点云数据并计算其scancontext描述。
17.作为优选的，获取当前时刻下机器人在地图坐标系下的位姿话题，提取 当前关键帧；计算机器人里程计位姿态与当前最近的关键帧位姿的相对变换， 所述相对变换包括机器人的空间位置变换和机器人的空间旋转角度变换，具 体包括：
18.设机器人发布的里程计位姿为ti，当前关键帧的位姿tk
current
，设机器人初 始里程计位姿为t0,令机器人初始关键帧位姿tk0＝t0；
19.当机器人在运行定位的时候，计算其中，δt为i时刻的机 器人里程计位姿态与当前最近的关键帧位姿的相对变换,δt包括空间位置变 换δp和空间旋转角度变换δq。
20.作为优选的，所述s3包括：
21.s31、将当前位姿的一帧点云数据用scan context表示；
22.s32、在保存好的地图关键帧中scan context中搜索出相近的候选关键 帧，获得最近邻的多个候选关键帧；
23.s33、遍历候选关键帧，选择距离最近的帧作为匹配帧并返回此关键帧。
24.作为优选的，所述s31和s32之间还包括：
25.计算scan context的每一行的均值作为ringkey；
26.所述s32具体包括：用保存的第一关键帧的ringkey构建kd-tree以搜 索候选关键帧。
27.作为优选的，所述s33具体包括：
28.不断对任一候选关键帧的scan context进行循环右移操作，依次计算出 候选关键帧与当前scan context的距离值，选择其中距离最近的一个候选关 键帧作为匹配帧，并返回此关键帧。
29.作为优选的，所述s4包括：
30.获取搜索匹配的关键帧，将所述关键帧的位姿作为下一步点云匹配的初 始位姿；
31.计算当前的激光点云和关键帧点云的fpfh，使用截断最小乘和半定松弛 算法获得激光点云和关键帧点云的相对旋转变量；
32.将所述相对旋转变量作为迭代最近点算法的初值，计算获得实际的激光 点云和关键帧点云的相对位姿变换；
33.将所述相对位姿变换乘以关键帧的点云，获得全局位姿；
34.将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差；
35.将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈值， 则重定位成功，否则，重定位失败。
36.本发明还公开了一种基于激光雷达的重定位系统，包括：
37.建图模块，所述建图模块用于驱动机器人遍历整个环境空间，通过机器 人上的激光雷达传感器采集环境信息，构建完整的点云地图；
38.点云数据库构建模块，所述点云数据库构建模块驱动机器人继续工作并 记录当前时刻的点云数据和位姿信息，将当前时刻的激光点云数据和其位姿 信息作为关键帧保存，直至机器人跑完建图模块获取的点云地图，获得包含 关键帧信息的点云数据库；
39.搜索匹配模块，当机器人定位丢失时，所述搜索匹配模块获取机器人当 前时刻的点云数据，将机器人当前时刻的点云数据与点云数据库进行匹配搜 索，获得搜索匹配对应的关键帧；
40.重定位模块，所述重定位模块根据搜索匹配的关键帧，计算机器人全局 位姿信息以实现重定位。
41.作为优选的，所述重定位模块具体执行以下操作：
42.获取搜索匹配的关键帧，将所述关键帧的位姿作为下一步点云匹配的初 始位姿；
43.计算当前的激光点云和关键帧点云的fpfh，使用截断最小乘和半定松弛 算法获得激光点云和关键帧点云的相对旋转变量；
44.将所述相对旋转变量作为迭代最近点算法的初值，计算获得实际的激光 点云和关键帧点云的相对位姿变换；
45.将所述相对位姿变换乘以关键帧的点云，获得全局位姿；
46.将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差；
47.将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈值， 则重定位成功，否则，重定位失败。
48.本发明的上述技术方案相比现有技术具有以下优点：
49.1、本发明在定位模式下建立重定位地图，节省内存，简单高效。
50.2、本发明运用scan context进行点云的匹配搜索，准确快捷。
51.3、本发明基于截断最小二乘和fpfh特征计算匹配初值，精度更高。
52.4、本发明不依赖gps信号，应用范围广。
53.5、本发明中的机器人不受所处环境的影响，在光照变化显著或者环境 纹理特征缺失的情况下，不会出现误匹配甚至于匹配失败，重定位效果好。
附图说明
54.图1为本发明基于激光雷达的重定位方法的流程图；
55.图2为基于激光雷达重定位的系统示意图；
56.图3为基于激光传感器的重定位地图构建流程图；
57.图4为基于点云scan context的搜索匹配算法；
58.图5为重定位全局位姿计算流程图。
具体实施方式
59.下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术 人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的 限定。
60.参照图1所示，本发明公开了一种基于激光雷达的重定位方法，包括以 下步骤：
61.步骤一、机器人遍历整个环境空间，通过机器人上的激光雷达传感器采 集环境信息，构建完整的点云地图。
62.步骤二、机器人继续工作并记录当前时刻的点云数据和位姿信息，将当 前时刻的激光点云数据和其位姿信息作为关键帧保存，直至机器人跑完步骤 一中的点云地图，获得包含关键帧信息的点云数据库。其中，所述关键帧的 结构体还包括激光点云数据的scan context(上下文扫描)描述。
63.机器人继续工作并记录当前时刻的点云数据和位姿信息，将当前时刻的 激光点云数据和其位姿信息作为关键帧保存，包括：获取当前时刻下机器人 在地图坐标系下的位姿话题，提取当前关键帧；计算机器人里程计位姿态与 当前最近的关键帧位姿的相对变换，所述相对变换包括机器人的空间位置变 换和机器人的空间旋转角度变换；当机器人的空间位置变换大于预设距离阈 值或者当机器人的空间旋转角度变换大于预设角度阈值时，创建新的关键帧； 将创建的新的关键帧赋值给当前关键帧以更新当前关键帧；保存更新后的当 前的关键帧的位姿、当前时刻的点云数据并计算其scan context描述。
64.具体的，设机器人发布的里程计位姿为ti，当前关键帧的位姿tk
current
，设 机器人初始里程计位姿为t0,令机器人初始关键帧位姿tk0＝t0；
65.当机器人在运行定位的时候，计算其中，δt为i时刻的机 器人里程计位姿态与当前最近的关键帧位姿的相对变换,δt包括空间位置变 换δp和空间旋转角度变换δq。
66.步骤三、当机器人定位丢失时，获取机器人当前时刻的点云数据，将机 器人当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹配对应的 关键帧，包括：
67.s31、将当前位姿的一帧点云数据用scan context表示；
68.s32、在保存好的地图关键帧中scan context中搜索出相近的候选关键 帧，获得最近邻的多个候选关键帧；
69.s33、遍历候选关键帧，选择距离最近的帧作为匹配帧并返回此关键帧。
70.其中，所述s31和s32之间还包括：计算scan context的每一行的均值 作为ringkey；所述s32具体包括：用保存的第一关键帧的ringkey构建 kd-tree以搜索候选关键帧。
71.所述s33具体包括：不断对任一候选关键帧的scan context进行循环右 移操作，依次计算出候选关键帧与当前scan context的距离值，选择其中距 离最近的一个候选关键帧作为匹配帧，并返回此关键帧。
72.步骤四、根据搜索匹配的关键帧，计算机器人全局位姿信息以实现重定 位，包括：
73.s41、获取搜索匹配的关键帧，将所述关键帧的位姿作为下一步点云匹配 的初始位姿；
74.s42、计算当前的激光点云和关键帧点云的fpfh，使用截断最小乘和半定 松弛算法获得激光点云和关键帧点云的相对旋转变量；
75.s43、将所述相对旋转变量作为迭代最近点算法的初值，计算获得实际的 激光点云和关键帧点云的相对位姿变换；
76.s44、将所述相对位姿变换乘以关键帧的点云，获得全局位姿；
77.s45、将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差；
78.s46、将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈 值，则重定位成功，否则，重定位失败。
79.本发明还公开了一种基于激光雷达的重定位系统，包括建图模块、点云 数据库构建模块、搜索匹配模块和重定位模块。
80.所述建图模块用于驱动机器人遍历整个环境空间，通过机器人上的激光 雷达传感器采集环境信息，构建完整的点云地图。
81.所述点云数据库构建模块驱动机器人继续工作并记录当前时刻的点云数 据和位姿信息，将当前时刻的激光点云数据和其位姿信息作为关键帧保存， 直至机器人跑完建图模块获取的点云地图，获得包含关键帧信息的点云数据 库。
82.当机器人定位丢失时，所述搜索匹配模块获取机器人当前时刻的点云数 据，将机器人当前时刻的点云数据与点云数据库进行匹配搜索，获得搜索匹 配对应的关键帧。
83.所述重定位模块根据搜索匹配的关键帧，计算机器人全局位姿信息以实 现重定位，具体的：获取搜索匹配的关键帧，将所述关键帧的位姿作为下一 步点云匹配的初始位姿；计算当前的激光点云和关键帧点云的fpfh，使用截 断最小乘和半定松弛算法获得激光点云和关键帧点云的相对旋转变量；将所 述相对旋转变量作为迭代最近点算法的初值，计算获得实际的激光点云和关 键帧点云的相对位姿变换；将所述相对位姿变换乘以关键帧的点云，获得全 局位姿；将全局位姿传入定位节点，所述定位节点计算全局位姿的匹配误差； 将所述匹配误差与预设误差阈值比较，若匹配误差小于预设误差阈值，则重 定位成功，否则，重定位失败。
84.下面，结合具体实施例对本发明中的技术方案做进一步说明。
85.1.系统组成
86.本发明系统主要包括以下三个模块：1.构建重定位地图模块；2.scancontext匹配搜索模块；3.全局位姿匹配计算模块。其系统示意图如图2所 示。
87.2.构建重定位地图
88.激光slam中点云地图是由一系列激光点云及其scan context描述和其 在地图中所对应的位姿组成的。因此，本发明中定义的关键帧结构体包括激 光点云数据与其scan context描述和其位姿信息。
89.当在ros(机器人操作系统)环境下运行本方案定位节点时，会发布当前 时刻下机器人在地图坐标系下的位姿话题。设机器人发布的里程计位姿为 ti，当前关键帧的位姿tk
current
，设机器人初始里程计位姿为t0,令机器人初 始关键帧位姿为tk0＝t0。当机器人在运行定位的时候，计算其中δt为i时刻的机器人里程计位姿态与当前最近的关键帧位姿的相对变 换。其中δt包括，空间位置变换δp和空间旋转角度变换δq。
90.每当δp大于一定距离或者δq大于一定角度时(角度和距离阈值可视地 图大小确定)，将创建一个新的关键帧，并把刚刚创建的关键帧赋值给 tk
current
，并保存当前的关键帧位姿tkj，以及当前时刻的点云数据并计算其 scan context描述。其流程图见图3。
91.3.scan context搜索匹配
92.当重定位地图构建完毕后，重定位节点运行期间导入地图。地图中关键 帧位姿及
其点云是一一对应的。当定位丢失后，计算过程如下：
93.将当前位姿的一帧点云数据用scan context表示，并在保存好的地图 关键帧中scan context中搜索出相近的候选关键帧。可选地，为了加快搜 索速度，在存储关键帧数据scan context时，可以计算scan context的每 一行的均值作为ringkey,然后依此ringkey值构造kd-tree结构。
94.通常上一步会得到最近邻的几个候选关键帧，又因scan context的数 据表现形式与机器人的姿态有关，所以需要不断对任一候选关键帧的scancontext进行循环右移操作，依次计算出与当前scan context的距离值， 然后选择其中距离最小的一个关键帧，并返回此关键帧。其流程图见图4所 示。
95.4.点云匹配计算全局位姿
96.主要思想：计算当前激光点云与检索到的关键帧之间的位姿相对变换， 将计算得到的相对位姿变换乘上检索到的关键帧对应的地图位姿，得到的位 姿即机器人在地图上的全局位姿。参照图5所示，其主要流程如下：
97.a.取出步骤3搜索匹配对应的关键帧，将此关键帧的位姿作为下一步点 云匹配的初始位姿。
98.b.对关键帧的点云和当前的激光点云计算fpfh(点云特征直方图)，使 用截断最小二乘和半定松弛算法解出两帧点云的相对旋转变量。
99.c.以此作为初值放入icp(iterative closest point，迭代最近点)算 法中，计算实际的两帧点云的相对位姿变换。
100.d.最后将计算后的位姿变换乘以关键帧的点云，得到的就是全局位姿。
101.e.将计算得到的全局位姿，传入定位节点，如果定位节点计算出此位姿 匹配误差大，则说明重定位位姿不正确，则重定位失败，需要人工定位。
102.具体的，本发明的开发环境及数据格式如下：开发语言：c ；开发依 赖开源软件库：ros,pcl,teaserplusplus,eigen；数据存储格式：ros 格式,txt格式。
103.其中一实施例具体如下：
104.1.在ros(机器人操作系统)环境下运行定位节点，计算通过eigen库计算出平移变换量t和旋转变换量r。
105.2.当t》2米或者r》15度，保存当前的位姿，点云数据。
106.3.将当前位姿的一帧点云数据用scan context表示。
107.4、将当前时刻的位姿，点云数据组成一个关键帧，重复2,3,4步骤， 直到完成重定位建图。
108.5、启动重定位节点，导入建好的重定位地图，等待定位程序发出重定 位请求。
109.6、当前位姿的一帧点云数据用scan context表示，并在保存的地图关 键帧scan context中搜索出最近的关键帧。
110.7、对当前时刻的点云和找到的最近关键帧提取fpfh特征，用 teaserplusplus库将fpfh特征作为输入，计算两帧点云的相对旋转量。
111.8、将两帧点云和步骤7计算好的相对旋转量作为初值放入icp计算， 得到的位姿乘以步骤6返回的关键帧位姿，即为全局重定位位姿。如果定位 节点位姿误差大于1.5，则
需要手动重定位。
112.本领域内的技术人员应明白，本技术的实施例可提供为方法、系统、或 计算机程序产品。因此，本技术可采用完全硬件实施例、完全软件实施例、 或结合软件和硬件方面的实施例的形式。而且，本技术可采用在一个或多个 其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘 存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
113.本技术是参照根据本技术实施例的方法、设备(系统)、和计算机程序 产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程 图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流 程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算 机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使 得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实 现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定 的功能的装置。
114.这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理 设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储 器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程 或多个流程和/或方框图一个方框或多个方框中指定的功能。
115.这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上， 使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现 的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程 图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的 步骤。
116.显然，上述实施例仅仅是为清楚地说明所作的举例，并非对实施方式的 限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出 其它不同形式变化或变动。这里无需也无法对所有的实施方式予以穷举。而 由此所引伸出的显而易见的变化或变动仍处于本发明创造的保护范围之中。