多足机器人关节控制方法、装置和多足机器人

摘要

本发明实施例公开了一种多足机器人关节控制方法、装置和多足机器人，方法包括：构建躯干受力到所有支撑腿的足底期望支撑力的映射矩阵；对躯干受力进行目标优先级划分并确定每一优先级目标的权重矩阵并基于映射矩阵和权重矩阵构建各优先级目标关于足底期望支撑力的优化模型；对各优化模型依次求解以得到对应的足底期望支撑力；其中，当前优先级目标对应的足底期望支撑力作为下一优先级目标的足底期望支撑力的求解基础；基于足底期望支撑力和雅克比矩阵计算支撑腿的关节力矩以用于关节控制。本发明的技术方案可很好地解决现有技术中无论如何调整躯干受力的权重而各分量都会相互影响的这一大难题，且对于不同步态，也无需在更换步态时调整权重等。

权利要求

1.一种多足机器人关节控制方法，其特征在于，包括：获取多足机器人当前的躯干位姿和躯干受力，及每条支撑腿的各个关节角，其中，所述躯干受力包括所述躯干在不同方向的受力；

根据所述躯干位姿、所述躯干受力和所述每条支撑腿的各个关节角构建所述躯干受力到所有支撑腿的足底期望支撑力的映射矩阵；

对所述不同方向的受力进行目标优先级划分以得到若干个优先级目标，确定每一优先级目标的权重矩阵并基于所述映射矩阵和所述权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型；

按照优先级由高到低的顺序，对各优先级目标的所述优化模型依次求解以得到各优先级目标对应的足底期望支撑力；其中，当前优先级目标对应的足底期望支撑力作为下一优先级目标对应的足底期望支撑力的求解基础；

基于求解得到的所述足底期望支撑力和对应支撑腿的雅克比矩阵计算所述对应支撑腿的各关节力矩以用于关节控制；

其中，所述确定每一优先级目标的权重矩阵并基于所述映射矩阵和所述权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型，包括：根据对应的优先级目标中的元素个数及各元素在所述躯干受力中的位置，从所述映射矩阵中选取对应的行向量构建所述对应的优先级目标的子映射矩阵；

对于第i个优先级目标Fi，对应的权重矩阵记为Ki，对应的子映射矩阵记为Li，1≤i≤N，其中，符号argmin表示使目标取得最小值时的变量值；pinv表示伪逆运算符号，则对应的足底期望支撑力FLi采用如下优化模型中的任意一种进行求解：优化模型一：

优化模型二：

2.根据权利要求1所述的方法，其特征在于，所述不同方向的受力包括所述躯干分别在X、Y和Z方向受到的力和力矩，所述根据所述躯干位姿、所述躯干受力和所述每条支撑腿的各个关节角构建所述躯干受力到所有支撑腿的足底期望支撑力的映射矩阵，包括：在所述多足机器人的躯干上选取一点作为躯干坐标系的原点，根据所述躯干位姿和对应支撑腿的各个关节角通过正运动学算法计算所述躯干坐标系的原点到对应支撑腿的足底末端的位置矢量；

根据所有支撑腿的足底期望支撑力、所述躯干的重力和所述躯干受到的三维力构建第一方程，所述三维力由X、Y和Z方向的力构成；

根据各支撑腿的足底期望支撑力与对应支撑腿的所述位置矢量及所述躯干受到的三维力矩构建第二方程，所述三维力矩由X、Y和Z方向的力矩构成；

基于所述第一方程和所述第二方程计算所述躯干受力到所有支撑腿的足底期望支撑力的映射矩阵。

3.根据权利要求2所述的方法，其特征在于，若所述多足机器人为四足机器人且每个腿具有3个关节，当所述四足机器人为四腿支撑时，所述躯干受力记为FB，四条支撑腿的足底期望支撑力分别记为FFL、FFR、FRL和FRR，此时的映射矩阵记为Lall，（Fx，Fy，Fz）和（τx，τy，τz）分别为所述躯干在X，Y，Z方向上受到的力和力矩，则所述映射矩阵满足如下公式：

4.根据权利要求1所述的方法，其特征在于，所述当前优先级目标对应的足底期望支撑力作为下一优先级目标对应的足底期望支撑力的求解基础包括：在当前优先级目标对应的足底期望支撑力的零空间中求解下一优先级目标对应的足底期望支撑力。

5.根据权利要求4所述的方法，其特征在于，若所述多足机器人在水平面上行走，所述不同方向的受力划分为高优先级的第一优先级目标和低优先级的第二优先级目标，其中，Fx、Fy和Fz分别表示躯干在X、Y和Z方向上受到的力，τx、τy和τz分别表示躯干在X、Y和Z方向上受到的力矩，则有：所述第一优先级目标

所述第二优先级目标

6.根据权利要求5所述的方法，其特征在于，所述按照优先级由高到低的顺序，对各优先级目标的所述优化模型依次求解以得到各优先级目标对应的足底期望支撑力，包括：对所述第一优先级目标构建的优化模型进行最优求解，得到满足所述第一优先级目标的足底期望支撑力，并求解所述第一优先级目标对应的足底期望支撑力的所述零空间；

在所述第一优先级目标对应的足底期望支撑力的所述零空间中，对所述第二优先级目标构建的优化模型进行最优求解，将能够满足或最接近所述第二优先级目标对应的解作为所述第二优先级目标的足底期望支撑力；

其中，若所述第一优先级目标F1对应的足底期望支撑力FL1的零空间表示为N(L1)，所述第一优先级目标F1对应的子映射矩阵为L1；所述第二优先级目标的权重矩阵为K2且对应的子映射矩阵为L2，则求解所述第二优先级目标F2对应的足底期望支撑力FL2时，计算方式如下：

7.根据权利要求1所述的方法，其特征在于，所述多足机器人的每个腿中距离躯干最近的关节记为第一关节，所述基于求解得到的所述足底期望支撑力和对应支撑腿的雅克比矩阵计算所述对应支撑腿的各关节力矩，包括：以对应支撑腿的所述第一关节为基座，根据所述对应支撑腿的各关节角度计算所述第一关节到足底末端的雅克比矩阵；

通过将所述雅克比矩阵的转置与所述对应支撑腿的足底期望支撑力进行相乘以计算得到所述对应支撑腿的各关节力矩；

将所述各关节力矩发送至对应支撑腿的对应关节电机以进行关节控制。

8.一种多足机器人关节控制装置，其特征在于，包括：信息获取模块，用于获取多足机器人当前的躯干位姿和躯干受力，及每条支撑腿的各个关节角，其中，所述躯干受力包括所述躯干在不同方向的受力；

映射矩阵构建模块，用于根据所述躯干位姿、所述躯干受力和所述每条支撑腿的各个关节角构建所述躯干受力到所有支撑腿的足底期望支撑力的映射矩阵；

目标优先级划分模块，用于对所述躯干在不同方向的受力进行目标优先级划分以得到若干个优先级目标，确定每一优先级目标的权重矩阵并基于所述映射矩阵和所述权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型；

足底期望支撑力求解模块，用于按照优先级由高到低的顺序，对各优先级目标的所述优化模型依次求解以得到各优先级目标对应的足底期望支撑力；其中，当前优先级目标对应的足底期望支撑力作为下一优先级目标对应的足底期望支撑力的求解基础；

关节力矩控制模块，用于基于求解得到的所述足底期望支撑力和对应支撑腿的雅克比矩阵计算所述对应支撑腿的各关节力矩以用于关节控制；

其中，所述确定每一优先级目标的权重矩阵并基于所述映射矩阵和所述权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型，包括：根据对应的优先级目标中的元素个数及各元素在所述躯干受力中的位置，从所述映射矩阵中选取对应的行向量构建所述对应的优先级目标的子映射矩阵；

对于第i个优先级目标Fi，对应的权重矩阵记为Ki，对应的子映射矩阵记为Li，1≤i≤N，其中，符号argmin表示使目标取得最小值时的变量值；pinv表示伪逆运算符号，则对应的足底期望支撑力FLi采用如下优化模型中的任意一种进行求解：优化模型一：

优化模型二：

9.一种多足机器人，其特征在于，利用权利要求1至7中任一项所述的多足机器人关节控制方法来进行关节控制。

10.一种计算机可读存储介质，其特征在于，其存储有计算机程序，在所述计算机程序被执行时，实施根据权利要求1至7中任一项所述的多足机器人关节控制方法。

说明书

多足机器人关节控制方法、装置和多足机器人

技术领域

[0001] 本发明涉及多足机器人技术领域，尤其涉及一种多足机器人关节控制方法、装置和多足机器人。

背景技术

[0002] 无论是位置控制关节还是力控制关节的四足机器人，都需要计算该机器人各个关节所需要的力矩。在控制该四足机器人行走过程中，控制目标通常是使机器人的躯干到达某个状态（位置和速度），控制器计算得到的就是机器人躯干到达指定状态需要的虚拟力和力矩，这是一个6维向量（简称6维力）。由于机器人的躯干不能直接受到除重力以外的外力，因此需要将机器人的躯干受力映射到与地面接触的足底的支撑力来实现运动控制。

[0003] 目前，在求解该足底支撑力的过程中，传统的优化求解方法是通过调节这虚拟6维力的权重矩阵来区分这6维力中各分量的重要性，例如，当认为机器人x方向的运动比较重要而y方向的运动不太重要，就会在构建优化问题时让x方向虚拟力的权重远远大于y方向虚拟力的权重，进而求解到对应的足底期望支撑力。然而，虽然采用基于权重矩阵的方法的求解结果是最优的，但是其求解的计算时间长，而且代码复杂，对硬件要求也高。此外，该类方法还会带来两个问题，即首先无论权重差别多大，低权重的分量仍然会影响高权重的分量；其次对于不同的步态，由于机器人的支撑腿数量和位置不同，往往需要不同的权重，因此对于该权重矩阵需根据不同的情况而不断调整，导致计算更加复杂等。

发明内容

[0004] 有鉴于此，本发明目的是为了克服现有技术中的不足，提供一种多足机器人关节控制方法、装置和多足机器人。

[0005] 本发明一实施例提供一种多足机器人关节控制方法，包括：

[0006] 获取多足机器人当前的躯干位姿和躯干受力，及每条支撑腿的各个关节角，其中，所述躯干受力包括所述躯干在不同方向的受力；

[0007] 根据所述躯干位姿、所述躯干受力及所述每条支撑腿的各个关节角构建所述躯干受力到所有支撑腿的足底期望支撑力的映射矩阵；

[0008] 对所述不同方向的受力进行目标优先级划分以得到若干优先级目标，然后确定每一优先级目标的权重矩阵并基于所述映射矩阵和所述权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型；

[0009] 按照优先级由高到低的顺序，对各优先级目标的所述优化模型依次求解以得到各优先级目标对应的足底期望支撑力；其中，当前优先级目标对应的足底期望支撑力作为下一优先级目标对应的足底期望支撑力的求解基础；

[0010] 基于求解得到的所述足底期望支撑力和对应支撑腿的雅克比矩阵计算所述对应支撑腿的各关节力矩以用于关节控制。

[0011] 在一些实施例中，所述不同方向的受力包括所述躯干分别在X、Y和Z方向受到的力和力矩，所述根据所述躯干位姿、所述躯干受力和所述每条支撑腿的各个关节角构建所述躯干受力到所有支撑腿的足底期望支撑力的映射矩阵，包括：

[0012] 在所述多足机器人的躯干上选取一点作为躯干坐标系的原点，根据所述躯干的位姿和对应支撑腿的各个关节角通过正运动学算法计算每一支撑腿的足底末端到所述躯干坐标系原点的位置矢量；

[0013] 根据所有支撑腿的足底期望支撑力、所述躯干的重力和所述躯干受到的三维力构建第一方程，所述三维力由X、Y和Z方向的力构成；

[0014] 根据各支撑腿的足底期望支撑力与对应支撑腿的所述位置矢量及所述躯干受到的三维力矩构建第二方程，所述三维力矩由X、Y和Z方向的力矩构成；

[0015] 基于所述第一方程和所述第二方程计算所述躯干受力到各支撑腿的足底期望支撑力的映射矩阵。

[0016] 在一些实施例中，若所述多足机器人为四足机器人并且每个腿具有3个关节，当所述四足机器人为四腿支撑时，所述躯干受力记为FB，四条支撑腿的足底期望支撑力分别记为FFL、FFR、FRL和FRR，此时的映射矩阵记为Lall，（ ）和（ ）分别为所述躯干在x，y，z方向上受到的力和力矩，则所述映射矩阵满足如下公式：

[0017] ；

[0018] 。

[0019] 在一些实施例中，所述确定每一优先级目标的权重矩阵并基于所述映射矩阵和所述权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型，包括：

[0020] 根据对应的优先级目标中的元素个数及各元素在所述躯干受力中的位置，从所述映射矩阵中选取对应的行向量构建所述对应的优先级目标的子映射矩阵；

[0021] 对于第i个优先级目标Fi，对应的权重矩阵记为Ki，对应的子映射矩阵记为Li，1≤i≤N，则对应的足底期望支撑力FLi采用如下优化模型中的任意一种进行计算：

[0022] 优化模型一： ；

[0023] 优化模型二： 。

[0024] 在一些实施例中，所述当前优先级目标对应的足底期望支撑力作为下一优先级目标对应的足底期望支撑力的求解基础包括：

[0025] 在当前优先级目标对应的足底期望支撑力的零空间中求解下一优先级目标对应的足底期望支撑力。

[0026] 在一些实施例中，若所述多足机器人在水平面上行走，所述不同方向的受力划分为高优先级的第一优先级目标和低优先级的第二优先级目标，其中， 分别表示躯干在x、y和z方向上受到的力， 和 分别表示躯干在x、y和z方向上受到的力矩，则有：

[0027] 所述第一优先级目标 ；

[0028] 所述第二优先级目标 。

[0029] 在上述的实施例中，所述按照优先级由高到低的顺序，对各优先级目标的所述优化问题依次求解以得到各优先级目标对应的足底期望支撑力，包括：

[0030] 对所述第一优先级目标构建的优化模型进行最优求解，得到满足所述第一优先级目标的足底期望支撑力，并求解所述第一优先级目标对应的足底期望支撑力的所述零空间；

[0031] 在所述第一优先级目标对应的足底期望支撑力的所述零空间中，对所述第二优先级目标构建的优化模型进行最优求解，将能够满足或最接近所述第二优先级目标对应的解作为所述第二优先级目标的足底期望支撑力；

[0032] 其中，若所述第一优先级目标 对应的足底期望支撑力 的零空间表示为，所述第一优先级目标 对应的子映射矩阵为 ；所述第二优先级目标的权重矩阵为 且对应的子映射矩阵为 ，则求解所述第二优先级目标 对应的足底期望支撑力时，计算方式如下：

[0033] 。

[0034] 在一些实施例中，所述多足机器人的每个腿中距离躯干最近的关节记为第一关节，所述基于求解得到的所述足底期望支撑力和对应支撑腿的雅克比矩阵计算所述对应支撑腿的各关节力矩以用于关节控制，包括：

[0035] 以对应支撑腿的所述第一关节为基座，根据所述对应支撑腿的各关节角度计算所述第一关节到足底末端的雅克比矩阵；

[0036] 通过将所述雅克比矩阵的转置与所述对应支撑腿的足底期望支撑力进行点乘以计算得到所述对应支撑腿的各关节力矩；

[0037] 将所述各关节力矩发送至对应支撑腿的对应关节电机以进行关节控制。

[0038] 本发明另一实施例提出一种多足机器人关节控制装置，包括：

[0039] 信息获取模块，用于获取多足机器人当前的躯干位姿和躯干受力，及每条支撑腿的各个关节角，其中，所述躯干受力包括所述躯干在不同方向的受力；

[0040] 映射矩阵构建模块，用于根据所述躯干位姿、所述躯干受力和所述每条支撑腿的各个关节角构建所述躯干受力到所有支撑腿的足底期望支撑力的映射矩阵；

[0041] 目标优先级划分模块，用于对所述躯干在不同方向的受力进行目标优先级划分以得到若干个优先级目标，确定每一优先级目标的权重矩阵并基于所述映射矩阵和所述权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型；

[0042] 足底期望支撑力求解模块，用于按照优先级由高到低的顺序，对各优先级目标的所述优化模型依次求解以得到各优先级目标对应的足底期望支撑力；其中，当前优先级目标对应的足底期望支撑力作为下一优先级目标对应的足底期望支撑力的求解基础；

[0043] 关节力矩控制模块，用于基于求解得到的所述足底期望支撑力和对应支撑腿的雅克比矩阵计算所述对应支撑腿的各关节力矩以用于关节控制。

[0044] 本发明又一实施例提出一种多足机器人，利用上述的多足机器人关节控制方法进行关节控制。

[0045] 本发明再一实施例提出一种计算机可读存储介质，其存储有计算机程序，在所述计算机程序被执行时，实施上述的多足机器人关节控制方法。

[0046] 本发明的实施例具有如下优点：

[0047] 本发明实施例的技术方案根据机器人运动学先计算得到从机器人躯干受力到各支撑腿的足底期望支撑力的映射矩阵，再利用分层思想，对该躯干受力进行目标优先级划分并为每一优先级目标构建优化模型以用于求解对应的足底期望支撑力；最后利用这些求解得到的足底期望支撑力并基于雅克比矩阵进行关节力矩计算以用于关节控制；其中，将高低先级目标的对应的足底期望支撑力作为低优先级目标的求解基础，使得低优先级目标的求解任务完全不会影响高优先级目标的求解任务，不仅可以很好地解决现有技术中无论如何调整权重矩阵的各权重都会影响相互影响的难题，而且对于不同步态，也不需要在更换步态时调整权重；此外，由于分层计算，代码量小，计算速度快，对硬件的性能需求也可降低等。

附图说明

[0048] 为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。

[0049] 图1示出了本发明实施例的多足机器人关节控制方法的第一流程示意图；

[0050] 图2a、图2b和图2c分别示出了本发明实施例的多足机器人的不同支撑状态示意图；

[0051] 图3示出了本发明实施例的多足机器人关节控制方法的第二流程示意图；

[0052] 图4示出了本发明实施例的多足机器人关节控制方法的包括3个关节的腿的关节力矩分析示意图；

[0053] 图5示出了本发明实施例的多足机器人关节控制装置的结构示意图。

具体实施方式

[0054] 下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

[0055] 通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

[0056] 在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

[0057] 此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

[0058] 除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

[0059] 实施例1

[0060] 请参照图1，本实施例提出一种多足机器人关节控制方法。该方法主要包括以下步骤：

[0061] 步骤S10，获取多足机器人当前的躯干位姿和躯干受力、及每条支撑腿的各个关节角。

[0062] 示范性地，可通过惯性测量单元IMU或其他姿态传感器等测量得到该多足机器人的躯干位姿（包括位置和姿态）。该躯干受力可由控制器计算得到，通常地，当需要控制机器人躯干到达某个状态时，需要根据规划的躯干轨迹或目标状态等先确定该多足机器人所需的躯干受力。其中，该躯干受力包括该躯干在不同方向上的受力。通常地，该躯干受力为一个六维向量，包括分别在X、Y和Z方向受到的力和X、Y和Z方向受到的力矩。

[0063] 对于机器人而言，腿的相位包括支撑相和摆动相两种，支撑相是指步行足落地到抬起前所处的状态，而摆动相则指将步行足抬起至下落到支撑面的过程。本实施例中，上述的支撑腿是指当前处于支撑相的腿，也称着地脚。其中，可根据机器人各腿的相位信息来判断出当前处于支撑相的腿的数量，即上述的支撑腿的数量。

[0064] 对于步骤S10，将获取每条支撑腿的各个关节角。可以理解，上述的关节角是指一条腿中各个关节的角度。通常地，机器人的每条腿上均设有相应的传感器以采集各关节信息，对于每条支撑腿的各个关节角，示范性地，可分别通过对应支撑腿上设置的角位移传感器或位置编码器等采集得到。

[0065] 步骤S20，构建躯干受力到所有支撑腿的足底期望支撑力的映射矩阵。

[0066] 步骤S20主要用于根据步骤S10中获取的躯干位姿、躯干受力和每条支撑腿的各个关节角等信息来构建该多足机器人的躯干受力到支撑腿的足底期望支撑力的映射矩阵。值得注意的是，机器人所处的支撑状态与当前处于支撑相的腿数量有关。当机器人所处的支撑状态时不同，对应的映射矩阵也将不同。

[0067] 例如，若一机器人有N条腿，根据各腿的相位得知处于支撑相的腿数量为 ，处于摆动相的腿数量为 。以一四足机器人为例，图2a、图2b和图2c列出了该四足机器人的三种典型的支撑情况，分别为四腿支撑（ ）、对角腿支撑（ ）和同侧腿支撑（）。当然，该四足机器人还有其他支撑情况，在此就不一一列举。

[0068] 本实施例中，该躯干受力包括三维力和三维力矩，其中，三维力是指躯干分别在X、Y和Z方向受到的力，而三维力矩是指躯干分别在X、Y和Z方向受到的力矩。本实施例中，将基于该躯干受到的三维力和三维力矩分别与各支撑腿的足底期望支撑力之间的关系方程来求解出映射矩阵。

[0069] 在一种实施例中，如图3所示，步骤S20包括以下子步骤：

[0070] 子步骤S210，在机器人躯干上选取一点作为躯干坐标系的原点，根据该躯干的位姿和对应支撑腿的各个关节角通过正运动学算法计算该躯干坐标系原点到对应支撑腿的足底末端的位置矢量。本实施例中，优选地，将该机器人躯干的质心作为躯干坐标系的原点。

[0071] 示范性地，在得知该机器人所处的支撑期后，可计算每个支撑腿的足底末端相对原点的位置。如图2a所示，若一条腿有3个关节，则该支撑腿的足底能够产生一个3维的支撑力，于是，根据该支撑腿的各个关节角及该躯干的姿态，基于机器人正运动学算法可求解得到该支撑腿的足底末端在躯干质心坐标系中的位置 ，即：

[0072] ；

[0073] 其中， 分别表示该支撑腿的三个关节角； 分别表示该躯干的姿态，即滚转角、俯仰角和偏航角； 代表从基座坐标系到足底末端的正运动学函数，具体可参见已有的相关文献，在此不再赘述。

[0074] 子步骤S220，根据所有支撑腿的足底期望支撑力、躯干的重力和躯干受到的三维力构建第一方程。

[0075] 经对躯干的受力分析可知，该躯干受到的三维力应等于各个支撑腿的足底期望支撑力和该躯干的重力的矢量和。基于此，可建立得到第一方程。

[0076] 子步骤S230，根据各支撑腿的足底期望支撑力与对应支撑腿的位置矢量及躯干受到的三维力矩构建第二方程。

[0077] 以躯干坐标系原点为轴，该躯干受到的三维力矩应等于原点的每一支撑腿的足底末端到位置矢量与对应支撑腿的足底期望支撑力的叉乘的和。基于此，可建立得到第二方程。

[0078] 子步骤S240，基于该第一方程和第二方程计算躯干受力到所有支撑腿的足底期望支撑力的映射矩阵。

[0079] 联立上述的两个方程，即可计算得到该六维的躯干受力到所有支撑腿的足底期望支撑力的映射，并用映射矩阵表示。应当理解，上述的步骤S210与S220之间，以及步骤S220和S230之间的步骤执行顺序并不作限定。

[0080] 下面以图2a所示的四足支撑为例，对映射矩形的求解进行说明。

[0081] 若四腿分别表示为FL（Front-Left）、FR（Front-Right）、RL(Rear-Left)、RR(Rear-Right)，即对应于前左腿、前右腿、后左腿和后右腿，则每条腿产生的3维的支撑力依次表示为 。如图2a所示，坐标系原点位于机器人的躯干质心处，原点到各支撑腿的足底末端分别表示为 ，即有：

[0082] ；

[0083] ；

[0084] ；

[0085] 。

[0086] 其中， 分别表示前左腿FL的三个关节角； 分别表示前右腿FR的三个关节角； 分别表示后左腿RL的三个关节角；
分别表示后右腿RR的三个关节角； 分别代表四
条腿的从基座坐标系到足底的运动学函数。

[0087] 若躯干受力 用六维向量 表示，其中， 分别为躯干在x，y，z方向上受到的力， 为x，y，z方向上受到的力矩，而躯干的质量为m，重力加速度为g，此处认为躯干质心与躯干坐标系重合，若不重合则应考虑重力相对于躯干坐标系的力矩即可。于是，建立得到上述各腿的支撑力与躯干受到的三维力的第一方程：

[0088] ；

[0089] 以及，上述各支撑腿的支撑力与躯干受到的三维力矩之间的第二方程：

[0090] 。

[0091] 于是，通过整理上述两个等式，即可以得到从躯干受力到四条支撑腿的足底期望支撑力的映射矩阵 ，该映射矩阵还可表示为：

[0092]

[0093] 同理，通过上述步骤也可以计算得到如图2b和图2c各自对应的映射矩阵 和，由于均为两腿支撑，此时映射矩阵大小 。

[0094] 由于上述求解到的映射矩阵并不存在逆矩阵，即表明物理上表示有多组足底期望支撑力均能满足该组躯干受力。为此，需要构建一个优化问题求解各支撑腿的足底期望支撑力。为了方便描述，将用 来表示支撑腿的足底期望支撑力组成的向量；L表示对应的映射矩阵。例如，四足支撑期时， ；双足支撑期时，或 ，对应有 或 。

[0095] 步骤S30，对躯干在不同方向的受力进行目标优先级划分以得到若干个优先级目标，确定每一优先级目标的权重矩阵并基于该映射矩阵和该权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型。

[0096] 对于上述步骤S30，主要是利用分层的思想来逐步求解出所有支撑腿的足底期望支撑力。本实施例中，通过将躯干受力中的不同方向上的力进行不同优先级的任务目标划分，可方便减化各足底期望支撑力的求解运算量。

[0097] 示范性地，可按照躯干在不同方向的受力的重要程度进行任务目标划分。其中，划分的优先级数量可根据实际需求进行选取，但应满足1

[0098] 在一种实施例中，以机器人在水平路面上行走为例，认为保证机器人躯干的俯仰角（沿y方向的旋转）和滚转角（沿x方向的旋转）以及沿z方向的高度是保证机器人平衡的首要目标，其他次之，那么对于该六维躯干受力中的这6个元素可划分为2个优先级目标，即优先级数量为2，这两个优先级目标分别为：

[0099] 第一优先级目标 ；

[0100] 第二优先级目标 。

[0101] 可以理解，对于划分的优先级数量及每个优先级目标中的元素个数，上述的划分成2个优先级目标且每一优先级目标包括3个元素仅为一种举例，实际运用中可根据实际需求进行相应划分，在此并不作限定。

[0102] 示范性地，上述步骤S30中的“确定每一优先级目标的权重矩阵并基于该映射矩阵和该权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型”主要包括：

[0103] 根据对应的优先级目标中的元素个数及各元素在该躯干受力中的位置，并从求解出的映射矩阵中选取对应的行向量构建该对应的优先级目标的子映射矩阵。

[0104] 得到各优先级目标对应的子映射矩阵后，对于第i个优先级目标Fi，对应的预设的权重矩阵记为Ki，对应的子映射矩阵记为Li，1≤i≤N，则对应的足底期望支撑力FLi采用如下优化模型中的任意一种进行求解：

[0105] 优化模型一： ；

[0106] 优化模型二： ；

[0107] 其中，符号 表示使目标取得最小值时的变量值； 表示伪逆运算符号。优选地，采用上述的伪逆求解法进行求解，可减小计算机的计算负担，从而提高代码的运算速度。其中，上述的各权重矩阵可以根据经验值进行设置。

[0108] 仍以上述的第一优先级目标 为例，由于包含 三个元素，且这三个元素分别位于六维的躯干受力中的第3至5行，故可从由步骤S10中求解的映射矩阵中选取第3至5行的向量来构成该第一优先级目标对应的子映射矩阵。可选地，该第一优先级目标 对应的优化模型采用： 。同理，该第二优先级目标 对应的优化模型
采用： 。

[0109] 步骤S40，按照优先级由高到低的顺序，对各优先级目标的优化模型进行依次求解以得到各优先级目标对应的足底期望支撑力；其中，当前优先级目标对应的足底期望支撑力作为下一优先级目标对应的足底期望支撑力的求解基础。

[0110] 对于步骤S40，主要是利用分层思想，使低优先级目标的求解以高优先级目标的求解结果作为基础，以保证低优先级目标任务不会对高优先级目标任务产生影响。

[0111] 在一种实施方式中，所述的求解基础包括：在当前优先级目标对应的足底期望支撑力的零空间中求解下一优先级目标对应的足底期望支撑力，即在高低优先级目标的解零空间中寻找低优先级目标的解。

[0112] 示范性地，该步骤S40包括，先对优先级最高的目标所构建的优化模型进行最优求解，以得到满足该第一优先级目标的足底期望支撑力。同时，求解到优先级最高的目标的足底期望支撑力后，进一步求解对应的零空间。

[0113] 然后，在该第一优先级目标的足底期望支撑力的该零空间中，对该第二个优先级目标进行最优求解，并将能够满足或尽量满足该第二个优先级目标对应的解作为该第二个优先级目标对应的足底期望支撑力。以此类推，对于其他的优先级目标，依次从上一优先级目标的解零空间中计算得到下一个优先级目标的足底期望支撑力。

[0114] 值得注意的是，在上一优先级目标对应的零空间中，该下一个优先级目标对应的优化模型的解可能不能直接满足当前优化级目标，此时，可将选取最接近该当前优化级目标的解作为对应的足底期望支撑力。可以理解的是，所述的最接近，可以是指通过上述优化模型求解得到的解与目标解之间的偏差最小，或者说，是最趋近于期望目标的最优解。例如，可以是计算得到的解与期望目标之间的距离最近等等。

[0115] 仍以上述的划分为2个优先级目标为例，根据该第一优先级目标 对应的关于足底期望支撑力的优化模型为 ，经最优求解可得到对应的解 ，将其作为第一优先级目标 对应的足底期望支撑力。然后，求解出该第一优先级目标 对应的足底期望支撑力的零空间 。接着，在上述的零空间中，对
第二优先级目标 构建的优化模型进行最优求解，其计算方式如下：

[0116] 。

[0117] 其中，将能够满足该第二优先级目标 的解优先将其作为该第二优先级目标 的足底期望支撑力 。当然，若不存在直接满足目标的解，即不存在同时满足第一优先级目标的零空间和第二优先级目标的优化模型的解，此时则在该第一优先级目标的零空间中，将最能趋近于达到该第二优先级目标 的解作为该第二优先级目标 的足底期望支撑力。

[0118] 可以理解的是，除了上述的零空间求解决方法外，只要能够保证低优先级目标任务不会对高优先级目标任务产生影响即可，也可以采用其他的方法来求解各优先级目标对应的足底期望支撑力，在此并不作限定。

[0119] 步骤S50，基于求解得到的足底期望支撑力和对应支撑腿的雅克比矩阵计算对应支撑腿的各关节力矩以用于关节控制。

[0120] 示范性地，所有优先级目标求解得到的足底期望支撑力即为各支撑腿的足底期望支撑力。于是，基于各支撑腿的足底期望支撑力，可计算各支撑腿的各个关节所需的力矩。若以每条腿的关节数n=3为例，如图4所示，一条支撑腿的足底期望支撑力完全由3个关节力矩 决定。

[0121] 对于机器人而言，其躯干的和腿的尺寸比例通常是固定的，躯干的惯量和腿惯量的比例受各自的质量影响较大，一般地，当躯干质量与单条腿质量之比大于7的时候，则该机器人的腿部质量和惯量就可以忽略。为方便地计算支撑腿的关节力矩，本实施例将忽略机器人腿的质量和惯量。

[0122] 示范性地，将基于雅克比矩阵来计算关节的力矩。若机器人的每个腿中距离躯干最近的关节记为第一关节，则根据对应支撑腿的各关节角度可以计算出该第一关节到足底末端的雅克比矩阵，进而通过将该雅克比矩阵的转置与对应支撑腿的足底期望支撑力进行相乘，即可计算得到对应支撑腿的各关节力矩。示范性地，利用如下公式即可计算得到支撑腿的关节力矩： ；其中，雅克比矩阵由该支撑腿的n个关节角 决定，；为该支撑腿的足底期望支撑力， 为关节力矩， 。

[0123] 于是，将上述求解得到的各关节力矩发送至当前支撑腿的对应关节电机中，以使各关节电机根据该关节力矩进行相应的关节控制。

[0124] 本实施例的多足机器人关节控制方法通过计算得到从机器人躯干受力到各支撑腿的足底期望支撑力的映射矩阵，并利用分层思想，对该躯干受力进行优先级目标划分并为每一优先级目标构建对应的优化模型以用于求解各优先级目标对应的足底期望支撑力；最后将根据求解得到的足底期望支撑力并基于雅克比矩阵进行各支撑腿的关节力矩计算；
其中，将高优先级目标的解作为基础来求解低优先级目标对应的足底期望支撑力，如在高优先级目标的解零空间中求解低优先级的解，这样使低优先级目标的求解任务不会影响高优先级目标，不仅可以很好地解决现有技术中无论如何调整权重矩阵的各权重都会影响相互影响的难题，而且对于不同步态，也不需要在更换步态时调整权重；此外，由于分层计算，代码量小，计算速度快，对硬件的性能需求也可降低等。

[0125] 实施例2

[0126] 请参照图5，基于上述实施例1的方法，本实施例提出一种多足机器人关节控制装置10，包括：

[0127] 信息获取模块110，用于获取多足机器人当前的躯干位姿和躯干受力，及每条支撑腿的各个关节角，其中，所述躯干受力包括所述躯干在不同方向的受力。

[0128] 映射矩阵构建模块120，用于根据所述躯干位姿、所述躯干受力和所述每条支撑腿的各个关节角构建所述躯干受力到所有支撑腿的足底期望支撑力的映射矩阵。

[0129] 目标优先级划分模块130，用于对所述躯干在不同方向的受力进行目标优先级划分以得到若干个优先级目标，确定每一优先级目标的权重矩阵并基于所述映射矩阵和所述权重矩阵构建各优先级目标的关于足底期望支撑力的优化模型。

[0130] 足底期望支撑力求解模块140，用于按照优先级由高到低的顺序，对各优先级目标所述优化模型依次求解以得到各优先级目标对应的足底期望支撑力；其中，当前优先级目标对应的足底期望支撑力作为下一优先级目标对应的足底期望支撑力的求解基础。

[0131] 关节力矩控制模块150，用于基于求解得到的所述足底期望支撑力和对应支撑腿的雅克比矩阵计算所述对应支撑腿的各关节力矩以用于关节控制。

[0132] 可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1的可选项同样适用于本实施例，故在此不再详述。

[0133] 本发明的另一实施例还提出一种多足机器人，例如，可包括但不限于为四足机器人、六足机器人等等。在分析该多足机器人的步态设计及路径规划等时，可执行上述实施例1的方法或实施例2的装置中的各模块进行关节控制。

[0134] 本发明的又一实施例还提供了一种计算机可读存储介质，其存储有计算机程序，在所述计算机程序被实施时，执行上述的多足机器人关节控制方法。

[0135] 在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

[0136] 另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

[0137] 所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是智能手机、个人计算机、服务器、或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。

[0138] 以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。