CN115503850B - 足式机器人足端触地检测方法及系统 - Google Patents

Abstract

本发明提供了一种足式机器人足端触地检测方法及系统，属于足式机器人技术领域。所述检测方法，包括：根据预处理后的机器人行走前方点云数据进行落足点选择，得到落足点位置和摆动相足端轨迹的规划结果；根据落足点位置和摆动相足端轨迹的规划结果，基于关节力矩进行触地检测，判断是否为提前触地、按预期触地和延迟触地中的一种，如是，则判定为触地，足式机器人切换为支撑相；如否，则判定为踏空，切换至四足支撑；本发明提升了足端触地检测准确度。

Description

足式机器人足端触地检测方法及系统

技术领域

本发明涉及足式机器人技术领域，特别涉及一种足式机器人足端触地检测方法及系统。

背景技术

本部分的陈述仅仅是提供了与本发明相关的背景技术，并不必然构成现有技术。

足式机器人腿部支撑相和摆动相通常采用不同的控制方法，因此准确的足端触地检测方法尤为关键，检测错误会造成踏空或磕绊，严重时可能导致机器人摔倒。

常规的足式机器人足端触地检测主要由以下几种：

（1）在足端安装触地开关，但由于脚与地面的冲击较强，该触地开关极易损坏，需要频繁更换；

（2）在小腿安装一维力传感器，这种方式解决了机械冲击容易损坏的问题，但需要小腿接近垂直入地，而当机器人越障时，小腿可能接近水平，这时一维力传感器在竖直方向检测不到触地力，会导致漏检测；

（3）在脚踝安装六维力传感器，这样可以解决越障时小腿水平触地检测问题，但六维力传感器较笨重，安装在足端会显著增加腿部转动惯量，影响高速奔跑；

（4）使用地形探测传感器，如双目相机、结构光相机、TOF传感器等，扫描地形轮廓，但当地面有杂草、塑料袋等物体时，地面物体虽有轮廓却无法提供足够的支撑力，若只依赖地形探测传感器，可能导致踏空。

发明内容

为了解决现有技术的不足，本发明提供了一种足式机器人足端触地检测方法及系统，采用“外部传感器+内部传感器”相结合的方案，首先使用地形探测传感器扫描地形，预估落足点位置，然后采用关节力矩传感器推算足端受力情况，在腿下落时对提前触地、按预期触地、延迟触地和踏空进行动态判断，内部传感器和外部传感器相结合，提升了足端触地检测准确度。

为了实现上述目的，本发明采用如下技术方案：

本发明第一方面提供了一种足式机器人足端触地检测方法。

一种足式机器人足端触地检测方法，包括以下过程：

根据预处理后的机器人行走前方点云数据进行落足点选择，得到落足点位置和摆动相足端轨迹的规划结果；

根据落足点位置和摆动相足端轨迹的规划结果，基于关节力矩进行触地检测，判断是否为提前触地、按预期触地和延迟触地中的一种，如是，则判定为触地，足式机器人切换为支撑相；如否，则判定为踏空，切换至四足支撑。

作为本发明第一方面可选的一种实现方式，基于关节力矩进行触地检测，判断是否提前触地，如是，则判定为触地，足式机器人切换为支撑相；如否，继续执行触地检测；

判断是否按预期触地，如是，则判定为触地，足式机器人切换为支撑相；如否，足部继续下探设定距离后继续进行触地检测；

判断是否延迟触地，如是，则判定为触地，足式机器人切换为支撑相；如否，则判定为踏空，切换至四足支撑。

作为本发明第一方面可选的一种实现方式，进行触地检测，包括：

在腿部摆动过程中，实时检测关节力矩，并根据关节力矩计算足端所受外力，当足端所受外力大于或等于第一预设阈值时判定为腿部触地。

作为本发明进一步的限定，根据关节力矩计算足端所受外力，包括：

其中，

为一条腿三个关节的角度，/>

为一条腿三个关节的角加速度，/>

为一条腿三个关节的角速度，M为质量矩阵，C为哥式力矩阵和向心力矩阵，G为重力矩阵，τ为关节力矩向量，J为单腿雅可比矩阵，F为足端所受外力。

作为本发明第一方面可选的一种实现方式，根据预处理后的点云数据进行落足点选择，包括：

对预处理后的点云数据进行平面分割，获得多个平面，对每个平面求取法向量，根据法向量计算每个平面的斜度系数；

若某一平面的斜度系数小于第二预设阈值，则判定该平面过于倾斜不适合踩踏，删除该平面对应的点云数据，得到斜度系数大于或等于第二预设阈值的剩余平面，基于剩余平面进行落足点选择。

本发明第二方面提供了一种足式机器人足端触地检测系统。

一种足式机器人足端触地检测系统，包括：

落足点选择模块，被配置为：根据预处理后的机器人行走前方点云数据进行落足点选择，得到落足点位置和摆动相足端轨迹的规划结果；

触地检测模块，被配置为：根据落足点位置和摆动相足端轨迹的规划结果，基于关节力矩进行触地检测，判断是否为提前触地、按预期触地和延迟触地中的一种，如是，则判定为触地，足式机器人切换为支撑相；如否，则判定为踏空，切换至四足支撑。

作为本发明第二方面可选的一种实现方式，进行触地检测，包括：

在腿部摆动过程中，实时检测关节力矩，并根据关节力矩计算足端所受外力，当足端所受外力大于或等于第一预设阈值时判定为腿部触地；

根据关节力矩计算足端所受外力，包括：

其中，

为一条腿三个关节的角度，/>

为一条腿三个关节的角加速度，/>

为一条腿三个关节的角速度，M为质量矩阵，C为哥式力矩阵和向心力矩阵，G为重力矩阵，τ为关节力矩向量，J为单腿雅可比矩阵，F为足端所受外力。

作为本发明第二方面可选的一种实现方式，根据预处理后的点云数据进行落足点选择，包括：

对预处理后的点云数据进行平面分割，获得多个平面，对每个平面求取法向量，根据法向量计算每个平面的斜度系数；

若某一平面的斜度系数小于第二预设阈值，则判定该平面过于倾斜不适合踩踏，删除该平面对应的点云数据，得到斜度系数大于或等于第二预设阈值的剩余平面，基于剩余平面进行落足点选择。

本发明第三方面提供了一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现如本发明第一方面所述的足式机器人足端触地检测方法中的步骤。

本发明第四方面提供了一种足式机器人，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明第一方面所述的足式机器人足端触地检测方法中的步骤。

与现有技术相比，本发明的有益效果是：

本发明创造性的提出了一种足式机器人足端触地检测方法，采用“外部传感器+内部传感器”的方案，首先使用地形探测传感器扫描地形，预估落足点位置，然后采用关节力矩传感器推算足端受力情况，在腿下落时动态判断是否提前触地、正常触地、延迟触地或踏空，提高了场景适应性，提升了足端触地检测准确度，避免了采用传统检测方式产生的误检问题，防止了机器人因踏空或磕绊导致倾倒，且小腿和脚部无需安装任何传感器，减轻了腿部重量和转动惯量，延长了设备寿命，提升了涉水通过能力。

本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

构成本发明的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。

图1为本发明实施例1提供的足式机器人足端触地检测方法的流程示意图；

图2为本发明实施例2提供的足式机器人足端触地检测系统的结构示意图。

具体实施方式

下面结合附图与实施例对本发明作进一步说明。

应该指出，以下详细说明都是示例性的，旨在对本发明提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本发明的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

实施例1：

如图1所示，本发明实施例1提供了一种足式机器人足端触地检测方法，包括以下过程：

S1：使用地形探测传感器扫描地形，根据机器人站立时的传感器安装高度和安装角度，标定基准水平地面坐标系，其中Z轴为竖直向上，高于水平地面的判定为凸障碍物，低于水平地面的判定为凹障碍物。

S2：使用滤波器对三维点云进行预处理，如使用统计滤波器或半径滤波器去除孤立点，使用体素滤波器降采样，使用直通滤波器切除非脚前方区域的点云。

S3：使用RANSAC方法对点云进行平面分割，获得多个平面，对每个平面求取法向量，得到法向量：

其中，i为1到N的整数，n _i 为第i个平面的法向量，x _i 、y _i 和z _i 为第i个平面的法向量n _i 的三维坐标。

S4：计算每个平面的斜度系数k _i ：

若k _i 小于某预设阈值（这里的预设阈值为根据历史数据得到的经验值，即第二预设阈值），则判定该平面过于倾斜不适合踩踏，将该平面对应的点云删除。

S5：基于剩余的平面进行落足点选择，将平面位置信息传入步态规划算法，用以规划落足点位置和摆动相足端轨迹（这里可以获取到预设的落足点和预计的落足时间）。该步态规划算法可采用已有算法或其他算法，这里不再赘述。

S6：在腿部摆动过程中，实时检测关节力矩，并用以下方程推算足端所受外力：

其中，

为一条腿三个关节的角度（为3×1矩阵），/>

为一条腿三个关节的角加速度（为3×1矩阵），/>

为一条腿三个关节的角速度（为3×1矩阵），M为质量矩阵（为3×3矩阵），C为哥式力矩阵和向心力矩阵（为3×3矩阵），G为重力矩阵（为3×1矩阵），τ为关节力矩向量（为3×1矩阵），J为单腿雅可比矩阵（为3×3矩阵），F为足端所受外力（为3×1矩阵，三个元素分别为x方向、y方向和z方向的力）。

由于建模误差和传感器噪声，F的估计值会有小范围波动，但与足端触地带来的支持力相距甚远，因此超过一个阈值即可判定为触地状态（这里的阈值为根据历史数据得到的经验值，即第一预设阈值）；

对于非足端触地的情况（如小腿磕到地面凸起处），该方法估计得到的F也会超出阈值，虽然此时足端悬空，估计的F有较大偏差，但由于腿部触地部位已提供了足够的支持力，因此也可判定为触地。

S7：根据前述方法控制摆动腿运动到期望落足点。若提前检测到触地，则判定为触地；若未按时检测到触地，则继续下探一段预设距离；若超过预设距离仍未检测到触地，则判定为踏空，强制切换到四条腿支撑的状态，努力找回平衡。

具体的，包括：

根据落足点位置和摆动相足端轨迹的规划结果，基于关节力矩进行触地检测，判断是否提前触地，如是，则判定为触地，足式机器人切换为支撑相（即将进行触地检测的足端切换为支撑相）；如否，继续执行触地检测；

基于关节力矩进行触地检测，判断是否按预期触地，如是，则判定为触地，足式机器人切换为支撑相（即需要检测是否触地的足端切换为支撑相）；如否，则继续进行触地检测；

足部继续下探设定距离，基于关节力矩进行触地检测，判断是否延迟触地，如是，则判定为触地，足式机器人切换为支撑相（即需要检测是否触地的足端切换为支撑相）；如否，则判定为踏空，切换至四足支撑。

实施例2：

如图2所示，本发明实施例2提供了一种足式机器人足端触地检测系统，包括：

落足点选择模块，被配置为：根据预处理后的机器人行走前方点云数据进行落足点选择，得到落足点位置和摆动相足端轨迹的规划结果；

触地检测模块，被配置为：根据落足点位置和摆动相足端轨迹的规划结果，基于关节力矩进行触地检测，判断是否为提前触地、按预期触地和延迟触地中的一种，如是，则判定为触地，足式机器人切换为支撑相（即将进行触地检测的足端切换为支撑相）；如否，则判定为踏空，切换至四足支撑。

落足点选择模块，具体的，包括：

1）使用地形探测传感器扫描地形，根据机器人站立时的传感器安装高度和安装角度，标定基准水平地面坐标系，其中Z轴为竖直向上，高于水平地面的判定为凸障碍物，低于水平地面的判定为凹障碍物。

2）使用滤波器对三维点云进行预处理，如使用统计滤波器或半径滤波器去除孤立点，使用体素滤波器降采样，使用直通滤波器切除非脚前方区域的点云；

3）使用RANSAC方法对点云进行平面分割，获得多个平面，对每个平面求取法向量，得到法向量：

其中，i为1到N的整数，n _i 为第i个平面的法向量，x _i 、y _i 和z _i 为第i个平面的法向量n _i 的三维坐标。

4）计算每个平面的斜度系数：

若k _i 小于某预设阈值（这里的预设阈值为根据历史数据得到的经验值，即第二预设阈值），则判定该平面过于倾斜不适合踩踏，将该平面对应的点云删除；

5）基于剩余的平面进行落足点选择，将平面位置信息传入步态规划算法，用以规划落足点位置和摆动相足端轨迹，该步态规划算法可采用已有算法或其他算法，这里不再赘述。

触地检测模块中，基于关节力矩进行触地检测，包括：

在腿部摆动过程中，实时检测关节力矩，并用以下方程推算足端所受外力：

其中，

为一条腿三个关节的角度（为3×1矩阵），/>

为一条腿三个关节的角加速度（为3×1矩阵），/>

为一条腿三个关节的角速度（为3×1矩阵），M为质量矩阵（为3×3矩阵），C为哥式力矩阵和向心力矩阵（为3×3矩阵），G为重力矩阵（为3×1矩阵），τ为关节力矩向量（为3×1矩阵），J为单腿雅可比矩阵（为3×3矩阵），F为足端所受外力（为3×1矩阵，三个元素分别为x方向、y方向和z方向的力）。

由于建模误差和传感器噪声，F的估计值会有小范围波动，但与足端触地带来的支持力相距甚远，因此超过一个阈值即可判定为触地状态（这里的阈值为根据历史数据得到的经验值，即第一预设阈值）；

对于非足端触地的情况（如小腿磕到地面凸起处），该方法估计得到的F也会超出阈值，虽然此时足端悬空，估计的F有较大偏差，但由于腿部触地部位已提供了足够的支持力，因此也可判定为触地。

实施例3：

本发明实施例3提供了一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现如本发明实施例1所述的足式机器人足端触地检测方法中的步骤。

实施例4：

本发明实施例4提供了一种足式机器人，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明实施例1所述的足式机器人足端触地检测方法中的步骤。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用硬件实施例、软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器和光学存储器等）上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备（系统）、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体（Read-Only Memory，ROM）或随机存储记忆体（Random AccessMemory，RAM）等。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.一种足式机器人足端触地检测方法，其特征在于，包括以下过程：

根据预处理后的机器人行走前方点云数据进行落足点选择，得到落足点位置和摆动相足端轨迹的规划结果；

根据落足点位置和摆动相足端轨迹的规划结果，采用外部传感器与内部传感器相结合，基于关节力矩进行触地检测，判断是否为提前触地、按预期触地和延迟触地中的一种，如是，则判定为触地，足式机器人切换为支撑相；如否，则判定为踏空，切换至四足支撑；

基于关节力矩进行触地检测，判断是否提前触地，如是，则判定为触地，足式机器人切换为支撑相；如否，继续执行触地检测；

判断是否按预期触地，如是，则判定为触地，足式机器人切换为支撑相；如否，足部继续下探设定距离后继续进行触地检测；

判断是否延迟触地，如是，则判定为触地，足式机器人切换为支撑相；如否，则判定为踏空，切换至四足支撑；

根据预处理后的点云数据进行落足点选择，包括：

对预处理后的点云数据进行平面分割，获得多个平面，对每个平面求取法向量，根据法向量计算每个平面的斜度系数；

若某一平面的斜度系数小于第二预设阈值，则判定该平面过于倾斜不适合踩踏，删除该平面对应的点云数据，得到斜度系数大于或等于第二预设阈值的剩余平面，基于剩余平面进行落足点选择。

2.如权利要求1所述的足式机器人足端触地检测方法，其特征在于，

进行触地检测，包括：

在腿部摆动过程中，实时检测关节力矩，并根据关节力矩计算足端所受外力，当足端所受外力大于或等于第一预设阈值时判定为腿部触地。

3.如权利要求2所述的足式机器人足端触地检测方法，其特征在于，

根据关节力矩计算足端所受外力，包括：

其中，q为一条腿三个关节的角度，

为一条腿三个关节的角加速度，/>

为一条腿三个关节的角速度，M为质量矩阵，C为哥式力矩阵和向心力矩阵，G为重力矩阵，τ为关节力矩向量，J为单腿雅可比矩阵，F为足端所受外力。

4.一种足式机器人足端触地检测系统，其特征在于，包括：

落足点选择模块，被配置为：根据预处理后的机器人行走前方点云数据进行落足点选择，得到落足点位置和摆动相足端轨迹的规划结果；

触地检测模块，被配置为：根据落足点位置和摆动相足端轨迹的规划结果，基于关节力矩进行触地检测，判断是否为提前触地、按预期触地和延迟触地中的一种，如是，则判定为触地，足式机器人切换为支撑相；如否，则判定为踏空，切换至四足支撑；

基于关节力矩进行触地检测，判断是否提前触地，如是，则判定为触地，足式机器人切换为支撑相；如否，继续执行触地检测；

判断是否按预期触地，如是，则判定为触地，足式机器人切换为支撑相；如否，足部继续下探设定距离后继续进行触地检测；

判断是否延迟触地，如是，则判定为触地，足式机器人切换为支撑相；如否，则判定为踏空，切换至四足支撑；

根据预处理后的点云数据进行落足点选择，包括：

对预处理后的点云数据进行平面分割，获得多个平面，对每个平面求取法向量，根据法向量计算每个平面的斜度系数；

若某一平面的斜度系数小于第二预设阈值，则判定该平面过于倾斜不适合踩踏，删除该平面对应的点云数据，得到斜度系数大于或等于第二预设阈值的剩余平面，基于剩余平面进行落足点选择。

5.如权利要求4所述的足式机器人足端触地检测系统，其特征在于，

进行触地检测，包括：

在腿部摆动过程中，实时检测关节力矩，并根据关节力矩计算足端所受外力，当足端所受外力大于或等于第一预设阈值时判定为腿部触地；

根据关节力矩计算足端所受外力，包括：

其中，q为一条腿三个关节的角度，

为一条腿三个关节的角加速度，/>

为一条腿三个关节的角速度，M为质量矩阵，C为哥式力矩阵和向心力矩阵，G为重力矩阵，τ为关节力矩向量，J为单腿雅可比矩阵，F为足端所受外力。

6.一种计算机可读存储介质，其上存储有程序，其特征在于，该程序被处理器执行时实现如权利要求1-3任一项所述的足式机器人足端触地检测方法中的步骤。

7.一种足式机器人，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-3任一项所述的足式机器人足端触地检测方法中的步骤。

Images