[发明专利]运动装置的轨迹数据处理方法及其处理系统

说明书

本发明提供一种运动装置的轨迹数据处理方法及其处理系统，所述获取方法包括获取运动装置外部环境轮廓的相继两帧点云，以及所述两帧点云之间编码器的相对平移量和惯性测量单元的角度改变量，其中，分别将所述两帧点云标记为参考帧点云和当前帧点云；利用所述平移改变量和所述角度改变量构建初始变换矩阵；利用所述初始变换矩阵对所述当前帧点云进行数据变换，以获取过渡帧点云；利用所述参考帧点云和所述过渡帧点云得到对应点对集合；利用所述对应点对集合得到所述运动装置的轨迹数据。利用本发明，可以有效减小运动装置运行轨迹推算时的累积误差，提高运动装置运行轨迹推算的准确性。

技术领域

本发明涉及机器人控制技术领域，特别涉及一种运动装置的轨迹数据处理方法及其处理系统。

背景技术

基于激光扫描的自主移动机器人技术已经有越来越成熟的应用。激光扫描所得到的点云是对机器人外部环境轮廓的一种描述，该描述实质上给出了机器人离环境边缘的距离信息。根据这些距离信息，机器可以实现自身位置的确定，也可以实现实时避障的功能。因此充分利用点云的信息是机器人自主移动的一个基本能力。

点云匹配是机器人同步定位与建图(Simultaneously Localization andMapping)的重要组成部分。在相继的两帧点云中，其有极其相似的形状。由于环境轮廓的刚体性质，点云匹配输出的是两帧点云之间的旋转量和平移量。若给定其中一帧点云的位置坐标，则可以计算出另一帧点云的位置。以此类推，通过机器人所搭载激光扫描的点云序列，基于点云匹配算法我们可以得到机器人的运动轨迹。

点云匹配实质上是一个优化问题。一般采用迭代算法求解此类优化问题，此时迭代过程非常强烈依赖初始值，如果初始值给得不好，则迭代算法容易陷入局部最优解，此局部最优解是机器人轨迹推算累积误差的来源。因此如果给出一个比较接近全局最优解的初始值就很重要。

另一方面，在点云匹配中，误差函数是根据对应点对建立起来的，而对应点对是根据最近点规则计算得来的。此时，最近点的搜索将占用绝大部分的计算时间。如何减小最近点搜索范围是点云匹配的一个基本问题。

发明内容

鉴于以上所述现有技术的缺点，本发明的目的在于提供一种运动装置的轨迹数据处理方法及其处理系统，通过合理选择初始变换矩阵，用于解决现有技术中运动装置运行轨迹推算时累计误差较大的技术问题。

为实现上述目的及其他相关目的，本发明提供一种运动装置的轨迹数据处理方法，所述运动装置的轨迹数据处理方法包括：

获取运动装置外部环境轮廓的相继两帧点云，以及所述两帧点云之间编码器的相对平移量和惯性测量单元的角度改变量，其中，分别将所述两帧点云标记为参考帧点云和当前帧点云；

利用所述相对平移量和所述角度改变量构建初始变换矩阵；

利用所述初始变换矩阵对所述当前帧点云进行变换，以获取过渡帧点云；

利用所述参考帧点云和所述过渡帧点云得到对应点对集合；

利用所述对应点对集合得到所述运动装置的轨迹数据。

在一实施例中，所述利用所述对应点对集合得到所述运动装置的轨迹数据的步骤包括：

建立关于所述对应点对集合中所有对应点对误差度量的目标函数；

优化求解所述目标函数，以获得目标变量增量；

根据所述目标变量增量更新所述初始变换矩阵，以获取所述运动装置的轨迹数据。

在一实施例中，所述优化求解所述目标函数，以获得目标变量增量的步骤包括：

关于目标变量线性优化所述目标函数，以得到关于所述目标变量的方程组；

求解所述目标变量的方程组，以获得目标变量增量。