四足机器人运动规划的方法、装置、系统及存储介质

技术领域

本申请涉及足式机器人技术领域，具体涉及一种四足机器人运动规划的方法、装置、系统及存储介质。

背景技术

随着人工智能的发展，足式机器人广泛应用于多个领域。现有足式机器人的运动规划方案大多基于躯干为单一刚体的足式机器人进行运动规划研究。

现有的足式机器人当将腿部视为轻质腿时，整个机器人可视为单一刚体，建立动力学模型，应用于速度较低的行走步态、对脚小跑步态。然而随着机器人运动要求的提高，由于机器人的限制，现有技术的四足机器人根本无法适应更高速的运动以及各种运动步态。

因此，需要一种四足机器人运动规划的新方案。

发明内容

有鉴于此，本说明书实施例提供一种四足机器人运动规划的方法、装置、系统及存储介质，适用于足式机器人运动规划过程。

本说明书实施例提供以下技术方案：

本说明书实施例提供一种四足机器人运动规划方法，所述四足机器人运动规划方法包括：

根据含脊椎结构四足机器人的参数，分别得到含脊椎结构四足机器人前半躯干和后半躯干对应的初始牛顿欧拉方程；

根据前半躯干和后半躯干对应的初始牛顿欧拉方程，结合机器人运动过程中受力点与足端、脊椎的作用力，获得前半躯干和后半躯干质心位置的惯量矩阵；

根据初始牛顿欧拉方程和惯量矩阵构建机器人运动状态方程；其中，运动状态方程用于在机器人运动过程中采用前一离散时间的各部分躯干状态推断后一离散时间的各部分躯干状态；

根据运动状态方程和机器人预设运动轨迹构建优化问题，获得四足机器人运动规划过程中的优化运动速度和优化运动时间。

本说明书实施例还提供一种四足机器人运动规划装置，所述四足机器人运动规划装置，包括：

输入模块，用于根据含脊椎结构四足机器人的参数，分别得到含脊椎结构四足机器人前半躯干和后半躯干对应的初始牛顿欧拉方程；

获取模块，用于根据前半躯干和后半躯干对应的初始牛顿欧拉方程，结合机器人运动过程中受力点与足端、脊椎的作用力，获得前半躯干和后半躯干质心位置的惯量矩阵；

构建模块，用于根据初始牛顿欧拉方程和惯量矩阵构建机器人运动状态方程；其中，运动状态方程用于在机器人运动过程中采用前一离散时间的各部分躯干状态推断后一离散时间的各部分躯干状态；

优化模块，用于根据运动状态方程和机器人预设运动轨迹构建优化问题，获得四足机器人运动规划过程中优化运动速度和优化运动时间。

与现有技术相比，本说明书实施例采用的上述至少一个技术方案能够达到的有益效果至少包括：

本说明书实施例通过对可控脊椎中主动/被动脊椎对应的前后两部分躯干建立动力学模型，进而得到运动过程中的运动状态方程，以此对四足机器人不同运动类型下的高速运动进行规划，本说明书实施例的动力学模型更为简洁，且突破现有技术四足机器人无法实现更高速的运动步态规划。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是本申请中主动/被动脊椎的四足机器人受力分析图；

图2是本申请中四足机器人运动规划的方法流程图；

图3是本申请中四足机器人运动规划过程的示意图。

具体实施方式

下面结合附图对本申请实施例进行详细描述。

以下通过特定的具体实例说明本申请的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本申请的其他优点与功效。显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。本申请还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本申请的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本申请，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目和方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。

还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本申请的基本构想，图式中仅显示与本申请中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践。

现有技术四足机器人基于单一刚体躯干，将腿部视为轻质腿时，整个机器人可视为单一刚体，建立动力学模型，主要应用于速度较低的行走步态、对角小跑步态。随着机器人速度等性能要求的提高，现有技术四足机器人无法实现更高速的运动。

若基于单一刚体躯干的足式机器人以较高的速度运动时，如基于对角小跑步态，需通过提高步幅、提高步频来实现。然而提高步幅即增加腿部结构的尺寸，这将导致在相等的所需足端力下，腿部关节电机需提供更大的力矩；提高步频即减小单腿的运动周期，增大往复运动频率，从而要求腿部关节电机在更高的转速下工作。因此，受限于电机的性能，对角小跑步态存在速度上界，不适用于较高的运动速度。

即使基于包含脊椎的足式机器人大多基于结构设计，由于对应的空间自由度较为复杂，采用单一刚体适用的动力学模型也无法实现更高速的运动。

基于此，本说明书实施例提出了一种四足机器人运动规划方案：结合图1包含脊椎四足机器人受力的分析，本申请基于前后两部分躯干建立动力学模型，建立机器人运动状态方程，通过采用前一离散时间的各部分躯干状态推断后一离散时间的各部分躯干状态，将机器人运动过程不断优化，从而使得包含脊椎的四足机器人在某项给定运动任务中，根据指定的初始状态、运动过程所需时间、运动步态、终点状态等优化给出机器人可行的运动规划方案。

本说明书实施例通过对可控脊椎中主动/被动脊椎对应的前后两部分躯干建立动力学模型，进而得到运动过程中的运动状态方程，以此实现含脊椎结构四足机器人的高速运动规划。本说明书实施例的动力学模型更为简洁。

以下结合附图，说明本申请各实施例提供的技术方案。

如图2所示，本说明书提供的一种四足机器人运动规划方法的流程图。如图2所示，该方法包括步骤S210-步骤S240。其中，步骤S210、根据含脊椎结构四足机器人的参数，分别得到含脊椎结构四足机器人前半躯干和后半躯干对应的初始牛顿欧拉方程。步骤S220、根据前半躯干和后半躯干对应的初始牛顿欧拉方程，结合机器人运动过程中受力点与足端、脊椎的作用力，获得前半躯干和后半躯干质心位置的惯量矩阵。步骤S230、根据初始牛顿欧拉方程和惯量矩阵构建机器人运动状态方程；其中，运动状态方程用于在机器人运动过程中采用前一离散时间的各分布躯干状态推断后一离散时间的各部分躯干状态。步骤S240、根据运动状态方程和机器人预设运动轨迹构建优化问题，获得四足机器人运动规划过程中的优化运动速度和优化运动时间。其中本说明书实施例中步骤S210和步骤S220可以同时进行也可以依次进行。

步骤S210、根据含脊椎结构四足机器人的参数，分别得到含脊椎结构四足机器人前半躯干和后半躯干对应的初始牛顿欧拉方程。

其中，含脊椎结构四足机器人的参数包括物理参数和运动参数。物理参数包括：四足机器人前半躯干与后半躯干质量、转动惯量、物理尺寸，地面摩擦系数，重力加速度常量。运动参数包括：前后躯干质心的初始位置、速度与终点位置、速度，运动总时间，时间上的离散化计数N。

具体地根据含脊椎结构四足机器人的参数，分别得到含脊椎结构四足机器人前半躯干和后半躯干对应的初始牛顿欧拉方程。

初始牛顿欧拉方程：

其中，i＝1,2，为前、后躯干的编号，j＝1,2,3,4，为机器人周身四足的编号。q

步骤S220、根据前半躯干和后半躯干对应的初始牛顿欧拉方程，结合机器人运动过程中受力点与足端、脊椎的作用力，获得前半躯干和后半躯干质心位置的惯量矩阵。

结合上述实施例，由方程(1)推得有：

对方程(2)的左边展开有：

从而方程(2)可写为：

因此，根据前半躯干和后半躯干对应的初始牛顿欧拉方程结合机器人运动过程中受力点与足端、脊椎的作用力，如r

步骤S230、根据初始牛顿欧拉方程和惯量矩阵构建机器人运动状态方程；其中，运动状态方程用于在机器人运动过程中采用前一离散时间的各分布躯干状态推断后一离散时间的各部分躯干状态。

结合上述实施例，以q

其中，

E为三阶单位阵

u＝[f

除上述的状态量x与控制量u外，B中的r

步骤S240、根据运动状态方程和机器人预设运动轨迹构建优化问题，获得四足机器人运动规划过程中的优化运动速度和优化运动时间。

结合上述实施例，结合机器人预设运动轨迹，如在机器人给定的运动轨迹上，即将前后躯干质心的初始位置、速度与终点位置、速度，运动总时间，时间上的离散化计数N进行设定。利用运动状态方程获得四足机器人运动规划过程中的优化运动速度和优化运动时间。例如，给定的运动轨迹设置了前、后躯干质心的初始位置在A点，速度为0，终点位置为D，速度也均为0，运动总时间为10s。给定时间10s内前2s加速运动至B点，后4s由B点运动匀速运动至C点，最后4s减速运动至D点。这样离散化的计数时间点分别为2s,6s,10s处。因此根据运动状态方程基于各部分躯干状态[x,u,r]

在一些实施例中，根据运动状态方程对应的初始躯干状态与预设运动轨迹对应的目标躯干状态设定目标函数，以使得运动状态方程得到的初始躯干状态和目标躯干状态趋同；其中目标函数采用初始躯干状态与目标躯干状态的误差的加权平方表示。其中躯干状态包括脊椎质心、姿态角、线速度和角速度、足端三方向上承受的力、脊椎位置与足端位置。

结合上述实施例，构建完成机器人运动状态方程，获得待优化量如状态量x与控制量u，B中的r

一些实施例为使得状态方程对应的初始躯干态与目标躯干状态趋同(即表示由状态方程对应的动力学关系得到的躯干状态与期望躯干状态无限接近)，通过初始躯干状态与目标躯干状态设定目标函数如min f(目标函数)，通过在给出0～t

其中根据初始躯干状态与目标躯干状态设定目标函数包括将初始躯干状态的自变量与期望的自变量(即目标躯干状态)轨迹之间的误差的加权平方和设定为目标函数，但具体目标函数的设定可根据实际情况添加其他内容来获得。

在一些实施例中，四足机器人运动规划方法还包括获取优化机器人运动过程中运动状态方程的约束条件。

结合上述实施例，基于运动状态方程和预设运动轨迹对机器人运动规划优化过程中运动状态方程的约束条件。其中约束条件包括等式约束条件和不等式约束条件。基于运动状态方程设置躯干刚性连接约束条件和末端时刻的状态量约束条件。

具体地，基于运动状态方程，优化过程中的等式约束包括：1、初始状态量约束，如0时刻的状态量x与0时刻的x

不等式约束包括：1、足端工作空间约束，在给定的关节角范围与四足机器人各部分构件尺寸下，足端保持在其工作空间范围内。2、足端力约束，对于足端的正向力，其始终非负，且小于足式机器人所能承受的最大地面正向力；对于足端的法向力，在给定地面摩擦系数后，其大小不超过最大静摩擦。

在一些实施例中，四足机器人的参数包括物理参数和运动参数。

结合上述实施例四足机器人运动规划过程中给定的物理参数包括：四足机器人前半躯干与后半躯干质量、转动惯量、物理尺寸，地面摩擦系数，重力加速度常量。运动参数，包括：前后躯干质心的初始位置、速度与终点位置、速度，运动总时间，时间上的离散化计数N。本说明书实施例也需基于运动规划过程中预设运动轨迹给定期望轨迹包括：自变量在N步上的期望轨迹[x

在一些实施例中，四足机器人运动规划方法包括：确定初始躯干状态对应自变量的上界、下界。

具体地，四足机器人运动规划过程中根据不同的运动类型，如跳跃步态与疾驰步态运动，可对四足机器人运动规划过程中的初始躯干状态对应自变量设置上界、下界，以便根据运动状态方程和机器人预设运动轨迹获得四足机器人运动规划过程中的优化运动速度和优化运动时间。

例如以跳跃步态的平地直线运动为例，将0～t

在一些实施例中，机器人预设运动轨迹中设置多个离散时间对应机器人的设定运动位置；四足机器人运动规划方法还包括：基于机器人运动规划获得运动规划轨迹，其中运动规划轨迹涵盖更高的运动速度和包含设定运动位置。

结合上述实施例中，本说明书实施例的含脊椎结构四足机器人运动规划过程中在预设运动轨迹中设置多个离散时间对应机器人的设定运动位置，如给定的运动轨迹设置了前、后躯干质心的初始位置在A点，速度为0，终点位置为D，速度也均为0，运动总时间为10s。给定时间10s内前2s加速运动至B点，后4s由B点运动匀速运动至C点，最后4s减速运动至D点。

结合上述实施例，基于机器人运动规划获得运动规划轨迹，即机器人在总时间内以更高的速度实现可行性运动轨迹规划。现有技术中的低速运动无法按照预设运动轨迹来完成运动任务。本说明书实施例在预设运动轨迹中设置多个离散时间对应机器人的设定运动位置，机器人需在2s内加速运动至B点，如需要机器人在2s内速度达到15m/s，对于现有技术这个速度是无法实现的，而本说明书实施例中通过不断优化，提升机器人运动速度从而使得机器人按照预设运动轨迹来完成任务。最终不仅实现机器人以更高的运动速度按照预设运动轨迹实现可行性运动轨迹规划，这样还实现机器人以更短的时间达到设定运动位置。

相比于现有技术机器人的低速运动根本无法实现上述预设运动轨迹，而本申请通过建立更简洁的动力学模型，实现含脊椎结构的四足机器人以更高的速度完成多种运动任务，如单步规划，大距离运动过程规划等。

如图3的运动规划过程，以跳跃步态的平地直线运动为例，介绍本说明书的具体实施方式。给定前后躯干的质量m

选取原点附近为运动起点，质心高度为height

基于起点与终点的状态量，通过线性插值给出位置与姿态的期望轨迹，线速度的期望轨迹取为恒定的v

以自变量[x,u,r]

如下不等式约束：1、足端工作空间约束。假定每条腿的三个关节可实现在0～π范围内运动，则足端仅需保证相对于肩(髋)部的距离不大于2l；2、足端力约束。对于足端的正向力，其始终非负，且小于足式机器人所能承受的最大地面正向力；对于足端的法向力，在给定地面摩擦系数后，其大小不超过μf

状态量x中，跳跃步态直线运动时仅需俯仰运动，故可约束脊椎另外两个自由度的运动，即质心在x轴与z轴上的姿态角上下界均取为0；对在y轴上的姿态角，为θ

选取合适的求解器，用于求解非线性优化问题。一种常用的非线性求解器叫Casadi。对上述构建的优化问题予以求解，获得满足动力学模型的最接近期望轨迹的可行最优运动轨迹。

结合上述实施例对可控的主动/被动脊椎的四足机器人运动规划，采用轻质腿假设，将含脊椎结构的足式机器人简化为前后躯干，在两个质心上建立动力学模型，相比基于某半个躯干的质心状态量与脊椎关节角度建立拉格朗日方程的方案，动力学方程中无脊椎关节角所带来的非线性部分，实现更加简洁的动力学模型构建。从而实现更加高速的运动任务以及缩短运动时间，还实现多种运动任务的优化，如单步规划，大距离运动过程规划。

本说明书实施例提供的一种四足机器人运动规划装置，所述四足机器人运动规划装置包括：

输入模块，用于根据含脊椎结构四足机器人的参数，分别得到含脊椎结构四足机器人前半躯干和后半躯干对应的初始牛顿欧拉方程；

获取模块，用于根据前半躯干和后半躯干对应的初始牛顿欧拉方程，结合机器人运动过程中受力点与足端、脊椎的作用力，获得前半躯干和后半躯干质心位置的惯量矩阵；

构建模块，用于根据初始牛顿欧拉方程和惯量矩阵构建机器人运动状态方程；其中，运动状态方程用于在机器人运动过程中采用前一离散时间的各部分躯干状态推断后一离散时间的各分部躯干状态；

优化模块，用于根据运动状态方程和机器人预设运动轨迹构建优化问题，获得四足机器人运动规划过程中的优化运动速度和优化运动时间。

本说明书实施例提供的四足机器人运动规划系统，该系统包括：处理器、存储器和计算机程序；其中

存储器，用于存储所述计算机程序，该存储器还可以是闪存(flash)。所述计算机程序例如是实现上述方法的应用程序、功能模块等。

处理器，用于执行所述存储器存储的计算机程序，以实现上述方法中设备执行的各个步骤。具体可以参见前面方法实施例中的相关描述。

可选地，存储器既可以是独立的，也可以跟处理器集成在一起。

当所述存储器是独立于处理器之外的器件时，所述设备还可以包括：

总线，用于连接所述存储器和处理器。

本发明还提供一种可读存储介质，所述可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时用于实现上述的各种实施方式提供的方法。

其中，可读存储介质可以是计算机存储介质，也可以是通信介质。通信介质包括便于从一个地方向另一个地方传送计算机程序的任何介质。计算机存储介质可以是通用或专用计算机能够存取的任何可用介质。例如，可读存储介质耦合至处理器，从而使处理器能够从该可读存储介质读取信息，且可向该可读存储介质写入信息。当然，可读存储介质也可以是处理器的组成部分。处理器和可读存储介质可以位于专用集成电路(ApplicationSpecific Integrated Circuits，简称：ASIC)中。另外，该ASIC可以位于用户设备中。当然，处理器和可读存储介质也可以作为分立组件存在于通信设备中。可读存储介质可以是只读存储器(ROM)、随机存取存储器(RAM)、CD-ROM、磁带、软盘和光数据存储设备等。

要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。

本说明书中的各个实施例之间相同相似的部分互相参见即可，每个实施例侧重说明的都是与其他实施例的不同之处。尤其，对于后面说明的产品实施例而言，由于其与方法是对应的，描述比较简单，相关之处参见系统实施例的部分说明即可。

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