结合实时运动学预测的足式机器人全身控制方法及装置

1.本技术涉及机器人控制技术领域，特别涉及一种结合实时运动学预测的足式机器人全身控制方法及装置。

背景技术：

2.目前常见的足式机器人的控制方法主要可以分为简化模型方法、完整模型方法、混合模型方法以及生物机理方法四类。
3.其中，简化模型方法通过将足式机器人抽象成某个易于分析的简单模型，根据简化模型的动力学特性对足式机器人进行控制，该模型方法包括：线性倒立摆模型(linear inverted pendulum model，lipm)的控制方法、使用弹簧负载倒立摆(spring loaded inverted pendulum，slip)模型实现的“三分控制”方法、主动阻抗控制方法、使用简化模型的模型预测控制(model predictive control，mpc)方法等；
4.完整模型方法通过将足式机器人建模成由多个刚性连杆和关节节点组成的模型，从而根据完整的动力学模型设计合适的控制方法，其包括：全身控制(whole body control，wbc)方法、使用完整模型的mpc方法等；
5.混合模型方法在足式机器人不同运动状态下使用不同的动力学模型，例如：混合零动态(hybrid zero dynamics，hzd)控制方法等；
6.生物机理方法是根据足式动物的生物机理设计的控制方法，使足式机器人按照足式动物的步态模式运动，例如：基于中枢模式发生器(central pattern generator，cpg)的控制方法等。
7.在上述方法中，mpc可以考虑将来一段时间内的参考轨迹和系统状态，展现出出的控制效果，但是由于其计算量巨大，为了保证mpc的实时运行，mpc中通常只考虑简化模型。而wbc虽然欠缺对未来状态的考虑，但其可以处理当前时刻的完整机器人运动方程，因此，mpc和wbc结合的控制方法正逐步成为足式机器人的主流控制方法。
8.mpc方法是在每一个控制帧内，根据规划好的从当前时刻开始的一段有限时间的运动轨迹以及当前的机器人状态，求解一个离散时间的有限时间长度的最优控制问题，得到未来一段时间内的最优控制序列，并且执行序列中的第一帧控制量。其中的最优控制问题是含有机器人动力学模型等式约束以及其他不等式约束的优化问题。2018年，麻省理工学院技术人员在mpc中使用了单刚体模型，并且进行了线性化处理，使得构建出来的优化问题属于二次规划(quadratic programming，qp)问题，从而可以实现实时控制，其mpc的运行频率可以达到30hz。mpc方法除了用于控制以外，也可以用于运动规划，且每一次求解mpc问题后均可得到未来一段时间内的优化后的运动轨迹。
9.wbc方法。
10.当机器人具有较多的自由度时，为了让机器人执行合适的运动，通常会在操作空间同时给机器人规划多个运动任务，但是受限于机器人本身的运动学和动力学限制，上述任务不一定能够同时很好地完成，有些任务之间有时甚至可能存在矛盾，由此，可利用wbc
方法对上述任务进行优先级区分，重要的或高优先级的任务应该尽可能地完成，而不重要的或低优先级的任务应该尽量在不影响高优先级任务的情况下完成。wbc方法被广泛应用于各种类型的机器人中，足式机器人正是其应用的典型。2016年，技术人员咋电力驱动四足机器人anymal上使用具有严格任务优先级分层的wbc方法，实现了在没有视觉等感知地形的传感器、也没有特殊规划方法的情况下在不平路面行走时的自然适应能力。
11.wbc方法不考虑未来的运动轨迹，只考虑当前的机器人状态和当前的期望运动轨迹。wbc的实现方法主要有零空间投影(null space projection,nsp)方法和基于优化的方法两大类。这里只介绍与本文技术相关的基于优化的方法。wbc可以通过求解单个qp问题来实现也可以通过求解多个qp问题来实现。前者的做法是构建一个qp问题，在同时考虑机器人运动方程、关节角度、力矩限制等约束条件下，尽可能地完成表达在代价函数中的各个任务。这些任务通过权重矩阵来调整它们的相对重要性，因此该方法也称为加权的二次规划(weighted qp，wqp)方法。后者的做法是将不同优先级的任务(和约束)放在不同的qp问题中，先求解高优先级任务的qp问题，然后求解低优先级任务的qp问题。在求解低优先级任务的qp问题时，还必须在高优先级任务的qp问题的最优解集中寻低优先级任务的qp问题的最优解。这种做法也称为分层的二次规划(hierarchical qp，hqp)方法。后者虽然可以实现前者无法实现的严格的任务优先级分层，但是后者的计算量要求远高于前者。
12.目前，mpc方法和wbc方法由于其出众的控制效果，已被广泛应用于足式机器人的控制中，将两者结合的方法也逐步成为足式机器人的主流控制方法，然而，mpc的计算量要求巨大，其优化变量的维数正比于预测的帧数，即便只考虑线性系统的状态方程，最终的qp问题求解可能依然需要消耗大量时间。而足式机器人的运动方程通常都是非线性的，如果在mpc中考虑完整的机器人运动方程，最终需要求解非凸优化问题，计算量较大。因此mpc中通常选择使用简化的(近似的)机器人模型来降低计算量。
13.wbc中虽然考虑了完整的机器人运动方程，但是它只能考虑当前时刻的参考轨迹，基于优化的wbc方法最终得到的最优解仅仅是对于当前时刻而言的最优解，而非将来一段时间内的最优解，wbc无法像mpc那样对参考轨迹进行重优化，因此wbc往往要求规划器提供合理且光滑的参考轨迹。
14.由此，相关技术选择将mpc和wbc结合的方法，通过先计算考虑了简化模型的mpc，进而使用考虑了完整模型的wbc来对控制量进行修正。从而不仅可以考虑将来一段时间的参考轨迹，而且考虑完整的机器人运动方程，使得控制效果得到明显提升，然而两个基于优化的方法串联提高了对计算效率的要求。
15.综上所述，相关技术中的足式机器人的控制方法计算量要求巨大，使得控制算法无法达到更高的运行频率，难以表现出更好的控制性能，亟待解决。

技术实现要素：

16.本技术提供一种结合实时运动学预测的足式机器人全身控制方法及装置，以解决现有的足式机器人的控制方法计算量要求巨大，使得控制算法无法达到更高的运行频率，难以改善机器人的控制性能等问题。
17.本技术第一方面实施例提供一种结合实时运动学预测的足式机器人全身控制方法，包括以下步骤：生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设
时长内的任务空间参考运动轨迹；获取所述足式机器人的当前状态，并根据所述当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测rkp算法进行计算，得到多个期望加速度；以及将所述多个期望加速度传递至全身控制wbc，以协调所述多个子运动任务，直至所述足式机器人结束所述全身运动任务。
18.可选地，在本技术的一个实施例中，所述预设rkp算法的优化问题的表达式为：
[0019][0020][0021]
其中，分别为第k帧的任务空间坐标、速度和加速度；分别为第k帧的任务空间坐标、速度和加速度；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；是决策变量向量，由整个预测视界中的任务空间加速度组成；是正定对角矩阵，表示加权轨迹跟踪误差，是正数，为加权系统输入的范数。
[0022]
可选地，在本技术的一个实施例中，所述将所述多个期望加速度传递至全身控制wbc，以协调所述多个子运动任务，包括：求解最优解的加速度和足底力旋量；基于所述最优解的加速度和足底力旋量，根据机器人执行器的类型计算相应的控制指令，并控制所述机器人执行器执行所述控制指令。
[0023]
可选地，在本技术的一个实施例中，所述生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹，包括：将所述足式机器人的原地周期性蹲起运动任务分解为双脚保持与地面静止子运动任务和躯干运动子运动任务。
[0024]
可选地，在本技术的一个实施例中，所述生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹，包括：生成所述躯干运动子运动任务的沿z轴方向的上下运动轨迹、沿x轴方向的前后运动轨迹、绕y轴旋转的仰俯角运动轨迹及其他3个自由度上保持初始状态的运动轨迹；生成所述双脚保持与地面静止子运动任务的保持与地面稳定接触的轨迹。
[0025]
本技术第二方面实施例提供一种结合实时运动学预测的足式机器人全身控制方法，包括：生成模块，用于生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹；计算模块，用于获取所述足式机器人的当前状态，并根据所述当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测rkp算法进行计算，得到多个期望加速度；以及协调模块，用于将所述多个期望加速度传递至全身控制wbc，以协调所述多个子运动任务，直至所述足式机器人结束所述全身运动任务。
[0026]
可选地，在本技术的一个实施例中，所述预设rkp算法的优化问题的表达式为：
[0027][0028][0029]
其中，分别为第k帧的任务空间坐标、速度和加速度；分别为第k帧的任务空间坐标、速度和加速度；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；是决策变量向量，由整个预测视界中的任务空间加速度组成；是正定对角矩阵，表示加权轨迹跟踪误差，是正数，为加权系统输入的范数。
[0030]
可选地，在本技术的一个实施例中，所述协调模块包括：求解单元，用于求解最优解的加速度和足底力旋量；控制单元，用于基于所述最优解的加速度和足底力旋量，根据机器人执行器的类型计算相应的控制指令，并控制所述机器人执行器执行所述控制指令。
[0031]
可选地，在本技术的一个实施例中，所述生成模块包括：分解单元，用于将所述足式机器人的原地周期性蹲起运动任务分解为双脚保持与地面静止子运动任务和躯干运动子运动任务。
[0032]
可选地，在本技术的一个实施例中，所述生成模块还包括：第一保持单元，用于生成所述躯干运动子运动任务的沿z轴方向的上下运动轨迹、沿x轴方向的前后运动轨迹、绕y轴旋转的仰俯角运动轨迹及其他3个自由度上保持初始状态的运动轨迹；第二保持单元，用于生成所述双脚保持与地面静止子运动任务的保持与地面稳定接触的轨迹。
[0033]
本技术第三方面实施例提供一种电子设备，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如上述实施例所述的结合实时运动学预测的足式机器人全身控制方法。
[0034]
本技术第四方面实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储计算机程序，该程序被处理器执行时实现如上的结合实时运动学预测的足式机器人全身控制方法。
[0035]
由此，本技术的实施例具有以下有益效果：
[0036]
本技术的实施例可以通过生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹；获取足式机器人的当前状态，并根据当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测rkp算法进行计算，得到多个期望加速度；将多个期望加速度传递至全身控制wbc，以协调多个子运动任务，直至足式机器人结束全身运动任务。本技术通过将复杂的机器人运动方程和不等式约束利用wbc进行处理，通过rkp(real-time kinematics prediction，实时运动学预测)考虑将来一段时间的参考轨迹，并重新对其进行优化，得到更合适的运动轨迹，尤其适合处理存在跳变的参考轨迹，且rkp存在闭式解，可有效提高其求解效率，改善运行频率，极大降低计算量，弥补wbc的缺陷。由此，解决了现有的足式机器人的控制方法计算量要求巨大，使得控制算法无法达到更高的运行频率，难以改善机器人的控制性能等问题。
[0037]
本技术附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本技术的实践了解到。
附图说明
[0038]
本技术上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
[0039]
图1为根据本技术实施例提供的一种结合实时运动学预测的足式机器人全身控制方法的流程图；
[0040]
图2为本技术的一个实施例提供的一种结合实时运动学预测的足式机器人全身控制方法的执行逻辑示意图；
[0041]
图3为根据本技术实施例的结合实时运动学预测的足式机器人全身控制装置的示例图；
[0042]
图4为本技术实施例提供的电子设备的结构示意图。
[0043]
其中，10-结合实时运动学预测的足式机器人全身控制装置、101-生成模块、102-计算模块、103-协调模块、401-存储器、402-处理器、403-通信接口。
具体实施方式
[0044]
下面详细描述本技术的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本技术，而不能理解为对本技术的限制。
[0045]
下面参考附图描述本技术实施例的结合实时运动学预测的足式机器人全身控制方法及装置。针对上述背景技术中提到的问题，本技术提供了一种结合实时运动学预测的足式机器人全身控制方法，在该方法中，通过生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹；获取足式机器人的当前状态，并根据当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测rkp算法进行计算，得到多个期望加速度；将多个期望加速度传递至全身控制wbc，以协调多个子运动任务，直至足式机器人结束全身运动任务。本技术通过将复杂的机器人运动方程和不等式约束利用wbc进行处理，通过rkp考虑将来一段时间的参考轨迹，并重新对其进行优化，得到更合适的运动轨迹，尤其适合处理存在跳变的参考轨迹，且rkp存在闭式解，可有效提高其求解效率，改善运行频率，极大降低计算量，弥补wbc的缺陷。由此，解决了现有的足式机器人的控制方法计算量要求巨大，使得控制算法无法达到更高的运行频率，难以改善机器人的控制性能等问题。
[0046]
具体而言，图1为本技术实施例所提供的一种结合实时运动学预测的足式机器人全身控制方法的流程图。
[0047]
如图1所示，该结合实时运动学预测的足式机器人全身控制方法包括以下步骤：
[0048]
在步骤s101中，生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹。
[0049]
本领域技术人员可以理解的是，本技术的实施例在考虑将来一段时间内足式机器人的参考轨迹的同时，结合完整机器人模型的实时控制算法，实现足式机器人的全身控制。
[0050]
为了使技术人员更为清晰地了解足式机器人全身运动控制的流程，本技术的实施例以双足机器人的原地周期性蹲起运动为例，对结合实时运动学预测的足式机器人全身控制的工作原理进行详细介绍。
[0051]
在控制足式机器人进行全身运动之前，本技术的实施例可将全身运动任务分解成多个子运动任务，上述任务分解过程并非通过算法实现，而是由上述足式机器人全身控制方法的使用人员根据其期望机器人实现的整体运动状态进行任务分解，从而为生成上述多个子任务对应地空间参考运动轨迹提供依据和指导。
[0052]
可选地，在本技术的一个实施例中，生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹，包括：将足式机器人的原地周期性蹲起运动任务分解为双脚保持与地面静止子运动任务和躯干运动子运动任务。
[0053]
具体地，本技术的实施例在进行任务分解时，必须保证所有任务的维度大于或等于机器人的总自由度，例如，一个只有躯干和双腿的双足机器人，其躯干有6个被动自由度，每条腿上有5个驱动关节，因此整个机器人有6+5+5＝16个自由度。
[0054]
进而，本技术的实施例可将原地蹲起运动分为躯干运动和双脚稳定接触地面两个子任务，其中，躯干运动子任务的维度是6维，而双脚稳定接触地面子任务的维度是12维(每个脚板各6维)，因此所有任务的维度大于机器人的总自由度，即两个任务已经用尽了机器人的所有自由度；如果所有任务的维度小于机器人的总自由度，将导致机器人有冗余自由度，即机器人的整体运动未被完整指定，则会导致机器人的运动结果可能无法预知，故而对于有更多冗余自由度的机器人而言，可以设置更多的任务。
[0055]
需要说明的是，通过上述任务分解，从而为机器人生成运动参考轨迹，而通常只有浮动基座(对于一般的双足机器人而言，可以将躯干视为浮动基座)和执行器末端(例如脚、手的末端等)需要参考运动轨迹，因此通常的子任务均是浮动基座和执行器末端的运动任务。
[0056]
此外，还有一些其他情况的任务也可设置，例如，最小化足底力旋量任务和最小化关节转速任务等，其无需参考轨迹，仅是从节省能量等角度而设置的任务。
[0057]
需要注意的是，本技术的实施例可以根据实际情况再设置一个最小化关节加速度和足底力旋量的任务作为优化问题的正则项。
[0058]
由此，本技术的实施例通过将全身运动任务分解成多个子任务，从而为后续空间参考运动轨迹的生成提供基础，保证了足式机器人全身控制的实现。
[0059]
可选地，在本技术的一个实施例中，生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹，包括：生成躯干运动子运动任务的沿z轴方向的上下运动轨迹、沿x轴方向的前后运动轨迹、绕y轴旋转的仰俯角运动轨迹及其他3个自由度上保持初始状态的运动轨迹；生成双脚保持与地面静止子运动任务的保持与地面稳定接触的轨迹。
[0060]
需要说明的是，在将全身运动任务分解成多个子任务后，进一步地，本技术的实施例可以为每个子任务生成将来一段时间内的任务空间参考运动轨迹，
[0061]
对于双足机器人原地周期性蹲起运动而言，躯干的子任务需要的运动轨迹如下所述：
[0062]
沿z轴方向的上下运动轨迹；
[0063]
沿x轴方向的前后运动轨迹；
[0064]
绕y轴旋转的仰俯角运动轨迹；
[0065]
其他三个自由度上保持初始状态的运动轨迹；
[0066]
双脚的子任务仅需保持与地面稳定接触的轨迹。
[0067]
作为一种可以实现地方式，在双足机器人原地周期性蹲起运动中，本技术的实施例也可通过较为简单的方法生成参考轨迹，即根据技术人员的指令，直接指定不同时刻的参考位置和速度，其规划的结果即为三角波(指定速度)或方波(指定位置)。
[0068]
本领域技术人员应当了解的是，即使技术人员指定了蹲起的幅度、频率等信息，机器人的参考运动轨迹仍有无数种选择，例如可以将余弦曲线、三角波曲线甚至是方波曲线等作为机器人躯干z轴方向上的参考运动轨迹，即便是位置和速度均不连续的方波曲线，本技术实施例的控制算法仍然可以实现很好的跟踪效果。
[0069]
对于其他的运动形式而言，例如双足机器人的行走、空翻等，在具体实现过程中，本技术实施例的控制算法对其规划算法的选择也可不做要求，且由于机器人的运动形式不同，其所需要的规划算法也不同，例如行走运动过程中，腿部摆动的落点选择较为关键，而空翻运动过程中，其质心角动量的规划较为关键，上述两种运动所需的规划算法通常截然不同，对于空翻等复杂动作(高难度动作)，规划算法和控制算法同样关键，因此当规划算法不合适时，控制算法可能也无法保证复杂动作很好的执行。
[0070]
需要注意的是，在具体执行过程中，技术人员可以根据实际情况采取合适地规划算法生成参考轨迹，于此不做具体限定。
[0071]
由此，本技术的实施例为每个子任务生成将来一段时间内的任务空间参考运动轨迹(包括位置和速度)，从而为利用rkp算法实现机器人全身控制提供理论和技术支撑。
[0072]
在步骤s102中，获取足式机器人的当前状态，并根据当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测rkp算法进行计算，得到多个期望加速度。
[0073]
在生成多个子运动任务在未来一段时间内的任务空间参考运动轨迹后，进而，本技术的实施例可以根据机器人的反馈状态，对各个子任务的参考运动轨迹的各个分量分别使用rkp算法；其中，双脚和躯干的任务共18维，因此需求解18个rkp的优化问题，得到相应的期望加速度。
[0074]
需要注意的是，rkp在上述过程中不仅起到了在线轨迹重优化的作用，也起到了反馈控制的作用。
[0075]
可选地，在本技术的一个实施例中，预设rkp算法的优化问题的表达式为：
[0076][0077][0078]
其中，分别为第k帧的任务空间坐标、速度和加速度；
为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；是决策变量向量，由整个预测视界中的任务空间加速度组成；是正定对角矩阵，表示加权轨迹跟踪误差，是正数，为加权系统输入的范数。
[0079]
需要说明的是，本技术实施例所采用的rkp算法采用类似mpc的方式优化机器人任务空间中的位置和速度轨迹。然而，与常规的mpc不同，rkp并不涉及机器人的动力学模型(无论是完整的模型还是简化的模型)，而是考虑机器人任务空间运动的二阶线性运动学，并选择任务空间加速度作为系统输入，且代价函数中的惩罚取代了不等式约束来规范过度的系统输入。
[0080]
由此，便可推导出优化问题的闭式解，且使得所需的计算成本最小。
[0081]
在本技术的实施例中，rkp优化问题的表达式如下所示：
[0082][0083]
其中，分别为第k帧的任务空间坐标、速度和加速度；分别为第k帧的任务空间坐标、速度和加速度；为将来n帧的参考状态轨迹；n为预测视界(即帧数)；δt为帧的时间间隔；是决策变量向量，由整个预测视界中的任务空间加速度组成；是正定对角矩阵，表示加权轨迹跟踪误差，是正数，为加权系统输入的范数。
[0084]
可以理解的是，由于本技术的实施例仅采用线性运动学方法，对任务空间的不同轴向运动进行解耦，因此，本技术的实施例可以通过rkp单独应用于任务空间中的任一运动分量。
[0085]
进一步地，若将rkp看成一个子系统，则其输入为当前状态和将来n帧的参考状态轨迹，输出为n帧的最优加速度轨迹，故而，对于后续的wbc而言，仅需当前帧的最优加速度
[0086]
令状态方程可以写成
[0087]
x＝a
qp
x0+b
qpuꢀꢀ
(2)
[0088]
其中：
[0089][0090]
而x0是状态反馈，式(1)的代价函数可重写为下式：
[0091]
j(u)＝||x
ref-x||q+‖u‖rꢀꢀ
(3)
[0092]
其中：
[0093][0094][0095][0096]
将式(2)代入式(3)，可以得到一个等价的无约束qp问题:
[0097][0098]
其中：
[0099][0100][0101]
令可得到式(1)的最优解，如下所示：
[0102][0103]
其中，由于γ是独立于状态的，因此可以事先计算，极大减少了求解问题式(1)的时间；此外，每个控制周期中仅需应用因此本技术的实施例可以选择仅计算u
*
的第一行。
[0104]
总的来说，求解式(1)的时间复杂度为o(n2)，但是从式(1)求得到的时间复杂度是o(n)；一般来说，n≤100，故而式(1)完全可以进行实时求解，从而允许rkp与随后的wbc以相同的频率执行，而对于大多数mpc方法而言，其求解优化问题的时间复杂度都无法达到o(n)。
[0105]
可以理解的是，由于rkp可以考虑将来一段时间内的参考轨迹，且状态方程简单，不含有不等式约束，故而rkp可以以更高的频率运行；举例而言，在双足机器人原地蹲起控制中，本技术的实施例可使用主频2.30ghz的intel core i7-11800h的中央处理器时，rkp可与wbc以1khz的频率同频运行；此外，更高的控制频率将得到更优异的控制性能，例如，在mit的mini-cheetah四足机器人中，其mpc可以运行在40hz，而wbc运行在500hz，对于wbc而言，mpc提供的轨迹就像某种方波，其只能每隔25ms发生一次数值跳变。而在本技术的实施例中，由于rkp与wbc同频运行，对于wbc而言，rkp提供的轨迹是连续且光滑的，当某种扰动出现时，运行在更高频率的rkp能够比低频的mpc更快地做出响应。
[0106]
在步骤s103中，将多个期望加速度传递至全身控制wbc，以协调多个子运动任务，直至足式机器人结束全身运动任务。
[0107]
在对各个子任务的参考运动轨迹的各个分量分别使用rkp算法得到多个期望加速度后，进一步地，本技术的实施例可将通过多个rkp(对应各个子任务的各个自由度)计算出
来的期望加速度均传递给wbc，利用wbc协调上述运动子任务。
[0108]
可选地，在本技术的一个实施例中，将多个期望加速度传递至全身控制wbc，以协调多个子运动任务，包括：求解最优解的加速度和足底力旋量；基于最优解的加速度和足底力旋量，根据机器人执行器的类型计算相应的控制指令，并控制机器人执行器执行控制指令。
[0109]
可以理解的是，由于rkp已经起到了反馈控制的作用，因此，wbc可以选择由单个qp实现，其优化变量可以选择为广义关节空间中的加速度和足底力旋量wf。
[0110]
此外，除了上述提到的躯干任务和双脚任务以外，本技术的实施例可增设一个最小化足底力旋量的任务(该任务不需要参考轨迹)，同时考虑适当的约束如下所述：
[0111]
浮动基动力学方程等式约束；
[0112]
足底力的摩擦锥不等式约束；
[0113]
关节力矩饱和约束；
[0114]
关节转速饱和约束；
[0115]
关节功率饱和约束。
[0116]
在求解出最优解和后，本技术的实施例可根据机器人执行器的类型计算出相应的控制指令，例如，对于位控型执行器，可以直接对进行两次积分，得到关节位置指令；对于力控型执行器，可以根据式(6)计算出关节力矩指令τj：
[0117][0118]
其中，sj为驱动关节的选择矩阵，m广义质量矩阵，h是包含了科里奥利力、离心力和重力的项，jf是两只脚的接触雅可比矩阵。
[0119]
最后，执行器执行关节位置指令或关节力矩指令，双足机器人便可完成原地蹲起运动。
[0120]
需要说明的是，本技术的实施例中的wbc必须通过求解优化问题实现，但并不限制其具体实现内容，即不论是使用wqp或hqp均可；此外，由于技术人员完全可以根据实际的情况选择合适的优化变量，故而本技术的实施例中并不指定wbc中优化问题的优化变量物理含义；同样地，由于对于不同的足式机器人以及不同的工况而言，wbc的任务和约束设置也不同，因此本技术的实施例中的wbc并不限制具体的任务或约束的设置，例如，上述双足机器人原地蹲起过程中，wbc的各个约束条件并非都是必须的，去掉一条关节转速约束或者关节功率约束后，该控制方法在低速运动情况下仍然可行。
[0121]
可以理解的是，由于rkp的计算量相对于wbc而言较小，在中央处理器算力不变的情况下，可进一步提高wbc的运行频率(如从500hz提高到1khz)；且当rkp和wbc的控制频率已经很高(如1khz)，更高的控制频率对控制效果的提升较小时，本技术的实施例可以限制rkp和wbc的控制频率(如1khz)，从而为中央处理器留出更多的算力完成其他任务(如机器人的视觉定位计算任务等)，进一步优化和改善机器人全身运动的整体控制效果。
[0122]
根据本技术实施例提出的结合实时运动学预测的足式机器人全身控制方法，本技术的实施例通过将复杂的机器人运动方程和不等式约束利用wbc进行处理，通过rkp考虑将来一段时间的参考轨迹，并重新对其进行优化，得到更合适的运动轨迹，尤其适合处理存在跳变的参考轨迹，且rkp存在闭式解，可有效提高其求解效率，改善运行频率，极大降低计算
量，弥补wbc的缺陷。
[0123]
下述结合附图对本技术的结合实时运动学预测的足式机器人全身控制方法进行介绍。
[0124]
图2为本技术实施例结合实时运动学预测的足式机器人全身控制方法的执行逻辑示意图。如图2所示，结合实时运动学预测的足式机器人全身控制方法的执行步骤如下所示：
[0125]
s201：根据用户对机器人的运动要求，将全身运动任务分解成多个子任务，例如，双足机器人原地蹲起任务可以分解为双脚静止和躯干静止两个子任务；
[0126]
s202：根据用户的指令，为每个子任务生成将来一段时间内的任务空间参考运动轨迹(包括位置和速度)，例如，躯干的六个自由度上的参考运动轨迹；
[0127]
s203：根据机器人的反馈状态，对各个子任务的参考运动轨迹的各个分量都分别使用rkp算法，得到更合适的、光滑的加速度轨迹；
[0128]
s204：根据机器人的反馈状态，使用wbc协调各个子任务的加速度轨迹，最终输出控制指令，对于力控型驱动器，输出关节例句指令；对于位控型驱动器，输出关节位置指令。
[0129]
其次参照附图描述根据本技术实施例提出的结合实时运动学预测的足式机器人全身控制装置。
[0130]
图3是本技术实施例的结合实时运动学预测的足式机器人全身控制装置的方框示意图。
[0131]
如图3所示，该结合实时运动学预测的足式机器人全身控制装置10包括：生成模块101、计算模块102以及协调模块103。
[0132]
其中，生成模块101，用于生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹。
[0133]
计算模块102，用于获取足式机器人的当前状态，并根据当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测rkp算法进行计算，得到多个期望加速度。
[0134]
协调模块103，用于将多个期望加速度传递至全身控制wbc，以协调多个子运动任务，直至足式机器人结束全身运动任务。
[0135]
可选地，在本技术的一个实施例中，预设rkp算法的优化问题的表达式为：
[0136][0137][0138]
其中，分别为第k帧的任务空间坐标、速度和加速度；分别为第k帧的任务空间坐标、速度和加速度；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；是决策变量向量，由整个预测视界中的任务空间加速度组成；是正定对角矩阵，表示加权轨迹跟踪误差，是正数，为加权系统输入
的范数。
[0139]
可选地，在本技术的一个实施例中，协调模块103包括：求解单元和控制单元。
[0140]
其中，求解单元，用于求解最优解的加速度和足底力旋量。
[0141]
控制单元，用于基于最优解的加速度和足底力旋量，根据机器人执行器的类型计算相应的控制指令，并控制机器人执行器执行控制指令。
[0142]
可选地，在本技术的一个实施例中，生成模块101包括：分解单元，用于将足式机器人的原地周期性蹲起运动任务分解为双脚保持与地面静止子运动任务和躯干运动子运动任务。
[0143]
可选地，在本技术的一个实施例中，生成模块101还包括：第一保持单元和第二保持单元。
[0144]
其中，第一保持单元，用于生成躯干运动子运动任务的沿z轴方向的上下运动轨迹、沿x轴方向的前后运动轨迹、绕y轴旋转的仰俯角运动轨迹及其他3个自由度上保持初始状态的运动轨迹。
[0145]
第二保持单元，用于生成双脚保持与地面静止子运动任务的保持与地面稳定接触的轨迹。
[0146]
需要说明的是，前述对结合实时运动学预测的足式机器人全身控制方法实施例的解释说明也适用于该实施例的结合实时运动学预测的足式机器人全身控制装置，此处不再赘述。
[0147]
根据本技术实施例提出的结合实时运动学预测的足式机器人全身控制装置，通过生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹；获取足式机器人的当前状态，并根据当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测rkp算法进行计算，得到多个期望加速度；将多个期望加速度传递至全身控制wbc，以协调多个子运动任务，直至足式机器人结束全身运动任务。本技术通过将复杂的机器人运动方程和不等式约束利用wbc进行处理，通过rkp考虑将来一段时间的参考轨迹，并重新对其进行优化，得到更合适的运动轨迹，尤其适合处理存在跳变的参考轨迹，且rkp存在闭式解，可有效提高其求解效率，改善运行频率，极大降低计算量，弥补wbc的缺陷。
[0148]
图4为本技术实施例提供的电子设备的结构示意图。该电子设备可以包括：
[0149]
存储器401、处理器402及存储在存储器401上并可在处理器402上运行的计算机程序。
[0150]
处理器402执行程序时实现上述实施例中提供的结合实时运动学预测的足式机器人全身控制方法。
[0151]
进一步地，电子设备还包括：
[0152]
通信接口404，用于存储器401和处理器402之间的通信。
[0153]
存储器401，用于存放可在处理器402上运行的计算机程序。
[0154]
存储器401可能包含高速ram存储器，也可能还包括非易失性存储器(non-volatile memory)，例如至少一个磁盘存储器。
[0155]
如果存储器401、处理器402和通信接口404独立实现，则通信接口404、存储器401和处理器402可以通过总线相互连接并完成相互间的通信。总线可以是工业标准体系结构
(industry standard architecture，简称为isa)总线、外部设备互连(peripheral component，简称为pci)总线或扩展工业标准体系结构(extended industry standard architecture，简称为eisa)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，图4中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
[0156]
可选地，在具体实现上，如果存储器401、处理器402及通信接口404，集成在一块芯片上实现，则存储器401、处理器402及通信接口404可以通过内部接口完成相互间的通信。
[0157]
处理器402可能是一个中央处理器(central processing unit，简称为cpu)，或者是特定集成电路(application specific integrated circuit，简称为asic)，或者是被配置成实施本技术实施例的一个或多个集成电路。
[0158]
本实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上的结合实时运动学预测的足式机器人全身控制方法。
[0159]
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本技术的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或n个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
[0160]
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本技术的描述中，“n个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
[0161]
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或n个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本技术的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本技术的实施例所属技术领域的技术人员所理解。
[0162]
在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或n个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式光盘只读存储器(cdrom)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。
[0163]
应当理解，本技术的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，n个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。
[0164]
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
[0165]
此外，在本技术各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。
[0166]
上述提到的存储介质可以是只读存储器，磁盘或光盘等。尽管上面已经示出和描述了本技术的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本技术的限制，本领域的普通技术人员在本技术的范围内可以对上述实施例进行变化、修改、替换和变型。

技术特征：

1.一种结合实时运动学预测的足式机器人全身控制方法，其特征在于，包括以下步骤：生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹；获取所述足式机器人的当前状态，并根据所述当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测rkp算法进行计算，得到多个期望加速度；以及将所述多个期望加速度传递至全身控制wbc，以协调所述多个子运动任务，直至所述足式机器人结束所述全身运动任务。2.根据权利要求1所述的方法，其特征在于，所述预设rkp算法的优化问题的表达式为：征在于，所述预设rkp算法的优化问题的表达式为：k＝0,1,
…
,n-1，其中，λ
k
,分别为第k帧的任务空间坐标、速度和加速度；分别为第k帧的任务空间坐标、速度和加速度；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；是决策变量向量，由整个预测视界中的任务空间加速度组成；是正定对角矩阵，表示加权轨迹跟踪误差，是正数，为加权系统输入的范数。3.根据权利要求1所述的方法，其特征在于，所述将所述多个期望加速度传递至全身控制wbc，以协调所述多个子运动任务，包括：求解最优解的加速度和足底力旋量；基于所述最优解的加速度和足底力旋量，根据机器人执行器的类型计算相应的控制指令，并控制所述机器人执行器执行所述控制指令。4.根据权利要求1所述的方法，其特征在于，所述生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹，包括：将所述足式机器人的原地周期性蹲起运动任务分解为双脚保持与地面静止子运动任务和躯干运动子运动任务。5.根据权利要求4所述的方法，其特征在于，所述生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹，还包括：生成所述躯干运动子运动任务的沿z轴方向的上下运动轨迹、沿x轴方向的前后运动轨迹、绕y轴旋转的仰俯角运动轨迹及其他3个自由度上保持初始状态的运动轨迹；生成所述双脚保持与地面静止子运动任务的保持与地面稳定接触的轨迹。6.一种结合实时运动学预测的足式机器人全身控制装置，其特征在于，包括：生成模块，用于生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹；计算模块，用于获取所述足式机器人的当前状态，并根据所述当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测rkp算法进行计
算，得到多个期望加速度；以及协调模块，用于将所述多个期望加速度传递至全身控制wbc，以协调所述多个子运动任务，直至所述足式机器人结束所述全身运动任务。7.根据权利要求6所述的装置，其特征在于，所述预设rkp算法的优化问题的表达式为：7.根据权利要求6所述的装置，其特征在于，所述预设rkp算法的优化问题的表达式为：k＝0,1,
…
,n-1，其中，λ
k
,分别为第k帧的任务空间坐标、速度和加速度；分别为第k帧的任务空间坐标、速度和加速度；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；为将来n帧的参考状态轨迹；n为预测视界；δt为帧的时间间隔；是决策变量向量，由整个预测视界中的任务空间加速度组成；是正定对角矩阵，表示加权轨迹跟踪误差，是正数，为加权系统输入的范数。8.根据权利要求6所述的装置，其特征在于，所述协调模块包括：求解单元，用于求解最优解的加速度和足底力旋量；控制单元，用于基于所述最优解的加速度和足底力旋量，根据机器人执行器的类型计算相应的控制指令，并控制所述机器人执行器执行所述控制指令。9.根据权利要求6所述的装置，其特征在于，所述生成模块包括：分解单元，用于将所述足式机器人的原地周期性蹲起运动任务分解为双脚保持与地面静止子运动任务和躯干运动子运动任务。10.根据权利要求9所述的装置，其特征在于，所述生成模块还包括：第一保持单元，用于生成所述躯干运动子运动任务的沿z轴方向的上下运动轨迹、沿x轴方向的前后运动轨迹、绕y轴旋转的仰俯角运动轨迹及其他3个自由度上保持初始状态的运动轨迹；第二保持单元，用于生成所述双脚保持与地面静止子运动任务的保持与地面稳定接触的轨迹。11.一种电子设备，其特征在于，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如权利要求1-5任一项所述的结合实时运动学预测的足式机器人全身控制方法。12.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行，以用于实现如权利要求1-5任一项所述的结合实时运动学预测的足式机器人全身控制方法。

技术总结

本申请涉及一种结合实时运动学预测的足式机器人全身控制方法及装置，其中，方法包括：生成由足式机器人的全身运动任务分解成多个子运动任务在未来预设时长内的任务空间参考运动轨迹；获取足式机器人的当前状态，并根据当前状态对每个子运动任务的任务空间参考运动轨迹的每个分量分别利用预设实时运动学预测RKP算法进行计算，得到多个期望加速度；将多个期望加速度传递至全身控制WBC，以协调多个子运动任务，直至足式机器人结束全身运动任务。由此，解决了现有的足式机器人的控制方法计算量要求巨大，使得控制算法无法达到更高的运行频率，难以改善机器人的控制性能等问题。难以改善机器人的控制性能等问题。难以改善机器人的控制性能等问题。