CN109405826A - 一种机器人定位方法和系统 - Google Patents
一种机器人定位方法和系统 Download PDFInfo
- Publication number
- CN109405826A CN109405826A CN201811366331.1A CN201811366331A CN109405826A CN 109405826 A CN109405826 A CN 109405826A CN 201811366331 A CN201811366331 A CN 201811366331A CN 109405826 A CN109405826 A CN 109405826A
- Authority
- CN
- China
- Prior art keywords
- imu
- timestamp
- angle
- data
- laser
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
本发明涉及一种机器人定位方法和系统，涉及距离测量领域。包括以下步骤：S1：获取待测物体在当前测量间隔内包括时间戳的激光数据和n组包括时间戳的初始IMU实时数据，n＝1，2，3…；S2：分别将每组所述初始IMU实时数据减去预设值得到n组IMU实时数据并保存；S3：将所述激光数据的时间戳映射到每组IMU数据的时间戳的线性时间轴上；S4：按照线性关系计算所述激光数据时间戳投影点对应的IMU数据，并作为最终IMU数据；S5：根据所述最终IMU数据与上一测量间隔内计算得到的最终IMU数据计算得到所述待测物体的底盘姿态变化矩阵。本方案解决了机器人定位时由于无法处理大角度旋转激光匹配的问题导致定位丢失的技术问题，适用于机器人定位。
Description
技术领域
本发明涉及距离测量领域，特别涉及一种机器人定位方法和系统。
背景技术
现有的机器人定位时，通常使用ICP算法作为激光匹配算法，由于激光传感器自身的离散测量特性，导致ICP算法的收敛速度和精度无法得到保证。因此基于该算法，延伸出了多种改进方法，比如变种PL-ICP(Point to Line ICP)等扫描匹配算法在实际中也获得广泛应用，它具有二阶收敛性，且可在有限步数中收敛，但使用这种算法的机器人在使用过程中由于无法处理大角度旋转激光匹配的问题导致出现定位丢失的问题。
发明内容
本发明所要解决的技术问题是如何解决机器人定位时由于无法处理大角度旋转激光匹配导致出现定位丢失的问题。
本发明解决上述技术问题的技术方案如下：一种机器人定位方法，包括以下步骤：
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为。
通过上述方案能够得到激光坐标系姿态变化矩阵。
读者应理解，在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
以上，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。
Claims (10)
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.一种机器人定位系统，其特征在于：包括激光测距模块、惯导模块、处理模块和输出模块；所述激光测距模块用于获取待测物体在当前测量间隔内包括时间戳的激光数据；所述惯导模块用于获取n组包括时间戳的初始IMU实时数据，n＝1，2，3…；所述处理模块用于分别将每组所述初始IMU实时数据减去预设值得到n组IMU实时数据并保存，然后将所述激光数据的时间戳映射到每组IMU数据的时间戳的线性时间轴上，选择与所述激光数据时间最接近的IMU实时数据作为最终IMU数据，根据所述最终IMU数据与上一测量间隔内计算得到的最终IMU数据计算得到所述待测物体的底盘姿态变化矩阵，再根据所述底盘姿态变化矩阵得到激光坐标系姿态变化矩阵，最后根据所述激光坐标系姿态变化矩阵和激光匹配算法得到定位结果。
7.根据权利要求6所述的机器人定位系统，其特征在于：所述惯导模块具体用于获取待测物体在当前测量间隔内包括时间戳的激光数据和3组包括时间戳的初始IMU实时数据。
8.根据权利要求6所述的机器人定位系统，其特征在于：根据以下公式得到最终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数据。
9.根据权利要求6所述的机器人定位系统，其特征在于：如果所述最终IMU数据与上一组IMU实时数据的差值Δangle大于等于π，则Δangle＝Δangle-sign(Δangle)*2π，然后将Δangle转换为四元数形式，然后将四元数形式的Δangle转换为tf矩阵ΔTFbase。
10.根据权利要求6所述的机器人定位系统，其特征在于：所述激光坐标系姿态变化矩阵根据以下公式计算获得：ΔTFlaser＝TFlaser->base*TFbase->world*ΔTFbase*TFworld->base*TFbase->laser；其中TFlaser->base为机器人底盘坐标系相对于激光坐标系的相对位姿，TFbase->world为世界坐标系相对于机器人底盘坐标系的相对位姿，TFworld->base为机器人底盘坐标系相对于世界坐标系的相对位姿，TFbase->laser为激光坐标系相对于机器人底盘坐标系的相对位姿。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811366331.1A CN109405826B (zh) | 2018-11-16 | 2018-11-16 | 一种机器人定位方法和系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811366331.1A CN109405826B (zh) | 2018-11-16 | 2018-11-16 | 一种机器人定位方法和系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109405826A true CN109405826A (zh) | 2019-03-01 |
| CN109405826B CN109405826B (zh) | 2020-12-29 |
Family
ID=65473590
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811366331.1A Active CN109405826B (zh) | 2018-11-16 | 2018-11-16 | 一种机器人定位方法和系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109405826B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2002031528A (ja) * | 2000-07-14 | 2002-01-31 | Asia Air Survey Co Ltd | モービルマッピング用空間情報生成装置 |
| CN104180793A (zh) * | 2014-08-27 | 2014-12-03 | 北京建筑大学 | 一种用于数字城市建设的移动空间信息获取装置和方法 |
| CN105157687A (zh) * | 2015-09-08 | 2015-12-16 | 北京控制工程研究所 | 一种基于wMPS的动态物体的位置姿态测量方法 |
| CN106643601A (zh) * | 2016-12-13 | 2017-05-10 | 杭州亿恒科技有限公司 | 工业机器人动态六维参数测量方法 |
| CN107688184A (zh) * | 2017-07-24 | 2018-02-13 | 宗晖（上海）机器人有限公司 | 一种定位方法以及系统 |
| CN107782240A (zh) * | 2017-09-27 | 2018-03-09 | 首都师范大学 | 一种二维激光扫描仪标定方法、系统及装置 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106225790B (zh) * | 2016-07-13 | 2018-11-02 | 百度在线网络技术（北京）有限公司 | 一种无人车定位精度的确定方法及装置 |
-
2018
- 2018-11-16 CN CN201811366331.1A patent/CN109405826B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2002031528A (ja) * | 2000-07-14 | 2002-01-31 | Asia Air Survey Co Ltd | モービルマッピング用空間情報生成装置 |
| CN104180793A (zh) * | 2014-08-27 | 2014-12-03 | 北京建筑大学 | 一种用于数字城市建设的移动空间信息获取装置和方法 |
| CN105157687A (zh) * | 2015-09-08 | 2015-12-16 | 北京控制工程研究所 | 一种基于wMPS的动态物体的位置姿态测量方法 |
| CN106643601A (zh) * | 2016-12-13 | 2017-05-10 | 杭州亿恒科技有限公司 | 工业机器人动态六维参数测量方法 |
| CN107688184A (zh) * | 2017-07-24 | 2018-02-13 | 宗晖（上海）机器人有限公司 | 一种定位方法以及系统 |
| CN107782240A (zh) * | 2017-09-27 | 2018-03-09 | 首都师范大学 | 一种二维激光扫描仪标定方法、系统及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109405826B (zh) | 2020-12-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20200065995A1 (en) | System and method for tying together machine vision coordinate spaces in a guided assembly environment | |
| Yakimovsky et al. | A system for extracting three-dimensional measurements from a stereo pair of TV cameras | |
| CN105858188B (zh) | 具备三维传感器的搬运机器人系统 | |
| CN104842352B (zh) | 利用了视觉反馈的机器人系统 | |
| US9233469B2 (en) | Robotic system with 3D box location functionality | |
| KR102056664B1 (ko) | 센서를 이용한 작업 방법 및 이를 수행하는 작업 시스템 | |
| CN111347426B (zh) | 一种基于3d视觉的机械臂精确放置轨迹规划方法 | |
| US20220230348A1 (en) | Method and apparatus for determining a three-dimensional position and pose of a fiducial marker | |
| CN112720487B (zh) | 基于自适应动态力平衡的机械臂抓取方法和系统 | |
| US11074448B2 (en) | Method for 3D mapping of 2D point of interest | |
| CN106323286B (zh) | 一种机器人坐标系与三维测量坐标系的变换方法 | |
| CN114310901B (zh) | 用于机器人的坐标系标定方法、装置、系统以及介质 | |
| CN109540140A (zh) | 一种融合ssd目标识别和里程计信息的移动机器人定位方法 | |
| CN111982058A (zh) | 一种基于双目相机的测距方法、系统、设备和可读存储介质 | |
| CN113733100B (zh) | 巡检操作机器人的目标定位方法、装置、设备及存储介质 | |
| CN115439633A (zh) | 标定方法、装置和电子设备 | |
| CN109405826A (zh) | 一种机器人定位方法和系统 | |
| CN110142748A (zh) | 一种适用于喷涂焊接行业的机器人快速示教系统及方法 | |
| CN107843259B (zh) | 一种基于vckf的多机器人协同导航定位方法 | |
| Zhuang et al. | Camera calibration issues in robot calibration with eye-on-hand configuration | |
| US20150081100A1 (en) | Method for calibrating kinematics | |
| CN112338922A (zh) | 一种五轴机械臂抓取放置方法及相关装置 | |
| Liu et al. | Robust industrial robot real-time positioning system using VW-camera-space manipulation method | |
| US20230033339A1 (en) | Image processing system | |
| Henriksson et al. | Maximizing the use of computational resources in multi-camera feedback control |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |