CN105242677A

CN105242677A - Quadruped robot biped support phase force hybrid force control method

Info

Publication number: CN105242677A
Application number: CN201510465173.5A
Authority: CN
Inventors: 马宏绪; 刘益彰; 安宏雷; 饶锦辉; 韦庆; 王剑; 王建文; 郎琳; 张献鹏; 许可; 许佳奇
Original assignee: National University of Defense Technology
Current assignee: National University of Defense Technology
Priority date: 2015-07-31
Filing date: 2015-07-31
Publication date: 2016-01-13
Anticipated expiration: 2035-07-31
Also published as: CN105242677B

Abstract

A quadruped robot biped support phase force position hybrid control method, the steps are: S1: project the overall motion of the robot onto the radial plane and the normal plane; S2: establish a control model; the robot along the radial plane The motion is simplified to a plane seven-link model, and the motion on the normal plane is simplified to a linear inverted pendulum model; then the plane seven-link model is simplified to a plane virtual telescopic leg model; the control target of the plane virtual telescopic leg model is the height of the center of mass, Pitch angle and horizontal displacement of the body; S3: Hybrid control according to the control model; establish the kinematic equation of the plane virtual telescopic leg model and establish its dynamic equation through the Newton-Euler method, and use the position servo method for the height of the center of mass and the pitch angle of the body To control the horizontal displacement of the virtual telescopic leg plane model, the double-loop control method of the position of the outer ring and the force of the foot end of the inner ring is adopted. The invention has the advantages of good control effect, improved robot adaptability and the like.

Description

Phase-force-position hybrid control method for bipedal support of quadruped robot

技术领域technical field

本发明主要涉及到机器人运动控制技术领域，特指一种适用于四足机器人的双足支撑相位力位混合控制方法。The invention mainly relates to the technical field of robot motion control, in particular to a biped support phase force-position mixed control method suitable for a quadruped robot.

背景技术Background technique

移动机器人能够到达人类无法到达或因环境危险而不宜到达的地方，四足仿生机器人是仿照四足哺乳动物的运动模式的一类足式机器人，由于其可以在岩石、陡坡等复杂地形有非常强的适应能力，可以在非结构环境下进行物资运输、巡逻等任务，并能代替人类进行危险操作，因而具有重大的研究价值。Mobile robots can reach places that humans cannot reach or are not suitable for due to environmental hazards. Quadruped bionic robots are a type of legged robots that imitate the movement patterns of quadruped mammals. Its adaptability can carry out tasks such as material transportation and patrolling in unstructured environments, and it can replace humans in dangerous operations, so it has great research value.

四足机器人一般由四条仿生腿和一个本体组成，每条腿包含一个侧向关节和至少两个前向关节。通过模仿学习自然界中四足机器人的运动方式，目前四足机器人的运动方式主要有三种步态：TROT步态(对角小跑步态)、BOUND步态(奔跑步态)和WALK步态(爬行步态)。A quadruped robot generally consists of four bionic legs and a body, each leg contains a lateral joint and at least two forward joints. By imitating the movement of quadruped robots in nature, there are currently three main gaits for quadruped robots: TROT (diagonal trotting), BOUND (running), and WALK (crawling). gait).

四足机器人的控制是典型的浮动基座控制问题，控制目标一般为其本体位姿，而其本体位姿主要由支撑腿控制。由于四足机器人自由度繁多，要实现对机器人的整体控制非常复杂，因此需要对模型进行适当简化。The control of a quadruped robot is a typical floating base control problem. The control target is generally its body pose, which is mainly controlled by the supporting legs. Due to the many degrees of freedom of the quadruped robot, it is very complicated to realize the overall control of the robot, so the model needs to be simplified appropriately.

在对四足机器人进行建模时，多连杆平面模型和SLIP模型是目前比较常用且有效的简化模型，这两个模型均能体现trot步态的运动学和动力学特性，基于模型的控制方法大多根据其运动学和动力学方程来设计控制器。When modeling a quadruped robot, the multi-link planar model and the SLIP model are commonly used and effective simplified models, both of which can reflect the kinematics and dynamics of the trot gait, and the model-based control Most methods design controllers based on their kinematics and dynamics equations.

基于运动学的控制方法采用伺服控制率，根据传感器信息和控制目标计算各关节位置信息，并通过位置伺服律实现对期望轨迹的跟踪，由于位置伺服控制具有较大的刚度，因而能使机器人具备较强的负载能力，但在非结构化环境下容易出现摆动腿提前或滞后落地的情况，这种情况下产生的非预期冲击对本体位姿的影响很大，因此需要设计算法以适应较大的位姿冲击。The control method based on kinematics adopts the servo control rate, calculates the position information of each joint according to the sensor information and the control target, and realizes the tracking of the expected trajectory through the position servo law. Because the position servo control has a large stiffness, it can make the robot have Strong load capacity, but in an unstructured environment, it is easy for the swing leg to land early or late. In this case, the unexpected impact has a great impact on the body pose, so it is necessary to design an algorithm to adapt to the larger pose impact.

基于动力学的控制方法一般采用计算力矩方法等多环控制方法，通过机器人本体位姿信息计算足端期望力，并通过计算关节驱动力矩来控制足端期望力，进而达到控制机器人本体位姿的目的。这种控制方法具有较好的柔顺性，对非结构化环境具有较强的适应能力。但是，其缺点在于在两条支撑腿之间可能产生内力，在支撑腿切换到摆动腿模式的时刻，内力瞬间释放，对摆动腿和本体的姿态产生非预期冲击，并且驱动器的刚度受整体质量的影响相对于位置控制较大。因此，力控制方法需要设计合理的力分配方法，在控制机器人位姿的同时能够增强机器人的运动能力。The dynamics-based control method generally adopts multi-loop control methods such as the calculation torque method, calculates the expected force of the foot end through the robot body pose information, and controls the foot end expected force by calculating the joint drive torque, and then achieves the goal of controlling the robot body pose. Purpose. This control method has good flexibility and strong adaptability to unstructured environments. However, its disadvantage is that an internal force may be generated between the two supporting legs. When the supporting leg is switched to the swing leg mode, the internal force is released instantaneously, causing an unexpected impact on the posture of the swing leg and the body, and the stiffness of the driver is affected by the overall mass. The influence is larger than that of the position control. Therefore, the force control method needs to design a reasonable force distribution method, which can enhance the robot's motion ability while controlling the robot's pose.

发明内容Contents of the invention

本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种控制效果好、能够提高机器人适应能力的四足机器人双足支撑相位力位混合控制方法。The technical problem to be solved by the present invention is: aiming at the technical problems existing in the prior art, the present invention provides a biped support phase-force-position mixed control method for a quadruped robot with good control effect and improved adaptability of the robot.

为解决上述技术问题，本发明采用以下技术方案：In order to solve the problems of the technologies described above, the present invention adopts the following technical solutions:

一种四足机器人双足支撑相位力位混合控制方法，其步骤为：A quadruped robot biped support phase force position hybrid control method, the steps are:

S1：将机器人的整体运动投影到径向平面和法向平面上；所述径向平面为对脚支撑并垂直于水平面的平面；所述法向平面为通过本体质心并且垂直于径向平面的平面；S1: Project the overall motion of the robot onto a radial plane and a normal plane; the radial plane is a plane that supports the feet and is perpendicular to the horizontal plane; the normal plane is a plane that passes through the center of mass of the body and is perpendicular to the radial plane flat;

S2：建立控制模型；机器人沿径向平面上的运动简化为平面七连杆模型，在法向平面上的运动简化为线性倒立摆模型；然后将平面七连杆模型简化为平面虚拟伸缩腿模型；平面虚拟伸缩腿模型的控制目标为质心高度、本体俯仰角以及水平位移；S2: Establish the control model; the robot's motion along the radial plane is simplified to a plane seven-link model, and its motion on the normal plane is simplified to a linear inverted pendulum model; then the plane seven-link model is simplified to a plane virtual telescopic leg model ; The control targets of the plane virtual telescopic leg model are the height of the center of mass, the pitch angle of the body and the horizontal displacement;

S3：根据控制模型进行混合控制；建立平面虚拟伸缩腿模型的运动学方程并通过牛顿-欧拉方法建立其动力学方程，对于质心高度和本体俯仰角通过位置伺服控制方法，对虚拟伸缩腿平面模型的水平位移采用力控制方式。S3: Carry out hybrid control according to the control model; establish the kinematic equation of the plane virtual telescopic leg model and establish its dynamic equation through the Newton-Euler method, use the position servo control method for the height of the center of mass and the pitch angle of the body, and control the plane of the virtual telescopic leg The horizontal displacement of the model is controlled by force.

作为本发明的进一步改进：在步骤S3中，通过腿长来控制本体质心高度和俯仰角，通过控制水平足端力来控制本体水平位移，以实现对本体位姿的近似解耦控制。As a further improvement of the present invention: in step S3, the height of the center of mass and the pitch angle of the body are controlled by the length of the legs, and the horizontal displacement of the body is controlled by controlling the horizontal foot force, so as to achieve approximate decoupling control of the body pose.

作为本发明的进一步改进：采用经典PID控制器作为力外环控制器，使支撑腿足端接触力准确跟踪期望力。As a further improvement of the present invention: the classic PID controller is used as the force outer loop controller, so that the contact force at the foot end of the supporting leg can accurately track the expected force.

作为本发明的进一步改进：采用位置伺服控制器对膝关节和踝关节进行控制以实现对关节角度的跟踪。As a further improvement of the present invention: a position servo controller is used to control the knee joint and the ankle joint to track the joint angle.

与现有技术相比，本发明的优点在于：Compared with the prior art, the present invention has the advantages of:

1、本发明在四足机器人Trot步态运动时，能够实现对本体位姿的准确跟踪控制，并对不连续的期望轨迹具有较强的适应性。1. The present invention can realize accurate tracking control of the body pose when the quadruped robot is moving in Trot gait, and has strong adaptability to discontinuous expected trajectories.

2、本发明基于足端正压力的力分配策略能够有效避免机器人在行走过程中支撑腿足端滑动的现象，并使机器人具备较强的加速性能。2. The force distribution strategy based on the positive pressure of the foot end of the present invention can effectively avoid the phenomenon that the foot end of the supporting leg of the robot slides during walking, and enables the robot to have strong acceleration performance.

3、本发明能够提高四足机器人对非结构化环境的适应能力，实现机器人在不平整地面上的行走。3. The invention can improve the adaptability of the quadruped robot to the unstructured environment, and realize the walking of the robot on the uneven ground.

4、本发明能够通过增大位置控制器刚度提高机器人的负载能力。4. The present invention can improve the load capacity of the robot by increasing the stiffness of the position controller.

5、本发明结构清晰、层次分明，具有较好的理论价值和工程意义。5. The present invention has a clear structure and clear layers, and has good theoretical value and engineering significance.

附图说明Description of drawings

图1是本发明在具体应用实例中四足机器人平台的结构示意图。Fig. 1 is a schematic structural diagram of a quadruped robot platform in a specific application example of the present invention.

图2是本发明在具体应用实例中径向平面投影的示意图。Fig. 2 is a schematic diagram of radial plane projection in a specific application example of the present invention.

图3是本发明在具体应用实例中平面虚拟伸缩腿结构的解耦控制框图。Fig. 3 is a decoupling control block diagram of the planar virtual telescopic leg structure in a specific application example of the present invention.

图4是本发明方法的流程示意图。Fig. 4 is a schematic flow chart of the method of the present invention.

具体实施方式detailed description

以下将结合说明书附图和具体实施例对本发明做进一步详细说明。The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

本发明的方法主要适用于四足机器人。如图1所示，为四足机器人的系统结构，由本体和四条腿组成，每条腿包含一个髋部侧向转动关节和三个前向转动关节(髋前关节、膝关节和踝关节)。每个关节均由液压驱动器驱动，并装有用于检测驱动器长度和驱动力的位移传感器和力传感器。每条腿足端均装设三维力传感器，用于检测机器人足端与环境的接触力信息，并通过IMU检测机器人在惯性系下的位姿信息。上述四足机器人具有如下机械结构特性：(1)机器人重心接近本体几何中心；(2)机器人本体的质量远大于腿部的质量。The method of the present invention is mainly applicable to quadruped robots. As shown in Figure 1, it is the system structure of a quadruped robot, which consists of a body and four legs, each leg contains a hip lateral rotation joint and three forward rotation joints (hip front joint, knee joint and ankle joint) . Each joint is driven by a hydraulic drive and is equipped with displacement and force sensors to detect the drive length and driving force. The foot end of each leg is equipped with a three-dimensional force sensor, which is used to detect the contact force information between the foot end of the robot and the environment, and detect the pose information of the robot in the inertial system through the IMU. The above-mentioned quadruped robot has the following mechanical structural characteristics: (1) the center of gravity of the robot is close to the geometric center of the body; (2) the mass of the robot body is much greater than that of the legs.

在本发明中，以TROT步态双足支撑相为例，说明四足机器人双足支撑时的力位混合控制方法。本发明的四足机器人双足支撑相位力位混合控制方法，它采用力位混合控制的方法对机器人本体位姿进行解耦控制，以增强机器人对非结构地面和负载变化的适应能力。本发明提出了基于足端正压力的力分配方法，该方法减小了支撑腿足端发生滑动的可能性，并提高了机器人的加速性能。In the present invention, taking the biped support phase of the TROT gait as an example, the force-position mixing control method for the biped support of the quadruped robot is described. The quadruped robot biped support phase force-position mixed control method of the present invention adopts the force-position mixed control method to decouple the pose of the robot body to enhance the adaptability of the robot to unstructured ground and load changes. The present invention proposes a force distribution method based on the positive pressure of the foot end, which reduces the possibility of sliding of the foot end of the supporting leg and improves the acceleration performance of the robot.

如图4所示，本发明的四足机器人双足支撑相位力位混合控制方法，步骤为：As shown in Figure 4, the quadruped robot biped support phase force position hybrid control method of the present invention, the steps are:

由于四足机器人自由度繁多，建立机器人整体的运动学和动力学方程较为繁杂，因此需将四足机器人进行合理简化：trot步态为两个对脚支撑状态的重复切换过程，因此本发明将机器人的整体运动投影到由对脚支撑并垂直于水平面的径向平面，以及通过质心并垂直于径向平面的法向平面。Due to the many degrees of freedom of the quadruped robot, it is relatively complicated to establish the kinematics and dynamic equations of the robot as a whole, so it is necessary to rationally simplify the quadruped robot: the trot gait is a repeated switching process of two pairs of support states, so the present invention will The overall motion of the robot is projected onto a radial plane supported by the pair of feet and perpendicular to the horizontal plane, and a normal plane passing through the center of mass and perpendicular to the radial plane.

S2：建立控制模型；S2: Establish a control model;

在具体应用实例中，机器人沿径向平面上的运动可简化为平面七连杆模型，在法向平面上的运动则可简化为线性倒立摆模型。In a specific application example, the motion of the robot along the radial plane can be simplified to a plane seven-link model, and the motion on the normal plane can be simplified to a linear inverted pendulum model.

其中，平面七连杆模型单腿具有三个前向转动关节，由于单腿的运动学及动力学特性可等效为伸缩腿结构模型，因此又将平面七连杆模型简化为具有伸缩腿结构的平面虚拟模型。平面虚拟伸缩腿模型的控制目标为质心高度、本体俯仰角以及水平位移。Among them, the single leg of the planar seven-link model has three forward rotating joints. Since the kinematics and dynamics of the single leg can be equivalent to a telescopic leg structure model, the planar seven-link model is simplified to have a telescopic leg structure. plane virtual model. The control objectives of the planar virtual telescopic leg model are the height of the center of mass, the pitch angle of the body and the horizontal displacement.

S3：根据控制模型进行混合控制；建立平面虚拟伸缩腿模型的运动学方程并通过牛顿-欧拉方法建立其动力学方程，对于质心高度和本体俯仰角通过位置伺服控制方法，对虚拟伸缩腿平面模型的水平位移采用力控制方式。即：S3: Carry out hybrid control according to the control model; establish the kinematic equation of the plane virtual telescopic leg model and establish its dynamic equation through the Newton-Euler method, use the position servo control method for the height of the center of mass and the pitch angle of the body, and control the plane of the virtual telescopic leg The horizontal displacement of the model is controlled by force. which is:

首先，建立该模型的运动学方程并通过牛顿-欧拉方法建立其动力学方程。由于位置控制方法具有很大刚度，因此对于本体质心高度及俯仰角通过位置伺服控制方法来提高机器人的负重能力。First, the kinematic equation of the model is established and its dynamic equation is established by the Newton-Euler method. Because the position control method has great rigidity, the robot's load-bearing capacity is improved by the position servo control method for the height of the center of mass and the pitch angle of the body.

由于平面虚拟伸缩腿模型共含有4个驱动关节，而整体控制目标只有三个，因此需添加一个约束条件才能实现对模型的整体控制。Since the planar virtual telescopic leg model contains a total of four driving joints, but the overall control objectives are only three, it is necessary to add a constraint condition to realize the overall control of the model.

由于支撑腿水平方向足端力受摩擦锥的自然约束，整体结构亦受ZMP点的动力学约束，为了使虚拟伸缩腿模型避免足端滑动，需在水平方向足端力添加一个约束方程，即为水平方向的力分配问题。因此，对虚拟伸缩腿平面模型的水平位移采用力控制方式。Since the force at the foot end in the horizontal direction of the supporting leg is naturally constrained by the friction cone, and the overall structure is also subject to the dynamic constraints of the ZMP point, in order to prevent the virtual telescopic leg model from sliding at the foot end, a constraint equation needs to be added to the force at the foot end in the horizontal direction, namely For the force distribution problem in the horizontal direction. Therefore, the force control method is adopted for the horizontal displacement of the virtual telescopic leg plane model.

在具体应用实例中，四足机器人在径向平面内的投影可以简化为一个七连杆结构，如图2所示(左前腿和右后腿作为支撑腿)。其中，(x,z)为本体质心在惯性坐标系下的位置，m为本体质量，L₀为机器人本体长度，d为本体的宽度，l₁,l₂,l₃分别为腿部踝关节、膝关节、髋关节的长度，θ₁,θ₂,θ₃为左前腿踝关节角、膝关节角、髋关节角，θ₄,θ₅,θ₆为右后腿踝关节角、膝关节角、髋关节角，为本体俯仰角，O、A、B、C、D分别为世界系原点(即两支撑腿足端连线中点)、右后腿髋部与本体连接点、左前腿髋部与本体连接点、右后腿足端点、左前腿足端点，F_x1,F_z1,F_x2,F_z2分别为C点和D点受到的接触力。由于腿部质量相对于本体质量很小，在建模时忽略腿部质量与惯量。In a specific application example, the projection of the quadruped robot in the radial plane can be simplified as a seven-link structure, as shown in Figure 2 (the left front leg and the right rear leg are used as supporting legs). Among them, (x, z) is the position of the center of mass of the body in the inertial coordinate system, m is the mass of the body, L ₀ is the length of the robot body, d is the width of the body, l ₁ , l ₂ , l ₃ are the ankle joints of the legs , the length of the knee joint and the hip joint, θ ₁ , θ ₂ , and θ ₃ are the ankle joint angles, knee joint angles, and hip joint angles of the left foreleg, θ ₄ , θ ₅ , and θ ₆ are the ankle joint angles, knee joint angles of the right hind leg angle, hip angle, is the pitch angle of the body, O, A, B, C, and D are the origin of the world system (ie, the midpoint of the line connecting the feet of the two supporting legs), the connection point between the hip of the right rear leg and the body, and the connection point between the hip of the left front leg and the body , the end point of the right hind leg, the end point of the left front leg, F _x1 , F _z1 , F _x2 , F _z2 are the contact forces on point C and point D respectively. Since the mass of the legs is very small relative to the mass of the body, the mass and inertia of the legs are ignored during modeling.

由图2中左边的示意图可以得到腿长||AC||和||BD||分别为：From the schematic diagram on the left in Figure 2, the leg lengths ||AC|| and ||BD|| are respectively:

$| | | | A A C C | | | | = = \sqrt{{(({l l}_{11} + + {l l}_{22} c c o o s the s ((π π - - {θ θ}_{44})) + + {l l}_{33} cos cos (({θ θ}_{55} - - {θ θ}_{44}))))}^{22} + + {(({l l}_{22} s the s i i n no ((π π - - {θ θ}_{44})) + + {l l}_{33} s the s i i n no (({θ θ}_{55} - - {θ θ}_{44}))))}^{22}} - - - - - - ((11))$

$| | | | B B D D. | | | | = = \sqrt{{(({l l}_{11} + + {l l}_{22} c c o o s the s ((π π - - {θ θ}_{11})) + + {l l}_{33} cos cos (({θ θ}_{22} - - {θ θ}_{11}))))}^{22} + + {(({l l}_{22} s the s i i n no ((π π - - {θ θ}_{11})) + + {l l}_{33} s the s i i n no (({θ θ}_{22} - - {θ θ}_{11}))))}^{22}} - - - - - - ((22))$

由式(1)～(2)可知，腿长||AC||和||BD||分别和关节角度θ₄,θ₅和θ₁,θ₂相对应。因此，从运动学上来说，图2中左图示意的关节腿可简化为右图所示的虚拟伸缩腿模型，其中α₃,α₄分别为前后腿入射角。From equations (1) to (2), we can see that leg lengths ||AC|| and ||BD|| correspond to joint angles θ ₄ , θ ₅ and θ ₁ , θ ₂ respectively. Therefore, in terms of kinematics, the articulated leg shown on the left in Figure 2 can be simplified to the virtual telescopic leg model shown on the right, where α ₃ and α ₄ are the incident angles of the front and rear legs, respectively.

由图2可建立平面虚拟伸缩腿结构运动学模型如下：From Figure 2, the structural kinematics model of the plane virtual telescopic leg can be established as follows:

$y the y = = \frac{{L L}_{11} s the s i i n no (({α α}_{33})) + + {L L}_{22} s the s i i n no (({α α}_{44}))}{22} - - - - - - ((44))$

$x x = = \frac{{L L}_{11} c c o o s the s (({α α}_{33})) + + {L L}_{22} c c o o s the s (({α α}_{44}))}{22} - - - - - - ((55))$

同样可得其动力学方程如下：The kinetic equation can also be obtained as follows:

${f f}_{x x 11} + + {f f}_{x x 22} = = m m \overset{\cdot\cdot \cdot\cdot}{x x} - - - - - - ((66))$

${f f}_{z z 11} + + {f f}_{z z 22} = = m m g g + + m m \overset{\cdot\cdot \cdot\cdot}{z z} - - - - - - ((77))$

对髋关节力矩则有：For hip joint moments:

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

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

对于动力学方程(6)～(10)，图2中右图所示的伸缩腿模型和图2中左图所示的关节腿模型也是等效的。因此，从运动学和动力学方面均可将图2的左图中所示的平面七连杆模型简化为图2的右图中所示的虚拟伸缩腿模型。For the dynamic equations (6) to (10), the telescopic leg model shown in the right figure in Fig. 2 and the articulated leg model shown in the left figure in Fig. 2 are also equivalent. Therefore, the plane seven-link model shown in the left diagram of Figure 2 can be simplified to the virtual telescopic leg model shown in the right diagram of Figure 2 from the aspects of kinematics and dynamics.

基于上述本发明的方法，在具体应用实例中，需要对位姿解耦控制；Based on the above-mentioned method of the present invention, in specific application examples, it is necessary to decouple the control of pose;

对于入射角α₃,α₄来说有：For incident angles α ₃ , α ₄ there are:

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

故而有：Therefore there are:

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

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

对于前向位移来说，由方程(5)可知存在奇异点α₃＝α₄＝π/2，因此不适合用腿长来控制前向位移。但对于方程(3)、(4)来说不存在奇异点，因此，可以通过腿长l₁,l₂来控制本体俯仰角和质心高度，为简便起见采取通过控制来简介控制本体俯仰角的方法，并令 For the forward displacement, it can be known from the equation (5) that there is a singular point α ₃ =α ₄ =π/2, so it is not suitable to use the leg length to control the forward displacement. But there is no singular point for equations (3) and (4), therefore, the body pitch angle and the height of the center of mass can be controlled by the leg lengths l ₁ , l ₂ , for simplicity, the control To briefly control the pitch angle of the body method, and make

将(3)、(4)写为矩阵形式则为：Writing (3) and (4) in matrix form is:

$[\begin{matrix} {S S}_{y the y} \\ y the y \end{matrix}] = = {J J}_{11} [\begin{matrix} {L L}_{11} \\ {L L}_{22} \end{matrix}] - - - - - - ((1414))$

其中， $J_{1} = [\begin{matrix} \frac{s i n (α_{3})}{2 L_{0}} & - \frac{s i n (α_{4})}{2 L_{0}} \\ \frac{s i n (α_{3})}{2} & \frac{\sin (α_{4})}{2} \end{matrix}] .$ in, $J_{1} = [\begin{matrix} \frac{the s i no (α_{3})}{2 L_{0}} & - \frac{the s i no (α_{4})}{2 L_{0}} \\ \frac{the s i no (α_{3})}{2} & \frac{\sin (α_{4})}{2} \end{matrix}] .$

由式(3)可知J₁非奇异，并且前向位移对本体俯仰角和质心高度的影响可通过雅克比矩阵J₁消除，因此可认为是运动学近似解耦的。It can be seen from formula (3) that J ₁ is non-singular, and the influence of forward displacement on body pitch angle and centroid height can be eliminated by Jacobian matrix J ₁ , so it can be considered as kinematics approximate decoupling.

由于平面伸缩腿结构的控制目标有水平位移、质心高度和本体俯仰角，其中质心高度和本体俯仰角通过腿长来进行控制，前向位移可通过足端力控制来实现。在动力学方程中仅与前向位置相关的只有式(6)，需要添加一个水平力分配方程才能实现对足端力的计算，进而实现对水平位移的控制。为了使足端发生滑动的可能性最小，采用如下所述力分配方法，并采用双环控制方法来进行控制。Since the control objectives of the planar telescopic leg structure include horizontal displacement, height of the center of mass and pitch angle of the body, the height of the center of mass and the pitch angle of the body are controlled by the length of the leg, and the forward displacement can be realized by controlling the force of the foot. In the dynamic equation, only formula (6) is related to the forward position. It is necessary to add a horizontal force distribution equation to realize the calculation of the foot force, and then realize the control of the horizontal displacement. In order to minimize the possibility of foot slippage, the force distribution method described below is used, and the control method is adopted by the double loop control method.

定义足端发生滑动的可能性为SCI(滑动约束指标)：Define the possibility of sliding at the foot end as SCI (Sliding Constraint Index):

$S S C C I I = = m m a a x x ((\frac{| | {F f}_{x x} | |}{| | {F f}_{z z} | |})) - - - - - - ((1515))$

其中F_x，F_z为机器人在步行过程中受到的地面反力。SCI值越小，足端发生滑动的可能性越小。为使SCI尽可能小，则可采用力分配方式为：Among them, F _x and F _z are the ground reaction force that the robot receives during walking. The smaller the SCI value, the less likely the tip of the foot will slip. In order to make the SCI as small as possible, the force distribution method can be used as follows:

f_x1d/f_x2d＝f_z1/f_z2(16)f _x1d /f _x2d =f _z1 /f _z2 (16)

联立(6)、(16)可得期望足端力为：By combining (6) and (16), the expected foot end force can be obtained as:

$[\begin{matrix} {f f}_{x x 11 d d} \\ {f f}_{x x 22 d d} \end{matrix}] = = [\begin{matrix} {f f}_{z z 11 r r} / / (({f f}_{z z 11 r r} + + {f f}_{z z 22 r r})) \\ {f f}_{z z 22 r r} / / (({f f}_{z z 11 r r} + + {f f}_{z z 22 r r})) \end{matrix}] m m \overset{\cdot\cdot \cdot\cdot}{x x} - - - - - - ((1717))$

由式(17)计算水平方向足端期望力时是和质心高度和本体俯仰角完全无关的，因此可认为外环，即水平足端力和另两个控制目标是动力学解耦的。The calculation of the expected horizontal foot force by formula (17) has nothing to do with the height of the center of mass and the pitch angle of the body. Therefore, it can be considered that the outer ring, that is, the horizontal foot force, is dynamically decoupled from the other two control objectives.

对于水平方向足端力须通过关节力矩来进行控制，由(9)、(10)可得：The force of the foot end in the horizontal direction must be controlled by the joint torque, from (9) and (10) we can get:

$[\begin{matrix} {τ τ}_{11} \\ {τ τ}_{22} \end{matrix}] = = {J J}_{22} [\begin{matrix} {f f}_{x x 11} \\ {f f}_{x x 22} \end{matrix}] + + G G - - - - - - ((1818))$

其中， $J_{2} = [\begin{matrix} L_{1} s i n (α_{3}) & 0 \\ 0 & L_{2} \sin (α_{4}) \end{matrix}], G = [\begin{matrix} - L_{1} c o s (α_{3}) f_{z 1} \\ - L_{2} c o s (α_{4}) f_{z 2} \end{matrix}] .$ in, $J_{2} = [\begin{matrix} L_{1} the s i no (α_{3}) & 0 \\ 0 & L_{2} \sin (α_{4}) \end{matrix}], G = [\begin{matrix} - L_{1} c o the s (α_{3}) f_{z 1} \\ - L_{2} c o the s (α_{4}) f_{z 2} \end{matrix}] .$

由式(3)可知J₂非奇异，并且通过J₂和补偿项G消除了质心高度和俯仰角对关节力矩的影响，因此可认为内环是动力学近似解耦的。It can be seen from formula (3) that J ₂ is non-singular, and the influence of centroid height and pitch angle on joint torque is eliminated through J ₂ and the compensation term G, so the inner ring can be considered to be dynamically approximately decoupled.

采用上述所示方法，即通过腿长来控制本体质心高度和俯仰角，通过控制水平足端力来控制本体水平位移，可实现对本体位姿的近似解耦控制。Using the method shown above, that is, the height of the center of mass and pitch angle of the body is controlled by the length of the legs, and the horizontal displacement of the body is controlled by controlling the horizontal foot force, so that the approximate decoupling control of the body pose can be realized.

基于上述本发明的方法，在具体应用实例中，需要进行控制器设计；Based on the above-mentioned method of the present invention, in specific application examples, controller design is required;

1)力控制器设计；1) Force controller design;

基于公式(17)，设计经典PID控制器作为力外环控制器，具体形式如下：Based on formula (17), the classic PID controller is designed as the force outer loop controller, the specific form is as follows:

$[\begin{matrix} {f f}^{' '}_{x x 11 d d} \\ {f f}^{' '}_{x x 22 d d} \end{matrix}] = = m m [\begin{matrix} {f f}_{z z 11 r r} / / (({f f}_{z z 11 r r} + + {f f}_{z z 22 r r})) \\ {f f}_{z z 22 r r} / / (({f f}_{z z 11 r r} + + {f f}_{z z 22 r r})) \end{matrix}] [[{Kp Kp}_{x x} (({x x}_{d d} - - x x)) + + {Kd k}_{x x} (({\overset{\cdot &Center Dot;}{x x}}_{d d} - - \overset{\cdot &Center Dot;}{x x})) + + {Ki Ki}_{x x} {&Integral; &Integral;}_{00}^{t t} (({x x}_{d d} - - x x)) d d τ τ]] - - - - - - ((1919))$

其中，f'_x1d,f'_x2d为期望的足端接触力，x_d为期望的x方向位置，f_z1r,f_z2r为竖直方向足端力。Among them, f' _x1d , f' _x2d are the expected foot contact force, x _d is the expected position in the x direction, f _z1r , f _z2r are the vertical foot force.

由于机器人在运动中要保持平衡受到ZMP点的约束，对于虚拟伸缩腿结构来说，为维持机器人的双足支撑状态，ZMP点需限制在双足连线的线段上。在不考虑质心高度和俯仰角动态的情况下，该约束形式如下：Since the robot needs to keep its balance in motion, it is constrained by the ZMP point. For the virtual telescopic leg structure, in order to maintain the bipedal support state of the robot, the ZMP point needs to be limited to the line segment of the bipedal line. Without considering the height of the center of mass and the dynamics of the pitch angle, the constraint form is as follows:

${k k}_{11} \frac{{x x}_{11} - - x x}{z z} g g \leq \leq \frac{{Kp Kp}_{x x} (({x x}_{d d} - - x x)) + + {Kd k}_{x x} (({\overset{\cdot \cdot}{x x}}_{d d} - - \overset{\cdot &Center Dot;}{x x})) + + {Ki Ki}_{x x} {&Integral; &Integral;}_{00}^{t t} (({x x}_{d d} - - x x)) d d τ τ}{m m} \leq \leq {k k}_{11} \frac{x x - - {x x}_{22}}{z z} g g - - - - - - ((2020))$

其中，0<k₁<1，x₁,x₂分别为D点和C点水平位置。Wherein, 0<k ₁ <1, x ₁ and x ₂ are the horizontal positions of point D and point C respectively.

另外，足端受摩擦锥的自然约束条件：f'_x1d<μf_z1r、f'_x1d<μf_z1d，μ为摩擦系数，控制器变为：In addition, the foot is subject to the natural constraints of the friction cone: f' _x1d < _{μf z1r} , f' _x1d < _{μf z1d} , μ is the friction coefficient, and the controller becomes:

${f f}_{x x 11 d d} = = \{\begin{matrix} {f f}^{' '}_{x x 11 d d} & - - {f f}_{x x 11__m m a a x x} < < {f f}^{' '}_{x x 11 d d} < < {f f}_{x x 11__m m a a x x},, {f f}_{x x 11__m m a a x x} = = {k k}_{22} {μf μf}_{z z 11 r r} \\ - - {f f}_{x x 11__m m a a x x} & {f f}^{' '}_{x x 11 d d} \leq \leq - - {f f}_{x x 11__m m a a x x},, {f f}_{x x 11__m m a a x x} = = {k k}_{22} {μf μf}_{z z 11 r r} \\ {f f}_{x x 11__m m a a x x} & {f f}^{' '}_{x x 11 d d} &GreaterEqual; &Greater Equal; {f f}_{x x 11__m m a a x x},, {f f}_{x x 11__m m a a x x} = = {k k}_{22} {μf μf}_{z z 11 r r} \end{matrix} - - - - - - ((21 twenty one))$

${f f}_{x x 22 d d} = = \{\begin{matrix} {f f}^{' '}_{x x 22 d d} & - - {f f}_{x x 22__m m a a x x} < < {f f}^{' '}_{x x 22 d d} < < {f f}_{x x 22__m m a a x x},, {f f}_{x x 22__m m a a x x} = = {k k}_{22} {μf μf}_{z z 22 r r} \\ - - {f f}_{x x 22__m m a a x x} & {f f}^{' '}_{x x 22 d d} \leq \leq - - {f f}_{x x 22__m m a a x x},, {f f}_{x x 22__m m a a x x} = = {k k}_{22} {μf μf}_{z z 22 r r} \\ {f f}_{x x 22__m m a a x x} & {f f}^{' '}_{x x 22 d d} &GreaterEqual; &Greater Equal; {f f}_{x x 22__m m a a x x},, {f f}_{x x 22__m m a a x x} = = {k k}_{22} {μf μf}_{z z 22 r r} \end{matrix} - - - - - - ((22 twenty two))$

通过调节参数Kp_x,Kd_x,Ki_x可以保证在控制前向位移的同时，避免机器人在运动过程中出现翻转或足端滑动的现象。By adjusting the parameters Kp _x , Kd _x , Ki _x , it can ensure that the forward displacement is controlled while avoiding the phenomenon of the robot turning over or the feet sliding during the movement.

基于公式(18)，采用经典的PID控制器即可使支撑腿足端接触力准确跟踪期望力。Based on formula (18), the contact force at the foot end of the supporting leg can accurately track the desired force by using a classic PID controller.

2)位置控制器设计；2) Position controller design;

基于公式(14)，可得逆运动学方程：Based on formula (14), the inverse kinematics equation can be obtained:

$[\begin{matrix} {L L}_{11} \\ {L L}_{22} \end{matrix}] = = {J J}_{11}^{- - 11} [\begin{matrix} {S S}_{y the y} \\ y the y \end{matrix}] - - - - - - ((23 twenty three))$

采用位置伺服控制器：Using a position servo controller:

$f f = = {Kp Kp}_{L L} (({L L}_{d d} - - L L)) + + {Kv Kv}_{L L} (({\overset{\cdot &Center Dot;}{L L}}_{d d} - - \overset{\cdot \cdot}{L L})) - - - - - - ((24 twenty four))$

其中，f为作动器上的推力。通过调整控制器参数Kp_L,Kv_L即可实现对腿长的控制，并可使位置控制器具有很大的刚度。但由于Trot步态中在切换腿或在非规则地面运行时经常出现位置估计不连续现象，对于实际值和期望值之间的小偏差情况一般经过驱动力限幅就可保持机器人平衡运行，但Trot步态很容易发生出现实际腿长和期望腿长的差异很大，这时就需要通过添加合适的过渡过程使期望腿长连续可导。在添加过渡过程时需保证足端点接触地面，在本体俯仰角保持不变的情况下可得对腿长变化的约束条件如下：Among them, f is the thrust on the actuator. By adjusting the controller parameters Kp _L and Kv _L , the control of the leg length can be realized, and the position controller can have great stiffness. However, due to the discontinuity of position estimation often occurs when switching legs or running on irregular ground in Trot gait, for small deviations between the actual value and the expected value, the robot can generally be kept in balance after the driving force is limited, but Trot The gait is easy to cause a large difference between the actual leg length and the expected leg length. At this time, it is necessary to add a suitable transition process to make the expected leg length continuously derivable. When adding the transition process, it is necessary to ensure that the end point of the foot touches the ground. When the pitch angle of the body remains unchanged, the constraints on the change of the leg length can be obtained as follows:

由式(7)可得：From formula (7) can get:

$m m \overset{\cdot\cdot \cdot\cdot}{z z} = = {f f}_{z z 11} + + {f f}_{z z 22} - - m m g g > > - - m m g g - - - - - - ((2525))$

又有：And again:

$\begin{matrix} {a a}_{L L 11} = = \overset{\cdot\cdot \cdot\cdot}{z z} / / sin sin (({α α}_{33})) \\ {a a}_{L L 22} = = \overset{\cdot\cdot \cdot\cdot}{z z} / / sin sin (({α α}_{44})) \end{matrix} - - - - - - ((2626))$

故可得对腿长加速度的近似约束条件为：Therefore, the approximate constraints on leg length acceleration can be obtained as:

$\begin{matrix} {a a}_{L L 11} > > - - g g / / sin sin (({α α}_{33})) \\ {a a}_{L L 22} > > - - g g / / sin sin (({α α}_{44})) \end{matrix} - - - - - - ((2727))$

因此，对腿长可添加一个加速度受限的二阶过渡过程如下：Therefore, an acceleration-limited second-order transition process can be added to the leg length as follows:

$\begin{matrix} {a a}^{' '}_{L L 11} = = {Kp Kp}_{a a 11} (({L L}_{11} - - {L L}_{11 r r})) + + {Kd k}_{a a 11} {\overset{\cdot \cdot}{L L}}_{11 r r} \\ {a a}^{' '}_{L L 22} = = {Kp Kp}_{a a 22} (({L L}_{22} - - {L L}_{22 r r})) + + {Kd k}_{a a 22} {\overset{\cdot &Center Dot;}{L L}}_{22 r r} \end{matrix} - - - - - - ((2828))$

对加速度添加限幅后式(29)变为：After adding a limit to the acceleration, equation (29) becomes:

$\begin{matrix} {a a}_{L L 11} = = \{\begin{matrix} {a a}^{' '}_{L L 11} & {a a}^{' '}_{L L 11} > > {a a}_{L L 11__min min},, {a a}_{L L 11__min min} = = - - {k k}_{22} g g / / sin sin (({α α}_{33})) \\ {a a}_{L L 11__min min} & {a a}^{' '}_{L L 11} \leq \leq {a a}_{L L 11__min min},, {a a}_{L L 11__min min} = = - - {k k}_{22} g g / / sin sin (({α α}_{33})) \end{matrix} \\ {a a}_{L L 22} = = \{\begin{matrix} {a a}^{' '}_{L L 22} & {a a}^{' '}_{L L 22} > > {a a}_{L L 22__min min},, {a a}_{L L 22__min min} = = - - {k k}_{22} g g / / sin sin (({α α}_{44})) \\ {a a}_{L L 22__min min} & {a a}^{' '}_{L L 22} > > {a a}_{L L 22__min min},, {a a}_{L L 22__min min} = = - - {k k}_{22} g g / / sin sin (({α α}_{44})) \end{matrix} \end{matrix} - - - - - - ((2929))$

故腿长期望可给定为：Therefore, the expected leg length can be given as:

$\begin{matrix} {L L}_{11 d d} = = {L L}_{1010} + + \underset{00 ~ ~ t t}{&Integral; &Integral; &Integral; &Integral;} {a a}_{L L 11} d d τ τ d d τ τ \\ {L L}_{22 d d} = = {L L}_{2020} + + \underset{00 ~ ~ t t}{&Integral; &Integral; &Integral; &Integral;} {a a}_{L L 22} d d τ τ d d τ τ \end{matrix} - - - - - - ((3030))$

其中，L₁₀,L₂₀分别为两腿初始腿长。Among them, L ₁₀ and L ₂₀ are the initial leg lengths of the two legs respectively.

如前所述可得平面虚拟伸缩腿模型整体控制框图，如图3所示。As mentioned above, the overall control block diagram of the plane virtual telescopic leg model can be obtained, as shown in Figure 3.

3)七连杆模型关节位置伺服控制器设计；3) Design of the servo controller for the joint position of the seven-link model;

对于七连杆结构的控制器设计和虚拟伸缩腿模型控制器类似，需对髋关节采用力控制方式，对膝关节和踝关节采用位置伺服控制方式。由于单腿前向含有三个关节，相对于腿长控制多了一个自由度，添加三个自由度的目的主要是考虑液压缸的驱动性能，增强单腿的驱动能力或改善系统性能。因此，在以下进行逆运动学解算时，为简化运算过程，添加关节约束如下：The design of the controller for the seven-link structure is similar to that of the virtual telescopic leg model controller, which requires force control for the hip joint and position servo control for the knee and ankle joints. Since the single leg contains three joints in the forward direction, there is one more degree of freedom compared to the leg length control. The purpose of adding three degrees of freedom is mainly to consider the driving performance of the hydraulic cylinder, enhance the driving ability of the single leg or improve the system performance. Therefore, in the following inverse kinematics calculation, in order to simplify the calculation process, the joint constraints are added as follows:

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

由图2中左图所示和式(31)易得：From the left figure in Figure 2 and formula (31), it is easy to get:

${θ θ}_{44 d d} = = {θ θ}_{55 d d} = = a a c c o o s the s ((\frac{{l l}_{22}^{22} + + 44 {l l}_{11}^{22} - - {L L}_{22 d d}^{22}}{44 {l l}_{11} {l l}_{22}})) - - - - - - ((3232))$

${θ θ}_{11 d d} = = {θ θ}_{22 d d} = = a a c c o o s the s ((\frac{{l l}_{22}^{22} + + 44 {l l}_{11}^{22} - - {L L}_{11 d d}^{22}}{44 {l l}_{11} {l l}_{22}})) - - - - - - ((3333))$

对膝关节和踝关节角度采用伺服控制器：Using servo controllers for knee and ankle angles:

$τ τ = = {Kp Kp}_{θ θ} (({θ θ}_{d d} - - θ θ)) + + {Kv Kv}_{θ θ} (({\overset{\cdot &Center Dot;}{θ θ}}_{d d} - - \overset{\cdot &Center Dot;}{θ θ})) - - - - - - ((3434))$

其中，τ为关节驱动力矩。通过调整控制器参数Kp_θ,Kv_θ即可实现对关节角度的跟踪控制。Among them, τ is the joint driving torque. By adjusting the controller parameters Kp _θ and Kv _θ , the tracking control of the joint angle can be realized.

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。The above are only preferred implementations of the present invention, and the protection scope of the present invention is not limited to the above-mentioned embodiments, and all technical solutions under the idea of the present invention belong to the protection scope of the present invention. It should be pointed out that for those skilled in the art, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A method for hybrid control of the phase force position of the two-foot support of a four-foot robot is characterized by comprising the following steps:

s1: projecting the overall motion of the robot onto a radial plane and a normal plane; the radial plane is a plane which supports the feet and is vertical to the horizontal plane; the normal plane is a plane passing through the center of mass of the body and perpendicular to the radial plane;

s2: establishing a control model; the motion of the robot along the radial plane is simplified into a plane seven-connecting-rod model, and the motion on the normal plane is simplified into a linear inverted pendulum model; then simplifying the plane seven-connecting-rod model into a plane virtual telescopic leg model; the control targets of the plane virtual telescopic leg model are the height of a mass center, a pitch angle of the body and horizontal displacement;

s3: performing mixing control according to a control model; establishing a kinematic equation of a plane virtual telescopic leg model and a dynamic equation thereof by a Newton-Euler method, and adopting a force control mode for the horizontal displacement of the virtual telescopic leg plane model by a position servo control method for the height of a mass center and a body pitch angle.

2. The hybrid control method for biped support phase force and position of the quadruped robot as claimed in claim 1, wherein in step S3, the height of the center of mass and the pitch angle of the body are controlled by the leg length, and the horizontal displacement of the body is controlled by controlling the horizontal foot end force, so as to realize the approximate decoupling control of the posture of the body.

3. The biped robot biped support phase force position hybrid control method according to claim 2, characterized in that a classical PID controller is adopted as a force outer loop controller, so that the support leg foot end contact force accurately tracks the expected force.

4. The biped robot biped support phase force position hybrid control method according to claim 2, characterized in that the knee joint and the ankle joint are controlled by a position servo controller to realize the tracking of the joint angle.