CN110345936A - 运动装置的轨迹数据处理方法及其处理*** - Google Patents

Abstract

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

Description

运动装置的轨迹数据处理方法及其处理***

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

在一实施例中，所述运动装置的轨迹数据处理方法还包括：

判断所述轨迹数据是否满足终止条件的步骤；

若“否”，则将获取的所述轨迹数据作为初始变换矩阵，返回所述利用所述初始变换矩阵对所述当前帧点云进行据变换，以获取过渡帧点云的步骤进行迭代，直到所述轨迹数据满足终止条件，将满足所述终止条件时的所述运动轨迹数据作为最终数据并输出。

在一实施例中，所述将获取的所述轨迹数据作为初始变换矩阵，返回所述利用所述初始变换矩阵对所述当前帧点云进行据变换，以获取过渡帧点云的步骤进行迭代时，在利用所述参考帧点云和所述过渡帧点云得到对应点对集合的步骤中，采用的预设距离阈值比上次的所述预设距离阈值小。

在一实施例中，所述运动装置包括机器人。

在一实施例中，所述利用所述参考帧点云和所述过渡帧点云得到对应点对集合的步骤包括：

以获取所述参考帧点云时所述运动装置的朝向为参考视线，对于所述过渡帧点云中的每一个点，在所述参考视线附近区域搜索所述参考帧点云中与该点对应的点列；

计算每一个点与所述参考帧点云中与该点对应的点列之间的距离，选取距离最小且满足预设距离阈值的两点建立对应点对；

遍历所述过渡帧点云中的所有点，以建立所述对应点对集合。

在一实施例中，所述以获取所述参考帧点云时所述运动装置的朝向为参考视线，对于所述过渡帧点云中的每一个点，在所述参考视线附近区域搜索所述参考帧点云中与该点对应的点列的步骤包括：

以所述参考帧点云所在位置的机器人朝向为参考视线，对于所述过渡帧点云中的每一个点，从所述参考视线开始往两边逐点搜索所述参考帧点云中与该点对应的点列。

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

服务装置；

点云获取装置，设置于运动装置上，所述点云获取装置与所述服务装置连接，所述点云获取装置用于获取所述运动装置的外部环境轮廓点云；

编码器，设置于所述运动装置上，所述编码器与所述服务装置连接，所述编码器用于获取所述运动装置的相对平移量；

惯性测试单元，设置于所述运动装置上，所述惯性测试单元与所述服务装置连接，所述惯性测试单元用于获取所述运动装置的角度改变量；

其中，所述服务装置用于根据获取的所述外部环境轮廓点云、所述相对平移量以及所述角度改变量获取轨迹数据。

为实现上述目的及其他相关目的，本发明还提供一种服务装置，所述服务装置包括：

通信器，用于与外部通信；

存储器，用于存储计算机程序；

处理器，连接所述通信器及存储器，用于运行所述计算机程序以执行上述任意一项的所述运动装置的轨迹数据处理方法。

为实现上述目的及其他相关目的，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序；所述计算机程序运行时执行上述任意一项所述的运动装置的轨迹数据处理方法。

利用本发明的运动装置的轨迹数据处理方法及其处理***，通过将编码器的相对平移量和惯性测量单元的角度改变量作为初始变换矩阵，可以有效减小运动装置运行轨迹推算时的累积误差，提高运动装置运行轨迹推算的准确性，所得到的最终刚体变换(也即轨迹数据)具有非常高的可信度，从而减少了轨迹推算的累积误差，同时也提高了运算速度，对于自主移动机器人的落地应用具有十分重要的意义；

另外，在本发明中，以参考帧点云所在运动装置(例如机器人)朝向为参考视线进行对应点搜索时，可以自主设置搜索的点数范围以及预设距离阈值，并且采用从参考视线开始往两边逐点的搜索策略，如此可以减少搜索计算量，提高了运算速度；

又，本发明在对应点对的集合上选取平方距离函数作为误差函数(目标函数)，该函数具有凸性以及光滑性，能够方便后续线性化逼近；

又，在本发明中，更新初始变换矩阵重新作用在当前帧点云，再进行对应点对的搜索时，采用了减小预设距离阈值的方式，如此可以逐步得到真实的对应点对；

当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。

附图说明

图1显示为本发明的运动装置的轨迹数据处理方法的流程图。

图2显示为图1中步骤S40的子流程图。

图3显示为图1中步骤S50的子流程图。

图4显示为本发明的运动装置的运行轨迹处理***的框图。

图5显示为本发明的运动装置的运行轨迹处理***中服务装置的框图。

图6显示为本发明的运动装置的运行轨迹处理***的工作流程图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

请参阅图1-6。需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

如图1-6所示，本发明的实施例公开一种运动装置的轨迹数据处理方法，所述运动装置的轨迹数据处理方法至少包括以下步骤：步骤S10、获取运动装置外部环境轮廓的相继两帧点云，以及所述两帧点云之间编码器12的相对平移量(也即平移改变量)和惯性测量单元13的角度改变量，其中，分别将所述两帧点云标记为参考帧点云和当前帧点云；步骤S20、将所述平移改变量和所述角度改变量作为初始变换矩阵；步骤S30、利用所述初始变换矩阵对所述当前帧点云进行变换，以获取过渡帧点云；步骤S40、利用所述参考帧点云和所述过渡帧点云得到对应点对集合；步骤S50、利用所述对应点对集合得到所述运动装置的轨迹数据。利用本实施例的运动装置的轨迹数据处理方法，通过合理选择初始刚体变换(初始变换矩阵)，以编码器的相对平移量和惯性测量单元的角度改变量，能够有效减少运动装置运动轨迹推算过程中的累计误差。

需要说明的是，在本实施例中，所述运动装置例如可以包括能够自动执行工作的机器装置，例如机器人，所述机器人身上安装有点云获取装置11、编码器12、惯性测量单元13(Inertial Measurement Unit，IMU)以及处理装置14，所述轨迹数据是指机器人的移动轨迹数据。当然，在其他实施例中，所述运动装置例如也可以是安装有点云获取装置、编码器、惯性测量单元以及处理装置14的电动车等其他运动装置。需要说明的是，在其他实施例中，所述处理装置14也可以是独立于运动装置的装置，也即其可以不安装于运动装置上。

在步骤S10中，所述参考帧点云和所述当前帧点云是通过安装于机器人身上的点云点云获取装置11进行获取的。所述点云获取装置11例如可以是激光扫描装置等能够获取机器人运行时外部环境轮廓的点云(包含许多个点)的设备，激光扫描装置获取的点云实质上给出了机器人离环境边缘的距离信息，根据这些距离信息，机器人可以实现自身位置的确定，也可以实现实时避障的功能，为便于后续说明，将所述参考帧点云记为{P_i}，所述当前帧点云{Q_i}。在一个具体的实施例中，通过激光扫描装置扫描的获取的每帧点云例如可以包括1081个点，每一个点都可用旋转角度和平移量来表示。

在步骤S10中，所述惯性测量单元13用于获取运动装置的角度改变量，是测量物体三轴姿态角(或角速率)以及加速度的装置。一般地，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标***独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。需要说明的是，为了提高可靠性，还可以为每个轴配备更多的传感器。

在步骤S10中，所述编码器12(encoder)是将信号(如比特流)或数据进行编制、转换为可用以通讯、传输和存储的信号形式的设备。所述编码器可以是增量式编码器和绝对式编码器；增量式编码器是将位移转换成周期性的电信号，再把这个电信号转变成计数脉冲，用脉冲的个数表示位移的大小。绝对式编码器的每一个位置对应一个确定的数字码，因此它的示值只与测量的起始和终止位置有关，而与测量的中间过程无关。在本实施例中，所述编码器12例如可以安装于机器人的轮子的驱动电机的输出轴上。

在步骤S20中，由于编码器12和惯性测量单元13输出的是高频数据流，因此在获取两帧点云时已经输出大量编码器平移量和惯性测量单元的角度量，此时需要将这些量进行复合运算以获取两帧点云之间编码器12的相对平移量(x₀，y₀)和惯性测量单元13的角度改变量θ₀，才能用来作为初始刚体变换(也即初始变换矩阵)。

在步骤S30中，将编码器12的相对平移量(x₀，y₀)和惯性测量单元13的角度改变量θ₀作为初始变换矩阵作用于所述当前帧点云，也即利用初始变换矩阵对所述当前帧点云中的每个点进行数据变换，以获取过渡帧点云，以使得所述过渡帧点云充分靠近参考帧。

如图2所示，在步骤S40中，所述利用所述参考帧点云和所述过渡帧点云得到对应点对集合的步骤还至少包括以下步骤：步骤S41、以获取所述参考帧点云时所述运动装置的朝向为参考视线，对于所述过渡帧点云中的每一个点，在所述参考视线附近区域搜索所述参考帧点云中与该点对应的点列；步骤S42、计算每一个点与所述参考帧点云中与该点对应的点列之间的距离，选取距离最小且满足预设距离阈值的两点建立对应点对；步骤S43、遍历所述过渡帧点云中的所有点，以建立所述对应点对集合。

进一步地，在步骤S41中，为了提高搜索效率(计算效率)，在本实施例中，以所述参考帧点云所在位置的机器人朝向为参考视线，对于所述过渡帧点云中的每一个点，从所述参考视线开始往两边逐点搜索所述参考帧点云中与该点对应的点列。

在步骤S41中，以获取所述参考帧点云时所述运动装置的朝向为参考参考视线进行对应点搜索时，需要设置点数范围以及预设距离阈值。

在本实施例中，将获取所述当前帧点云时所述机器人的朝向(当前视线)与所述参考视线之间夹角定义为视线夹角，将激光扫描装置(点云获取装置)两根激光线束之间夹角定义为线束夹度，所述点数范围可以根据所述视线夹角和所述线束夹角来确定。在本实施例中，所述视线夹角的范围例如可以是0-20°。

在一实施例中，例如可以将视线视线夹角除以所述线束夹角的商加一个自然数(例如可以为0、1、2……)的两倍作为所述点数范围；作为示例，所述视线夹角例如是10度，所述线束夹角为0.25，则可以所述点数范围为80加一个自然数，例如所述点数范围取80。在另一实施例中，可以设定一个具体的值作为点数范围，例如可以是50。需要说明的是，在其他实施例中，可以根据实际情况进行点数范围的设定，并不限于上述两种方法。

在本实施例中，所述预设距离阈值是根据两帧点云之间机器人的运行距离进行设置，例如可以根据两帧点云之间机器人的最大平移量进行设置，最大平移量可以根据两帧点云之间的时间间隔和机器人的最大运动速度进行计算获得。需要说明的是，本实施例中，要获取机器人的运行轨迹数据，一般需要进行多次的迭代过程，在第一次利用所述参考帧点云和所述过渡帧点云得到对应点对集合的步骤中，采用两帧点云之间机器人的最大平移量作为预设距离阈值，而后续每进行依次迭代，所采用的预设距离阈值比上次的所述预设距离阈值小，可以理解的是，前后两次预设距离阈值的减少方式可以根据实际情况进行灵活的设置。作为示例，每进行一次迭代，本次所采用的预设距离阈值例如可以采用上次所述预设距离阈值的二分之一，也即采用减半的方式进行预设距离阈值的设定。

作为示例，所述机器人的运行速率例如可以是2m/s，两帧点云之间的时间间隔例如可以是30ms，可以算出机器人的最大平移量是0.06m，也即可以将第一次的预设距离阈值设置为0.06m，后续每次迭代时，距离阈值依次可以设置为0.03m、0.015m、0.075m、……。

在步骤S42和S43中，通过加入预设距离阈值的条件，可以排除一部分不符合要求的对应点对，可以将建立的对应点对集合记为{P'_i，Q'_i}。

如图3所示，在步骤S5中，利用所述对应点对集合得到所述运动装置的轨迹数据的步骤至少包括以下步骤：步骤S51、建立关于所述对应点对集合中所有对应点对误差度量的目标函数；步骤S52、采用优化算法求解所述目标函数，以获得目标变量增量；步骤S53、根据所述目标变量增量更新所述初始变换矩阵，以获取所述运动装置的轨迹数据。

具体地，在步骤S51中，针对步骤S4中获取的对应点对集合{P'_i，Q'_i}给定误差度量，并建立如下的误差函数(目标函数)：

上式表示使arg min后面的表达式达到最小值时R和t的取值，其中，式中R是关于旋转角度θ的旋转矩阵，t是平移向量(x，y)，N是对应点对集合{P'_i，Q'_i}中对应点对的个数。需要说明的是，在对应点对集合{P'_i，Q'_i}上，本实施例选取的是平方距离函数，该函数具有凸性以及光滑性，方便后续步骤S52中的线性优化逼近。可以理解的是，在其他实施例中，也可以选取其他合适形式的函数形式。

具体地，在步骤S52和步骤S53中，关于目标变量线性化上述优化问题中的目标函数，得到如下关于目标变量-旋转角度θ和平移量x，y的线性方程组：

式中，P'_ix和P'_iy分别为对应点对集合{P'_i，Q'_i}中第i个点在平移量x和y方向的平移分量，Q'_ix和Q'_iy分别为对应点对集合{P'_i，Q'_i}中第i个点在平移量x和y方向的平移分量，N是对应点对集合{P'_i，Q'_i}中对应点对的个数。

例如可采用高斯消元法计算可得到目标变量增量(Δx,Δy,Δθ)，此时更新初始变换矩阵为：

(x₁,y₁,θ₁)＝(x₀,y₀,θ₀)+(Δx,Δy,Δθ)

将更新后的初始变换矩阵作为运动装置的轨迹数据。

需要说明的是，一般情况下，只通过执行一次S10-S50的步骤所获取的轨迹数据并不能够满足试验精度的要求，误差较大，为了进一步减少轨迹推算的累积误差，提高运动装置运行轨迹推算的准确性，所述运动装置的轨迹数据处理方法还包括步骤S60、判断所述轨迹数据是否满足终止条件的步骤，若“否”，则将获取的所述轨迹数据作为初始变换矩阵，返回所述利用所述初始变换矩阵对所述当前帧点云进行据变换，以获取过渡帧点云的步骤进行迭代(也即返回步骤S30)，直到所述轨迹数据满足终止条件，将满足所述终止条件时的所述运动轨迹数据作为最终数据并输出。

利用本发明的运动装置的轨迹数据处理方法，通过将编码器12的相对平移量和惯性测量单元13的角度改变量作为初始变换矩阵，可以有效减小运动装置运行轨迹推算时的累积误差，提高运动装置运行轨迹推算的准确性，所得到的最终刚体变换(也即轨迹数据)具有非常高的可信度，从而减少了轨迹推算的累积误差，同时也提高了运算速度，对于自主移动机器人(运动装置)的落地应用具有十分重要的意义

在本实施例中，判断所述运动轨迹数据是否满足终止条件是通过判断所述目标变量增量是否满足终止条件来进行的；若“是”，则将更新后的初始变换矩阵作为刚体变换矩阵输出到服务装置14存储，所述刚体变换矩阵也即两帧点云(参考帧点云和当前帧点云)之间的旋转量和平移量，作为所述轨迹数据；若“否”，则将获取的所述轨迹数据作为初始变换矩阵，返回所述利用所述初始变换矩阵对所述当前帧点云进行据变换，以获取过渡帧点云的步骤(也即返回步骤S30)进行迭代，直到所述目标变量增量满足终止条件，当所述目标变量增量满足所述终止条件时，则以该次获取的所述运动装置的轨迹数据作为最终数据并输出。

在本实施例中，判断所述目标变量增量是否满足终止条件的步骤既可以位于步骤S52和步骤S53之间，也可以位于步骤S53之后。当判断所述目标变量增量是否满足终止条件的步骤位于骤S52和步骤S53之间时，若判定结果为“否”，则执行步骤S53，根据所述目标变量增量更新所述初始变换矩阵，并将更新后的初始变换矩阵作为初始变换矩阵，返回步骤S30继续执行步骤S30以及后续步骤，直到所述目标变量增量满足所述终止条件。当判断所述目标变量增量是否满足终止条件的步骤位于步骤S53之后，若判定结果为“否”，则将更新后的初始变换矩阵作为初始变换矩阵，返回步骤S30继续执行步骤S30以及后续步骤，直到所述目标变量增量满足所述终止条件。

需要说明的是，在步骤S60中，返回所述利用所述初始变换矩阵对所述当前帧点云进行据变换，以获取过渡帧点云的步骤(返回步骤30)进行迭代时，在利用所述参考帧点云和所述过渡帧点云得到对应点对集合的步骤中，采用了减少预设距离阈值的方式得到所述对应点对集合。详见上文相关部分描述，在此不做赘述。

在步骤S60中，所述终止条件例如可以包括满足预设条件或迭代次数超过预设迭代次数。其中，例如可采用目标变量增量是否足够小(小于某一规定数值)或者是否不再改变作为所述预设条件，所述预设迭代次数可以根据需要进行合理设置，作为示例，所述预设迭代次数例如可以为20次，需要说明的是，预设条件和预设迭代次数可以根据实际情况进行合理的设置，不以此处列举为限。

需要说明的是，所述迭代次数的记录可以通过后文提到的服务装置14中的存储器143中存储的计算机程序来实现，也可以通过设定专门的计数器来实现。当采用专门的计数器时，计数器可以直接和服务装置14的处理器141连接，将计数器的迭代次数发送给处理器141，用于判定迭代次数是否超过预设迭代次数，所述计数器也作为运动装置的运行轨迹处理***的一个元件。

如图4-6所示，本发明的实施例还提供一种用于实现上述运动装置的轨迹数据处理***，所述处理***包括服务装置14，以及设置于机器人上的点云点云获取装置11，编码器12以及惯性测试单元14；所述点云获取装置11与所述服务装置14连接，用于获取所述运动装置的外部环境轮廓点云；所述编码器12与所述服务装置14连接，所述编码器12用于获取所述运动装置的相对平移量；所述惯性测试单元14与所述服务装置14连接，所述惯性测试单元14用于获取所述运动装置的角度改变量；其中，所述服务装置14用于根据获取的所述外部环境轮廓点云、所述相对平移量以及所述角度改变量获取轨迹数据。图6示出了本实施例的运动装置的运行轨迹处理***的工作流程图，其具体流程详见上文的相关部分描述，在此不做赘述。

在本实施例中，所述服务装置14包括：通信器142，用于与外部通信；存储器143，用于存储计算机程序；处理器141，连接所述通信器142及存储器143，用于运行所述计算机程序以执行所述运动装置的轨迹数据处理方法。

需要说明的是，上述存储器142中的计算机程序可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，电子设备，或者网络设备等)执行本发明各个实施例方法的全部或部分步骤。所述计算机可读存储介质例如可以包括U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

需要说明的是，通过本实施例的运动装置的轨迹数据处理方法及获取***获取的机器人轨迹数据，若给定其中一帧点云的位置坐标，则可以计算出另一帧点云的位置。以此类推，通过机器人所搭载激光扫描装置获取的点云序列，我们可以得到机器人的运动轨迹。因此，本实施例的运动装置的轨迹数据处理方法及获取***可应用于机器人同步定位与建图(Simultaneously Localization and Mapping)。

需要说明是，本实施例的运动装置的轨迹数据处理方法及获取***，基于短时间内编码器12和惯性测量单元13给出的相对改变量较为准确的事实，将此改变量作为点云匹配(帧点云数据处理)的初始变换矩阵，如此可以避免优化问题陷入局部最优解，再进行迭代式求解优化问题并更新初始刚体变换；在每一次迭代过程中通过参考帧点云所在机器人朝向作为参考视线来搜索对应点对，如此可以减少搜索计算量；如此所得到的最终刚体变换(也即轨迹数据)具有非常高的可信度，从而减少了轨迹推算的累积误差，同时也提高了运算速度，对于自主移动机器人的落地应用具有十分重要的意义。

在本文的描述中，提供了许多特定细节，诸如部件和/或方法的实例，以提供对本发明实施例的完全理解。然而，本领域技术人员将认识到可以在没有一项或多项具体细节的情况下或通过其他设备、***、组件、方法、部件、材料、零件等等来实践本发明的实施例。在其他情况下，未具体示出或详细描述公知的结构、材料或操作，以避免使本发明实施例的方面变模糊。

在整篇说明书中提到“一个实施例(one embodiment)”、“实施例(anembodiment)”或“具体实施例(a specific embodiment)”意指与结合实施例描述的特定特征、结构或特性包括在本发明的至少一个实施例中，并且不一定在所有实施例中。因而，在整篇说明书中不同地方的短语“在一个实施例中(in one embodiment)”、“在实施例中(inan embodiment)”或“在具体实施例中(in a specific embodiment)”的各个表象不一定是指相同的实施例。此外，本发明的任何具体实施例的特定特征、结构或特性可以按任何合适的方式与一个或多个其他实施例结合。应当理解本文所述和所示的发明实施例的其他变型和修改可能是根据本文教导的，并将被视作本发明精神和范围的一部分。

还应当理解还可以以更分离或更整合的方式实施附图所示元件中的一个或多个，或者甚至因为在某些情况下不能操作而被移除或因为可以根据特定应用是有用的而被提供。

另外，除非另外明确指明，附图中的任何标志箭头应当仅被视为示例性的，而并非限制。此外，除非另外指明，本文所用的术语“或”一般意在表示“和/或”。在术语因提供分离或组合能力是不清楚的而被预见的情况下，部件或步骤的组合也将视为已被指明。

如在本文的描述和在下面整篇权利要求书中所用，除非另外指明，“一个(a)”、“一个(an)”和“该(the)”包括复数参考物。同样，如在本文的描述和在下面整篇权利要求书中所用，除非另外指明，“在…中(in)”的意思包括“在…中(in)”和“在…上(on)”。

本发明所示实施例的上述描述(包括在说明书摘要中所述的内容)并非意在详尽列举或将本发明限制到本文所公开的精确形式。尽管在本文仅为说明的目的而描述了本发明的具体实施例和本发明的实例，但是正如本领域技术人员将认识和理解的，各种等效修改是可以在本发明的精神和范围内的。如所指出的，可以按照本发明所述实施例的上述描述来对本发明进行这些修改，并且这些修改将在本发明的精神和范围内。

本文已经在总体上将***和方法描述为有助于理解本发明的细节。此外，已经给出了各种具体细节以提供本发明实施例的总体理解。然而，相关领域的技术人员将会认识到，本发明的实施例可以在没有一个或多个具体细节的情况下进行实践，或者利用其它装置、***、配件、方法、组件、材料、部分等进行实践。在其它情况下，并未特别示出或详细描述公知结构、材料和/或操作以避免对本发明实施例的各方面造成混淆。

因而，尽管本发明在本文已参照其具体实施例进行描述，但是修改自由、各种改变和替换意在上述公开内，并且应当理解，在某些情况下，在未背离所提出发明的范围和精神的前提下，在没有对应使用其他特征的情况下将采用本发明的一些特征。因此，可以进行许多修改，以使特定环境或材料适应本发明的实质范围和精神。本发明并非意在限制到在下面权利要求书中使用的特定术语和/或作为设想用以执行本发明的最佳方式公开的具体实施例，但是本发明将包括落入所附权利要求书范围内的任何和所有实施例及等同物。因而，本发明的范围将只由所附的权利要求书进行确定。

Claims (10)

1.一种运动装置的轨迹数据处理方法，其特征在于，包括：

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

通过所述平移改变量和所述角度改变量构建初始变换矩阵；

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

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

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

2.根据权利要求1所述的运动装置的轨迹数据处理方法，其特征在于，所述利用所述对应点对集合得到所述运动装置的轨迹数据的步骤包括：

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

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

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

3.根据权利要求2所述的运动装置的轨迹数据处理方法，其特征在于，所述采用优化算法求解所述目标函数，以获得目标变量增量的步骤包括：

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

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

4.根据权利要求2所述的运动装置的轨迹数据处理方法，其特征在于，所述获取方法还包括：

判断所述轨迹数据是否满足终止条件的步骤；

若“否”，则将获取的所述轨迹数据作为初始变换矩阵，返回所述利用所述初始变换矩阵对所述当前帧点云进行据变换，以获取过渡帧点云的步骤进行迭代，直到所述轨迹数据满足终止条件，将满足所述终止条件时的所述运动轨迹数据作为最终数据并输出。

5.根据权利要求4所述的运动装置的轨迹数据处理方法，其特征在于，所述将获取的所述轨迹数据作为初始变换矩阵，返回所述利用所述初始变换矩阵对所述当前帧点云的每个点进行数据变换，以获取过渡帧点云的步骤进行迭代时，在利用所述参考帧点云和所述过渡帧点云得到对应点对集合的步骤中，采用的预设距离阈值比上次的所述预设距离阈值小。

6.根据权利要求1所述的运动装置的轨迹数据处理方法，其特征在于，所述运动装置包括机器人。

7.根据权利要求1-6任意一项所述的运动装置的轨迹数据处理方法，其特征在于，所述利用所述参考帧点云和所述过渡帧点云得到对应点对集合的步骤包括：

以获取所述参考帧点云时所述运动装置的朝向为参考视线，对于所述过渡帧点云中的每一个点，在所述参考视线附近区域搜索所述参考帧点云中与该点对应的点列；

计算每一个点与所述参考帧点云中与该点对应的点列之间的距离，选取距离最小且满足预设距离阈值的两点建立对应点对；

遍历所述过渡帧点云中的所有点，以建立所述对应点对集合。

8.根据权利要求7所述的运动装置的轨迹数据处理方法，其特征在于，所述以获取所述参考帧点云时所述运动装置的朝向为参考视线，对于所述过渡帧点云中的每一个点，在所述参考视线附近区域搜索所述参考帧点云中与该点对应的点列的步骤包括：

以所述参考帧点云所在位置的机器人朝向为参考视线，对于所述过渡帧点云中的每一个点，从所述参考视线开始往两边逐点搜索所述参考帧点云中与该点对应的点列。

9.一种运动装置的运行轨迹处理***，其特征在于，包括：

服务装置；

点云获取装置，设置于运动装置上，所述点云获取装置与所述服务装置连接，所述点云获取装置用于获取所述运动装置的外部环境轮廓点云；

编码器，设置于所述运动装置上，所述编码器与所述服务装置连接，所述编码器用于获取所述运动装置的相对平移量；

惯性测试单元，设置于所述运动装置上，所述惯性测试单元与所述服务装置连接，所述惯性测试单元用于获取所述运动装置的角度改变量；

其中，所述服务装置用于根据获取的所述外部环境轮廓点云、所述相对平移量以及所述角度改变量获取轨迹数据。

10.一种服务装置，其特征在于，包括：

通信器，用于与外部通信；

存储器，用于存储计算机程序；

处理器，连接所述通信器及存储器，用于运行所述计算机程序以执行如权利要求1至8中任意一项所述的运动装置的轨迹数据处理方法。