CN105955272B - 服务机器人多飞行时间传感器的融合方法 - Google Patents
服务机器人多飞行时间传感器的融合方法 Download PDFInfo
- Publication number
- CN105955272B CN105955272B CN201610345903.2A CN201610345903A CN105955272B CN 105955272 B CN105955272 B CN 105955272B CN 201610345903 A CN201610345903 A CN 201610345903A CN 105955272 B CN105955272 B CN 105955272B
- Authority
- CN
- China
- Prior art keywords
- robot
- barrier
- axis
- coordinate system
- barrier 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.)
- Active
Links
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 8
- 230000004888 barrier function Effects 0.000 claims abstract description 56
- 238000005259 measurement Methods 0.000 claims abstract description 11
- 238000000034 method Methods 0.000 claims abstract description 8
- 238000006243 chemical reaction Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 11
- 238000011835 investigation Methods 0.000 claims description 4
- 238000007906 compression Methods 0.000 claims description 3
- 230000006835 compression Effects 0.000 claims description 3
- 238000013144 data compression Methods 0.000 claims description 3
- 238000001514 detection method Methods 0.000 claims 1
- 230000009466 transformation Effects 0.000 claims 1
- 230000004927 fusion Effects 0.000 abstract description 8
- 230000015572 biosynthetic process Effects 0.000 abstract description 3
- 241001269238 Data Species 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000011897 real-time detection Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
-
- 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
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/88—Sonar systems specially adapted for specific applications
- G01S15/93—Sonar systems specially adapted for specific applications for anti-collision purposes
- G01S15/931—Sonar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
- G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0255—Control of position or course in two dimensions specially adapted to land vehicles using acoustic signals, e.g. ultra-sonic singals
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
- G06F18/251—Fusion techniques of input or preprocessed data
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Aviation & Aerospace Engineering (AREA)
- Theoretical Computer Science (AREA)
- Acoustics & Sound (AREA)
- Computer Networks & Wireless Communication (AREA)
- Data Mining & Analysis (AREA)
- Artificial Intelligence (AREA)
- Bioinformatics & Computational Biology (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Biology (AREA)
- Evolutionary Computation (AREA)
- General Engineering & Computer Science (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供一种服务机器人多飞行时间传感器的融合方法,包括:步骤1,获取机器人的位置缓存o={pos,t};步骤2,获取传感器检测到的障碍物数据z={d,r,var,t,h};步骤3,将障碍物数据转换到机器人坐标系;步骤4,根据机器人的位置缓存,将障碍物数据中的障碍物距离d转换到机器人坐标系下;步骤5,将已经变换到机器人坐标系下的障碍物数据进行转换,形成的栅格;步骤6,对转换后的障碍物数据做阈值处理,高于阈值的判定为障碍物。本发明具有以下优点:一种统一的融合框架,适用于所有ToF传感器2)由于通过高斯模糊和不同传感器的不确定性融合,提高了最终障碍物测量的确定性。
Description
技术领域
本发明涉及机械领域,具体说是一种服务机器人多飞行时间传感器的融合方法。
背景技术
移动服务机器人为了能在动态不确定环境中实现自主导航任务,需要使用基于飞行时间(ToF)的传感器实时检测周围的障碍物。目前大多数服务机器人直接采用扇面激光雷达,或利用人工规则简单融合超声传感器。为简化后期机器人行为规划层的处理逻辑,这里提出一种通用的传感器融合技术用于融合多种ToF传感器。
发明内容
针对现有技术中的缺陷,本发明目的在于提供一种提高融合后障碍物测量的确定性的服务机器人多飞行时间传感器的融合方法。
为解决上述技术问题,本发明提供一种服务机器人多飞行时间传感器的融合方法,包括:
步骤1,获取机器人的位置缓存o={pos,t};其中,pos为机器人在全局坐标中的位置和旋转,t为获得该位置的时间;
步骤2,获取传感器检测到的障碍物数据z={d,r,var,t,h};其中,d为检测到的障碍物距离,r为相对于机器人朝向的角度,var为距离测量的不确定性,t为测量障碍物的时间,h表示障碍物的高度;
步骤3,将障碍物数据转换到机器人坐标系,其中,机器人坐标系是以机器人的中心为原点,以机器人的朝向为X轴,Y轴垂直于X轴,Z轴垂直于X轴和Y轴所在平面,X轴和Y轴所在平面与地面平行;
步骤4,根据机器人的位置缓存,将障碍物数据中的障碍物距离d转换到机器人坐标系下;
P1=inv(T1)*T0*p
其中,p表示传感器获取障碍物数据时机器人的位置,P1为转换到机器人坐标系后的位置,inv(T1)表示T1的反变换矩阵,T0为p对应的齐次变换矩阵为,T1为P1对应的齐次变换矩阵;
步骤5,将已经变换到机器人坐标系下的障碍物数据进行转换,形成的栅格;
步骤6,对转换后的障碍物数据做阈值处理,高于阈值的判定为障碍物。
优选地,步骤5包括:
步骤5.1,将已经变换到机器人坐标系下的障碍物数据压缩到2维;
步骤5.2,将压缩的障碍物数据转换到极坐标系下;
步骤5.3,对转换到极坐标系下的障碍物数据进行离散化,得到对应的2维栅格;其中,2维栅格是以机器人的中心为原点,以theta作为横坐标轴,以distance作为纵坐标轴,横坐标轴与纵坐标轴垂直;
步骤5.3,对离散化后的障碍物数据进行高斯模糊处理,并将落在同一栅格上的障碍物数据叠加。
优选地,传感器为飞行时间传感器。
与现有技术相比,本发明具有以下优点:一种统一的融合框架,适用于所有ToF传感器2)由于通过高斯模糊和不同传感器的不确定性融合,提高了最终障碍物测量的确定性。3)由于通过机器人位置历史缓存,可将历史ToF数据保留并融合到当前机器人位置,可有效解决传感器盲区问题。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述,本发明的其它特征.目的和优点将会变得更明显。
图1为本发明服务机器人多飞行时间传感器的融合方法障碍物数据从极坐标系转换到2维栅格示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明,但不以任何形式限制本发明。应当指出的是,对本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变化和改。
如图1所示,本发明提供一种服务机器人多飞行时间传感器的融合方法,包括:
步骤1,获取机器人的位置缓存o={pos,t};其中,pos为机器人在全局坐标中的位置和旋转,t为获得该位置的时间;
步骤2,获取传感器检测到的障碍物数据z={d,r,var,t,h};其中,d为检测到的障碍物距离,r为相对于机器人朝向的角度,var为距离测量的不确定性,t为测量障碍物的时间,h表示障碍物的高度;
步骤3,将障碍物数据转换到机器人坐标系,其中,机器人坐标系是以机器人的中心为原点,以机器人的朝向为X轴,Y轴垂直于X轴,Z轴垂直于X轴和Y轴所在平面,X轴和Y轴所在平面与地面平行;
步骤4,根据机器人的位置缓存,将障碍物数据中的障碍物距离d转换到机器人坐标系下;
P1=inv(T1)*T0*p
其中,p表示传感器获取障碍物数据时机器人的位置,P1为转换到机器人坐标系后的位置,inv(T1)表示T1的反变换矩阵,T0为p对应的齐次变换矩阵为,T1为P1对应的齐次变换矩阵。
步骤5,将已经变换到机器人坐标系下的障碍物数据进行转换,形成的栅格;
步骤6,对转换后的障碍物数据做阈值处理,高于阈值的判定为障碍物。
步骤5包括:
步骤5.1,将已经变换到机器人坐标系下的障碍物数据压缩到2维;
步骤5.2,将压缩的障碍物数据转换到极坐标系下;
步骤5.3,对转换到极坐标系下的障碍物数据进行离散化,得到对应的2维栅格;其中,2维栅格是以机器人的中心为原点,以theta作为横坐标轴,以distance作为纵坐标轴,横坐标轴与纵坐标轴垂直;
步骤5.3,对离散化后的障碍物数据进行高斯模糊处理,并将落在同一栅格上的障碍物数据叠加。
传感器为飞行时间传感器
ToF传感器可用一序列点z={d,r,var,t,h}表示。其中d为检测到的障碍物距离,r为相对于机器人朝向的角度,var表示距离测量的不确定性,t表示该点测量的时间,h表示该点的高度。对于单点超声传感器,每次测量即为数组大小为1的点序列。
建立一个机器人位置的缓存,该缓存中机器人位置可以由机器人里程计得到,或由基于地图匹配的定位信息得到。该缓存中每个元素为o={pos,t}其中pos为机器人的位置和旋转,t为该位置获得的时间。在得到机器人所有ToF传感器数据后,将所有ToF测得的障碍物数据z转换到笛卡尔坐标系p,再根据机器人位置缓存,将每个ToF的距离数据转换到机器人当前坐标系下。假设ToF数据获得时的机器人位置为o0,对应的齐次变换矩阵为T0,机器人当前位置为o1,对应的齐次变换矩阵为T1。则转换到当前机器人坐标系下的ToF坐标为:
P1=inv(T1)*T0*p
首先将机器人2D极坐标系局以delta theta和delta distance分成2D栅格。然后,将所有变换到当前机器人坐标系下的ToF传感器数据压缩到2D,转换到极坐标系后再离散化到对应的栅格。对每个离散化后的数据点根据该传感器的不确定性,对其进行高斯模糊处理,并将落在同一栅格的数据叠加。
在完成所有ToF数据的处理后,对最终形成的栅格,做阈值处理,高于阈值的便为障碍物。
以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变化或修改,这并不影响本发明的实质内容。在不冲突的情况下,本申请的实施例和实施例中的特征可以任意相互组合。
Claims (1)
1.一种服务机器人多飞行时间传感器的融合方法,其特征在于,包括如下步骤:
步骤1,获取机器人的位置缓存o={pos,t};其中,pos为机器人在全局坐标中的位置和旋转角度,t为获得该位置的时间;
步骤2,获取多飞行时间传感器检测到的障碍物数据z={d,r,var,t,h};其中,d为检测到的障碍物距离,r为相对于机器人朝向的角度,var为距离测量的不确定性,t为测量障碍物的时间,h表示障碍物的高度;
步骤3,将障碍物数据转换到笛卡尔坐标系;
步骤4,根据机器人的位置缓存,将障碍物数据中的障碍物距离d转换到机器人坐标系下;
P1=inv(T1)*T0*p
其中,p表示多飞行时间传感器获取障碍物数据时机器人的位置,P1为转换到机器人坐标系后的位置,inv(T1)表示T1的反变换矩阵,T0为p对应的齐次变换矩阵,T1为P1对应的齐次变换矩阵;机器人坐标系是以机器人的中心为原点,以机器人的朝向为X轴,Y轴垂直于X轴,Z轴垂直于X轴和Y轴所在平面,X轴和Y轴所在平面与地面平行;
步骤5,将已经变换到机器人坐标系下的障碍物数据进行转换,形成栅格;
步骤6,对转换后的障碍物数据做阈值处理,高于阈值的判定为障碍物;
步骤5包括:
步骤5.1,将已经变换到机器人坐标系下的障碍物数据压缩到2维;
步骤5.2,将压缩的障碍物数据转换到极坐标系下;
步骤5.3,对转换到极坐标系下的障碍物数据进行离散化,得到对应的2维栅格;其中,2维栅格是以机器人的中心为原点,以theta作为横坐标轴,以distance作为纵坐标轴,横坐标轴与纵坐标轴垂直;
步骤5.3,对离散化后的障碍物数据进行高斯模糊处理,并将落在同一栅格上的障碍物数据叠加。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610345903.2A CN105955272B (zh) | 2016-05-23 | 2016-05-23 | 服务机器人多飞行时间传感器的融合方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610345903.2A CN105955272B (zh) | 2016-05-23 | 2016-05-23 | 服务机器人多飞行时间传感器的融合方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105955272A CN105955272A (zh) | 2016-09-21 |
CN105955272B true CN105955272B (zh) | 2019-07-26 |
Family
ID=56909508
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610345903.2A Active CN105955272B (zh) | 2016-05-23 | 2016-05-23 | 服务机器人多飞行时间传感器的融合方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105955272B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108814452A (zh) * | 2018-08-22 | 2018-11-16 | 上海炬佑智能科技有限公司 | 扫地机器人及其障碍检测方法 |
CN112987734B (zh) * | 2021-02-23 | 2023-05-02 | 京东科技信息技术有限公司 | 机器人行驶方法、装置及电子设备、存储介质及程序产品 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101413806A (zh) * | 2008-11-07 | 2009-04-22 | 湖南大学 | 一种实时数据融合的移动机器人栅格地图创建方法 |
CN102156476A (zh) * | 2011-04-14 | 2011-08-17 | 山东大学 | 智能空间与护士机器人多传感器***及其信息融合方法 |
CN102520721A (zh) * | 2011-12-08 | 2012-06-27 | 北京控制工程研究所 | 一种基于双目立体视觉的巡视探测器自主避障规划方法 |
CN104002747A (zh) * | 2014-06-10 | 2014-08-27 | 北京联合大学 | 一种基于无人驾驶汽车的多激光雷达栅格地图融合*** |
JP2014228943A (ja) * | 2013-05-20 | 2014-12-08 | 日本電産エレシス株式会社 | 車両用外界センシング装置、その軸ズレ補正プログラム及びその軸ズレ補正方法 |
-
2016
- 2016-05-23 CN CN201610345903.2A patent/CN105955272B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101413806A (zh) * | 2008-11-07 | 2009-04-22 | 湖南大学 | 一种实时数据融合的移动机器人栅格地图创建方法 |
CN102156476A (zh) * | 2011-04-14 | 2011-08-17 | 山东大学 | 智能空间与护士机器人多传感器***及其信息融合方法 |
CN102520721A (zh) * | 2011-12-08 | 2012-06-27 | 北京控制工程研究所 | 一种基于双目立体视觉的巡视探测器自主避障规划方法 |
JP2014228943A (ja) * | 2013-05-20 | 2014-12-08 | 日本電産エレシス株式会社 | 車両用外界センシング装置、その軸ズレ補正プログラム及びその軸ズレ補正方法 |
CN104002747A (zh) * | 2014-06-10 | 2014-08-27 | 北京联合大学 | 一种基于无人驾驶汽车的多激光雷达栅格地图融合*** |
Non-Patent Citations (3)
Title |
---|
基于多传感器信息融合的移动机器人导航综述;丁伟 等;《传感器与微***》;20061231;第25卷(第7期);全文 |
机器人多传感器信息融合研究综述;赵小川 等;《机器人多传感器信息融合研究综述》;20081231;第27卷(第8期);全文 |
移动机器人多传感器信息融合技术综述;司现军 等;《机电工程》;20041231;第21卷(第2期);全文 |
Also Published As
Publication number | Publication date |
---|---|
CN105955272A (zh) | 2016-09-21 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106997049B (zh) | 一种基于激光点云数据的检测障碍物的方法和装置 | |
US11709058B2 (en) | Path planning method and device and mobile device | |
CN111324121B (zh) | 一种基于激光雷达的移动机器人自动充电方法 | |
CN107991680B (zh) | 动态环境下基于激光雷达的slam方法 | |
CN105716604A (zh) | 基于地磁序列的移动机器人室内定位方法及*** | |
CN105404844A (zh) | 一种基于多线激光雷达的道路边界检测方法 | |
WO2019126950A1 (zh) | 一种定位方法、云端服务器、终端、***、电子设备及计算机程序产品 | |
CN105091885B (zh) | 机器人及自身位置推定方法 | |
CN110095786B (zh) | 基于一线激光雷达的三维点云地图生成方法及*** | |
CN105324729A (zh) | 用于模型化车辆的周围环境的方法 | |
CN111856499B (zh) | 基于激光雷达的地图构建方法和装置 | |
CN109828302A (zh) | 一种基于多振动传感器的震源定位方法及装置 | |
CN105955272B (zh) | 服务机器人多飞行时间传感器的融合方法 | |
CN105652895A (zh) | 基于激光传感器的移动机器人人体跟踪***及跟踪方法 | |
CN115139303A (zh) | 一种栅格井盖检测方法、装置、设备及存储介质 | |
CN114264297B (zh) | Uwb和视觉slam融合算法的定位建图方法及*** | |
CN110109146B (zh) | 基于多线激光雷达的路面检测方法及装置 | |
CN113433568B (zh) | 一种激光雷达观测模拟方法及装置 | |
Wang et al. | A robust lidar state estimation and map building approach for urban road | |
CN112486172A (zh) | 道路边缘检测方法及机器人 | |
CN108253968B (zh) | 基于三维激光的绕障方法 | |
CN111113405B (zh) | 一种机器人获得位置服务的方法及机器人 | |
CN109917418A (zh) | 一种激光雷达无反射区域的测量方法 | |
Wu et al. | Method for detecting glass wall with LiDAR and ultrasonic sensor | |
CN102707301A (zh) | 定位装置及其定位方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CP01 | Change in the name or title of a patent holder | ||
CP01 | Change in the name or title of a patent holder |
Address after: 201210 Shanghai City, Pudong New Area Chinese Jing (Shanghai) Free Trade Zone No. 351 Building No. 2 room A652-2 Patentee after: Shanghai TIMI robot Co.,Ltd. Address before: 201210 Shanghai City, Pudong New Area Chinese Jing (Shanghai) Free Trade Zone No. 351 Building No. 2 room A652-2 Patentee before: SHANGHAI TMI ROBOT TECHNOLOGY Co.,Ltd. |