CN109093626B - 四足机器人的机身姿态控制方法及装置 - Google Patents

Abstract

本发明提供了一种四足机器人的机身姿态控制方法及装置，该方法包括：实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；基于各足端的位置坐标计算待控制四足机器人的足端支撑平面相对于机身平面的角度；基于角度，机身姿态角和机身姿态角速度计算待控制四足机器人的机身姿态角控制量；基于机身姿态角控制量计算待控制四足机器人的各关节目标位置，以根据各关节目标位置控制待控制四足机器人的各关节进行运动。在本发明中，能够实现待控制四足机器人的实时机身姿态控制，增强了待控制四足机器人的运动稳定性和地形适应的能力，缓解了现有的姿态调整方式实时性差，严重影响四足机器人运动稳定性的技术问题。

Description

四足机器人的机身姿态控制方法及装置

技术领域

本发明涉及机器人的技术领域，尤其是涉及一种四足机器人的机身姿态控制方法及装置。

背景技术

理论上四足机器人能够适应人类可到达的所有地形环境且不会对地形造成明显损害，具有巨大的研究价值和市场前景。四足机器人的运动需要机身运动和四足端运动间的时间与空间的协调配合，且需要获知自身状态信息的全反馈及对周围环境和地形信息的全感知，是一个非常复杂且高要求的运动轨迹规划和实时控制过程。保持机身稳定是四足机器人运动的前提，这就需要对机器人的机身姿态进行控制。

四足机器人主要采用walk步态和trot步态进行运动，walk步态主要用于低速静步态，trot步态主要用于中高速动步态。可采用基于位置规划和控制方法或基于力矩规划和控制方法来实现此类步态。目前基于位置规划和控制的四足步态方法中，多依赖于静稳定假设，在步态规划中，当姿态角偏差较大时才加入姿态调整规划。但是，在实际的运动过程中，若基础支撑平台角度不断变化或四足机器人以爬行步态运行于复杂崎岖地形时，上述依赖于大姿态角偏差的姿态调整很难保证四足机器人的实时稳定性，使得四足机器人不断晃动甚至倾倒，导致无法正常运动。

综上所述，现有的依赖于大姿态角偏差的姿态调整方式实时性差，严重影响了四足机器人的运动稳定性。

发明内容

有鉴于此，本发明的目的在于提供一种四足机器人的机身姿态控制方法及装置，以缓解现有的依赖于大姿态角偏差的姿态调整方式实时性差，严重影响四足机器人运动稳定性的技术问题。

第一方面，本发明实施例提供了一种四足机器人的机身姿态控制方法，包括：

实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；

基于所述各足端的位置坐标计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度；

基于所述角度，所述机身姿态角和所述机身姿态角速度计算所述待控制四足机器人的机身姿态角控制量；

基于所述机身姿态角控制量计算所述待控制四足机器人的各关节目标位置，以根据所述各关节目标位置控制所述待控制四足机器人的各关节进行运动。

结合第一方面，本发明实施例提供了第一方面的第一种可能的实施方式，其中，实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标包括：

通过设置于所述待控制四足机器人上的检测设备获取所述机身姿态角，其中，所述检测设备包括以下任一种：惯性测量单元IMU，陀螺仪；

当所述检测设备为所述IMU时，通过所述IMU获取所述机身姿态角速度；

当所述检测设备为所述陀螺仪时，对所述待控制四足机器人的机身姿态角进行差分计算，得到所述机身姿态角速度；

通过正运动学对所述待控制四足机器人的各关节角进行计算，得到各足端在机身坐标系中的位置坐标，并将所述各足端在机身坐标系中的位置坐标作为所述各足端的位置坐标。

结合第一方面，本发明实施例提供了第一方面的第二种可能的实施方式，其中，基于所述各足端的位置坐标计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度包括：

根据第一算式

和第二算式

计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度，

其中，φ_fb表示所述足端支撑平面相对于机身平面的横滚角度，θ_fb表示所述足端支撑平面相对于机身平面的俯仰角度，i可取f或h，j可取l或r，f表示前腿，h表示后腿，l表示左侧腿，r表示右侧腿，p_liz，p_riz，p_jfz，p_jhz分别表示对应的足端的位置坐标中Z轴方向坐标值，p_liy，p_riy分别表示对应的足端的位置坐标中Y轴方向坐标值，p_jfx，p_jhx分别表示对应的足端的位置坐标中X轴方向坐标值。

结合第一方面，本发明实施例提供了第一方面的第三种可能的实施方式，其中，基于所述角度，所述机身姿态角和所述机身姿态角速度计算所述待控制四足机器人的机身姿态角控制量包括：

根据机身姿态角控制量计算算式

计算所述待控制四足机器人的机身姿态角控制量，

其中，φ_cmd，θ_cmd，ψ_cmd表示所述机身姿态角控制量，φ，θ，ψ表示所述机身姿态角，

表示所述机身姿态角速度，φ_d，θ_d，ψ_d表示理想机身姿态角，

表示理想机身姿态角速度，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，k_φ，k_θ，k_ψ和

表示控制参数量。

结合第一方面，本发明实施例提供了第一方面的第四种可能的实施方式，其中，基于所述机身姿态角控制量计算所述待控制四足机器人的各关节目标位置包括：

基于所述机身姿态角控制量计算旋转矩阵；

基于所述旋转矩阵计算足端控制位置值；

根据逆运动学对所述足端控制位置值进行计算，得到各关节目标位置，以根据所述各关节目标位置控制所述待控制四足机器人的各关节进行运动。

结合第一方面，本发明实施例提供了第一方面的第五种可能的实施方式，其中，基于所述旋转矩阵计算足端控制位置值包括：

根据足端控制位置计算算式

计算所述待控制四足机器人的足端控制位置值，其中，

表示所述待控制四足机器人的足端控制位置值，

表示足端规划位置值，R表示所述旋转矩阵。

第二方面，本发明实施例还提供了一种四足机器人的机身姿态控制装置，包括：

获取模块，用于实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；

第一计算模块，用于基于所述各足端的位置坐标计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度；

第二计算模块，用于基于所述角度，所述机身姿态角和所述机身姿态角速度计算所述待控制四足机器人的机身姿态角控制量；

第三计算模块，用于基于所述机身姿态角控制量计算所述待控制四足机器人的各关节目标位置，以根据所述各关节目标位置控制所述待控制四足机器人的各关节进行运动。

结合第二方面，本发明实施例提供了第二方面的第一种可能的实施方式，其中，所述获取模块包括：

第一获取单元，用于通过设置于所述待控制四足机器人上的检测设备获取所述机身姿态角，其中，所述检测设备包括以下任一种：惯性测量单元IMU，陀螺仪；

第二获取单元，用于当所述检测设备为所述IMU时，通过所述IMU获取所述机身姿态角速度；

差分计算单元，用于当所述检测设备为所述陀螺仪时，对所述待控制四足机器人的机身姿态角进行差分计算，得到所述机身姿态角速度；

第一计算单元，用于通过正运动学对所述待控制四足机器人的各关节角进行计算，得到各足端在机身坐标系中的位置坐标，并将所述各足端在机身坐标系中的位置坐标作为所述各足端的位置坐标。

结合第二方面，本发明实施例提供了第二方面的第二种可能的实施方式，其中，所述第一计算模块包括：

第二计算单元，用于根据第一算式

和第二算式

计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度，

其中，φ_fb表示所述足端支撑平面相对于机身平面的横滚角度，θ_fb表示所述足端支撑平面相对于机身平面的俯仰角度，i可取f或h，j可取l或r，f表示前腿，h表示后腿，l表示左侧腿，r表示右侧腿，p_liz，p_riz，p_jfz，p_jhz分别表示对应的足端的位置坐标中Z轴方向坐标值，p_liy，p_riy分别表示对应的足端的位置坐标中Y轴方向坐标值，p_jfx，p_jhx分别表示对应的足端的位置坐标中X轴方向坐标值。

结合第二方面，本发明实施例提供了第二方面的第三种可能的实施方式，其中，所述第二计算模块包括：

第三计算单元，用于根据机身姿态角控制量计算算式

计算所述待控制四足机器人的机身姿态角控制量，

其中，φ_cmd，θ_cmd，ψ_cmd表示所述机身姿态角控制量，φ，θ，ψ表示所述机身姿态角，

表示所述机身姿态角速度，φ_d，θ_d，ψ_d表示理想机身姿态角，

表示理想机身姿态角速度，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，k_φ，k_θ，k_ψ和

表示控制参数量。

本发明实施例带来了以下有益效果：

在本实施例中，实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；然后基于各足端的位置坐标计算待控制四足机器人的足端支撑平面相对于机身平面的角度；进而基于角度，机身姿态角和机身姿态角速度计算待控制四足机器人的机身姿态角控制量；最终基于机身姿态角控制量计算待控制四足机器人的各关节目标位置，以根据各关节目标位置控制待控制四足机器人的各关节进行运动。通过上述描述可知，在本实施例中，在待控制四足机器人的运动过程中，能够实现待控制四足机器人的实时机身姿态控制，增强了待控制四足机器人的运动稳定性和地形适应的能力，缓解了现有的依赖于大姿态角偏差的姿态调整方式实时性差，严重影响四足机器人运动稳定性的技术问题。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点在说明书、权利要求书以及附图中所特别指出的结构来实现和获得。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

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

图1为本发明实施例提供的基于位置的四足步态规划与控制的流程图；

图2为本发明实施例提供的一种四足机器人的机身姿态控制方法的流程图；

图3为本发明实施例提供的四足机器人的坐标系统的示意图；

图4为本发明实施例提供的姿态角度关系的示意图；

图5为本发明实施例提供的基于机身姿态角控制量计算待控制四足机器人的各关节目标位置的流程图；

图6为本发明实施例提供的加入了机身姿态控制的基于位置的四足步态规划与控制的流程图；

图7为本发明实施例提供的一种四足机器人的机身姿态控制装置的示意图。

图标：

10-获取模块；20-第一计算模块；30-第二计算模块；40-第三计算模块。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

目前，基于位置规划和控制的四足步态方法中，都是在步态规划中，事先规划出姿态偏差较大时才进行姿态调整。如图1所示，具体过程为，根据各腿的相序关系、步频和速度要求及运动关系，规划机身质心运动轨迹和足端摆动运动轨迹；然后由足端和机身之间的相互关系，转换得到各足端的运动轨迹(这些过程都是在步态规划层进行的，规划得到足端的运动轨迹)；再由逆运动学或虚拟模型计算得到各关节的运动轨迹；然后由关节位置或力控制实现运动，从而完成运动行走。该方式在四足机器人实际运动过程中难以保证机身的姿态(因为不是动态实时的过程，可能姿态偏差已经很大，但还没检测到，或者是检测到了并未及时规划执行)，且姿态调整过程与原位置规划过程的耦合非常复杂。

基于此，本发明实施例提供的一种四足机器人的机身姿态控制方法，能够实现待控制四足机器人的实时机身姿态控制，增强了待控制四足机器人的运动稳定性和地形适应的能力。

为便于对本实施例进行理解，首先对本发明实施例所公开的一种四足机器人的机身姿态控制方法进行详细介绍。

实施例一：

根据本发明实施例，提供了一种四足机器人的机身姿态控制方法的实施例，需要说明的是在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

图2是根据本发明实施例的一种四足机器人的机身姿态控制方法，如图2所示，该方法包括如下步骤：

步骤S202，实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；

在本发明实施例中，该四足机器人的机身姿态控制方法可应用于控制单元。如图3所示，设固定坐标系为{O_w}(实际为地球坐标系)，待控制四足机器人的机身坐标系为{O_b}，其中，机身坐标系{O_b}的原点固联在机身质心处，其x轴指向待控制四足机器人前向方向，z轴沿重力方向的反方向(机器人处于水平面时)，y轴满足右手定则，而{O_w}的各轴方向与待控制四足机器人初始时刻时{O_b}的各轴同向。图3中，每一个圆柱表示待控制四足机器人的关节，带有斜线的横线表示地面。

另外，机身姿态角包括：横滚角(绕x_w轴旋转，在本发明实施例中，用字母φ表示)，俯仰角(绕y_w轴旋转，在本发明实施例中，用字母θ表示)，和偏航角(绕z_w轴旋转，在本发明实施例中，用字母ψ表示)，其中，横滚角和俯仰角的调节控制与足端支撑平面相对于机身平面的角度密切相关，所以下文中，在计算待控制四足机器人的足端支撑平面相对于机身平面的角度时，就用横滚角和俯仰角的大小来表示的。

下文中再对获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标的过程进行具体介绍。

步骤S204，基于各足端的位置坐标计算待控制四足机器人的足端支撑平面相对于机身平面的角度；

具体的，待控制四足机器人的足端支撑平面相对于机身平面的角度用于后续进行机身姿态的控制。

步骤S206，基于角度，机身姿态角和机身姿态角速度计算待控制四足机器人的机身姿态角控制量；

该机身姿态角控制量用于使得待控制四足机器人处于理想的姿态。

步骤S208，基于机身姿态角控制量计算待控制四足机器人的各关节目标位置，以根据各关节目标位置控制待控制四足机器人的各关节进行运动。

具体的，该各关节位置(即各关节的运动轨迹)相较于现有技术计算得到的各关节的运动轨迹，增加了实时性的姿态控制，所以更加科学，能够使得待控制四足机器人在运动的过程中更加平稳。

在本实施例中，实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；然后基于各足端的位置坐标计算待控制四足机器人的足端支撑平面相对于机身平面的角度；进而基于角度，机身姿态角和机身姿态角速度计算待控制四足机器人的机身姿态角控制量；最终基于机身姿态角控制量计算待控制四足机器人的各关节目标位置，以根据各关节目标位置控制待控制四足机器人的各关节进行运动。通过上述描述可知，在本实施例中，在待控制四足机器人的运动过程中，能够实现待控制四足机器人的实时机身姿态控制，增强了待控制四足机器人的运动稳定性和地形适应的能力，缓解了现有的依赖于大姿态角偏差的姿态调整方式实时性差，严重影响四足机器人运动稳定性的技术问题。

上述内容对本发明的四足机器人的机身姿态控制方法进行了简单介绍，下面对其中涉及到的具体内容进行详细介绍。

在本发明的一个可选实施方式中，步骤S202，实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标包括如下步骤：

(1)通过设置于待控制四足机器人上的检测设备获取机身姿态角，其中，检测设备包括以下任一种：惯性测量单元IMU，陀螺仪；

具体的，待控制四足机器人上设置有检测设备，该检测设备可以为惯性测量单元IMU，也可以为陀螺仪。检测设备能够测得待控制四足机器人的机身姿态角，并将测得的机身姿态角发送至控制单元，控制单元就能获取机身姿态角。其中，机身姿态角包括：横滚角，俯仰角，偏航角。

(2)当检测设备为IMU时，通过IMU获取机身姿态角速度；

如果检测设备为IMU，IMU能够测得待控制四足机器人的机身姿态角速度，并将机身姿态角速度发送至控制单元，控制单元就能获取机身姿态角速度。

(3)当检测设备为陀螺仪时，对待控制四足机器人的机身姿态角进行差分计算，得到机身姿态角速度；

如果检测设备为陀螺仪，陀螺仪无法直接测得待控制四足机器人的机身姿态角速度，可以通过测得的两次机身姿态角进行差分计算，得到机身姿态角速度。

需要说明的是，机身姿态角速度包括：横滚角角速度，俯仰角角速度，偏航角角速度分别表示为

(4)通过正运动学对待控制四足机器人的各关节角进行计算，得到各足端在机身坐标系中的位置坐标，并将各足端在机身坐标系中的位置坐标作为各足端的位置坐标。

上述内容对获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标的过程进行了详细介绍，下面对计算待控制四足机器人的足端支撑平面相对于机身平面的角度的过程进行具体描述。

在本发明的一个可选实施方式中，步骤S204，基于各足端的位置坐标计算待控制四足机器人的足端支撑平面相对于机身平面的角度包括：

根据第一算式

和第二算式

计算待控制四足机器人的足端支撑平面相对于机身平面的角度，

其中，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，i可取f或h，j可取l或r，f表示前腿，h表示后腿，l表示左侧腿，r表示右侧腿，p_liz，p_riz，p_jfz，p_jhz分别表示对应的足端的位置坐标中Z轴方向坐标值，p_liy，p_riy分别表示对应的足端的位置坐标中Y轴方向坐标值，p_jfx，p_jhx分别表示对应的足端的位置坐标中X轴方向坐标值。

具体的，对于四足支撑或爬行步态，一般至少有三条支撑足，此时足端支撑平面相对于机身平面的角度(可用横滚角和俯仰角表示)计算为：

其中，φ_fb表示足端支撑平面相对于机身平面的横滚角度，i可取f或h，f表示前腿，h表示后腿，l表示左侧腿，r表示右侧腿，如p_lfz表示左前腿足端的位置坐标中Z轴方向坐标值，p_lfy表示左前腿足端的位置坐标中Y轴方向坐标值，其它相似参量与该参量的含义相似，此处不再一一进行举例说明。

其中，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，j可取l或r，f表示前腿，h表示后腿，l表示左侧腿，r表示右侧腿，该式中的参量与上述φ_fb算式中的参量含义相似，此处不再赘述。

上述内容对计算足端支撑平面相对于机身平面的角度的过程进行了详细介绍，下面对计算机身姿态角控制量的过进行具体描述。

在本发明的一个可选实施方式中，步骤S206，基于角度，机身姿态角和机身姿态角速度计算待控制四足机器人的机身姿态角控制量包括：

根据机身姿态角控制量计算算式

计算待控制四足机器人的机身姿态角控制量，

其中，φ_cmd，θ_cmd，ψ_cmd表示机身姿态角控制量，φ，θ，ψ表示机身姿态角，

表示机身姿态角速度，φ_d，θ_d，ψ_d表示理想机身姿态角，

表示理想机身姿态角速度，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，k_φ，k_θ，k_ψ和

表示控制参数量。

具体的，IMU或陀螺仪所反馈的机身姿态角一般是机身相对于世界坐标系{O_w}的，一般机身姿态角控制量也指机身相对于世界坐标系{O_w}的，而机身姿态是由足端支撑平面来实现的，足端支撑平面与地形密切相关，机身姿态角(以俯仰角为例)和足端支撑平面相对于机身平面的角度之间的关系如图4所示。

通过PD控制可得机身姿态角控制量表示为：

其中，φ_cmd，θ_cmd，ψ_cmd表示机身姿态角控制量，φ，θ，ψ表示机身姿态角，

表示机身姿态角速度，φ_d，θ_d，ψ_d表示理想机身姿态角，

表示理想机身姿态角速度，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，k_φ，k_θ，k_ψ和

表示控制参数量。

具体的，所谓理想机身姿态角(相对于世界坐标系)为步态控制时想要实现的角度，是人为设定的值，同理理想机身姿态角速度也是人为设定的。

上述内容对计算机身姿态角控制量的过程进行了详细介绍，下面对计算各关节目标位置的过程进行具体描述。

在本发明的一个可选实施方式中，参考图5，步骤S208，基于机身姿态角控制量计算待控制四足机器人的各关节目标位置包括如下步骤：

步骤S501，基于机身姿态角控制量计算旋转矩阵；

具体的，旋转矩阵为机身姿态角控制量的函数，其简化的表达式为：R＝R_z(ψ_cmd)R_y(θ_cmd)R_x(φ_cmd)。

步骤S502，基于旋转矩阵计算足端控制位置值；

具体的，根据足端控制位置计算算式

计算待控制四足机器人的足端控制位置值，其中，

表示待控制四足机器人的足端控制位置值，

表示足端规划位置值，R表示旋转矩阵。

需要说明的是，足端规划位置值是步态规划层规划得到的一个足端位置值，现有的技术方案就是通过逆运动学对该足端规划位置值进行计算，得到各关节目标位置，进而指导机器人进行运动的。

而本发明中是在这个足端规划位置值的基础上，根据待控制四足机器人的实时姿态(即R)对足端规划位置值进行耦合后实现的姿态控制，使得待控制四足机器人在实际运动时更加稳定。

步骤S503，根据逆运动学对足端控制位置值进行计算，得到各关节目标位置，以根据各关节目标位置控制待控制四足机器人的各关节进行运动。

加入了机身姿态控制的基于位置的四足步态规划与控制流程如图6所示。增加机身姿态控制的环节后，能够根据四足机器人的实时运动数据对足端规划位置值进行调整，使得最终得到的足端控制位置值更加科学，准确，考虑到了实时性的问题，使得机器人在实际运动过程中更加平稳。

此外，在实际应用时，当实际机身姿态角与理想机身姿态角(用户想要实现的角度)相差较大时，即支撑平面角度突变或理想机身姿态角突变时，应在实际机身姿态角与理想机身姿态角间进行插值规划，否则会由于关节刚性太大引起机身振动甚至倾倒。

比如，初始时刻时，机身姿态角为0度，但是突然将四足机器人搬到30度的斜面上时，计算得到的机身姿态角控制量太大，四足机器人的各关节的关节目标位置达到了关节刚性的极限，四足机器人可能会晃动，为了避免四足机器人的晃动，可以在0度到30度之间进行插值，使得四足机器人的机身姿态角平稳过度到30度。

实施例二：

本发明实施例还提供了一种四足机器人的机身姿态控制装置，该四足机器人的机身姿态控制装置主要用于执行本发明实施例上述内容所提供的四足机器人的机身姿态控制方法，以下对本发明实施例提供的四足机器人的机身姿态控制装置做具体介绍。

图7是根据本发明实施例的一种四足机器人的机身姿态控制装置的示意图，如图7所示，该四足机器人的机身姿态控制装置主要包括获取模块10，第一计算模块20，第二计算模块30和第三计算模块40，其中：

获取模块，用于实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；

第一计算模块，用于基于各足端的位置坐标计算待控制四足机器人的足端支撑平面相对于机身平面的角度；

第二计算模块，用于基于角度，机身姿态角和机身姿态角速度计算待控制四足机器人的机身姿态角控制量；

第三计算模块，用于基于机身姿态角控制量计算待控制四足机器人的各关节目标位置，以根据各关节目标位置控制待控制四足机器人的各关节进行运动。

在本实施例中，实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；然后基于各足端的位置坐标计算待控制四足机器人的足端支撑平面相对于机身平面的角度；进而基于角度，机身姿态角和机身姿态角速度计算待控制四足机器人的机身姿态角控制量；最终基于机身姿态角控制量计算待控制四足机器人的各关节目标位置，以根据各关节目标位置控制待控制四足机器人的各关节进行运动。通过上述描述可知，在本实施例中，在待控制四足机器人的运动过程中，能够实现待控制四足机器人的实时机身姿态控制，增强了待控制四足机器人的运动稳定性和地形适应的能力，缓解了现有的依赖于大姿态角偏差的姿态调整方式实时性差，严重影响四足机器人运动稳定性的技术问题。

可选地，获取模块包括：

第一获取单元，用于通过设置于待控制四足机器人上的检测设备获取机身姿态角，其中，检测设备包括以下任一种：惯性测量单元IMU，陀螺仪；

第二获取单元，用于当检测设备为IMU时，通过IMU获取机身姿态角速度；

差分计算单元，用于当检测设备为陀螺仪时，对待控制四足机器人的机身姿态角进行差分计算，得到机身姿态角速度；

第一计算单元，用于通过正运动学对待控制四足机器人的各关节角进行计算，得到各足端在机身坐标系中的位置坐标，并将各足端在机身坐标系中的位置坐标作为各足端的位置坐标。

可选地，第一计算模块包括：

第二计算单元，用于根据第一算式

和第二算式

计算待控制四足机器人的足端支撑平面相对于机身平面的角度，

其中，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，i可取f或h，j可取l或r，f表示前腿，h表示后腿，l表示左侧腿，r表示右侧腿，p_liz，p_riz，p_jfz，p_jhz分别表示对应的足端的位置坐标中Z轴方向坐标值，p_liy，p_riy分别表示对应的足端的位置坐标中Y轴方向坐标值，p_jfx，p_jhx分别表示对应的足端的位置坐标中X轴方向坐标值。

可选地，第二计算模块包括：

第三计算单元，用于根据机身姿态角控制量计算算式

计算待控制四足机器人的机身姿态角控制量，

其中，φ_cmd，θ_cmd，ψ_cmd表示机身姿态角控制量，φ，θ，ψ表示机身姿态角，

表示机身姿态角速度，φ_d，θ_d，ψ_d表示理想机身姿态角，

表示理想机身姿态角速度，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，k_φ，k_θ，k_ψ和

表示控制参数量。

可选地，第三计算模块包括：

第四计算单元，用于基于机身姿态角控制量计算旋转矩阵；

第五计算单元，用于基于旋转矩阵计算足端控制位置值；

第六计算单元，用于根据逆运动学对足端控制位置值进行计算，得到各关节目标位置，以根据各关节目标位置控制待控制四足机器人的各关节进行运动。

可选地，第五计算单元包括：

计算子单元，用于根据足端控制位置计算算式

计算待控制四足机器人的足端控制位置值，其中，

表示待控制四足机器人的足端控制位置值，

表示足端规划位置值，R表示旋转矩阵。

本发明实施例所提供的装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。

本发明实施例所提供的四足机器人的机身姿态控制方法及装置的计算机程序产品，包括存储了程序代码的计算机可读存储介质，所述程序代码包括的指令可用于执行前面方法实施例中所述的方法，具体实现可参见方法实施例，在此不再赘述。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统和装置的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

另外，在本发明实施例的描述中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

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

在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”仅用于描述目的，而不能理解为指示或暗示相对重要性。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (8)

1.一种四足机器人的机身姿态控制方法，其特征在于，包括：

实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；

基于所述各足端的位置坐标计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度；

基于所述角度，所述机身姿态角和所述机身姿态角速度计算所述待控制四足机器人的机身姿态角控制量；

基于所述机身姿态角控制量计算所述待控制四足机器人的各关节目标位置，以根据所述各关节目标位置控制所述待控制四足机器人的各关节进行运动；

其中，基于所述角度，所述机身姿态角和所述机身姿态角速度计算所述待控制四足机器人的机身姿态角控制量包括：

根据机身姿态角控制量计算算式

计算所述待控制四足机器人的机身姿态角控制量，

其中，φ_cmd，θ_cmd，ψ_cmd表示所述机身姿态角控制量，φ，θ，ψ表示所述机身姿态角，

表示所述机身姿态角速度，φ_d，θ_d，ψ_d表示理想机身姿态角，

表示理想机身姿态角速度，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，k_φ，k_θ，k_ψ和

表示控制参数量。

2.根据权利要求1所述的方法，其特征在于，实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标包括：

通过设置于所述待控制四足机器人上的检测设备获取所述机身姿态角，其中，所述检测设备包括以下任一种：惯性测量单元IMU，陀螺仪；

当所述检测设备为所述IMU时，通过所述IMU获取所述机身姿态角速度；

当所述检测设备为所述陀螺仪时，对所述待控制四足机器人的机身姿态角进行差分计算，得到所述机身姿态角速度；

通过正运动学对所述待控制四足机器人的各关节角进行计算，得到各足端在机身坐标系中的位置坐标，并将所述各足端在机身坐标系中的位置坐标作为所述各足端的位置坐标。

3.根据权利要求1所述的方法，其特征在于，基于所述各足端的位置坐标计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度包括：

根据第一算式

和第二算式

计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度，

其中，φ_fb表示所述足端支撑平面相对于机身平面的横滚角度，θ_fb表示所述足端支撑平面相对于机身平面的俯仰角度，i可取f或h，j可取l或r，f表示前腿，h表示后腿，l表示左侧腿，r表示右侧腿，p_liz，p_riz，p_jfz，p_jhz分别表示对应的足端的位置坐标中Z轴方向坐标值，p_liy，p_riy分别表示对应的足端的位置坐标中Y轴方向坐标值，p_jfx，p_jhx分别表示对应的足端的位置坐标中X轴方向坐标值。

4.根据权利要求1所述的方法，其特征在于，基于所述机身姿态角控制量计算所述待控制四足机器人的各关节目标位置包括：

基于所述机身姿态角控制量计算旋转矩阵；

基于所述旋转矩阵计算足端控制位置值；

根据逆运动学对所述足端控制位置值进行计算，得到各关节目标位置，以根据所述各关节目标位置控制所述待控制四足机器人的各关节进行运动。

5.根据权利要求4所述的方法，其特征在于，基于所述旋转矩阵计算足端控制位置值包括：

根据足端控制位置计算算式

计算所述待控制四足机器人的足端控制位置值，其中，

表示所述待控制四足机器人的足端控制位置值，

表示足端规划位置值，R表示所述旋转矩阵。

6.一种四足机器人的机身姿态控制装置，其特征在于，包括：

获取模块，用于实时获取待控制四足机器人的机身姿态角，机身姿态角速度，各足端的位置坐标；

第一计算模块，用于基于所述各足端的位置坐标计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度；

第二计算模块，用于基于所述角度，所述机身姿态角和所述机身姿态角速度计算所述待控制四足机器人的机身姿态角控制量；

第三计算模块，用于基于所述机身姿态角控制量计算所述待控制四足机器人的各关节目标位置，以根据所述各关节目标位置控制所述待控制四足机器人的各关节进行运动；

其中，所述第二计算模块包括：

第三计算单元，用于根据机身姿态角控制量计算算式

计算所述待控制四足机器人的机身姿态角控制量，

其中，φ_cmd，θ_cmd，ψ_cmd表示所述机身姿态角控制量，φ，θ，ψ表示所述机身姿态角，

表示所述机身姿态角速度，φ_d，θ_d，ψ_d表示理想机身姿态角，

表示理想机身姿态角速度，φ_fb表示足端支撑平面相对于机身平面的横滚角度，θ_fb表示足端支撑平面相对于机身平面的俯仰角度，k_φ，k_θ，k_ψ和

表示控制参数量。

7.根据权利要求6所述的装置，其特征在于，所述获取模块包括：

第一获取单元，用于通过设置于所述待控制四足机器人上的检测设备获取所述机身姿态角，其中，所述检测设备包括以下任一种：惯性测量单元IMU，陀螺仪；

第二获取单元，用于当所述检测设备为所述IMU时，通过所述IMU获取所述机身姿态角速度；

差分计算单元，用于当所述检测设备为所述陀螺仪时，对所述待控制四足机器人的机身姿态角进行差分计算，得到所述机身姿态角速度；

第一计算单元，用于通过正运动学对所述待控制四足机器人的各关节角进行计算，得到各足端在机身坐标系中的位置坐标，并将所述各足端在机身坐标系中的位置坐标作为所述各足端的位置坐标。

8.根据权利要求6所述的装置，其特征在于，所述第一计算模块包括：

第二计算单元，用于根据第一算式

和第二算式

计算所述待控制四足机器人的足端支撑平面相对于机身平面的角度，

其中，φ_fb表示所述足端支撑平面相对于机身平面的横滚角度，θ_fb表示所述足端支撑平面相对于机身平面的俯仰角度，i可取f或h，j可取l或r，f表示前腿，h表示后腿，l表示左侧腿，r表示右侧腿，p_liz，p_riz，p_jfz，p_jhz分别表示对应的足端的位置坐标中Z轴方向坐标值，p_liy，p_riy分别表示对应的足端的位置坐标中Y轴方向坐标值，p_jfx，p_jhx分别表示对应的足端的位置坐标中X轴方向坐标值。

Images