CN113534831A - 一种提高四足机器人爬楼梯能力的静步态规划方法及装置 - Google Patents

Abstract

本发明提出一种提高四足机器人爬楼梯能力的静步态规划方法及装置，提高四足机器人爬楼梯能力的静步态规划方法具体包括步骤：S1、获取初始化h、h_f和h_r；S2、检测d_f、d_ff、d_r和d_rr；S3、根据d_f和d_r确定四足机器人在爬楼梯时的具体情况，基于具体情况确定步长d和迈步高度Δh_f、Δh_r；S4、根据(h,Δh_f,Δh_r,d)执行爬行步态周期运动序列；S5、更新h,h_f,h_r；S6、判断是否停止，若否，返回所述步骤S2；S7、结束规划。本发明分别针对爬楼梯姿势和爬行步态周期运动序列进行了优化，并且，在规划过程中，分析爬楼梯过程中可能出现的情况，解决了四足机器人攀爬楼梯困难的问题。

Description

一种提高四足机器人爬楼梯能力的静步态规划方法及装置

技术领域

本发明涉及四足机器人步态规划领域，特别是一种提高四足机器人爬楼梯能力的静步态规划方法及装置。

背景技术

为了进一步解放人类双手，提高工作效率或是帮助人类完成一些安全系数低、难于实现的任务，越来越多的研究团队开发出各式各样的机器人来服务人类。常见的机器人分为轮式、履带式和腿足式机器人，由于轮式和履带式机器人行走路径连续且机械结构的自由度较少，因此灵活度较差，只适合在相对平坦的地面执行任务，而腿足式机器人的落脚点是离散的，且可以根据需要添加多个关节来提高运动的灵活性，因此可以适应更多的场景。目前，足式机器人在抢险救援、地形勘探、教育娱乐、物资运输等领域都表现出了广阔的应用前景。相比于双足机器人，四足机器人的稳定性和承载能力更好，相比于六足机器人，四足机器人的机械结构更简单，因此其一直是研究的热点。

对于四足机器人而言，步态规划是保证其完成各种任务的最关键一步。受到自然界中各种生物运动方式的启发，研究人员设计出了行走步态(walk)、踱步步态(pace)、跳跃步态(bound)、小跑步态(trot)、奔跑步态(gallop)等各种步态。其中，行走步态又被称为静步态。在四足机器人行走过程中，某几条腿的足端与地面接触以支撑身体，其他腿在空中摆动到目标位置。采用静步态的四足机器人在行走过程中至少有三条腿的足端接触地面，质心的投影始终落在由支撑腿的足端构成的支撑多边形内，因此具有很好的运动稳定性。其余步态都属于动步态，采用动步态的四足机器人的支撑多边形区域较小，因此需要通过快速运动来保持动态稳定性，对机械结构的要求较高，。

要想使四足机器人能够在实际生活场景中得到广泛应用，爬楼梯是其必不可少的一项技能。过去几十年，许多研究团队都发明出了可以爬楼梯的四足机器人。由于采用静步态的机器人有更大的稳定裕度且更容易实施，因此最早发明的爬楼梯机器人采用静步态。东京工业大学发明了TITAN系列爬楼梯机器人，其中，TITAN III机器人可以借助传感器实现自主爬楼梯，TITAN VI机器人可以爬过常规的坡度为30-40°的楼梯。哈尔滨工业大学发明了混合运动模式的四足机器人，提出了一种静态爬楼梯步态。香港大学优化了采用静步态的四足机器人Pegasus的足端轨迹，使机器人可以自主爬过楼梯。苏黎世联邦理工学院提出了一种新颖的姿态优化方法，使四足机器人ANYmal可以穿越高17cm，宽29cm的楼梯。相较于静步态，采用动步态可以提高运动速度。斯坦福大学提出了一种动态跳跃步态，使LittleDog机器人可以爬过和机器人腿一样高的台阶。宾夕法尼亚大学研发的Minitaur四足机器人，通过采用平行弹性腿机构，可以像青蛙一样跳过台阶高度为19.6cm的楼梯。最近几年，很多四足机器人开始采用对角步态爬楼梯，例如SpotMIT Cheetah及SCalf机器人。

综上可以看出，各个研究团队对采用动步态和静步态爬楼梯均做了大量的研究，且部分四足机器人可以爬过标准尺寸的楼梯。虽然采用动步态可以提高运动速度，但采用该步态会降低四足机器人的承载能力和安全性，一旦摔倒会对机器人造成很大的损坏，而且采用动步态对四足机器人的机械结构要求较高，不利于其推广应用，而采用静步态的四足机器人运动稳定性较好且更容易实现。

对于图1a-1c所示的爬行姿势，由于存在某几条腿的弯曲方向与楼梯的上升方向一致，因此会增加与楼梯碰撞的概率，对于图1d所示的爬行姿势，由于前腿的弯曲程度远大于后腿，导致前腿膝关节产生的力矩远大于后腿，而且，受到身体位置的限制，抬腿高度较小，无法攀爬很陡的楼梯。对于图1e所示的爬行姿势，质心的投影位置更靠近后脚，容易向后倾倒。

对于图1a-1c,1e,1f所示的五种爬行姿势，四足机器人从当前位置和姿态切换到下一个位置和姿态时需要调整身体的俯仰角度以使身体与前后脚所构成的斜坡平行。现有技术往往采用两种调整身体的策略：1)在前腿还没有登上下一节楼梯时调整身体的高度和俯仰角；2)在两条前腿均登上下一节楼梯之后再调整身体的高度和俯仰角。对于第一种策略，当机器人要爬上比较陡的楼梯时，如图2a所示，可能会出现脚离地的情况。对于第二种策略，受到身体的限制，身体下方会出现一个半圆形的足端不可达区域，灰色区域代表前腿足端的工作空间，当抬第一条前腿时，摆动腿的足端位于髋部的前方，因此不易出现足端运动轨迹位于不可达区域的情况，当抬第二条前腿时，如图2b所示，由于此时身体已经向前运动，容易出现足端运动轨迹位于不可达区域的情况。

发明内容

为了解决现有技术中采用静步态的四足机器人爬楼梯比较困难的技术问题，本发明提出一种提高四足机器人爬楼梯能力的静步态规划方法及装置。

为此，本发明提出的提高四足机器人爬楼梯能力的静步态规划方法具体包括如下步骤：

S1、获取初始化h、h_f和h_r，h_f代表前腿足端的迈步高度，h_r代表后腿足端的迈步高度，h代表前后腿足端的初始高度差；

S2、检测d_f、d_ff、d_r和d_rr，d_f、d_ff、d_r和d_rr分别代表前后腿的足端到台阶边缘的距离；

S3、根据d_f和d_r确定四足机器人在爬楼梯时的具体情况，基于具体情况确定步长d和迈步高度Δh_f、Δh_r；

S4、根据(h,Δh_f,Δh_r,d)执行爬行步态周期运动序列；

S5、更新h,h_f,h_r；

S6、判断是否停止，若否，返回所述步骤S2，若是，进入下一步；

S7、结束规划。

进一步地，在所述步骤S3中，选择最大步长为15cm，设定距离各级台阶边缘2cm外的区域为安全区域，当d_r≤13cm，d_f≤13cm时，前后腿均上台阶，当d_r≤13cm，d_f＞13cm时，后腿上台阶，当d_r＞13cm，d_f≤13cm时，前腿上台阶，当d_r＞13cm，d_f＞13cm时，前后腿均不上台阶。

进一步地，在前后腿均上台阶时，Δh_f＝h_f，Δh_r＝h_r，d＝min{15，d_ff-2，d_rr-2}；在后腿上台阶时，Δh_f＝0，Δh_r＝h_r，d＝min{15，d_f-2，d_rr-2}；在前腿上台阶时，Δh_f＝h_f，Δh_r＝0，d＝min{15，d_ff-2，d_r-2}；在前后腿均不上台阶时，Δh_f＝0，Δh_r＝0，d＝min{15，d_f-2，d_r-2}。

进一步地，在爬楼梯过程中，所述四足机器人采用最佳爬行姿势进行运动，所述最佳爬行姿势具备以下四个条件：

C1、所有腿均朝下楼梯的方向弯曲；

C2、身体始终与前后腿的足端所形成的斜坡保持水平，令身体的偏转角度为α；

C3、令身体的高度为h_o；

C4、前后腿的足端在水平面上投影间距保持不变。

进一步地，所述偏转角度α的计算公式为α＝arctan(h/L)，所述高度h_o的计算公式为h_o＝H+h/2，h代表前后腿足端的高度差，H代表四足机器人位于水平面时身体距地面的高度，L代表前后腿的足端在水平面上的投影间距。

进一步地，在所述步骤S4中，一个爬行周期可以用爬行步态周期开始时的偏转角度α、爬行步态周期结束时的偏转角度α'、身体在爬行步态周期开始时的高度h_o和身体在爬行步态周期结束时的高度h'_o来描述，在一个爬行步态周期内，身体需要偏转的角度为：Δα＝α′-α，身体需要上升的高度为：Δh_o＝h′_o-h_o。

进一步地，α＝arctan(h/L)，α'＝arctan[(h+h_f+h_r)/L]，h_o＝H+h/2，h'_o＝H+(h+h_f+h_r)/2,h代表前后腿足端的高度差，H代表四足机器人位于水平面时身体距地面的高度，L代表前后腿的足端在水平面上的投影间距，h_f代表前腿足端的迈步高度，h_r代表后腿足端的迈步高度。

进一步地，在所述步骤S4中，所述爬行步态周期运动序列包括三个重心调整阶段、四个腿摆动阶段和一个调整身体高度和俯仰角度的阶段，在抬起第二条前腿的同时调整身体的高度及俯仰角度。

进一步地，所述四足机器人的迈步次序为右后腿－右前腿－左后腿－左前腿。

进一步地，所述四足机器人采用横滚髋关节-俯仰髋关节-俯仰膝关节结构。

本发明提出的提高四足机器人爬楼梯能力的静步态规划装置包括中央处理器和存储装置，所述存储装置存储有能够被所述中央处理器运行的程序，所述中央处理器通过运行所述程序能够实现上述提高四足机器人爬楼梯能力的静步态规划方法。

本发明提出的计算机可读存储介质存储有能够被中央处理器运行的程序，所述程序在被所述中央处理器运行的过程中能够实现上述提高四足机器人爬楼梯能力的静步态规划方法。

相对于现有技术，本发明具有如下有益效果：

1)在算法中分析爬楼梯过程中可能出现的情况，解决了一般的四足机器人无法攀爬陡峭楼梯的问题；

2)算法求解速度很快，适用于实时求解，有利于四足机器人的推广应用。

在本发明的一些实施例中，还具有如下有益效果：

1)针对爬楼梯姿势进行优化，既可以避免四足机器人在爬行过程中与楼梯发生碰撞，又可以提高最大迈步高度，且运算较为简单；

2)针对爬行步态周期运动序列进行优化，通过在第二条前腿抬起的同时调整身体俯仰角度和高度，避免了出现足端离地和与楼梯发生碰撞的情况，大大提高了四足机器人的爬楼梯能力。

附图说明

图1a是现有技术中四足机器人的爬楼梯姿势示意图；

图1b是现有技术中四足机器人的爬楼梯姿势示意图；

图1c是现有技术中四足机器人的爬楼梯姿势示意图；

图1d是现有技术中四足机器人的爬楼梯姿势示意图；

图1e是现有技术中四足机器人的爬楼梯姿势示意图；

图1f是现有技术中四足机器人的爬楼梯姿势示意图；

图2a是现有技术中爬楼梯时提前调整姿态的示意图；

图2b是现有技术中爬楼梯时延迟调整姿态的示意图；

图3a是本发明实施例四足机器人机器人的仿真模型结构的正视图；

图3b是本发明实施例四足机器人机器人的仿真模型结构的侧视图；

图4是本发明实施例四足机器人的足端轨迹示意图；

图5是本发明实施例四足机器人的备选姿势A示意图；

图6是本发明实施例四足机器人的备选姿势B示意图；

图7是本发明实施例四足机器人的备选姿势C示意图；

图8是本发明实施例不同备选姿势在相同抬腿高度时的对比示意图；

图9是本发明实施例最佳爬行姿势的参数示意图；

图10是本发明实施例爬行步态周期的示意图；

图11是现有技术中四足机器人采用静步态在平地行走的单个步态周期示意图；

图12是本发明实施例改进的上楼梯过程中的爬行步态周期示意图；

图13是本发明实施例静步态规划方法的流程图；

图14是本发明实施例满足安全性的爬行姿势示意图。

具体实施方式

为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图说明本发明的具体实施方式。

如图3a-3b所示，本发明所针对的四足机器人采用常见的横滚髋关节-俯仰髋关节-俯仰膝关节结构，大腿和小腿连杆长度均为30cm，其中，横滚髋关节提供腿部的横向摆动自由度，俯仰髋关节和俯仰膝关节提供腿部的纵向摆动自由度，其他机械结构参数见表1。在本发明中，选择如图4所示的分段直线轨迹作为四足机器人的足端轨迹。

表1

为了方便解决问题，可以把爬楼梯任务可以分解为两个子问题。第一个问题是设计出四足机器人位于楼梯不同位置的静态姿势。第二个问题是设计出一个爬行周期内身体和足端的运动次序，使得四足机器人可以从一个位置和姿势切换到另一个位置和姿势。为了提高运动稳定性和爬楼梯能力，本专利对两个问题都做出了优化。

针对爬行姿势优化的问题，不同于在平地上运动，在爬楼梯的过程中，四足机器人前后腿的足端高度差在改变，足端还可能与楼梯发生碰撞，而爬楼梯的姿势可以影响机器人的足端可达工作空间，关节力矩，安全性等，因此调整机器人的姿势使其更好地适应楼梯就显得尤为重要。为了简化问题，假设身体不做滚转运动和偏航运动，两条前腿和两条后腿在竖直平面的投影分别重合，因此可以在二维平面内研究爬行姿势。通过改变腿部弯曲方向、质心位置及身体俯仰角度，可以设计出图1a-1f所示的六种不同的爬行姿态，以下从三个方面分析这六种姿态。

(1)腿部弯曲方向

本发明所针对的四足机器人每条腿有三个自由度，给定足端目标位置后进行逆运动学求解往往会得到关节角的两组解。为了避免腿部与楼梯发生碰撞，机器人腿部应该向下楼梯的方向弯曲，因此排除图1a-1c所示的爬行姿态。

(2)质心位置

为了获得最大的稳定裕度，质心的投影应该位于由足端构成的支撑多边形的中心。对于图1e所示的爬行姿态，质心的投影位置更靠近后腿一侧，导致四足机器人容易向后侧倾倒，因此，排除图1e所示的爬行姿态。

基于以上分析，确定出爬行姿势必须满足的两个条件：1)腿部向下楼梯方向弯曲；2)质心投影位置在支撑三角形的中心。

(3)身体俯仰角度

由于爬楼梯过程中前后腿的足端有高度差，因此身体可以选择不同的俯仰角度，不同的俯仰角度会影响足端工作空间，进而影响最大抬腿高度，而且，足端抬起时既可以始终位于髋关节的正下方，也可以保持前后脚在水平面上的投影间距不变。不同的抬腿方式会导致最大抬腿高度的不同，进而影响爬楼梯的能力。因此，设计出满足前两个条件的三种备选姿势。姿势A(如图5所示)：身体始终保持水平，且足端抬起时始终位于髋关节的正下方；姿势B(如图6所示)：身体与前后脚所形成的斜坡保持水平，且足端抬起时始终位于髋关节的正下方；姿势C(如图7所示)：身体与前后脚的所形成的斜坡保持水平，且足端抬起时始终保持前后脚在水平面上的投影间距不变。

下面分别从最大抬腿高度、膝关节力矩和复杂度这三个方面分析这三种姿势，进而对比确定出最佳爬行姿势。

(1)最大抬腿高度

从图5-7可以看出，对于姿势A，由于受到身体高度的限制，最大抬腿高度小于髋关节到足端的竖直距离，对于姿势B和姿势C，当后腿变直时，前腿的足端可以抬起最大高度，通过计算机仿真可得到姿势A到C的最大抬腿高度分别为：0.52m,0.64m和0.62m。

(2)膝关节力矩

通过图8可以看出，当采用姿势A时，前腿的弯曲程度远大于后腿，因此，前腿的膝关节会产生更大的力矩，当采用姿势B和姿势C时，前后腿的弯曲程度几乎相同，因此前后腿膝关节提供几乎相同的力矩。

(3)复杂度

在三种备选姿势中，姿势A最简单。对于姿势B，由于前后腿的足端在水平面上的投影距离随前后脚的高度差变化而变化，因此，姿势B比姿势C复杂。

表2对于上述比较结果进行了总结，通过表2可以看出，姿势B和姿势C的最大抬腿高度均高于姿势A，姿势B和姿势C的膝关节产生的力矩均小于姿势A，对于姿势B和姿势C，二者的最大抬腿高度和膝关节产生的力矩差别很小，但姿势C更简单。因此，最终选定姿势C为最优爬楼梯姿势。

表2

如图9所示，在最佳爬行姿势中，h代表前后腿足端的高度差，H代表四足机器人位于水平面时身体距地面的高度，L代表前后腿的足端在水平面上的投影间距，最佳爬行姿势需满足以下四个条件：

条件1(腿部弯曲方向)：所有腿均朝下楼梯的方向弯曲；

条件2(身体俯仰角度)：身体始终与前后腿的足端所形成的斜坡保持水平，令身体的偏转角度为α，计算可得：

α＝arctan(h/L) (1)

条件3(身体高度)：用h_o代表身体高度，计算可得：

h_o＝H+h/2 (2)

条件4(足端位置)：前后腿的足端在水平面上投影间距保持不变。

当满足以上条件时，四足机器人在楼梯各个位置的姿势是唯一确定的。

针对四足机器人从一个位置和姿势切换到下一个位置和姿势的身体及足端运动次序，将四足机器人从一个位置和姿势切换到下一个位置和姿势称为爬行步态的一个周期，如图10所示，h_f代表前腿足端的迈步高度，h_r代表后腿足端的迈步高度，h代表前后腿足端的初始高度差，d代表行走步长，一个爬行周期可以用这四个参数来描述。

分别令α、α'为身体在爬行步态周期开始和结束时的偏转角度，计算可得：

α＝arctan(h/L)

α'＝arctan[(h+h_f+h_r)/L] (3)

根据条件2，在一个爬行步态周期内，身体需要偏转的角度为：Δα＝α′-α。

分别令h_o、h'_o为身体在爬行步态周期开始和结束时的高度，计算可得：

h_o＝H+h/2

h'_o＝H+(h+h_f+h_r)/2 (4)

根据条件3，在一个爬行步态周期内，身体需要上升的高度为：Δh_o＝h′_o-h_o。

为了保证四足机器人的运动稳定性并提高其爬楼梯的能力，需要设定一个爬行步态周期内身体和足端的运动次序。当四足机器人在平地上运动时，静态步态的单个步态周期一般包含三个重心调整阶段和四个腿摆动阶段。在重心调整阶段，机器人在四条腿的支撑下将其重心调整到双三角稳定区域。在腿摆动阶段，机器人的身体由三条腿支撑且保持不动，摆动腿的足端移动到期望落足点。四足机器人共有六种非奇异的静步态迈步顺序，通常选择其中最常用且可以提供最大稳定裕度的迈步次序：右后腿－右前腿－左后腿－左前腿。静态步态的一个步态周期可以用图11表示，其中，身体和足端用5个小圆表示，指代身体的小圆位于正方形正中间，指代足端的小圆位于正方形的顶点处，身体摆动阶段和腿摆动阶段用包含五个小圆的大圆表示，连接大圆的带方向的圆弧指示着各阶段的执行次序。当小圆变为实心状态时，小圆指代的身体在移动或腿在摆动(足端在运动)，大圆旁圆弧外侧的指令代表该阶段足端依次执行的动作，大圆旁圆弧内侧的指令代表该阶段身体依次执行的动作，“x”,“y”,和“z”分别代表沿三个坐标轴的运动。

当四足机器人攀爬楼梯时，根据上述分析，需要在以上的静态步态周期中增加调整身体高度和身体俯仰角度的运动序列。常见的调整方式有两种：(1)在爬行周期开始时，即前腿还没有登上下一节楼梯时进行调整；(2)在爬行周期结束前，即两条前腿已经登上下一节楼梯时进行调整。这两种调整方式存在各自的不足。为了克服它们的不足，本发明在抬起第二条前腿的同时调整身体的高度及俯仰角度，用p描述身体的俯仰运动，图12显示的是上楼梯过程中的爬行步态周期，当四足机器人下楼梯时，各阶段的执行顺序反向即可。

如图13所示，本发明实施例提出的提高四足机器人爬楼梯能力的静步态规划方法具体包括如下步骤：

S1、获取初始化h、h_f和h_r，如图14所示，h_f代表前腿足端的迈步高度，h_r代表后腿足端的迈步高度，h代表前后腿足端的初始高度差；

S2、检测d_f、d_ff、d_r和d_rr，如图14所示，d_f、d_ff、d_r和d_rr分别代表前后腿的足端到台阶边缘的距离；

S3、确定步长d和迈步高度Δh_f、Δh_r，为了避免四足机器人爬楼梯过程中滑倒，四足机器人的落脚点不能位于台阶的边缘处，因此，设定距离各级台阶边缘2cm外的区域为安全区域，通过选择合适的步长，即可满足落足点位于安全区域内，考虑到运动学约束，我们选择最大步长为15cm。以前脚为例，如果d_f＜15-2＝13cm，则其可以迈到下一台阶，反之，下一个落脚点仍在本级台阶上，四足机器人在爬楼梯时可以分为四种情况：

当d_r≤13cm，d_f≤13cm时，前后腿均上台阶，此时，Δh_f＝h_f，Δh_r＝h_r，d＝min{15，d_ff-2，d_rr-2}；

当d_r≤13cm，d_f＞13cm时，后腿上台阶，此时，Δh_f＝0，Δh_r＝h_r，d＝min{15，d_f-2，d_rr-2}；

当d_r＞13cm，d_f≤13cm时，前腿上台阶，此时，Δh_f＝h_f，Δh_r＝0，d＝min{15，d_ff-2，d_r-2}；

当d_r＞13cm，d_f＞13cm时，前后腿均不上台阶，此时，Δh_f＝0，Δh_r＝0，d＝min{15，d_f-2，d_r-2}。

S4、根据(h,Δh_f,Δh_r,d)执行爬行步态周期运动序列；

S5、更新h,h_f,h_r；

S6、判断是否停止，若否，返回步骤S2，若是，进入下一步；

S7、结束规划。

本发明实施例提出的提高四足机器人爬楼梯能力的静步态规划装置包括中央处理器和存储装置，存储装置存储有能够被中央处理器运行的程序，中央处理器通过运行程序能够实现上述提高四足机器人爬楼梯能力的静步态规划方法。

为了验证本发明提出的静步态规划方法的可行性和在提高爬楼梯能力方面的有效性，在两个场景中验证该算法。

测试场景1：坡度很陡的楼梯，楼梯由左右对称的10个台阶构成，每个台阶的高度为20cm，宽度为26cm，坡度为37.6°，采用本发明提出的静步态爬楼梯规划方法，四足机器人可以成功穿越整个楼梯，在楼梯不同的位置，四足机器人的身体可以很好地调整姿势以适应楼梯。

测试场景2：宽度很窄的楼梯，该楼梯由3个台阶构成，每个台阶的高度为18cm，宽度为5cm，坡度为74.5°，该场景是一个更有挑战性的测试地形，采用本发明提出的静步态爬楼梯规划方法，四足机器人成功完成了这个有挑战性的任务。由于采用对角步态的四足机器人不可完成该任务，采用跳跃步态的四足机器人若想完成该任务，需装配可提供很大力矩的电机，因此，本专利提出的方法对于一般的四足机器人爬过陡峭的楼梯就显得尤为重要。

本发明提出的提高四足机器人爬楼梯能力的静步态规划方法对爬行姿势和爬行周期的运动序列均做出了优化，相对于现有技术，本发明具有如下有益效果：

1、采用本发明优化后的爬楼梯姿势，既可以避免四足机器人在爬行过程中与楼梯发生碰撞，又可以提高最大迈步高度，且运算较为简单；

2、本发明优化了常见的爬行步态周期运动序列，通过在第二条前腿抬起的同时调整身体俯仰角度和高度，避免了出现足端离地和与楼梯发生碰撞的情况，大大提高了四足机器人的爬楼梯能力；

3、基于优化后的爬楼梯姿势和爬行步态周期运动序列，本发明综合考虑了安全性和运动学约束，分析了爬楼梯过程中可能出现的四种情况，提出了四足机器人爬楼梯算法，解决了一般的四足机器人无法攀爬陡峭楼梯的问题，适用于四足机器人爬过陡峭的楼梯和宽度很窄的楼梯，而且，本发明提出的算法求解速度很快，适用于实时求解，有利于四足机器人的推广应用。

以上所揭露的仅为本发明较佳实施例而已，当然不能以此来限定本发明之权利范围。应当指出，对于本技术领域的技术人员，在不脱离本发明设计结构及原理的前提下对本发明方案所作的等同变化都视作本发明的保护范围。

Claims (12)

1.一种提高四足机器人爬楼梯能力的静步态规划方法，其特征在于,具体包括如下步骤：

S1、获取初始化h、h_f和h_r，h_f代表前腿足端的迈步高度，h_r代表后腿足端的迈步高度，h代表前后腿足端的初始高度差；

S2、检测d_f、d_ff、d_r和d_rr，d_f、d_ff、d_r和d_rr分别代表前后腿的足端到台阶边缘的距离；

S3、根据d_f和d_r确定四足机器人在爬楼梯时的具体情况，基于具体情况确定步长d和迈步高度Δh_f、Δh_r；

S4、根据(h,Δh_f,Δh_r,d)执行爬行步态周期运动序列；

S5、更新h,h_f,h_r；

S6、判断是否停止，若否，返回所述步骤S2，若是，进入下一步；

S7、结束规划。

2.根据权利要求1所述的静步态规划方法，其特征在于，在所述步骤S3中，选择最大步长为15cm，设定距离各级台阶边缘2cm外的区域为安全区域，当d_r≤13cm，d_f≤13cm时，前后腿均上台阶，当d_r≤13cm，d_f＞13cm时，后腿上台阶，当d_r＞13cm，d_f≤13cm时，前腿上台阶，当d_r＞13cm，d_f＞13cm时，前后腿均不上台阶。

3.根据权利要求2所述的静步态规划方法，其特征在于，在前后腿均上台阶时，Δh_f＝h_f，Δh_r＝h_r，d＝min{15，d_ff-2，d_rr-2}；在后腿上台阶时，Δh_f＝0，Δh_r＝h_r，d＝min{15，d_f-2，d_rr-2}；在前腿上台阶时，Δh_f＝h_f，Δh_r＝0，d＝min{15，d_ff-2，d_r-2}；在前后腿均不上台阶时，Δh_f＝0，Δh_r＝0，d＝min{15，d_f-2，d_r-2}。

4.根据权利要求1所述的静步态规划方法，其特征在于，在爬楼梯过程中，所述四足机器人采用最佳爬行姿势进行运动，所述最佳爬行姿势具备以下四个条件：

C1、所有腿均朝下楼梯的方向弯曲；

C2、身体始终与前后腿的足端所形成的斜坡保持水平，令身体的偏转角度为α；

C3、令身体的高度为h_o；

C4、前后腿的足端在水平面上投影间距保持不变。

5.根据权利要求4所述的静步态规划方法，其特征在于，所述偏转角度α的计算公式为α＝arctan(h/L)，所述高度h_o的计算公式为h_o＝H+h/2，h代表前后腿足端的高度差，H代表四足机器人位于水平面时身体距地面的高度，L代表前后腿的足端在水平面上的投影间距。

6.根据权利要求4所述的静步态规划方法，其特征在于，在所述步骤S4中，一个爬行周期可以用爬行步态周期开始时的偏转角度α、爬行步态周期结束时的偏转角度α'、身体在爬行步态周期开始时的高度h_o和身体在爬行步态周期结束时的高度h'_o来描述，在一个爬行步态周期内，身体需要偏转的角度为：Δα＝α′-α，身体需要上升的高度为：Δh_o＝h′_o-h_o。

7.根据权利要求6所述的静步态规划方法，其特征在于，α＝arctan(h/L)，α'＝arctan[(h+h_f+h_r)/L]，h_o＝H+h/2，h'_o＝H+(h+h_f+h_r)/2,h代表前后腿足端的高度差，H代表四足机器人位于水平面时身体距地面的高度，L代表前后腿的足端在水平面上的投影间距，h_f代表前腿足端的迈步高度，h_r代表后腿足端的迈步高度。

8.根据权利要求1所述的静步态规划方法，其特征在于，在所述步骤S4中，所述爬行步态周期运动序列包括三个重心调整阶段、四个腿摆动阶段和一个调整身体高度和俯仰角度的阶段，在抬起第二条前腿的同时调整身体的高度及俯仰角度。

9.根据权利要求8所述的静步态规划方法，其特征在于，所述四足机器人的迈步次序为右后腿－右前腿－左后腿－左前腿。

10.根据权利要求1-9所述的静步态规划方法，其特征在于，所述四足机器人采用横滚髋关节-俯仰髋关节-俯仰膝关节结构。

11.一种提高四足机器人爬楼梯能力的静步态规划装置，其特征在于，包括中央处理器和存储装置，所述存储装置存储有能够被所述中央处理器运行的程序，所述中央处理器通过运行所述程序能够实现如权利要求1-10中任一项所述的提高四足机器人爬楼梯能力的静步态规划方法。

12.一种计算机可读存储介质，其特征在于，存储有能够被中央处理器运行的程序，所述程序在被所述中央处理器运行的过程中能够实现如权利要求1-10中任一项所述的提高四足机器人爬楼梯能力的静步态规划方法。

Images