CN105242677B - 四足机器人双足支撑相位力位混合控制方法 - Google Patents

Abstract

一种四足机器人双足支撑相位力位混合控制方法，其步骤为：S1：将机器人的整体运动投影到径向平面和法向平面上；S2：建立控制模型；机器人沿径向平面上的运动简化为平面七连杆模型，在法向平面上的运动简化为线性倒立摆模型；然后将平面七连杆模型简化为平面虚拟伸缩腿模型；平面虚拟伸缩腿模型的控制目标为质心高度、本体俯仰角以及水平位移；S3：根据控制模型进行混合控制；建立平面虚拟伸缩腿模型的运动学方程并通过牛顿‑欧拉方法建立其动力学方程，对于质心高度和本体俯仰角通过位置伺服方法进行控制，对虚拟伸缩腿平面模型的水平位移采用外环位置、内环足端力的双环控制方法。本发明具有控制效果好、能够提高机器人适应能力等优点。

Description

四足机器人双足支撑相位力位混合控制方法

技术领域

本发明主要涉及到机器人运动控制技术领域，特指一种适用于四足机器人的双足支撑相位力位混合控制方法。

背景技术

移动机器人能够到达人类无法到达或因环境危险而不宜到达的地方，四足仿生机器人是仿照四足哺乳动物的运动模式的一类足式机器人，由于其可以在岩石、陡坡等复杂地形有非常强的适应能力，可以在非结构环境下进行物资运输、巡逻等任务，并能代替人类进行危险操作，因而具有重大的研究价值。

四足机器人一般由四条仿生腿和一个本体组成，每条腿包含一个侧向关节和至少两个前向关节。通过模仿学习自然界中四足机器人的运动方式，目前四足机器人的运动方式主要有三种步态：TROT步态(对角小跑步态)、BOUND步态(奔跑步态)和WALK步态(爬行步态)。

四足机器人的控制是典型的浮动基座控制问题，控制目标一般为其本***姿，而其本***姿主要由支撑腿控制。由于四足机器人自由度繁多，要实现对机器人的整体控制非常复杂，因此需要对模型进行适当简化。

在对四足机器人进行建模时，多连杆平面模型和SLIP模型是目前比较常用且有效的简化模型，这两个模型均能体现trot步态的运动学和动力学特性，基于模型的控制方法大多根据其运动学和动力学方程来设计控制器。

基于运动学的控制方法采用伺服控制率，根据传感器信息和控制目标计算各关节位置信息，并通过位置伺服律实现对期望轨迹的跟踪，由于位置伺服控制具有较大的刚度，因而能使机器人具备较强的负载能力，但在非结构化环境下容易出现摆动腿提前或滞后落地的情况，这种情况下产生的非预期冲击对本***姿的影响很大，因此需要设计算法以适应较大的位姿冲击。

基于动力学的控制方法一般采用计算力矩方法等多环控制方法，通过机器人本***姿信息计算足端期望力，并通过计算关节驱动力矩来控制足端期望力，进而达到控制机器人本***姿的目的。这种控制方法具有较好的柔顺性，对非结构化环境具有较强的适应能力。但是，其缺点在于在两条支撑腿之间可能产生内力，在支撑腿切换到摆动腿模式的时刻，内力瞬间释放，对摆动腿和本体的姿态产生非预期冲击，并且驱动器的刚度受整体质量的影响相对于位置控制较大。因此，力控制方法需要设计合理的力分配方法，在控制机器人位姿的同时能够增强机器人的运动能力。

发明内容

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种控制效果好、能够提高机器人适应能力的四足机器人双足支撑相位力位混合控制方法。

为解决上述技术问题，本发明采用以下技术方案：

一种四足机器人双足支撑相位力位混合控制方法，其步骤为：

S1：将机器人的整体运动投影到径向平面和法向平面上；所述径向平面为对脚支撑并垂直于水平面的平面；所述法向平面为通过本体质心并且垂直于径向平面的平面；

S2：建立控制模型；机器人沿径向平面上的运动简化为平面七连杆模型，在法向平面上的运动简化为线性倒立摆模型；然后将平面七连杆模型简化为平面虚拟伸缩腿模型；平面虚拟伸缩腿模型的控制目标为质心高度、本体俯仰角以及水平位移；

S3：根据控制模型进行混合控制；建立平面虚拟伸缩腿模型的运动学方程并通过牛顿-欧拉方法建立其动力学方程，对于质心高度和本体俯仰角通过位置伺服控制方法，对虚拟伸缩腿平面模型的水平位移采用力控制方式。

作为本发明的进一步改进：在步骤S3中，通过腿长来控制本体质心高度和俯仰角，通过控制水平足端力来控制本体水平位移，以实现对本***姿的近似解耦控制。

作为本发明的进一步改进：采用经典PID控制器作为力外环控制器，使支撑腿足端接触力准确跟踪期望的足端接触力。

作为本发明的进一步改进：采用位置伺服控制器对膝关节和踝关节进行控制以实现对关节角度的跟踪。

与现有技术相比，本发明的优点在于：

1、本发明在四足机器人Trot步态运动时，能够实现对本***姿的准确跟踪控制，并对不连续的期望轨迹具有较强的适应性。

2、本发明基于足端正压力的力分配策略能够有效避免机器人在行走过程中支撑腿足端滑动的现象，并使机器人具备较强的加速性能。

3、本发明能够提高四足机器人对非结构化环境的适应能力，实现机器人在不平整地面上的行走。

4、本发明能够通过增大位置控制器刚度提高机器人的负载能力。

5、本发明结构清晰、层次分明，具有较好的理论价值和工程意义。

附图说明

图1是本发明在具体应用实例中四足机器人平台的结构示意图。

图2是本发明在具体应用实例中径向平面投影的示意图。

图3是本发明在具体应用实例中平面虚拟伸缩腿结构的解耦控制框图。

图4是本发明方法的流程示意图。

具体实施方式

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。

本发明的方法主要适用于四足机器人。如图1所示，为四足机器人的***结构，由本体和四条腿组成，每条腿包含一个髋部侧向转动关节和三个前向转动关节(髋前关节、膝关节和踝关节)。每个关节均由液压驱动器驱动，并装有用于检测驱动器长度和驱动力的位移传感器和力传感器。每条腿足端均装设三维力传感器，用于检测机器人足端与环境的接触力信息，并通过IMU检测机器人在惯性系下的位姿信息。上述四足机器人具有如下机械结构特性：(1)机器人重心接近本体几何中心；(2)机器人本体的质量远大于腿部的质量。

在本发明中，以TROT步态双足支撑相为例，说明四足机器人双足支撑时的力位混合控制方法。本发明的四足机器人双足支撑相位力位混合控制方法，它采用力位混合控制的方法对机器人本***姿进行解耦控制，以增强机器人对非结构地面和负载变化的适应能力。本发明提出了基于足端正压力的力分配方法，该方法减小了支撑腿足端发生滑动的可能性，并提高了机器人的加速性能。

如图4所示，本发明的四足机器人双足支撑相位力位混合控制方法，步骤为：

S1：将机器人的整体运动投影到径向平面和法向平面上；所述径向平面为对脚支撑并垂直于水平面的平面；所述法向平面为通过本体质心并且垂直于径向平面的平面；

由于四足机器人自由度繁多，建立机器人整体的运动学和动力学方程较为繁杂，因此需将四足机器人进行合理简化：trot步态为两个对脚支撑状态的重复切换过程，因此本发明将机器人的整体运动投影到由对脚支撑并垂直于水平面的径向平面，以及通过质心并垂直于径向平面的法向平面。

S2：建立控制模型；

在具体应用实例中，机器人沿径向平面上的运动可简化为平面七连杆模型，在法向平面上的运动则可简化为线性倒立摆模型。

其中，平面七连杆模型单腿具有三个前向转动关节，由于单腿的运动学及动力学特性可等效为伸缩腿结构模型，因此又将平面七连杆模型简化为具有伸缩腿结构的平面虚拟模型。平面虚拟伸缩腿模型的控制目标为质心高度、本体俯仰角以及水平位移。

S3：根据控制模型进行混合控制；建立平面虚拟伸缩腿模型的运动学方程并通过牛顿-欧拉方法建立其动力学方程，对于质心高度和本体俯仰角通过位置伺服控制方法，对虚拟伸缩腿平面模型的水平位移采用力控制方式。即：

首先，建立该模型的运动学方程并通过牛顿-欧拉方法建立其动力学方程。由于位置控制方法具有很大刚度，因此对于本体质心高度及俯仰角通过位置伺服控制方法来提高机器人的负重能力。

由于平面虚拟伸缩腿模型共含有4个驱动关节，而整体控制目标只有三个，因此需添加一个约束条件才能实现对模型的整体控制。

由于支撑腿水平方向足端力受摩擦锥的自然约束，整体结构亦受ZMP点的动力学约束，为了使虚拟伸缩腿模型避免足端滑动，需在水平方向足端力添加一个约束方程，即为水平方向的力分配问题。因此，对虚拟伸缩腿平面模型的水平位移采用力控制方式。

在具体应用实例中，四足机器人在径向平面内的投影可以简化为一个七连杆结构，如图2所示(左前腿和右后腿作为支撑腿)。其中，(x,z)为本体质心在惯性坐标系下的位置，m为本体质量，L₀为机器人本体长度，d为本体的宽度，l₁,l₂,l₃分别为腿部踝关节、膝关节、髋关节的长度，θ₁,θ₂,θ₃为左前腿踝关节角、膝关节角、髋关节角，θ₄,θ₅,θ₆为右后腿踝关节角、膝关节角、髋关节角，为本体俯仰角，O、A、B、C、D分别为世界系原点(即两支撑腿足端连线中点)、右后腿髋部与本体连接点、左前腿髋部与本体连接点、右后腿足端点、左前腿足端点，F_x1,F_z1,F_x2,F_z2分别为C点和D点受到的接触力。由于腿部质量相对于本体质量很小，在建模时忽略腿部质量与惯量。

由图2中左边的示意图可以得到腿长||AC||和||BD||分别为：

由式(1)～(2)可知，腿长||AC||和||BD||分别和关节角度θ₄,θ₅和θ₁,θ₂相对应。因此，从运动学上来说，图2中左图示意的关节腿可简化为右图所示的虚拟伸缩腿模型，其中α₃,α₄分别为前后腿入射角。

由图2可建立平面虚拟伸缩腿结构运动学模型如下：

同样可得其动力学方程如下：

对髋关节力矩则有：

τ₁＝L₁sin(α₃)f_x1-L₁cos(α₃)f_z1 (9)

τ₂＝L₂sin(α₄)f_x1-L₂cos(α₄)f_z1 (10)

对于动力学方程(6)～(10)，图2中右图所示的伸缩腿模型和图2中左图所示的关节腿模型也是等效的。因此，从运动学和动力学方面均可将图2的左图中所示的平面七连杆模型简化为图2的右图中所示的虚拟伸缩腿模型。

基于上述本发明的方法，在具体应用实例中，需要对位姿解耦控制；

对于入射角α₃,α₄来说有：

0<α₃<π,0<α₄<π (11)

故而有：

0<sin(α₃)≤1,0<sin(α₄)≤1 (12)

-1<cos(α₃)<1,-1<cos(α₄)<1 (13)

对于前向位移来说，由方程(5)可知存在奇异点α₃＝α₄＝π/2，因此不适合用腿长来控制前向位移。但对于方程(3)、(4)来说不存在奇异点，因此，可以通过腿长l₁,l₂来控制本体俯仰角和质心高度，为简便起见采取通过控制来简介控制本体俯仰角的方法，并令

将(3)、(4)写为矩阵形式则为：

其中，

由式(3)可知J₁非奇异，并且前向位移对本体俯仰角和质心高度的影响可通过雅克比矩阵J₁消除，因此可认为是运动学近似解耦的。

由于平面伸缩腿结构的控制目标有水平位移、质心高度和本体俯仰角，其中质心高度和本体俯仰角通过腿长来进行控制，前向位移可通过足端力控制来实现。在动力学方程中仅与前向位置相关的只有式(6)，需要添加一个水平力分配方程才能实现对足端力的计算，进而实现对水平位移的控制。为了使足端发生滑动的可能性最小，采用如下所述力分配方法，并采用双环控制方法来进行控制。

定义足端发生滑动的可能性为SCI(滑动约束指标)：

其中F_x，F_z为机器人在步行过程中受到的地面反力。SCI值越小，足端发生滑动的可能性越小。为使SCI尽可能小，则可采用力分配方式为：

f_x1d/f_x2d＝f_z1/f_z2 (16)

联立(6)、(16)可得期望足端力为：

由式(17)计算水平方向足端期望力时是和质心高度和本体俯仰角完全无关的，因此可认为外环，即水平足端力和另两个控制目标是动力学解耦的。

对于水平方向足端力须通过关节力矩来进行控制，由(9)、(10)可得：

其中，

由式(3)可知J₂非奇异，并且通过J₂和补偿项G消除了质心高度和俯仰角对关节力矩的影响，因此可认为内环是动力学近似解耦的。

采用上述所示方法，即通过腿长来控制本体质心高度和俯仰角，通过控制水平足端力来控制本体水平位移，可实现对本***姿的近似解耦控制。

基于上述本发明的方法，在具体应用实例中，需要进行控制器设计；

1)力控制器设计；

基于公式(17)，设计经典PID控制器作为力外环控制器，具体形式如下：

其中，f'_x1d,f'_x2d为期望的足端接触力，x_d为期望的x方向位置，f_z1r,f_z2r为竖直方向足端力。

由于机器人在运动中要保持平衡受到ZMP点的约束，对于虚拟伸缩腿结构来说，为维持机器人的双足支撑状态，ZMP点需限制在双足连线的线段上。在不考虑质心高度和俯仰角动态的情况下，该约束形式如下：

其中，0<k₁<1，x₁,x₂分别为D点和C点水平位置。

另外，足端受摩擦锥的自然约束条件：f'_x1d<μf_z1r、f'_x1d<μf_z1d，μ为摩擦系数，控制器变为：

通过调节参数Kp_x,Kd_x,Ki_x可以保证在控制前向位移的同时，避免机器人在运动过程中出现翻转或足端滑动的现象。

基于公式(19)，采用经典的PID控制器即可使支撑腿足端接触力准确跟踪期望的足端接触力。

2)位置控制器设计；

基于公式(14)，可得逆运动学方程：

采用位置伺服控制器：

其中，f为作动器上的推力。通过调整控制器参数Kp_L,Kv_L即可实现对腿长的控制，并可使位置控制器具有很大的刚度。但由于Trot步态中在切换腿或在非规则地面运行时经常出现位置估计不连续现象，对于实际值和期望值之间的小偏差情况一般经过驱动力限幅就可保持机器人平衡运行，但Trot步态很容易发生出现实际腿长和期望腿长的差异很大，这时就需要通过添加合适的过渡过程使期望腿长连续可导。在添加过渡过程时需保证足端点接触地面，在本体俯仰角保持不变的情况下可得对腿长变化的约束条件如下：

由式(7)可得：

又有：

故可得对腿长加速度的近似约束条件为：

因此，对腿长可添加一个加速度受限的二阶过渡过程如下：

对加速度添加限幅后式(29)变为：

故腿长期望可给定为：

其中，L₁₀,L₂₀分别为两腿初始腿长。

如前所述可得平面虚拟伸缩腿模型整体控制框图，如图3所示。

3)七连杆模型关节位置伺服控制器设计；

对于七连杆结构的控制器设计和虚拟伸缩腿模型控制器类似，需对髋关节采用力控制方式，对膝关节和踝关节采用位置伺服控制方式。由于单腿前向含有三个关节，相对于腿长控制多了一个自由度，添加三个自由度的目的主要是考虑液压缸的驱动性能，增强单腿的驱动能力或改善***性能。因此，在以下进行逆运动学解算时，为简化运算过程，添加关节约束如下：

θ₄＝θ₅,θ₁＝θ₂ (31)

由图2中左图所示和式(31)易得：

对膝关节和踝关节角度采用伺服控制器：

其中，τ为关节驱动力矩。通过调整控制器参数即可实现对关节角度的跟踪控制。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (4)

1.一种四足机器人双足支撑相位力位混合控制方法，其特征在于，步骤为：

S1：将机器人的整体运动投影到径向平面和法向平面上；所述径向平面为对脚支撑并垂直于水平面的平面；所述法向平面为通过本体质心并且垂直于径向平面的平面；

S2：建立控制模型；机器人沿径向平面上的运动简化为平面七连杆模型，在法向平面上的运动简化为线性倒立摆模型；然后将平面七连杆模型简化为平面虚拟伸缩腿模型；平面虚拟伸缩腿模型的控制目标为质心高度、本体俯仰角以及水平位移；

S3：根据控制模型进行混合控制；建立平面虚拟伸缩腿模型的运动学方程并通过牛顿-欧拉方法建立其动力学方程，对于质心高度和本体俯仰角通过位置伺服控制方法，对虚拟伸缩腿平面模型的水平位移采用力控制方式。

2.根据权利要求1所述的四足机器人双足支撑相位力位混合控制方法，其特征在于，在步骤S3中，通过腿长来控制本体质心高度和俯仰角，通过控制水平足端力来控制本体水平位移，以实现对本***姿的近似解耦控制。

3.根据权利要求2所述的四足机器人双足支撑相位力位混合控制方法，其特征在于，采用经典PID控制器作为力外环控制器，使支撑腿足端接触力准确跟踪期望的足端接触力。

4.根据权利要求2所述的四足机器人双足支撑相位力位混合控制方法，其特征在于，采用位置伺服控制器对膝关节和踝关节进行控制以实现对关节角度的跟踪。