CN105955272A - 服务机器人多飞行时间传感器的融合方法 - Google Patents

服务机器人多飞行时间传感器的融合方法 Download PDF

Info

Publication number
CN105955272A
CN105955272A CN201610345903.2A CN201610345903A CN105955272A CN 105955272 A CN105955272 A CN 105955272A CN 201610345903 A CN201610345903 A CN 201610345903A CN 105955272 A CN105955272 A CN 105955272A
Authority
CN
China
Prior art keywords
robot
barrier
axis
coordinate system
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.)
Granted
Application number
CN201610345903.2A
Other languages
English (en)
Other versions
CN105955272B (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.)
Shanghai TIMI robot Co.,Ltd.
Original Assignee
Shanghai Titanium Robot Technology 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 Shanghai Titanium Robot Technology Co Ltd filed Critical Shanghai Titanium Robot Technology Co Ltd
Priority to CN201610345903.2A priority Critical patent/CN105955272B/zh
Publication of CN105955272A publication Critical patent/CN105955272A/zh
Application granted granted Critical
Publication of CN105955272B publication Critical patent/CN105955272B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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 (3)

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，对转换后的障碍物数据做阈值处理，高于阈值的判定为障碍物。
2.根据权利要求1所述的服务机器人多飞行时间传感器的融合方法，其特征在于，步骤5包括：
步骤5.1，将已经变换到机器人坐标系下的障碍物数据压缩到2维；
步骤5.2，将压缩的障碍物数据转换到极坐标系下；
步骤5.3，对转换到极坐标系下的障碍物数据进行离散化，得到对应的2维栅格；其中，2维栅格是以机器人的中心为原点，以theta作为横坐标轴，以distance作为纵坐标轴，横坐标轴与纵坐标轴垂直；
步骤5.3，对离散化后的障碍物数据进行高斯模糊处理，并将落在同一栅格上的障碍物数据叠加。
3.根据权利要求1所述的服务机器人多飞行时间传感器的融合方法，其特征在于，传感器为飞行时间传感器。
CN201610345903.2A 2016-05-23 2016-05-23 服务机器人多飞行时间传感器的融合方法 Active CN105955272B (zh)

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 true CN105955272A (zh) 2016-09-21
CN105955272B 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)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108814452A (zh) * 2018-08-22 2018-11-16 上海炬佑智能科技有限公司 扫地机器人及其障碍检测方法
WO2022179270A1 (zh) * 2021-02-23 2022-09-01 京东科技信息技术有限公司 机器人行驶方法、装置及电子设备、存储介质及程序产品

Citations (5)

* Cited by examiner, † Cited by third party
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 日本電産エレシス株式会社 車両用外界センシング装置、その軸ズレ補正プログラム及びその軸ズレ補正方法

Patent Citations (5)

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

* Cited by examiner, † Cited by third party
Title
丁伟 等: "基于多传感器信息融合的移动机器人导航综述", 《传感器与微系统》 *
司现军 等: "移动机器人多传感器信息融合技术综述", 《机电工程》 *
赵小川 等: "机器人多传感器信息融合研究综述", 《机器人多传感器信息融合研究综述》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108814452A (zh) * 2018-08-22 2018-11-16 上海炬佑智能科技有限公司 扫地机器人及其障碍检测方法
WO2022179270A1 (zh) * 2021-02-23 2022-09-01 京东科技信息技术有限公司 机器人行驶方法、装置及电子设备、存储介质及程序产品

Also Published As

Publication number Publication date
CN105955272B (zh) 2019-07-26

Similar Documents

Publication Publication Date Title
CN105955258B (zh) 基于Kinect传感器信息融合的机器人全局栅格地图构建方法
JP6760114B2 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
CN107991680B (zh) 动态环境下基于激光雷达的slam方法
CN105874350B (zh) 校准装置、校准方法及校准程序
CN107808123B (zh) 图像可行域检测方法、电子设备、存储介质、检测系统
CN109584238B (zh) 一种基于立体视觉的弓网运行状况在线检测系统及方法
CN109144072A (zh) 一种基于三维激光的机器人智能避障方法
CN103424112B (zh) 一种基于激光平面辅助的运动载体视觉导航方法
CN105547635B (zh) 一种用于风洞试验的非接触式结构动力响应测量方法
CN103605978A (zh) 基于三维实景数据的城市违章建筑识别系统及方法
CN102788572B (zh) 一种工程机械吊钩姿态的测量方法、装置及系统
CN108733053A (zh) 一种基于机器人的智能道路检测方法
CN105260988A (zh) 一种高精地图数据的处理方法和装置
CN103472823A (zh) 一种智能机器人用的栅格地图创建方法
US20130272578A1 (en) Information processing apparatus, information processing method, and program
CN114444158B (zh) 一种基于三维重建的地下巷道形变预警方法及系统
JP6465421B1 (ja) 構造物変状検出装置
CN104359415B (zh) 一种水火弯板角变形量测量方法及系统
CN108535789A (zh) 一种基于机场跑道的异物识别系统
CN105324729A (zh) 用于模型化车辆的周围环境的方法
CN106447698B (zh) 一种基于距离传感器的多行人跟踪方法和系统
CN105571636A (zh) 一种用于定位目标的方法及测量设备
CN111308495B (zh) 一种雷达测距生成室内户型3d数据的方法
CN112455502A (zh) 基于激光雷达的列车定位方法及装置
US20090226094A1 (en) Image correcting device and method, and computer program

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

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.

CP01 Change in the name or title of a patent holder