CN113524177A - 一种足式机器人的控制方法 - Google Patents

Abstract

本发明公开了一种足式机器人的控制方法，包括步骤：获取足式机器人的移动指令，并确定移动指令对应的对地移动轨迹；根据对地移动轨迹，确定各关节的转角；获取足式机器人的初始位置，根据初始位置和各关节的转角，确定各连杆的对地轨迹；根据关节的转角、对地移动轨迹以及连杆的对地轨迹，确定关节的速度、加速度和力，以及连杆的速度、加速度和力，以对足式机器人进行控制。由于先通过对地移动轨迹，确定各关节的转角，再根据初始位置和关节的转角，确定连杆的对地轨迹，从而可以得到关节的速度、加速度和力，以及连杆的速度、加速度和力，对足式机器人的全身建模，最大限度的表达机器人的动力学特征，提高了机器人的动态性能。

Description

一种足式机器人的控制方法

技术领域

本发明涉及机器人控制技术领域，尤其涉及的是一种足式机器人的控制方法。

背景技术

目前四足机器人还处于实验室研究阶段，暂时还没有大规模的商业应用，主要作为一个原型机平台，被一些公司以及科研机构做研究使用。对于四足机器人，未来会有很多应用场景，包括搜寻营救、机器外骨骼、人类伴侣、带有动力装置的义肢以及邮寄包裹等。

与真实的人类和腿式移动的动物相比，机器人在整体的运动灵活性，承载能力，执行多任务，持续长时间运行，自主导航等方面还有很大欠缺，需进一步深入的探究。足式机器人在行走过程中需要面对极其复杂多变的外部环境。

四足机器人在行走过程中需要高频的动态响应性能，这就需要简化模型，是控制器能够快速的求解计算运动学，动力学问题。现有技术中，模型求解计算较慢，机器人脚可能已经落地了，求解计算还没有完成，造成机器人的动态性能较低。

因此，现有技术还有待于改进和发展。

发明内容

本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种足式机器人的控制方法旨在解决现有技术中机器人的动态性能较低的问题。

本发明解决技术问题所采用的技术方案如下：

一种足式机器人的控制方法，所述足式机器人包括：身体以及与身体连接的若干条腿；每条腿包括：相互连接的至少两个连杆，相邻两个连杆、所述连杆与所述身体均通过关节连接；其中，所述控制方法包括步骤：

获取所述足式机器人的移动指令，并确定所述移动指令对应的对地移动轨迹；

根据所述对地移动轨迹，确定各所述关节的转角；

获取所述足式机器人的初始位置，根据所述初始位置和各所述关节的转角，确定各所述连杆的对地轨迹；

根据所述关节的转角、所述对地移动轨迹以及所述连杆的对地轨迹，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制。

所述的足式机器人的控制方法，其中，所述获取所述足式机器人的初始位置，根据所述初始位置和各所述关节的转角，确定各所述连杆的对地轨迹，包括：

根据所述初始位置和各所述关节的转角，确定基于旋量理论的运动学模型；

求解所述运动学模型，得到各所述连杆的对地轨迹。

所述的足式机器人的控制方法，其中，所述腿的数量有4条，4条腿按照前后左右的方式分布，每条腿中的连杆有3个；所述运动学模型为：

^BS₁₁＝(B_l/2，0，-B_w/2，-1，0，0)；^BS₁₂＝(B_l/2，0，-B_w/2-l₁，0，0，-1)；^BS₁₃＝(B_l/2，-l₂，-B_w/2-l₁，0，0，-1)

^BS₂₁＝(-B_l/2，0，-B_w/2，-1，0，0)；BS₂₂＝(-B_l/2，0，-B_w/2-l₁，0，0，-1)；^BS₂₃＝(-B_l/2，-l₂，-B_w/2-l₁，0，0，-1)

^BS₃₁＝(-B_l/2，0，B_w/2，1，0，0)；^BS₃₂＝(-B_l/2，0，B_w/2+l₁，0，0，1)；^BS₃₃＝(-B_l/2，-l₂，B_w/2+l₁，0，0，1)

^BS₄₁＝(B_l/2，0，B_w/2，1，0，0)；^BS₄₂＝(B_l/2，0，B_w/2+l₁，0，0，1)；^BS₄₃＝(B_l/2，-l₂，B_w/2+l₁，0，0，1)

其中，^BS_ij表示在身体坐标系下第i条腿的第j个关节的初始位置的速度螺旋；B_l表示前腿和后腿的间距，B_w表示左腿和右腿的间距，l₁表示第一连杆的长度，l₂表示第二连杆的长度，S_ij表示第i条腿的第j个关节的螺旋轴，T_v表示速度螺旋转换矩阵，

表示初始速度螺旋在身体坐标系下的表达，

表示初始的螺旋，ee_i表示第i条腿的末端位姿，P(·)表示齐次变换矩阵，θ_ij表示第i条腿的第j个关节的角度，ee_io表示初始时刻的第i条腿的末端位姿，∏表示求和操作。

所述的足式机器人的控制方法，其中，所述根据所述关节的转角、所述对地移动轨迹以及所述连杆的对地轨迹，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制，包括：

根据所述对地移动轨迹，确定所述足式机器人的移动步态；

当所述移动步态为行走步态时，根据所述关节的转角以及所述连杆的对地轨迹，确定基于旋量理论的动力学模型；

求解所述动力学模型，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制。

所述的足式机器人的控制方法，其中，所述当所述移动步态为行走步态时，根据所述关节的转角以及所述连杆的对地轨迹，确定基于旋量理论的动力学模型，包括：

当所述移动步态为行走步态时，根据所述关节的转角以及所述连杆的对地轨迹，确定约束矩阵；

根据所述约束矩阵，确定所述动力学模型。

所述的足式机器人的控制方法，其中，所述动力学模型为：

其中，I表示连杆惯量，C表示约束矩阵，T表示转置，a表示所有连杆的加速度，η表示所有关节的作用力，f_p表示受力参数，c_a表示运动参数。

所述的足式机器人的控制方法，其中，所述根据所述关节的转角、所述对地移动轨迹以及所述连杆的对地轨迹，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制，还包括：

当所述移动步态为腾空步态，根据所述关节的转角以及所述连杆的对地轨迹，确定所述足式机器人的整体的约束矩阵；

根据所述整体的约束矩阵，确定各连杆的加速度；

针对每个连杆，根据该连杆的加速度，确定该连杆在下一个循环中该连杆的速度。

所述的足式机器人的控制方法，其中，所述根据所述对地移动轨迹，确定各所述关节的转角，包括：

根据所述对地移动轨迹，确定所述足式机器人的脚尖对腿轨迹；

根据所述脚尖对腿轨迹，确定各所述关节的转角。

一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其中，所述处理器执行所述计算机程序时实现上述任一项所述方法的步骤。

一种计算机可读存储介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现上述任一项所述的方法的步骤。

有益效果：由于先通过对地移动轨迹，确定各关节的转角，再根据初始位置和关节的转角，确定连杆的对地轨迹，从而可以得到关节的速度、加速度和力，以及连杆的速度、加速度和力，对足式机器人的全身建模，最大限度的表达机器人的动力学特征，提高了机器人的动态性能。

附图说明

图1是本发明中四足机器人及坐标系的示意图。

图2是本发明中腿的示意图。

图3是本发明中腿在yoz平面投影的第一状态图。

图4是本发明中腿在yoz平面投影的第二状态图。

图5是本发明中腿在两个坐标系下的示意图。

图6是本发明中腿在x'oy'平面投影的第一状态图。

图7是本发明中腿在x'oy'平面投影的第二状态图。

图8是本发明中足式机器人快行走步态的示意图。

图9是本发明中足式机器人慢行走步态的示意图。

图10是本发明中足式机器人的控制方法的流程图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

请同时参阅图1-图10，本发明提供了一种足式机器人的控制方法的一些实施例。足式机器人是指通过足或腿实现移动的机器人，所述足式机器人包括：身体以及与身体连接的若干条腿；每条腿包括：相互连接的至少两个连杆，相邻两个连杆、所述连杆与所述身体均通过关节连接。足式机器人的腿中，可以通过改变连杆与连杆之间的夹角，实现腿的变形，从而实现足式机器人的移动。

足式机器人的腿的数量可以根据需要设置，通常足式机器人的腿的数量有至少2条。为了确保足式机器人移动的稳定性，足式机器人可以设置4条腿。当然，足式机器人还可以设置更多条腿，随着腿的数量的增多，稳定性增高并不多。可以理解的是，足式机器人的腿可实现足式机器人的行走，奔跑以及跳跃。

如图10所示，本发明的一种足式机器人的控制方法，包括以下步骤：

步骤S100、获取所述足式机器人的移动指令，并确定所述移动指令对应的对地移动轨迹。

具体地，足式机器人得到移动指令后，确定移动指令对应的对地移动轨迹，对地移动轨迹是指在地面作为参照的移动轨迹，也就是说，对地移动轨迹为在地面上建立坐标系，足式机器人在该坐标系中移动的轨迹，以地面建立坐标系时，地面作为参照物，是固定不动的。

为了为了使机器人能够平滑稳定的行走移动，对于轨迹的规划还需满足一定的约束条件，即位置和速度要连续可导。为实现这一目的，轨迹的规划使用了梯形曲线，让每一步行走都经历一个加速、匀速、减速的过程，公式(1)为梯形曲线的函数关系，后面所有的轨迹都是在此基础上规划出来的。

其中，s(t)表示一步的移动距离，a表示加速阶段的加速度，t表示时间，t_a表示加速阶段的时间，T表示一步的时间，v表示速度。

足式机器人是一个移动的平台，需要同时完成四个脚尖以及身体的轨迹规划。得到腿和身体相对地面坐标系的坐标，然后根据公式(2)转移到腿坐标系下，再结合运动学反解求出关节的转角，然后发送给对应的关节的电机。对地轨迹包括：脚尖对地轨迹和身体对地轨迹，如公式(3)所示为身体轨迹规划的公式。

其中，^Lx、^Ly、^Lz分别表示腿坐标系下x轴、y轴、z轴方向上的坐标，L表示前进或后退方向上的步长，H表示步高，W表示左或右方向上的步长，π表示圆周率，sin(·)表示正弦函数，cos(·)表示余弦函数，t表示时间，T表示一步的时间，s(t)表示一步的移动距离。

其中，^Bx、^By、^Bz分别表示身体坐标系下x轴、y轴、z轴方向上的坐标，x_prepos、y_prepos、z_prepos分别表示x轴、y轴、z轴方向上的初始位置的坐标，L表示前进或后退方向上的步长，W表示左或右方向上的步长，t表示时间，T表示一步的时间，n表示步数。

步骤S200、根据所述对地移动轨迹，确定各所述关节的转角。

具体地，得到对地移动轨迹之后，根据对地移动轨迹确定各关节的转角。本申请中腿的数量有4条，4条腿按照前后左右的方式分布，每条腿中的连杆有3个。当采用4条腿，且每条腿采用3个连杆时，关节有12个，因此，需要确定每个关节的转角。在每条腿中，与身体连接的关节为第一关节，与第一关节连接的连杆为第一连杆，与第一连杆连接的关节为第二关节，与第二关节连接的连杆为第二连杆，与第二连杆连接的关节为第三关节，与第三关节连接的连杆为第三连杆。

步骤S200具体包括：

步骤S210、根据所述对地移动轨迹，确定所述足式机器人的脚尖对腿轨迹。

步骤S220、根据所述脚尖对腿轨迹，确定各所述关节的转角。

具体地，众所周知，连杆的速度和惯量在不同的坐标系下的表达是不同的。因此定义初始时刻的位姿以及坐标系是非常必要的。在四足机器人控制的下层，完成对身体和脚尖的轨迹规划，那么定义该轨迹的参考坐标系为地面坐标系，地面是固定不动的，而身体的脚的坐标系是相对运动的，为浮动坐标系。根据对地移动轨迹确定脚尖对腿轨迹，在求解逆运动学时，使用以下公式(4)计算出脚尖相对于腿坐标系的坐标，进而求得关节转角。

其中，^Lx_ee表示在腿坐标系下脚尖在x轴方向上的坐标，^Gx表示在地面坐标系下脚尖在x轴方向上的坐标，^BP_G(t)表示在地面坐标系下脚尖在x轴方向上的坐标，^Ly_ee表示在腿坐标系下脚尖在y轴方向上的坐标，^Gy表示在地面坐标系下脚尖在y轴方向上的坐标，^Lz_ee表示在腿坐标系下脚尖在z轴方向上的坐标，^Gz表示在地面坐标系下脚尖在z轴方向上的坐标。

运动学逆解是已知足尖在腿坐标系下的表达(^Lx_ee，^Ly_ee，^Lz_ee)，求解关节转角(q₁，q₂，q₃)。在求解之前，根据实际机器人的行走情况作了限定来简化计算。初始位置定义，连杆参数以及表达如图1和图2所示，机器人在实际运动过程中脚尖不会运动到身体所在平面的上半部分，即：

如图3和图4所示，计算时先将结构投影到YoZ平面分析，计算出第一关节的转角q₁，然后由于第二关节的转角q₂和第三关节的转角q₃始终在一个平面上，将坐标系原点转移到第二关节处分析计算，将问题转化为求解平面两连杆机构的逆运动学，最后分类讨论的出第二关节的转角q₂和第三关节的转角q₃。

在yoz平面内。C点的投影点为D点，由几何关系得：

其中，α为DO与z轴的夹角，β为AO与DO的夹角。

根据结构可以分析得知，不论第一关节的转角q₁转过多少角度。第二连杆和第三连杆始终保持在同一个平面上，将改平面作为基准面建立新的坐标系来求解q₂和q₃。建立如图5所示的坐标系xyz’。在新坐标系下末端的坐标转换公式为：

x′＝^Lx_ee (8)

其中，l₁表示第一连杆的长度。

根据三角函数关系可得：

其中，φ为AC与x轴的夹角，

为AC与AB之间的夹角。

由图6和图7可知，当末端在同一个位置时由于曲腿的方式不同，会有不同的解，定义当机器人为外曲腿模式时，为图6和图7中虚线部分，此时q₃＞0。

当q₃＞0时，

当q₃＜0时，

以图1所示的位置为初始位置，关节转角会相对初始位置变化，绕旋转轴逆时针为正，顺时针为负。

步骤S300、获取所述足式机器人的初始位置，根据所述初始位置和各所述关节的转角，确定各所述连杆的对地轨迹。

具体地，正运动学在已知初始位置和关节转角的情况下求出所有连杆相对地面坐标系的表达。由于在行走过程中机器人本体是移动的，所以先以身体中心为参考点，求出所有连杆相对于身体坐标的表达，然后根据身体坐标系和地面坐标系的转换公式转移到地面上。求解使用了旋量理论来建立运动学模型。

步骤S300具体包括：

步骤S310、根据所述初始位置和各所述关节的转角，确定基于旋量理论的运动学模型。

步骤S320、求解所述运动学模型，得到各所述连杆的对地轨迹。

具体地，将所有的关节和连杆参数传递到身体坐标系B进行分析。因此，当求解运动学时，假设身体坐标系是相对静止的。运动学正向问题是从每个关节的输入角度求解脚尖在身体坐标系B下的位置。在计算角度之前，首先要确定机器人的初始位置，以确定初始速度的螺旋。为便于计算，将所有四条腿垂直向下的位置定义为初始位置，即如图1所示的位置。

所述运动学模型为：

其中，^BS_ij表示在身体坐标系下第i条腿的第j个关节的初始位置的速度螺旋；B_l表示前腿和后腿的间距，B_w表示左腿和右腿的间距，l₁表示第一连杆的长度，l₂表示第二连杆的长度，S_ij表示第i条腿的第j个关节的螺旋轴，T_v表示速度螺旋转换矩阵，

表示初始速度螺旋在身体坐标系下的表达，

表示初始的螺旋，ee_i表示第i条腿的末端位姿，P(·)表示齐次变换矩阵，θ_ij表示第i条腿的第j个关节的角度，ee_io表示初始时刻的第i条腿的末端位姿，Π表示求和操作。

步骤S400、根据所述关节的转角、所述对地移动轨迹以及所述连杆的对地轨迹，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制。

具体地，求解出了机器人在任意时刻下的关节的转角、连杆以及关节相对身体坐标系的位置，将在此基础上建立动力学模型，求解关节和连杆的速度，加速度和力。动力学模型是在已知机器人运动状态和外力的情况下求解关节转矩。在申请的机器人平台中，所有的关节均为转动关节。即每个关节有5维的约束和1维运动。因此，可以根据pluker基坐标系定义初始时刻的约束力螺旋和驱动力螺旋的约束矩阵。

速度的求解和求动力学的方法有很大关联。本文将机器人不同的状态分为了不同的拓扑机构进行分析，四足机器人在运动个过程中主要有以下三种情况，即四脚着地(stand)，双腿着地(trot和walk)以及四条腿悬空(bound)三种情况。在不同的状态下机器人的拓扑机构是不一样的，动力学模型也不一样。在stand和trot状态下，机器人可看做一个有地系统，可以通过建立约束矩阵求得所有杆件的速度，然后代入动力学需模型计算求解。而在bound状态下机器人为一个无地系统无法通过此办法解决，这里先根据初始的速度求得加速度，然后使用积分的办法求出下一次的速度，然后迭代求解。

步骤S400具体包括：

步骤S410、根据所述对地移动轨迹，确定所述足式机器人的移动步态。

具体地，根据仿生学的研究，对于四足动物来讲，在不同的速度下有不同的步态。当在慢速行走是使用慢行走(walk)步态，速度快一些是使用慢行走(trot)步态，追逐猎物或逃跑时使用(腾空)bound步态。随着行走速度的加快，步态不一样，迈腿的顺序以及支撑腿和摆动腿的时间占空比也不一样。在walk和trot步态下的轨迹规划，使得机器人可以在三维空间内任方向上随意行走。如图8和图9所示为这两种步态下的迈步顺序、步长和时间占空比，其中灰色为摆动腿，白色为支撑腿。在每一步之间采用椭圆轨迹过度，其中步长、步高和迈步时间是可调的。在trot步态下，由于加速段和减速段所走的路程为其他时间的一半，会造成腿走的位移与身体走的位移相差半个步长。所以这里将初始和结束时迈步的步长设定为半个步长，但行走的时间和其他时间段是一样的。

当机器人以trot步态行走时，两条腿支撑，另外两条腿摆动，对于动力学模型存在两种不同的拓扑结构，但是两种结构的分析方式是一样的。此时机器人是一个全约束机构，包括身体和摆动相的两条腿。整体的动力学模型建模方式是一样的。

步骤S420、当所述移动步态为行走步态时，根据所述关节的转角以及所述连杆的对地轨迹，确定基于旋量理论的动力学模型。

具体地，这里的行走步态包括慢行走步态(walk)和快行走步态(trot)，当移动步态为行走步态时，根据关节的转角以及连杆的对地轨迹，确定估计与旋量理论的动力学模型。

步骤S420具体包括：

步骤S421、当所述移动步态为行走步态时，根据所述关节的转角以及所述连杆的对地轨迹，确定约束矩阵。

步骤S422、根据所述约束矩阵，确定所述动力学模型。

所述动力学模型为：

其中，I表示连杆惯量，C表示约束矩阵，T表示转置，a表示所有连杆的加速度，η表示所有关节的作用力，f_p表示受力参数，c_a表示运动参数。

机器人的每条腿与地面相当于被动球铰副连接，有三维移动被约束，此时身体是末端，机器人可看做一个并联机构，定义每个关节对其父连杆的力为负，对子连杆的力为正，可建立约束矩阵。需要说明的是，对于单个关节来说，与该关节的上端连接的连杆为父连杆，与该关节的下端连接的连杆为子连杆，例如，就每条腿来说，第一关节的父连杆为身体，第一关节的子连杆为第一连杆。

得到每个关节的约束矩阵后，接着将所有关节的约束矩阵写成一个更大的约束矩阵C。其中每一行表示一个连杆，这里一共包含13个连杆(需要说明的是，身体(body)也相当于1个连杆)，每一列表示一个关节，而且对于同一个关节约束和驱动是分别考虑的。

根据约束矩阵的公式，C_ij表示j关节对i连杆的作用力，定义每一个关节对父连杆的作用力为负，对子连杆的作用力为正。可得到如下作用力的约束矩阵，具体为一个稀疏矩阵，因此，可以通过特殊的算法进行计算求解，来提高动力学的计算效率。

对于约束

对于驱动

其中，m表示约束矩阵的行，n表示列。

表示初始约束力螺旋在身体坐标系下的表达，

表示初始驱动力螺旋在身体坐标系下的表达。i表示第i条腿，j表示第j个关节。

其中，±1表示每一个关节的速度约束螺旋和驱动螺旋，body表示身体，l_ij表示第i条腿第j个连杆的长度，r_ij表示第i条腿第j个关节的约束力，m_ij表示第i条腿第j个关节的驱动力。

根据能量守恒可知：

其中，C表示约束矩阵，T表示转置，C_v表示关节旋转的功率，v表示所有连杆的速度，

表示第i条腿第j个关节的角速度。求的所有连杆当前的速度就知道了机器人此时全身的运动学状态。

同一个运动学状态下的连杆惯量在不同的坐标系下表达是不一样的，所以把所有连杆的惯量转通过公式(4)移到地面坐标系下研究。

首先，在方程中，每个连杆的惯性将从连杆的质心传递到身体：

其中，^BI_ij表示连杆惯量在身体坐标系下的表达，^BT_f表示力螺旋转换矩阵，I_ijo表示在地面坐标系下第i条腿的第j个关节的初始时刻的连杆惯量，T表示转置。

其中，连杆惯量在地面坐标系下的表达，表达了连杆质心与参考坐标系之间的关系，可以组合成一个大矩阵：

I＝diag(^BI_ij) (25)

其中，I表示连杆惯量，diag(·)表示对角矩阵函数，I_ij表示连杆惯量在身体坐标系下的表达。

对于任意的连杆，受力平衡条件为：

f_total＝Ia+v×*Iv (26)

其中f_total包括重力，外力，约束力，I表示连杆惯量，a表示连杆旋量加速度，v表示旋量速度，因此：

-Ia+f_e＝-f_e-f_g+v×*Iv (27)

其中，f_c表示约束力，f_e表示外力，f_g表示重力。

定义f_p＝-f_e-f_g+v×*Iv (28)

其中，f_p表示受力参数。

此外对公式(上面求速度的公式)两边求导可得

其中，

表示旋量加速度，

表示第i条腿第j个关节的角加速度，

表示约束矩阵的求导，T表示转置，v表示旋量速度。

定义

其中，c_a表示运动参数。

则：C^Ta＝C_a (31)

将所有杆件的平衡方程组合到一起，然后合并公式(3)我们可以得带整个系统的动力学方程：

其中，I表示连杆惯量，C表示约束矩阵，T表示转置，a为所有连杆的加速度，η为所有关节的作用力，包括约束力和驱动力，f_p表示，c_a表示运动参数。

步骤S430、求解所述动力学模型，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制。

具体地，当确定动力学模型后，求解动力学模型，得到关节的速度、加速度和力，以及连杆的速度、加速度和力，从而可以根据关节的速度、加速度和力，以及连杆的速度、加速度和力实现对足式机器人进行控制。

步骤S440、当移动步态为腾空步态，根据所述关节的转角以及所述连杆的对地轨迹，确定所述足式机器人的整体的约束矩阵。

步骤S450、根据所述整体的约束矩阵，确定各连杆的加速度。

步骤S460、针对每个连杆，根据该连杆的加速度，确定该连杆在下一个循环中该连杆的速度。

当移动步态为腾空步态时，根据关节的转角以及连杆的对地轨迹，确定足式机器人的整体的约束矩阵。当机器人跳跃或者奔跑时，有一段时间是腾空状态，此时机器人是一个无地系统，包括5个末端，属于欠约束机构，则无法通过计算约束来求得所有杆件的速度。

足式机器人的模型为浮基模型。很难求出每条腿的速度。需要用积分来计算。当机器人开始行走时，它的位置是已知的，速度为零。这组数据可以用来获得连杆的实际加速度。然后在下一个循环中，可以通过数值积分来获得空间速度。

此时机器人整体的约束矩阵为：

得到连杆的加速度之后，可以在下一个循环迭代计算出下一次连杆的速度。虽然连杆的加速度是时刻变化的，但是由于采用高频的实时控制，可以以1000Hz的频率计算，能够使快速根据连杆加速的变化刷新速度，已达到连续稳定的控制效果。

基于上述任意一实施例的足式机器人的控制方法，本发明还提供了一种计算机设备的较佳实施例：

本发明实施例的计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现如下步骤：

获取所述足式机器人的移动指令，并确定所述移动指令对应的对地移动轨迹；

根据所述对地移动轨迹，确定各所述关节的转角；

获取所述足式机器人的初始位置，根据所述初始位置和各所述关节的转角，确定各所述连杆的对地轨迹；

根据所述关节的转角、所述对地移动轨迹以及所述连杆的对地轨迹，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制。

应用旋量理论简化四足机器人的运动学和动力学建模，使控制过程求解更加快速，从而在更短的时间内完成相关计算和规划，进而提高机器人的动态性能

基于上述任意一实施例的足式机器人的控制方法，本发明还提供了一种计算机可读存储介质的较佳实施例：

本发明实施例的计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如下步骤：

获取所述足式机器人的移动指令，并确定所述移动指令对应的对地移动轨迹；

根据所述对地移动轨迹，确定各所述关节的转角；

获取所述足式机器人的初始位置，根据所述初始位置和各所述关节的转角，确定各所述连杆的对地轨迹；

根据所述关节的转角、所述对地移动轨迹以及所述连杆的对地轨迹，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制。

应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (10)

1.一种足式机器人的控制方法，所述足式机器人包括：身体以及与身体连接的若干条腿；每条腿包括：相互连接的至少两个连杆，相邻两个连杆、所述连杆与所述身体均通过关节连接；其特征在于，所述控制方法包括步骤：

获取所述足式机器人的移动指令，并确定所述移动指令对应的对地移动轨迹；

根据所述对地移动轨迹，确定各所述关节的转角；

获取所述足式机器人的初始位置，根据所述初始位置和各所述关节的转角，确定各所述连杆的对地轨迹；

根据所述关节的转角、所述对地移动轨迹以及所述连杆的对地轨迹，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制。

2.根据权利要求1所述的足式机器人的控制方法，其特征在于，所述获取所述足式机器人的初始位置，根据所述初始位置和各所述关节的转角，确定各所述连杆的对地轨迹，包括：

根据所述初始位置和各所述关节的转角，确定基于旋量理论的运动学模型；

求解所述运动学模型，得到各所述连杆的对地轨迹。

3.根据权利要求2所述的足式机器人的控制方法，其特征在于，所述腿的数量有4条，4条腿按照前后左右的方式分布，每条腿中的连杆有3个；所述运动学模型为：

^BS₁₁＝(B_l/2，0，-B_w/2，-1，0，0)；^BS₁₂＝(B_l/2，0，-B_w/2-l₁，0，0，-1)；^BS₁₃＝(B_l/2，-l₂，-B_w/2-l₁，0，0，-1)

^BS₂₁＝(-B_l/2，0，-B_w/2，-1，0，0)；^BS₂₂＝(-B_l/2_，0，-B_w/2-l₁，0，0，-1)；^BS₂₃＝(-B_l/2，-l₂，-B_w/2-l₁，0，0，-1)

^BS₃₁＝(-B_l/2，0，B_w/2，1，0，0)；^BS₃₂＝(-B_l/2，0，B_w/2+l₁，0，0，1)；^BS₃₃＝(-B_l/2，-l₂，B_w/2+l₁，0，0，1)

^BS₄₁＝(B_l/2，0，B_w/2，1，0，0)；^BS₄₂＝(B_l/2，0，B_w/2+l₁，0，0，1)；^BS₄₃＝(B_l/2，-l₂，B_w/2+l₁，0，0，1)

其中，^BS_ij表示在身体坐标系下第i条腿的第j个关节的初始位置的速度螺旋；B_l表示前腿和后腿的间距，B_w表示左腿和右腿的间距，l₁表示第一连杆的长度，l₂表示第二连杆的长度，S_ij表示第i条腿的第j个关节的螺旋轴，T_v表示速度螺旋转换矩阵，

表示初始速度螺旋在身体坐标系下的表达，

表示初始的螺旋，ee_i表示第i条腿的末端位姿，P(·)表示齐次变换矩阵，θ_ij表示第i条腿的第j个关节的角度，ee_io表示初始时刻的第i条腿的末端位姿，Π表示求和操作。

4.根据权利要求1所述的足式机器人的控制方法，其特征在于，所述根据所述关节的转角、所述对地移动轨迹以及所述连杆的对地轨迹，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制，包括：

根据所述对地移动轨迹，确定所述足式机器人的移动步态；

当所述移动步态为行走步态时，根据所述关节的转角以及所述连杆的对地轨迹，确定基于旋量理论的动力学模型；

求解所述动力学模型，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制。

5.根据权利要求4所述的足式机器人的控制方法，其特征在于，所述当所述移动步态为行走步态时，根据所述关节的转角以及所述连杆的对地轨迹，确定基于旋量理论的动力学模型，包括：

当所述移动步态为行走步态时，根据所述关节的转角以及所述连杆的对地轨迹，确定约束矩阵；

根据所述约束矩阵，确定所述动力学模型。

6.根据权利要求5所述的足式机器人的控制方法，其特征在于，所述动力学模型为：

其中，I表示连杆惯量，C表示约束矩阵，T表示转置，a表示所有连杆的加速度，η表示所有关节的作用力，f_p表示受力参数，c_a表示运动参数。

7.根据权利要求4所述的足式机器人的控制方法，其特征在于，所述根据所述关节的转角、所述对地移动轨迹以及所述连杆的对地轨迹，确定所述关节的速度、加速度和力，以及所述连杆的速度、加速度和力，以对所述足式机器人进行控制，还包括：

当所述移动步态为腾空步态，根据所述关节的转角以及所述连杆的对地轨迹，确定所述足式机器人的整体的约束矩阵；

根据所述整体的约束矩阵，确定各连杆的加速度；

针对每个连杆，根据该连杆的加速度，确定该连杆在下一个循环中该连杆的速度。

8.根据权利要求1所述的足式机器人的控制方法，其特征在于，所述根据所述对地移动轨迹，确定各所述关节的转角，包括：

根据所述对地移动轨迹，确定所述足式机器人的脚尖对腿轨迹；

根据所述脚尖对腿轨迹，确定各所述关节的转角。

9.一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至8中任一项所述方法的步骤。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至8中任一项所述的方法的步骤。

Images