一种四足机器人的静步态规划方法与流程

本发明涉及四足机器人步态规划的技术领域，尤其涉及一种四足机器人的静步态规划方法及计算机可读存储介质。

背景技术：

随着人类社会的发展和科学技术的进步，人们探索未知世界的愿望越来越强烈。然而，考虑到安全性等因素，仅凭人类自身的力量很难到达许多地形复杂的或未知的地方，因此人们开发出各种行走机器人来代替自己工作。目前使用最多的行走机器人分为轮式和履带式机器人，但由于轮式和履带式机器人的行走路径是连续的，因此它们只可以在相对平坦的路面上行驶，而地球上超过百分之五十的路面都是崎岖不平甚至带有大量禁区的，这大大限制了它们的应用。受到陆生哺乳动物的启发，近几十年来，越来越多的研究者开始关注足式机器人。相较于轮式和履带式机器人，足式机器人的足端轨迹是离散的点，因此只要在地面上选定了合适的落足点，其便可穿越各种复杂地形。而且，足式机器人的腿部往往具有多个自由度，这使其具有更高的运动灵活性。在足式机器人中，四足机器人比双足机器人具有更好的运动稳定性和更大的承载能力，比六足机器人具有更简单的机械结构和更少的冗余自由度，因此愈来愈成为研究的热点。

一般而言，四足机器人的步态规划包括落足点的选择，重心轨迹规划和摆动足足端轨迹规划。关于重心轨迹规划，常见的规划方法主要分为两类，一类考虑提高机器人的运动稳定性，另一类考虑提高机器人的运动快速性。其中，考虑运动稳定性的重心轨迹规划方法以稳定裕度最大作为优化目标，且通常采用ssm判据衡量四足机器人的静态稳定性大小。该规划方法求解出支撑三角形内稳定裕度最大的点作为重心移动轨迹的终点。但是，采用这种方法计算出的重心移动轨迹往往很长，进而导致能量利用率下降及移动用时增加。另一类重心轨迹规划方法通过缩短重心在重心调整阶段的移动距离来提高机器人运动的快速性。例如，j.zicokolter提出了双三角理论，将重心调整阶段由四个缩减为两个，大大缩短了重心移动距离。binli提出了一种新的重心调整策略，将重心调整分为水平调整和竖直调整两种类型，缩短了重心移动距离。但是，在这类规划方法中，重心往往以固定速度移动，这样可能会导致两个问题。其一是在某些时刻，四足机器人腿部某几个关节的关节角速度会超限，其二是在另外一些时刻，四足机器人腿部所有关节都没有达到关节角速度限值，也即重心没有以可达到的最大线速度移动。对于足端轨迹规划，大多数规划方法在确定足端轨迹后，给定足端一个固定的移动速度，但这样处理同样会导致四足机器人某几个关节的关节角速度超限和足端无法以可达到的最大线速度移动的问题。

综上，现有技术中采用静步态的四足机器人存在行走速度缓慢的问题。

以上背景技术内容的公开仅用于辅助理解本发明的构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

技术实现要素：

本发明为了解决现有的问题，提供一种四足机器人的静步态规划方法及计算机可读存储介质。

为了解决上述问题，本发明采用的技术方案如下所述：

一种四足机器人的静步态规划方法，包括如下步骤：s1：预先选定多组沿“s”形分布的位于同一水平面的不规则落足点；s2：确定所述四足机器人的重心调整的目标区域，所述目标区域满足两个约束条件：稳定性约束和足端工作空间约束；s3：在重心调整阶段，从所述重心调整的目标区域中选择一个距离重心初始投影位置最近的点作为重心调整目标点，采用重心轨迹优化算法确定最短重心移动轨迹；s4：采用速度优化算法获取在关节角速度约束条件下的最大身体和足端移动速度；s5：所述四足机器人以所述最大身体和足端移动速度按照所述落足点行走。

优选地，所述稳定性约束包括在腿摆动阶段，所述四足机器人的重心投影位置始终位于由支撑腿的足端构成的支撑三角形内。

优选地，采用内插支撑三角形代替所述支撑三角形来判断机器人的稳定性；若重心投影位置在腿摆动阶段始终位于所述内插三角形内则判断所述四足机器人是稳定的。

优选地，将所述四足机器人的重心投影到支撑腿的落足点所在的支撑水平面上，并设定平面坐标系{p}，然后通过四只支撑足在所述平面坐标系{p}中的坐标确定重心调整的目标区域；所述平面坐标系{p}中，原点o代表重心调整阶段开始时重心的投影位置，o′代表重心调整的期望位置，s0代表所述内插三角形各边与所述支撑三角形各边的距离，lij代表连接第i条腿的落足点和第j条腿的落足点的直线，sij代表o′到lij的距离；四足机器人的静态稳定性判据表示为：

sij≥s0,i,j∈{a1,a2,a3,a4}且i≠j

其中，a1＝{1,2,3},a2＝{1,2,4},a3＝{1,3,4},a4＝{2,3,4}；

记pi(xi,yi),其中i∈{1,2,3,4}为第i条腿的足端在所述平面坐标系{p}中的坐标，则直线lij的直线方程为：

aijx+bijy+cij＝0

其中，aij＝yi-yj，bij＝xj-xi，cij＝xi(yj-yi)+yi(xi-xj)；

将满足稳定性约束的重心调整目标区域命名为区域a，区域a表示为：

其中，p′o(x′o,y′o)代表重心调整的目标位置在所述平面坐标系{p}中的坐标。

优选地，所述足端工作空间约束包括：在所述重心调整阶段，重心移动过程中每条腿的落足点必须位于其工作空间内；在所述腿摆动阶段，摆动腿的期望落足点必须位于其可达工作空间内。

优选地，确定每条腿在所述支撑水平面上的可达工作区域是圆形，且圆心为工作空间的球心在支撑水平面上的投影点，半径等于

其中，l2是大腿连杆长度，l3是小腿连杆长度，h是躯干高度；

记pi(xi,yi),其中，i∈{1,2,3,4}为第i条腿的足端在所述平面坐标系{p}中的坐标，k为下一个相邻阶段摆动腿的标号，p′k(x′k,y′k)为摆动腿期望落足点的坐标，则将所述区域a中满足足端工作空间约束的重心调整目标区域区域b需要满足如下所示的足端工作空间约束条件：

(xi-(x′o+αb))²+(yi-(y′o+βa))²≤r²

(x′k-(x′o+αb))²+(y′k-(y′o+βa))²≤r²

其中，

优选地，从所述重心调整的目标区域中选择一个距离重心初始投影位置po(xo,yo)最近的点p′o(x′o,y′o)作为重心调整目标点，记d为po(xo,yo)与p′o(x′o,y′o)之间的距离，则重心轨迹优化问题描述为：

优选地，采用速度优化算法获取在关节角速度约束条件下的最大足端移动速度包括：s41：获取每条腿的足端线速度与所述腿上横滚髋关节、俯仰髋关节和俯仰膝关节三个关节的关节角速度的关系式；将四足机器人的右后腿、右前腿、左后腿及左前腿分别标为腿1、腿2、腿3、腿4，根据四足机器人结构的对称性可知，腿1和腿3的足端线速度与三个关节角速度的关系式相等，腿2和腿4的足端线速度与三个关节角速度的关系式相等；在躯干的形心处建立坐标系{ob}，在滚转髋关节的中心建立坐标系{oi0},其中，i＝1,2,3,4，基于d-h规则，在所述滚转髋关节，所述俯仰髋关节，所述俯仰膝关节的中心分别建立坐标系{oi1},{oi2},{oi3}；记pi(xi,yi,zi)为第i条腿的足端在坐标系{ob}中的坐标，vi(vix,viy,viz)代表第i条腿的足端线速度，θij代表第i条腿第j个关节的关节角，wij代表第i条腿第j个关节的关节角速度，则足端位置pi(xi,yi,zi)与对应三个关节角θij的关系式表示为：

pi＝[λ(l2s2-l3s3)+αb,s1(l2c2+l3c3)+βa,-c1(l2c2+l3c3)]

其中，

上式得到足端速度vi(vix,viy,viz)与对应腿的三个关节角速度wij的关系式为：

vix＝λ(l2wi2c2-l3wi3c3)

viy＝wi1c1(l2c2+l3c3)-s1(l2wi2s2+l3wi3s3)

viz＝wi1s1(l2c2+l3c3)+c1(l2wi2s2+l3wi3s3)

其中，c1＝cos(θi1)，s1＝sin(θi1)，c2＝cos(θi2)，s2＝sin(θi2)，c3＝cos(-θi3-θi2)，s3＝sin(-θi3-θi2)，

s42：获取所述横滚髋关节、所述俯仰髋关节和所述俯仰膝关节的关节角速度的比例关系；足端垂直抬起的轨迹为线段ab，垂直落下的轨迹为线段cd，水平移动的轨迹为线段bc；当足端沿着线段ab和线段cd移动时，足端线速度在x轴和y轴的分量均为0，用公式表示为：

λ(l2wi2c2-l3wi3c3)＝0

wi1c1(l2c2+l3c3)-s1(l2wi2s2+l3wi3s3)＝0

当足端沿着线段ab和线段cd移动时，三个关节角速度的比例关系为：

wi1/wi2＝(s1l2(c2s3+c3s2))/(c1c3(l2c2+l3c3))

wi1/wi3＝(s1l2l3(c2s3+c3s2))/(c1(l2c2+l3c3)²)

当足端沿线段bc移动时，足端线速度在各个方向的分量需要满足：足端线速度在z轴的分量为0；足端线速度在y轴的分量与x轴的分量的比值等于线段bc的斜率k，用公式表示为：

wi1c1(l2c2+l3c3)-s1(l2wi2s2+l3wi3s3)＝kλ(l2wi2c2-l3wi3c3)

wi1s1(l2c2+l3c3)+c1(l2wi2s2+l3wi3s3)＝0

当足端沿着线段bc移动时，三个关节角速度的比例关系为：

s43：获取所述最大足端移动速度；当足端沿着线段ab和线段cd移动时，wi2和wi3的方向是已知的，而wi1的方向未知，根据三个关节角速度的比值确定出wi1的符号；通过比较三个关节角速度的比值，便可得到达到最大角速度的关节；结合三个关节角速度的方向信息，得出所有的关节角速度；当足端沿着线段bc移动时，wi1的方向是已知的，而wi2和wi3的方向未知，根据三个关节角速度的比值确定wi2和wi3的符号。通过比较三个关节角速度的比值，便可得到达到最大角速度的关节；结合三个关节角速度的方向信息，得出所有的关节角速度；根据每条腿的足端线速度与所述腿的三个关节角速度的关系式，获得四个最大足端移动速度。

优选地，选择四个所述最大足端移动速度中最小的一个作为最大身体移动速度。

本发明又提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上任一所述方法的步骤。

本发明的有益效果为：提供一种四足机器人的静步态规划方法及计算机可读存储介质，通过同时考虑了稳定性约束和足端工作空间约束来限制重心调整的目标位置，使四足机器人在移动过程中可以保持稳定，且四足机器人的足端可以摆动到期望落足点；利用重心轨迹优化算法求解出了最短重心移动轨迹，从而缩短了调整重心到稳定区域所需要的时间，提高了四足机器人的行走速度。

再次，利用速度优化算法求解出了在关节角速度约束条件下的最大身体以及足端移动速度，从而提高了四足机器人的行走速度。

在本发明提出的静步态规划方法中，两种优化算法的结合大大提高了四足机器人的行走速度，解决了采取静步态行走的四足机器人行走速度缓慢的问题。而且，两种优化算法的求解速度都很快，适用于实时求解，有利于四足机器人的推广应用。

本发明提出的静步态规划方法适用于四足机器人穿越平坦地形和包含禁区的崎岖地形。

附图说明

图1是本发明实施例中四足机器人仿真模型的示意图。

图2是本发明实施例中一种测试地形的示意图。

图3是本发明实施例中一种采用静步态行走的四足机器人的单个步态周期的示意图。

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

图5是本发明实施例中一种四足机器人的静步态规划方法的示意图。

图6是本发明实施例中一种内插支撑三角形的示意图。

图7是本发明实施例中一种满足足端工作空间约束的重心调整目标区域的示意图。

图8是本发明实施例中采用速度优化算法获取在关节角速度约束条件下的最大足端移动速度的方法示意图。

图9是本发明实施例中一种内插三角形中满足动力学约束的可行重心投影区域的示意图。

图10是本发明实施例中第一种求解三个关节角速度的算法流程图。

图11是本发明实施例中第二种求解三个关节角速度的算法流程图。

图12是本发明实施例中四足机器人穿越测试地形的足端轨迹的示意图。

图13是本发明实施例中关节角速度的变化情况的示意图。

图14是本发明实施例中四足机器人的重心位置在前进方向上的运动轨迹的示意图。

具体实施方式

为了使本发明实施例所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

需要说明的是，当元件被称为“固定于”或“设置于”另一个元件，它可以直接在另一个元件上或者间接在该另一个元件上。当一个元件被称为是“连接于”另一个元件，它可以是直接连接到另一个元件或间接连接至该另一个元件上。另外，连接既可以是用于固定作用也可以是用于电路连通作用。

需要理解的是，术语“长度”、“宽度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明实施例和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多该特征。在本发明实施例的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

由于四足机器人的步态规划决定了机器人的运动方式，进而可以使机器人完成不同的任务，因此，其一直是四足机器人研究领域的重点。在步态规划前，首先要确定四足机器人的行走步态。四足机器人的行走步态种类较多，按照机器人行走过程中重心投影是否始终位于由支撑足足端所构成的支撑多边形内，可将行走步态分为静态步态和动态步态。按照机器人行走过程中是否可以根据地形特征灵活地调整其重心移动距离及摆动腿次序，可将行走步态分为自由步态和周期性步态。采用静步态行走的四足机器人始终有大于等于三条腿接触地面，因此具有更高的运动稳定性，且从机械结构的设计来说更容易实现。采用自由步态行走的四足机器人可以根据地形情况自由地调节身体和腿的移动规律，因此具有更高的地形适应性和运动灵活性。基于以上分析，本发明中的四足机器人采用具有静态特征的自由步态作为行走步态。

如图1所示，本发明采用的四足机器人仿真模型的示意图。四足机器人包括四条腿，每条腿有三个关节，分别为横滚髋关节2，俯仰髋关节1和俯仰膝关节4。其中，横滚髋关节2提供腿部的横向摆动自由度，俯仰髋关节1和俯仰膝关节4提供腿部的纵向摆动自由度；大腿连杆3用于连接横滚髋关节2和俯仰膝关节4；小腿连杆5用于连接俯仰膝关节4和足端6。

表1给出了四足机器人的机械结构参数。

表1四足机器人的机械结构参数

在本发明中，为了简化问题并不失实用性，假定落足点是根据已知地形信息提前选定的。当四足机器人穿越崎岖地形时，需要不停地调整前进的方向来适应地形环境。为了更好地模拟这一情况，在本发明的一种实施例中，预先选定了17组大致沿“s”形分布的落足点。而且，为了确保四足机器人在行走过程中的每一步都能落在目标落足点上，选择17*4个木桩来限制落足点的位置。

如图2所示，是本发明实施例中一种测试地形的示意图。其中，圆形、菱形、正方形、五角星形木桩分别代表右后腿、右前腿、左后腿、左前腿的落足点。

可以理解的是，本发明提出的方法适用于根据地形状况预先选好数组落足点的场景。因为在离线状态下，可以采用多种图像、数据处理方法及结合经验从复杂地形信息中选出若干组合适的落脚点。而搭载在四足机器人上的计算机计算速度不会很快，若实时计算落脚点的话，可能会由于算法复杂造成求解缓慢的问题。

静态步态的单个步态周期一般包含四个重心调整阶段和四个腿摆动阶段。在重心调整阶段，机器人在四条腿的支撑下将其重心调整到稳定区域。在腿摆动阶段，机器人的身体由三条腿支撑且保持不动，摆动腿的足端移动到期望落足点。四足机器人共有六种非奇异的静步态迈步顺序，本发明选择其中最常用且可以提供最大稳定裕度的迈步次序：右后腿－右前腿－左后腿－左前腿。

如图3所示，是本发明实施例一种采用静步态行走的四足机器人的单个步态周期的示意图。图中以黑色方块代表重心调整阶段，白色方块代表腿摆动阶段。

如图4所示，是本发明实施例中四足机器人的足端轨迹的示意图。图中以分段直线轨迹作为四足机器人的足端轨迹。其中，竖直轨迹的长度为7cm，水平轨迹的长度为相邻两个落足点间的距离。可以理解的是，竖直轨迹的高度是人为规定的，取了一个经验值，也可以根据具体情况取其他的值。

本发明基于以下几个假设进行静步态规划：

1、四足机器人的质心与身体的形心重合。

2、在行走过程中，四足机器人的躯干始终处于一个固定的高度。

3、根据已知地形信息提前选定的若干组落足点都位于同一水平面内，即前述实施例中17组木桩的高度相同。

以一个直观的公式来看，v＝s/t，要想四足机器人的行走提高速度，第一个方法是缩短移动距离，也即重心轨迹优化算法。第二个方法是直接提高行走速度，由于机器人机械结构的限制，速度不可能达到无穷大，因此可以求解出在关节角速度约束条件下的最大身体和足端移动速度，这样可以直接提高走完全程的行走速度，也即速度优化算法。

如图5所示，一种四足机器人的静步态规划方法，包括如下步骤：

s1：预先选定多组沿“s”形分布的位于同一水平面的不规则落足点；

s2：确定所述四足机器人的重心调整的目标区域，所述目标区域满足两个约束条件：稳定性约束和足端工作空间约束；

s3：在重心调整阶段，从所述重心调整的目标区域中选择一个距离重心初始投影位置最近的点作为重心调整目标点，采用重心轨迹优化算法确定最短重心移动轨迹；

s4：采用速度优化算法获取在关节角速度约束条件下的最大身体和足端移动速度；

s5：所述四足机器人以所述最大身体和足端移动速度按照所述落足点行走。

本发明的方法同时考虑了稳定性约束和足端工作空间约束来限制重心调整的目标位置，使四足机器人在移动过程中可以保持稳定，且四足机器人的足端可以摆动到期望落足点。

进一步地，利用重心轨迹优化算法求解出了最短重心移动轨迹，从而缩短了调整重心到稳定区域所需要的时间，提高了四足机器人的行走速度。

再次，利用速度优化算法求解出了在关节角速度约束条件下的最大身体以及足端移动速度，从而提高了四足机器人的行走速度。

在本发明提出的静步态规划方法中，两种优化算法的结合大大提高了四足机器人的行走速度，解决了采取静步态行走的四足机器人行走速度缓慢的问题。而且，两种优化算法的求解速度都很快，适用于实时求解，有利于四足机器人的推广应用。

本发明提出的静步态规划方法适用于四足机器人穿越平坦地形和包含禁区的崎岖地形。

为了提高采用静步态行走的四足机器人的行走速度，本发明提出一种以重心移动距离最短作为优化目标的重心轨迹优化算法。在求解最短重心轨迹前，首先需要确定重心调整的目标区域。为了使四足机器人在行走过程中可以保持稳定，且摆动腿的足端可以移动到期望落足点，该区域需要满足两个约束条件：稳定性约束和足端工作空间约束。

对于采用静态步态的四足机器人而言，若其重心投影位置始终位于由支撑腿的足端构成的支撑多边形内，即可判断其是稳定的。由于在重心调整阶段，静态稳定判据总是成立的，因此只需要考虑腿摆动阶段，若重心投影位置在腿摆动阶段始终位于由支撑腿的足端构成的支撑三角形内，即可判断四足机器人是稳定的。为了减小模型测量的不精确及不确定干扰带来的误差对运动造成的影响，保证机器人行走过程中具有足够的稳定裕度，用一个内插三角形代替支撑三角形来判断机器人的稳定性。这样定义后，静态稳定判据变为：若重心投影位置在腿摆动阶段始终位于内插三角形内，即可判断四足机器人是稳定的。

如图6所示，是本发明实施例中内插支撑三角形的示意图。其中，内插三角形的区域为区域a。在规划重心调整的目标区域时，首先将四足机器人的重心投影到支撑腿的落足点所在的支撑水平面上，并设定如图所示的平面坐标系{p}，然后通过四只支撑足在平面坐标系中的坐标确定重心调整的目标区域。其中，o代表重心调整阶段开始时重心的投影位置，o′代表重心调整的期望位置，s0代表内插三角形各边与支撑三角形各边的距离，lij代表连接第i条腿的落足点和第j条腿的落足点的直线，sij代表o′到lij的距离。基于以上定义，四足机器人的静态稳定性判据可以表示为：

sij≥s0,i,j∈{a1,a2,a3,a4}且i≠j(1)

其中，a1＝{1,2,3},a2＝{1,2,4},a3＝{1,3,4},a4＝{2,3,4}。

记pi(xi,yi),(i∈{1,2,3,4})为第i条腿的足端在{p}中的坐标，则直线lij的直线方程如式(2)所示：

aijx+bijy+cij＝0(2)

其中，aij＝yi-yj，bij＝xj-xi，cij＝xi(yj-yi)+yi(xi-xj)。

将满足稳定性约束的重心调整目标区域命名为区域a，区域a即为内插支撑三角形区域，用来判断稳定性。重心在区域a的范围内移动就是稳定的，不在区域a内就是不稳定的。

p′o(x′o,y′o)代表重心调整的目标位置在{p}中的坐标，结合公式(1)和公式(2)，区域a可以表示为：

由于四足机器人每条腿的工作空间都是有限的，因此还需要考虑足端工作空间约束对重心调整的目标区域的影响。一般而言，足端工作空间约束从两个方面限制重心调整的目标区域：1)在重心调整阶段，重心移动过程中每条腿的落足点必须位于其工作空间内；2)在腿摆动阶段，摆动腿的期望落足点必须位于其可达工作空间内。

将区域a中满足足端工作空间约束的重心调整目标区域命名为区域b，为了求解该区域，本发明首先需要确定每条腿在支撑水平面上的可达工作区域。假设所有关节的关节角变化范围为(0,2π)，可以求解出每条腿的工作空间为球心位于俯仰髋关节中心，半径为l2+l3的球形，进而可以推导出每条腿在支撑水平面上的可达工作区域为圆形，且圆心为工作空间的球心在支撑水平面上的投影点，半径等于

记pi(xi,yi),(i∈{1,2,3,4})为第i条腿的足端在{p}中的坐标，k为下一个相邻阶段摆动腿的标号，p′k(x′k,y′k)为摆动腿期望落足点的坐标，则区域b除了需要满足公式(6)中的稳定性约束条件外，还需要满足公式(7)所示的足端工作空间约束条件：

其中，

由公式(7)可知，区域b由五个圆形区域确定出来，这五个圆形区域分别以当前落足点位置pi(xi,yi),(i∈{1,2,3,4})和摆动腿期望落足点p′k(x′k,y′k)为圆心，以r为半径。

如图7所示，是本发明实施例中满足足端工作空间约束的重心调整目标区域的示意图。其中，圆弧为圆形区域边界的一部分，斜线部分组成的区域为区域b。

确定了满足稳定性约束和足端工作空间约束的重心调整目标区域后，本发明从该区域中选择一个距离重心初始投影位置最近的点作为重心调整目标点。可以理解的是，区域b内离初始投影位置o最近的点只有一个。o与区域b的位置关系有两种。一种情况o位于区域b内，此时最短重心移动距离为0，即重心无需移动。另一种情况o位于区域b外，以o为圆心，不断增加半径画圆，总会有一个圆与区域b相切，切点即为区域b内离初始投影位置最近的点。

由于两点间线段最短，本发明规定重心沿直线轨迹移动。记d为po(xo,yo)与p′o(x′o,y′o)之间的距离，则该重心轨迹优化问题可以描述为：

通过求解该优化问题，可以得到重心调整的目标区域内距离重心初始投影位置最近的点，进而可以确定出最短重心移动轨迹。由于静态步态的单个周期内共包含四个重心调整阶段，因此一个周期内共需求解四次优化算法。

提高四足机器人行走速度的另一个方法是使身体和足端以尽可能大的速度移动。由于机械结构的限制，四足机器人腿部关节的关节角速度都有上限值。因此，本发明提出一种速度优化算法来求解在关节角速度约束条件下的最大身体和足端移动速度。

为了求解在关节角速度约束条件下的最大足端移动速度，首先本发明需要推导出每条腿的足端线速度与该条腿上三个关节(横滚髋关节，俯仰髋关节和俯仰膝关节)的关节角速度的关系式。其次，为了使足端能沿期望的轨迹移动，足端线速度沿直角坐标系三个坐标轴的分量需要满足一定的约束条件。基于这些约束，本发明可以确定出每条腿三个关节角速度的比例关系。当某条腿的足端以最大速度移动时，该条腿上某个关节的关节角速度一定达到了上限值，但本发明无法提前确定是哪个关节。因此，本发明提出一种算法来确定达到最大角速度的关节，再利用三个关节角速度的比例关系即可求解出所有关节角速度。最后，根据每条腿的足端线速度与该条腿三个关节角速度的关系式，便可求得最大足端移动速度。

如图8所示，采用速度优化算法获取在关节角速度约束条件下的最大足端移动速度的方法包括：

s41：获取每条腿的足端线速度与所述腿上横滚髋关节、俯仰髋关节和俯仰膝关节三个关节的关节角速度的关系式；

s42：获取所述横滚髋关节、所述俯仰髋关节和所述俯仰膝关节的关节角速度的比例关系；

s43：获取所述最大足端移动速度。

其中，获取每条腿的足端线速度与所述腿上横滚髋关节、俯仰髋关节和俯仰膝关节三个关节的关节角速度的关系式包括：

将四足机器人的右后腿、右前腿、左后腿及左前腿分别标号1、2、3、4。根据四足机器人结构的对称性可知，腿1和腿3的足端线速度与三个关节角速度的关系式相等，腿2和腿4的足端线速度与三个关节角速度的关系式相等。

如图9所示，是本发明实施例中内插三角形中满足动力学约束的可行重心投影区域的示意图。在躯干的形心处建立坐标系{ob}，在滚转髋关节的中心建立坐标系{oi0},(i＝1,2,3,4)。基于d-h规则，在滚转髋关节，俯仰髋关节，俯仰膝关节的中心分别建立坐标系{oi1},{oi2},{oi3}。

记pi(xi,yi,zi)为第i条腿的足端在{ob}中的坐标，vi(vix,viy,viz)代表第i条腿的足端线速度，θij代表第i条腿第j个关节的关节角，wij代表第i条腿第j个关节的关节角速度，则足端位置pi(xi,yi,zi)与对应三个关节角θij的关系式表示为：

pi＝[λ(l2s2-l3s3)+αb,s1(l2c2+l3c3)+βa,-c1(l2c2+l3c3)](6)

其中，

对公式(9)左右两边求导，可以得到足端速度vi(vix,viy,viz)与对应腿的三个关节角速度wij的关系式为：

其中，c1＝cos(θi1)，s1＝sin(θi1)，c2＝cos(θi2)，s2＝sin(θi2)，c3＝cos(-θi3-θi2)，s3＝sin(-θi3-θi2)，

其中，获取所述横滚髋关节、所述俯仰髋关节和所述俯仰膝关节的关节角速度的比例关系包括：

四足机器人的足端轨迹如图5所示，足端垂直抬起的轨迹为ab，垂直落下的轨迹为cd，水平移动的轨迹为bc。当足端沿着ab和cd移动时，足端线速度在x轴和y轴的分量均为0，用公式表示为：

由公式(11)可推导出，当足端沿着ab和cd移动时，三个关节角速度的比例关系为：

当足端沿bc移动时，足端线速度在各个方向的分量需要满足：(1)足端线速度在z轴的分量为0。(2)足端线速度在y轴的分量与x轴的分量的比值等于轨迹bc的斜率k，用公式表示为：

由公式(13)可推导出，当足端沿着bc移动时，三个关节角速度的比例关系为：

其中，获取所述最大足端移动速度包括：

当某条腿的足端以最大速度移动时，该条腿上某个关节的关节角速度一定达到了上限值，但本发明无法提前确定是哪个关节。因此，本发明提出一种算法来确定达到最大角速度的关节。

当足端沿着ab和cd移动时，wi2和wi3的方向是已知的，而wi1的方向未知，因此本发明首先需要计算出三个关节角速度的比值，根据该比例关系便可确定出wi1的符号。其次，通过比较三个关节角速度的比值，便可求解出达到最大角速度的关节。最后，结合三个关节角速度的方向信息，便可得出所有的关节角速度。

如图10所示，是本发明的一种实施例中第一种求解三个关节角速度的算法流程图。

当足端沿着bc移动时，wi1的方向是已知的，而wi2和wi3的方向未知，因此本发明首先需要计算出三个关节角速度的比值，根据该比例关系便可确定wi2和wi3的符号。其次，通过比较三个关节角速度的比值，便可求解出达到最大角速度的关节。最后，结合三个关节角速度的方向信息，便可得出所有的关节角速度。

如图11所示，是本发明的一种实施例中第二种求解三个关节角速度的算法流程图。

最后，根据公式(10)推导出的每条腿的足端线速度与该条腿三个关节角速度的关系式，便可求得最大足端移动速度。

当四足机器人的躯干移动时，足端固定不动，因此本发明需要求解在关节角约束的条件下，身体相对于足端的最大移动速度。由于身体相对于足端的速度与足端相对于身体的速度互为相反数，因此本发明可以直接利用上一节推导出的公式来求最大身体移动速度。利用上一节的公式可以求解出四个最大速度，为了保证每条腿的关节角速度都不会超限，本发明选择其中最小的一个作为最大身体移动速度。

为了验证本发明提出的静步态规划方法在提高行走速度方面的有效性，本发明将本发明提出的方法与另外两个常见的静步态规划方法进行对比。为了便于描述，本发明将三种方法分别命名为方法1，方法2和方法3。其中，方法1为本发明提出的方法，方法2以稳定裕度最大作为规划重心轨迹的优化目标，方法3选取了一个固定值作为四足机器人的身体及足端移动速度，而没有进行速度优化。

如图12所示，是采用方法1时的四足机器人穿越测试地形的足端轨迹示意图。

如图13所示，是本发明四足机器所有关节的关节角速度变化情况的示意图。从图13中可以看出，四足机器人在行走过程中所有关节的关节角速度wi1、wi2和wi3都没有超出限值，由此可以证明本发明提出的方法是可行的。

如图14所示，是分别采用这三种方法后四足机器人的重心位置在前进方向上的运动轨迹示意图。其中，黑色连续曲线代表方法1，断点线代表方法2，另一条线代表方法3。从图中可以得到，采用方法1，方法2，和方法3时，四足机器人从起点运动到终点的时间分别为205s，372s,和272s。由此可以计算出，相较于方法2和方法3，本发明提出的方法将行走速度分别提高了81.6％和32.8％，即本发明提出的静步态规划方法可以大大提高采用静步态行走的四足机器人的行走速度。

本申请实施例还提供一种控制装置，包括处理器和用于存储计算机程序的存储介质；其中，处理器用于执行所述计算机程序时至少执行如上所述的方法。

本申请实施例还提供一种存储介质，用于存储计算机程序，该计算机程序被执行时至少执行如上所述的方法。

本申请实施例还提供一种处理器，所述处理器执行计算机程序，至少执行如上所述的方法。

所述存储介质可以由任何类型的易失性或非易失性存储设备、或者它们的组合来实现。其中，非易失性存储器可以是只读存储器(rom，readonlymemory)、可编程只读存储器(prom，programmableread-onlymemory)、可擦除可编程只读存储器(eprom，erasableprogrammableread-onlymemory)、电可擦除可编程只读存储器(eeprom，electricallyerasableprogrammableread-onlymemory)、磁性随机存取存储器(fram，ferromagneticrandomaccessmemory)、快闪存储器(flashmemory)、磁表面存储器、光盘、或只读光盘(cd-rom，compactdiscread-onlymemory)；磁表面存储器可以是磁盘存储器或磁带存储器。易失性存储器可以是随机存取存储器(ram，randomaccessmemory)，其用作外部高速缓存。通过示例性但不是限制性说明，许多形式的ram可用，例如静态随机存取存储器(sram，staticrandomaccessmemory)、同步静态随机存取存储器(ssram，synchronousstaticrandomaccessmemory)、动态随机存取存储器(dram，dynamicrandomaccessmemory)、同步动态随机存取存储器(sdram，synchronousdynamicrandomaccessmemory)、双倍数据速率同步动态随机存取存储器(ddrsdram，doubledataratesynchronousdynamicrandomaccessmemory)、增强型同步动态随机存取存储器(esdram，enhancedsynchronousdynamicrandomaccessmemory)、同步连接动态随机存取存储器(sldram，synclinkdynamicrandomaccessmemory)、直接内存总线随机存取存储器(drram，directrambusrandomaccessmemory)。本发明实施例描述的存储介质旨在包括但不限于这些和任意其它适合类型的存储器。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统和方法，可以通过其它的方式实现。以上所描述的设备实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个单元或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或单元的间接耦合或通信连接，可以是电性的、机械的或其它形式的。

上述作为分离部件说明的单元可以是、或也可以不是物理上分开的，作为单元显示的部件可以是、或也可以不是物理单元，即可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。

另外，在本发明各实施例中的各功能单元可以全部集成在一个处理单元中，也可以是各单元分别单独作为一个单元，也可以两个或两个以上单元集成在一个单元中；上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。

或者，本发明上述集成的单元如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实施例的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、rom、ram、磁碟或者光盘等各种可以存储程序代码的介质。

本申请所提供的几个方法实施例中所揭露的方法，在不冲突的情况下可以任意组合，得到新的方法实施例。

本申请所提供的几个产品实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的产品实施例。

本申请所提供的几个方法或设备实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的方法实施例或设备实施例。

以上内容是结合具体的优选实施方式对本发明所做的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。