步态自动切换的足式机器人落足控制方法及系统

技术领域

本发明涉及足式机器人技术领域，特别涉及一种步态自动切换的足式机器人落足控制方法及系统。

背景技术

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

现有足式机器人本体控制系统(如图1中的虚线框所示)，通过将期望输入(躯干位姿、步态、足端轨迹、落足点等)输入控制器，控制器再结合状态估计器依据当前机器人躯干姿态、步态、腿部关节电机位置/速度/力矩数据得到的本体三维空间姿态、速度、足部是否触地等状态信息，利用虚拟模型控制(VMC)、模型预测控制(MPC)等控制算法，解算机器人腿部关节电机输出并由各关节电机驱动器执行，实现对期望输入的跟踪，完成复杂地面条件下的运动。当机器人运动异常时，通过手动急停输入切断电机驱动器对关节电机的控制，实现对机器人的保护。

发明人发现，目前的足式机器人行走控制存在如下问题：

(1)由于环境的复杂性，受限于目前机器人控制系统未将各种环境因素引入控制流程，这也导致机器人在面临连续不规则台阶、地形突变等复杂情况时，适应能力不足，影响本体及其上安装设备的安全；

(2)常规的足端触地检测方式包括在足端安装触地开关和在小腿安装一维力传感器，由于脚与地面的冲击较强，触地开关极易损坏，需要频繁更换，小腿的一维力传感器解决了机械冲击容易损坏的问题，但需要小腿接近垂直入地，而当机器人越障时，小腿可能接近水平，这时一维力传感器在竖直方向检测不到触地力，会导致漏检测。

发明内容

为了解决现有技术的不足，本发明通过对平整路面以及砂石、草地、台阶等路面环境下机器人本体状态与姿态数据的采集以及机器人摔倒对本体及设备防护需求分析，在人工设定的防护规则基础上，提出了一种步态自动切换的足式机器人落足控制方法及系统，实现了对机器人足端地面环境的主动探测以及基于预设规则的步态及落足点自适应切换和修正。

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

本发明第一方面提供了一种步态自动切换的足式机器人落足控制方法。

一种步态自动切换的足式机器人落足控制方法，包括以下过程：

根据机器人行进前方图像数据，对机器人行进前方环境进行分类，得到不同路况区域对应的点云数据；

根据各路况区域内的点云法向量进行聚类，并计算聚类区域的高程信息，将带有高程信息的点云数据存入点云特征数据队列；

将点云特征队列中的点云数据进行迭代匹配合并，得到机器人足端周边局部环境特征地图，通过对局部地图中不同平面区域法线进行迭代计算，得到落足点。

作为本发明第一方面可选的一种实现方式，基于预先标定的图像数据与点云数据的对应关系，得到各路况区域对应的点云数据，提取各区域的点云法向量；

将各路况区域内法向量一致或者差异在设定范围内的点云进行聚类，并计算聚类区域的高程信息，将带有高程信息的点云数据存入点云特征数据队列。

作为本发明第一方面进一步的限定，基于RANSAC算法提取局部环境特征地图中的平面区域，通过对局部地图中不同区域的法线进行迭代计算，对各平面内法线一致性进行评价；

若某一平面区域的法线方向垂直或倾斜角度在设定范围内，则此平面区域能够作为机器人行走备选落足点使用，对相应的点云进行标记。

作为本发明第一方面更进一步的限定，通过将局部地图点云投影至平面栅格，并将栅格所包含点的高度均值填入栅格，得到可用于落足的地面网格数据。

作为本发明第一方面可选的一种实现方式，若底面存在障碍物，且障碍物尺寸大于设定值，则其表面可能存在可落足点，进行障碍物上的落足点选取，包括：

若前方有斜坡但倾角小于第一设定阈值，则能够进行落足；如前方有石子路，倾角大于第一设定阈值且变化范围大于第二设定阈值，则不能够进行落足；如前方有石子路，倾角小于第一设定阈值且变化范围小于第二设定阈值，则能够进行落足。

作为本发明第一方面可选的一种实现方式，根据识别到的不同路况区域，调整机器人足部腾空时间与地面接触时间占比、足底离地高度以及步长，配置适用对应环境的步态规则参数。

作为本发明第一方面可选的一种实现方式，依据当前感知环境特征，并与已知环境特征进行匹配，动态选择当前环境的步态。

作为本发明第一方面可选的一种实现方式，依据局部环境特征地图，依据与期望落足点最近及落足后反作用力最小原则，根据对当前步态期望落足点人工设定邻域内可落足区域的评价，对当前步态各足端落足点进行选择，对控制器步态输入量进行动态修正。

本发明第二方面提供了一种步态自动切换的足式机器人落足控制系统。

一种步态自动切换的足式机器人落足控制系统，包括：

区域划分模块，被配置为：根据机器人行进前方图像数据，对机器人行进前方环境进行分类，得到不同路况区域对应的点云数据；

点云聚类模块，被配置为：根据各路况区域内的点云法向量进行聚类，并计算聚类区域的高程信息，将带有高程信息的点云数据存入点云特征数据队列；

落足点获取模块，被配置为：将点云特征队列中的点云数据进行迭代匹配合并，得到机器人足端周边局部环境特征地图，通过对局部地图中不同平面区域法线进行迭代计算，得到落足点。

本发明第三方面提供了一种步态自动切换的足式机器人足端触地检测方法。

一种步态自动切换的足式机器人足端触地检测方法，包括以下过程：

利用本发明第一方面所述的步态自动切换的足式机器人落足控制方法得到落足点位置，基于落足点位置得到摆动相足端轨迹的规划结果；

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

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

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

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

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

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

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

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

本发明第四方面提供了一种步态自动切换的足式机器人足端触地检测系统。

一种步态自动切换的足式机器人足端触地检测系统，包括：

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

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

本发明第五方面一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现如本发明第一方面所述的步态自动切换的足式机器人落足控制方法或者本发明第三方面所述的步态自动切换的足式机器人足端触地检测方法中的步骤。

本发明第六方面提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明第一方面所述的步态自动切换的足式机器人落足控制方法或者本发明第三方面所述的步态自动切换的足式机器人足端触地检测方法中的步骤。

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

1、本发明创新的提出了一种步态自动切换的足式机器人落足控制方法，通过对平整路面以及砂石、草地、台阶等路面环境下机器人本体状态与姿态数据的采集以及机器人摔倒对本体及设备防护需求分析，在人工设定的防护规则基础上，实现了对机器人足端地面环境的主动探测以及基于预设规则的步态及落足点自适应切换和修正；在原有机器人本体控制系统外层增加了基于环境感知的落足点与步态反馈控制回路，通过对机器人不同运行路面环境步态的主动切换及落足点修正，提升了机器人复杂环境中运行的稳定性和安全性。

2、本发明创造性的提出了一种步态自动切换的足式机器人足端触地检测方法，采用“外部传感器+内部传感器”的方案，避免了以往各种触地检测方式的缺陷，且小腿和脚部无需安装任何传感器，减轻了腿部重量和转动惯量，延长了设备寿命，同时提升了涉水通过能力。

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

附图说明

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

图1为本发明实施例1提供的步态自动切换的足式机器人落足控制方法的原理示意图。

具体实施方式

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

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

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

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

实施例1：

如图1所示，本发明实施例1提供了一种步态自动切换的足式机器人落足控制方法，包括以下过程：

S1：环境感知

主要由机器人前部安装的固态激光传感器、双目相机及3D激光雷达组成，通过不同传感器的组合降低机器人对环境探测盲区，实现了机器人对近中远不同距离环境信息的有效采集。

S2：特征识别

基于环境感知模块输出的当前环境图像、点云等信息，通过特征识别分类算法，提取当前机器人所处环境特征(表面纹理、法向量、高程等)，实现对机器人当前环境特征的分类，同时构建机器人足端局部特征地图，为后续落足及步态控制提供依据。

S2.1：获取机器人行进前方图像数据，利用LBP方法(Local Binary Patterns)提取机器人前方纹理特征，之后利用卷积神经网络，对机器人前方环境进行分类，获取可能存在的平坦路面、台阶、水面等区域信息。

S2.2：基于预先标定的图像与点云区域对应关系，获取上述步骤中在的平坦路面、台阶、水面等区域点云，利用PCL点云库(Point Cloud Library)中的法向量提取函数，提取区域内各点法向量。

S2.3：利用K-Means聚类算法，将法向量一致区域点云进行聚类，并计算聚类区域的高程信息，实现了对视觉提取区域的量化处理，将带有高程信息的点云数据压入固定长度特征数据队列。

S2.4：在机器人行进过程中，利用ICP算法(Iterative Closest Point)，将点云特征队列中的点云数据进行迭代匹配合并，实现对机器人运行足端周边局部环境特征地图的构建，从而为后续落足及步态控制提供依据。

S3：落足控制

基于前置下倾固态激光传感器实时扫描足式移动底盘足底地面信息，回传给足式移动底盘步态控制模块，从而调整足式移动底盘落足点，避免踏空或踏上障碍物。

S3.1：利用平面提取算法提取局部环境特征地图中的平面区域，本实施例基于RANSAC算法(Random Sample Consensus)实现，通过对局部地图中不同区域法线进行迭代计算，对平面内法线一致性进行评价，若法线方向垂直或倾斜角度在设定范围内，说明平面倾斜不大，可以作为机器人行走备选落足点使用，并在点云内部进行标记处理；

S3.2：之后，通过将局部地图点云投影至平面栅格，并将栅格包含点的高度均值填入栅格，从而形成可用于落足的地面网格数据。

针对地面上存在障碍物的情况，若障碍物尺寸较大则其表面可能存在可落足点，另外通过可供调整的阈值进行落足点提取：

如前方有斜坡但倾角小于第一设定阈值，则可以进行落足；

如前方有石子路，倾角大于第一设定阈值且变化大于第二设定阈值，则不可以落足；

如前方有石子路，倾角小于第一设定阈值且变化小于第二设定阈值，则可提供落足，此时主要依靠机器人平衡稳定控制算法保持稳定，选择落足点无法确定石子某个部位是否踏上去保证不失稳。

本实施例中，落足区域的倾角不能太大，另外该区域内倾角变化不能太剧烈，倾角变化通过计算栅格间的高度变化程度进行衡量的，如果超过设定阈值则认为变化剧烈，不适合落足

S4：步态控制

该模块可接受人工配置的不同环境下步态规则，基于机器人本体状态及环境特征信息选择适合当前环境的安全步态，之后在当前足端邻域内搜索落足点控制模块输出的可落足区域，结合期望步态信息，实现对控制器步态输入量的动态修正，保证机器人复杂环境下的稳定运行。

S4.1：依据不同环境特征(平地、台阶、水面等)，人为调整机器人足部腾空时间与地面接触时间占比、足底离地高度、步长等变量(通过人工在相关环境中进行测试后得到的一种经验值，后续会对落足点进行选择从而实现步态的修正)，配置适用对应环境的步态规则参数。

S4.2：依据当前感知环境特征，并于已知环境特征进行匹配，动态选择当前环境的步态(通过视觉识别出地面环境特征，然后依据特征选择上述设置步态)。

S4.3：依据建立的局部环境地图，依据与期望落足点最近及落足后反作用力最小原则，通过对当前步态期望落足点人工设定邻域内可落足区域的评价，对当前步态各足端落足点进行选择，从而实现对控制器步态输入量的动态修正。

局部环境地图中所有可落足栅格集合为：

P＝{P

P

实施例2：

本发明实施例2提供了一种步态自动切换的足式机器人落足控制系统，包括：

区域划分模块，被配置为：根据机器人行进前方图像数据，对机器人行进前方环境进行分类，得到不同路况区域对应的点云数据；

点云聚类模块，被配置为：根据各路况区域内的点云法向量进行聚类，并计算聚类区域的高程信息，将带有高程信息的点云数据存入点云特征数据队列；

落足点获取模块，被配置为：将点云特征队列中的点云数据进行迭代匹配合并，得到机器人足端周边局部环境特征地图，通过对局部地图中不同平面区域法线进行迭代计算，得到落足点。

所述系统的工作方法与实施例1提供的步态自动切换的足式机器人落足控制方法相同，这里不再赘述。

实施例3：

本发明实施例3提供了一种步态自动切换的足式机器人足端触地检测方法，包括以下过程：

利用本发明实施例1所述的步态自动切换的足式机器人落足控制方法得到落足点位置，基于落足点位置得到摆动相足端轨迹的规划结果；

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

基于关节力矩进行触地检测，判断是否按预期触地，如是，则判定为触地，足式机器人切换为支撑相；如否，则执行下一步；

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

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

其中，q为一条腿三个关节的角度(为3×1矩阵)，

由于建模误差和传感器噪声，F的估计值会有小范围波动，但与足端触地带来的支持力相距甚远，因此超过一个阈值即可判定为触地状态；

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

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

实施例4：

本发明实施例4提供了一种步态自动切换的足式机器人足端触地检测系统，包括：

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

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

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

其中，q为一条腿三个关节的角度(为3×1矩阵)，

由于建模误差和传感器噪声，F的估计值会有小范围波动，但与足端触地带来的支持力相距甚远，因此超过一个阈值即可判定为触地状态；

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

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

实施例5：

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

实施例6：

本发明实施例6提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明实施例1所述的步态自动切换的足式机器人落足控制方法或者本发明实施例3所述的步态自动切换的足式机器人足端触地检测方法中的步骤。

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

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

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

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

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

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