CN109405826A - 一种机器人定位方法和系统 - Google Patents

一种机器人定位方法和系统 Download PDF

Info

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
Application number
CN201811366331.1A
Other languages
English (en)
Other versions
CN109405826B (zh
Inventor
董又维
徐江伟
李剑平
王浩然
李伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing C&W Electronics Group Co Ltd
Original Assignee
Beijing C&W Electronics Group Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing C&W Electronics Group Co Ltd filed Critical Beijing C&W Electronics Group Co Ltd
Priority to CN201811366331.1A priority Critical patent/CN109405826B/zh
Publication of CN109405826A publication Critical patent/CN109405826A/zh
Application granted granted Critical
Publication of CN109405826B publication Critical patent/CN109405826B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; 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/16Navigation; 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/165Navigation; 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
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Length Measuring Devices By Optical Means (AREA)

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为激光坐标系相对于机器人底盘坐标系的相对位姿。
CN201811366331.1A 2018-11-16 2018-11-16 一种机器人定位方法和系统 Active CN109405826B (zh)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106225790B (zh) * 2016-07-13 2018-11-02 百度在线网络技术(北京)有限公司 一种无人车定位精度的确定方法及装置

Patent Citations (6)

* Cited by examiner, † Cited by third party
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
US11049280B2 (en) System and method for tying together machine vision coordinate spaces in a guided assembly environment
CN105858188B (zh) 具备三维传感器的搬运机器人系统
US9233469B2 (en) Robotic system with 3D box location functionality
CN111347426B (zh) 一种基于3d视觉的机械臂精确放置轨迹规划方法
US12073582B2 (en) Method and apparatus for determining a three-dimensional position and pose of a fiducial marker
KR102056664B1 (ko) 센서를 이용한 작업 방법 및 이를 수행하는 작업 시스템
CN113733100B (zh) 巡检操作机器人的目标定位方法、装置、设备及存储介质
CN110174056A (zh) 一种物体体积测量方法、装置及移动终端
US11074448B2 (en) Method for 3D mapping of 2D point of interest
CN106323286B (zh) 一种机器人坐标系与三维测量坐标系的变换方法
WO2023134237A1 (zh) 用于机器人的坐标系标定方法、装置、系统以及介质
CN109540140A (zh) 一种融合ssd目标识别和里程计信息的移动机器人定位方法
CN111982058A (zh) 一种基于双目相机的测距方法、系统、设备和可读存储介质
CN115439633A (zh) 标定方法、装置和电子设备
CN115488878A (zh) 机器人视觉系统的手眼标定方法、系统、终端及介质
WO2022208963A1 (ja) ロボット制御用のキャリブレーション装置
CN109405826A (zh) 一种机器人定位方法和系统
Zhuang et al. Camera calibration issues in robot calibration with eye-on-hand configuration
Liu et al. Robust industrial robot real-time positioning system using VW-camera-space manipulation method
Lei et al. Visually guided robotic tracking and grasping of a moving object
Henriksson et al. Maximizing the use of computational resources in multi-camera feedback control
CN115533890A (zh) 适应性移动操作设备及方法
CN108152535A (zh) 一种加速度计校准方法及装置
CN115810052A (zh) 相机的标定方法、装置、电子设备及存储介质
Chang et al. On recursive calibration of cameras for robot hand-eye systems

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