CN112650268B - Robot motion control method, device, robot and storage medium - Google Patents

Abstract

The application is suitable for the technical field of robot control, and particularly relates to a motion control method and device for a robot, the robot and a storage medium. The motion control method of the robot comprises the steps that a preset outer ring controller obtains a preset attitude angle deviation value and calculates a zero moment point expected value of the robot, wherein the outer ring controller is an active disturbance rejection controller; and acquiring a zero moment point deviation value by a preset inner ring controller and calculating to obtain a moment compensation value of the tail end joint of the robot. After the output torque compensation value is calculated, the torque compensation value and the torque expected value of the tail end joint of the robot are obtained, the torque deviation value is generated, and the torque deviation value is provided for the tail end joint actuator of the robot so as to control the gait motion of the robot. The application combines the attitude angle feedback value and the zero moment point measurement value to control the gait of the robot, thereby improving the accuracy of the robot gait control.

Description

Robot motion control method, device, robot and storage medium

Technical Field

The present application belongs to the field of robot control technology, and in particular, relates to a robot motion control method, device, robot and storage medium.

Background technique

A key issue in humanoid robot research is to increase walking speed while maintaining walking stability. Reasonable gait control can improve the walking stability of humanoid robots. The existing robot gait control method generally calculates the torque compensation value of the robot's end joint based on the measured robot's posture angle information, and then controls the robot's gait based on the torque compensation value of the end joint and the expected value. The accuracy of the robot's gait control is not high.

Summary of the invention

In view of this, the embodiments of the present application provide a robot motion control method, device, robot and storage medium, which can improve the accuracy of the robot gait control.

A first aspect of an embodiment of the present application provides a motion control method for a robot, comprising:

Acquire a preset attitude angle expected value of the robot and a preset attitude angle feedback value of the robot detected by a preset inertial measurement sensor of the robot, and generate a preset attitude angle deviation value;

A preset outer loop controller obtains the preset attitude angle deviation value and calculates the expected value of the zero moment point of the robot, and the outer loop controller is an anti-disturbance controller;

Obtaining an expected value of the zero moment point of the robot, obtaining a measured value of the zero moment point of the robot, and generating a zero moment point deviation value;

The inner loop controller is preset to obtain the zero torque point deviation value and calculate the torque compensation value of the end joint of the robot;

Obtaining a torque compensation value of the end joint of the robot and an expected torque value obtained through robot gait planning, and generating a torque deviation value;

The torque deviation value is provided to the end joint actuator of the robot to control the gait movement of the robot.

In one possible implementation, the outer loop controller calculates the zero-order value and the first-order value of the preset attitude angle expected value according to the preset attitude angle expected value, and calculates the expected value of the zero-torque point of the robot according to the zero-order value and the first-order value of the preset attitude angle expected value and the preset attitude angle feedback value.

In one possible implementation, the inner-loop controller is a variable PD controller, which receives the expected value of the zero torque point of the robot and the measured value of the zero torque point at the current moment, the first-order output value of the inner-loop controller at the previous moment, and the first-order derivative of the measured value of the zero torque point at the current moment, calculates the zero-order value of the torque compensation value, the first-order value of the torque compensation value, and the second-order value of the torque compensation value of the end joint of the robot at the current moment, and uses the zero-order value of the torque compensation value as the output of the inner-loop controller, and outputs the first-order value of the torque compensation value and the second-order value of the torque compensation value as the input of the inner-loop controller at the next moment.

In one possible implementation, the inner-loop controller is an anti-disturbance controller, and the tracking differentiator of the inner-loop controller receives the expected value of the zero torque point of the robot output by the outer-loop controller, and outputs the zero-order value and the first-order value of the expected value of the zero torque point of the robot; the extended state observer of the inner-loop controller receives the zero torque point measurement value of the robot and outputs the zero torque point feedback value after the extended observation processing; the nonlinear state error feedback device of the inner-loop controller receives the zero-order value and the first-order value of the expected value of the zero torque point of the robot and receives the zero torque point feedback value, and outputs the torque compensation value of the end joint of the robot.

In a possible implementation, the zero-torque point measurement value of the robot is obtained by detecting the contact force between the robot and the ground through a six-dimensional force sensor of the robot, and is calculated based on the contact force between the robot and the ground, or is calculated through robot dynamics.

In a possible implementation, the preset attitude angle is a forward pitch attitude angle, and the torque compensation value of the end joint of the robot is the torque compensation value of the forward joint of the robot to control the movement of the forward joint of the robot.

In a possible implementation, the preset attitude angle is a lateral rolling attitude angle, and the torque compensation value of the end joint of the robot is the torque compensation value of the lateral joint of the robot to control the movement of the lateral joint of the robot.

A second aspect of an embodiment of the present application provides a motion control device for a robot, comprising:

A first acquisition module, used to acquire an expected value of a preset attitude angle of the robot and a feedback value of a preset attitude angle of the robot detected by a preset inertial measurement sensor of the robot, and generate a preset attitude angle deviation value;

A first calculation module is used to preset an outer loop controller to obtain the preset attitude angle deviation value and calculate the expected value of the zero moment point of the robot, wherein the outer loop controller is an anti-disturbance controller;

A second acquisition module is used to acquire an expected value of a zero moment point of the robot, acquire a measured value of a zero moment point of the robot, and generate a zero moment point deviation value;

A second calculation module is used to preset an inner loop controller to obtain the zero torque point deviation value and calculate the torque compensation value of the end joint of the robot;

A third acquisition module is used to acquire the torque compensation value of the end joint of the robot and the expected torque value obtained through the robot gait planning, and generate a torque deviation value;

The control module is used to provide the torque deviation value to the end joint actuator of the robot to control the gait movement of the robot.

In a possible implementation manner, the first calculation module is specifically configured to:

The outer loop controller calculates the zero-order value and the first-order value of the preset attitude angle expected value according to the preset attitude angle expected value, and calculates the zero-torque point expected value of the robot according to the zero-order value and the first-order value of the preset attitude angle expected value and the preset attitude angle feedback value.

In one possible implementation, the inner loop controller is a variable PD controller, and the second calculation module is specifically used for: the inner loop controller receives the expected value of the zero torque point of the robot and the zero torque point measurement value at the current moment, the first-order output value of the inner loop controller at the previous moment, and the first-order derivative of the zero torque point measurement value at the current moment, calculates the zero-order value of the torque compensation value, the first-order value of the torque compensation value, and the second-order value of the torque compensation value of the end joint of the robot at the current moment, and uses the zero-order value of the torque compensation value as the output of the inner loop controller, and outputs the first-order value of the torque compensation value and the second-order value of the torque compensation value as the input of the inner loop controller at the next moment.

In one possible implementation, the inner loop controller is an anti-disturbance controller, and the second calculation module is specifically used for: the tracking differentiator of the inner loop controller receives the expected value of the zero torque point of the robot output by the outer loop controller, and outputs the zero-order value and the first-order value of the expected value of the zero torque point of the robot; the extended state observer of the inner loop controller receives the zero torque point measurement value of the robot and outputs the zero torque point feedback value after the extended observation processing; the nonlinear state error feedback device of the inner loop controller receives the zero-order value and the first-order value of the expected value of the zero torque point of the robot and receives the zero torque point feedback value, and outputs the torque compensation value of the end joint of the robot.

In a possible implementation, the zero-torque point measurement value of the robot is obtained by detecting the contact force between the robot and the ground through a six-dimensional force sensor of the robot, and is calculated based on the contact force between the robot and the ground, or is calculated through robot dynamics.

In a possible implementation, the preset attitude angle is a forward pitch attitude angle, and the torque compensation value of the end joint of the robot is the torque compensation value of the forward joint of the robot to control the movement of the forward joint of the robot.

In a possible implementation, the preset attitude angle is a lateral rolling attitude angle, and the torque compensation value of the end joint of the robot is the torque compensation value of the lateral joint of the robot to control the movement of the lateral joint of the robot.

A third aspect of an embodiment of the present application provides a robot, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the robot motion control method as described in the first aspect above is implemented.

A fourth aspect of an embodiment of the present application provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the robot motion control method as described in the first aspect above is implemented.

A fifth aspect of an embodiment of the present application provides a computer program product. When the computer program product runs on a terminal device, the terminal device executes the robot motion control method described in any one of the above-mentioned first aspects.

Compared with the prior art, the embodiments of the present application have the following beneficial effects: obtaining the expected value of the preset attitude angle of the robot and the preset attitude angle feedback value of the robot detected by the preset inertial measurement sensor of the robot, and generating the preset attitude angle deviation value; the preset outer loop controller obtains the preset attitude angle deviation value and calculates the expected value of the zero torque point of the robot, and the outer loop controller is an anti-disturbance controller; the expected value of the zero torque point of the robot is obtained, and the zero torque point measurement value of the robot is obtained, and the zero torque point deviation value is generated; the preset inner loop controller obtains the zero torque point deviation value and calculates the torque compensation value of the end joint of the robot. That is, the present application calculates the compensation value of the torque of the end joint of the robot by the preset attitude angle feedback value and the zero torque point measurement value, and the zero torque point is an important indicator of the stable walking of the robot. Therefore, the present application improves the accuracy of the calculated torque compensation value relative to the compensation value of the torque of the end joint of the robot calculated only by the attitude angle feedback value. At the same time, the use of the anti-disturbance controller to calculate the expected value of the zero torque point of the robot body can compensate for the disturbance error of the preset attitude angle measurement value, and improve the accuracy of the output zero torque point expected value. After calculating the torque compensation value, the expected torque value of the robot's end joint is obtained, and the torque deviation value is generated. The torque deviation value is provided to the robot's end joint actuator to control the robot's gait movement, thereby improving the accuracy of the robot's gait control.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG1 is a schematic diagram of an implementation flow of a motion control method for a robot provided in an embodiment of the present application;

FIG2 is a schematic diagram of a coordinate system of a robot provided in an embodiment of the present application;

FIG3 is a schematic diagram of a motion control method for a robot provided by an embodiment of the present application;

FIG4 is a flow chart of an active disturbance rejection control algorithm provided in an embodiment of the present application;

FIG5 is a specific flow chart of a motion control method for a robot provided by an embodiment of the present application;

FIG6 is a specific flow chart of a motion control method for a robot provided by another embodiment of the present application;

FIG7 is a schematic diagram of a motion control device for a robot provided in an embodiment of the present application;

FIG8 is a schematic diagram of the structure of the robot provided in an embodiment of the present application.

Detailed ways

In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

In order to illustrate the technical solution described in this application, a specific embodiment is provided below for illustration.

It should be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of described features, integers, steps, operations, elements and/or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and/or groups thereof.

It should also be understood that the terms used in this application specification are only for the purpose of describing specific embodiments and are not intended to limit the application. As used in this application specification and the appended claims, unless the context clearly indicates otherwise, the singular forms "a", "an" and "the" are intended to include plural forms.

It should be further understood that the term “and/or” used in the specification and appended claims refers to any combination and all possible combinations of one or more of the associated listed items, and includes these combinations.

As used in this specification and the appended claims, the term "if" may be interpreted as "when" or "upon" or "in response to determining" or "in response to detecting," depending on the context. Similarly, the phrases "if it is determined" or "if [described condition or event] is detected" may be interpreted as meaning "upon determination" or "in response to determining" or "upon detection of [described condition or event]" or "in response to detecting [described condition or event]," depending on the context.

In addition, in the description of the present application, the terms "first", "second", "third", etc. are only used to distinguish the description and cannot be understood as indicating or implying relative importance.

The existing robot motion control method generally calculates the torque compensation value of the robot's foot joint based on the measured robot posture angle information, and then plans the robot's gait based on the torque compensation value of the foot joint and the expected value. The accuracy of gait planning is not high.

To this end, the present application provides a robot motion control method that can improve the accuracy of the robot's gait planning.

The following is an exemplary description of the robot motion control method provided in the present application.

Please refer to FIG. 1 , a robot motion control method provided by an embodiment of the present application includes:

S101: Acquire a preset attitude angle expected value of a robot and a preset attitude angle feedback value of the robot detected by a preset inertial measurement sensor of the robot, and generate a preset attitude angle deviation value.

Among them, the preset attitude angle is the forward pitch attitude angle or the lateral roll attitude angle, as shown in Figure 2. In the robot coordinate system, the forward pitch attitude angle is the offset angle of the robot in the front-to-back direction, that is, the offset angle of the robot on the xoz plane. The lateral roll attitude angle is the offset angle of the robot in the left-right direction, that is, the offset angle of the robot on the yoz plane. The expected value of the preset attitude angle of the robot is set according to the state of the robot. For example, if the robot needs to stand on the ground with both feet, the expected value of the preset attitude angle can be 0. The preset attitude angle deviation value is the difference between the preset attitude angle expected value and the preset attitude angle feedback value.

S102: Preset an outer loop controller to obtain the preset attitude angle deviation value and calculate the expected value of the zero torque point of the robot, and the outer loop controller is an anti-disturbance control controller.

Wherein, the expected value of the zero moment point is a coordinate value, which can be the y-axis coordinate value in the robot coordinate system shown in FIG2, or the x-axis coordinate value. If the preset attitude angle is the forward pitch attitude angle, the zero moment point is the x-axis coordinate value, and if the preset attitude angle is the lateral roll attitude angle, the zero moment point is the y-axis coordinate value. As shown in FIG3, the preset attitude angle deviation value is calculated according to the preset attitude angle expected value _θd and the preset attitude angle measured value θ, and the preset attitude angle deviation value is input into the outer loop controller, and the outer loop controller outputs the robot's zero moment point expected value _pd . Active Disturbance Rejection Control (ADRC) is a controller using the ADRC algorithm.

In a possible implementation, a preset attitude angle deviation value is input into an ADRC algorithm model of an outer loop controller, and an expected value of the zero torque point of the robot is output.

In another possible implementation, the process of the outer loop controller calculating the expected value of the zero torque point of the robot according to the preset attitude angle deviation value is as follows: the outer loop controller calculates the zero-order value and the first-order value of the preset attitude angle expected value according to the preset attitude angle expected value, and calculates the expected value of the zero torque point of the robot according to the zero-order value and the first-order value of the preset attitude angle expected value and the preset attitude angle feedback value. Specifically, the tracking differentiator of the outer loop controller receives the preset attitude angle expected value and outputs the zero-order value and the first-order value of the preset attitude angle expected value of the robot; the extended state observer of the outer loop controller receives the preset attitude angle feedback value of the robot and outputs the preset attitude angle observation value after extended observation processing, wherein the extended observation processing includes but is not limited to smoothing and filtering processing; the nonlinear state error feedback device of the outer loop controller receives the zero-order value and the first-order value of the preset attitude angle expected value, and receives the preset attitude angle observation value, and outputs the expected value of the zero torque point of the robot. In a possible implementation, the calculation process of the ADRC algorithm is shown in FIG4, and the ADRC algorithm includes a tracking differentiator, an extended state observer, and a nonlinear state error feedback device.

The corresponding formula for the tracking differentiator is

Wherein, k represents an integer greater than or equal to 0, v(k) represents the input value of the tracking differentiator at the kth moment, h represents the integration step (period), and r represents the tracking factor; v ₁ (k) and v ₂ (k) represent the output values of the tracking differentiator at the kth moment. When k＝0, v ₁ (k)＝0, v ₂ (k)＝0; fhan(m,v ₂ (k),r,h) represents the nonlinear function of m,v ₂ (k),r,h. fhan(m,v ₂ (k),r,h) is defined as

sign represents a mathematical sign function.

The fhan function can solve the problem of high-frequency oscillation when the system enters a steady state after the function is directly discretized.

The formula corresponding to the extended state observer is

Among them, y(k) represents the feedback value at the kth moment, and also represents the input value of the extended state observer at the kth moment, _b0 represents the control input coefficient, h represents the integration step (period), δ represents the filter factor, _β1 , _β2 and _β3 represent the output error gain parameters respectively, _z1 (k), _z2 (k), _z3 (k) represent the output values of the extended state observer at the kth moment, and when k=0, _z1 (k), _z2 (k), _z3 (k) are all 0.

fal(e,α,δ) is a nonlinear function, and fal(e,α,δ) is defined as:

Among them, fal(e,0.5,δ) represents the value of the nonlinear function when α = 0.5, and fal(e,0.25,δ) represents the value of the nonlinear function when α = 0.25.

The formula of the nonlinear state error feedback is:

Among them, α ₁ and α ₂ both represent constants between [0,1], γ ₁ and γ ₂ are error gain parameters respectively, fal(n ₁ ,α ₁ ,δ) represents the value of the nonlinear function when e＝n ₁ and α＝α ₁ , fal(n ₂ ,α ₂ ,δ) represents the value of the nonlinear function when e＝n ₂ and α＝α ₂ , and u ₀ represents the output value of the nonlinear state error feedback device at the kth moment.

After obtaining the output value of the error feedback device at the kth moment, according to the following formula

u＝u ₀ −z ₃ (k)/b ₀ (Formula 4)

Calculate the value u of the control object at the kth moment, which is also the output value of the ADRC algorithm at the kth moment. The control object is a device that performs further calculations based on the input value u. For example, the control object is the actuator of the robot, and the actuator plans the robot's gait based on the input value.

Correspondingly, the process of the outer loop controller calculating the expected value of the robot's zero torque point according to the preset attitude angle deviation value is as follows: the preset attitude angle expected value is used as the input value v(k) of the tracking differentiator at the kth moment, and v ₁ (k) and v ₂ (k) are calculated according to Formula 1, then v ₁ (k) represents the zero-order value of the preset attitude angle expected value, and v ₂ (k) represents the first-order value of the preset attitude angle expected value. The preset attitude angle feedback value is used as the feedback value y(k) of the extended state observer at the kth moment, and z ₁ (k), z ₂ (k) and z ₃ (k) are calculated according to Formula 2, then z ₁ (k), z ₂ (k) and z ₃ (k) are the preset attitude angle observation values. The nonlinear state error feedback device receives the zero-order value and the first-order value of the attitude angle expected value, as well as z ₁ (k) and z ₂ (k) in the attitude angle observation value, and calculates u ₀ according to Formula 3, and finally calculates u according to z ₃ (k) and Formula 4, and u is the zero torque point expected value. Since z ₃ (k) is introduced in the ADRC algorithm, z ₃ (k) represents the observed value of the total disturbance. Therefore, calculating the expected value of the zero moment point according to z ₃ (k) and Formula 4 can compensate for the disturbance error of the preset attitude angle feedback value, thereby improving the accuracy of the output zero moment point expected value.

S103: Obtaining the expected value of the zero moment point of the robot, obtaining the measured value of the zero moment point of the robot, and generating a zero moment point deviation value.

In one possible implementation, the zero-torque point measurement value can be calculated based on the contact force between the robot and the ground. The contact force between the robot and the ground is measured by the robot's six-dimensional force sensor or torque sensor, and the zero-torque point measurement value is a coordinate value. The zero-torque point is related to the robot's current posture. For example, when a bipedal robot stands on one foot and only the right foot is on the ground, the zero-torque point is located at the robot's right foot. The zero-torque point deviation value is the difference between the zero-torque point expected value and the zero-torque point measured value. S104: The preset inner loop controller obtains the zero-torque point deviation value and calculates the torque compensation value of the robot's end joint.

As shown in Figure 3, the inner loop controller calculates the torque compensation value _τc of the robot's end joint according to the zero torque point expected value _pd and the zero torque point measured value p. The torque compensation value of the robot's end joint is the torque compensation value of the forward joint or the torque compensation value of the lateral joint. The torque compensation value of the forward joint is used to control the movement of the robot's forward joint, and the torque compensation value of the lateral joint is used to control the movement of the robot's lateral joint.

If the preset attitude angle is the forward pitch attitude angle, the torque compensation value of the end joint is the torque compensation value of the forward joint, that is, the torque compensation value of the robot on the xoz plane; if the preset attitude angle is the lateral roll attitude angle, the torque compensation value of the end joint is the torque compensation value of the lateral joint, that is, the torque compensation value of the robot on the yoz plane.

The inner loop controller may be an ADRC or a variable PD controller (ie, a VPD controller), where the VPD controller is a controller that uses a VPD algorithm.

In a possible implementation, the inner loop controller is a VPD controller, the zero torque point deviation value is input into the VPD algorithm model of the inner loop controller, and the torque compensation value of the end joint of the robot is output.

In one possible implementation, the inner-loop controller is a VPD controller, and the process of the inner-loop controller calculating the torque compensation value of the end joint of the robot according to the zero-torque point deviation value is as follows: the inner-loop controller receives the expected value of the zero-torque point output by the outer-loop controller, and calculates the zero-order value of the torque compensation value, the first-order value of the torque compensation value, and the second-order value of the torque compensation value at the current moment according to the expected value of the zero-torque point, the zero-torque point measurement value at the current moment, the first-order output value of the inner-loop controller at the previous moment, and the first-order derivative of the zero-torque point measurement value obtained after first-order derivative processing of the zero-torque point measurement value at the current moment, and uses the zero-order value of the torque compensation value as the output of the inner-loop controller, and outputs the first-order value of the torque compensation value and the second-order value of the torque compensation value as the input of the inner-loop controller at the next moment. The inner loop controller calculates the zero-order value of the torque compensation value at the next moment, the first-order value of the torque compensation value, and the second-order value of the torque compensation value according to the first-order value of the torque compensation value at the current moment, the second-order value of the torque compensation value at the current moment, the expected value of the zero torque point at the next moment, and the measured value of the zero torque point at the next moment.

In one possible implementation, the VPD algorithm is defined as:

Wherein, k represents an integer greater than or equal to 0, k′ represents an integer greater than or equal to 0, v _d (k) and vel(k) represent the input values at the kth moment, v(k) represents the feedback value at the kth moment, represents the first-order derivative processing of the feedback value v(k), vel(k′) represents the first-order output value at the kth moment, dt is the integration step (cycle), _kp , _kd and _kv represent the error gain parameters respectively, and u represents the output value at the kth moment. When k＝0, v(k)＝0,/> v(k)＝0.

Compared with the traditional PID algorithm, the VPD algorithm can effectively solve the contradiction between rapid response and overshoot.

Correspondingly, the process of the inner loop controller calculating the compensation value of the torque of the end joint of the robot according to the zero torque point deviation value is as follows.

The expected value of the zero moment point is taken as the input value v _a (k) at the kth moment, the measured value of the zero moment point at the current moment is taken as the feedback value v(k) at the kth moment, the first-order output value of the inner loop controller at the previous moment is taken as vel(k), and the first-order derivative of the zero moment point measured value obtained by performing first-order derivative processing on the zero moment point measured value at the current moment is taken as Substitute into formula 5 and calculate pos(k′), then pos(k′) is the torque compensation value at the current moment, that is, the zero-order value of the torque compensation value. According to formula 5, the first-order derivative vel(k′) of pos(k′) can also be calculated, that is, the first-order value of the torque compensation value. According to vel(k′), the second-order derivative acc(k′) of pos(k′) can be further calculated, that is, the second-order value of the torque compensation value. The first-order value of the torque compensation value and the second-order value of the torque compensation value are output by the inner loop controller and serve as the input of the inner loop controller at the next moment.

Since the VPD algorithm can effectively solve the contradiction between the rapidity of response and the overshoot of the control method, the VPD algorithm is used to calculate the compensation value of the torque of the robot's foot joint, which can improve the accuracy of the calculation result.

In a possible implementation, the inner loop controller is ADRC, and the zero torque point deviation value of the robot is input into the ADRC algorithm model of the inner loop controller, and the torque compensation value of the end joint of the robot is output.

In another possible implementation, the inner loop controller is ADRC, and the process of the inner loop controller calculating the torque compensation value of the robot's end joint according to the zero torque point deviation value is as follows: the tracking differentiator of the inner loop controller receives the zero torque point expected value output by the outer loop controller, and outputs the zero-order value and the first-order value of the zero torque point expected value. The extended state observer of the inner loop controller receives the zero torque point measurement value and outputs the zero torque point feedback value after the extended observation processing, wherein the extended observation processing includes but is not limited to smoothing and filtering processing. The nonlinear state error feedback device receives the zero-order value and the first-order value of the zero torque point expected value, receives the zero torque point feedback value, and outputs the torque compensation value.

If the ADRC algorithm described in Formulas 1 to 4 is used, the method in which the inner loop controller calculates the torque compensation value of the end joint of the robot is as follows.

The expected value of the zero moment point is taken as the input value v(k) of the tracking differentiator at the kth moment, and v ₁ (k) and v ₂ (k) are calculated according to formula 1, then v ₁ (k) represents the zero-order value of the expected value of the zero moment point, and v ₂ (k) represents the first-order value of the expected value of the zero moment point. The measured value of the zero moment point is taken as the feedback value y(k) of the extended state observer at the kth moment, and z ₁ (k), z ₂ (k) and z ₃ (k) are calculated according to formula 2, then z ₁ (k), z ₂ (k) and z ₃ (k) are the zero moment point feedback values. The nonlinear state error feedback device receives the zero-order value and first-order value of the expected value of the zero moment point, as well as z ₁ (k) and z ₂ (k) in the zero moment point feedback value, and calculates u ₀ according to formula 3, and finally calculates u according to z ₃ (k) and formula 4, and u is the torque compensation value.

Since z ₃ (k) is introduced into the ADRC algorithm, z ₃ (k) represents the observed value of the total disturbance. Therefore, calculating the compensation value of the torque of the robot's end joint according to Formula 4 and z ₃ (k) can compensate for the disturbance error of the measured value of the zero torque point, thereby improving the accuracy of the output torque compensation value of the end joint.

S105: Obtain the torque compensation value of the end joint of the robot and the expected torque value obtained through the robot gait planning, and generate a torque deviation value.

The expected torque value is obtained when the robot is pre-planned for gait, which can be the expected torque value of the end joint on the yoz plane, or the expected torque value of the end joint of the foot on the xoz plane. The torque deviation value is the difference between the expected torque value and the torque compensation value.

In a possible implementation, a gait planning strategy is obtained in advance, and the expected torque value of the end joint of the robot is determined according to the gait planning strategy. For example, the walking route of the robot is determined according to the current position of the robot and the position to be reached, the gait planning strategy is determined according to the walking route, and the expected torque value of the end joint of the robot at each moment is determined according to the gait planning strategy.

S106: Providing the torque deviation value to the end joint actuator of the robot to control the gait movement of the robot.

As shown in Figure 3, the torque compensation value _τc and the torque expected value _τd are provided to the actuator to control the gait movement of the robot. The torque compensation value provided to the actuator includes the torque compensation value on the yoz plane and the torque compensation value on the xoz plane, the torque expected value provided to the actuator includes the torque expected value on the yoz plane and the torque expected value on the xoz plane, and the actuator can be a servo.

Specifically, the compensation value of the torque of the robot's end joint on the yoz plane is added to the expected value of the torque of the end joint on the yoz plane, and the compensation value of the torque of the end joint on the xoz plane is added to the expected value of the torque of the end joint on the xoz plane to obtain the final torque of the end joint on the yoz plane and the torque on the xoz plane. The gait of the robot is controlled according to the final torque of the end joint on the yoz plane and the torque on the xoz plane.

In the above embodiment, the preset attitude angle expected value of the robot and the preset attitude angle feedback value of the robot detected by the preset inertial measurement sensor of the robot are obtained, and the preset attitude angle deviation value is generated; the preset outer loop controller obtains the preset attitude angle deviation value and calculates the expected value of the zero torque point of the robot, and the outer loop controller is an anti-disturbance controller; the expected value of the zero torque point of the robot is obtained, and the zero torque point measurement value of the robot is obtained, and the zero torque point deviation value is generated; the preset inner loop controller obtains the zero torque point deviation value and calculates the torque compensation value of the end joint of the robot. That is, the present application calculates the compensation value of the torque of the end joint of the robot by the preset attitude angle feedback value and the zero torque point measurement value, and the zero torque point is an important indicator of the stable walking of the robot. Therefore, the present application improves the accuracy of the calculated torque compensation value relative to the compensation value of the torque of the end joint of the robot calculated only by the attitude angle feedback value. At the same time, the use of the anti-disturbance controller to calculate the expected value of the zero torque point of the robot body can compensate for the disturbance error of the preset attitude angle measurement value, and improve the accuracy of the output zero torque point expected value. After calculating the torque compensation value, the expected torque value of the robot's end joint is obtained, and the torque deviation value is generated. The torque deviation value is provided to the robot's end joint actuator to control the robot's gait movement, thereby improving the accuracy of the robot's gait control.

In one embodiment, the outer loop controller is ADRC, and the inner loop controller adopts a VPD controller. Correspondingly, the specific process of the robot motion control method is shown in FIG5. First, the expected value θ _yd of the attitude angle on the yoz plane is input into the outer loop controller. The outer loop controller adopts the ADRC algorithm, and θ _yd is used as the input value of the tracking differentiator in the ADRC algorithm, and outputs v ₁ and v ₂ . The measured value θ _y of the offset angle on the yoz plane is used as the input value of the extended state observer, and z ₁ , z ₂ , and z ₃ are output. Then u ₀ is calculated based on v ₁ , v ₂ , z ₁ , and z ₂ . Finally, the output value u ₁ is calculated based on u _0. u ₁ is the expected value p _yd of the y-axis coordinate in the expected value of the robot's zero torque point. Then the expected value p _yd of the y-axis coordinate, the measured value p _y of the y-axis coordinate, and the first-order derivative of p _y are calculated. Input the inner loop controller, which uses the VPD algorithm to calculate the output value u ₂ , which is the torque compensation value τ _cy on the yoz plane. The same method is used to calculate the torque compensation value τ _cx on the xoz plane. Finally, the torque compensation value τ _cy on the yoz plane, the torque compensation value τ _cx on the xoz plane, and the corresponding torque expected values τ _dy and τ _dx are input into the actuator to control the robot's _gait .

In the above embodiment, the outer loop controller adopts the ADRC algorithm and the inner loop controller adopts the VPD algorithm. When calculating the expected value of the zero torque point of the robot, the error disturbance is compensated; when calculating the torque compensation value of the end joint of the robot, the contradiction between the rapidity of response and the overshoot of the control method is resolved, thereby improving the accuracy of the robot's gait planning.

In another embodiment, the outer loop controller is ADRC, and the inner loop controller is ADRC. Correspondingly, the specific flow of the motion control method of the robot is shown in FIG6. First, the expected value θ _yd of the attitude angle on the yoz plane is input into the outer loop controller. The outer loop controller adopts the ADRC algorithm, and θ _yd is used as the input value of the tracking differentiator in the ADRC algorithm, and outputs v ₁ and v ₂ . The attitude angle measurement value θ _y on the yoz plane is used as the input value of the extended state observer, and z ₁ , z ₂ , and z ₃ are output. Then u ₀ is calculated based on v ₁ , v ₂ , z ₁ , and z _2. Finally, the output value u ₃ is calculated based on u ₀ , and u ₃ is the expected value p _yd of the y-axis coordinate in the expected value of the zero moment point of the robot. Then the expected value p _yd of the y-axis coordinate is input into the inner loop controller. The inner loop controller adopts the ADRC algorithm, and p _yd is used as the input value of the tracking differentiator in the ADRC algorithm, and v′ ₁ and v′ ₂ are output. The measured value p _y of the y-axis coordinate in the zero-torque point measurement value is used as the input value of the extended state observer, and z′ ₁ , z′ ₂ , and z′ ₃ are output. Then u′ ₀ is calculated according to v′ ₁ , v′ ₂ , z′ ₁ , and z′ _2. Finally, the output value u ₄ is calculated according to u′ ₀ , and u ₄ is the torque compensation value τ _cy on the yoz plane. The torque compensation value v _cx on the xoz plane is calculated by the same method. Finally, the torque compensation value τ _cy on the yoz plane, the torque compensation value τ _cx on the xoz plane, and the corresponding torque expected values τ _dy and τ _dx are input into the actuator to control the gait of the robot.

In the above embodiment, the outer loop controller adopts the ADRC algorithm and the inner loop controller adopts the ADRC algorithm. When calculating the expected value of the zero torque point of the robot and the torque compensation value of the foot joint of the robot, the error disturbance can be compensated, thereby improving the accuracy of the robot's gait planning.

It should be understood that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

Corresponding to the robot motion control method described in the above embodiment, FIG7 shows a structural block diagram of the robot motion control device provided in the embodiment of the present application. For the sake of convenience, only the part related to the embodiment of the present application is shown.

As shown in Figure 7, the robot's motion control device includes:

A first acquisition module 10 is used to acquire an expected value of a preset attitude angle of the robot and a feedback value of a preset attitude angle of the robot detected by a preset inertial measurement sensor of the robot, and generate a preset attitude angle deviation value;

A first calculation module 20 is used to preset an outer loop controller to obtain the preset attitude angle deviation value and calculate the expected value of the zero moment point of the robot, wherein the outer loop controller is an auto-disturbance rejection controller;

A second acquisition module 30 is used to acquire an expected value of a zero moment point of the robot, acquire a measured value of a zero moment point of the robot, and generate a zero moment point deviation value;

A second calculation module 40 is used to preset an inner loop controller to obtain the zero torque point deviation value and calculate the torque compensation value of the end joint of the robot;

A third acquisition module 50 is used to acquire the torque compensation value of the end joint of the robot and the expected torque value obtained through the robot gait planning, and generate a torque deviation value;

The control module 60 is used to provide the torque deviation value to the end joint actuator of the robot to control the gait movement of the robot.

In a possible implementation, the first calculation module 20 is specifically configured to:

The outer loop controller calculates the zero-order value and the first-order value of the preset attitude angle expected value according to the preset attitude angle expected value, and calculates the zero-torque point expected value of the robot according to the zero-order value and the first-order value of the preset attitude angle expected value and the preset attitude angle feedback value.

In one possible implementation, the inner loop controller is a variable PD controller, and the second calculation module 40 is specifically used for: the inner loop controller receives the expected value of the zero torque point of the robot and the zero torque point measurement value at the current moment, the first-order output value of the inner loop controller at the previous moment, and the first-order derivative of the zero torque point measurement value at the current moment, calculates the zero-order value of the torque compensation value, the first-order value of the torque compensation value, and the second-order value of the torque compensation value of the end joint of the robot at the current moment, and uses the zero-order value of the torque compensation value as the output of the inner loop controller, and outputs the first-order value of the torque compensation value and the second-order value of the torque compensation value as the input of the inner loop controller at the next moment.

In one possible implementation, the inner loop controller is an anti-disturbance controller, and the second calculation module 40 is specifically used for: the tracking differentiator of the inner loop controller receives the expected value of the zero torque point of the robot output by the outer loop controller, and outputs the zero-order value and the first-order value of the expected value of the zero torque point of the robot; the extended state observer of the inner loop controller receives the zero torque point measurement value of the robot and outputs the zero torque point feedback value after the extended observation processing; the nonlinear state error feedback device of the inner loop controller receives the zero-order value and the first-order value of the expected value of the zero torque point of the robot and receives the zero torque point feedback value, and outputs the torque compensation value of the end joint of the robot.

In a possible implementation, the zero-torque point measurement value of the robot is obtained by detecting the contact force between the robot and the ground through a six-dimensional force sensor of the robot, and is calculated based on the contact force between the robot and the ground, or is calculated through robot dynamics.

In a possible implementation, the preset attitude angle is a forward pitch attitude angle, and the torque compensation value of the end joint of the robot is the torque compensation value of the forward joint of the robot to control the movement of the forward joint of the robot.

In a possible implementation, the preset attitude angle is a lateral rolling attitude angle, and the torque compensation value of the end joint of the robot is the torque compensation value of the lateral joint of the robot to control the movement of the lateral joint of the robot.

It should be noted that the information interaction, execution process, etc. between the above-mentioned devices/units are based on the same concept as the method embodiment of the present application. Their specific functions and technical effects can be found in the method embodiment part and will not be repeated here.

FIG8 is a schematic diagram of the structure of the robot provided in an embodiment of the present application. As shown in FIG8 , the robot of this embodiment includes: a processor 11, a memory 12, and a computer program 13 stored in the memory 12 and executable on the processor 11. When the processor 11 executes the computer program 13, the steps in the control method embodiment of the above-mentioned robot are implemented, such as steps S101 to S106 shown in FIG1 . Alternatively, when the processor 11 executes the computer program 13, the functions of each module/unit in the above-mentioned device embodiments are implemented, such as the functions of the first acquisition module 10 to the control module 60 shown in FIG7 .

Exemplarily, the computer program 13 may be divided into one or more modules/units, which are stored in the memory 12 and executed by the processor 11 to complete the present application. The one or more modules/units may be a series of computer program instruction segments capable of completing specific functions, which are used to describe the execution process of the computer program 13 in the terminal device.

Those skilled in the art will understand that FIG8 is merely an example of a robot and does not constitute a limitation on the robot. The robot may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the robot may also include input and output devices, network access devices, buses, etc.

The processor 11 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.

The memory 12 may be an internal storage unit of the robot, such as a hard disk or memory of the robot. The memory 12 may also be an external storage device of the robot, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the robot. Furthermore, the memory 12 may include both an internal storage unit and an external storage device of the robot. The memory 12 is used to store the computer program and other programs and data required by the robot. The memory 12 may also be used to temporarily store data that has been output or is to be output.

The technicians in the relevant field can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

In the embodiments provided in the present application, it should be understood that the disclosed devices/terminal equipment and methods can be implemented in other ways. For example, the device/terminal equipment embodiments described above are only schematic. For example, the division of the modules or units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

If the integrated module/unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present application implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device capable of carrying the computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electrical carrier signal, telecommunication signal and software distribution medium, etc.

Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.

The embodiments described above are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, a person skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.

Claims (10)

1. A method of controlling movement of a robot, comprising:

Acquiring a preset attitude angle expected value of a robot and a preset attitude angle feedback value of the robot detected by a preset inertial measurement sensor of the robot, and generating a preset attitude angle deviation value;

The method comprises the steps that a preset outer ring controller obtains a preset attitude angle deviation value and calculates a zero moment point expected value of the robot, wherein the outer ring controller is an active disturbance rejection controller;

acquiring a zero moment point expected value of the robot, acquiring a zero moment point measured value of the robot, and generating a zero moment point deviation value;

The preset inner ring controller obtains the zero moment point deviation value and calculates a moment compensation value of the tail end joint of the robot;

Acquiring a moment compensation value of a tail end joint of the robot and a moment expected value obtained through gait planning of the robot, and generating a moment deviation value;

The moment offset value is provided to an end joint actuator of the robot to control gait motion of the robot.

2. The method of claim 1, wherein the outer ring controller calculates a zero-order value and a first-order value of the preset attitude angle expected value according to the preset attitude angle expected value, and calculates a zero-moment expected value of the robot according to the zero-order value and the first-order value of the preset attitude angle expected value and the preset attitude angle feedback value.

3. The motion control method of a robot according to claim 1 or 2, wherein the inner ring controller is a variable PD controller, the inner ring controller receives a zero moment point expected value of the robot and a zero moment point measured value at a current time, a first order output value of the inner ring controller at a previous time, a first derivative of the zero moment point measured value at the current time, calculates a zero order value of a moment compensation value of an end joint of the robot at the current time, a first order value of the moment compensation value, a second order value of the moment compensation value, and takes the zero order value of the moment compensation value as an output of the inner ring controller, and the first order value of the moment compensation value and the second order value output of the moment compensation value are taken as inputs of the inner ring controller at a next time;

The variable PD controller is a controller adopting a VPD algorithm, wherein the VPD algorithm is defined as follows:

Wherein k represents an integer greater than or equal to 0, k' represents an integer greater than or equal to 0, v _d (k) and vel (k) represent input values at the kth time, v (k) represents feedback values at the kth time, The feedback value v (k) is subjected to first derivative processing, vel (k') represents a first-order output value at the kth time, dt is an integration step (period), k _p、k_d and k _v represent error gain parameters, and u represents an output value at the kth time.

4. The motion control method of a robot according to claim 1 or 2, wherein the inner loop controller is an active disturbance rejection controller, and a tracking differentiator of the inner loop controller receives a zero moment point desired value of the robot output by the outer loop controller and outputs a zero order value and a first order value of the zero moment point desired value of the robot; the extended state observer of the inner ring controller receives the zero moment point measured value of the robot and outputs a zero moment point feedback value after extended observation treatment; and the nonlinear state error feedback device of the inner loop controller receives the zero-order value and the first-order value of the zero moment point expected value of the robot, receives the zero moment point feedback value and outputs a moment compensation value of the tail end joint of the robot.

5. The method of claim 1, wherein the zero moment point measurement of the robot is obtained by detecting a contact force of the robot with the ground through a six-dimensional force sensor of the robot, and is calculated according to the contact force of the robot with the ground, or is obtained by calculation through robot dynamics.

6. The method of controlling motion of a robot according to claim 1, wherein the preset attitude angle is a forward pitch attitude angle, and the moment compensation value of the distal joint of the robot is a moment compensation value of the forward joint of the robot to control motion of the forward joint of the robot.

7. The method of claim 1, wherein the preset attitude angle is a lateral roll attitude angle, and the moment compensation value of the distal joint of the robot is a moment compensation value of the lateral joint of the robot to control the movement of the lateral joint of the robot.

8. A motion control apparatus of a robot, comprising:

The first acquisition module is used for acquiring a preset attitude angle expected value of the robot and a preset attitude angle feedback value of the robot detected by a preset inertial measurement sensor of the robot, and generating a preset attitude angle deviation value;

The first calculation module is used for presetting an outer ring controller to obtain the preset attitude angle deviation value and calculating a zero moment point expected value of the robot, wherein the outer ring controller is an active disturbance rejection controller;

The second acquisition module is used for acquiring the expected value of the zero moment point of the robot, acquiring the measured value of the zero moment point of the robot and generating a deviation value of the zero moment point;

the second calculation module is used for presetting an inner ring controller to obtain the zero moment point deviation value and calculating a moment compensation value of the tail end joint of the robot;

The third acquisition module is used for acquiring a moment compensation value of the tail end joint of the robot and a moment expected value obtained through gait planning of the robot and generating a moment deviation value;

And the control module is used for providing the moment deviation value for an end joint actuator of the robot so as to control gait movement of the robot.

9. A robot comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that the processor implements the method according to any of claims 1 to 7 when executing the computer program.

10. A computer readable storage medium storing a computer program, characterized in that the computer program when executed by a processor implements the method according to any one of claims 1 to 7.