基于局部环境感知的四足机器人自主攀爬楼梯方法及系统

技术领域

本发明涉及四足机器人技术领域，尤其涉及一种基于局部环境感知的四足机器人自主攀爬楼梯方法及系统。

背景技术

随着电驱动关节性能与运动控制算法的不断进步，四足机器人开始逐渐进入各种实际场景中完成作业，而这些场景大都由平地、斜坡与楼梯组成。现有的四足机器人运动控制器能够有效应对平地与斜坡等非崎岖路况，而对于楼梯等起伏地形还未能实现稳定且鲁棒的适应，这限制了四足机器人在生活中的应用范围。

在不借助外部传感器感知地形的情况下，机器人利用姿态、关节角度与速度等本体信息设计摆动腿和支撑腿控制策略，以此来应对未知的地形起伏，这一方法存在一定的成功作业案例，但这种“盲走”不可避免的会出现台阶绊腿、踩空等现象而导致姿态起伏过大甚至摔倒，上述方法鲁棒性低、稳定性差，远远达不到作业的性能需求。

随着视觉、雷达等传感器以及定位建图(SLAM,Simultaneous Localization andMapping,即时定位与地图构建)技术的发展，现有研究中，开始在机器人躯干周围拓展外部感知设备来使机器人主动获取地形的几何信息，增加作业的性能与稳定。目前常用的方法是融合机器人自身的位姿估计、视觉传感器与激光传感器等数据，建立环境的全局地形高程地图，根据机器人在地图中的位置针对台阶地形规划足端轨迹。然而，这种方法需要获取机器人的准确位置，而机器人位姿估计本身存在严重的漂移，随着运动距离增加，位姿估计的累计误差使得地形图精度越来越差，最终无法准确定位楼梯台阶的准确位置；其次，全局的地形高程地图一般需要多个传感器的融合来提高精度，这些传感器大都价格昂贵，大大增加了应用成本；最后，多传感器融合、建图与定位等算法复杂度高，大都需要高运算速度与并行处理的内核，成本高。

发明内容

为解决上述现有技术的不足，本发明提供了一种基于局部环境感知的四足机器人自主攀爬楼梯方法及系统，通过单个相机有效视野实时扫描四足机器人落足点区域，获取实时局部数据，不依赖全局地图，而仅使用实时局部距离信息来规划运动轨迹，仅使用单个深度相机即可完成可靠的感知运动，实现可靠的攀爬楼梯作业，算法复杂度低，硬件系统成本低。

第一方面，本发明提供了一种基于局部环境感知的四足机器人自主攀爬楼梯方法。

一种基于局部环境感知的四足机器人自主攀爬楼梯方法，四足机器人本体的腹部设置视觉相机模块，视觉相机模块包括单个深度相机，所述方法包括：

获取机器人当前视角下结构化楼梯的深度数据，利用PEAC算法对深度数据聚类及平面分割，得到楼梯平面点云数据；

基于楼梯平面点云数据，利用PCA算法计算得到当前楼梯的每个平面，确定楼梯每个平面所属类型，所述类型包括水平面和竖直面；

基于楼梯的水平面和竖直面，计算楼梯特征参数、偏航角误差和出楼梯标志位数据；

基于楼梯特征参数、偏航角误差和出楼梯标志位数据，计算生成足端运动轨迹，并引入PD修正算法和地形适应算法，生成机器人质心运动轨迹，生成的轨迹利用模型预测控制和全身运动控制跟踪，得到关节作用量，控制机器人腿部关节运动，完成楼梯攀爬任务。

进一步的技术方案，控制机器人腿部关节运动，完成楼梯攀爬任务，包括：

持续监测出楼梯标志位数据，判断机器人当前所处攀爬阶段；

在不同攀爬阶段，依据楼梯特征参数计算四个足端相距当前竖直面之间的距离，并求解下一步态时刻的落足点，利用贝塞尔曲线生成摆动腿起始点与落足点之间的足端运动轨迹；

根据偏航角误差，通过PD修正算法修正机器人的朝向，确定期望偏航角；通过地形适应算法，拟合四个足端所在平面，以平面倾角作为期望俯仰角；基于期望偏航角和期望俯仰角，生成机器人质心运动轨迹；

基于足端运动轨迹和机器人质心运动轨迹，利用模型预测控制和全身运动控制跟踪，得到关节作用量，控制机器人腿部关节运动，完成楼梯攀爬任务。

第二方面，本发明提供了一种基于局部环境感知的四足机器人自主攀爬楼梯系统。

一种基于局部环境感知的四足机器人自主攀爬楼梯系统，包括：

视觉相机模块，包括单个深度相机，单个深度相机设置在四足机器人本体的腹部，用于获取机器人当前视角下结构化楼梯的深度数据；

路由器模块，用于将视觉信息模块得到的深度数据传输至板载计算机模块；

板载计算模块，用于处理深度数据并执行运动控制算法；

四足机器人本体，用于搭载视觉相机模块、路由器模块和板载计算模块，并执行楼梯攀爬任务；

所述处理深度数据并执行运动控制算法，包括：

利用PEAC算法对深度数据聚类及平面分割，得到楼梯平面点云数据；

基于楼梯平面点云数据，利用PCA算法计算得到当前楼梯的每个平面，确定楼梯每个平面所属类型，所述类型包括水平面和竖直面；

基于楼梯的水平面和竖直面，计算楼梯特征参数、偏航角误差和出楼梯标志位数据；

基于楼梯特征参数、偏航角误差和出楼梯标志位数据，生成足端运动轨迹，并引入PD修正算法和地形适应算法，生成机器人质心运动轨迹，生成的轨迹利用模型预测控制和全身运动控制跟踪，得到关节作用量，控制机器人腿部关节运动，完成楼梯攀爬任务。

进一步的技术方案，控制机器人腿部关节运动，完成楼梯攀爬任务，包括：

持续监测出楼梯标志位数据，判断机器人当前所处攀爬阶段；

在不同攀爬阶段依据楼梯特征参数计算四个足端相距当前竖直面之间的距离，并求解下一步态时刻的落足点，利用贝塞尔曲线生成摆动腿起始点与落足点之间的足端运动轨迹；

根据偏航角误差，通过PD修正算法修正机器人的朝向，确定期望偏航角；通过地形适应算法，拟合四个足端所在平面，以平面倾角作为期望俯仰角；基于期望偏航角和期望俯仰角，生成机器人质心运动轨迹；

基于足端运动轨迹和机器人质心运动轨迹，利用模型预测控制和全身运动控制跟踪，得到关节作用量，控制机器人腿部关节运动，完成楼梯攀爬任务。

以上一个或多个技术方案存在以下有益效果：

1、本发明提供了一种基于局部环境感知的四足机器人自主攀爬楼梯方法及系统，通过单个相机有效视野实时扫描四足机器人落足点区域，获取实时局部数据，不依赖全局地图，而仅使用实时局部距离信息来规划运动轨迹，仅使用单个深度相机即可完成可靠的感知运动，实现可靠的攀爬楼梯作业，算法复杂度低，成本低。

2、本发明中，在四足机器人腹部安置单个深度相机，这一安置方式减少了机器人表面的占用空间，而且相机有效视野实时扫描四足机器人落足点区域，可以实现“看到哪，走到哪”；所提出的四足机器人攀爬楼梯算法，不依赖于全局地图，仅仅使用基于单个深度相机获取的实时局部距离信息来规划运动轨迹，实现可靠的攀爬楼梯作业，这一过程避免了多传感器融合、建图定位的复杂步骤，能够实时运行，且不受机器人位姿估计漂移的影响，解决全局建图累计误差造成的定位精度下降、落足位置失真越来越严重的难题。

附图说明

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

图1为本发明实施例中四足机器人的结构示意图；

图2为本发明实施例所述基于局部环境感知的四足机器人自主攀爬楼梯方法的坐标系定义图；

图3为本发明实施例所述基于局部环境感知的四足机器人攀爬楼梯方法的分阶段示意图。

图4为本发明实施例所述基于局部环境感知的四足机器人攀爬楼梯方法的视觉信息处理流程图。

图5为本发明实施例所述基于局部环境感知的四足机器人攀爬楼梯方法的运动控制算法流程图。

具体实施方式

应该指出，以下详细说明都是示例性的，仅是为了描述具体实施方式，旨在对本发明提供进一步的说明，并非意图限制根据本发明的示例性实施方式。除非另有指明，本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

正如背景技术所述，现有的四足机器人完成攀爬楼梯作业的运动控制方法中，存在一定的问题：首先，当前四足机器人感知系统通常需要多传感器融合，成本昂贵，应用性不高；其次，多传感器融合、建图与定位等算法复杂度高；最后，机器人在构建好的全局地图中进行规划运动则需要准确的位姿估计，而足式机器人的位姿估计存在严重的漂移，进而导致最终规划运动误差较大，极大降低了算法性能。为了解决上述问题，本发明提出了一种基于局部环境感知的四足机器人自主攀爬楼梯方法及系统，借助记载视觉感知信息，不依赖全局地图，而仅使用实时局部信息来实时规划足端运动轨迹，赋予四足机器人成熟稳定的楼梯攀爬能力。

实施例一

本实施例提供了一种基于局部环境感知的四足机器人自主攀爬楼梯方法，其中，四足机器人包括四足机器人本体、视觉相机模块、板载计算模块和路由器模块，所述方法包括：

步骤S1、获取机器人当前视角下结构化楼梯的深度数据，利用PEAC算法对深度数据聚类及平面分割，得到楼梯平面点云数据；

步骤S2、基于楼梯平面点云数据，利用PCA算法计算得到当前楼梯的每个平面，确定楼梯每个平面所属类型，所述类型包括水平面和竖直面；

步骤S3、基于楼梯的水平面和竖直面，计算楼梯特征参数、偏航角误差和出楼梯标志位数据；

步骤S4、基于楼梯特征参数、偏航角误差和出楼梯标志位数据，生成足端运动轨迹，并引入PD修正算法和地形适应算法，生成机器人质心运动轨迹，生成的轨迹利用模型预测控制和全身运动控制跟踪，得到关节作用量，控制机器人腿部关节运动，完成楼梯攀爬任务。

其中，控制机器人腿部关节运动，完成楼梯攀爬任务，包括：

持续监测出楼梯标志位数据，判断机器人当前所处攀爬阶段；

在不同攀爬阶段依据楼梯特征参数计算四个足端相距当前竖直面之间的距离，并求解下一步态时刻的落足点，利用贝塞尔曲线生成摆动腿起始点与落足点之间的足端运动轨迹；

根据偏航角误差，通过PD修正算法修正机器人的朝向，确定期望偏航角；通过地形适应算法，拟合四个足端所在平面，以平面倾角作为期望俯仰角；基于期望偏航角和期望俯仰角，生成机器人质心运动轨迹；

基于足端运动轨迹和机器人质心运动轨迹，利用模型预测控制和全身运动控制跟踪，得到关节作用量，控制机器人腿部关节运动，完成楼梯攀爬任务。

通过下述内容对本实施例所提出的基于局部环境感知的四足机器人自主攀爬楼梯方法进行更详细的介绍。

在本实施例中，所采用的四足机器人如图1所示，包括四足机器人本体、视觉相机模块、板载计算模块和路由器模块。

其中，视觉相机模块设置在四足机器人本体的腹部，用于获取机器人当前视角下结构化楼梯的深度数据；该视觉相机模块包括单个深度相机，该深度相机通过相机支架固定设置在四足机器人本体的腹部，与四足机器人本体躯干呈30°倾角向下，在该视野下能够实时采集四足机器人两个前脚的落足位置，实现“看到哪，走到哪”的实时感知控制。

板载计算模块设置在四足机器人本体上方前侧，用于处理视觉信息(即深度数据)和特征提取算法计算，如提取楼梯的几何特征参数、机器人偏航角误差、机器人出楼梯标志位等。

路由器模块设置在四足机器人本体上方前侧，用于不同模块或设备之间数据信息的传输，在本实施例中，板载计算模块包括板载计算机，路由器模块包括路由器，四足机器人本体躯干上方固定设有外保护壳，外保护壳中由上到下依次设置路由器和板载计算机，该路由器作为深度相机和板载计算器之间数据传输的桥梁，用于将视觉信息模块得到的数据传输至板载计算机。

四足机器人本体用于搭载上述模块或设备并执行楼梯攀爬作业。

控制上述四足机器人进行楼梯攀爬作业，实现自主攀爬楼梯，如图4所示，方法包括：

步骤S1中，获取机器人当前视角下结构化楼梯的深度数据，利用PEAC算法对深度数据聚类及平面分割，得到楼梯平面点云数据。具体的，使用单个深度相机获取机器人当前视角下结构化楼梯的深度数据，通过路由器将深度数据传递至板载计算机，在板载计算机中运行PEAC算法对深度数据聚类及平面分隔。由于传感器本身存在噪声以及相机四周存在与楼梯场景无关的杂点，对平面分割后的数据进行裁剪、滤波、点云转换等操作得到可用的楼梯平面点云数据。

步骤S2中，基于楼梯平面点云数据，利用PCA算法计算得到当前楼梯的每个平面，确定楼梯每个平面所属类型，所述类型包括水平面和竖直面。具体的，使用主成分分析算法(PCA)操作楼梯平面点云数据，计算得到每个平面的中心点与法向量，其中这些向量均表示在机体坐标系下，坐标系的定义如图2所示。同时，机器人自身会向板载计算机发送机体坐标系下的重力向量，依据每个平面的法向量与重力向量之间的夹角，判断每个平面的所属类型，即判断每个平面是否归属于楼梯的水平面和竖直面，该判断公式如1-1所示：

上式中，δ

步骤S3中，基于楼梯的水平面和竖直面，计算楼梯特征参数、偏航角误差和出楼梯标志位数据。具体的，楼梯特征参数包括单个台阶的高度和深度，因此，通过两个水平面之间的距离计算楼梯的高度d

上式中，

通过机器人坐标系下x轴方向的前向单位向量l

考虑到在前脚踏出楼梯面后，机器人后脚会失去视觉信息，此时需要改变后脚的轨迹规划算法，因此需要在攀爬楼梯过程中持续监测出楼梯标志位。通过连续两个平面中心距离的差值判断机器人是否为出楼梯状态，用于改变足端规划方法，公式如1-6所示：

上式中，

通过坐标变换将相机至楼梯竖直面中心点的向量l

上式中，

通过上述计算得到相应的变量，这些变量均通过路由器传递到机器人内部的板载计算设备中，用于运动控制算法的执行。

步骤S4中，基于楼梯特征参数、偏航角误差和出楼梯标志位数据，生成足端运动轨迹，并引入PD修正算法和地形适应算法，生成机器人质心运动轨迹，生成的轨迹利用模型预测控制和全身运动控制跟踪，得到关节作用量，控制机器人完成楼梯攀爬任务。

具体的，如图5所示，机器人内部的板载计算机获取到上述计算得到的楼梯特征参数、偏航角误差与出楼梯标志位数据后，开始执行运动控制算法。首先，依据机器人相对楼梯的位置将整个攀爬过程划分为5个阶段，持续监测出楼梯标志位数据，判断机器人当前所处攀爬阶段；其次，在不同攀爬阶段依据楼梯特征参数计算四个足端相距当前竖直面之间的距离，并求解下一步态时刻的落足点；然后，利用贝塞尔曲线生成摆动腿起始点与落足点之间的足端运动轨迹；之后，根据偏航角误差，引入PD修正算法计算期望偏航角，用于修正机器人的朝向，保证头部始终朝着楼梯正中间，同时引入地形适应算法，拟合四个足端所在的平面，以该平面的倾角作为机器人的期望俯仰角，增加运动的稳定性；最后，生成的足端运动轨迹和机器人质心运动轨迹(可简称为质心轨迹与腿部轨迹)使用模型预测控制(MPC)与全身运动控制(WBC)跟踪，得到关节作用量并传递至电机，控制机器人完成楼梯攀爬任务，实现可靠控制。

首先，通过视觉处理端传送的楼梯深度d

由于检测竖直面并不是当前腿所在台阶竖直面，因此需要分情况处理来得到脚至所在台阶竖直面的距离。根据四足机器人腿在楼梯中的个数将整个过程划分为5个阶段，分别为四条腿在平面，两条腿进楼梯，四条腿进楼梯，两条腿出楼梯，四条腿出楼梯，如图3所示。在其中前三个阶段含有视觉信息，后两个阶段无视觉信息，因此，需要持续检测标志位，判断机器人所处攀爬阶段，不同阶段具有不同的视觉信息，在不同的阶段采用不同的算法。

在使用视觉信息的情况下(前三个阶段，即机器人进楼梯状态)，直接使用求余操作得到当前前腿至所在台阶竖直面的距离d

d

上式中，d

对于后两个不含有视觉信息的阶段(后两个阶段，即机器人出楼梯阶段)，根据d

在获取到每个足端至所在台阶竖直面的距离后，根据原始的摆动腿跨度是否已经超过当前距离来分阶段处理落足点，当跨度远远小于距离时对落足点不做任何处理，当跨度接近距离时对原始的落足点(表示在世界坐标系下)的x与z坐标叠加一个增量使其跨过当前台阶。

其次，上楼梯的过程中需要实时修正四足机器人的俯仰角与偏航角以增加稳定性。对于俯仰角，使用平面函数拟合四个脚所在的平面并求出平面的倾角作为期望俯仰角；对于偏航角，使用视觉处理端获取的偏航角误差与PD模型修正期望偏航角，公式如1-9所示：

上式中，k

最后，基于生成的躯干(即质心)与腿部(即足端)的期望轨迹，使用MPC优化跟踪轨迹与系统输入，优化方程如公式1-10所示：

上式中，x

根据上述任务得到优化的支撑腿足底力输入

上式中，

叠加所有任务加速度得到总的期望配置空间加速度

上式中，W

通过以上步骤得到关节空间的作用量，将该作用量传递给驱动器，控制机器人腿部关节运动，以此完成楼梯攀爬任务。

实施例二

本实施例提供了一种基于局部环境感知的四足机器人自主攀爬楼梯系统，包括：

视觉相机模块，包括单个深度相机，单个深度相机设置在四足机器人本体的腹部，用于获取机器人当前视角下结构化楼梯的深度数据；

路由器模块，用于将视觉信息模块得到的深度数据传输至板载计算机模块；

板载计算模块，用于处理深度数据并执行运动控制算法；

四足机器人本体，用于搭载视觉相机模块、路由器模块和板载计算模块，并执行楼梯攀爬任务。

其中，处理深度数据并执行运动控制算法，包括：

利用PEAC算法对深度数据聚类及平面分割，得到楼梯平面点云数据；

基于楼梯平面点云数据，利用PCA算法计算得到当前楼梯的每个平面，确定楼梯每个平面所属类型，所述类型包括水平面和竖直面；

基于楼梯的水平面和竖直面，计算楼梯特征参数、偏航角误差和出楼梯标志位数据；

基于楼梯特征参数、偏航角误差和出楼梯标志位数据，生成足端运动轨迹，并引入PD修正算法和地形适应算法，生成机器人质心运动轨迹，生成的轨迹利用模型预测控制和全身运动控制跟踪，得到关节作用量，控制机器人腿部关节运动，完成楼梯攀爬任务。

以上实施例二中涉及的各步骤与方法实施例一相对应，具体实施方式可参见实施例一的相关说明部分。

本领域技术人员应该明白，上述本发明的各模块或各步骤可以用通用的计算机装置来实现，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。本发明不限制于任何特定的硬件和软件的结合。

以上所述仅为本发明的优选实施例，虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。