CN116592888A - 一种巡逻机器人全局定位的方法、系统、装置和介质 - Google Patents
一种巡逻机器人全局定位的方法、系统、装置和介质 Download PDFInfo
- Publication number
- CN116592888A CN116592888A CN202310507289.5A CN202310507289A CN116592888A CN 116592888 A CN116592888 A CN 116592888A CN 202310507289 A CN202310507289 A CN 202310507289A CN 116592888 A CN116592888 A CN 116592888A
- Authority
- CN
- China
- Prior art keywords
- global
- map
- robot
- positioning
- cloud data
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 238000005457 optimization Methods 0.000 claims abstract description 15
- 230000033001 locomotion Effects 0.000 claims abstract description 13
- 238000013519 translation Methods 0.000 claims abstract description 13
- 238000004590 computer program Methods 0.000 claims description 18
- 238000001914 filtration Methods 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000003860 storage Methods 0.000 claims description 9
- 238000004422 calculation algorithm Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 238000013459 approach Methods 0.000 claims description 3
- 239000012535 impurity Substances 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 2
- 230000004807 localization Effects 0.000 claims description 2
- 238000005516 engineering process Methods 0.000 abstract description 8
- 238000012937 correction Methods 0.000 abstract description 7
- 230000007246 mechanism Effects 0.000 abstract description 7
- 238000010586 diagram Methods 0.000 description 8
- 230000008901 benefit Effects 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 108010001267 Protein Subunits Proteins 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011065 in-situ storage Methods 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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/20—Instruments for performing navigational calculations
-
- 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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
- G01C21/30—Map- or contour-matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/43—Determining position using carrier phase measurements, e.g. kinematic positioning; using long or short baseline interferometry
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Abstract
本申请涉及一种巡逻机器人全局定位的方法、系统、装置和介质，其中，该方法包括：通过RTK和IMU获取机器人当前的全局粗定位，并将三维激光雷达点云数据与全局粗定位的点云数据进行ICP配准，得到全局初始定位；在机器人运动状态下，根据雷达点云数据计算LiDAR里程计，根据LiDAR里程计从全局地图中提取子地图，计算雷达点云数据与子地图的scan‑map匹配；根据LiDAR里程计旋转与平移量判断获取关键帧，根据关键帧构建LiDAR里程计因子、scan‑map匹配因子以及RTK全局约束因子，并融合scan‑map匹配因子、LiDAR里程计因子和RTK全局约束因子，通过Gtsam优化得到机器人的实时全局定位结果。通过本申请，解决了相关技术中存在的初始定位不准确，缺少全局约束和错误矫正机制的问题，提高了定位准确度。
Description
技术领域
本申请涉及导航定位技术领域，特别是涉及一种巡逻机器人全局定位的方法、系统、装置和介质。
背景技术
随着计算机技术的快速发展，自动驾驶领域的发展也越来越迅速，其中，自动驾驶技术中最重要的就是如何准确定位。如今的自动驾驶定位一般采用的是组合定位的方式，例如，先获取车辆上设置的激光雷达采集的连续的多帧点云数据；针对每帧点云数据，将点云数据与点云数据在点云地图中对应的子地图进行点云地图匹配，匹配结果为点云数据的初步定位结果；根据该初步定位结果，可以获取点云数据在点云地图中的全局定位结果；最后根据全局定位结果以及车辆的车端里程计、陀螺仪等提供的相对位姿，获取车辆的最终定位结果。
然而，现有的定位方法中，仅基于点云匹配进行初步定位，会导致定位结果过于依赖原始点云地图的精度，若原始点云地图存在鬼影，那么初始定位结果就会不准确；里程计提供的相对位姿只能提供局部精确的定位信息，全局定位不准确，且里程计的误差会随着机体的不断运动而变大，比如发生车轮打滑等恶劣情况时，其很难提供准确定位信息；激光雷达也只能提供局部精确的定位信息，且在雨雪等恶劣天气，点云数据不精确；此外，现有定位技术中还缺少定位错误矫正机制。
因此，针对现有技术中存在的初始定位不准确，缺少全局约束和错误矫正机制的问题，尚未提出有效的解决方案。
发明内容
本申请实施例提供了一种巡逻机器人全局定位的方法、系统、装置和介质，以至少解决相关技术中存在的初始定位不准确，缺少全局约束和错误矫正机制的问题。
第一方面，本申请实施例提供了一种巡逻机器人全局定位的方法，所述方法包括：
通过RTK传感器和IMU获取机器人当前的全局粗定位，并将三维激光雷达点云数据与所述全局粗定位的点云数据进行ICP配准，得到全局初始定位；
在机器人运动状态下，根据雷达点云数据计算LiDAR里程计，并根据所述LiDAR里程计从全局地图中提取子地图，计算雷达点云数据与所述子地图的scan-map匹配；
根据LiDAR里程计旋转与平移量判断获取关键帧，根据所述关键帧构建LiDAR里程计因子、scan-map匹配因子以及RTK全局约束因子，并融合所述scan-map匹配因子、LiDAR里程计因子和RTK全局约束因子，通过Gtsam优化得到机器人的实时全局定位结果。
在其中一些实施例中，在通过RTK传感器和IMU获取机器人当前的全局粗略定位之前，所述方法包括：
预先采集待巡逻场景数据，并根据所述场景数据，通过LIO-SAM构建当前场景的全局地图；
通过体素滤波滤除所述全局地图中的杂质点，并通过直通滤波滤除所述全局地图中z值较大的点，得到优化后的全局地图。
在其中一些实施例中，将三维激光雷达点云数据与所述全局粗定位的点云数据进行ICP配准包括：
根据全局粗定位，计算三维激光雷达点云数据与全局地图的最佳ICP匹配转换关系，构建误差方程，具体误差方程如下：
其中，R和t分别为全局粗定位Tinit的旋转矩阵和平移向量的初值，Pglobal为全局地图，Pinit为当前点云数据。
在其中一些实施例中，根据雷达点云数据计算LiDAR里程计包括：
通过IMU积分数据去除LiDAR的运动畸变，然后对所述雷达点云数据进行滤波处理，得到降噪的激光雷达采样数据；
计算所述激光雷达采样数据的曲率值，提取出其中曲率值较小的plane特征点和曲率值较大的edge特征点；
通过匹配帧间edge特征点和plane特征点，构建误差方程，并通过LM优化算法计算得到LiDAR里程计。
在其中一些实施例中，根据所述LiDAR里程计从全局地图中提取子地图，并计算雷达点云数据与所述子地图的scan-map匹配包括：
以所述LiDAR里程计位置为圆心、预设长度为半径的标准，从全局地图中提取局部子地图；
将所述雷达点云数据与所述局部子地图进行scan-map匹配计算，得到相较于全局地图的没有漂移的位姿变化。
在其中一些实施例中，在机器人进行巡逻运动时，所述方法包括：
预设巡逻点位，通过所述巡逻点位判断当前机器人的定位是否准确。
在其中一些实施例中，通过所述巡逻点位判断当前机器人的定位是否准确包括：
当机器人接近巡逻点位时，在RTK有信号的情况下，读取RTK定位信息，若巡逻点位的RTK坐标值与定位坐标值误差在阈值范围内，则认为机器人定位准确，继续向下一个巡逻点位移动；若巡逻点位的RTK坐标值与定位坐标值误差超过阈值范围，则机器人重新进行全局初始定位，并搜索与之最接近的巡逻点；
若RTK无信号，则计算Gtsam优化的坐标值与巡逻点位的坐标值之间的误差，若误差在阈值范围内，则认为机器人定位准确，继续向下一个巡逻点位移动；若误差超过阈值范围，则机器人重新进行全局初始定位，并搜索与之最接近的巡逻点。
第二方面，本申请实施例提供了一种巡逻机器人全局定位的系统，所述系统包括：
初定位模块，用于通过RTK传感器和IMU获取机器人当前的全局粗定位，并将三维激光雷达点云数据与所述全局粗定位的点云数据进行ICP配准，得到全局初始定位；
计算匹配模块，用于在机器人运动状态下，根据雷达点云数据计算LiDAR里程计，并根据所述LiDAR里程计从全局地图中提取子地图，计算雷达点云数据与所述子地图的scan-map匹配；
实时定位模块，用于根据LiDAR里程计旋转与平移量判断获取关键帧，根据所述关键帧构建LiDAR里程计因子、scan-map匹配因子以及RTK全局约束因子，并融合所述scan-map匹配因子、LiDAR里程计因子和RTK全局约束因子，通过Gtsam优化得到机器人的实时全局定位结果。
第三方面，本申请实施例提供了一种电子装置，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述第一方面所述的方法。
第四方面，本申请实施例提供了一种存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述第一方面所述的方法。
相比于相关技术，本申请实施例提供的巡逻机器人全局定位的方法，通过RTK传感器和IMU获取机器人当前的全局粗定位，并将三维激光雷达点云数据与全局粗定位的点云数据进行ICP配准，得到全局初始定位；然后，在机器人运动状态下，根据雷达点云数据计算LiDAR里程计，并根据LiDAR里程计从全局地图中提取子地图，计算雷达点云数据与子地图的scan-map匹配；最后，根据LiDAR里程计旋转与平移量判断获取关键帧，根据关键帧构建LiDAR里程计因子、scan-map匹配因子以及RTK全局约束因子，并融合scan-map匹配因子、LiDAR里程计因子和RTK全局约束因子，通过Gtsam优化得到机器人的实时全局定位结果。
本申请根据RTK传感器数据和IMU数据实现巡逻机器人在点云地图中的粗略定位，然后根据激光点云迭代最近点配准算法(Iterative Closest Point，简称ICP)优化得到精确的机器人全局初始定位，最后使用激光雷达里程计、scan-map匹配和RTK全局约束相融合，完成机器人的实时定位。此外，本申请中还设置了巡逻点位，通过预先设置的巡逻点位可确保机器人定位准确，解决了相关技术中存在的初始定位不准确，缺少全局约束和错误矫正机制的问题。
附图说明
此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1是根据本申请实施例的巡逻机器人全局定位的方法的流程图；
图2是根据本申请实施例的巡逻机器人全局定位的系统的结构框图；
图3是根据本申请实施例的电子设备的内部结构示意图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行描述和说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请提供的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。此外，还可以理解的是，虽然这种开发过程中所作出的努力可能是复杂并且冗长的，然而对于与本申请公开的内容相关的本领域的普通技术人员而言，在本申请揭露的技术内容的基础上进行的一些设计，制造或者生产等变更只是常规的技术手段，不应当理解为本申请公开的内容不充分。
在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域普通技术人员显式地和隐式地理解的是，本申请所描述的实施例在不冲突的情况下，可以与其它实施例相结合。
除非另作定义，本申请所涉及的技术术语或者科学术语应当为本申请所属技术领域内具有一般技能的人士所理解的通常意义。本申请所涉及的“一”、“一个”、“一种”、“该”等类似词语并不表示数量限制，可表示单数或复数。本申请所涉及的术语“包括”、“包含”、“具有”以及它们任何变形，意图在于覆盖不排他的包含；例如包含了一系列步骤或模块(单元)的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可以还包括没有列出的步骤或单元，或可以还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本申请所涉及的“连接”、“相连”、“耦接”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电气的连接，不管是直接的还是间接的。本申请所涉及的“多个”是指大于或者等于两个。“和/或”描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。本申请所涉及的术语“第一”、“第二”、“第三”等仅仅是区别类似的对象，不代表针对对象的特定排序。
本实施例提供了一种巡逻机器人全局定位的方法，图1是根据本申请实施例的巡逻机器人全局定位的方法的流程图，如图1所示，该流程包括如下步骤：
步骤S101，通过RTK传感器和IMU获取机器人当前的全局粗定位，并将三维激光雷达点云数据与全局粗定位的点云数据进行ICP配准，得到全局初始定位。
优选的，在通过RTK传感器和IMU获取机器人当前的全局粗略定位之前，预先采集待巡逻场景数据，并根据该场景数据，通过LIO-SAM构建当前场景的全局地图；通过体素滤波滤除全局地图中的杂质点，并通过直通滤波滤除全局地图中z值较大的点，得到优化后的全局地图用于后期机器人的全局定位。
需要说明的是，优化后的全局地图存储在ENU坐标系下，机器人起点位置为ENU坐标原点。
通过上述步骤得到用于全局定位的地图之后，启动机器人，并通过RTK传感器和IMU获取机器人当前初始状态下的全局粗定位。需要说明的是，RTK传感器采用千寻位置的D300高精度厘米级接收器输出经纬度和海拔数据；IMU采用的是LPMS九轴IMU，具有三轴陀螺仪、三轴加速度计和三轴磁力计，采集数据的频率为100hz；巡逻机器人的主控操作系统为Ubuntu18.04+ROS Melodic。
在一实施例中，通过RTK传感器和IMU获取机器人当前初始状态下的全局粗定位的步骤包括：
S1、将RTK传感器的经度、纬度、海拔数据转换为东北天坐标系下的坐标值。具体的，RTK传感器在接收到数据后，对status_fix状态数据进行判断，如果接收到的是no_status_fix，则表示未搜索到信号；如果status_fix状态数据为0，则表示定位未增强，不可使用；而如果status_fix状态数据为1，则表示定位成功。其中，无定位输出的状态下机器人原地等待30秒，若30秒后仍无定位输出，则机器人向移动终端发送初始化失败提醒；而当接收到RTK信号时，将RTK传感器的经纬高(LLA)坐标转换为东北天(ENU)坐标，具体转换公式如下式(1)所示：
其中，lat表示全局地图坐标原点的纬度值，Δlon、Δlat、Δalt分别表示当前机器人RTK传感器的经、纬、高与全局地图坐标原点位置RTK的经、纬、高的差值，(tE，tN，tU)表示当前机器人在ENU坐标系下的位置，R为地球近似半径。
S2、以IMU传感器的偏航角，作为机器人启动时的全局粗略方向，由RTK和IMU结合得到机器人启动时的全局粗略定位。具体地，通过下式(2)得到机器人启动时的全局粗略定位Tinit：
Tinit＝[tE，tN，tU，roll，pitch，yaw] (2)
其中，roll为IMU传感器中的翻滚角，pitch为俯仰角，yaw为偏航角，tE、tN、tU表示机器人在ENU坐标系下的位置。
接着，将通过激光雷达获取的三维激光雷达点云数据与上述全局粗定位的点云数据进行ICP配准，得到全局初始定位。优选的，其具体步骤包括：
S1、对三维激光点云数据进行滤波处理以减少数据噪声，得到降噪的点云数据，其中，滤波处理方法包括但不限于体素滤波、中值滤波和统计滤波；
S2、根据全局粗定位Tinit，计算降噪后的三维激光雷达点云数据与全局地图的最佳ICP匹配转换关系，构建误差方程，具体误差方程如下式(3)所示：
其中，R和t分别为全局粗定位Tinit的旋转矩阵和平移向量的初值，Pglobal为全局地图，Pinit为当前点云数据。需要说明的是，本实施例中的激光雷达采用的是velodyne-VLP16型号，扫描频率为10Hz，水平角度分辨率为0.2°，垂直扫描范围为-15°～+15°。
通过上述最小化误差方程，可计算得到机器人的全局初始定位。其中，若ICP匹配得分低于预设阈值，则需要调整机器人的姿态，重新计算初始定位。
步骤S102，在机器人运动状态下，根据雷达点云数据计算LiDAR里程计，并根据LiDAR里程计从全局地图中提取子地图，计算雷达点云数据与子地图的scan-map匹配。
通过步骤S101完成机器人的初始定位后，在机器人运动状态下，根据雷达点云数据计算LiDAR里程计，并根据LiDAR里程计从全局地图中提取子地图，计算雷达点云数据与子地图的scan-map匹配。
在一实施例中，根据雷达点云数据计算LiDAR里程计的步骤包括：
S1、通过IMU积分数据去除LiDAR里程计的运动畸变，具体的，通过IMU积分数据，计算出激光雷达一帧扫描结束时刻相对于起始时刻的位姿变换其中，激光雷达一帧扫描的起始时刻为t，结束时刻为t+1。
根据点云的水平角，可以计算出一帧点云中某点i的相对时间reltime，计算公式如下式(4)所示：
其中，ori表示点云水平角，scanPeriod表示激光雷达工作周期。
接着，根据相对时间内插出点i相对于起始时刻的位姿变换计算经矫正后的点云坐标P，具体计算公式如下式(5)-(6)所示：
S2、对雷达点云数据P进行滤波处理，减少数据噪声得到降噪的激光雷达采样数据；
S3、计算激光雷达采样数据的曲率值，提取出其中曲率值较小的plane特征点和曲率值较大的edge特征点，其中，曲率计算公式如下式(7)所示：
其中，r(i，j)表示点到激光雷达距离，Li表示一帧点云数据中的第i根扫描线。
需要说明的是，为了从所有方向均匀地提取特征点，需要根据点云索引，将每帧点云阵列按照水平方向划分为六个子区间；然后计算子区间每条线上点的曲率，通过曲率大小来区分不同类型的特征点。具体的，将每个子区间中不同线上的点按照曲率大小进行排名，然后从六个子区间中的每一个子区间的每一线中分别选出曲率排名前40的边缘点和曲率排名后80的平面点作为edge特征点和plane特征点。
S4、通过匹配当前帧特征点和上一帧特征点，构建误差方程，并通过LM优化算法计算得到LiDAR里程计TL。
在一实施例中，根据LiDAR里程计从全局地图中提取子地图，并计算雷达点云数据与子地图的scan-map匹配包括：
首先，以LiDAR里程计位置为圆心、预设长度为半径的标准，从全局地图中提取局部子地图；然后将雷达点云数据与局部子地图进行scan-map匹配计算，得到相较于全局地图的没有漂移的位姿变化[x，y，z，roll，pitch，yaw]。
可选的，如需区分考虑plane特征点和edge特征点，则可将plane地图和edge地图分开保存，通过plane_scan匹配plane_map，计算得到[z，roll，pitch]；根据平面约束，通过edge_scan匹配edge_map，计算得到[x，y，yaw]。
步骤S103，根据LiDAR里程计旋转与平移量判断获取关键帧，根据关键帧构建LiDAR里程计因子、scan-map匹配因子以及RTK全局约束因子，并融合scan-map匹配因子、LiDAR里程计因子和RTK全局约束因子，通过Gtsam优化得到机器人的实时全局定位结果。
最后，根据LiDAR里程计旋转与平移量判断获取关键帧，优选的，本实施例中判断关键帧的条件为：任何一个旋转量大于给定阈值或者平移量大于给定阈值的帧就认为是关键帧是。
在提取得到关键帧后，根据关键帧分别构建LiDAR里程计因子、scan-map匹配因子以及RTK全局约束因子，并融合scan-map匹配因子、LiDAR里程计因子和RTK全局约束因子到iSAM2中，通过Gtsam优化得到机器人的实时全局定位结果，包括坐标值及航向角。
需要说明的是，进行优化计算时，z坐标值不使用RTK数据，仍使用计算得到的LiDAR里程计的结果；若RTK失去信号，则不使用RTK全局约束因子。
通过上述步骤S101至步骤S103，本实施例根据RTK传感器数据和IMU数据实现巡逻机器人在点云地图中的粗略定位，然后根据激光点云迭代最近点配准算法(IterativeClosest Point，简称ICP)优化得到精确的机器人全局初始定位，最后使用激光雷达里程计、scan-map匹配和RTK全局约束相融合，完成机器人的实时定位。此外，本申请中还设置了巡逻点位，通过预先设置的巡逻点位可确保机器人定位准确，解决了相关技术中存在的初始定位不准确，缺少全局约束和错误矫正机制的问题。
在其中一些实施例中，在机器人进行巡逻运动时，预设巡逻点位，通过巡逻点位判断当前机器人的定位是否准确。例如，在全局地图中每隔10米左右设置一个巡逻点位，并记录各巡逻点位在全局地图中的坐标值。
当机器人接近巡逻点位时，在RTK有信号的情况下，读取RTK定位信息，若巡逻点位的RTK坐标值与定位坐标值误差在阈值范围内，则认为机器人定位准确，继续向下一个巡逻点位移动；若巡逻点位的RTK坐标值与定位坐标值误差超过阈值范围，则机器人重新进行全局初始定位，并搜索与之最接近的巡逻点；
若RTK无信号，则计算Gtsam优化的坐标值与巡逻点位的坐标值之间的误差，若误差在阈值范围内，则认为机器人定位准确，继续向下一个巡逻点位移动；若误差超过阈值范围，则机器人重新进行全局初始定位，并搜索与之最接近的巡逻点。
通过设置巡逻点位，可有效提高机器人全局定位的鲁棒性和安全性。
需要说明的是，在上述流程中或者附图的流程图中示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
本实施例还提供了一种巡逻机器人全局定位的系统，该系统用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”、“单元”、“子单元”等可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。
图2是根据本申请实施例的巡逻机器人全局定位的系统的结构框图，如图2所示，该系统包括初定位模块21、计算匹配模块22和实时定位模块23：
初定位模块21，用于通过RTK传感器和IMU获取机器人当前的全局粗定位，并将三维激光雷达点云数据与全局粗定位的点云数据进行ICP配准，得到全局初始定位；计算匹配模块22，用于在机器人运动状态下，根据雷达点云数据计算LiDAR里程计，并根据LiDAR里程计从全局地图中提取子地图，计算雷达点云数据与子地图的scan-map匹配；实时定位模块23，用于根据LiDAR里程计旋转与平移量判断获取关键帧，根据关键帧构建LiDAR里程计因子、scan-map匹配因子以及RTK全局约束因子，并融合scan-map匹配因子、LiDAR里程计因子和RTK全局约束因子，通过Gtsam优化得到机器人的实时全局定位结果。
通过上述系统，本实施例根据RTK传感器数据和IMU数据实现巡逻机器人在点云地图中的粗略定位，然后根据激光点云迭代最近点配准算法(Iterative Closest Point，简称ICP)优化得到精确的机器人全局初始定位，最后使用激光雷达里程计、scan-map匹配和RTK全局约束相融合，完成机器人的实时定位。此外，本申请中还设置了巡逻点位，通过预先设置的巡逻点位可确保机器人定位准确，解决了相关技术中存在的初始定位不准确，缺少全局约束和错误矫正机制的问题。
需要说明的是，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。
此外，需要说明的是，上述各个模块可以是功能模块也可以是程序模块，既可以通过软件来实现，也可以通过硬件来实现。对于通过硬件来实现的模块而言，上述各个模块可以位于同一处理器中；或者上述各个模块还可以按照任意组合的形式分别位于不同的处理器中。
本实施例还提供了一种电子装置，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。
可选地，上述电子装置还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。
另外，结合上述实施例中的巡逻机器人全局定位的方法，本申请实施例可提供一种存储介质来实现。该存储介质上存储有计算机程序；该计算机程序被处理器执行时实现上述实施例中的任意一种巡逻机器人全局定位的方法。
在一个实施例中，提供了一种计算机设备，该计算机设备可以是终端。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种巡逻机器人全局定位的方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
在一个实施例中，图3是根据本申请实施例的电子设备的内部结构示意图，如图3所示，提供了一种电子设备，该电子设备可以是服务器，其内部结构图可以如图3所示。该电子设备包括通过内部总线连接的处理器、网络接口、内存储器和非易失性存储器，其中，该非易失性存储器存储有操作系统、计算机程序和数据库。处理器用于提供计算和控制能力，网络接口用于与外部的终端通过网络连接通信，内存储器用于为操作系统和计算机程序的运行提供环境，计算机程序被处理器执行时以实现一种巡逻机器人全局定位的方法，数据库用于存储数据。
本领域技术人员可以理解，图3中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的电子设备的限定，具体的电子设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，该计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
本领域的技术人员应该明白，以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。
Claims (10)
1.一种巡逻机器人全局定位的方法，其特征在于，所述方法包括：
通过RTK传感器和IMU获取机器人当前的全局粗定位，并将三维激光雷达点云数据与所述全局粗定位的点云数据进行ICP配准，得到全局初始定位；
在机器人运动状态下，根据雷达点云数据计算LiDAR里程计，并根据所述LiDAR里程计从全局地图中提取子地图，计算雷达点云数据与所述子地图的scan-map匹配；
根据LiDAR里程计旋转与平移量判断获取关键帧，根据所述关键帧构建LiDAR里程计因子、scan-map匹配因子以及RTK全局约束因子，并融合所述scan-map匹配因子、LiDAR里程计因子和RTK全局约束因子，通过Gtsam优化得到机器人的实时全局定位结果。
2.根据权利要求1所述的方法，其特征在于，在通过RTK传感器和IMU获取机器人当前的全局粗略定位之前，所述方法包括：
预先采集待巡逻场景数据，并根据所述场景数据，通过LIO-SAM构建当前场景的全局地图；
通过体素滤波滤除所述全局地图中的杂质点，并通过直通滤波滤除所述全局地图中z值较大的点，得到优化后的全局地图。
3.根据权利要求2所述的方法，其特征在于，将三维激光雷达点云数据与所述全局粗定位的点云数据进行ICP配准包括：
根据全局粗定位，计算三维激光雷达点云数据与全局地图的最佳ICP匹配转换关系，构建误差方程，具体误差方程如下：
其中，R和t分别为全局粗定位Tinit的旋转矩阵和平移向量的初值，Pglobal为全局地图，Pinit为当前点云数据。
4.根据权利要求1所述的方法，其特征在于，根据雷达点云数据计算LiDAR里程计包括：
通过IMU积分数据去除LiDAR的运动畸变，然后对所述雷达点云数据进行滤波处理，得到降噪的激光雷达采样数据；
计算所述激光雷达采样数据的曲率值，提取出其中曲率值较小的plane特征点和曲率值较大的edge特征点；
通过匹配帧间edge特征点和plane特征点，构建误差方程，并通过LM优化算法计算得到LiDAR里程计。
5.根据权利要求1所述的方法，其特征在于，根据所述LiDAR里程计从全局地图中提取子地图，并计算雷达点云数据与所述子地图的scan-map匹配包括：
以所述LiDAR里程计位置为圆心、预设长度为半径的标准，从全局地图中提取局部子地图；
将所述雷达点云数据与所述局部子地图进行scan-map匹配计算，得到相较于全局地图的没有漂移的位姿变化。
6.根据权利要求1所述的方法，其特征在于，在机器人进行巡逻运动时，所述方法包括：
预设巡逻点位，通过所述巡逻点位判断当前机器人的定位是否准确。
7.根据权利要求6所述的方法，其特征在于，通过所述巡逻点位判断当前机器人的定位是否准确包括：
当机器人接近巡逻点位时，在RTK有信号的情况下，读取RTK定位信息，若巡逻点位的RTK坐标值与定位坐标值误差在阈值范围内，则认为机器人定位准确，继续向下一个巡逻点位移动；若巡逻点位的RTK坐标值与定位坐标值误差超过阈值范围，则机器人重新进行全局初始定位，并搜索与之最接近的巡逻点；
若RTK无信号，则计算Gtsam优化的坐标值与巡逻点位的坐标值之间的误差，若误差在阈值范围内，则认为机器人定位准确，继续向下一个巡逻点位移动；若误差超过阈值范围，则机器人重新进行全局初始定位，并搜索与之最接近的巡逻点。
8.一种巡逻机器人全局定位的系统，其特征在于，所述系统包括：
初定位模块，用于通过RTK传感器和IMU获取机器人当前的全局粗定位，并将三维激光雷达点云数据与所述全局粗定位的点云数据进行ICP配准，得到全局初始定位；
计算匹配模块，用于在机器人运动状态下，根据雷达点云数据计算LiDAR里程计，并根据所述LiDAR里程计从全局地图中提取子地图，计算雷达点云数据与所述子地图的scan-map匹配；
实时定位模块，用于根据LiDAR里程计旋转与平移量判断获取关键帧，根据所述关键帧构建LiDAR里程计因子、scan-map匹配因子以及RTK全局约束因子，并融合所述scan-map匹配因子、LiDAR里程计因子和RTK全局约束因子，通过Gtsam优化得到机器人的实时全局定位结果。
9.一种电子装置，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行权利要求1至7中任一项所述的方法。
10.一种存储介质，其特征在于，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行权利要求1至7中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310507289.5A CN116592888A (zh) | 2023-05-08 | 2023-05-08 | 一种巡逻机器人全局定位的方法、系统、装置和介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310507289.5A CN116592888A (zh) | 2023-05-08 | 2023-05-08 | 一种巡逻机器人全局定位的方法、系统、装置和介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116592888A true CN116592888A (zh) | 2023-08-15 |
Family
ID=87610821
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310507289.5A Pending CN116592888A (zh) | 2023-05-08 | 2023-05-08 | 一种巡逻机器人全局定位的方法、系统、装置和介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116592888A (zh) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107450561A (zh) * | 2017-09-18 | 2017-12-08 | 河南科技学院 | 移动机器人的自主路径规划与避障系统及其使用方法 |
| CN113654555A (zh) * | 2021-09-14 | 2021-11-16 | 上海智驾汽车科技有限公司 | 一种基于多传感器数据融合的自动驾驶车辆高精定位方法 |
| CN114236552A (zh) * | 2021-11-12 | 2022-03-25 | 苏州玖物互通智能科技有限公司 | 基于激光雷达的重定位方法及系统 |
| CN114488183A (zh) * | 2021-12-29 | 2022-05-13 | 深圳优地科技有限公司 | 障碍物点云的处理方法、装置、设备以及可读存储介质 |
| CN114639085A (zh) * | 2022-02-28 | 2022-06-17 | 广州赛特智能科技有限公司 | 交通信号灯识别方法、装置、计算机设备和存储介质 |
| CN115077541A (zh) * | 2022-07-01 | 2022-09-20 | 智道网联科技(北京)有限公司 | 自动驾驶车辆的定位方法、装置及电子设备、存储介质 |
| CN115342805A (zh) * | 2022-06-27 | 2022-11-15 | 淮阴工学院 | 一种高精度机器人定位导航系统及导航方法 |
| CN115372989A (zh) * | 2022-08-19 | 2022-11-22 | 中国人民解放军陆军工程大学 | 基于激光雷达的越野自动小车长距离实时定位系统及方法 |
| CN115407357A (zh) * | 2022-07-05 | 2022-11-29 | 东南大学 | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 |
-
2023
- 2023-05-08 CN CN202310507289.5A patent/CN116592888A/zh active Pending
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107450561A (zh) * | 2017-09-18 | 2017-12-08 | 河南科技学院 | 移动机器人的自主路径规划与避障系统及其使用方法 |
| CN113654555A (zh) * | 2021-09-14 | 2021-11-16 | 上海智驾汽车科技有限公司 | 一种基于多传感器数据融合的自动驾驶车辆高精定位方法 |
| CN114236552A (zh) * | 2021-11-12 | 2022-03-25 | 苏州玖物互通智能科技有限公司 | 基于激光雷达的重定位方法及系统 |
| CN114488183A (zh) * | 2021-12-29 | 2022-05-13 | 深圳优地科技有限公司 | 障碍物点云的处理方法、装置、设备以及可读存储介质 |
| CN114639085A (zh) * | 2022-02-28 | 2022-06-17 | 广州赛特智能科技有限公司 | 交通信号灯识别方法、装置、计算机设备和存储介质 |
| CN115342805A (zh) * | 2022-06-27 | 2022-11-15 | 淮阴工学院 | 一种高精度机器人定位导航系统及导航方法 |
| CN115077541A (zh) * | 2022-07-01 | 2022-09-20 | 智道网联科技(北京)有限公司 | 自动驾驶车辆的定位方法、装置及电子设备、存储介质 |
| CN115407357A (zh) * | 2022-07-05 | 2022-11-29 | 东南大学 | 基于大场景的低线束激光雷达-imu-rtk定位建图算法 |
| CN115372989A (zh) * | 2022-08-19 | 2022-11-22 | 中国人民解放军陆军工程大学 | 基于激光雷达的越野自动小车长距离实时定位系统及方法 |
Non-Patent Citations (4)
| Title |
|---|
| 吕品等: "面向变化场景的 LiDAR鲁棒定位与地图维护方法", 《仪 器 仪 表 学 报》, vol. 43, no. 04, pages 291 * |
| 章弘凯等: "一种多层次数据融合的SLAM定位算法", 《机器人》, vol. 43, pages 641 - 652 * |
| 苏涛: "面向越野环境的多传感器融合SLAM技术研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》, no. 03, pages 035 - 316 * |
| 范绪兵: "基于多传感器融合的复杂工况下自动驾驶定位与建图算法研究", 《中国优秀中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》, no. 01, pages 035 - 1228 * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109631887B (zh) | 基于双目、加速度与陀螺仪的惯性导航高精度定位方法 | |
| US9270891B2 (en) | Estimation of panoramic camera orientation relative to a vehicle coordinate frame | |
| US20200134866A1 (en) | Position estimation system and position estimation method | |
| WO2022007776A1 (zh) | 目标场景区域的车辆定位方法、装置、设备和存储介质 | |
| US10996072B2 (en) | Systems and methods for updating a high-definition map | |
| CN113406682B (zh) | 一种定位方法、装置、电子设备及存储介质 | |
| CN109596121B (zh) | 一种机动站自动目标检测与空间定位方法 | |
| CN109871739B (zh) | 基于yolo-sioctl的机动站自动目标检测与空间定位方法 | |
| CN110160545B (zh) | 一种激光雷达与gps的增强定位系统及方法 | |
| CN113252048B (zh) | 一种导航定位方法、导航定位系统及计算机可读存储介质 | |
| CN115077541A (zh) | 自动驾驶车辆的定位方法、装置及电子设备、存储介质 | |
| CN111241224B (zh) | 目标距离估计的方法、系统、计算机设备和存储介质 | |
| CN111383324B (zh) | 点云地图的构建方法、装置、计算机设备和存储介质 | |
| CN110989619A (zh) | 用于定位对象的方法、装置、设备和存储介质 | |
| CN112150550B (zh) | 一种融合定位方法及装置 | |
| CN116184430B (zh) | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 | |
| CN116592888A (zh) | 一种巡逻机器人全局定位的方法、系统、装置和介质 | |
| CN113865586B (zh) | 安装角度的估计方法、装置和自动驾驶系统 | |
| CN116242373A (zh) | 一种融合多源数据的高精度导航定位方法及系统 | |
| CN116045964A (zh) | 高精地图更新方法及装置 | |
| CN114019954B (zh) | 航向安装角标定方法、装置、计算机设备和存储介质 | |
| US11477371B2 (en) | Partial image generating device, storage medium storing computer program for partial image generation and partial image generating method | |
| CN112835086B (zh) | 确定车辆位置的方法和装置 | |
| CN114322996B (zh) | 一种多传感器融合定位系统的位姿优化方法和装置 | |
| CN114897942A (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 |