一种机器人定位方法和系统与流程

本发明涉及距离测量领域，特别涉及一种机器人定位方法和系统。

背景技术：

现有的机器人定位时，通常使用icp算法作为激光匹配算法，由于激光传感器自身的离散测量特性，导致icp算法的收敛速度和精度无法得到保证。因此基于该算法，延伸出了多种改进方法，比如变种pl-icp(pointtolineicp)等扫描匹配算法在实际中也获得广泛应用，它具有二阶收敛性，且可在有限步数中收敛，但使用这种算法的机器人在使用过程中由于无法处理大角度旋转激光匹配的问题导致出现定位丢失的问题。

技术实现要素：

本发明所要解决的技术问题是如何解决机器人定位时由于无法处理大角度旋转激光匹配导致出现定位丢失的问题。

本发明解决上述技术问题的技术方案如下：一种机器人定位方法，包括以下步骤：

s1：获取待测物体在当前测量间隔内包括时间戳的激光数据和n组包括时间戳的初始imu实时数据，n＝1，2，3…；

s2：分别将每组所述初始imu实时数据减去预设值得到n组imu实时数据并保存；

s3：将所述激光数据的时间戳映射到每组imu数据的时间戳的线性时间轴上；

s4：按照线性关系计算所述激光数据时间戳投影点对应的imu数据，并作为最终imu数据；

s5：根据所述最终imu数据与上一测量间隔内计算得到的最终imu数据计算得到所述待测物体的底盘姿态变化矩阵；

s6：根据所述底盘姿态变化矩阵得到激光坐标系姿态变化矩阵；

s7：根据所述激光坐标系姿态变化矩阵和激光匹配算法得到定位结果。

本方案首先获取激光数据和初始imu实时数据，imu实时数据相比初始imu实时数据减去了预设值，避免初始imu实时数据的误差影响计算结果，通过选择将激光数据的时间戳映射到每组imu数据的时间戳上，获取与所述激光数据时间最接近的最终imu数据，再得到激光坐标系姿态变化矩阵配合激光匹配算法得到定位结果。

本发明的有益效果是：通过imu数据，解决了由于没有准确的朝向角度数据导致机器人大角度旋转时定位丢失的技术问题。

在上述技术方案的基础上，本发明还可以做如下改进。

进一步，步骤s1具体为：获取待测物体在当前测量间隔内包括时间戳的激光数据和3组包括时间戳的初始imu实时数据。

采用上述进一步方案的有益效果是，相比多于3组的初始imu实时数据，3组即可实现目的，这样设置更加节约资源。

进一步，步骤s4中，根据以下公式得到最终imu数据：

当timestamp_scan<＝timestamp_imu_2时，curr_angle＝angle_1+(angle_2–angle_1)*(timestamp_scan-timestamp_imu_1)/(timestamp_imu_2-timestamp_imu_1)；

当timestamp_scan>timestamp_imu_2时，curr_angle＝angle_2+(angle_3–angle_2)*(timestamp_scan-timestamp_imu_2)/(timestamp_imu_3-timestamp_imu_2)；

其中，timestamp_imu_1、timestamp_imu_2、timestamp_imu_3依次代表3个按时间先后排序的imu实时数据，所述imu实时数据包括朝向角度值，angle_1为timestamp_imu_1对应的朝向角度值，angle_2为timestamp_imu_2对应的朝向角度值，angle_3为timestamp_imu_3对应的朝向角度值，timestamp_scan为所述激光数据时间戳，curr_angle为最终imu数据的角度值，然后将curr_angle转换为四元数形式保存在imu数据中得到最终imu数据。

采用上述进一步方案的有益效果是，选择出更接近激光数据的时间戳的imu实时数据。

进一步，步骤s5中，具体包括：如果所述最终imu数据与上一组imu实时数据的差值δangle大于等于π，则δangle＝δangle-sign(δangle)*2π，然后将δangle转换为四元数形式，然后将四元数形式的δangle转换为tf矩阵δtfbase。

采用上述进一步方案的有益效果是，得到底盘姿态变化矩阵。

进一步，步骤s6中的激光坐标系姿态变化矩阵根据以下公式计算获得：δtflaser＝tflaser->base*tfbase->world*δtfbase*tfworld->base*tfbase->laser；其中tflaser->base为机器人底盘坐标系相对于激光坐标系的相对位姿，tfbase->world为世界坐标系相对于机器人底盘坐标系的相对位姿，tfworld->base为机器人底盘坐标系相对于世界坐标系的相对位姿，tfbase->laser为激光坐标系相对于机器人底盘坐标系的相对位姿。

采用上述进一步方案的有益效果是，得到激光坐标系姿态变化矩阵。

进一步，一种机器人定位系统，包括激光测距模块、惯导模块、处理模块和输出模块；所述激光测距模块用于获取待测物体在当前测量间隔内包括时间戳的激光数据；所述惯导模块用于获取n组包括时间戳的初始imu实时数据，n＝1，2，3…；所述处理模块用于分别将每组所述初始imu实时数据减去预设值得到n组imu实时数据并保存，然后将所述激光数据的时间戳映射到每组imu数据的时间戳的线性时间轴上，选择与所述激光数据时间最接近的imu实时数据作为最终imu数据，根据所述最终imu数据与上一测量间隔内计算得到的最终imu数据计算得到所述待测物体的底盘姿态变化矩阵，再根据所述底盘姿态变化矩阵得到激光坐标系姿态变化矩阵，最后根据所述激光坐标系姿态变化矩阵和激光匹配算法得到定位结果。

采用上述进一步方案的有益效果是，通过imu数据，解决了由于没有准确的朝向角度数据导致机器人大角度旋转时定位丢失的技术问题。

进一步，所述惯导模块具体用于获取待测物体在当前测量间隔内包括时间戳的激光数据和3组包括时间戳的初始imu实时数据。

采用上述进一步方案的有益效果是，相比多于3组的初始imu实时数据，3组即可实现目的，这样设置更加节约资源。

进一步，根据以下公式得到最终imu数据：

当timestamp_scan<＝timestamp_imu_2时，curr_angle＝angle_1+(angle_2–angle_1)*(timestamp_scan-timestamp_imu_1)/(timestamp_imu_2-timestamp_imu_1)；

当timestamp_scan>timestamp_imu_2时，curr_angle＝angle_2+(angle_3–angle_2)*(timestamp_scan-timestamp_imu_2)/(timestamp_imu_3-timestamp_imu_2)；

其中，timestamp_imu_1、timestamp_imu_2、timestamp_imu_3依次代表3个按时间先后排序的imu实时数据，所述imu实时数据包括朝向角度值，angle_1为timestamp_imu_1对应的朝向角度值，angle_2为timestamp_imu_2对应的朝向角度值，angle_3为timestamp_imu_3对应的朝向角度值，timestamp_scan为所述激光数据时间戳，curr_angle为最终imu数据的角度值，然后将curr_angle转换为四元数形式保存在imu数据中得到最终imu数据。

采用上述进一步方案的有益效果是，选择出更接近激光数据的时间戳的imu实时数据。

进一步，如果所述最终imu数据与上一组imu实时数据的差值δangle大于等于π，则δangle＝δangle-sign(δangle)*2π，然后将δangle转换为四元数形式，然后将四元数形式的δangle转换为tf矩阵δtfbase。

采用上述进一步方案的有益效果是，得到底盘姿态变化矩阵。

进一步，所述激光坐标系姿态变化矩阵根据以下公式计算获得：δtflaser＝tflaser->base*tfbase->world*δtfbase*tfworld->base*tfbase->laser；其中tflaser->base为机器人底盘坐标系相对于激光坐标系的相对位姿，tfbase->world为世界坐标系相对于机器人底盘坐标系的相对位姿，tfworld->base为机器人底盘坐标系相对于世界坐标系的相对位姿，tfbase->laser为激光坐标系相对于机器人底盘坐标系的相对位姿。

采用上述进一步方案的有益效果是，得到激光坐标系姿态变化矩阵。

附图说明

图1为本发明机器人定位方法的实施例的方法示意图；

图2为本发明机器人定位方法的其它实施例的imu数据预测示意图；

图3为本发明机器人定位方法的其它实施例的系统结构示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

实施例基本如附图1和附图2所示：

本实施例中机器人定位方法，包括以下步骤：

s1：获取待测物体在当前测量间隔内包括时间戳的激光数据5和n组包括时间戳的初始imu实时数据6，本实施例中的n为3；

s2：分别将每组所述初始imu实时数据6减去预设值得到n组imu实时数据7并保存，本实施例中的预设值为根据实验得到的imu数据的延迟值，本实施例中的预设值为10ms；

s3：将所述激光数据5的时间戳映射到每组imu数据的时间戳的线性时间轴上；

s4：按照线性关系计算所述激光数据时间戳投影点对应的imu数据，并作为最终imu数据；

s5：根据所述最终imu数据7与上一组imu实时数据得到底盘角度变化量δangle，使用四元数表示为[qx,qy,qz,qw],然后得到待测物体的底盘姿态变化矩阵δtfbase；

s6：根据所述底盘姿态变化矩阵得到激光坐标系姿态变化矩阵δtflaser:δtflaser＝tflaser->base*tfbase->world*δtfbase*tfworld->base*tfbase->laser；

s7：根据所述激光坐标系姿态变化矩阵和激光匹配算法得到定位结果。

本方案首先获取激光数据5和初始imu实时数据6，imu实时数据7相比初始imu实时数据6减去了预设值，避免初始imu实时数据6的误差影响计算结果，通过选择将激光数据5的时间戳映射到每组imu数据的时间戳上，获取与所述激光数据5时间最接近的最终imu数据，再得到激光坐标系姿态变化矩阵配合激光匹配算法得到定位结果。

本发明的有益效果是：通过imu数据，解决了由于没有准确的朝向角度数据导致机器人大角度旋转时定位丢失的技术问题。

在上述技术方案的基础上，本发明还可以做如下改进。

可选的，在一些其它实施例中，步骤s1具体为：获取待测物体在当前测量间隔内包括时间戳的激光数据5和3组包括时间戳的初始imu实时数据6。

相比多于3组的初始imu实时数据6，3组即可实现目的，这样设置更加节约资源。

可选的，在一些其它实施例中，步骤s4中，根据以下公式得到最终imu数据：

当timestamp_scan<＝timestamp_imu_2时，curr_angle＝angle_1+(angle_2–angle_1)*(timestamp_scan-timestamp_imu_1)/(timestamp_imu_2-timestamp_imu_1)；

当timestamp_scan>timestamp_imu_2时，curr_angle＝angle_2+(angle_3–angle_2)*(timestamp_scan-timestamp_imu_2)/(timestamp_imu_3-timestamp_imu_2)；

其中，timestamp_imu_1、timestamp_imu_2、timestamp_imu_3依次代表3个按时间先后排序的imu实时数据7，所述imu实时数据7包括朝向角度值，angle_1为timestamp_imu_1对应的朝向角度值，angle_2为timestamp_imu_2对应的朝向角度值，angle_3为timestamp_imu_3对应的朝向角度值，timestamp_scan为所述激光数据5时间戳，curr_angle为最终imu数据的角度值，然后将curr_angle转换为四元数形式保存在imu数据中得到最终imu数据。

通过上述方案能够选择出更接近激光数据5的时间戳的imu实时数据7。

可选的，在一些其它实施例中，步骤s5中，具体包括：如果最终imu数据7与上一组imu实时数据的差值δangle大于等于π，则δangle＝δangle-sign(δangle)*2π，然后将δangle转换为四元数形式，然后将四元数形式的δangle转换为tf矩阵δtfbase。

通过上述方案能够得到底盘姿态变化矩阵。

可选的，在一些其它实施例中，步骤s6中的激光坐标系姿态变化矩阵根据以下公式计算获得：δtflaser＝tflaser->base*tfbase->world*δtfbase*tfworld->base*tfbase->laser；其中tflaser->base为机器人底盘坐标系相对于激光坐标系的相对位姿，tfbase->world为世界坐标系相对于机器人底盘坐标系的相对位姿，tfworld->base为机器人底盘坐标系相对于世界坐标系的相对位姿，tfbase->laser为激光坐标系相对于机器人底盘坐标系的相对位姿。tflaser->base与tfbase->laser互逆，tfbase->world与tfworld->base互逆。所有相对位姿包含平移向量和旋转向量，分别可表示为：

平移分量：[δx,δy,δz]

旋转分量：使用四元数表示为[qx,qy,qz,qw]。

这两个分量根据底盘结构、传感器测量、定位算法计算得出。

通过上述方案能够得到激光坐标系姿态变化矩阵。

可选的，在一些其它实施例中，如附图3所示，一种机器人定位系统，包括激光测距模块1、惯导模块2、处理模块3和输出模块4；所述激光测距模块1用于获取待测物体在当前测量间隔内包括时间戳的激光数据5；所述惯导模块2用于获取n组包括时间戳的初始imu实时数据6，n＝1，2，3…；所述处理模块3用于分别将每组所述初始imu实时数据6减去预设值得到n组imu实时数据7并保存，然后将所述激光数据5的时间戳映射到每组imu数据的时间戳的线性时间轴上，选择与所述激光数据5时间最接近的imu实时数据7作为最终imu数据，根据所述最终imu数据与上一测量间隔内计算得到的最终imu数据计算得到待测物体的底盘姿态变化矩阵，再根据所述底盘姿态变化矩阵得到激光坐标系姿态变化矩阵，最后根据所述激光坐标系姿态变化矩阵和激光匹配算法得到定位结果。

通过imu数据，解决了由于没有准确的朝向角度数据导致机器人大角度旋转时定位丢失的技术问题。

可选的，在一些其它实施例中，惯导模块2具体用于获取待测物体在当前测量间隔内包括时间戳的激光数据和3组包括时间戳的初始imu实时数据。

相比多于3组的初始imu实时数据6，3组即可实现目的，这样设置更加节约资源。

可选的，在一些其它实施例中，根据以下公式得到最终imu数据：

当timestamp_scan<＝timestamp_imu_2时，curr_angle＝angle_1+(angle_2–angle_1)*(timestamp_scan-timestamp_imu_1)/(timestamp_imu_2-timestamp_imu_1)；

当timestamp_scan>timestamp_imu_2时，curr_angle＝angle_2+(angle_3–angle_2)*(timestamp_scan-timestamp_imu_2)/(timestamp_imu_3-timestamp_imu_2)；

其中，timestamp_imu_1、timestamp_imu_2、timestamp_imu_3依次代表3个按时间先后排序的imu实时数据7，所述imu实时数据7包括朝向角度值，angle_1为timestamp_imu_1对应的朝向角度值，angle_2为timestamp_imu_2对应的朝向角度值，angle_3为timestamp_imu_3对应的朝向角度值，timestamp_scan为所述激光数据5时间戳，curr_angle为最终imu数据的角度值，然后将curr_angle转换为四元数形式保存在imu数据中得到最终imu数据。

通过上述方案能够选择出更接近激光数据5的时间戳的imu实时数据7。

可选的，在一些其它实施例中，如果最终imu数据与上一组imu实时数据7的差值δangle大于等于π，则δangle＝δangle-sign(δangle)*2π，然后将δangle转换为四元数形式，然后将四元数形式的δangle转换为tf矩阵δtfbase。

通过上述方案能够得到底盘姿态变化矩阵。

可选的，在一些其它实施例中，激光坐标系姿态变化矩阵根据以下公式计算获得：δtflaser＝tflaser->base*tfbase->world*δtfbase*tfworld->base*tfbase->laser；其中tflaser->base为，tfbase->world为，tfworld->base为，tfbase->laser为。

通过上述方案能够得到激光坐标系姿态变化矩阵。

读者应理解，在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

以上，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。