CN113618740A - 双足机器人舞蹈平衡控制方法、装置和双足机器人 - Google Patents

Abstract

本申请实施例提供一种双足机器人舞蹈平衡控制方法、装置和双足机器人，该方法包括：在机器人至少存在一条支撑腿的情况下，获取机器人反馈的实际关节角度和实际足底受力，并利用实时反馈的关节角度及足底受力信息来计算ZMP偏差，进而利用该ZMP偏差进行期望动量计算，并结合将关节参考角度与实际关节角度的差值作为优化变量的关节角速度优化函数，最终计算出能够同时满足舞蹈动作跟踪及平衡运动的关节角度以进行控制。本方案可以在不需要严格设计舞蹈运动轨迹的情况下，实现舞蹈动作下的整机稳定，具有较强的外界抗干扰能力和快速响应能力。

Description

双足机器人舞蹈平衡控制方法、装置和双足机器人

技术领域

本申请涉及机器人控制技术领域，尤其涉及一种双足机器人舞蹈平衡控制方法、装置和双足机器人。

背景技术

双足机器人在做一些复杂的舞蹈动作时，由于存在机器人关节的物理限位等各种因素，因此需要通过严格设计整个舞蹈动作的轨迹，以避免出现如失衡等现象，从而保证机器人在运动过程中的稳定性。由于对轨迹设定的要求非常高，如果出现某些轨迹设计不合理，则很难保证机器人的动态稳定；并且通过不断试错，往往需要花费较多时间在轨迹设定上。

发明内容

本申请实施例提供一种双足机器人舞蹈平衡控制方法、装置和双足机器人，该方法可以在不需要严格设计舞蹈运动轨迹的情况下，实现舞蹈动作下的整机稳定，具有较强的外界抗干扰能力和快速响应能力。

本申请的实施例提供一种双足机器人舞蹈平衡控制方法，包括：

在机器人至少存在一条支撑腿的情况下，获取该机器人在对应控制周期反馈的实际关节角度和实际足底受力；

根据所述实际足底受力计算该机器人的实际零力矩点，并根据期望零力矩点和所述实际零力矩点计算零力矩点偏差；

根据所述零力矩点偏差和所述实际足底受力在竖直方向的分量计算该机器人的期望力，并根据所述期望力计算该机器人的期望动量；

根据所述期望动量、所述实际关节角度和按照舞蹈动作生成的关节参考轨迹，基于关节角速度优化函数计算优化的关节角速度，并根据所述优化的关节角速度得到优化的关节角度；

利用所述优化的关节角度控制该机器人在对应控制周期执行相应舞蹈动作。

在一种实施例中，所述根据所述期望动量、所述实际关节角度和按照舞蹈动作生成的关节参考轨迹，基于关节角速度优化函数计算优化的关节角速度，包括：

根据所述关节参考轨迹获取对应控制周期的机器人各个关节的参考关节角度，并计算所述参考关节角度与所述实际关节角度之间的差值；

利用所述期望动量、所述差值和所述关节角速度优化函数，计算得到所述优化的关节角速度。

在一种实施例中，所述关节角速度优化函数以所述关节参考轨迹对应的参考关节角度和实际关节角度之间的差值为优化变量，并基于所述机器人的惯量矩阵与关节角速度的乘积等于期望动量、以及对所述关节角速度进行积分得到所述机器人的关节角度的关系构建得到。

在一种实施例中，所述关节角速度优化函数的表达式如下：

其中，Δθ＝q_d-q_m；

其中，

表示该机器人的惯量矩阵的伪逆；H_d表示对应控制周期的期望动量；E为单位矩阵；Δθ表示所述关节参考轨迹对应的参考关节角度和所述实际关节角度之间的差值；

表示该机器人的关节角速度。

在一种实施例中，所述根据所述零力矩点偏差和所述实际足底受力在竖直方向的分量计算该机器人的期望力，包括：

根据所述零力矩点偏差在所述支撑腿所在接触平面的X方向上的偏差分量和所述实际足底受力在竖直方向的分量计算该机器人绕X轴的期望力矩分量；

根据所述零力矩点偏差在所述支撑腿所在接触平面的Y方向上的偏差分量和所述实际足底受力在竖直方向的分量计算该机器人绕Y轴的期望力矩分量；

基于绕X轴和Y轴的所述期望力矩分量得到该机器人的期望力。

在一种实施例中，所述期望动量与所述期望力满足如下关系：

H_d ^(t)＝H_d ^(t-1)+F_d·Δt；

其中，H_d ^(t)和H_d ^(t-1)分别表示t时刻和t-1时刻的期望动量；F_d表示t时刻的期望力；Δt为该机器人的控制指令周期。

在一种实施例中，所述按照舞蹈动作生成的关节参考轨迹，包括：

按照舞蹈动作确定该机器人各个关节的关节参考位置，并利用所述关节参考位置生成该机器人的关节参考轨迹；或者，

选取该机器人的若干关键节点，并按照舞蹈动作规划出每个关键节点的轨迹，根据所述每个关键节点的轨迹通过逆运动学计算所有关节的参考位置，以生成该机器人的关节参考轨迹。

在一种实施例中，所述实际足底受力通过所述支撑腿上设置的六维力传感器测量得到，在忽略所述六维力传感器与接触地面的距离时，所述根据所述实际足底受力计算该机器人的实际零力矩点，包括：

根据测量到的所述实际足底受力在绕Y轴的力矩分量和在X轴上的力分量计算所述实际零力矩点在X轴上的坐标；

根据测量到的所述实际足底受力在绕X轴的力矩分量和在Y轴上的力分量计算所述实际零力矩点在Y轴上的坐标，所述X轴和Y轴上的坐标即为该机器人的零力矩点的实际位置。

本申请的实施例还提供一种双足机器人舞蹈平衡控制装置，包括：

获取模块，用于在机器人至少存在一条支撑腿的情况下，获取该机器人在对应控制周期反馈的实际关节角度和实际足底受力；

偏差计算模块，用于根据所述实际足底受力计算该机器人的实际零力矩点，并根据期望零力矩点和所述实际零力矩点计算零力矩点偏差；

动量计算模块，用于根据所述零力矩点偏差和所述实际足底受力在竖直方向的分量计算该机器人脚踝的期望力，并根据所述期望力计算该机器人的期望动量；

关节角度优化模块，用于根据所述期望动量、所述实际关节角度和按照舞蹈动作生成的关节参考轨迹，基于关节角速度优化函数计算优化的关节角速度，并根据所述优化的关节角速度得到优化的关节角度；

控制模块，用于利用所述优化的关节角度控制该机器人在对应控制周期执行相应舞蹈动作。

本申请的实施例还提供一种双足机器人，所述双足机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施上述的双足机器人舞蹈平衡控制方法。

本申请的实施例还提供一种可读存储介质，其存储有计算机程序，所述计算机程序在处理器上执行时，实施上述的双足机器人舞蹈平衡控制方法。

本申请的实施例具有如下有益效果：

本申请实施例的双足机器人舞蹈平衡控制方法通过按照想要执行的舞蹈动作给机器人预先设计出对应的关节参考轨迹，并利用实时反馈的关节角度及足底受力信息来计算机器人的ZMP偏差，进而利用该ZMP偏差计算期望动量，最后结合将关节参考角度与实际关节角度的差值作为优化变量的关节角速度优化函数，最终计算出能够同时满足舞蹈动作跟踪及平衡运动的关节角度以进行控制。该方法可以不需要设计非常精确的运动轨迹，就可以实现在舞蹈动作跟踪的情况下保证机器人的平衡稳定，具有较强的抗外界干扰能力等。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了一种双足机器人舞蹈的应用示意图；

图2示出了本申请实施例的双足机器人舞蹈平衡控制方法的流程示意图；

图3示出了本申请实施例的双足机器人舞蹈平衡控制方法的实际零力矩点计算的流程示意图；

图4示出了本申请实施例的双足机器人舞蹈平衡控制方法的期望力计算的流程示意图；

图5示出了本申请实施例的双足机器人舞蹈平衡控制装置的结构示意图。

具体实施方式

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

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

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

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

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

实施例1

请参照图1和2，本实施例提出一种双足机器人舞蹈平衡控制方法，可应用于如图1所示的机器人跳舞场景。该方法基于ZMP和动量进行平衡控制，可以实现双足机器人在舞蹈动作跟踪下的整机稳定。

示范性地，如图2所示，该双足机器人舞蹈平衡控制方法包括：

步骤S110，在机器人至少存在一条支撑腿的情况下，获取该机器人在对应控制周期反馈的实际关节角度和实际足底受力。

本实施例中，在双足机器人全身执行舞蹈动作时，将始终保证至少有一条腿处于支撑状态(称支撑腿)，该支撑腿将处于原始位置，即不会发生位置移动。可以理解，某些时刻，该机器人的支撑腿可以是左腿或右腿，有时还可以是两腿一起站在各自的原始位置，例如，当机器人只进行腰部以上的动作的时刻等。

其中，对于机器人各个关节的角度q(又称关节角度)，例如，可通过对应支撑腿上设置的角位移传感器或位置编码器等采集得到，但不限于此。

足底受力同样可通过机器人身上设置的相应力或力矩传感器采集得到。例如，在一种实施方式中，可在双足机器人每条腿的足底各安装一个六维力传感器，可选取传感器坐标与足底受力运动的坐标系相同，于是该六维力传感器将测量得到该支撑腿的足底受力情况。当然，也可以采用其他的力传感器进行测量并反馈得到，这里的六维力传感器仅仅是一种示例而已。

例如，该六维力传感的输出记为F_r，其中，F_r为六维向量，表示如下：

F_r＝[f_x f_y f_z τ_x τ_y τ_z]^T。

其中，f_x、f_y和f_z分别表示该支撑腿在X轴、Y轴和Z轴三个方向上受到的力分量；τ_x、τ_y和τ_z分别表示该支撑腿绕X轴、Y轴和Z轴受到的力矩分量。

对于上述步骤S110，示范性地，在机器人进行执行跳舞动作的过程中，可通过相应位置的传感器等来实时测量并反馈至控制器，以便获取到上述的机器人的实际关节角度以及足底受力的情况，进而利用这些实际的测量结果进行后续控制。

步骤S120，根据该实际足底受力计算该机器人的实际零力矩点，并根据期望零力矩点和该实际零力矩点计算零力矩点偏差。

其中，上述的零力矩点又为ZMP(Zero Moment Point)，是指地面上的一个点，支撑腿足底受到的地面反作用力绕该点在地面上的力矩分量为零。ZMP可用于判定该双足机器人是否动态稳定运动，若ZMP落在脚掌的范围内，则机器人可以稳定进行站立、行走等。

为了保证机器人在进行舞蹈动作跟踪时，能够保证整体稳定，本实施例将计算出机器人的期望ZMP与实时的实际ZMP之间的偏差，进而根据该偏差来动态调整自身的关节角度，从而实现舞蹈动作下的整机稳定。

在一种实施方式中，如图3所示，上述实际零力矩点的计算，包括：

子步骤S210，根据测量到的实际足底受力在绕Y轴的力矩分量和在X轴上的力分量计算实际零力矩点在X轴上的坐标。

子步骤S220，根据测量到的实际足底受力在绕X轴的力矩分量和在Y轴上的力分量计算实际零力矩点在Y轴上的坐标。

示范性地，对于实际ZMP在X轴上的坐标p_x，可采用如下公式计算：

其中，τ_y为测量得到的实际足底受力F在绕Y轴的力矩分量；f_x为测量得到的实际足底受力F在X轴上的受力分量。

同理，对于实际ZMP在Y轴上的坐标p_y，则有：

其中，τ_x为测量得到的实际足底受力F在绕X轴的力矩分量；f_y为测量得到的实际足底受力F在Y轴上的受力分量。

于是，可得到ZMP的实际位置为{p_x，p_y}。同时，由于机器人始终存在至少一条支撑腿与地面接触且处于其原始位置，在一种实施例中，可设置该机器人的期望ZMP的位置为{0，0}。

对于上述步骤S120，示范性地，在计算出ZMP的实际位置后，可将设定的期望位置与其进行作差处理，从而得到ZMP存在的偏差。

步骤S130，根据该零力矩点偏差和该实际足底受力在竖直方向的分量计算该机器人的期望力，并根据该期望力计算该机器人的期望动量。

为了获取期望力，这里假设机器人的支撑腿上的六维力传感器与地面之间的距离为零，即忽略力传感器与接触平面之间的距离，以便利用ZMP计算方式来获取到机器人脚踝的期望力矩。

在一种实施方式中，如图4所示，机器人的期望力的计算，包括：

子步骤S310，根据零力矩点偏差在支撑腿所在接触平面的Y方向上的偏差分量和该实际足底受力在竖直方向的分量计算该机器人绕X轴的期望力矩分量。

示范性地，对于机器人脚踝绕X轴的期望力矩分量τ_xd，可按照如下公式计算：

其中，Δp_y表示在Y方向上的ZMP偏差，f_z表示实际足底受力在竖直方向的分量，可直接通过上述的六维力传感器测量得到。

子步骤S320，根据零力矩点偏差在支撑腿所在接触平面的X方向上的偏差分量和该实际足底受力在竖直方向的分量计算该机器人绕Y轴的期望力矩分量。

示范性地，对于绕Y轴的期望力矩分量τ_yd，同理有：

其中，Δp_x表示在X方向上的ZMP偏差。

而对于绕Z轴的期望力矩分量，由于机器人的支撑腿始终与地面接触，故绕Z轴的期望力矩分量为零，即不产生作用力矩。

子步骤S330，基于绕X轴和Y轴的期望力矩分量得到该机器人的期望力。

本实施例中，计算该期望力时，在仅考虑角动量的控制的情况下，此时可得到该机器人支撑腿脚踝的期望力Fd为：

F_d＝[0 0 0 τ_xd τ_yd 0]^T。

进而，利用计算得到的期望力F_d，可计算出机器人为满足期望ZMP时所需的期望动量。示范性地，可通过对期望的作用力进行积分得到动量。

在一种实施方式中，该期望动量与期望力之间满足如下表达式：

H_d ^(t)＝H_d ^(t-1)+F_d·Δt；

其中，H_d ^(t)和H_d ^(t-1)分别表示t时刻和t-1时刻的期望动量；F_d表示t时刻的期望力；Δt为该机器人的控制指令周期。

步骤S140，根据该期望动量、实际关节角度和按照舞蹈动作生成的关节参考轨迹，基于关节角速度优化函数计算优化的关节角速度，并根据该优化的关节角速度得到优化的关节角度。

其中，上述的优化的关节角速度是指，考虑到机器人在执行舞蹈动作且保持平衡所需的期望的关节角速度。而通过对该关节角速度积分，即可计算得到所需的期望的关节角度。

对于上述的关节参考轨迹，可根据机器人需要执行的具体舞蹈动作来生成，生成的方式并不作限定。值得注意的是，本实施例并不需要严格设计舞蹈动作轨迹，由于机器人在运动过程中会通过基于动量控制的控制器进行位置动态调节，因此可以设定不需要非常精确的运动轨迹作为平衡控制器的参考即可，可以大大降低对轨迹设定的要求。

例如，在一种实施方式中，可按照每个舞蹈动作规划出该机器人所有关节的各个关节参考位置，然后根据各个关节的所有关节参考位置来生成该机器人对应关节的参考轨迹。

在另一实施方式中，也可选取该机器人身上的若干个关键节点，并按照想要执行的舞蹈动作规划出每个关键节点的运动轨迹，进而，根据每个关键节点的运动轨迹通过机器人的逆运动学计算所有关节的参考位置，以生成该机器人的关节参考轨迹。

例如，对于每个关键节点P，其位姿可记为一个六维列向量，P＝[x y z rx ry rz]^T，其中，(x y z)和(rx ry rz)分别表示在世界坐标系中的位置和姿态。假设有5个关键节点，可通过逆运动学函数IK()求解得到所有关节的角度q_d，即有：

q_d＝IK(P₁，P₂，P₃，P₄，P₅，)。

可以理解，上述的关键节点是指机器人能够执行舞蹈动作的重点部位，例如，若该双足机器人为具有双手和双脚的仿人机器人，如图1所示，则该关键节点可选取5个，分别是两只手掌、两只脚板和躯干。当然，如果该双足机器人不具有双手，则该关键节点可包括3个，即两只脚板和躯干。

本实施例中，该关节角速度优化函数反映了机器人各个关节的关节角度、期望动量和关节角速度之间的关系，可通过预先构建得到。示范性地，依据机器人学原理可知，惯量矩阵与关节角度、关节角速度有关，而动量还可以通过惯量矩阵计算得到，即：

其中，M表示惯量矩阵，

表示机器人所有关节的关节角速度。

在一种实施方式中，该关节角速度优化函数以关节参考轨迹对应的参考关节角度和实际关节角度之间的差值为优化变量，其中，Δθ＝q_d-q_m，Δθ表示参考关节角度q_d和测量的实际关节角度q_m之间的差值；由于机器人的关节角速度与关节角度之间的积分关系，结合上述的惯量矩阵、关节角速度与期望动量之间的计算关系，可以构建得到用于求解关节角速度的优化函数。

考虑到矩阵可能存在不可逆的情况，为保证在任意时刻都可以得到一个解，避免出现方程无解的情况，在一种可选的实施方式中，可通过采用惯量矩阵的伪逆来进行计算。示范性地，该关节角速度优化函数的表达式可为：

其中，Δθ＝q_d-q_m；

其中，

表示该机器人的惯量矩阵的伪逆；H_d表示对应控制周期的期望动量；E为单位矩阵；Δθ表示参考关节角度和测量的实际关节角度之间的差值；

表示该机器人的关节角速度。

此外，对于矩阵不可逆的情况，例如，还可以通过增加相应的松弛变量w等方式来构建出相应的关节角速度优化函数。而对于惯量矩阵可逆的情况下，则可利用上述的惯量矩阵与期望动量满足的关系、参考关节角度和实际关节角度之间的差值直接构建出使实际关节角度尽量趋近于参考关节角度的优化函数，比如，

其中，Δθ＝q_d-q_m。当然，在实际使用中，可以根据实际需求对该优化函数作适应性调整。

可以理解，上述构建的关节角速度优化函数通过将参考关节角度与测量到的实际关节角度之间的差值作为优化变量，即使得机器人的实际关节角度尽量趋近于参考关节角度，还考虑了机器人为保持平衡所需的期望动量，这样可以使得最终计算得到的关节角度可同时满足机器人平衡且舞蹈动作跟踪的需求。

对于上述步骤S140，示范性地，可根据生成的关节参考轨迹来从中获取到机器人各个关节在对应控制周期的参考关节角度，并计算该参考关节角度与测量到的实际关节角度之间的差值；进而，将计算到的期望动量和该差值代入预先构建的关节角速度优化函数中，即可计算得到优化的关节角速度。由于关节角度与关节角速度存在积分关系，在计算到优化的关节角速度后，对其进行积分操作，即可得到优化的关节角度。

步骤S150，利用该优化的关节角度控制该机器人在对应控制周期执行相应舞蹈动作。

示范性地，可将该优化的关节角度作为下一控制周期的指令，并发送到机器人对应关节的电机，从而驱动该机器人执行相应的舞蹈动作。

本实施例的双足机器人舞蹈平衡控制方法通过先按照想要执行的舞蹈动作给机器人设计一个关节参考轨迹，并利用实时反馈的关节角度及足底受力信息来计算机器人的ZMP偏差，进而利用该ZMP偏差进行期望动量计算，最后结合将关节参考角度与实际关节角度的差值作为优化变量的关节角速度优化函数，最终计算出能够同时满足舞蹈动作跟踪及平衡运动的关节角度以进行控制。该方法可以不需要设计非常精确的运动轨迹，就可以实现在舞蹈动作跟踪的情况下保证机器人的平衡稳定，具有较强的抗外界干扰能力等。

实施例2

请参照图5，基于上述实施例1的方法，本实施例提出一种双足机器人舞蹈平衡控制装置100，示范性地，该双足机器人舞蹈平衡控制装置100包括：

获取模块110，用于在机器人至少存在一条支撑腿的情况下，获取该机器人在对应控制周期反馈的实际关节角度和实际足底受力。

偏差计算模块120，用于根据实际足底受力计算该机器人的实际零力矩点，并根据期望零力矩点和实际零力矩点计算零力矩点偏差。

动量计算模块130，用于根据零力矩点偏差和实际足底受力在竖直方向的分量计算该机器人脚踝的期望力，并根据期望力计算该机器人的期望动量。

关节角度优化模块140，用于根据期望动量、实际关节角度和按照舞蹈动作生成的关节参考轨迹，基于关节角速度优化函数计算优化的关节角速度，并根据优化的关节角速度得到优化的关节角度。

控制模块150，用于利用优化的关节角度控制该机器人在对应控制周期执行相应舞蹈动作。

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

本申请还提供了一种双足机器人，示范性地，该双足机器人包括处理器和存储器，其中，存储器存储有计算机程序，处理器通过运行计算机程序，从而使双足机器人执行上述的双足机器人舞蹈平衡控制方法或者上述双足机器人舞蹈平衡控制装置中的各个模块的功能。

本申请还提供了一种可读存储介质，用于储存上述双足机器人中使用的计算机程序。

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

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

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

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

Claims (11)

1.一种双足机器人舞蹈平衡控制方法，其特征在于，包括：

在机器人至少存在一条支撑腿的情况下，获取该机器人在对应控制周期反馈的实际关节角度和实际足底受力；

根据所述实际足底受力计算该机器人的实际零力矩点，并根据期望零力矩点和所述实际零力矩点计算零力矩点偏差；

根据所述零力矩点偏差和所述实际足底受力在竖直方向的分量计算该机器人的期望力，并根据所述期望力计算该机器人的期望动量；

根据所述期望动量、所述实际关节角度和按照舞蹈动作生成的关节参考轨迹，基于关节角速度优化函数计算优化的关节角速度，并根据所述优化的关节角速度得到优化的关节角度；

利用所述优化的关节角度控制该机器人在对应控制周期执行相应舞蹈动作。

2.根据权利要求1所述的双足机器人舞蹈平衡控制方法，其特征在于，所述根据所述期望动量、所述实际关节角度和按照舞蹈动作生成的关节参考轨迹，基于关节角速度优化函数计算优化的关节角速度，包括：

根据所述关节参考轨迹获取对应控制周期的机器人各个关节的参考关节角度，并计算所述参考关节角度与所述实际关节角度之间的差值；

利用所述期望动量、所述差值和所述关节角速度优化函数，计算得到所述优化的关节角速度。

3.根据权利要求1或2所述的双足机器人舞蹈平衡控制方法，其特征在于，所述关节角速度优化函数以所述关节参考轨迹对应的参考关节角度和实际关节角度之间的差值为优化变量，并基于所述机器人的惯量矩阵与关节角速度的乘积等于期望动量、以及对所述关节角速度进行积分得到所述机器人的关节角度的关系构建得到。

4.根据权利要求3所述的双足机器人舞蹈平衡控制方法，其特征在于，所述关节角速度优化函数的表达式如下：

其中，Δθ＝q_d-q_m；

其中，

表示该机器人的惯量矩阵的伪逆；H_d表示对应控制周期的期望动量；E为单位矩阵；Δθ表示所述关节参考轨迹对应的参考关节角度和所述实际关节角度之间的差值；

表示该机器人的关节角速度。

5.根据权利要求1所述的双足机器人舞蹈平衡控制方法，其特征在于，所述根据所述零力矩点偏差和所述实际足底受力在竖直方向的分量计算该机器人的期望力，包括：

根据所述零力矩点偏差在所述支撑腿所在接触平面的X方向上的偏差分量和所述实际足底受力在竖直方向的分量计算该机器人绕X轴的期望力矩分量；

根据所述零力矩点偏差在所述支撑腿所在接触平面的Y方向上的偏差分量和所述实际足底受力在竖直方向的分量计算该机器人绕Y轴的期望力矩分量；

基于绕X轴和Y轴的所述期望力矩分量得到该机器人的期望力。

6.根据权利要求1或5所述的双足机器人舞蹈平衡控制方法，其特征在于，所述期望动量与所述期望力满足如下关系：

H_d ^(t)＝H_d ^(t-1)+F_d·Δt；

其中，H_d ^(t)和H_d ^(t-1)分别表示t时刻和t-1时刻的期望动量；F_d表示t时刻的期望力；Δt为该机器人的控制指令周期。

7.根据权利要求1或2所述的双足机器人舞蹈平衡控制方法，其特征在于，所述按照舞蹈动作生成的关节参考轨迹，包括：

按照舞蹈动作确定该机器人各个关节的关节参考位置，并利用所述关节参考位置生成该机器人的关节参考轨迹；或者，

选取该机器人的若干关键节点，并按照舞蹈动作规划出每个关键节点的轨迹，根据所述每个关键节点的轨迹通过逆运动学计算所有关节的参考位置，以生成该机器人的关节参考轨迹。

8.根据权利要求1或2所述的双足机器人舞蹈平衡控制方法，其特征在于，所述实际足底受力通过所述支撑腿上设置的六维力传感器测量得到，在忽略所述六维力传感器与接触地面的距离时，所述根据所述实际足底受力计算该机器人的实际零力矩点，包括：

根据测量到的所述实际足底受力在绕Y轴的力矩分量和在X轴上的力分量计算所述实际零力矩点在X轴上的坐标；

根据测量到的所述实际足底受力在绕X轴的力矩分量和在Y轴上的力分量计算所述实际零力矩点在Y轴上的坐标，所述X轴和Y轴上的坐标即为该机器人的零力矩点的实际位置。

9.一种双足机器人舞蹈平衡控制装置，其特征在于，包括：

获取模块，用于在机器人至少存在一条支撑腿的情况下，获取该机器人在对应控制周期反馈的实际关节角度和实际足底受力；

偏差计算模块，用于根据所述实际足底受力计算该机器人的实际零力矩点，并根据期望零力矩点和所述实际零力矩点计算零力矩点偏差；

动量计算模块，用于根据所述零力矩点偏差和所述实际足底受力在竖直方向的分量计算该机器人脚踝的期望力，并根据所述期望力计算该机器人的期望动量；

关节角度优化模块，用于根据所述期望动量、所述实际关节角度和按照舞蹈动作生成的关节参考轨迹，基于关节角速度优化函数计算优化的关节角速度，并根据所述优化的关节角速度得到优化的关节角度；

控制模块，用于利用所述优化的关节角度控制该机器人在对应控制周期执行相应舞蹈动作。

10.一种双足机器人，其特征在于，所述双足机器人包括处理器和存储器，所述存储器存储有计算机程序，所述处理器用于执行所述计算机程序以实施权利要求1-8中任一项所述的双足机器人舞蹈平衡控制方法。

11.一种可读存储介质，其特征在于，其存储有计算机程序，所述计算机程序在处理器上执行时，实施根据权利要求1-8中任一项所述的双足机器人舞蹈平衡控制方法。

Images