服务机器人多飞行时间传感器的融合方法
【专利摘要】本发明提供一种服务机器人多飞行时间传感器的融合方法，包括：步骤1，获取机器人的位置缓存o＝{pos，t}；步骤2，获取传感器检测到的障碍物数据z＝{d，r，var，t，h}；步骤3，将障碍物数据转换到机器人坐标系；步骤4，根据机器人的位置缓存，将障碍物数据中的障碍物距离d转换到机器人坐标系下；步骤5，将已经变换到机器人坐标系下的障碍物数据进行转换，形成的栅格；步骤6，对转换后的障碍物数据做阈值处理，高于阈值的判定为障碍物。本发明具有以下优点：一种统一的融合框架，适用于所有ToF传感器2)由于通过高斯模糊和不同传感器的不确定性融合，提高了最终障碍物测量的确定性。
【专利说明】
服务机器人多飞行时间传感器的融合方法
技术领域
[0001]本发明涉及机械领域，具体说是一种服务机器人多飞行时间传感器的融合方法。 【背景技术】
[0002]移动服务机器人为了能在动态不确定环境中实现自主导航任务，需要使用基于飞行时间(ToF)的传感器实时检测周围的障碍物。目前大多数服务机器人直接采用扇面激光雷达，或利用人工规则简单融合超声传感器。为简化后期机器人行为规划层的处理逻辑，这里提出一种通用的传感器融合技术用于融合多种ToF传感器。
【发明内容】
[0003]针对现有技术中的缺陷，本发明目的在于提供一种提高融合后障碍物测量的确定性的服务机器人多飞行时间传感器的融合方法。
[0004]为解决上述技术问题，本发明提供一种服务机器人多飞行时间传感器的融合方法，包括:
[0005]步骤1，获取机器人的位置缓存〇={p〇S，t};其中，pos为机器人在全局坐标中的位置和旋转，t为获得该位置的时间；
[0006]步骤2,获取传感器检测到的障碍物数据2={(14，似^11};其中，(1为检测到的障碍物距离，r为相对于机器人朝向的角度，var为距离测量的不确定性，t为测量障碍物的时间，h表示障碍物的高度；
[0007]步骤3,将障碍物数据转换到机器人坐标系，其中，机器人坐标系是以机器人的中心为原点，以机器人的朝向为X轴，Y轴垂直于X轴，Z轴垂直于X轴和Y轴所在平面，X轴和Y轴所在平面与地面平行；
[0008]步骤4,根据机器人的位置缓存，将障碍物数据中的障碍物距离d转换到机器人坐标系下；
[0009]pi = inv(Tl)*T0*p
[0010]其中，p表示传感器获取障碍物数据时机器人的位置，P1为转换到机器人坐标系后的位置，inv(Tl)表示T1的反变换矩阵，T0为p对应的齐次变换矩阵为，T1为P1对应的齐次变换矩阵；
[0011]步骤5,将已经变换到机器人坐标系下的障碍物数据进行转换，形成的栅格；
[0012]步骤6,对转换后的障碍物数据做阈值处理，高于阈值的判定为障碍物。
[0013]优选地，步骤5包括:
[0014]步骤5.1，将已经变换到机器人坐标系下的障碍物数据压缩到2维；
[0015]步骤5.2,将压缩的障碍物数据转换到极坐标系下；
[0016]步骤5.3,对转换到极坐标系下的障碍物数据进行离散化，得到对应的2维栅格;其中，2维栅格是以机器人的中心为原点，以theta作为横坐标轴，以distance作为纵坐标轴， 横坐标轴与纵坐标轴垂直；
[0017]步骤5.3,对离散化后的障碍物数据进行高斯模糊处理，并将落在同一栅格上的障碍物数据叠加。
[0018] 优选地，传感器为飞行时间传感器。
[0019]与现有技术相比，本发明具有以下优点:一种统一的融合框架，适用于所有ToF传感器2)由于通过高斯模糊和不同传感器的不确定性融合，提高了最终障碍物测量的确定性。3)由于通过机器人位置历史缓存，可将历史ToF数据保留并融合到当前机器人位置，可有效解决传感器盲区问题。【附图说明】
[0020]通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征.目的和优点将会变得更明显。
[0021]图1为本发明服务机器人多飞行时间传感器的融合方法障碍物数据从极坐标系转换到2维栅格示意图。【具体实施方式】[〇〇22]下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改。
[0023]如图1所示，本发明提供一种服务机器人多飞行时间传感器的融合方法，包括:
[0024]步骤1，获取机器人的位置缓存〇={P〇S，t};其中，pos为机器人在全局坐标中的位置和旋转，t为获得该位置的时间；
[0025]步骤2,获取传感器检测到的障碍物数据2={(14，似^11};其中，(1为检测到的障碍物距离，r为相对于机器人朝向的角度，var为距离测量的不确定性，t为测量障碍物的时间，h表示障碍物的高度；
[0026]步骤3,将障碍物数据转换到机器人坐标系，其中，机器人坐标系是以机器人的中心为原点，以机器人的朝向为X轴，Y轴垂直于X轴，Z轴垂直于X轴和Y轴所在平面，X轴和Y轴所在平面与地面平行；[〇〇27]步骤4,根据机器人的位置缓存，将障碍物数据中的障碍物距离d转换到机器人坐标系下；
[0028]pi = inv(Tl)*T0*p
[0029]其中，p表示传感器获取障碍物数据时机器人的位置，P1为转换到机器人坐标系后的位置，inv(Tl)表示T1的反变换矩阵，T0为p对应的齐次变换矩阵为，T1为P1对应的齐次变换矩阵。
[0030]步骤5,将已经变换到机器人坐标系下的障碍物数据进行转换，形成的栅格；
[0031]步骤6,对转换后的障碍物数据做阈值处理，高于阈值的判定为障碍物。[〇〇32] 步骤5包括:[〇〇33]步骤5.1，将已经变换到机器人坐标系下的障碍物数据压缩到2维；
[0034]步骤5.2，将压缩的障碍物数据转换到极坐标系下；
[0035]步骤5.3，对转换到极坐标系下的障碍物数据进行离散化，得到对应的2维栅格;其中，2维栅格是以机器人的中心为原点，以theta作为横坐标轴，以distance作为纵坐标轴， 横坐标轴与纵坐标轴垂直；[〇〇36]步骤5.3,对离散化后的障碍物数据进行高斯模糊处理，并将落在同一栅格上的障碍物数据叠加。[〇〇37] 传感器为飞行时间传感器[〇〇38] ToF传感器可用一序列点z = {d，r，var，t，h}表示。其中d为检测到的障碍物距离，r 为相对于机器人朝向的角度，var表示距离测量的不确定性，t表示该点测量的时间，h表示该点的高度。对于单点超声传感器，每次测量即为数组大小为1的点序列。
[0039]建立一个机器人位置的缓存，该缓存中机器人位置可以由机器人里程计得到，或由基于地图匹配的定位信息得到。该缓存中每个元素为〇={p〇s，t}其中pos为机器人的位置和旋转，t为该位置获得的时间。在得到机器人所有ToF传感器数据后，将所有ToF测得的障碍物数据z转换到笛卡尔坐标系P，再根据机器人位置缓存，将每个ToF的距离数据转换到机器人当前坐标系下。假设ToF数据获得时的机器人位置为〇0,对应的其次变换矩阵为T0， 机器人当前位置为〇1，对于的齐次变换矩阵为T1。则转换到当前机器人坐标系下的ToF坐标为:
[0040]pi = inv(Tl)*T0*p
[0041]首先将机器人2D极坐标系局以delta theta和delta distance分成2D栅格。然后， 将所有变换到当前机器人坐标系下的ToF传感器数据压缩到2D，转换到极坐标系后再离散化到对应的栅格。对每个离散化后的数据点根据该传感器的不确定性，对其进行高斯模糊处理，并将落在同一栅格的数据叠加。[〇〇42]在完成所有ToF数据的处理后，对最终形成的栅格，做阈值处理，高于阈值的便为障碍物。[〇〇43]以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。
【主权项】
1.一种服务机器人多飞行时间传感器的融合方法，其特征在于，包括如下步骤:步骤1，获取机器人的位置缓存〇={p〇s，t};其中，pos为机器人在全局坐标中的位置和 旋转，t为获得该位置的时间；步骤2,获取传感器检测到的障碍物数据2 = {(1^，^^11};其中，(1为检测到的障碍物 距离，r为相对于机器人朝向的角度，var为距离测量的不确定性，t为测量障碍物的时间，h 表示障碍物的高度；步骤3,将障碍物数据转换到机器人坐标系，其中，机器人坐标系是以机器人的中心为 原点，以机器人的朝向为X轴，Y轴垂直于X轴，Z轴垂直于X轴和Y轴所在平面，X轴和Y轴所在 平面与地面平行；步骤4,根据机器人的位置缓存，将障碍物数据中的障碍物距离d转换到机器人坐标系 下；Pl = inv(Tl)*TO*p其中，P表示传感器获取障碍物数据时机器人的位置，P1为转换到机器人坐标系后的位 置，inv(Tl)表示T1的反变换矩阵，TO为p对应的齐次变换矩阵为，T1为P1对应的齐次变换矩 阵；步骤5，将已经变换到机器人坐标系下的障碍物数据进行转换，形成的栅格；步骤6,对转换后的障碍物数据做阈值处理，高于阈值的判定为障碍物。2.根据权利要求1所述的服务机器人多飞行时间传感器的融合方法，其特征在于，步骤 5包括:步骤5.1，将已经变换到机器人坐标系下的障碍物数据压缩到2维；步骤5.2，将压缩的障碍物数据转换到极坐标系下；步骤5.3,对转换到极坐标系下的障碍物数据进行离散化，得到对应的2维栅格;其中，2 维栅格是以机器人的中心为原点，以theta作为横坐标轴，以distance作为纵坐标轴，横坐 标轴与纵坐标轴垂直；步骤5.3,对离散化后的障碍物数据进行高斯模糊处理，并将落在同一栅格上的障碍物 数据叠加。3.根据权利要求1所述的服务机器人多飞行时间传感器的融合方法，其特征在于，传感 器为飞行时间传感器。
【文档编号】G01S15/93GK105955272SQ201610345903
【公开日】2016年9月21日
【申请日】2016年5月23日
【发明人】潘晶, 冯义兴, 沈满, 苏至钒, 刘亮, 许涛
【申请人】上海钛米机器人科技有限公司