CN116945151A - 控制足式机器人的方法和装置及足式机器人 - Google Patents

控制足式机器人的方法和装置及足式机器人 Download PDF

Info

Publication number
CN116945151A
CN116945151A CN202310274755.XA CN202310274755A CN116945151A CN 116945151 A CN116945151 A CN 116945151A CN 202310274755 A CN202310274755 A CN 202310274755A CN 116945151 A CN116945151 A CN 116945151A
Authority
CN
China
Prior art keywords
foot
foot robot
robot
plane
mechanical
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310274755.XA
Other languages
English (en)
Inventor
王帅
郑宇�
左晋帛
迟万超
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tencent Technology Shenzhen Co Ltd
Original Assignee
Tencent Technology Shenzhen Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tencent Technology Shenzhen Co Ltd filed Critical Tencent Technology Shenzhen Co Ltd
Priority to CN202310274755.XA priority Critical patent/CN116945151A/zh
Publication of CN116945151A publication Critical patent/CN116945151A/zh
Pending legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
    • B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
    • B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Robotics (AREA)
  • Chemical & Material Sciences (AREA)
  • Combustion & Propulsion (AREA)
  • Transportation (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本公开的实施例提供一种控制足式机器人的方法、装置、一种足式机器人，一种控制足式机器人的设备、以及一种非易失性计算机可读存储介质。足式机器人包括基座和至少两条机械腿，每条机械腿包括至少一个关节，该方法包括：响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹，其中，所述第一期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第二期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹；以及基于足式机器人对应的动力学模型以及所述第一期望轨迹和第二期望轨迹，控制足式机器人的各个关节的动作。

Description

控制足式机器人的方法和装置及足式机器人
技术领域
本申请涉及足式机器人技术领域，具体涉及足式机器人规划控制技术领域，尤其涉及一种控制足式机器人的方法、装置、一种足式机器人，一种控制足式机器人的设备、以及一种非易失性计算机可读存储介质。
背景技术
足式机器人指的是能够以人类脚步行走的机器人，通常由多个关节、电机和传感器组成，能够模拟人类或动物的动作。目前，足式机器人(如四足足式机器人)已经能够执行多种不同的动作，例如，跳跃、空翻等。执行这些动作的足式机器人在落地过程中，往往因为缺乏有效的控制方案，导致足式机器人在落地过程中动作僵硬，各个关节受到过大的冲击力，机身回弹较大。甚至由于落地时受到的冲击力过大，导致足式机器人损坏。
尤其是执行空翻动作和落地动作的足式机器人，由于足式机器人在与平面接触的时刻，机身可能具有一个较大的倾斜角，不同机械腿接触平面的时刻往往存在一定的差异，容易导致机器人受到损坏或失去控制。基于此，如何在执行空翻动作的足式机器人的落地过程中，如何对足式机器人进行进一步控制成为了研究热点。
发明内容
针对以上问题，本公开提供了一种控制足式机器人的方法、装置、一种足式机器人，一种控制足式机器人的设备、以及一种非易失性计算机可读存储介质。
根据本公开的一方面，提出了一种控制足式机器人的方法，所述足式机器人包括基座和至少两条机械腿，所述每条机械腿包括至少一个关节，所述方法包括：响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹，其中，所述第一期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第二期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹；以及基于所述足式机器人对应的动力学模型以及所述第一期望轨迹和第二期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的所有机械腿均下落至与平面接触。
根据本公开的另一方面，提出了一种控制足式机器人的装置，所述足式机器人包括基座和至少两条机械腿，所述每条机械腿包括至少一个关节，所述装置包括：规划计算设备，被配置为响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹，其中，所述第一期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第二期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹；控制电机，被配置为基于所述足式机器人对应的动力学模型以及所述第一期望轨迹和第二期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的所有机械腿均下落至与平面接触。
根据本公开的另一方面，提出了一种足式机器人，包括：基座部；下肢部，其连接至所述基座部，所述下肢部包括四条下肢，其中每条下肢包括髋关节两个自由度和膝关节1个自由度；控制器，所述控制器设置在所述足式机器人上，且能够执行上述的方法。
根据本公开的另一方面，提出了一种控制足式机器人的设备，包括：处理器；和存储器，其中，所述存储器中存储有计算机可执行代码，所述计算机可执行代码当由所述处理器运行时，执行上述的方法。
根据本公开的另一方面，提出了一种非易失性计算机可读存储介质，其上存储有可执行代码，所述可执行代码在被处理器执行时，使得所述处理器执行上述方法。
本公开的实施例对落地过程中的足式机器人的建立了模型，并基于该模型，对足式机器人部分机械腿落地后的质心运动轨迹和足端位置轨迹进行了规划，并基于规划的质心运动轨迹和足端位置轨迹，求解各个电机的控制力矩，对足式机器人进行精准控制。从而，保证了足式机器人在落地过程中，各个关节仅受到较小的冲击力，机身回弹较小，倾斜角变化较小，在保证落地功能的同时对足式机器人形成了良好的抗冲击保护效果。
附图说明
为了更清楚地说明本公开的实施例的技术方案，下面将对实施例的描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本公开的一些示例性实施例，对于本领域普通技术人员来说，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。以下附图并未刻意按实际尺寸等比例缩放绘制，重点在于示出本公开的主旨。
图1是示出根据本公开的实施例的足式机器人的示意图。
图2是示出根据本公开的实施例的控制足式机器人的方法的流程图。
图3A是示出根据本公开的实施例的足式机器人进行简化所得到的二维平面模型中的虚拟机器人的示意图。
图3B是示出根据本公开的实施例的足式机器人在空翻过程中的各个阶段的示意图。
图3C是示出根据本公开的实施例的足式机器人与平面接触的瞬间的示意图。
图4是示出根据本公开的实施例的足式机器人落地过程中的质心的位置姿态的变化及足式机器人的简化模型中前后腿的受力情况的示意图。
图5是示出根据本公开的实施例的足式机器人落地过程中的机械腿与平面接触的示意图。
图6是示出根据本公开的实施例提供的一种三次样条差值的计算原理示意图。
图7A至图7F示出了根据本公开实施例的足式机器人在各种初始条件下落地的动作的仿真图和真机测试图。
图8示出了根据本公开实施例的足式机器人的控制系统架构。
图9示出了根据本公开实施例的足式机器人的示例性框图。
具体实施方式
为了使得本公开的目的、技术方案和优点更为明显，下面将参考附图详细描述根据本公开的示例实施例。显然，所描述的实施例仅仅是本公开的一部分实施例，而不是本公开的全部实施例，应理解，本公开不受这里描述的示例实施例的限制。
如本公开和权利要求书中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的步骤和元素，而这些步骤和元素不构成一个排它性的罗列，方法或者设备也可能包含其他的步骤或元素。
虽然本公开对根据本公开的实施例的装置中的某些模块做出了各种引用，然而，任何数量的不同模块可以被使用并运行在用户终端和/或服务器上。所述模块仅是说明性的，并且所述装置和方法的不同方面可以使用不同模块。
本公开中使用了流程图用来说明根据本公开的实施例的方法和装置所执行的操作。应当理解的是，前面或下面操作不一定按照顺序来精确地执行。相反，根据需要，可以按照倒序或同时处理各种步骤。同时，也可以将其他操作添加到这些过程中，或从这些过程移除某一步或数步操作。
为便于描述本公开，以下介绍与本公开有关的概念。
本公开的足式机器人是一种使用腿进行移动的机器人，其以动物为仿生对象，目的是依据工程技术和科学研究成果模拟动物的运动形式和复制动物的运动能力。足式机器人对各种环境(包括结构化环境(诸如公路、铁路、经处理的平整路面等)和非结构化环境(诸如山地、沼泽、崎岖不平的路面等))具有很强的适应能力，其能够适应地形的各种变化、翻越较高的障碍物，而且能够有效地减小载荷、提高系统的能量利用效率。足式机器人按照足数可分为单足、双足、四足、六足、八足等，其中，四足足式机器人具有超强的运动能力，其比双足足式机器人静态稳定性好，且比六足、八足足式机器人运动简单灵活，因此，四足足式机器人是研究足式机器人的一种常见选择。四足足式机器人的步态是四足足式机器人为了能够进行连续的移动，其四条腿在时间和空间上的协调关系。四足足式机器人的步态来自于四足哺乳动物的步态，其可以包括但不限于以下简化的三种形式：行走(walk)、小跑(trot)和跳跃(bound)。
本公开的控制足式机器人的方法可以是基于人工智能(Artificialintelligence，AI)的。人工智能是利用数字计算机或者数字计算机控制的机器模拟、延伸和扩展人的智能，感知环境、获取知识并使用知识获得最佳结果的理论、方法、技术及应用系统。换句话说，人工智能是计算机科学的一个综合技术，它企图了解智能的实质，并生产出一种新的能以人类智能相似的方式做出反应的智能机器。例如，对于基于人工智能的控制足式机器人的方法而言，其能够以类似于人类引导活体动物运动的方式来规划足式机器人的运动轨迹和步态，使得足式机器人的运动更灵活仿生。人工智能通过研究各种智能机器的设计原理与实现方法，使本公开的控制足式机器人的方法具有自动高效地基于足式机器人的当前运动状态设计足式机器人的后续运动轨迹和步态的功能。
综上所述，本公开的实施例提供的方案涉及人工智能、机器学习等技术，下面将结合附图对本公开的实施例进行进一步地描述。
图1是示出根据本公开的实施例的足式机器人的示意图。
如图1所示，以四足足式机器人为例，图1中的左图和右图分别示出了示例足式机器人的内部透视图和外部结构图。
该示例足式机器人能够基于四条机械腿进行移动。其中，每条机械腿可以包括大腿和小腿，且可包括大腿和小腿，且每条机械腿可包括至少一个关节。例如，每条机械腿上可以包括多个下肢关节，例如髋关节两个自由度和膝关节1个自由度。
除此之外，每条机械腿还可配置有多个电机，这些电机可以分别用来控制或组合控制四足足式机器人的髋关节两个自由度和膝关节1个自由度。需要说明的是，足式机器人上还可配置各式各样的传感器，如IMU(Inertial Measurement Unit，惯性测量单元)传感器和关节角编码器等；其中，IMU传感器可以实时地提供足式机器人的加速度和姿态信息，关节角编码器可以实时地提供足式机器人的各个关节的关节角信息(如关节角的角度、角速度反馈值等)。
示例足式机器人在上述提及的多个电机的控制下，已经能够实现空翻或跳跃等动作。然而执行这些动作的足式机器人最终都会以自由落地的形式落回平面。而如果在足式机器人的自由落地的过程中以及在足式机器人与平面接触的过程不对足式机器人进行控制，则会导致足式机器人在落地过程中动作僵硬，各个关节受到过大的冲击力，机身回弹较大。甚至在一些极端情况下，由于足式机器人在落地时受到的冲击力过大，导致足式机器人损坏。
目前工业界和学界已经提出了一些控制方案来对足式机器人的自由下落过程和空翻过程进行控制。例如，有些学者提出，四足足式机器人的每个足端与平面接触过程可以被等效为x轴和z轴方向上的两个虚拟弹簧的作用过程，如果利用PD控制方案来调整虚拟弹簧的刚度和阻尼参数，就能等效地推导出各个关节电机的输出力矩，从而就能使得足式机器人能够灵巧的落地。还有些学者提出，可以将机械腿和环境分别等效为两个不同的RLC模型，并基于这两个RLC模型，利用data-driven(其是一种机器学习控制方案)，就能推导出各个关节电机的输出力矩，从而就能使得足式机器人能够灵巧的落地。
然而这样的方案，都仅仅考虑了在足式机器人的机械腿模型或环境模型建立上弹簧阻尼模型，既没有考虑足式机器人在线速度上的变化也没有考虑足式机器人的质心在角速度上的变化，从而控制效果都有限。
因此，针对上述问题，一方面，本公开的实施例提供了一种控制足式机器人的方法，所述足式机器人包括基座和至少两条机械腿，每条机械腿包括至少一个关节，所述方法包括：响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹，其中，所述第一期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第二期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹；以及基于所述足式机器人对应的动力学模型以及所述第一期望轨迹和第二期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的所有机械腿均下落至与平面接触。
另一方面，本公开的实施例还提供了一种控制足式机器人的装置，所述足式机器人包括基座和至少两条机械腿，所述每条机械腿包括至少一个关节，所述装置包括：规划计算设备，被配置为响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹，其中，所述第一期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第二期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹；控制电机，被配置为基于所述足式机器人对应的动力学模型以及所述第一期望轨迹和第二期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的所有机械腿均下落至与平面接触。
又一方面，本公开的实施例还提供了一种足式机器人，包括：基座部；下肢部，其连接至所述基座部，所述下肢部包括四条下肢，其中每条下肢包括髋关节两个自由度和膝关节1个自由度；控制器，所述控制器设置在所述足式机器人上，且能够执行上述方法。
相比于传统的用于足式机器人的运动控制方案而言，本公开的实施例充分考虑到足式机器人落地时的实际情况，通过判断足式机器人是全部机械腿均与平面接触还是仅有部分机械腿与平面接触，分阶段地调整了/触发了足式机器人落地的控制策略，使得足式机器人在落地过程中具有更好稳定性、适应性、安全性和柔性，在保证落地功能的同时对足式机器人形成了良好的抗冲击保护效果。此外，本公开的实施例还进一步考虑了质心在重力方向和水平方向上变化，更适用于执行空翻动作的足式机器人的落地控制。
图2是示出根据本公开的实施例的用于足式机器人的控制方法20的流程图。根据本公开的实施例的用于足式机器人的控制方法200可以包括如图2所示的步骤S201-S202。如上所述，所述足式机器人包括基座和至少两条机械腿，所述每条机械腿包括至少一个关节。
在步骤S201中，响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹，其中，所述第一期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第二期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹。
作为一个示例，步骤S201可以由任意的计算设备执行。此处的计算机设备可以是终端或服务器；或者，此处的计算机设备也可以是由终端和服务器共同执行，对此不作限定。其中，终端可以是智能手机、电脑(如平板电脑、笔记本电脑、台式计算机等)、智能穿戴设备(如智能手表、智能眼镜)、智能语音交互设备、智能家电(如智能电视)、车载终端或飞行器等；服务器可以是独立的物理服务器，也可以是多个物理服务器构成的服务器集群或者分布式系统，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、CDN(Content Delivery Network，内容分发网络)、以及大数据和人工智能平台等基础云计算服务的云服务器，等等。进一步的，终端和服务器可位于区块链网络内或区块链网络外，对此不作限定；更进一步的，终端和服务器还可将内部所存储的任一数据上传至区块链网络进行存储，以防止内部所存储的数据被篡改，提升数据安全性。
例如，在足式机器人的下落过程中，足式机器人的各条机械腿与平面(如平面、桌面等)之间的接触情况会发生变化，使得足式机器人在与平面接触的过程中可具有多种运动形态。尤其是针对执行空翻动作的足式机器人在下落过程中，足式机器人在与平面接触时，可能先是前腿落地然后后腿再落地，或者后腿先落地然后前腿再落地，而不论是前腿先落地还是后腿先落地，足式机器人都可能具有一定的倾斜角，导致足式机器人可能存在前后倾的角速度或左右倾的角速度。此时需要动态地调整各条机械腿的关节力矩，以避免足式机器人摔倒。
为此，需要确定足式机器人与平面接触的接触信息，并基于部分机械腿接触平面的时刻机器人的姿态和状态信息来确定第一期望轨迹和第二期望轨迹。具体地，以四足机器人为例，根据本公开的实施例考虑到了足式机器人两条机械腿先与平面接触，然后剩下的两条机械腿再与平面接触的中间过程，并对这个中间过程进行了控制，以使得整个过程更加柔顺。本公开的实施例通过检测在部分机械腿落地，从而触发足式机器人的轨迹规划，直至所有机械腿都落地，从而保证了中间阶段的足式机器人的平衡和稳定。之后将参考图3A至图3C详细描述如何确定足式机器人与平面接触的接触信息的实施例，本公开在此就不再赘述。
如上所述，期望轨迹是指足式机器人在执行某种任务时，期望的运动轨迹。因此，第一期望轨迹指示在部分机械腿落地到所有机械腿都落地的中间阶段中，足式机器人质心在重力方向和水平方向上移动的期望轨迹，例如，第一期望轨迹指示所述足式机器人的质心在重力方向(以下又可称为竖直方向)和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，可以帮助机器人在执行落地任务时保持稳定性和平衡性。例如，第一期望轨迹可以包括所述足式机器人的质心在各个时间步期望的位置信息、速度信息、加速度信息、角速度信息、角加速度信息等等。当然本公开并不以此为限。
在一个示例中，第一期望轨迹可以以各个时间步对应的质心相关信息组成的时序数值序列来进行表示。可选地，该数值序列中的每个元素可以具有6个维度，其分别表示某个时间步对应的质心在x轴方向上的位置、质心在y轴方向上的位置、质心在z方向(重力方向)上的位置、足式机器人的倾斜角(pitch)、足式机器人的偏航角(yaw)以及足式机器人的滚动角(roll)等等。当然第一期望轨迹还可以以其他数据结构来进行表示，本公开并不以此为限。之后将参考图4进一步描述确定第一期望轨迹的示例方案，本公开在此就不再赘述。
各条机械腿远离所述基座的一端又称为足端，而第二期望轨迹指示在部分机械腿落地到所有机械腿都落地的中间阶段中，各条机械腿的足端的期望轨迹。例如，第二期望轨迹可以包括所述各条机械腿的足端的在各个时间步期望的位置信息、速度信息、加速度信息、角速度速度、角加速度信息等等。又例如，第二期望轨迹还可以包括所述各条机械腿的各个关节在各个时间步期望的位置信息、速度信息、加速度信息、角速度速度、角加速度信息等等。类似地，第二期望轨迹可以以各个时间步对应的各条机械腿相关信息组成的时序序列来进行表示。当然第二期望轨迹还可以以其他数据结构来进行表示，本公开并不以此为限。之后将参考图5至图6进一步描述确定第二期望轨迹的示例方案，本公开在此就不再赘述。
其中，一个时间步又可以被称为一帧。相邻时间步之间的时间差可以相同也可以不同。例如，由于足式机器人在刚刚与平面接触的一段时间，足式机器人的动作和受力都变化较小，因此时间步之间的差异可以被设定得较小，从而实现保证在前期足式机器人的动作控制足够灵活。而在足式机器人逐渐达到稳定状态的过程中，时间步之间的差异可以被设定得相对较大，以节省算力。本公开并不对相邻时间步之间的时间差进行限定。
在步骤S202中，基于所述足式机器人对应的动力学模型以及所述第一期望轨迹和第二期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的所有机械腿均下落至与平面接触。
足式机器人对应的动力学模型用于表征所述足式机器人运动过程中的各关节和质心角度、角速度、角加速度和关节力矩以及与外界接触力的随时间的变化关系。可选地，足式机器人对应的动力学模型还描述了该足式机器人的内部结构(例如，机械腿、机械臂、各个关节、传感器等)之间的几何结构和相互之间的关系，以及机器人在外部作用力和力矩作用下的运动状态。例如，该动力学模型可以以能量变化的角度来描述上述变化关系。例如，该动力学模型还可以以动量变化或受力变化的角度来描述上述变化关系。本公开对此不进行限定。
从足式机器人从下落至与平面接触的瞬间到所述足式机器人在平面上稳定站立的过程中，足式机器人受到的作用力包括重力、各个关节电机的驱动力以及平面向所述足式机器人提供的接触力(又称为支撑力)。基于这三个力以及足式机器人本体各部分的尺寸、质量、转动惯量、关节连接方式等信息，可以对应地建立所述足式机器人对应动力学模型。不同位姿的足式机器人受到的接触力和驱动力不同。基于所述足式机器人对应的动力学模型，确定在各个时间步所述平面与所述足式机器人之间的接触力，以使得所述足式机器人的质心的实际轨迹符合所述第一期望轨迹。
进一步地，基于所述足式机器人对应的动力学模型以及在各个时间步所述平面与所述足式机器人之间的接触力，还可以确定各个关节电机在各个时间步提供的电机力矩，以使得所述各条机械腿远离所述基座的一端的轨迹符合所述第二期望轨迹。
值得注意的是，本公开中所述的“符合”是指在真机实际测试时，所述足式机器人的质心的实际轨迹与所述第一期望轨迹非常接近甚至相同，所述各条机械腿远离所述基座的一端的轨迹与所述第二期望轨迹非常接近甚至相同。由于关节电机性能的限制以及简化模型不精准的因素，关节电机往往不能输出理想的力矩。同时，考虑到外界环境的变化(例如，突然出现风等其它的扰动)，往往难以控制足式机器人完全符合第一期望轨迹和第二期望轨迹。因此，本公开的实施例只需要实际轨迹和期望轨迹之间的差异足够小即可。
在本公开的一些实施例中，基于所述足式机器人对应的动力学模型，可以对应地求解出使得所述足式机器人的质心在各个时间步达到所述第一期望轨迹所指示的位置、速度、加速度、角速度、角加速度所需的接触力，所述接触力为平面向各个机械腿的端部提供的支撑力。并且，基于所述足式机器人对应的动力学模型以及上述接触力，可以进一步求解得到使得所述足式机器人的机械腿在各个时间步达到所述第二期望轨迹所指示的位姿所需的关节控制信息。
可选地，所述关节控制信息既可以是各个关节电机的加速度，也可以是关节电机的力矩。虽然从数学意义上说，这两个物理量作为控制关节电机转动的控制信息并没有太大的区别，但是在实际物理系统当中，并不是这两个物理量都可以被准确测量。因此，本领域技术人员在实验中，可以根据具体情况，选择数据测试效果较好，比较符合模型的物理量进行后续计算。
接着，在足式机器人的各条机械腿中的所有机械腿均下落至与平面接触后，足式机器人很可能还没达到稳定的状态，此时还需要对足式机器人进行第二阶段的控制，以实现足式机器人从各机械腿均与平面接触后到足式机器人到达稳定状态(稳定状态也即所述足式机器人的基座的高度不变且所述足式机器人的倾斜角方向的转动角为零)的控制。
具体地，可选地，方法200还包括步骤S203至步骤S204，以接着在检测到所有机械腿都落地后，触发新一轮的轨迹规划和控制，以使得足式机器人最终稳定在平衡态。
在步骤S203中，响应于确定所述足式机器人的所有机械腿均下落至与平面接触，确定所述足式机器人对应的第三期望轨迹和第四期望轨迹，其中，所述第三期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第四期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹。
在步骤S204中，基于所述足式机器人对应的动力学模型以及所述第三期望轨迹和第四期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的基座的高度不变且所述足式机器人的倾斜角方向的转动角为零。
学界和工业界现有对移动机器人的控制方案都没有考虑足式机器人的质心轨迹的规划，而仅考虑了弹簧模型，因此，难以实现落地过程中精准控制。相较之下，本公开的实施例通过对足式机器人在落地过程的分阶段控制，不仅能够自动地实现对足式机器人的轨迹和步态规划，还能保证了足式机器人在落地过程中，各个关节仅受到较小的冲击力，机身回弹较小，倾斜角变化较小，在保证落地功能的同时对足式机器人形成了良好的抗冲击保护效果。此外，本公开的实施例还进一步考虑了质心的倾斜角的变化，更适用于执行具有水平方向的落地动作的足式机器人的落地控制。
图3A是示出根据本公开的实施例的足式机器人进行简化所得到的二维平面模型中的虚拟机器人的示意图。图3B是示出根据本公开的实施例的足式机器人在空翻过程中的各个阶段的示意图。图3C是示出根据本公开的实施例的足式机器人与平面接触的瞬间的示意图。
参考图3A，图1中的足式机器人可以近似是在一个二维平面(x轴(水平轴)和z轴(竖直轴)所构成的平面)内运动；那么如果不考虑足式机器人的自身质量等的不对称，以及电机的控制误差等，足式机器人可以被简化为二维平面模型中的虚拟机器人进行控制和状态估计。虚拟机器人可包括一条虚拟前腿和一条虚拟后腿。其中，虚拟前腿是对图1中的足式机器人的两条前腿进行等效处理得到的，该虚拟前腿具体可以是通过将四足机器人的左右两侧前机械腿进行重合得到的；虚拟后腿是对足式机器人的两条后腿进行等效处理得到的，该虚拟后腿具体可以是通过将足式机器人的左右两侧后腿进行重合得到的。
参考图3B，其示出了足式机器人在进行后空翻动作中的四个阶段。
在第一阶段中，虚拟机器人的虚拟前腿和虚拟后腿同时接触平面，整个过程包括虚拟机器人以虚拟后腿作为位置点，虚拟前腿发力蹬地的过程，虚拟机器人受平面的反作用力，在接下来的阶段会使得虚拟前腿会离开平面，仅有虚拟后腿与平面接触。该阶段到虚拟前腿离开平面的瞬间结束。在第一阶段中，二维平面模型中的虚拟机器人与平面的虚拟接触点数量(contact point)为2个。
在第二阶段中，从虚拟机器人的虚拟前腿离开平面的瞬间开始，整个过程包括虚拟机器人以虚拟后腿与平面之间的接触点为圆心旋转的过程，到虚拟机器人的虚拟前腿和虚拟后腿同时离开平面为止。在第二阶段中，二维平面模型中的虚拟机器人与平面的虚拟接触点数量为1个。
在第三阶段中，从虚拟机器人的虚拟前腿和虚拟后腿同时离开平面开始，包括虚拟机器人在空中质心的姿态旋转接近360度的过程，到虚拟机器人的某一条腿或几条腿接触平面为止。在第三阶段中，二维平面模型中的虚拟机器人与平面的虚拟接触点数量为0个。
在第四阶段中，从虚拟机器人的虚拟前腿接触平面开始，直到虚拟机器人的虚拟前腿和虚拟后腿均落在平面上，然后逐渐稳定。在第四阶段中，在虚拟前腿和虚拟后腿均落地后，二维平面模型中的虚拟机器人与平面的虚拟接触点数量为从0个变为1个再变为2个。2个接触点为终末状态。
本公开的实施例主要针对第四阶段中虚拟前腿或虚拟后腿先落在平面上然后虚拟前腿和虚拟后腿均落在平面上，最后逐渐稳定的过程。参考图3C，要做到落地缓冲，就需要确定各条机械腿在当前时刻与平面之间的接触状态，并基于该接触状态触发是否开始计算第一期望轨迹和第二期望轨迹。所谓的当前时刻是指：在足式机器人的落地过程中，随着时间的推进，最新到达的系统时刻。例如，各条机械腿在当前时刻与平面之间的接触状态包括以下各项中的至少一项：足式机器人的各条机械腿是否与平面接触、各条机械腿与平面所成的夹角、各条机械腿与平面的接触点的数量、各个接触点的位置、虚拟机器人与平面的虚拟接触点数量、虚拟接触点的位置等等信息，以确定所述足式机器人的第一期望轨迹和第二期望轨迹。
在本公开的一些实施例中，所述接触状态是通过所述足式机器人在当前时刻对应的当前状态确定的。
在具体实现中，可调用足式机器人中的IMU传感器来确定所述足式机器人的当前状态信息。例如，可以通过IMU传感器来采集足式机器人在当前时刻的加速度信息(其可包括足式机器人在多个方向(如竖直方向、水平方向)上的加速度)和当前姿态信息，以及调用关节角编码器确定足式机器人的各个关节在当前时刻的关节角信息(如关节角的角度、角速度反馈值等)。其次，可将当前姿态信息和关节角信息(如关节角的角度、角速度反馈值)汇入腿部里程计(Leg odometry)计算出位置信息，该位置信息可包括：计算出的足式机器人的各条机械腿在当前时刻的位置。另外，可将加速度信息输入至状态空间观测器，以使得状态空间观测器可基于加速度信息和历史得到的足式机器人在当前时刻的状态估计结果输出位置观测结果，该位置观测结果可包括：观测出的足式机器人的各条机械腿在当前时刻的位置；其中，足式机器人在当前时刻的状态估计结果，可以是在当前时刻的上一时刻到达时，对足式机器人在当前时刻的状态进行估计所得到的，其可存储在向量或其他数据结构中，对此不作限定。然后，可根据位置信息和位置观测结果，对足式机器人在当前时刻的下一时刻的状态进行估计。
例如，还可将位置信息和位置观测结果作为扩展卡尔曼滤波(Extended KalmanFilter，EKF)单元的输入，以通过该单元进行状态估计，从而得到足式机器人在下一时刻的状态估计结果。所谓的扩展卡尔曼滤波是标准卡尔曼滤波(简称卡尔曼滤波)在非线性情形下的一种扩展形式，其可将非线性函数进行泰勒展开，省略高阶项，保留展开项的一阶项，以此来实现非线性函数线性化。可选的，也可将位置信息和位置观测结果作为卡尔曼滤波单元的输入或基于机器学习所得到的状态估计模型的输入，以通过卡尔曼滤波单元或状态估计模型进行状态估计，得到足式机器人在下一时刻的状态估计结果。其中，足式机器人在下一时刻的状态估计结果，可同时用于足式机器人的控制和下一次状态估计时的状态空间观测器的输入；也就是说，通过状态估计所得到的估计结果可用于足式机器人的反馈控制，从而形成闭环。
以上仅是获得当前状态信息的一些示例，本公开之后还参考图8进一步描述了获得当前状态信息的更详细的示例方案，在此就不再赘述。下面对如何根据所述足式机器人的当前状态信息确定接触信息的几种实施方式进行阐述。
由于任意机械腿与平面之间的接触信息发生变化时，机械腿对应的任一项的状态值可发生突变，因此可以通过机械腿的当前状态值来判断机械腿在当前时刻与平面之间的接触信息，也即是否与平面接触。具体地，根据当前状态信息确定接触信息的方式包括：获取任一机械腿在当前时刻的上一时刻的历史状态值，并从当前状态信息中确定任一机械腿的当前状态值，从而可根据历史状态值，判断任一机械腿的当前状态值是否存在突变。
在本公开实施例中，当前状态值存在突变是指当前状态值和历史状态值之间的差值大于预设差值。基于此，可计算历史状态值与任一项的当前状态值之间的差值；若计算出的差值大于预设差值，则判定当前状态值存在突变；若计算出的差值不大于预设差值，则判定当前状态值不存在突变。例如，设历史状态值为20，预设差值为50；若当前状态值为100，则由于100减去20等于80，而80大于50，因此可认为当前状态值存在突变；若当前状态值为30，则由于30减去20等于10，而10小于50，因此可认为当前状态值不存在突变。
若根据历史状态值判定任一机械腿的当前状态值存在突变，且该机械腿的当前状态值大于历史状态值，则确定该机械腿在当前时刻与平面接触。若根据历史状态值判定任一机械腿的当前状态值不存在突变，则将该机械腿在上一时刻与平面之间的接触信息作为当前时刻的接触信息。也即，若任一机械腿在上一时刻与平面接触，则确定该机械腿在当前时刻也与平面接触；若任一机械腿在上一时刻未与平面接触，则确定该机械腿在当前时刻也未与平面接触。
例如，在本公开的一些实施例中，所述当前状态信息可选地包括：各条机械腿的关节电机力矩或电流值或电压值。
一般的，当足式机器人的机械腿不接触平面(如不着地)而悬空时，机械腿的负载仅为机械腿的质量，由于足式机器人的机械腿质量相对于整体的质量可以忽略，故负载较小，各关节的反馈电流值以及关节电机力矩相对较小。而当足式机器人的机械腿接触平面(如触地)时，足式机器人的负载变成了自身全部的质量加上自身惯性作用下向下移动的等效惯性力，故负载较大，各关节的反馈电流值以及关节电机力矩相对较大。基于此，在检测到关节电机力矩或反馈电流值由较小值突然变大时，认为足式机器人从空中向平面(如平面)着陆。
又例如，在本公开的一些实施例中，所述当前状态信息包括：足式机器人的质心高度、质心姿态、各条机械腿对应的当前关节角信息。
具体地，可根据由外部视觉或者动作捕捉系统检测到的足式机器人质心高度和质心姿态，以及足式机器人的各关节角信息，计算足式机器人的足端与平面接触的时刻，从而判断相应腿在当前时刻是否与平面接触。
其中，根据当前状态信息检测机械腿在当前时刻与平面之间的接触信息的方式包括：根据质心高度、质心姿态以及任一机械腿对应的当前关节角信息，计算任一机械腿距离所述平面的高度；若计算出的高度小于或等于高度阈值(如数值0或0.005等)，则确定任一机械腿在当前时刻与平面接触；若计算出的高度大于高度阈值，则确定任一机械腿在当前时刻未与平面接触。
又例如，在本公开的一些实施例中，所述当前状态信息可包括：各条机械腿对应的当前足底触觉反馈值，足底触觉反馈值是通过相应腿的足底触觉传感器生成的。
具体地，可通过足底触觉传感器来判断相应腿在当前时刻是否与平面接触。并且，任一足底触觉传感器在检测到相应腿与所述平面接触时，生成第一数值作为足底触觉反馈值，在检测到相应腿未与所述平面接触时，生成第二数值作为足底触觉反馈值；此处的第一数值和第二数值可根据实际需求设定，例如第一数值为数值1，第二数值为数值0，或者第一数值为数值0，第二数值为数值1等。其中，根据当前状态信息检测机械腿在当前时刻与平面之间的接触信息的方式包括：从当前状态信息中获取机械腿对应的当前足底触觉反馈值；若获取到的当前足底触觉反馈值为第一数值，则确定任一机械腿在当前时刻与平面接触；若获取到的当前足底触觉反馈值为第二数值，则确定任一机械腿在当前时刻未与平面接触。
又例如，在本公开的一些实施例中，所述当前状态信息包括：足式机器人在竖直方向上的当前加速度。假设在当前时刻的上一时刻，足式机器人在竖直方向上的历史加速度已知；若根据历史加速度判定当前加速度发生突变，则确定足式机器人落地。
经实践表明：当足式机器人在平面稳定站立时，IMU传感器所采集到的足式机器人在z方向的加速度为一倍的重力加速度数值g；当足式机器人在空中完全失重状态下时，IMU传感器所采集到的足式机器人在z方向的加速度为接近于0；当足式机器人在平面足部用力踩踏平面准备飞起的过程，以及落地后向平面缓冲的过程中，IMU传感器所采集到的足式机器人在z方向的加速度均大于一倍的重力加速度g。据此可知，在足式机器人落地的瞬间，足式机器人在竖直方向上的加速度发生突变。
在该实施例中，当前加速度存在突变是指当前加速度和历史加速度之间的差值大于差值阈值；基于此，计算机设备可计算历史加速度与当前加速度之间的差值；若计算出的差值大于差值阈值，则判定当前加速度发生突变；若计算出的差值不大于差值阈值，则判定当前加速度未发生突变。例如，设历史加速度为2，差值阈值为5；若当前加速度为9，则由于9减去2等于7，而7大于5，因此可认为当前加速度发生突变；若当前加速度为4，则由于4减去2等于2，而2小于5，因此可认为当前加速度未发生突变。
应理解的是，上述只是示例性地阐述了确定机械腿的接触信息的一些具体实现，并非穷举。本公开并不以此为限。
接下来，参考图4来进一步描述如何确定足式机器人的第一期望轨迹的实施例。图4是示出根据本公开的实施例的足式机器人落地过程中的质心的位置姿态的变化及足式机器人的简化模型中前后腿的受力情况的示意图。
在图4中示出了六条曲线，分别是第一推力随时间变化的曲线(以下又称为第一推力曲线)、第二推力随时间变化的曲线(以下又称为第二推力曲线)、第三推力随时间变化的曲线(以下又称为第三推力曲线)、第四推力随时间变化的曲线(以下又称为第四推力曲线)、质心在竖直方向上的位置随时间变化的曲线(以下又称为质心高度曲线)、质心在竖直方向上的速度随时间变化的曲线、质心在水平方向上的位置随时间变化的曲线、质心在水平方向上的速度随时间变化的曲线、倾斜角曲线、角速度曲线。这六条曲线对应于第一期望轨迹。
具体地，这六条曲线的初始条件相同，其中，落地时刻初始高度为0.38m、落地时刻初始速度为-2.5m/s、平稳时刻最终高度为0.28m、落地时刻倾斜角为0.4rad、落地时刻倾斜角角速度为9rad/s、落地时刻水平方向(x方向)初始位置0m,落地时刻水平方向(x方向)初始速度1.5m/s,落地时刻x方向最终位置0m，落地时刻受到的竖直向上的冲击力上限为240N、全过程中受到的竖直向上的冲击力上限为240N，落地时刻受到的水平方向上的冲击力上限为240N、全过程中受到的水平方向上冲击力的上限为240N，落地时刻受到的竖直向上的冲击力的下限为0N、全过程中受到的竖直向上的冲击力的下限为0N，落地时刻受到的水平方向上的冲击力的下限为-240N、全过程中受到的水平方向上的冲击力下限为-240N。
例如，第一推力随时间变化的曲线指示足式机器人在落地过程中的前腿提供的竖直向上的推力随时间步变化，x轴为时间步，y轴为第一推力f1的推力大小(单位为牛顿)。如第一推力曲线所示，在所述足式机器人的前腿在时刻0与平面接触后，所述足式机器人的前腿的推力先逐渐升高后再逐渐降低之后再逐渐上升至稳定值。
例如，第二推力随时间变化的曲线指示足式机器人在落地过程中的前腿提供的水平方向上的推力随时间步变化，x轴为时间步，y轴为第二推力f2的推力大小(单位为牛顿)。如第二推力曲线所示，在所述足式机器人与平面接触后，所述足式机器人的前腿在水平方向上线提供与前进方向相反的摩擦力，该摩擦力逐渐减小，然后再提供反方向的摩擦力，最后稳定在稳定值0。
例如，第三推力随时间变化的曲线指示足式机器人在落地过程中的后腿提供的竖直向上的推力随时间步变化，x轴为时间步，y轴为第三推力f3的推力大小(单位为牛顿)。如第三推力曲线所示，在所述足式机器人的后腿在第16个时间步左右才与平面接触，然后提供较高的竖直方向上的支持力，之后逐渐降低之后再逐渐上升至稳定值。
例如，第四推力随时间变化的曲线指示足式机器人在落地过程中的后腿提供的水平方向上的推力随时间步变化，x轴为时间步，y轴为第四推力f4的推力大小(单位为牛顿)。如第四推力曲线所示，在所述足式机器人的后腿在第16个时间步左右才与平面接触，所述足式机器人的后腿在水平方向上线提供与前进方向相反的摩擦力，该摩擦力逐渐减小，然后再提供反方向的摩擦力，最后稳定在稳定值0。
例如，质心在竖直方向上的速度随时间变化的曲线指示足式机器人在落地过程中的质心高度随时间步变化，x轴为时间步，y轴为高度(单位为厘米)。该曲线示意性的示出了第一期望轨迹在z方向上的变化。如质心高度曲线所示，在所述足式机器人与平面接触后，所述足式机器人的质心的高度逐渐降低然后逐渐上升。
例如，质心在竖直方向上的速度随时间变化的曲线指示足式机器人在落地过程中的质心在重力方向上(z方向上)的速度随时间步变化，x轴为时间步，y轴为速度(单位为米/秒)。该曲线示意性的示出了第一期望轨迹在z方向上的速度变化。如质心在竖直方向上的速度随时间变化的曲线所示，在所述足式机器人与平面接触后，所述足式机器人的质心在竖直方向上的速度逐渐升高然后逐渐降低至0。
例如，质心在水平方向上的速度随时间变化的曲线指示足式机器人在落地过程中的质心在水平方向上的位移随时间步变化，x轴为时间步，y轴为高度(单位为厘米)。该曲线示意性的示出了第一期望轨迹在x方向上的变化。如质心在水平方向上的速度随时间变化的曲线所示，在所述足式机器人与平面接触后，所述足式机器人的质心的先朝前移动，并随着后腿的落地，质心再朝后摆动，最后慢慢调整至稳定态。
例如，质心在水平方向上的速度随时间变化的曲线指示足式机器人在落地过程中的质心在水平方向上(x方向上)的速度随时间步变化，x轴为时间步，y轴为速度(单位为米/秒)。该曲线示意性的示出了第一期望轨迹在x方向上的速度变化。如质心在水平方向上的速度随时间变化的曲线所示，在所述足式机器人与平面接触后，所述足式机器人的质心的先具有朝前移动的速度，并随着后腿的落地，质心再具有朝后摆动的速度至稳定态，也即速度为0。
例如，倾斜角曲线指示足式机器人在落地过程中的倾斜角随时间步变化，x轴为时间步，y轴为倾斜角角度(单位为弧度)。该曲线示意性的示出了第一期望轨迹中倾斜角的变化。如倾斜角曲线所示，在所述足式机器人与平面接触后，所述足式机器人的倾斜角的角度值逐渐增加后逐渐降低至0。
例如，角速度曲线指示足式机器人在落地过程中的倾斜角随时间步变化，x轴为时间步，y轴为倾斜角角速度(单位为弧度/秒)。该曲线示意性的示出了第一期望轨迹中倾斜角的角速度变化。如角速度曲线所示，在所述足式机器人与平面接触后，所述足式机器人的倾斜角的角速度逐渐降低至0后反方向增加，然后逐渐减小至0。
如图4所示，足式机器人首先以较大的加速度下落至任意一只前腿与平面接触。此时，与平面接触的机械腿承受平面给的较大的作用力，质心下降的速度逐渐减小。随着四条机械腿依次与平面接触(例如在第16个时间步左右，四条机械腿都与平面接触)，四条机械腿一起承受平面对足式机器人的作用力，并始终保持与平面的接触，直至足式机器人的质心达到预期的静止高度且倾斜角的角度在0附近。
基于此，为了实现足式机器人在落地过程中的缓冲效果，并且使得足式机器人的机身回弹较小，可以基于图4中质心高度曲线和倾斜角曲线，设定优化目标，以使得期望轨迹尽量实现预期的缓冲效果。例如，优化目标可以是超调较小、倾斜角的变化随时间的积分较小、竖直方向的高度随时间的积分较小、x方向位移量随时间的积分较小、最低的高度要高于某个安全阈值、力的变化不能发生突变、竖直方向的高度的变化速度要满足一定的约束条件等等。
在本公开的一些实施例中，可选地，在所述足式机器人的前腿下落至与平面接触而后腿未下落至于与平面接触的情况下，可以使用足式机器人对应的第一近似模型来确定所述足式机器人的第一期望轨迹。在所述第一近似模型中，所述足式机器人被近似为单刚体，并且在所述足式机器人与平面接触的过程中，所述前腿向所述单刚体提供竖直方向上第一推力以形成所述单刚体的向上推力，所述前腿向所述单刚体提供水平方向上的第二推力以形成所述单刚体的水平方向的推力，第一推力和第二推力形成的扭矩向所述单刚体提供质心角加速度。
例如，足式机器人可以被近似为一个质量为m的单刚体并且其绕着倾斜角对应的旋转轴旋转方向上的转动惯量为I。在足式机器人具有四条机械腿的情况下，两条前腿的在竖直方向上的合力为f1，两条前腿在水平方向上的合力为f2。基于这样的第一近似模型，据牛顿第二定律和欧拉公式，可以确定第一方程(1)，第一方程又称为动力学方程。具体地，所述动力学模型至少部分地基于所述第一近似模型，并且所述动力学模型指示所述单刚体在竖直方向上的加速度、第一推力和重力之间的关系，所述单刚体在水平方向上的加速度、和第二推力之间的关系，并且所述动力学模型还指示所述单刚体的倾斜角方向的转动角的角加速度与第一推力和第二推力之间的关系。
其中，以竖直向上为正方向，g为重力系数，其等于9.81。l为前腿的机械腿的长度。指示所述单刚体的质心在竖直方向(z方向)上的加速度。指示所述单刚体的质心在水平方向(x方向)上的加速度。指示所述单刚体的倾斜角方向的转动角的角加速度。θ指示所述单刚体的倾斜角方向的转动角的角度值。
将该动力学方程写成状态空间表示(State space representation)形式，也即如下示出的第二方程(2)。
第二方程(2)可以被简写成第三方程(3)的形式。值得注意的是，在本公开中，粗体用于表示向量(矩阵)。
其中，对应地，将第三方程(3)按照时间步(时间步的长度为Δt)离散化，可以得到第四方程(4)。
令Ad＝AcΔt+I，Bd＝BcΔt。基于模型预测控制(Model Predictive Control，MPC)。可以得到第五方程(5)。
其中，x1表示第一时间步对应的由质心竖直方向高度、质心竖直方向速度、重力加速度组成的向量，x2表示第二时间步对应的由质心竖直方向高度、质心竖直方向速度、重力加速度组成的向量，xk表示第k时间步对应的由质心竖直方向高度、质心竖直方向速度、重力加速度组成的向量，以此类推，其中，x0为初始时刻对应的由质心竖直方向高度、质心竖直方向速度、重力加速度组成的向量。第五方程(5)也可以被简写成第六方程(6)
X＝AapX0+BqpU (6)
其中，
第六方程(6)已经给出了每个时间步对应的数学表达式。基于此，可以对应地基于足式机器人下落中期望实现的缓冲效果，来设计本公开各个实施例对应的优化目标，以实现最优的第一期望轨迹的求解。例如，所述第一期望轨迹使得以下各项的组合达到极值：所述足式机器人的质心在水平方向上的位移量、所述足式机器人的质心波动量、所述足式机器人受到的冲击力总量、所述足式机器人的下蹲量、所述足式机器人受到的冲击力突变量。以上各项均可以有对应的权重系数，并且以各种方式进行组合。
例如，可以设定如第七方程(7)所示的优化目标-Z函数，以求解最优的推力U。Z函数事实上是用于求解如何使得以下各项中的一项或多项的组合达到极值的函数：所述足式机器人的质心在水平方向上的位移量、所述足式机器人受到的冲击力总量、所述足式机器人的下蹲量、所述足式机器人受到的冲击力突变量、和所述足式机器人的机身转动的角度。
Xref是一个常数向量，其表示图4中虚线所示的静止高度。x表示整个过程中的质心最低高度。
其中，Z函数的第1项可以作为所述足式机器人的质心波动量的一种表现形式，其为足式机器人应满足动力学方程加权后的值(权重系数为L)。例如，在图4中，第1项被示出为灰色区域的面积加权后的值。||AqpX0+BqpU-Xref||2越小，则说明在足式机器人下落的过程中，质心的波动越小，足式机器人越稳定。
Z函数的第2项可以作为所述足式机器人受到的冲击力总量的一种表现形式，其为足式机器人所受平面反作用力之和随时间的积分加权后的值(权重系数为K)。||U||2越小，则说明在足式机器人下落的过程中，足式机器人受到的冲击力的总和越小。
Z函数的第3项||h-x||2 Q表示在整个下落过程当中足式机器人质心的最低点与静止高度之间的距离加权后的值(权重系数为Q)。||h-x||2越小，则说明在足式机器人下落的过程中，足式机器人下蹲的程度越低(也即足式机器人不需蹲的太低也能保持平衡)。Z函数的第3项||h-x||2 Q可以作为所述足式机器人的下蹲量的一种表现形式。
Z函数的第4项表示相邻时间步之间平面向足式机器人提供的反作用力的差值加权后的值(权重系数为W)。||uk+1-uk||2越小，则说明在足式机器人下落的过程中，足式机器人受到冲击力的突变越小。Z函数的第4项可以作为所述足式机器人受到的冲击力突变量的一种表现形式。
以上仅是Z函数的一种组合方式，Z函数的上述各项也仅是所述足式机器人的质心波动量、所述足式机器人受到的冲击力总量、所述足式机器人的下蹲量、所述足式机器人受到的冲击力突变量的一种示例表示形式，本公开并不为限。
更进一步地，本公开的各项实施例通过上述的各个权重系数来调节各项对应的重要性。例如，当K越大，则说明方法20的控制方案中更看重机器人受到的冲击力。值得注意的是，本公开中加权方案包括多种。例如，加权方案可以是乘法加权方案，则Z函数的第1项就可以被进一步表示为(AqpX0+BqpU-Xref)TL（AqpX0+BqpU-Xref)。加权方案还可以是幂加权方案或加法方案，本公开并不以此为限。以此类推，Z函数的其余各项也可以以不同的加权方案进行计算，本公开在此就不再赘述。
在求解Z函数的过程中，还需要考虑以下各项约束条件。
例如，第一约束条件指示所述足式机器人的部分机械腿接触平面的第一个瞬间所受到的冲击力的大小小于足式机器人能够承受的最大冲击力。第一约束条件可以被表示为u0≤uU。其中，u0表示足式机器人接触平面的第一个瞬间所受到的冲击力的大小(其包括足式机器人接触地面的第1个瞬间所受到的力在z方向上的大小和足式机器人x方向在接触地面的第一个瞬时所受到的力的大小)，其将小于足式机器人能够承受的最大冲击力uU。足式机器人能够承受的最大冲击力uU取决于足式机器人的结构特性和刚体的强度，其示例取值为240N。本公开不受该示例取值的限制。
例如，第二约束条件指示所述足式机器人受到的竖直方向上的支持力小于所述平面能够提供的支持力的上限，并大于所述平面能够提供的竖直方向上的支持力的下限。第四约束条件指示所述足式机器人受到的冲击力小于所述平面能够提供的水平推力的上限，并大于所述平面能够提供的水平推力的下限。第二约束条件和第四约束条件可以被综合表示为FL≤u≤FU。FL表示平面能够提供的支持力和水平方向上的推力的下限，FU表示平面能够提供的支持力的上限。第二约束条件可以被分为两部分，第一部分为地面支持力的上下限限制，一般下限为0，因为z方向的力不能小于0，地面无法给机器人提供拉力，第二部分为x方向上的力的前后限制，其可正可负，例如其上限可以是240N，下限可以是-240N，x方向上的力能提供拉力或推力(例如，摩擦力)。
例如，第三约束条件指示所述足式机器人的质心的高度始终大于最低高度。第三约束条件可以被表示为第三约束条件指示足式机器人的质心的每个时刻的z方向高度一直大于最低高度x。其中，x为由最低高度序列值所组成的列向量。
此外，根据足式机器人的构型不同，还可以包括更多或更少的约束条件，本公开并不以此为限。
将第七方程(7)进行数学等价变换，可以得到第八方程(8)。
将第八方程(8)进行数学等价变换，可以得到第九方程(9)。
将第九方程(9)进行数学等价变换，可以得到第十方程(10)。
将第十方程(10)进行数学等价变换，可以得到第十一方程(11)。
其中，W满足第十二方程(12)。
其中，中不含变量U，其不会对Z函数的最小值产生影响，因此，可以不计算
也即，的最终可以以第十三方程(13)进行表示。
其中，
响应于求解出使得Z最小的U和x，即可进一步求解得到图4中的最优的第一期望轨迹，也即由质心在各个时间步对应的z方向和x方向上的数值序列。在本公开的另一些实施例中，还可以使用足式机器人的全模型来规划足式机器人的第一期望轨迹。这样的实施例规划的第一期望轨迹的精度较高，但往往需要较高的算力才能实现实时规划。
在本公开的另一些实施例中，可选地，在所述足式机器人的后腿下落至与平面接触而前腿未下落至与平面接触的情况下，则可以使用足式机器人对应的第二近似模型来确定所述足式机器人的第一期望轨迹。在所述第二近似模型中，所述足式机器人被近似为单刚体，并且在所述足式机器人与平面接触的过程中，所述后腿向所述单刚体提供竖直方向上的第三推力以形成所述单刚体的向上推力，所述后腿向所述单刚体提供水平方向上的第四推力以形成所述单刚体的水平方向的推力，第三推力和第四推力形成的扭矩向所述单刚体提供质心角加速度。
例如，足式机器人可以被近似为一个质量为m的单刚体并且其绕着倾斜角对应的旋转轴旋转方向上的转动惯量为I。在足式机器人具有四条机械腿的情况下，两条后腿的在竖直方向上的合力为f3，两条后腿在水平方向上的合力为f4。基于这样的第二近似模型，据牛顿第二定律和欧拉公式，可以确定第十四方程(14)，第十四方程又称为动力学方程。具体地，所述动力学模型至少部分地基于所述近似模型，并且所述动力学模型指示所述单刚体在竖直方向上的加速度、第三推力和重力之间的关系，所述单刚体在水平方向上的加速度和第四推力之间的关系，以及所述动力学模型还指示所述单刚体的倾斜角方向的转动角的角加速度与第三推力和第四推力之间的关系。
其中，以竖直向上为正方向，g为重力系数，其等于9.81。l为后腿的机械腿的长度。指示所述单刚体的质心在竖直方向(z方向)上的加速度。指示所述单刚体的质心在水平方向(x方向)上的加速度。指示所述单刚体的倾斜角方向的转动角的角加速度。θ指示所述单刚体的倾斜角方向的转动角的角度值。
将该动力学方程写成状态空间表示(State space representation)形式，也即如下示出的第十五方程(15)。
所述足式机器人的后腿下落至与平面接触而前腿未下落至与平面接触的情况的后续求解方式与在所述足式机器人的前腿下落至与平面接触而后腿未下落至与平面接触的情况相同/相似，为简明起见，本公开在此不再赘述。
在本公开的另一些实施例中，然后，在所述足式机器人的各条机械腿中的所有机械腿均下落至与平面接触后(例如图4示出的第16个时间步之后)，可以确定所述足式机器人对应的第三期望轨迹和第四期望轨迹，其中，所述第三期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第四期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹；基于所述足式机器人对应的动力学模型以及所述第三期望轨迹和第四期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的基座的高度不变且所述足式机器人的倾斜角方向的转动角为零。
可选地，在所述足式机器人的所有机械腿均下落至与平面接触的情况下，则可以使用足式机器人对应的第三近似模型来确定所述足式机器人的第三期望轨迹。其中，在所述第三近似模型中，所述足式机器人被近似为单刚体，并且在所述足式机器人与平面接触的过程中，所述前腿向所述单刚体提供竖直方向上的第一推力，所述后腿向所述单刚体提供竖直方向上的第三推力，第一推力和第三推力的合力形成所述单刚体的向上推力，所述前腿向所述单刚体提供水平方向上的第二推力，所述后腿向所述单刚体提供水平方向上的第四推力，第二推力和第四推力的合力形成所述单刚体的水平方向的推力，第一推力、第二推力、第三推力和第四推力形成的扭矩向所述单刚体提供质心角加速度。
例如，足式机器人可以被近似为一个质量为m的单刚体并且其绕着倾斜角对应的旋转轴旋转方向上的转动惯量为I。在足式机器人具有四条机械腿的情况下，两条前腿的在竖直方向上的合力为f1，两条前腿在水平方向上的合力为f2。两条后腿的在竖直方向上的合力为f3，两条后腿在水平方向上的合力为f4。基于这样的第三近似模型，据牛顿第二定律和欧拉公式，可以确定第十六方程(16)，第十六方程又称为动力学方程。具体地，所述动力学模型至少部分地基于所述近似模型，并且所述动力学模型指示所述单刚体在竖直方向上的加速度、第一推力、第三推力和重力之间的关系，所述单刚体在水平方向上的加速度、第二推力和第四推力之间的关系，以及所述动力学模型还指示所述单刚体的倾斜角方向的转动角的角加速度与第一推力、第二推力、第三推力和第四推力之间的关系。
其中，以竖直向上为正方向，g为重力系数，其等于9.81。l为前腿和后腿的机械腿的长度。指示所述单刚体的质心在竖直方向(z方向)上的加速度。指示所述单刚体的质心在水平方向(x方向)上的加速度。指示所述单刚体的倾斜角方向的转动角的角加速度。θ指示所述单刚体的倾斜角方向的转动角的角度值。
将该动力学方程写成状态空间表示(State space representation)形式，也即如下示出的第十七方程(17)。
所述足式机器人的所有机械腿均下落至与平面接触的后续求解方式与在所述足式机器人的前腿下落至与平面接触而后腿未下落至与平面接触的情况相同/相似，为简明起见，本公开在此不再赘述。
在本公开的上述实施例中，基于第一近似模型、第二近似模型、和/或第三近似模型，甚至全模型对足式机器人落地后的质心运动轨迹进行了规划，从而，保证了足式机器人在落地过程中，各个关节仅受到较小的冲击力，机身回弹较小，倾斜角变化较小，在保证落地功能的同时对足式机器人形成了良好的抗冲击保护效果。
接下来，参考图5和图6进一步描述如何确定足式机器人的第二期望轨迹的实施例。图5是示出根据本公开的实施例的足式机器人落地过程中的机械腿与平面接触的示意图。图6是示出根据本公开的实施例提供的一种三次样条差值的计算原理示意图。
由于执行空翻任务的足式机器人往往在下落至前腿或后腿与平面接触的瞬时时刻存在一定的倾倒趋势，因此，需要在第一阶段控制所述足式机器人的部分机械腿与平面接触后各个关节的动作，以使得所述足式机器人与平面接触的机械腿沿位移一段距离，并且调整与平面接触的机械腿的压缩状态，直至所述足式机器人的各条机械腿中的每条机械腿均下落至与平面接触。
更进一步地，即使足式机器人的各条机械腿中的每条机械腿均下落至与平面接触，该足式机器人的倾倒趋势可能仍未消失，或者机械腿仍处于过分压缩的状态，因此还需要对足式机器人进行第二阶段的控制，以使得该足式机器人达到目标高度，并且足式机器人的倾斜角方向的转动角为零。
如图5的左图所示，足式机器人的部分机械腿在与平面接触时，往往存在一定的倾倒趋势。假设在部分机械腿与平面接触的瞬时时刻(如图5所示的初始时刻，也即前腿刚与平面接触的瞬时时刻)，确定该足式机器人存在向前倾倒的趋势(或在该瞬时时刻存在较大的前向速度、或存在较大的倾斜角方向的角速度)，则可以将期望的前腿的足端中间位置设定为前腿与平面的接触位置的前方一定位置处。由此，第二期望轨迹的设计目的可以被确定为从部分机械腿在与平面接触到所有机械腿都落地的阶段，使得足式机器人的机械腿能够在空中伸展，让足端在接触到地面后有更长的缓冲时间和缓冲距离，以使得足式机器人能够应对更大的下落高度以及更大的x方向速度。设足端为C，足式机器人质心为O，世界系为W，则质心与足端的相对位置为POC，世界系与足端的相对位置为PWC，其二者具有关系：POC＝R*PWC，其中，R为足式机器人的质心的旋转矩阵。
可以根据足式机器人在与平面接触的瞬间是否具有x方向上的初速度来设计第二期望轨迹。在具有x方向上的初速度的示例实施例中，所述确定足式机器人的第二期望轨迹还包括：在部分机械腿与平面接触的瞬时时刻，确定各条机械腿远离所述基座的一端在初始时刻的足端位置坐标，将所述初始时刻的足端位置坐标作为足端初始位置；响应于所述足式机器人的质心在所述初始时刻不具备水平方向上的速度，基于所述足端的初始位置和预设的竖直方向上的增量，确定所述各条机械腿远离所述基座的一端在中间时刻的足端位置坐标，将所述中间时刻的足端位置坐标作为足端中间位置；以及基于所述足端初始位置和所述足端中间位置，使用三次样条差值确定所述各条机械腿远离所述基座的一端的运动轨迹作为第二期望轨迹；其中，在所述中间时刻，所述足式机器人的所有机械腿均与平面接触且所述足式机器人的速度不为零。
具体地，足式机器人的质心在初始时刻不具备水平方向上的速度的情况也即足式机器人在前腿或后腿落地时不具备x方向上的初速度。此时，第二期望轨迹的设计目标是机器人的足端位置应当尽量在世界系下保持竖直，且z方向长度更长，有更多的时间做动作，故可以设计PWC＝{x0，y0，z0+Δz}，其中，x0，y0，z0分别为标准站立状态下足式机器人的足端位置，Δz是我们设置的z方向增量，以使足式机器人的腿部伸展，Δz的示例值可取0.06m。
可选地，在不具有x方向上的初速度的示例实施例中，所述确定足式机器人的第二期望轨迹还包括：在部分机械腿与平面接触的瞬时时刻，确定各条机械腿远离所述基座的一端在初始时刻的足端位置坐标，将所述初始时刻的足端位置坐标作为足端初始位置；响应于所述足式机器人的质心在所述初始时刻具备水平方向上的速度，基于所述足端的初始位置、预设的前腿在水平方向上的位移增量、预设的后腿在水平方向上的位移增量、和预设的竖直方向上的增量，确定所述各条机械腿远离所述基座的一端在中间时刻的足端位置坐标，将所述中间时刻的足端位置坐标作为足端中间位置；以及基于所述足端初始位置和所述足端中间位置，使用三次样条差值确定所述各条机械腿远离所述基座的一端的运动轨迹作为第二期望轨迹；其中，在所述中间时刻，所述足式机器人的所有机械腿均与平面接触且所述足式机器人的速度不为零。
具体地，足式机器人的质心在初始时刻具备水平方向上的速度的情况也即足式机器人在前腿或后腿落地时具备x方向上的初速度的情况。此时需要将前腿的足端和后腿的足端进行区分对待。如果是前腿先落地，则可以设计PWC1＝{x0+Δxfront，y0，z0+Δz}。如果是后腿先落地，则可以设计PWC2＝{x0+Δxhind，y0，z0+Δz}。其中Δxfront和Δxhind分别为前后足端位置向前(或向后)伸展的距离，这是因为考虑到足式机器人落地后伴随着x方向初速度会因为惯性继续向前运动一段距离，该距离大小随着x方向速度的变化而变化。故为了使落地更加柔顺，设计Δxfront和Δxhind时可以与足式机器人x方向初速度大小相关，即Δxfront＝k1(vx0)，Δxhind＝k2(vx0)，在一些实施例中，k1(vx0)和k2(vx0)为线性函数，例如，Δxfront＝0.08*vx0和Δxhind＝0.02*vx0。
此外，在本公开的又一些实施例中，有可能在落地瞬间冲击力太大，导致足式机器人从平面弹跳起来。这种情况可以进一步对足端位置进行规划，例如，使得前腿和后腿尽量沿着垂直于所述水平方向和所述竖直方向构成的平面的方向(也即y轴方向，y轴方向垂直于x轴和z轴构成的平面)上张开，以减小机器人在竖直方向上的受力。
此时，所述确定足式机器人的第二期望轨迹还包括：在部分机械腿与平面接触的瞬时时刻，确定各条机械腿远离所述基座的一端在初始时刻的足端位置坐标，将所述初始时刻的足端位置坐标作为足端初始位置；响应于所述足式机器人的质心在所述初始时刻受到的冲击力大于预设阈值，基于所述足端的初始位置、预设的机械腿在水平方向的位移增量、预设的竖直方向上的增量、预设的机械腿在垂直于所述水平方向和所述竖直方向构成的平面的方向上的位移增量，确定所述各条机械腿远离所述基座的一端在中间时刻的足端位置坐标，将所述中间时刻的足端位置坐标作为足端中间位置；基于所述足端初始位置和所述足端中间位置，使用三次样条差值确定所述各条机械腿远离所述基座的一端的运动轨迹作为第二期望轨迹，其中，在所述中间时刻，所述足式机器人的所有机械腿均与平面接触且所述足式机器人的速度不为零。
如果足式机器人的质心在所述初始时刻受到的冲击力大于预设阈值，足式机器人可能在落地的瞬间就被弹起，为了更稳定的表现可以将y方向也规划出一个偏移量，使足式机器人左腿向左伸展，右腿向右伸展，该姿态能够有效防止回弹，使落地更稳，类似地左腿和右腿需要被区别对待，分别设计PWC1＝{x0，y0+Δyleft，z0+Δz}和PWC2＝{x0，yo+Δyright，z0+Δz}，其中Δyleft和Δyright为机器狗腿部向y方向伸展的距离，通常情况下Δyleft＝-Δyright。虽然y方向的伸展会使落地更稳，但这样的方案也可能使足式机器人在完成落地回弹后不容易回到站立的标准姿态，所以通常该偏移量可以被设置尽可能小。在一些实施例中，Δyleft和Δyright被设计为等于零。
具体地，上述三种情况也可以被总结为表1中的各个表达式。
具体的Δxfront、Δyleft、Δz、Δyright、Δxhind的大小在本公开中不做出限制，但仍需遵循足式机器人的物理限制。可选地，Δx与x方向初速度相关，同时为了整个工程的完成性和后续动作的进行，y方向的偏移量可以尽可能小。最后，通过上述过程设计出合理的PWC1、PWC2、PWC3、PWC4后，再通过其与POC的关系，利用在实时的旋转矩阵计算出质心与足端的相对位置为POC，最后将该相对位置解算到各个关节电机的转矩，从而实现完整的足端位置规划。
进一步地，针对步骤“确定各条机械腿远离所述基座的一端在初始时刻的足端位置坐标，将所述初始时刻的足端位置坐标作为足端初始位置”，在具体实现中，计算机设备可将当前时刻采集到的足式机器人的传感信息输入至腿部里程计中，以使腿部里程计根据传感信息对足式机器人的各条机械腿在当前时刻的位置进行计算，得到位置信息。
其中，足端位置坐标的位置信息可至少包括：其他三条机械腿在世界坐标系下的各个方向位置向量。不同的方向位置向量对应不同的坐标轴方向；一个方向位置向量用于指示。足式机器人的各条机械腿在相应坐标轴方向上的位置。其中，腿部里程计计算与水平轴方向对应的方向位置向量的方式包括：首先，可基于当前姿态信息计算旋转矩阵，所谓的旋转矩阵是指通过改变任一向量的方向，从而实现将该任一向量映射到机器人基坐标系中的矩阵；具体可根据当前姿态信息确定足式机器人的基座姿态角，根据该基座姿态角计算出旋转矩阵。以及，还可根据各个关节的关节角信息计算出参考位置向量，该参考位置向量用于指示：足式机器人的基座质心和各条机械腿的足端之间的相对位置。接着，可采用旋转矩阵将参考位置向量映射到机器人基坐标系中，得到目标位置向量；具体的，可采用旋转矩阵乘以参考位置向量，得到目标位置向量。
另外，可获取足式机器人的质心在世界坐标系下的三维位置向量。然后，可对目标位置向量在水平轴方向上的分量，以及三维位置向量在水平轴方向上的分量进行融合处理，得到水平轴方向对应的方向位置向量；此处的融合处理可包括求和处理。
虽然上述的各种场景均以世界坐标系为例进行的说明，但是本公开的各项实施例也可以以基于本体坐标系来进行计算。本公开并不以此为限。
针对步骤“基于所述足端初始位置和所述足端中间位置，使用三次样条差值确定所述各条机械腿远离所述基座的一端的运动轨迹作为第二期望轨迹”，在具体实现中，可见从图5的左图演变至图5的右图的过程中，各条机械腿的足端位置的x和y方向数值序列使用初始时刻和结束时刻的位置的三次样条差值(Cubic Spline Interpolation)实现。同理，足式机器人的质心的x和y方向数值序列使用初始时刻和结束时刻的位置的三次样条差值实现。
如图6所示，本公开的实施例提供的三次样条差值就是把已知数据分割成若干段，每段构造一个三次函数，并且保证曲线通过特定点，并保证在某些特定点满足一定的速度约束条件。示例性的，设已知数据包括(pa，va，ta)、(pb，tb)以及(pc，vc，tc)等三个数据，p表示位置，v表示速度，t表示时间，且将已知数据分割成两段为例，三次样条差值的计算原理可参见图6所示：可先构造得到f1(t)和f2(t)两个三次函数，然后可基于这两个三次函数和已知数据建立等式方程组，从而通过该等式方程组求解出三次多项式系数(a0，a1，a2，a3，b0，b1，b2，b3)；在求解出三次多项式系数后，后续便可利用这两个三次函数确定足式机器人在任一时刻所处的位置以及相应的速度，从而实现足式机器人的控制。
已经参考图4进行了详细描述了第一期望轨迹。进一步地，基于第一期望轨迹可以对应地求解各条机械腿的z方向数值序列。具体地，在所述足式机器人的四条机械腿均与平面接触后，其余机械腿的长度随着所述足式机器人的质心的高度的变化而变化。因此，各条机械腿的z方向数值序列可以被描述为：足式机器人的质心在z方向达到第一期望轨迹指示的位置时，各条机械腿的足端正好可以接触平面的高度。此外，也可以使用三次样条差值求解另外三条机械腿的z方向数值序列，本公开并不以此为限。
接下来，参考图7A至图7F进一步描述如何基于所述足式机器人的质心对应的动力学模型以及所述第一期望轨迹和第二期望轨迹来控制所述足式机器人与平面接触后各个关节的动作的实施例。其中，图7A至图7F示出了根据本公开实施例的足式机器人在各种初始条件下落地的动作的仿真图和真机测试图。
基于本公开的足式机器人的动力学方程及上述的第一期望轨迹对所述足式机器人进行控制的方案又称为模型预测控制(Model Predictive Control，MPC)。而根据动力学方程进一步结合第二期望轨迹对各个关节的控制方案又称全身动力学(Whole-BodyDynamics Control，WBC)。
在本公开的实施例将MPC和WBC相结合实现了执行空翻任务的足式机器人在落地过程中的缓冲控制，其可以被简单描述为：通过计算未来控制变量的轨迹(也即第一期望轨迹和第二期望轨迹)，来优化控制器的输出(也即各个关节电机的力矩)。优化过程在一个有限的时间窗口进行，并且利用时间窗口的初始系统信息进行优化。其中，时间窗口的起始时刻为足式机器人与平面接触的瞬间，终止时刻为足式机器人稳定站立的时刻。
作为一个示例，根据本公开的足式机器人的动力学方程可以以第十八方程(18)进行表示。
第十八方程(18)的前6行(如以下的第十九方程(19)所示)即足式机器人的质点动力学信息。
其中，Mp表示基座对应的质量和惯量矩阵。表示六维的质心位置和姿态向量，其是第一期望轨迹对应的序列。可选地，此时，质心位置仅考虑重力方向(z方向)的位置和足式机器人的倾斜角(pitch)方向的转动角度，而x方向、y方向均为零，横滚角(roll)方向的转动角度值和偏航角(yaw)方向的转动角度均为零。f为平面向4个足端提供的接触力，平面向每个足端提供的接触力均为3维的力，因此，f总维数为12。Cp表示基座的重力项、离心力项和科里奥利力项。表示基座的雅克比矩阵的转置。基于此，可以应用MPC求解出平面向足式机器人提供的接触力f。也即，基于所述足式机器人对应的动力学模型，能够确定在各个时间步所述平面与所述足式机器人的各条机械腿之间的接触力，以使得所述足式机器人的质心在重力方向和水平方向上移动的实际轨迹以及所述足式机器人的倾斜角变化的实际轨迹均符合所述第一期望轨迹或第三期望轨迹。
第十八方程(18)的下半部分(如以下的第二十方程(20)所示)即足式机器人的关节的动力学信息。
其中，Mθ表示每个关节对应的质量和惯量矩阵，θ表示所有可驱动的自由度的角度(在图1或图5所示的四足机器人中，其包括12个关节电机对应的自由度)。Cθ表示可驱动关节的重力项、离心力项和科里奥利力项。表示可驱动关节的雅克比矩阵的转置。为足式机器人的12个可以主动驱动的关节角加速度。τ为12个关节的输入力矩。根据第十九方程(19)求出的接触力f，机器人动力学模型的其他参数均已知，可以应用第二十方程(20)求出机器人各个关节力矩τ。
更进一步地，第二十方程(20)还可以被写成第二十一方程(21)的形式。
其中，可以由第二十二方程(22)求解。
其中，而，xd和由第二期望轨迹确定(其分别指示各条机械腿的足端期望位置和各条机械腿的足端期望速度)，x和则是各条机械腿的足端实际位置和各条机械腿的足端实际速度。而kp是比例控制反馈系数、kd是微分控制反馈系数。
也即，基于所述足式机器人对应的动力学模型以及在各个时间步所述平面与所述足式机器人的全部或部分机械腿之间的接触力，确定各个关节电机在各个时间步提供的电机力矩，以使得所述各条机械腿远离所述基座的一端的轨迹符合所述第二期望轨迹。
更进一步地，还可以针对每条机械腿的压缩和回弹状态分别设定上述的比例控制反馈系数。例如，根据每条机械腿的当前的关节角角度情况和机械腿正常站立的姿态下的关节角度的对比以及关节角度变化的趋势，确定每条机械腿是在被压缩还是回弹的状态。例如，假设针对某条机械腿，令其落地的初始时刻为t0，之后该机械腿开始进入被压缩的状态，直到时间t1，该机械腿被压缩到最低点开始回弹，到该机械腿回弹到正常站立的高度时间为t2。在t0、t1、t2三个时刻，分别设置比例控制反馈系数kp0，kp1，kp2。采用基于这三个时刻的比例控制反馈系数的三次样条差值或四次样条差值(g1或g2)，可以进一步获得其余中间时刻(时间步)的比例控制反馈系数kp。也即，可以使用第二十三方程(23)来获得其余中间时刻(时间步)的比例控制反馈系数kp。
同理，类似地，可以求解微分控制反馈系数。作为一个具体地实施例，可以令t0＝0，t1约等于0.1s，t2约等于0.2s。kp0＝500、kp1＝400、kp3＝250。kdo＝ka1＝kdd2＝2。本公开的实施例不受以上具体数值的取值限制。
此外，进一步地考虑到空翻任务的复杂性，在一些实施例中，还可以将执行空翻任务的足式机器人在落地过程中的缓冲控制进一步分为六个阶段或四个阶段，如果有两腿先落地到四腿落地的过程则分为六个阶段(对应下述第一、二、三、四、五、六阶段)，如果落地时刻即为四条腿同时落地则分为四个阶段(对应下述第三、四、五、六阶段)。
在第一阶段，两腿接触地面，进入两腿的规划阶段以得到第一期望轨迹和第二期望轨迹。通常该规划阶段用时约为0.002s，规划时间取决于机器人的参数和工程经验，作为一个具体实施例可以取0.5s，但值得注意的是，该规划时间并不会完全执行，一旦检测到所有机械腿都落地即进入下一个规划阶段。在该时间内，使用简化模型得到的质心和姿态规划轨迹作为下一阶段控制器需要追踪的目标序列，但仅考虑f1与f2作为控制量或者f3与f4作为控制量，具体根据前腿先落地还是后腿先落地决定。在第一阶段对应的总时间步内，可以生成第一期望轨迹和第二期望轨迹，确定控制器在第二阶段需要追踪(tracking)的目标序列，并按照实际观测情况调节控制器的相关权重数值。当然本公开的实施例不受以上具体数值的取值限制。
在第二阶段，在第一阶段对应的总时间步之后，可以根据工程经验设定预设时长，并将第一阶段结束时刻足式机器人实际的状态值与第一期望轨迹的终末值进行比较以确定第二阶段的质心位置和姿态的参考轨迹。具体地，可以使用底层的全身控制器(wholebody controller)追踪第一阶段生成的第一期望轨迹和第二期望轨迹，并按照实验现象调节实际进行控制的控制器相关权重参数。
在第三阶段，经过了前两个阶段的规划和控制，进入第三阶段时，足式机器人的所有机械腿均落地，从而进入四腿的规划阶段以得到第三期望轨迹和第四期望轨迹。通常该规划阶段用时约为0.014s，规划时间取决于机器人的参数和工程经验，作为一个具体实施例可以取0.5s。在该时间内，使用简化模型得到的质心和姿态规划轨迹作为下一阶段控制器需要追踪的目标序列，但需要考虑f1、f2、f3和f4四者作为控制量。在第三阶段对应的总时间步内，可以生成第三期望轨迹和第四期望轨迹，确定控制器在第四阶段需要追踪(tracking)的目标序列，并按照实际观测情况调节控制器的相关权重数值。当然本公开的实施例不受以上具体数值的取值限制。
在第四阶段，进行四腿控制阶段，在第三阶段对应的总时间步之后，可以根据工程经验设定预设时长，并将第三阶段结束时刻足式机器人实际的状态值与第三期望轨迹的终末值进行比较以确定第四阶段的质心位置和姿态的参考轨迹。具体地，可以使用底层的全身控制器(whole body controller)追踪第三阶段生成的第三期望轨迹和第四期望轨迹，并按照实验现象调节实际进行控制的控制器相关权重参数。
第五阶段，作为一个具体实施例，其时长可以取0.5s-0.7s，本公开不受以上具体数值的取值限制。在第五阶段，第三阶段得到的第三期望轨迹和第四期望轨迹已经用完了。此时，可以将第三阶段得到的第三期望轨迹和第四期望轨迹的末尾值与最终的稳定状态的站立高度和姿态之间做线性或三次样条差值，得到这一阶段的质心位置和姿态的参考轨迹，发给控制器，计算出各关节的前馈力矩，完成机器人控制。第五阶段由于开始时刻机器人状态的差异，可能在z方向、x方向或pitch方向上有较大的速度，需要适应性的调整控制器的相关权重数值。该部分的权重数值可能与第四阶段控制器的权重数值存在较大差异。
第六阶段，又称为JointPD状态，经过了前五个阶段的控制，进入该阶段时机器人的姿态基本符合机器人直接通过位置控实现站立的需求。质心位置和姿态的速度数值相对较小，可以直接从力控切换到JointPD的位置控站立姿态。这其中可能包括比例控制反馈系数kp和微分控制的反馈系数kd的差值策略。
在图7A中的第一仿真测试和图7B中的第一真机测试中，足式机器人具有z方向下落速度。仿真结果显示，当机器人从高度1.0米的位置下落时，在接触地面瞬间，腿长收缩，同时身体和地面之间的距离逐渐缩减，腿部给予机器人质心一个反作用力，该反作用力作用下机器人的本体姿态实现减速，但同时在机器人本体高度回弹的过程中不会让机器人的足端高度从地面反弹起来，整个质心高度变化过程中超调较小。在柔顺控制的结尾，机器人可以以一个预设姿态站立在地上，该姿态及质心的高度回弹过程整体符合预期。
在图7C中的第二仿真测试和图7D中的第二真机测试中，足式机器人具有倾斜角初始角度和z方向下落速度。仿真结果显示，当存在较大初始倾斜角角度时，机器人首先检测到两腿接触地面，此时两腿腿长收缩，同时身体和地面之间的距离逐渐缩减，后腿离地距离也逐渐缩短，同时在两腿阶段腿部给予反作用力，机器人的本体姿态开始减速。直到四腿都接触地面，进行通z方向竖直下落同样的动作，同图7A/图7B一样，机器人本体高度回弹的过程中始终不会让机器人的足端高度从地面反弹起来，且整个质心高度变化过程中超调(overshot)较小。整体回弹过程符合规划预期。
在图7E中的第三仿真测试和图7F中的第三真机测试中，足式机器人具有x方向初速度和z方向下落速度。仿真结果显示，当存在较大x方向初速度时，机器人在接触地面后有向前冲一段距离，然后再在腿的作用下拉回的过程。整个过程中，机器人质心高度柔顺变化的同时，其x方向也柔顺变化，直到回复到刚接触地面时的x姿态。整体回弹过程符合规划预期。
本公开的实施例对落体运动下的足式机器人的建立了模型，并基于该模型，对足式机器人落地后的质心运动轨迹和足端位置轨迹进行了规划，并基于规划的质心运动轨迹和足端位置轨迹，求解各个电机的控制力矩，对足式机器人进行控制。从而，保证了足式机器人在落地过程中，各个关节仅受到较小的冲击力，机身回弹较小，倾斜角变化较小，在保证落地功能的同时对足式机器人形成了良好的抗冲击保护效果。
接下来，参考图8进一步描述在控制所述足式机器人的各个关节的动作的过程中，如何获取足式机器人的当前状态以使得控制更为准确地。其中，图8示出了根据本公开实施例的足式机器人的控制系统架构。
虽然已经参考图7A至图7F描述了如何根据动力学模型、第一期望轨迹和第二期望轨迹来控制足式机器人的各个关节的动作，然而在实际的控制过程中，由于硬件的限制，往往无法精准地实现在每个时间步都能准确地输出对应的关节力矩。因此，还需要实时估计足式机器人的当前状态，以对控制信息进行微调。也即，步骤S202还包括：基于所述足式机器人对应的动力学模型以及所述在各个时间步所述平面与所述足式机器人的部分机械腿之间的接触力，确定各个关节电机在各个时间步提供的电机力矩；基于足式机器人的当前状态信息，调整各个关节电机在各个时间步提供的电机力矩。同时，步骤S204还包括：基于所述足式机器人对应的动力学模型以及所述在各个时间步所述平面与所述足式机器人的全部机械腿之间的接触力，确定各个关节电机在各个时间步提供的电机力矩；基于足式机器人的当前状态信息，调整各个关节电机在各个时间步提供的电机力矩。
首先，在足式机器人的空翻后落地的过程中，从部分机械腿或全部机械腿与平面接触的瞬时时刻开始至足式机器人稳定，可以根据当前时刻采集到的足式机器人的传感信息，计算足式机器人的各条机械腿在所述当前时刻的位置，得到位置计算结果。
其中，传感器信息是指由足式机器人中的传感器所采集到的信息。由前述可知，足式机器人中的传感器可包括IMU传感器以及关节角编码器等，IMU传感器可用于实时采集足式机器人的姿态信息和加速度，关节角编码器可实时提供足式机器人的每条机械腿上的各个关节的关节角信息(如关节角度、角速度反馈值等)；基于此，传感信息可包括：通过IMU传感器采集到的足式机器人的当前姿态信息，以及通过关节角编码器采集到的足式机器人的各个关节的关节角信息。
在具体实现中，计算机设备可将当前时刻采集到的足式机器人的传感信息输入至腿部里程计中，以使腿部里程计根据传感信息对足式机器人的各条机械腿在当前时刻的位置进行计算，得到位置计算结果。其中，位置计算结果可至少包括：世界坐标系下的至少两个方向位置向量，不同的方向位置向量对应不同的坐标轴方向；一个方向位置向量用于指示：足式机器人的各条机械腿在相应坐标轴方向上的位置。当足式机器人被近似在一个二维平面(x轴(水平轴)和z轴(竖直轴)所构成的平面)内运动时，该至少两个方向位置向量可包括：与水平轴方向对应的方向位置向量(采用psx表示)，以及与竖直轴方向对应的方向位置向量(采用psz表示)。可选的，考虑到足式机器人实际是在三维空间中运动的，因此至少两个方向位置向量还可包括：与垂直轴(y轴)方向对应的方向位置向量(采用psy表示)，对此不作限定。可理解的是：psx用于指示各条机械腿在x轴方向上的位置，psz用于指示各条机械腿在z轴方向上的位置，psy用于指示各条机械腿在y轴方向上的位置；且当足式机器人是四足机器人时，一条机械腿对应一个维度，则psx、psz以及psy的维度均为4维。
其中，腿部里程计计算与水平轴方向对应的方向位置向量的方式包括：首先，可基于当前姿态信息计算旋转矩阵(可采用R表示)，所谓的旋转矩阵是指通过改变任一向量的方向，从而实现将该任一向量映射到机器人基坐标系中的矩阵；具体可根据当前姿态信息确定足式机器人的基座姿态角，根据该基座姿态角计算出旋转矩阵。以及，还可根据各个关节的关节角信息计算出参考位置向量(可采用prel表示)，该参考位置向量用于指示：足式机器人的基座质心和各条机械腿的足端之间的相对位置。接着，可采用旋转矩阵将参考位置向量映射到机器人基坐标系中，得到目标位置向量；具体的，可采用旋转矩阵乘以参考位置向量，得到目标位置向量，采用pf表示目标位置向量，则pf＝Rprel。另外，可获取足式机器人的基座质心在世界坐标系下的三维位置向量(采用p0表示)。然后，可对目标位置向量在水平轴方向上的分量(采用pf(x)表示)，以及三维位置向量在水平轴方向上的分量(采用p0(x)表示)进行融合处理，得到水平轴方向对应的方向位置向量(采用psx表示)；此处的融合处理可包括求和处理，即psx＝p0(x)+pf(x)。
需要说明的是，腿部里程计计算与其他坐标轴(如竖直轴、垂直轴)方向对应的方向位置向量的方式，与计算与水平轴方向对应的方向位置向量的方式类似，在此不再赘述。并且，位置计算结果除了包括世界坐标系下的至少两个方向位置向量以外，还可包括机器人本体坐标系下的足端位置向量，以及机器人基坐标系下的足端速度向量等其他向量。其中，足端位置向量用于指示：足式机器人的各条机械腿的足端在机器人基坐标系下的三维位置；腿部里程计计算足端位置向量的方式可包括：对目标位置向量进行求反处理，得到足端位置向量。足端速度向量用于指示：足式机器人的各条机械腿的足端在机器人基坐标系下的三维速度；腿部里程计计算足端速度向量的方式可包括：对目标位置向量进行求导处理，并对求导结果进行求逆处理，得到足端速度向量。应理解的是，当足式机器人是四足机器人时，足端位置向量和足端速度向量的维度均为12维。
基于上述描述，当位置计算结果同时包括机器人基坐标系下的足端位置向量(采用ps表示)、机器人基坐标系下的足端速度向量(采用vs表示)、与水平轴方向对应的方向位置向量(采用psx表示)，以及与竖直轴方向对应的方向位置向量(采用psz表示)时，腿部里程计计算得到位置计算结果(y)的方式可参见第二十四方程(24)所示。
在第二十四方程(24)中，左下角标B代表机器人基坐标系下的定义，左下角标W表示世界坐标系下的定义；并且，(z)和(x)分别表示前面的向量在z轴和x轴上的分量。基于上述描述可知，本申请实施例不仅可将ps、vs以及psz添加至位置计算结果中，还可在位置计算结果中添加psx和对应的项，这样有助于提升在x轴方向上的状态估计的精准度。
应理解的是，由于位置计算结果中的每个信息(如ps、vs、psx、psz等)均包括了足式机器人的每条机械腿所对应的子向量，例如：ps包括了每条机械腿对应的子足端位置向量，任一子足端向量用于指示相应机械腿的足端在机器人基坐标系下的三维位置；vs包括了每条机械腿对应的子足端速度向量，任一子足端速度向量用于指示相应机械腿的足端在机器人基坐标系下的三维速度；psx包括了每条机械腿对应的x子方向位置向量，任一x子方向位置向量用于指示相应机械腿在x轴方向上的位置；psz包括了每条机械腿对应的z子方向位置向量，任一z子方向位置向量用于指示相应机械腿在z轴方向上的位置。那么，若从机械腿的维度对位置计算结果进行划分，则可认为位置计算结果包括各条机械腿的子位置计算结果，任一条机械腿的子位置计算结果可包括：位置计算结果中的每个信息中与任一条机械腿对应的子向量。
接着，根据历史得到的足式机器人在当前时刻的状态估计结果，观测足式机器人的各条机械腿在当前时刻的位置，得到位置观测结果。
应理解的是，部分机械腿或全部机械腿与平面接触的瞬时时刻开始至足式机器人稳定，状态估计是一个迭代的过程；即每当一个时刻到达时，计算机设备便可获取相应信息对足式机器人在该时刻的下一时刻的状态进行估计，从而得到相应的状态估计结果。基于此，足式机器人在当前时刻的状态估计结果，便是由计算机设备在当前时刻的上一时刻到达时，对足式机器人在当前时刻的状态进行估计所得到的。并且，足式机器人在当前时刻的状态估计结果可作为状态空间观测器中的状态向量；具体的，采用表示第k-1时间步的状态向量，则状态向量的定义可参见下述第二十五方程(25)所示。
在第二十五方程(25)中，p0和v0分别代表足式机器人的基座质心在世界坐标系下的三维位置向量和三维速度向量；p1，p2，p3以及p4分别表示足式机器人的四条机械腿的足端在世界坐标系下的位置向量。应理解的是，此处是以足式机器人为四足机器人为例，对状态向量进行示例性表示的，所以状态向量包含了p1，p2，p3以及p4这四个向量；若足式机器人为其它足数的机器人，则状态向量中位置向量的数量可作适应性调整。
在具体实现中，计算机设备可获取足式机器人在当前时刻的加速度信息，并将该加速度信息和足式机器人在当前时刻的状态估计结果输入至状态空间观测器中，以使得状态空间观测器根据该加速度信息和足式机器人在当前时刻的状态估计结果，观测足式机器人的各条机械腿在当前时刻的位置，得到位置观测结果。具体的，状态空间观测器可先采用内部的数学表达式(如下述第二十六方程(26)所示)，根据加速度信息和足式机器人在当前时刻(也即第k个时间步)的状态估计结果，对足式机器人在当前时刻的状态进行观测，得到状态观测结果(采用表示)；然后，采用下述第二十七方程(27)所示，根据该状态观测结果观测足式机器人的各条机械腿在当前时刻的位置，得到位置观测结果(ym)。
在上述公式第二十四方程(24)和第二十七方程(27)中，g表示重力加速度，a表示加速度信息，A、B和C这三个矩阵的形式可以如第二十八方程(28)至第三十方程(30)所示：
其中，dt表示一个控制周期的时间，I为单位矩阵，0表示零矩阵，I和0的下角标分别表示单位矩阵和零矩阵的维数。C1，C2对应于将状态观测结果(采用表示)中的对应状态(即对应维度下的向量)选择成为输出ym的选择矩阵。G1，G2用于将状态观测结果(采用表示)中的p1，p2，p3以及p4等向量在x轴方向和z轴方向上的分量选择出来以构成ym。
接着，计算设备将确定状态估计所需的参考信息。其中，参考信息可用于指示：在状态估计时，分别对位置计算结果和位置观测结果的信任程度。该参考信息可以是扩展卡尔曼滤波中的参数，也可以是卡尔曼滤波中的参数，还可以是状态估计模型中的参数等，对此不作限定；为便于阐述，后续主要以参考信息是扩展卡尔曼滤波中的参数为例进行说明。具体的，该参考信息可包括第一参数和第二参数；第一参数指示对位置计算结果的信任程度，第二参数指示对位置观测结果的信任程度。进一步的，由前述可知，位置计算结果可包括各条机械腿的子位置计算结果；那么相应的，第一参数可至少包括多个目标向量，且不同目标向量对应不同机械腿，任一目标向量用于指示对相应机械腿的子位置计算结果的信任程度。与位置计算结果类似，位置观测结果可包括各条机械腿的子位置观测结果；那么第二参数也可至少包括多个参考向量，且不同参考向量对应不同机械腿，任一参考向量用于指示对相应机械腿的子位置观测结果的信任程度。
经实践证明：对于腿部里程计所输出的位置计算结果中的ps和vs而言，在足式机器人的腿部不离开平面(如不离地)的时候，计算得到的ps和vs的准确性较高。因此，应合理选择扩展卡尔曼滤波的参数矩阵以实现：在足式机器人的腿部不离开平面时，增加对ps和vs的信任程度，即增加对位置计算结果的信任程度。
接着，根据参考信息、位置计算结果以及位置观测结果，对足式机器人在当前时刻的下一时刻的状态进行估计，得到足式机器人在下一时刻的状态估计结果。
在具体实现中，若参考信息为扩展卡尔曼滤波中的参数，则计算机设备在可根据参考信息、位置计算结果以及位置观测结果，进行扩展卡尔曼滤波处理，得到足式机器人在下一时刻的状态估计结果。其中，扩展卡尔曼滤波处理的过程可包括如下的第三十一方程(31)至第三十六方程(36)：
ye＝y-ym (31)
Pm＝APAT+Q (32)
S＝CPmCT+R (33)
Kk＝PmCTS-1 (34)
在上述第三十一方程(31)至第三十六方程(36)中，ye表示位置计算结果y和位置观测结果ym之间的差值，A和C可参见前述第二十二方程至第二十三方程的相关描述，AT和CT分别表示A的转置矩阵和C的转置矩阵。Q和R是扩展卡尔曼滤波中的参数矩阵，分别对应对不同信息源的信任程度；其中，Q代表前述提及的参考信息中的第二参数，R代表前述提及的参考信息中的第一参数，即R指示对位置计算结果y的信任程度，Q指示对位置观测结果ym的信任程度。P是一个预置的矩阵参数，Pm、S、Kk以及均表示扩展卡尔曼滤波过程中所得到的中间参数，S-1表示S的逆矩阵，表示足式机器人在下一时刻的状态估计结果。并且，在得到足式机器人在下一时刻的状态估计结果后，还可对P进行更新以便于下次进行扩展卡尔曼滤波处理；P的更新公式可参见下述第三十七方程(37)至第三十二方程(38)所示：
在上述第三十七方程(37)至第三十二方程(38)中，表示对P进行更新的过程中所得到的中间参数，表示的转置矩阵。
需要说明的是，上述只是示例性地对阐述了一种具体实施方式，并非穷举。例如，若参考信息为卡尔曼滤波中的参数，则计算机设备可根据参考信息、位置计算结果以及位置观测结果，进行卡尔曼滤波处理，得到足式机器人在下一时刻的状态估计结果。又如，若参考信息是状态估计模型中的参数，则可调用状态估计模型根据参考信息、位置计算结果以及位置观测结果，对足式机器人在当前时刻的下一时刻的状态进行估计，得到足式机器人在下一时刻的状态估计结果等等。
最后，基于足式机器人在下一时刻的状态估计结果，控制器将结合上述的所述足式机器人对应的动力学模型以及所述在各个时间步所述平面与所述足式机器人的各条机械腿之间的接触力，确定各个关节电机在各个时间步提供的电机力矩，以实现关节力矩的调整。
本公开的实施例从部分机械腿落地或全部机械腿的阶段开始执行状态估计，从而确保在落地后的状态估计可以在很短的时间内完成结果的收敛。具体的，一旦机器人检测到部分机械腿或全部机械腿与平面接触，状态估计器能在较短的时间内收敛，并依据关节角度和腿部里程计，计算得到准确的质心高度和姿态数值。真机实测结果表明收敛时间在0.004-0.01s内，即2-5个控制周期内，其结果较好的应用于落地时刻质心高度和姿态的规划器。
根据本公开的另一方面，提出了一种足式机器人900。图9示出了根据本公开实施例的足式机器人900的示例性框图。
该足式机器人900可以包括：基座部910、连接至该基座部的下肢部920，并且所述下肢部920可以包括四条下肢，其中每条下肢可以包括髋关节两个自由度和膝关节1个自由度。
所述下肢部是指该足式机器人用于实现运动的足式部件，其例如包括机械腿及将机械腿与基座相连且用于实现对该机械腿的运动控制的电机。本公开的实施例不受该下肢部的具体组成类型及其下肢数量的限制。
所述基座部是指该足式机器人的主体部分，例如可以为该足式机器人的躯干部，本公开的实施例不受该基座部的具体形状及组成的限制。
在一些实施例中，该基座部例如包括2个脊柱关节，该下肢部例如可以包括8个下肢关节，本公开的实施例不受该基座部及该下肢部所包括的具体关节个数的限制，也不受该足式机器人所具有的具体关节构型的限制。
该足式机器人还可以包括控制器930，该控制器设置在该足式机器人上，且能够执行如前所述的运动控制方法，具有如前所述的功能。
所述控制器例如包括处理装置。处理装置可以包括微处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列、状态机或用于处理从传感器线接收的电信号的其他处理器件。这种处理器件可以包括可编程电子设备，例如PLC，可编程中断控制器(PIC)、可编程逻辑器件(PLD)、可编程只读存储器(PROM)、电子可编程只读存储器等。
此外，该足式机器人还可以包括总线、存储器、传感器组件、通信模块和输入输出装置等。
总线可以是将该足式机器人的各部件互连并在各部件之中传递通信信息(例如，控制消息或数据)的电路。
传感器组件可以用于对物理世界进行感知，例如包括摄像头、红外传感器超声波传感器等。此外，传感器组件还可以包括用于测量足式机器人当前运行及运动状态的装置，例如霍尔传感器、激光位置传感器、或应变力传感器等。
通信模块例如可以通过有线或无效与网络连接，以便于与物理世界(例如，服务器)通信。通信模块可以是无线的并且可以包括无线接口，例如IEEE 802.11、蓝牙、无线局域网(WLAN)收发器、或用于接入蜂窝电话网络的无线电接口(例如，用于接入CDMA、GSM、UMTS或其他移动通信网络的收发器/天线)。在另一示例中，通信模块可以是有线的并且可以包括诸如以太网、USB或IEEE 1394之类的接口。
输入输出装置可以将例如从用户或任何其他外部设备输入的命令或数据传送到足式机器人的一个或多个其他部件，或者可以将从足式机器人的一个或多个其他部件接收的命令或数据输出到用户或其他外部设备。
多个足式机器人可以组成足式机器人系统以协同地完成一项任务，该多个足式机器人通信地连接到服务器，并且从服务器接收协同足式机器人指令。
根据本公开实施例的一个方面，提供了一种控制足式机器人的设备，所述控制足式机器人的设备包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现上述方法。
根据本公开实施例的一个方面，提供了一种计算机可读存储介质，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现上述方法。
技术中的程序部分可以被认为是以可执行的代码和/或相关数据的形式而存在的“产品”或“制品”，通过计算机可读的介质所参与或实现的。有形的、永久的储存介质可以包括任何计算机、处理器、或类似设备或相关的模块所用到的内存或存储器。例如，各种半导体存储器、磁带驱动器、磁盘驱动器或者类似任何能够为软件提供存储功能的设备。
所有软件或其中的一部分有时可能会通过网络进行通信，如互联网或其他通信网络。此类通信可以将软件从一个计算机设备或处理器加载到另一个。因此，另一种能够传递软件元素的介质也可以被用作局部设备之间的物理连接，例如光波、电波、电磁波等，通过电缆、光缆或者空气等实现传播。用来载波的物理介质如电缆、无线连接或光缆等类似设备，也可以被认为是承载软件的介质。在这里的用法除非限制了有形的“储存”介质，其他表示计算机或机器“可读介质”的术语都表示在处理器执行任何指令的过程中参与的介质。
本申请使用了特定词语来描述本申请的实施例。如“第一/第二实施例”、“一实施例”、和/或“一些实施例”意指与本申请至少一个实施例相关的某一特征、结构或特点。因此，应强调并注意的是，本说明书中在不同位置两次或多次提及的“一实施例”或“一个实施例”或“一替代性实施例”并不一定是指同一实施例。此外，本申请的一个或多个实施例中的某些特征、结构或特点可以进行适当的组合。
此外，本领域技术人员可以理解，本申请的各方面可以通过若干具有可专利性的种类或情况进行说明和描述，包括任何新的和有用的工序、机器、产品或物质的组合，或对他们的任何新的和有用的改进。相应地，本申请的各个方面可以完全由硬件执行、可以完全由软件(包括固件、常驻软件、微码等)执行、也可以由硬件和软件组合执行。以上硬件或软件均可被称为“数据块”、“模块”、“引擎”、“单元”、“组件”或“系统”。此外，本申请的各方面可能表现为位于一个或多个计算机可读介质中的计算机产品，该产品包括计算机可读程序编码。
除非另有定义，这里使用的所有术语(包括技术和科学术语)具有与本发明所属领域的普通技术人员共同理解的相同含义。还应当理解，诸如在通常字典里定义的那些术语应当被解释为具有与它们在相关技术的上下文中的含义相一致的含义，而不应用理想化或极度形式化的意义来解释，除非这里明确地这样定义。
上面是对本发明的说明，而不应被认为是对其的限制。尽管描述了本发明的若干示例性实施例，但本领域技术人员将容易地理解，在不背离本发明的新颖教学和优点的前提下可以对示例性实施例进行许多修改。因此，所有这些修改都意图包含在权利要求书所限定的本发明范围内。应当理解，上面是对本发明的说明，而不应被认为是限于所公开的特定实施例，并且对所公开的实施例以及其他实施例的修改意图包含在所附权利要求书的范围内。本发明由权利要求书及其等效物限定。

Claims (19)

1.一种控制足式机器人的方法，所述足式机器人包括基座和至少两条机械腿，每条机械腿包括至少一个关节，所述方法包括：
响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹，其中，所述第一期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第二期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹；以及
基于所述足式机器人对应的动力学模型以及所述第一期望轨迹和第二期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的所有机械腿均下落至与平面接触。
2.如权利要求1所述的方法，所述方法还包括：
响应于确定所述足式机器人的所有机械腿均下落至与平面接触，确定所述足式机器人对应的第三期望轨迹和第四期望轨迹，其中，所述第三期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，所述第四期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹；
基于所述足式机器人对应的动力学模型以及所述第三期望轨迹和第四期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的基座的高度不变且所述足式机器人的倾斜角方向的转动角为零。
3.如权利要求1所述的方法，其中，所述至少两条机械腿包括至少一条前腿和至少一条后腿，所述响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹包括：
响应于确定所述足式机器人的前腿下落至与平面接触而后腿未下落至于与平面接触，基于所述足式机器人对应的第一近似模型，来确定所述足式机器人对应的第一期望轨迹，
其中，在所述第一近似模型中，所述足式机器人被近似为单刚体，并且在所述足式机器人与平面接触的过程中，所述前腿向所述单刚体提供竖直方向上第一推力以形成所述单刚体的向上推力，所述前腿向所述单刚体提供水平方向上的第二推力以形成所述单刚体的水平方向的推力，第一推力和第二推力形成的扭矩向所述单刚体提供质心角加速度。
4.如权利要求1所述的方法，其中，所述至少两条机械腿包括至少一条前腿和至少一条后腿，所述响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹包括：
响应于确定所述足式机器人的后腿下落至与平面接触而前腿未下落至与平面接触，基于所述足式机器人对应的第二近似模型，来确定所述足式机器人对应的第一期望轨迹，
其中，在所述第二近似模型中，所述足式机器人被近似为单刚体，并且在所述足式机器人与平面接触的过程中，所述后腿向所述单刚体提供竖直方向上的第三推力以形成所述单刚体的向上推力，所述后腿向所述单刚体提供水平方向上的第四推力以形成所述单刚体的水平方向的推力，第三推力和第四推力形成的扭矩向所述单刚体提供质心角加速度。
5.如权利要求2所述的方法，其中，所述至少两条机械腿包括至少一条前腿和至少一条后腿，所述响应于确定所述足式机器人的所有机械腿均下落至与平面接触，确定所述足式机器人对应的第三期望轨迹和第四期望轨迹包括：
响应于确定所述足式机器人的所有机械腿均下落至与平面接触，基于所述足式机器人对应的第三近似模型，来确定所述足式机器人对应的第三期望轨迹，
其中，在所述第三近似模型中，所述足式机器人被近似为单刚体，并且在所述足式机器人与平面接触的过程中，
所述前腿向所述单刚体提供竖直方向上的第一推力，所述后腿向所述单刚体提供竖直方向上的第三推力，第一推力和第三推力的合力形成所述单刚体的向上推力，
所述前腿向所述单刚体提供水平方向上的第二推力，所述后腿向所述单刚体提供水平方向上的第四推力，第二推力和第四推力的合力形成所述单刚体的水平方向的推力，
第一推力、第二推力、第三推力和第四推力形成的扭矩向所述单刚体提供质心角加速度。
6.如权利要求1所述的方法，其中，所述第一期望轨迹使得以下各项中的一项或多项的组合达到极值：所述足式机器人的质心在水平方向上的位移量、所述足式机器人受到的冲击力总量、所述足式机器人的下蹲量、所述足式机器人受到的冲击力突变量、和所述足式机器人的机身转动的角度。
7.如权利要求6所述的方法，其中，所述第一期望轨迹满足以下各项约束条件：
第一约束条件，其指示所述足式机器人的部分机械腿接触平面的第一个瞬间所受到的冲击力的大小小于足式机器人能够承受的最大冲击力；
第二约束条件，其指示所述足式机器人受到的竖直方向上的支持力小于所述平面能够提供的支持力的上限，并大于所述平面能够提供的竖直方向上的支持力的下限；
第三约束条件，其指示所述足式机器人的质心的高度始终大于最低高度；
第四约束条件，其指示所述足式机器人受到的冲击力小于所述平面能够提供的水平推力的上限，并大于所述平面能够提供的水平推力的下限。
8.如权利要求1所述的方法，其中，所述响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹包括：
在部分机械腿与平面接触的瞬时时刻，确定各条机械腿远离所述基座的一端在初始时刻的足端位置坐标，将所述初始时刻的足端位置坐标作为足端初始位置；
响应于所述足式机器人的质心在所述初始时刻不具备水平方向上的速度，基于所述足端的初始位置和预设的竖直方向上的增量，确定所述各条机械腿远离所述基座的一端在中间时刻的足端位置坐标，将所述中间时刻的足端位置坐标作为足端中间位置；以及
基于所述足端初始位置和所述足端中间位置，使用三次样条差值确定所述各条机械腿远离所述基座的一端的运动轨迹作为第二期望轨迹；
其中，在所述中间时刻，所述足式机器人的所有机械腿均与平面接触且所述足式机器人的速度不为零。
9.如权利要求1所述的方法，其中，所述至少两条机械腿包括至少一条前腿和至少一条后腿，所述响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹包括：
在部分机械腿与平面接触的瞬时时刻，确定各条机械腿远离所述基座的一端在初始时刻的足端位置坐标，将所述初始时刻的足端位置坐标作为足端初始位置；
响应于所述足式机器人的质心在所述初始时刻具备水平方向上的速度，基于所述足端的初始位置、预设的前腿在水平方向上的位移增量、预设的后腿在水平方向上的位移增量、和预设的竖直方向上的增量，确定所述各条机械腿远离所述基座的一端在中间时刻的足端位置坐标，将所述中间时刻的足端位置坐标作为足端中间位置；以及
基于所述足端初始位置和所述足端中间位置，使用三次样条差值确定所述各条机械腿远离所述基座的一端的运动轨迹作为第二期望轨迹；
其中，在所述中间时刻，所述足式机器人的所有机械腿均与平面接触且所述足式机器人的速度不为零。
10.如权利要求1所述的方法，其中，所述响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹包括：
在部分机械腿与平面接触的瞬时时刻，确定各条机械腿远离所述基座的一端在初始时刻的足端位置坐标，将所述初始时刻的足端位置坐标作为足端初始位置；
响应于所述足式机器人的质心在所述初始时刻受到的冲击力大于预设阈值，基于所述足端的初始位置、预设的机械腿在水平方向的位移增量、预设的竖直方向上的增量、预设的机械腿在垂直于所述水平方向和所述竖直方向构成的平面的方向上的位移增量，确定所述各条机械腿远离所述基座的一端在中间时刻的足端位置坐标，将所述中间时刻的足端位置坐标作为足端中间位置；
基于所述足端初始位置和所述足端中间位置，使用三次样条差值确定所述各条机械腿远离所述基座的一端的运动轨迹作为第二期望轨迹，
其中，在所述中间时刻，所述足式机器人的所有机械腿均与平面接触且所述足式机器人的速度不为零。
11.如权利要求2所述的方法，其中，所述控制所述足式机器人的各个关节的动作包括：
响应于确定所述足式机器人的部分机械腿下落至与平面接触，基于所述足式机器人对应的动力学模型，确定在各个时间步所述平面与所述足式机器人的各条机械腿之间的接触力，以使得所述足式机器人的质心在重力方向上和水平方向上移动的实际轨迹以及所述足式机器人的倾斜角变化的实际轨迹均符合所述第一期望轨迹；
响应于确定所述足式机器人的所有机械腿均下落至与平面接触，基于所述足式机器人对应的动力学模型，确定在各个时间步所述平面与所述足式机器人的各条机械腿之间的接触力，以使得所述足式机器人的质心在重力方向上和水平方向上移动的实际轨迹以及所述足式机器人的倾斜角变化的实际轨迹均符合所述第三期望轨迹。
12.如权利要求11所述的方法，其中，所述基于所述足式机器人对应的动力学模型以及所述第一期望轨迹和第二期望轨迹，控制所述足式机器人的个关节的动作包括：
响应于确定所述足式机器人的部分机械腿下落至与平面接触，基于所述足式机器人对应的动力学模型以及所述在各个时间步所述平面与所述足式机器人的各条机械腿之间的接触力，确定各个关节电机在各个时间步提供的电机力矩，以使得所述各条机械腿远离所述基座的一端的轨迹符合所述第二期望轨迹；
响应于确定所述足式机器人的所有机械腿均下落至与平面接触，基于所述足式机器人对应的动力学模型以及所述在各个时间步所述平面与所述足式机器人的各条机械腿之间的接触力，确定各个关节电机在各个时间步提供的电机力矩，以使得所述各条机械腿远离所述基座的一端的轨迹符合所述第四期望轨迹。
13.如权利要求1所述的方法，其中，所述确定足式机器人的部分机械腿下落至与平面接触包括：
获取足式机器人的当前状态信息；
基于所述足式机器人的当前状态信息，确定接触信息，所述接触信息指示各条机械腿在当前时刻与平面之间的接触状态；以及
基于所述接触信息，确定所述足式机器人的部分机械腿下落至与平面接触。
14.如权利要求1所述的方法，其中，所述控制所述足式机器人各个关节的动作包括：
基于所述足式机器人对应的动力学模型以及所述在各个时间步所述平面与所述足式机器人的部分机械腿之间的接触力，确定各个关节电机在各个时间步提供的电机力矩；
基于足式机器人的当前状态信息，调整各个关节电机在各个时间步提供的电机力矩。
15.如权利要求13-14中的任一项所述的方法，其中，所述当前状态信息包括以下各项中的至少一项：
各条机械腿的关节电机力矩或电流值或电压值；
所述足式机器人的质心高度、质心姿态、各条机械腿对应的当前关节角信息；
各条机械腿对应的当前足底触觉反馈值；
所述足式机器人在重力方向上的当前加速度。
16.一种控制足式机器人的装置，所述足式机器人包括基座和至少两条机械腿，每条机械腿包括至少一个关节，所述装置包括：
规划计算设备，被配置为响应于确定所述足式机器人的部分机械腿下落至与平面接触，确定所述足式机器人对应的第一期望轨迹和第二期望轨迹，其中，
所述第一期望轨迹指示所述足式机器人的质心在重力方向和水平方向上移动的期望轨迹以及所述足式机器人的倾斜角的变化的期望轨迹，
所述第二期望轨迹指示各条机械腿远离所述基座的一端的期望轨迹；控制电机，被配置为基于所述足式机器人对应的动力学模型以及所述第一期望轨迹和第二期望轨迹，控制所述足式机器人的各个关节的动作，直至所述足式机器人的所有机械腿均下落至与平面接触。
17.一种足式机器人，包括：
基座部；
下肢部，其连接至所述基座部，所述下肢部包括四条下肢，其中每条下肢包括髋关节两个自由度和膝关节1个自由度；
控制器，所述控制器设置在所述足式机器人上，且能够执行如权利要求1-15中任一项所述的方法。
18.一种控制足式机器人的设备，包括：
处理器；和
存储器，其中，所述存储器中存储有计算机可执行代码，所述计算机可执行代码当由所述处理器运行时，执行权利要求1-15中任一所述的方法。
19.一种非易失性计算机可读存储介质，其上存储有可执行代码，所述可执行代码在被处理器执行时，使得所述处理器执行权利要求1-15中任一所述的方法。
CN202310274755.XA 2023-03-15 2023-03-15 控制足式机器人的方法和装置及足式机器人 Pending CN116945151A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310274755.XA CN116945151A (zh) 2023-03-15 2023-03-15 控制足式机器人的方法和装置及足式机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310274755.XA CN116945151A (zh) 2023-03-15 2023-03-15 控制足式机器人的方法和装置及足式机器人

Publications (1)

Publication Number Publication Date
CN116945151A true CN116945151A (zh) 2023-10-27

Family

ID=88453674

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310274755.XA Pending CN116945151A (zh) 2023-03-15 2023-03-15 控制足式机器人的方法和装置及足式机器人

Country Status (1)

Country Link
CN (1) CN116945151A (zh)

Similar Documents

Publication Publication Date Title
CN111538949B (zh) 冗余机器人逆运动学求解方法、装置和冗余机器人
CN113753150B (zh) 轮腿式机器人的控制方法、装置、设备及可读存储介质
JP7324932B2 (ja) 動的計画コントローラ
CN109311159B (zh) 一种具有增强的平衡运动和行为能力的移动机器人
US7805218B2 (en) Robot device and control method of robot device
JP2020108915A (ja) ロボット重心位置調整方法、装置、ロボット及びデータ記憶媒体
JP6781101B2 (ja) 非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム
CN111506100A (zh) 多足机器人关节控制方法、装置和多足机器人
WO2024021744A1 (zh) 一种控制足式机器人的方法、装置、电子设备、计算机可读存储介质、计算机程序产品及足式机器人
CN114161402B (zh) 机器人稳定控制方法、模型构建方法、装置和机器人
CN113515135A (zh) 多足机器人的控制方法、装置、电子设备及存储介质
US20240269846A1 (en) Method, apparatus, and device for controlling legged robot, legged robot, computer-readable storage medium, and computer program product
CN114489104B (zh) 机器人迈步控制方法及装置、机器人控制设备和存储介质
JP2004142095A (ja) ロボット装置及びロボット装置の制御方法
CN117260718B (zh) 一种四足机器人的自适应载荷补偿控制方法及系统
CN113858207A (zh) 人形机器人的倒地控制方法、装置、电子设备及存储介质
JP7221833B2 (ja) 非線形モデル予測制御装置
Hwang et al. Biped Balance Control by Reinforcement Learning.
WO2023165177A1 (zh) 构建机器人的控制器的方法、机器人的运动控制方法、装置以及机器人
Lim et al. Control design to achieve dynamic walking on a bipedal robot with compliance
CN116945151A (zh) 控制足式机器人的方法和装置及足式机器人
CN117008592A (zh) 足式机器人的运动控制方法、装置、设备及存储介质
CN114454981A (zh) 双足机器人的弹跳运动控制方法、装置及双足机器人
US20240181635A1 (en) Robot movement and interaction with massive bodies
Wang et al. Bionic Jumping of Humanoid Robot via Online Centroid Trajectory Optimization and High Dynamic Motion Controller

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
REG Reference to a national code

Ref country code: HK

Ref legal event code: DE

Ref document number: 40100501

Country of ref document: HK