【技术实现步骤摘要】
本专利技术涉及移动机器人定位，尤其涉及一种基于3d激光雷达的移动机器人位姿跟踪方法及装置。
技术介绍
1、自主导航功能是移动机器人智能化程度的重要评价指标，而具有自主导航功能的移动机器人首要解决的问题是确定机器人自身相对周围环境的位姿，即机器人的定位问题。传统的机器人定位方式，如使用全球定位系统（global positioning system，gps）进行导航，必须依赖于gps信号，移动机器人在树荫遮挡、gps信号弱等区域内的定位精度和可靠性难以保证，而且在室内、隧道等场景中由于无法接收gps信号还可能会导致机器人定位完全丢失。定位与建图（simultaneous localization and mapping，slam）技术可以解决机器人的定位和地图问题，但现有技术中用于自主导航的slam技术大多是基于2d栅格地图，2d栅格地图是使用二维平面描述三维空间，因此无法准确描述环境中的障碍物，也无法准确反映机器人相对地图的位姿，因而实际定位精度并不高。
2、其他如基于机器学习等的机器人定位方法，通过借助深度学习方法识别环境中的语义信息，能够利用语义信息完成机器人的定位，但机器学习方法对机器人搭载计算机的算力具有非常高的要求，并且该类方法的实时性较差，难以提供实时的机器人位姿信息。
3、综上，现有技术中针对于自主移动导航机器人定位方法不仅可靠性、实时性较差，而且缺少三维环境的细节描述，难以为自主导航机器人提供稳定准确的位姿信息。
技术实现思路
1、本专利技术
2、为解决上述技术问题，本专利技术提出的技术方案为：
3、一种基于3d激光雷达的移动机器人位姿跟踪方法，步骤包括：
4、步骤s01.地图构建：由搭载有3d激光雷达的移动机器人对所在环境进行遍历扫描，遍历过程中对扫描得到的各帧激光点云数据进行帧间匹配，依次得到相邻两帧点云数据之间的相对空间关系，根据所述相对空间关系选取关键帧，保存选取的各个关键帧以及关键帧对应的位姿；
5、步骤s02.位姿跟踪：当需要对移动机器人进行跟踪时，在机器人行进过程中，实时从激光雷达扫描得到的激光点云数据中选取出关键帧作为当前关键帧并进行位姿初始化，初始化后从保存的各关键帧中选取出与当前关键帧位姿相近的关键帧进行拼接形成局部子图，利用当前关键帧相对于所述局部子图的位姿确定出实时机器人的位姿，实现机器人的位姿跟踪。
6、进一步的，步骤s01中根据所述相对空间关系选取关键帧时，如果当前帧与上一关键帧之间的平移距离或者旋转角度的累积值大于预设空间阈值，则选取当前帧作为关键帧。
7、进一步的，步骤s02包括：
8、步骤s201.每隔指定时间间隔选取一帧激光点云数据作为当前关键帧；
9、步骤s202.对当前关键帧的位姿进行初始化，得到初始化后当前关键帧的位姿；
10、步骤s203.对当前连续激光数据帧进行帧间匹配得到当前空间变换关系；
11、步骤s204. 根据所述当前空间变换关系从保存的各关键帧中查找与初始化后当前关键帧位姿相近的关键帧，将查找到的关键帧拼接形成局部子图；
12、步骤s205.将当前关键帧与局部子图进行匹配得到当前帧相对于局部子图的位姿；
13、步骤s206.将所述当前帧相对于局部子图的位姿施加给局部子图的空间坐标得到当前机器人的位姿；
14、步骤s207.循环执行步骤s201～s206，完成机器人位姿跟踪。
15、进一步的，步骤s202中，如果当前关键帧激光点云数据为第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹的起始点位姿，如果当前关键帧激光点云数据为非第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹中与坐标空间距离上最近的关键帧的位姿，其中坐标按照计算得到，为上一时刻关键帧与对应的局部子图之间的相对空间关系，为局部子图的坐标矩阵。
16、进一步的，步骤s204中，按照下式将查找到的各关键帧拼接形成局部子图：
17、
18、其中，为第 i个当前关键帧中的激光点在局部子图中的坐标，为局部子图的坐标，为保存的关键帧的坐标。
19、进一步的，步骤s205中，如果当前关键帧经过位姿初始化后得到的初始化后当前关键帧的位姿在机器人历史轨迹中关键帧附近，关键帧所对应拼接得到的局部子图为，通过icp配准方法配准当前帧与局部子图得到当前关键帧相对于局部子图的空间坐标关系；步骤s206中，将当前关键帧与局部子图得到当前关键帧相对于局部子图的空间坐标关系施加给局部子图的空间坐标得到当前关键帧的空间坐标，当前关键帧的空间坐标按照下式计算得到：
20、
21、其中，为当前关键帧的空间坐标，即跟踪的机器人位姿，为当前关键帧与局部子图的相对空间关系，为局部子图的空间坐标。
22、进一步的，所述步骤s02中，采用icp（iterative closest point）配准方法对扫描得到的激光点云帧数据进行帧间匹配，得到相邻两帧点云数据之间的空间关系，包括：
23、对于两帧连续的激光点云数据和，其中与是两帧激光中相对应距离最小的激光点对，寻找欧式变换t使得两帧激光点云数据满足以下关系式：
24、
25、其中，r为旋转矩阵，t为平移向量，变换矩阵t与旋转矩阵r和平移向量t满足；
26、使用icp配准方法进行迭代计算直至误差平方和达到极小值，求解得到变换矩阵t。
27、一种基于3d激光雷达的移动机器人位姿跟踪装置，包括：
28、地图构建模块，用于由搭载有3d激光雷达的移动机器人对所在环境进行遍历扫描，遍历过程中对扫描得到的各帧激光点云数据进行帧间匹配，依次得到相邻两帧点云数据之间的相对空间关系，根据所述相对空间关系选取关键帧，保存选取的各个关键帧以及关键帧对应的位姿；
29、位姿跟踪模块，用于当需要对移动机器人进行跟踪时，在机器人行进过程中，实时从激光雷达扫描得到的激光点云数据中选取出关键帧作为当前关键帧并进行位姿初始化，初始化后从保存的各关键帧中选取出与当前关键帧位姿相近的关键帧进行拼接形成局部子图，利用当前关键帧相对于所述局部子图的位姿确定出实时机器人的位姿，实现机器人的位姿跟踪。
30、进一步的，所述位姿跟踪模块包括：
31、当前帧选取单元，用于每隔指定时间间隔选取一帧激光点云数据作为当前关键帧，并计算出对应的位姿以作为被跟踪的机器人位姿；
32、位姿初始化单元，用于对当前关键帧的位姿进行初始化，得到初始化后当前关键帧的位姿；
33本文档来自技高网...
【技术保护点】
1.一种基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤包括：
2.根据权利要求1所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S01中根据所述相对空间关系选取关键帧时，如果当前帧与上一关键帧之间的平移距离或者旋转角度的累积值大于预设空间阈值，则选取当前帧作为关键帧。
3.根据权利要求1所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S02包括：
4.根据权利要求3所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S202中，如果当前关键帧激光点云数据为第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹的起始点位姿，如果当前关键帧激光点云数据为非第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹中与坐标空间距离上最近的关键帧的位姿，其中坐标按照计算得到，为上一时刻关键帧与对应的局部子图之间的相对空间关系，为局部子图的坐标矩阵。
5.根据权利要求3所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S204中，按照下式将查找到的各关键帧拼接形成局部子图：
6.根据权利要求3所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤S205中，如果当前关键帧经过位姿初始化后得到的初始化后当前关键帧的位姿在机器人历史轨迹中关键帧附近，关键帧所对应拼接得到的局部子图为，通过ICP配准方法配准当前帧与局部子图得到当前关键帧相对于局部子图的空间坐标关系；步骤S206中，将当前关键帧与局部子图得到当前关键帧相对于局部子图的空间坐标关系施加给局部子图的空间坐标得到当前关键帧的空间坐标，当前关键帧的空间坐标按照下式计算得到：
7.根据权利要求1~6中任意一项所述的基于3D激光雷达的移动机器人位姿跟踪方法，其特征在于，所述步骤S02中，采用ICP配准方法对扫描得到的激光点云帧数据进行帧间匹配，得到相邻两帧点云数据之间的空间关系，包括：
8.一种基于3D激光雷达的移动机器人位姿跟踪装置，其特征在于，包括：
9.根据权利要求8所述的基于3D激光雷达的移动机器人位姿跟踪装置，其特征在于，所述位姿跟踪模块包括：
10.一种基于3D激光雷达的移动机器人位姿跟踪装置，包括3D激光雷达，搭载在移动机器人上，以用于对移动机器人周围环境进行扫描得到激光点云数据，其特征在于，还包括处理器以及存储器，所述存储器用于存储计算机程序，所述处理器用于执行所述计算机程序以执行如权利要求1～7中任意一项所述方法。
...【技术特征摘要】
1.一种基于3d激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤包括：
2.根据权利要求1所述的基于3d激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤s01中根据所述相对空间关系选取关键帧时，如果当前帧与上一关键帧之间的平移距离或者旋转角度的累积值大于预设空间阈值，则选取当前帧作为关键帧。
3.根据权利要求1所述的基于3d激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤s02包括：
4.根据权利要求3所述的基于3d激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤s202中，如果当前关键帧激光点云数据为第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹的起始点位姿，如果当前关键帧激光点云数据为非第一帧数据，将当前关键帧对应的位姿初始化为机器人历史轨迹中与坐标空间距离上最近的关键帧的位姿，其中坐标按照计算得到，为上一时刻关键帧与对应的局部子图之间的相对空间关系，为局部子图的坐标矩阵。
5.根据权利要求3所述的基于3d激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤s204中，按照下式将查找到的各关键帧拼接形成局部子图：
6.根据权利要求3所述的基于3d激光雷达的移动机器人位姿跟踪方法，其特征在于，步骤s20...
【专利技术属性】
技术研发人员：孙波，李道胜，李涛，宋海，
申请(专利权)人：苏州中德睿博智能科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。