一种机器人定位方法和系统技术方案

本发明专利技术涉及一种机器人定位方法和系统，涉及距离测量领域。包括以下步骤：S1：获取待测物体在当前测量间隔内包括时间戳的激光数据和n组包括时间戳的初始IMU实时数据，n＝1，2，3…；S2：分别将每组所述初始IMU实时数据减去预设值得到n组IMU实时数据并保存；S3：将所述激光数据的时间戳映射到每组IMU数据的时间戳的线性时间轴上；S4：按照线性关系计算所述激光数据时间戳投影点对应的IMU数据，并作为最终IMU数据；S5：根据所述最终IMU数据与上一测量间隔内计算得到的最终IMU数据计算得到所述待测物体的底盘姿态变化矩阵。本方案解决了机器人定位时由于无法处理大角度旋转激光匹配的问题导致定位丢失的技术问题，适用于机器人定位。

【技术实现步骤摘要】
一种机器人定位方法和系统
本专利技术涉及距离测量领域，特别涉及一种机器人定位方法和系统。
技术介绍
现有的机器人定位时，通常使用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对应的朝向角度值，a本文档来自技高网...

【技术保护点】
1.一种机器人定位方法，其特征在于，包括以下步骤：S1：获取待测物体在当前测量间隔内包括时间戳的激光数据和n组包括时间戳的初始IMU实时数据，n＝1，2，3…；S2：分别将每组所述初始IMU实时数据减去预设值得到n组IMU实时数据并保存；S3：将所述激光数据的时间戳映射到每组IMU数据的时间戳的线性时间轴上；S4：按照线性关系计算所述激光数据时间戳投影点对应的IMU数据，并作为最终IMU数据；S5：根据所述最终IMU数据与上一测量间隔内计算得到的最终IMU数据计算得到所述待测物体的底盘姿态变化矩阵；S6：根据所述底盘姿态变化矩阵得到激光坐标系姿态变化矩阵；S7：根据所述激光坐标系姿态变化矩阵和激光匹配算法得到定位结果。

【技术特征摘要】
1.一种机器人定位方法，其特征在于，包括以下步骤：S1：获取待测物体在当前测量间隔内包括时间戳的激光数据和n组包括时间戳的初始IMU实时数据，n＝1，2，3…；S2：分别将每组所述初始IMU实时数据减去预设值得到n组IMU实时数据并保存；S3：将所述激光数据的时间戳映射到每组IMU数据的时间戳的线性时间轴上；S4：按照线性关系计算所述激光数据时间戳投影点对应的IMU数据，并作为最终IMU数据；S5：根据所述最终IMU数据与上一测量间隔内计算得到的最终IMU数据计算得到所述待测物体的底盘姿态变化矩阵；S6：根据所述底盘姿态变化矩阵得到激光坐标系姿态变化矩阵；S7：根据所述激光坐标系姿态变化矩阵和激光匹配算法得到定位结果。2.根据权利要求1所述的机器人定位方法，其特征在于：步骤S1具体为：获取待测物体在当前测量间隔内包括时间戳的激光数据和3组包括时间戳的初始IMU实时数据。3.根据权利要求1所述的机器人定位方法，其特征在于：步骤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数据。4.根据权利要求1所述的机器人定位方法，其特征在于：步骤S5中，具体包括：如果所述最终IMU数据与上一组IMU实时数据的差值Δangle大于等于π，则Δangle＝Δangle-sign(Δangle)*2π，然后将Δangle转换为四元数形式，然后将四元数形式的Δangle转换为tf矩阵ΔTFbase。5.根据权利要求1所述的机器人定位方法，其特征在于：步骤S6中的激光坐标系姿态变化矩阵根据以下公式计算获得：ΔTFlaser＝TFlaser->base*TFbase->world*ΔTFbase*TFworld->base*TFbase->laser；其中TFlaser->base为机器人底盘坐标系相对于激光坐标系的相对位姿，TFbase->world为世界坐标系相对于机器人底盘坐标系的相对位姿，TFworld->base为机器人底盘坐标系相对于世界坐标系的相对位姿，TFbase->laser为激光坐标系相对于机器人底盘坐标系的相对位姿。6...

【专利技术属性】
技术研发人员：董又维，徐江伟，李剑平，王浩然，李伟，
申请(专利权)人：北京兆维电子集团有限责任公司，
类型：发明
国别省市：北京,11