CN111309039B

CN111309039B - 一种四足机器人姿态控制方法和装置

Info

Publication number: CN111309039B
Application number: CN202010125693.2A
Authority: CN
Inventors: 储振; 李超; 朱秋国
Original assignee: Hangzhou Deeprobotics Co ltd
Current assignee: Hangzhou Deeprobotics Co ltd
Priority date: 2020-02-27
Filing date: 2020-02-27
Publication date: 2023-08-22
Anticipated expiration: 2040-02-27
Also published as: CN111309039A

Abstract

本发明公开了一种四足机器人姿态控制方法和装置，该方法包括：根据机器人腿与支撑平面的接触情况，通过机器人腿各关节在世界坐标系下的坐标，计算当前时刻支撑平面在世界坐标系下的方程；根据支撑面在世界坐标系下的方程，计算四足机器人姿态角的期望轨迹；根据四足机器人姿态角的期望轨迹，求得各关节力矩，实现四足机器人的姿态控制。

Description

一种四足机器人姿态控制方法和装置

技术领域

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

背景技术

四足机器人相对轮式机器人的优势就是可以适用于各种复杂地形。但是该种优势的前提是机器人可以精确的感知地形信息并迅速调整身体姿态来适应地形。很多四足机器人的依赖于慢速下外部感知设备的信息或者需要通过特定步态(如walk)的足够多的接触点信息才能对地形变化做出反应。在复杂崎岖路面时，上述方法难以保证机器人的稳定性，严重情况下还会导致机器人翻倒，存在姿态控制的实时性、精度和适应性有限等问题。

发明内容

本发明的目的是提出一种四足机器人姿态控制方法和装置，以解决相关技术中存在的实时性、精度和适应性差等问题。

为了达到上述目的，本发明所采用的技术方案如下：

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

根据机器人腿与支撑平面的接触情况，通过机器人腿各关节在世界坐标系下的坐标，计算当前时刻支撑平面在世界坐标系下的方程；

根据支撑面在世界坐标系下的方程，计算四足机器人姿态角的期望轨迹；

根据四足机器人姿态角的期望轨迹，求得各关节力矩，实现四足机器人的姿态控制。

进一步地，机器人腿与支撑平面的接触情况分为以下几种情况：三条腿接触地面的情况、四条腿接触地面的情况和少于三条腿接触地面的情况。

进一步地，针对三条腿接触地面的情况，支撑平面在世界坐标系下的方程的系数矩阵可逆，直接计算出支撑平面在世界坐标系下的方程。

进一步地，针对四条腿接触地面的情况，支撑平面在世界坐标系下的方程是超定方程组，通过矩阵的伪逆求其最小二乘解，计算出的支撑平面是所有平面中距离四个接触点误差平方和最小的平面。

进一步地，针对少于三条腿接触地面的情况，其条件不足以确定一个平面，而是无数平面的集合，则需添加约束，取最近的支撑腿切换成摆动腿时的位置，补充条件至四条腿接触地面的情况。

进一步地，机器人腿各关节在世界坐标系下的坐标，包括：

建立坐标系，通过IMU得到机器人在世界坐标系下的欧拉角，进而得到机器人坐标系到世界坐标系的变换矩阵；

通过机器人各关节编码器和运动学分析，得到机器人各腿在世界坐标系下的坐标。

进一步地，根据支撑面在世界坐标系下的方程，计算四足机器人姿态角的期望轨迹，包括：

根据支撑平面在世界坐标系下的方程，计算实时的机器人姿态角的期望值；

通过IMU得到机器人当前在世界坐标系下的实际姿态角；

根据四足机器人姿态角的期望值和当前实际的姿态角，规划平滑曲线，获得四足机器人姿态角的期望轨迹。

进一步地，根据四足机器人姿态角的期望轨迹，求得各关节力矩，包括：

根据四足机器人姿态角的期望轨迹，计算在世界坐标系下作用于质心的力和力矩；

通过解二次规化问题实时计算各腿的接触力，进而计算各关节力矩。

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

第一计算模块，用于根据机器人腿与支撑平面的接触情况，通过机器人腿各关节在世界坐标系下的坐标，计算当前时刻支撑平面在世界坐标系下的方程；

第二计算模块，用于根据支撑面在世界坐标系下的方程，计算四足机器人姿态角的期望轨迹；

第三计算模块，用于根据四足机器人姿态角的期望轨迹，求得各关节力矩，实现四足机器人的姿态控制。

第三方面，根据本发明的实施例，提供一种设备，包括存储器、处理器，所述存储器中存储有可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述第一方面任一项所述的一种四足机器人姿态控制方法。

在本发明实施例中，仅依靠分析四足机器人腿与支撑平面接触的情况就可以得到实时的地形信息，因为考虑了所有可能发生的情况，所以对于任何步态以及没有外部感知设备的情况都可以适用，采用得到的信息规划机器人姿态角的期望轨迹并实现姿态控制，保证了姿态控制的实时性、精度和适应性，从而提升机器人在复杂地形下各种步态的稳定性。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是根据本发明实施例的一种四足机器人姿态控制方法的流程图；

图2是根据本发明实施例的四足机器人的坐标系；

图3是根据本发明实施例的一种四足机器人姿态控制装置的框图。

具体实施方式

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

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

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

实施例1：

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

步骤S101，根据机器人腿与支撑平面的接触情况，通过机器人腿各关节在世界坐标系下的坐标，计算当前时刻支撑平面在世界坐标系下的方程；

步骤S102，根据支撑面在世界坐标系下的方程，计算四足机器人姿态角的期望轨迹；

步骤S103，根据四足机器人姿态角的期望轨迹，求得各关节力矩，实现四足机器人的姿态控制。

根据本发明上述实施例，其中机器人腿各关节在世界坐标系下的坐标，包括：

具体地：

(1)建立四足机器人的坐标系如图2，其中世界坐标系W(xyz)、体心坐标系B(x’y’z’)。机器人单腿3自由度，所有关节位置θ＝[θ₁,θ₂,θ₃,θ₄]^T，θ₁,θ₂,θ₃,θ₄∈R³分别是各条腿的关节位置，体心坐标系下的变换矩阵为T。建立运动学关系，在体心坐标系下的各腿的坐标为p₁,p₂,p₃,p₄∈R³，所有腿的坐标为p，可得：

p＝[p₁,p₂,p₃,p₄]^T

p＝Tθ

(2)由四足机器人IMU可得体心的欧拉角为RPY＝[roll,pitch,yaw]^T，根据这个可以求出世界坐标系和体心坐标系之间的旋转矩阵为其中R_z,R_y,R_x为绕对应轴的旋转矩阵，可得到在世界坐标系下的各腿的坐标p_W为：

(3)通过机器人各关节编码器与运动学分析得到机器人各腿在世界坐标系下的坐标。

根据本发明上述实施例，其中机器人腿与支撑平面的接触情况分为以下几种情况：三条腿接触地面的情况、四条腿接触地面的情况和少于三条腿接触地面的情况。

进一步地，针对三条腿接触地面的情况，支撑平面在世界坐标系下的方程的系数矩阵可逆，直接计算出支撑平面在世界坐标系下的方程。针对四条腿接触地面的情况，支撑平面在世界坐标系下的方程是超定方程组，通过矩阵的伪逆求其最小二乘解，计算出的支撑平面是所有平面中距离四个接触点误差平方和最小的平面。针对少于三条腿接触地面的情况，其条件不足以确定一个平面，而是无数平面的集合，则可以通过人为添加约束，取最近的支撑腿切换成摆动腿时的位置，补充条件至四条腿接触地面的情况。

分析四足机器人腿与支撑平面的接触情况是为了适应其所有可能的步态，特别是一些高动态的步态。对于这些步态，经常会出现只有一条腿着地甚至有滞空的情况，这时候计算支撑平面的信息就非常少，人为添加的约束即状态切换时的信息由于机器人步态周期很短，所以误差较小；并且由于关节编码器以及IMU更新速度很快，所以由其计算出的接触平面的信息也更新的非常快，更新频率一般大于机器人步态频率的百倍以上，所以可以适用于这些高动态的步态。

下面以两条腿接触地面为例来进行说明支撑平面的计算方式。

由传感器可知机器人两条支撑腿接触地面时各关节角度θ_support∈R⁶；两条摆动腿最近时刻作为支撑腿时各关节角度θ_swing∈R⁶，则当前时刻支撑平面在世界坐标系下的方程计算步骤如下：

(1)世界坐标系下机器人各腿坐标p_Wi＝[x_wi,y_wi,z_wi](i＝1,2,3,4)，由前述坐标变换可得世界坐标系下所有腿的坐标p_W为：

(2)对于接触平面上任意一点r＝[x,y,z]与接触平面的法向量n＝[a,b,c]均满足以下关系：

(r-r₀)^Tn＝0

(x-x₀)a+(y-y₀)b+(z-z₀)c＝0

其中r₀＝[x₀,y₀,z₀]为平面上的点，将与y₀相关的全部移到右边记为d，可得：

xa+yb+zc＝d

由于法向量可以定义c＝1，所以最后平面上的所有点都满足下式：

xa+yb+z＝d

(3)将p_W1,p_W2,p_W3,p_W4分别带入可以得到一个过约束方程：

为了求其最小二乘解，可以通过伪逆求解可得就可以得到支撑平面在世界坐标系下的方程。

根据本发明上述实施例，根据支撑平面在世界坐标系下的方程，计算四足机器人姿态角的期望轨迹，包括：

(1)需要根据支撑平面在世界坐标系下的方程，将其转化成机器人姿态控制可用的数据。机器人的姿态控制涉及姿态角横滚(roll)、俯仰(pitch)和偏航(yaw)角，yaw为方向控制角度，期望值可由控制指令直接给出，与接触平面信息相关的为roll和pitch。根据支撑平面在世界坐标系下的方程，相应的姿态角期望值roll_des,pitch_des求解过程如下：

roll_des＝atan(a)

pitch_des＝-atan(b)

(2)得到姿态角期望值之后，姿态角的当前值可以由IMU得到，规划当前姿态角到期望姿态角之间的平滑曲线作为姿态角的期望轨迹。

对于四足机器人来说，其姿态角roll和pitch直接影响关节力矩与角度，值不合适的话会导致机器人极易达到关节限位或者力矩上限。所以在得到接触平面信息之后，需要根据其规划合适的姿态角的期望轨迹，并且需要保证曲线平滑，便于实现机器人的姿态控制。

下面以样条插值为例说明平滑曲线的计算方式。

步态周期为T，t为从当前步态周期开始的时间，由IMU得到的每一刻roll和pitch的当前值为α_a(t),β_a(t)，角速度为姿态角期望值为α_d(t),β_d(t)，角速度记为0rad/s，具体计算步骤如下：

(1)规划平滑曲线从实际值过渡到期望值，为了保证曲线一阶导数即角速度平滑连续，此处采用规划三次样条曲线α(t),β(t)，且a₁,b₁,c₁,d₁,a₂,b₂,c₂,d₂为曲线系数：

α(t)＝a₁t³+b₁t²+c₁t+d₁

β(t)＝a₂t³+b₂t²+c₂t+d₂

代入t₀,T时刻的条件：

(2)解出[a₁,b₁,c₁,d₁]^T,[a₂,b₂,c₂,d₂]^T，则可以得到姿态角的期望轨迹α(t),β(t)，该轨迹与IMU信息以及估计的支撑平面时刻相关。

根据本发明上述实施例，根据四足机器人姿态角的期望轨迹，求得各关节力矩，实现四足机器人的姿态控制，包括：

(1)质心的控制策略如下，作用于质心的控制量为b_d：

其中F_xyz,Torque_rpy为作用于质心的力和力矩，m,I为机器人的质量和转动惯量，p_d,p为质心的期望位置和实际位置，θ_d,θ,w_d,w为各姿态角的期望角度，实际角度，期望角速度和实际角速度，K_p,d,K_d,p,K_p,w,K_d,w为PD控制参数。

由于本处主要考虑姿态控制，所以对于p_d取步态相应规划的值即可；对于θ_d，取上述规划的姿态角的期望值，最后与roll和pitch相关的控制量为：

其中α,β为实际roll和pitch姿态角，K_p,w',K_d,w',I'为与roll和pitch相应的控制参数及转动惯量。

各腿的发力为F＝(F₁ ^T,F₂ ^T,F₃ ^T,F₄ ^T)^T，通过选择矩阵和位置矩阵A，得到b_d＝AF的形式，可以通过解二次规划问题得到各腿发力F_solution，其中C_E,c_E,C_I,c_I为等式约束和不等式约束的方程系数，此处可以为摩擦约束和最大力约束：

s.t.

C_EF+c_E＝0

C_IF+c_I≥0

通过机器人雅可比矩阵J，求得各关节力矩为τ＝J^TF_solution，实现四足机器人的姿态控制。

采用这种控制方式并且求解二次规划问题，可以保证机器人身体的各个自由度都是可控的，尤其对于姿态角roll和pitch，可以比较好的实现机器人的姿态控制。

实施例2：

图3为本发明实施例提供的一种四足机器人姿态控制装置的结构示意图，该装置可以执行任意本发明任意实施例所提供的一种四足机器人姿态控制方法，具备执行该方法相应的功能模块和有益效果。如图3所示，该装置包括：

第一计算模块91，用于根据机器人腿与支撑平面的接触情况，通过机器人腿各关节在世界坐标系下的坐标，计算当前时刻支撑平面在世界坐标系下的方程；

第二计算模块92，用于根据支撑面在世界坐标系下的方程，计算四足机器人姿态角的期望轨迹；

第三计算模块93，用于根据四足机器人姿态角的期望轨迹，求得各关节力矩，实现四足机器人的姿态控制。

根据本发明的实施例，还提供一种设备，包括存储器、处理器，所述存储器中存储有可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述实施例1所述的一种四足机器人姿态控制方法。

实施例3：

本发明实施例提供一种设备，包括存储器、处理器，所述存储器中存储有可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述第一方面所述的一种四足机器人溜蹄步态规划方法。

实施例4：

本发明实施例提供一种具有处理器可执行的非易失的程序代码的计算机可读介质，其特征在于，所述程序代码使所述处理器执行所述第一方面所述的一种四足机器人溜蹄步态规划方法。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

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

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims

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

根据四足机器人姿态角的期望轨迹，求得各关节力矩，实现四足机器人的姿态控制；

其中，机器人腿与支撑平面的接触情况分为以下几种情况：三条腿接触地面的情况、四条腿接触地面的情况和少于三条腿接触地面的情况；

针对三条腿接触地面的情况，支撑平面在世界坐标系下的方程的系数矩阵可逆，直接计算出支撑平面在世界坐标系下的方程；

针对四条腿接触地面的情况，支撑平面在世界坐标系下的方程是超定方程组，通过矩阵的伪逆求其最小二乘解，计算出的支撑平面是所有平面中距离四个接触点误差平方和最小的平面；

针对少于三条腿接触地面的情况，其条件不足以确定一个平面，而是无数平面的集合，则需添加约束，取最近的支撑腿切换成摆动腿时的位置，补充条件至四条腿接触地面的情况。

2.根据权利要求1所述的一种四足机器人姿态控制方法，其特征在于，机器人腿各关节在世界坐标系下的坐标，包括：

3.根据权利要求1所述的一种四足机器人姿态控制方法，其特征在于，根据支撑平面在世界坐标系下的方程，计算四足机器人姿态角的期望轨迹，包括：

通过IMU得到机器人当前在世界坐标系下的实际姿态角；

4.根据权利要求1所述的一种四足机器人姿态控制方法，其特征在于，根据四足机器人姿态角的期望轨迹，求得各关节力矩，包括：

通过解二次规划问题实时计算各腿的接触力，进而计算各关节力矩。

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

第三计算模块，用于根据四足机器人姿态角的期望轨迹，求得各关节力矩，实现四足机器人的姿态控制；

6.一种设备，包括存储器、处理器，所述存储器中存储有可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述权利要求1至4任一项所述的一种四足机器人姿态控制方法。