CN115096288B - 移动机器人导航轨迹解算精度和能力测试方法 - Google Patents
移动机器人导航轨迹解算精度和能力测试方法 Download PDFInfo
- Publication number
- CN115096288B CN115096288B CN202210706524.7A CN202210706524A CN115096288B CN 115096288 B CN115096288 B CN 115096288B CN 202210706524 A CN202210706524 A CN 202210706524A CN 115096288 B CN115096288 B CN 115096288B
- Authority
- CN
- China
- Prior art keywords
- track
- virtual
- robot
- frame
- sensor
- 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.)
- Active
Links
Images
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
-
- 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
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/90—Determination of colour characteristics
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10024—Color image
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20212—Image combination
- G06T2207/20221—Image fusion; Image merging
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
移动机器人导航轨迹解算精度和能力测试方法,解决了现有不便使用靶标轨迹评测机器人自身轨迹解算精度的问题,属于导航轨迹精度测试技术领域。本发明使用高精度激光扫描测距仪器,扫描环境获得场景完整点云和图像,采用虚拟轨迹和虚拟传感器类型与视角(与被测机器人使用的传感器类型和参数、视角相同),对该场景完整点云和图像进行采样,生成虚拟轨迹和虚拟传感器类型与视角对应的传感器数据帧序列,被测机器人使用该传感器数据帧序列(等同于该机器人在该实际环境中进行运动和数据采集),解算生成机器人运动轨迹,生成的机器人运动轨迹与虚拟轨迹(作为真值)进行比较,测试被测机器人轨迹解算精度和能力。
Description
技术领域
本发明涉及一种移动机器人导航轨迹解算精度和能力测试方法,属于导航轨迹精度测试技术领域。
背景技术
移动机器人可依靠自身传感器,进行导航,并可解算自身的运动轨迹。提高机器人导航轨迹精度是机器人领域的一个重要研究方向,导航轨迹精度测试是其中关键环节。
通过在机器人上放置靶标,使用激光追踪仪获取机器人运行轨迹是目前测试机器人导航轨迹精度最高的方法之一。但是该方法成本高,操作复杂,并且激光跟踪仪给出的是靶标的运行轨迹,存在机器人旋转运行时,靶标平移变化小,即轨迹对旋转运动轨迹误差大等问题;存在移动机器人依靠自身传感器解算的轨迹与靶标的轨迹参照系不统一,不便于使用靶标轨迹评测机器人自身轨迹解算精度问题。
发明内容
针对现有不便使用靶标轨迹评测机器人自身轨迹解算精度的问题,本发明提供一种移动机器人导航轨迹解算精度和能力测试方法。
本发明的一种移动机器人导航轨迹解算精度和能力测试方法,所述方法包括:
S1、确定场景,场景内分布有多个站点,每个站点使用单线三维激光扫描仪对场景进行扫描,得到场景的三维点云,同时采集以激光光轴为中心的彩色图像,对采集的三维点云进行颜色映射,得到三维彩色点云和图像;
单线三维激光扫描仪的测距精度优于被测移动机器人传感器精度;
根据在不同站点得到的三维彩色点云之间重叠区域,求取不同站点云旋转平移矩阵,将多站点三维彩色点云融合到一起,最终得到场景完整点云和图像;
S2、根据被测机器人,设定虚拟轨迹、虚拟传感器的类型、内参数和视角,采用虚拟轨迹、虚拟传感器的类型、内参数和视角对场景完整点云和图像进行采样,生成与虚拟轨迹、虚拟传感器类型和视角对应的传感器数据帧序列;所述虚拟传感器类型为被测移动机器人的传感器类型和参数相同;
S3、被测移动机器人对S2生成的传感器数据帧序列进行解算生成机器人运动轨迹,将该机器人运动轨迹与S2的虚拟轨迹对比,评价导航轨迹精度和能力。
作为优选,所述S2包括:
设定虚拟传感器类型、内参数、视角、取景框IoU阈值;
在空间中生成取景框,取景框形状、视场角、深度根据设定的传感器内参数决定;在空间中生成取景框后,计算虚拟轨迹中相邻轨迹帧之间取景框的IoU,如果相邻轨迹帧对应的取景框的IoU低于取景框IoU阈值,则根据需要在这两帧之间进行线性插值,插入补充轨迹帧,直到所有相邻轨迹帧所对应的取景框之间的IoU都大于设定的取景框IoU阈值;
逐帧读取插帧后的轨迹,根据每一帧轨迹的位置、视角,结合虚拟传感器的类型、内参数,计算虚拟传感器到场景完整点云中各点的距离值或光度值作为像素值的图像后输出,或直接按虚拟传感器的空间分辨率捕获虚拟传感器处采集范围内的点云后输出,循环这个过程,直到所有轨迹帧对应的输出都被计算完毕,最终得到与虚拟轨迹对齐的传感器数据帧序列。
作为优选,S3中,采用相对位姿误差RPE和绝对轨迹误差ATE的统计值评价机器人导航轨迹精度和能力。
本发明的有益效果:本发明使用高精度激光扫描测距仪器,扫描环境获得场景完整点云和图像,采用虚拟轨迹和虚拟传感器类型与视角(与被测机器人使用的传感器类型和参数、视角相同),对该场景完整点云和图像进行采样,生成虚拟轨迹和虚拟传感器类型与视角对应的传感器数据帧序列(是场景完整点云和图像的数据序列的子集合),被测机器人使用该传感器数据帧序列(等同于该机器人在该实际环境中进行运动和数据采集),解算生成机器人运动轨迹,生成的机器人运动轨迹与虚拟轨迹(作为真值)进行比较,测试被测机器人轨迹解算精度和能力。使用场景完整点云和图像数据,生成虚拟轨迹,再利用虚拟轨迹生成传感器的虚拟数据集,从原理上减少了误差;同时可以根据任意的虚拟轨迹生成被测机器人的传感器数据帧序列,极大地丰富了用于导航仿真的数据集的数据量,相比于其他方法,本发明可便捷、全面地测试评价机器人导航轨迹解算精度和能力。
附图说明
图1为本发明的流程示意图;
图2为多站点云融合的场景完整点云;
图3为虚拟轨迹设定;
图4为验证相邻帧之间取景框的IoU并插入轨迹帧,其中图4(a)为相邻帧对应的取景框的IoU低于取景框IoU阈值的情况,图4(b)为插入补充轨迹帧的情况。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例,都属于本发明保护的范围。
需要说明的是,在不冲突的情况下,本发明中的实施例及实施例中的特征可以相互组合。
下面结合附图和具体实施例对本发明作进一步说明,但不作为本发明的限定。
本实施方式的移动机器人导航轨迹解算精度和能力测试方法,包括:
步骤1、选定合适的场景,场景内分布有多个站点,每个站点使用单线高精度三维激光扫描仪对场景进行扫描,得到场景的三维点云,同时采集以激光光轴为中心的彩色图像,对采集的三维点云进行颜色映射,得到三维彩色点云和图像;单线高精度三维激光扫描仪的测距精度优于被测移动机器人传感器精度;根据在不同站点得到的三维彩色点云之间重叠区域,求取不同站点云旋转平移矩阵,将多站点维彩色点云融合到一起,最终得到场景完整点云和图像,如图2所示;
步骤2、设定虚拟轨迹、虚拟传感器的类型、内参数和视角,
采用虚拟轨迹和虚拟传感器类型与视角(与被测机器人使用的传感器类型和参数、视角相同),对该场景完整点云和图像进行采样,生成虚拟轨迹和虚拟传感器类型与视角对应的传感器数据帧序列(是场景完整点云和图像的数据序列的子集合);
步骤3、被测机器人使用步骤2生成的传感器数据帧序列(等同于该机器人在该实际环境中进行运动和数据采集),解算生成机器人运动轨迹,生成的机器人运动轨迹与虚拟轨迹(作为真值)进行比较,测试被测机器人轨迹解算精度和能力。
本实施方式中使用场景完整点云和图像数据,生成虚拟轨迹,再利用虚拟轨迹生成传感器的虚拟数据集,从原理上减少了误差;同时可以根据任意的虚拟轨迹生成被测机器人的传感器数据帧序列,极大地丰富了用于导航仿真的数据集的数据量,相比于其他方法,本发明可便捷、全面地测试评价机器人导航轨迹解算精度和能力。优选实施例中,本实施方式的步骤2包括:
根据需要对被测移动机器人的运动路径,姿态,可自行设置行进路线,行进时视角,路径长短,路径是否闭环,生成虚拟轨迹。
根据被测机器人,设定虚拟传感器类型、内参数、视角、取景框IoU阈值;在实际生成的传感器数据帧序列前,需要验证虚拟轨迹。首先在空间中生成取景框,取景框形状、视场角、深度根据设定的传感器内参数决定;在空间中生成取景框后,计算虚拟轨迹中相邻轨迹帧之间取景框的IoU,如果相邻轨迹帧对应的取景框的IoU低于取景框IoU阈值,如图4(a)第四位姿帧与第五位姿帧,则根据需要在这两帧之间进行线性插值,插入补充轨迹帧,如图4(b)所示,直到所有相邻轨迹帧所对应的取景框之间的IoU都大于设定的阈值;新轨迹记为:Q1,…,Qn∈SE(3)。
接下来使用轨迹生成传感器数据帧序列。逐帧读取插帧后的轨迹,根据每一帧轨迹的位置、视角,结合虚拟传感器的类型、内参数,计算虚拟传感器到场景完整点云中各点的距离值或光度值作为像素值的图像后输出,或直接按虚拟传感器的空间分辨率捕获虚拟传感器处采集范围内的点云后输出,循环这个过程,直到所有轨迹帧对应的输出都被计算完毕,最终得到与虚拟轨迹对齐的传感器数据帧序列。
本实施方式的步骤3中,被测机器人根据提供的传感器类型、内参数配置完成,使用传感器数据帧序列进行测试。读取传感器数据帧序列作为导航的输入,运行导航任务,计算出导航过程中机器人的轨迹,记为P1,…,Pn∈SE(3)。
本实施方式采用RPE(relative pose error,相对位姿误差)和ATE(absolutetrajectory error,绝对轨迹误差)的统计值评价机器人导航轨迹精度:
采用相对位姿误差RPE和绝对轨迹误差ATE的统计值评价机器人导航轨迹精度。
相对位姿误差RPE主要描述的是相隔固定帧数Δ的两帧位姿差的精度(相比真实位姿),相当于直接测量里程计的误差。虚拟轨迹与机器人运动轨迹的第i帧的相对位姿误差RPE为:
n表示机器人运动轨迹的帧数,Δ表示间隔帧数,已知总数n与间隔Δ的情况下,m=n-Δ,Qi+Δ表示虚拟轨迹中第i+Δ帧,Qi表示虚拟轨迹中第i帧,Pi表示机器人运动轨迹中第i帧,Pi+Δ表示机器人运动轨迹中第i+Δ帧;得到m=n-Δ个RPE,然后用均方根误差RMSE统计这个误差,得到一个总体值:
RMSE(E1:n,Δ)表示第1到n帧Ei的均方根误差;
根据RMSE(E1:n,Δ)评价移动机器人导航轨迹精度。
绝对轨迹误差ATE是估计位姿和真实位姿的直接差值,可以非常直观地反应算法精度和轨迹全局一致性,虚拟轨迹与机器人运动轨迹的第i帧的绝对轨迹误差为:
Fi表示使用最小二乘计算出一个虚拟轨迹到机器人运动轨迹的位姿变换矩阵S∈SE(3),SE(3)表示特殊欧式群,用来表示参数空间;
同样适用RMSE反应统计属性:
RMSE(F1:n,Δ)表示第1到n帧Fi的均方根误差。
机器人解算的轨迹对于虚拟轨迹的RPE和ATE的RMSE越小,则本评定其导航轨迹精度越高、能力越强。
虽然在本文中参照了特定的实施方式来描述本发明,但是应该理解的是,这些实施例仅仅是本发明的原理和应用的示例。因此应该理解的是,可以对示例性的实施例进行许多修改,并且可以设计出其他的布置,只要不偏离所附权利要求所限定的本发明的精神和范围。应该理解的是,可以通过不同于原始权利要求所描述的方式来结合不同的从属权利要求和本文中所述的特征。还可以理解的是,结合单独实施例所描述的特征可以使用在其他所述实施例中。
Claims (5)
1.移动机器人导航轨迹解算精度和能力测试方法,其特征在于,所述方法包括:
S1、确定场景,场景内分布有多个站点,每个站点使用单线三维激光扫描仪对场景进行扫描,得到场景的三维点云,同时采集以激光光轴为中心的彩色图像,对采集的三维点云进行颜色映射,得到三维彩色点云和图像;
单线三维激光扫描仪的测距精度优于被测移动机器人传感器精度;
根据在不同站点得到的三维彩色点云之间重叠区域,求取不同站点云旋转平移矩阵,将多站点三维彩色点云融合到一起,最终得到场景完整点云和图像;
S2、设定虚拟轨迹、虚拟传感器的类型、内参数和视角,采用虚拟轨迹、虚拟传感器的类型、内参数和视角对场景完整点云和图像进行采样,生成与虚拟轨迹、虚拟传感器类型和视角对应的传感器数据帧序列;所述虚拟传感器与被测移动机器人的传感器类型和参数相同;
S3、被测移动机器人对S2生成的传感器数据帧序列进行解算生成机器人运动轨迹,将该机器人运动轨迹与S2的虚拟轨迹对比,评价导航轨迹精度和能力;
所述S2包括:
根据被测机器人,设定虚拟传感器类型、内参数、视角、取景框IoU阈值;
在空间中生成取景框,取景框形状、视场角、深度根据设定的传感器内参数决定;在空间中生成取景框后,计算虚拟轨迹中相邻轨迹帧之间取景框的IoU,如果相邻轨迹帧对应的取景框的IoU低于取景框IoU阈值,则根据需要在这两帧之间进行线性插值,插入补充轨迹帧,直到所有相邻轨迹帧所对应的取景框之间的IoU都大于设定的取景框IoU阈值;
逐帧读取插帧后的轨迹,根据每一帧轨迹的位置、视角,结合虚拟传感器的类型、内参数,计算虚拟传感器到场景完整点云中各点的距离值或光度值作为像素值的图像后输出,或直接按虚拟传感器的空间分辨率捕获虚拟传感器处采集范围内的点云后输出,循环这个过程,直到所有轨迹帧对应的输出都被计算完毕,最终得到与虚拟轨迹对齐的传感器数据帧序列。
2.根据权利要求1所述的移动机器人导航轨迹解算精度和能力测试方法,其特征在于,所述S2中,
根据需要对被测移动机器人的运动路径,姿态,可自行设置行进路线,行进时视角,路径长短,路径是否闭环,生成虚拟轨迹。
3.根据权利要求1所述的移动机器人导航轨迹解算精度和能力测试方法,其特征在于,所述S3中,采用相对位姿误差RPE和绝对轨迹误差ATE的统计值评价机器人导航轨迹精度和能力。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210706524.7A CN115096288B (zh) | 2022-06-21 | 2022-06-21 | 移动机器人导航轨迹解算精度和能力测试方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210706524.7A CN115096288B (zh) | 2022-06-21 | 2022-06-21 | 移动机器人导航轨迹解算精度和能力测试方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115096288A CN115096288A (zh) | 2022-09-23 |
CN115096288B true CN115096288B (zh) | 2023-03-17 |
Family
ID=83292438
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210706524.7A Active CN115096288B (zh) | 2022-06-21 | 2022-06-21 | 移动机器人导航轨迹解算精度和能力测试方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115096288B (zh) |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104713559A (zh) * | 2015-02-01 | 2015-06-17 | 西北工业大学 | 一种高精度sins模拟器的设计方法 |
CN110802587A (zh) * | 2018-08-06 | 2020-02-18 | 北京柏惠维康科技有限公司 | 一种确定机器人安全线路的方法及装置 |
CN111238375A (zh) * | 2020-03-16 | 2020-06-05 | 北京卫星制造厂有限公司 | 基于激光跟踪仪的移动检测机器人大型构件外形重构方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9875557B2 (en) * | 2012-11-05 | 2018-01-23 | The Chancellor Masters And Scholars Of The University Of Oxford | Extrinsic calibration of imaging sensing devices and 2D LIDARs mounted on transportable apparatus |
CN111444811B (zh) * | 2020-03-23 | 2023-04-28 | 复旦大学 | 一种三维点云目标检测的方法 |
-
2022
- 2022-06-21 CN CN202210706524.7A patent/CN115096288B/zh active Active
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104713559A (zh) * | 2015-02-01 | 2015-06-17 | 西北工业大学 | 一种高精度sins模拟器的设计方法 |
CN110802587A (zh) * | 2018-08-06 | 2020-02-18 | 北京柏惠维康科技有限公司 | 一种确定机器人安全线路的方法及装置 |
CN111238375A (zh) * | 2020-03-16 | 2020-06-05 | 北京卫星制造厂有限公司 | 基于激光跟踪仪的移动检测机器人大型构件外形重构方法 |
Non-Patent Citations (2)
Title |
---|
Rongbin Xu 等.A novel dynamic checkpoint selection strategy for time-constrained massive cloud business workflows.2017,第97-102页. * |
郭军 ; 王炳荣 ; .机器人直线运动位置轨迹精度评估方法研究.2020,第37卷(第06期),第724-728页. * |
Also Published As
Publication number | Publication date |
---|---|
CN115096288A (zh) | 2022-09-23 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US9965870B2 (en) | Camera calibration method using a calibration target | |
Prescott et al. | Line-based correction of radial lens distortion | |
CN103307975B (zh) | 模型生成设备和方法以及信息处理设备和方法 | |
JP3880702B2 (ja) | 画像のオプティカルフロー検出装置及び移動体の自己位置認識システム | |
Luna et al. | Calibration of line-scan cameras | |
CN105069743A (zh) | 探测器拼接实时图像配准的方法 | |
CN108982901A (zh) | 一种匀速旋转体的转速测量方法 | |
CN112446844B (zh) | 一种基于点云特征提取与配准融合方法 | |
CN110703230A (zh) | 激光雷达与摄像头之间的位置标定方法 | |
Stavroulakis et al. | Rapid tracking of extrinsic projector parameters in fringe projection using machine learning | |
CN113295142B (zh) | 一种基于faro扫描仪和点云的地形扫描分析方法和装置 | |
CN113012238B (zh) | 一种多深度相机快速标定与数据融合的方法 | |
CN112712566B (zh) | 基于结构参数在线校正的双目立体视觉传感器测量方法 | |
Anchini et al. | A comparison between stereo-vision techniques for the reconstruction of 3-D coordinates of objects | |
CN115096288B (zh) | 移动机器人导航轨迹解算精度和能力测试方法 | |
CN113483879B (zh) | 一种小卫星颤振高速视频测量方法 | |
CN112598617B (zh) | 一种基于虚拟平台的外弹道光学测量精度分析方法 | |
Liu et al. | A novel method to calibrate the rotation axis of a line-structured light 3-dimensional measurement system | |
CN114299477A (zh) | 一种车辆视觉定位方法、系统、设备及可读存储介质 | |
Habib et al. | New approach for calibrating off-the-shelf digital cameras | |
Luo et al. | A Binocular Camera Calibration Method in Froth Flotation Based on Key Frame Sequences and Weighted Normalized Tilt Difference | |
CN117197241B (zh) | 一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法 | |
CN116664698B (zh) | 车载双目相机与gnss/imu的自动标定方法 | |
CN117934725B (zh) | 一种测试建筑物室内三维点云配准准确度的仿真方法 | |
CN114972536B (zh) | 一种航空面阵摆扫式相机定位和标定方法 |
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 |