Quadruped robot motion control method, system and equipment and storage medium

Abstract

The invention discloses a quadruped robot motion control method, system and device and a storage medium, and the method comprises the steps: dynamically selecting one of model prediction control and reinforcement learning reasoning as an effective mode for controlling the current advancing track of a robot according to target task information and robot positioning navigation data obtained in real time, according to the other control mode, an instruction output by tracking the effective mode is executed, so that undisturbed switching of the two control modes is achieved; according to the model prediction control, a kinematics model of the quadruped robot is established, Kalman filtering is combined for state estimation, and instructions of the positions, the speeds and the moments of all joints are output; and the reinforcement learning reasoning is used for training the reasoning model based on a reward function of tasks and safety energy efficiency, and outputting an instruction of each joint position. The requirements of the quadruped robot for adaptively passing through the complex terrain and accurately reaching the target position can be met at the same time, the control mode switching process is automatically completed, and mutual tracking is undisturbed.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a quadruped robot motion control method, system, equipment and storage medium. Background Art

[0002] Mainstream quadruped robots have 12 degrees of freedom consisting of motor-driven joints, and are significantly better than wheeled robots in navigating complex unstructured terrain. Therefore, they are usually used in working environments with a large amount of rugged terrain, where they are required to have robust and adaptive mobility, while meeting the needs of performing work tasks and having high control accuracy.

[0003] The Chinese patent with publication number CN113568422A provides a quadruped robot control method based on model predictive control to optimize reinforcement learning. The method optimizes the control of the quadruped robot based on reinforcement learning based on model predictive control to reduce the meaningless data generated during the reinforcement learning training process and reduce the demand for computing power through model-guided training. Its purpose is to use model predictive control to simplify the training of reinforcement learning. In essence, it is still a control method of reinforcement learning. In order to enhance adaptability, the multi-layer randomization contained in reinforcement learning determines its low control accuracy. In addition, the method is based on value and strategy for training. In the construction of the reward function, only the fixed reward of the environment is used, which cannot solve the problem of sparse rewards and is prone to not getting good training results due to the lack of environmental rewards.

[0004] A Chinese patent with publication number CN112936290A provides a quadruped robot motion planning method based on hierarchical reinforcement learning. The method obtains corresponding control instructions by combining an upper-layer behavior decision controller based on deep reinforcement learning and a lower-layer motion execution controller based on model predictive control. However, the method only uses reinforcement learning as a way to generate upper-layer control instructions. The specific control and execution levels still use model predictive control, which requires modeling of the robot and terrain environment. However, the terrain environment in which the quadruped robot operates is very complex, and it is difficult for the terrain model to cover all working conditions. In addition, the reinforcement learning framework of the method does not utilize the information of the robot joints, cannot take into account the energy efficiency and safety of the joints at the execution level, and cannot solve the problem of sparse rewards.

[0005] In summary, the existing technology has the following disadvantages:

[0006] The control layer uses the method of model predictive control output instructions, which requires modeling of the robot and the terrain environment. The terrain environment in which the quadruped robot operates is very complex, and it is difficult for the terrain model to exhaust all working conditions; the control layer uses the method of reinforcement learning control output instructions, and the multi-layer randomization contained in it determines that its control accuracy is low; therefore, the existing method has the problem that the adaptability of the terrain and the control accuracy cannot be well balanced. In addition, the existing reinforcement learning method only uses the fixed reward of the environment in the construction of the reward function, which cannot solve the problem of sparse rewards and is prone to not getting good training results due to the lack of environmental rewards. In addition, the existing model predictive control method needs to assume that the height of the sole is zero in the state observation link, and ignores the influence of angular velocity in the optimization solution of the sole force, which limits the accuracy of the model predictive control;. Summary of the invention

[0007] In order to address the deficiencies in the prior art, the present invention provides a quadruped robot motion control method, system, device and storage medium, which can maximize the advantages of model predictive control and reinforcement learning, solve the problem of being difficult to balance adaptively passing through complex terrain and accurately reaching the target position, and the control mode switching process is automatically completed, and mutual tracking is achieved without interference.

[0008] The present invention adopts the following technical solution.

[0009] In a first aspect, the present invention provides a quadruped robot motion control method, the method comprising:

[0010] According to the target task information and robot positioning and navigation data acquired in real time, one of the control modes of model predictive control and reinforcement learning reasoning is dynamically selected as the effective mode for controlling the current trajectory of the robot, and the other control mode executes and tracks the instructions output by the effective mode to achieve disturbance-free switching between the two control modes and complete the motion control of the quadruped robot;

[0011] Among them, when model predictive control is selected as the current control effectiveness mode, the kinematic model of the quadruped robot is established according to the data collected by the encoder and inertial measurement unit IMU and the robot speed command, and the state estimation is performed in combination with Kalman filtering to obtain the instructions for controlling the current joint position, speed and torque of the quadruped robot;

[0012] When reinforcement learning reasoning is selected as the current control effectiveness mode, the data collected by the encoder and inertial measurement unit IMU and the robot speed command are input into the reasoning model trained based on the task and safety and energy efficiency reward function for reasoning, and the instructions for controlling the current joint positions of the quadruped robot are obtained.

[0013] Optionally, the method further comprises the following steps:

[0014] The command output by the currently selected effective mode is used as the target value of the joint control in the robot, and the motor torque command is obtained through joint control calculation;

[0015] The motor torque command is input into the motor servo driver to drive the motor to operate, so as to achieve the control of the movement of the quadruped robot.

[0016] Optionally, the step of selecting the validation mode includes:

[0017] The distance between the current position of the robot and the target point is obtained from the target task information. When the distance is not greater than the set threshold L, the model predictive control is selected as the currently effective control method.

[0018] When the distance is greater than the threshold L, the maximum height difference between the current position and the target point terrain is obtained from the robot positioning and navigation data; when the height difference is greater than the set threshold H, reinforcement learning reasoning is selected as the currently effective control method; when the height difference is not greater than the threshold H, the current control method is maintained unchanged.

[0019] Optionally, the step of obtaining instructions for controlling the current joint positions, speeds and torques of the quadruped robot includes:

[0020] The kinematic model of the quadruped robot is established based on the speed measured by the encoder, the position of each joint, the acceleration, angular velocity, and rotation angle collected by the inertial measurement unit (IMU), and the geometric parameters of the quadruped robot.

[0021] Combined with the established kinematic model, the Kalman filter is used to estimate the state and obtain the current state of the quadruped robot.

[0022] Based on the current state of the quadruped robot, timing planning is performed for the movement of the sole of the foot in the contact phase and the swing phase respectively; wherein the timing planning of the swing phase includes planning the swing trajectory of the sole of the foot to the target position; the timing planning of the contact phase includes solving the sole force by establishing a single rigid body dynamics model;

[0023] Performing inverse kinematics on the planned foot swing trajectory to obtain the joint position and speed of the swing foot of the quadruped robot;

[0024] The plantar force is solved by inverse dynamics or inverse statics to obtain the joint torque of the ground contacting foot of the quadruped robot.

[0025] Optionally, the steps of establishing a kinematic model of the quadruped robot and performing state estimation in combination with Kalman filtering include:

[0026] According to the working condition that all four legs touch the ground, the state space equation describing the motion of the quadruped robot is constructed:

[0027] x=[p b v b p0 p1 p2 p3] T

[0028]

[0029] Where x is the state vector of the quadruped robot, which includes the body position p in the global coordinate system b 、Body speed v b , the positions of the four foot ends p0, p1, p2, p3; the superscript T represents the transpose of the matrix; the superscript · represents the first-order derivative of the variable; a b is the acceleration output by the IMU, R sb is the body posture in the world coordinate system output by IMU; g is the gravitational acceleration; 0 3×1 is a 3×1 zero matrix; 0 3×3 is a 3×3 zero matrix; 0 3×12 is a 3×12 zero matrix; 0 3×18 is a 3×18 zero matrix; 0 12×18 is a 12×18 zero matrix; I3 is a 3×3 unit diagonal matrix;

[0030] According to the joint angles and angular velocities measured by the robot's joint encoders, the position p of each foot end relative to the body in the body coordinate system is calculated by forward kinematics. bfB and speed v bfB , and then we get the observation equation:

[0031]

[0032] Where y is the observation matrix of the quadruped robot; ω b is the angular velocity output by the IMU; p sfB0～4 Respectively represent the positions of the four foot ends relative to the inertial system; v sfB0～4 Respectively represent the speed of the four foot ends relative to the inertial system; p bfB0～4 Respectively represent the positions of the four foot ends relative to the machine system; v bfB0～4 Respectively represent the speeds of the four foot ends relative to the machine system; [] × is the skewness function; I 12 is a 12×12 unit diagonal matrix; 0 12×12 is a 12×12 zero matrix;

[0033] The confidence level of the stable contact between the swinging foot and the ground is set to untrustworthy, that is, the corresponding items of the process noise and measurement noise covariance matrices Q and R are set to +∞, and the state space equation and the observation equation are combined with the Kalman filter to recursively estimate the system to obtain the observed state of the state vector x of the quadruped robot

[0034] Optionally, the state space equation and the observation equation are combined into a Kalman filter to perform recursive estimation of the system as follows:

[0035]

[0036] In the formula, is the prior estimate of the robot state vector x at time k; and are the posterior estimates of the robot state vector x at time k and time k-1 respectively; u k-1 is the matrix u at time k-1 in the state space equation; y k is the observation matrix y at time k; is the prior estimate of the P matrix at time k; and The P matrix is ​​the posterior estimate at time k and time k-1 respectively; the P matrix is ​​the covariance matrix of the robot state vector x; Q and R are the process noise and measurement noise covariance matrices respectively.

[0037] Optionally, the specific steps of the timing planning of the touchdown phase include:

[0038] According to the geometric and physical parameters of the quadruped robot, a single rigid body dynamics model is established;

[0039] The single rigid body dynamics model is converted into a state space form and discretized to obtain an equality constraint model retaining an angular velocity term;

[0040] Inputting the current state and speed command into the equation constraint model for prediction recursion to obtain the predicted state within a future set time period;

[0041] The QP problem model is constructed by taking the minimum deviation between the predicted state and the control target within the set time period as the solution goal, introducing friction constraints, and constructing the QP problem model;

[0042] The QP problem model is solved to obtain the plantar force of the quadruped robot.

[0043] Optionally, it is characterized in that the expression for establishing the single rigid body dynamics model is as follows:

[0044]

[0045]

[0046] In the formula, represents the fuselage position p in the global coordinate system b The second derivative of represents the first derivative of a variable; f i is the plantar force of the ith foot of the quadruped robot, m is the mass of the quadruped robot, g is the acceleration due to gravity, N is the moment of inertia of the quadruped robot, and r i is the position vector from the sole of the i-th foot of the quadruped robot to the center of mass; ω b is the angular velocity output by the IMU.

[0047] Optionally, the expression of the equality constraint model retaining the angular velocity term is as follows:

[0048]

[0049] In the formula, z(k+1) and z(k) represent the values ​​of vector z at time k+1 and time k respectively; E k represents the value of vector E at time k; F k represents the value of vector F at time k; f(k) represents the value of vector f at time k; where, And the expressions of vectors z, f, E, and F are as follows:

[0050]

[0051]

[0052] f=[f0 L f3] T

[0053]

[0054]

[0055] Where Δt is the discretized time; Θ is the Euler angle of the quadruped robot; R(Θ) is the conversion matrix between angular velocity and Euler angular rate; 1 3×3 is a 3×3 matrix whose elements are all 1; [] × is the skewness function; the superscript T indicates the transpose of the matrix; the superscript · indicates the first-order derivative of the variable; 0 1×3 is a 1×3 zero matrix; 0 3×3 is a 3×3 zero matrix.

[0056] Optionally, the expression for constructing the QP problem model is as follows:

[0057]

[0058] fx |≤μf z ,|f y |≤μf z ,f z >0

[0059] Where n represents the number of set time periods; z ref (k+1) represents the control target instruction given to the quadruped robot at time k+1; f x 、f y 、f z are the x, y, and z components of the plantar force in the world coordinate system; μ is the friction coefficient of the ground; W and G are two different weight matrices.

[0060] Optionally, the expression of the reward function based on task and safety energy efficiency is as follows:

[0061]

[0062]

[0063]

[0064] In the formula, the reward function r k Including task reward function and safety energy efficiency reward function α, β, γ, and λ are four different adjustable hyperparameters; is the linear speed instruction; v xy is the measured value of line speed; is the angular velocity command; ω z is the angular velocity measurement; τ is the joint torque; is the measured value of joint acceleration; IsColl(k) is the collision detection value of the quadruped robot at time k.

[0065] Optionally, the command output by the currently selected effective mode is used as the target value of the joint controller in the robot, and after the joint control operation, the calculation formula for obtaining the motor torque command is as follows:

[0066] τ m =(τ cmd +K p (q cmd -q)+K d (qd cmd -qd)) / h

[0067] In the formula, is τ m Motor torque command, τ cmd is the joint torque command, K pis the position stiffness coefficient, q cmd is the joint position command, q is the current position of the joint, K d is the velocity stiffness coefficient, qd cmd is the joint speed command, qd is the current joint speed, and h is the joint reduction ratio;

[0068] Among them, in the instructions output by reinforcement learning reasoning as the effective mode, the joint torque instructions and the joint speed instructions are both zero.

[0069] In a second aspect, the present invention provides a quadruped robot motion control system, which runs the steps of any method described in the first aspect of the present invention, and the system includes a control scheduler, a model predictive controller and a reinforcement learning reasoner that are interconnected; wherein:

[0070] A control scheduler is used to dynamically select the output instructions of the control modes in the model predictive controller and the reinforcement learning reasoner as the effective instructions for controlling the current trajectory of the robot according to the target task information and robot positioning and navigation data obtained in real time, and the other control mode executes and tracks the instructions output by the effective mode to achieve non-disturbance switching between the two control modes and complete the motion control of the quadruped robot;

[0071] A model predictive controller is used to establish a kinematic model of the quadruped robot based on the data collected by the encoder and the inertial measurement unit IMU and the robot speed command, and to perform state estimation in combination with the Kalman filter to obtain commands for controlling the current joint position, speed and torque of the quadruped robot; or to track the output command of the reinforcement learning reasoner;

[0072] The reinforcement learning reasoner is used to input the data collected by the encoder and the inertial measurement unit IMU and the robot speed command into the inference model trained based on the task and safety energy efficiency reward function for reasoning, so as to obtain the command for controlling the current joint positions of the quadruped robot; or to track the output command of the model prediction controller.

[0073] Optionally, the system further includes:

[0074] The joint controller is used to use the command output by the currently selected effective mode as the target value of the joint control in the robot, and obtain the motor torque command through joint control calculation;

[0075] The motor servo driver is used to execute the motor torque command to drive the motor to operate, so as to achieve the control of the movement of the quadruped robot.

[0076] In a third aspect, the present invention provides a terminal, including a processor and a storage medium;

[0077] The storage medium is used to store instructions;

[0078] The processor is used to operate according to the instructions to execute the steps of any one of the methods described in the first aspect of the present invention.

[0079] In a fourth aspect, the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of any one of the methods described in the first aspect of the present invention.

[0080] The beneficial effects of the present invention are as follows:

[0081] 1) The control process of the quadruped robot movement in the present invention integrates the two control methods of model predictive control and reinforcement learning reasoning. In the stage of traveling at a large distance from the destination point, the adaptive advantage of reinforcement learning reasoning is used to pass through complex terrain; in the stage of approaching the target point, the precise characteristics of model predictive control are used to achieve accurate control; in the model predictive control stage, since the quadruped robot is already close to the target point, a single flat ground model can be used to describe the terrain features, avoiding the problem of difficult modeling of terrain in model predictive control; the problem of difficulty in balancing adaptive passage through complex terrain and accurate arrival at the target position is solved.

[0082] 2) The present invention removes the plantar height observation term in the observation equation of the model predictive control state observation link, adds the angular velocity term in the physical equation of the plantar force optimization solution link, accurately models the simplified terms of conventional model predictive control, and further improves the accuracy of model predictive control.

[0083] 3) In the model training link of reinforcement learning reasoning, the present invention constructs a reward function based on tasks and safety and energy efficiency, which can solve the problem of not being able to obtain good training results when the rewards are sparse due to fixed rewards in the environment, and improves the control accuracy of reinforcement learning reasoning.

[0084] 4) The present invention maximizes the advantages of model predictive control and reinforcement learning. When one method of model predictive control or reinforcement learning reasoning is not selected as the output, it will track the output instructions of the other party in real time, and can perform undisturbed rapid switching. The switching process is completed automatically, thereby realizing intelligent control of the quadruped robot across different complex terrains. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] Figure 1 is a flow chart of the motion control method of the quadruped robot in the present invention;

[0086] Figure 2 A schematic diagram of the control architecture of the model predictive control in the present invention;

[0087] Figure 3 A schematic diagram of the results of motion control of an experimental case by the method of the present invention;

[0088] Description of reference numerals:

[0089] 1. Model prediction control trajectory; 2. Reinforcement learning control trajectory; 3. Control mode switching point; 4. Inspection task point; 5. Cement road surface; 6. Continuous stairs; 7. Grassland; 8. Cobblestone road. DETAILED DESCRIPTION

[0090] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. The embodiments described in the present invention are only part of the embodiments of the present invention, not all of the embodiments. Based on the spirit of the present invention, other embodiments obtained by ordinary technicians in this field without creative work are all within the scope of protection of the present invention.

[0091] Embodiment 1:

[0092] Reference Figure 1 An embodiment of the present invention provides a quadruped robot motion control method, which specifically includes: according to the target task information and robot positioning and navigation data acquired in real time, dynamically selecting one of the control modes of model predictive control and reinforcement learning reasoning as the effective mode for controlling the current moving trajectory of the robot, and the other control mode executes and tracks the instructions output by the effective mode to achieve disturbance-free switching between the two control modes and complete the motion control of the quadruped robot.

[0093] In this embodiment, the method of selecting one of model predictive control and reinforcement learning reasoning as the current control mode is: obtain the distance between the current position of the robot and the target point from the target task information, and when the distance is not greater than the set threshold L, select model predictive control as the current effective control mode; L is selected as >1 times the length of the robot; preferably, L is set to 2.4m. When the distance is greater than the threshold L, obtain the maximum height difference from the current position to the target point terrain from the robot positioning and navigation data; when the height difference is greater than the set threshold H, select reinforcement learning reasoning as the current effective control mode; when the height difference is not greater than the threshold H, maintain the current control mode unchanged; H is selected from 0 to 1 times the height of the robot; preferably, H is set to 0.3m.

[0094] As an embodiment of the present invention, when model predictive control is selected as the current control effectiveness mode, a kinematic model of a quadruped robot is established based on the data collected by the encoder and the inertial measurement unit IMU and the robot speed command, and state estimation is performed in combination with Kalman filtering to obtain instructions for controlling the current joint positions, speeds and torques of the quadruped robot; when reinforcement learning reasoning is selected as the current control effectiveness mode, the data collected by the encoder and the inertial measurement unit IMU and the robot speed command are input into the reasoning model obtained by training based on the task and safety and energy efficiency reward function for reasoning to obtain instructions for controlling the current joint positions of the quadruped robot.

[0095] Specifically, if Figure 1 As shown in the figure, when the selection result is model predictive control, the model predictive controller performs the control function, and the data selects the encoder, IMU data and robot speed command as input. After the control operation, the joint position, speed and torque commands are obtained, and the control commands are selected and executed. At this time, the reinforcement learning reasoner performs the tracking function, and the data selects the instructions of the model predictive control as input, which are directly output without reasoning, but its output instructions are not actually executed.

[0096] Dually, when the selection result is reinforcement learning, the reinforcement learning reasoner performs the control function, the data selects the encoder, IMU data and robot speed instructions as input, obtains the joint position instructions through model reasoning, and selects and executes its control instructions; at this time, the model predictive controller performs the tracking function, the data selects the reinforcement learning instructions as input, and outputs them directly without reasoning, but its output instructions are not actually executed.

[0097] It should be noted in particular that model predictive control and reinforcement learning are two control methods based on different principles. The command outputs of the control operations they perform simultaneously are quite different, and direct switching will cause large jitters. In the dynamic switching method proposed in the present invention, when one module executes control, the control command is output to another module for tracking through inter-module communication. The optimization solution of model predictive control and the reward function of reinforcement learning have change rate constraints on the change amount of the control command between the previous and next moments. In this way, both modules can obtain the actual output commands at the historical moment. The jitter caused by switching is avoided by constraining the change rate of the output commands, thereby achieving disturbance-free switching.

[0098] Furthermore, in this embodiment, the instructions output by the currently selected effective mode are used as the target value of the joint control in the robot, and the motor torque instruction is obtained through joint control operation; the motor torque instruction is input into the motor servo driver to drive the motor to operate, so as to achieve control of the movement of the quadruped robot.

[0099] In this embodiment, the formula for joint control operation is as follows:

[0100] τ m =(τ cmd +K p (q cmd -q)+K d (qd cmd -qd)) / h

[0101] In the formula, is τ m Motor torque command, τ cmd is the joint torque command, K p is the position stiffness coefficient, q cmd is the joint position command, q is the current position of the joint, K d is the velocity stiffness coefficient, qd cmd is the joint speed command, qd is the current joint speed, and h is the joint reduction ratio; in this embodiment, K p The value is 200, K d The value is 10;

[0102] Among them, in the instructions output by reinforcement learning reasoning as the effective mode, the joint torque instructions and the joint speed instructions are both zero.

[0103] like Figure 2 As shown, the steps of obtaining instructions for controlling the current joint positions, speeds and torques of the quadruped robot in the model predictive control include:

[0104] Step 1: Based on the speed measured by the encoder, the positions of each joint, the acceleration, angular velocity, and rotation angle collected by the inertial measurement unit (IMU), and the geometric parameters of the quadruped robot, a kinematic model is established;

[0105] In this embodiment, the process of establishing the kinematic model of the quadruped robot includes:

[0106] Step 1.1: According to the working condition that all four legs touch the ground, construct the state space equation describing the motion of the quadruped robot for state recursion:

[0107] x=[p b v b p0 p1 p2 p3] T

[0108]

[0109] Where x is the state vector of the quadruped robot, which includes the body position p in the global coordinate system b 、Body speed v b , the positions of the four foot ends p0, p1, p2, p3; the superscript T represents the transpose of the matrix; the superscript · represents the first-order derivative of the variable; a b is the acceleration output by the IMU, R sbis the body posture in the world coordinate system output by IMU; g is the gravitational acceleration; 0 3×1 is a 3×1 zero matrix; 0 3×3 is a 3×3 zero matrix; 0 3×12 is a 3×12 zero matrix; 0 3×18 is a 3×18 zero matrix; 0 12×18 is a 12×18 zero matrix; I3 is a 3×3 unit diagonal matrix;

[0110] Step 1.2: Based on the joint angles and angular velocities measured by the robot's joint encoders, the position p of each foot end relative to the body in the body coordinate system is calculated by forward kinematics. bfB and speed v bfB , and then we get the observation equation:

[0111]

[0112] Where y is the observation matrix of the quadruped robot; ω b is the angular velocity output by the IMU; p sfB0～4 Respectively represent the positions of the four foot ends relative to the inertial system; v sfB0～4 Respectively represent the speed of the four foot ends relative to the inertial system; p bfB0～4 Respectively represent the positions of the four foot ends relative to the machine system; v bfB0～4 Respectively represent the speeds of the four foot ends relative to the machine system; [] × is the skewness function; I 12 is a 12×12 unit diagonal matrix; 0 12×12 is a 12×12 zero matrix;

[0113] Step 2: Perform Kalman filter state estimation based on the established kinematic model to obtain the current state of the quadruped robot;

[0114] In this embodiment, the confidence level of the stable contact between the swinging foot and the ground is set to untrustworthy, that is, the corresponding items of the process noise and measurement noise covariance matrices Q and R are set to +∞, and the state space equation in step 1.1 and the observation equation in step 1.2 are combined with the Kalman filter to recursively estimate the system, and the observed state of the state vector x of the quadruped robot is obtained.

[0115]

[0116] In the formula, is the prior estimate of the robot state vector x at time k; and are the posterior estimates of the robot state vector x at time k and time k-1 respectively; u k-1is the matrix u at time k-1 in the state space equation; y k is the observation matrix y at time k; is the prior estimate of the P matrix at time k; and The P matrix is ​​the posterior estimate at time k and time k-1 respectively; the P matrix is ​​the covariance matrix of the robot state vector x; Q and R are the process noise and measurement noise covariance matrices respectively.

[0117] It is further explained that the current state of the quadruped robot obtained in step 2 includes position, speed, rotation angle and angular velocity; among them, the position and speed are obtained from the observed state The rotation angle and angular velocity are directly measured by IMU.

[0118] Step 3: Based on the current state of the quadruped robot, timing planning is performed for the movement of the sole of the foot in the ground contact phase and the swing phase respectively;

[0119] like Figure 2 As shown, the timing planning of the swing phase includes planning the swing trajectory of the foot from the swinging plantar to the target position; the timing planning of the touchdown phase includes solving the plantar force by establishing a single rigid body dynamics model;

[0120] Step 4, performing inverse kinematics on the planned foot swing trajectory to obtain the joint position and speed of the swing foot of the quadruped robot;

[0121] Step 5: Perform inverse dynamics or inverse statics to solve the plantar force to obtain the joint torque of the ground-contacting foot of the quadruped robot.

[0122] In this embodiment, the swing foot trajectory planning of the model predictive control in step 3 can plan the position and speed of the sole of the foot according to the starting point and the target point using a Bezier curve, a cycloid or a spline curve method. It is further explained that the swing foot trajectory planning in step 3, the inverse kinematics solution in step 4, and the inverse dynamics or inverse statics solution in step 5 are all prior arts and are not the research focus of the present invention, and the specific solution process will not be described in detail here.

[0123] As an embodiment of the present invention, the specific steps of timing planning of the touchdown phase in step 3 include:

[0124] Step 3.1, establish a single rigid body dynamics model based on the geometric and physical parameters of the quadruped robot;

[0125] In this embodiment, combining Newton's second law and the angular momentum theorem, the expression of the single rigid body dynamics model is established as follows:

[0126]

[0127]

[0128] In the formula, represents the fuselage position p in the global coordinate system b The second derivative of represents the first derivative of a variable; f i is the plantar force of the ith foot of the quadruped robot, m is the mass of the quadruped robot, g is the acceleration due to gravity, N is the moment of inertia of the quadruped robot, and r i is the position vector from the sole of the i-th foot of the quadruped robot to the center of mass; ω b is the angular velocity output by the IMU.

[0129] Step 3.2: Convert the single rigid body dynamics model into a state space form and discretize it to obtain an equality constraint model that retains the angular velocity term:

[0130]

[0131] In the formula, z(k+1) and z(k) represent the values ​​of vector z at time k+1 and time k respectively; E k represents the value of vector E at time k; F k represents the value of vector F at time k; f(k) represents the value of vector f at time k; where, And the expressions of vectors z, f, E, and F are as follows:

[0132]

[0133]

[0134] f=[f0 L f3] T

[0135]

[0136]

[0137] Where Δt is the discretized time; Θ is the Euler angle of the quadruped robot; R(Θ) is the conversion matrix between angular velocity and Euler angular rate; 1 3×3 is a 3×3 matrix whose elements are all 1; [] × is the skewness function; the superscript T indicates the transpose of the matrix; the superscript · indicates the first-order derivative of the variable; 0 1×3 is a 1×3 zero matrix; 0 3×3 is a 3×3 zero matrix.

[0138] Step 3.3, input the current state and speed command into the equation constraint model for prediction recursion to obtain the predicted state within the future set time period n; in this embodiment, n is selected from 10 to 20; one time period is 30ms.

[0139] Step 3.4: Taking the minimum deviation between the predicted state and the control target within the set time period as the solution target, and introducing the friction constraint, the QP problem model is constructed:

[0140]

[0141] f x |≤μf z ,|f y |≤μf z ,f z >0

[0142] Where n represents the number of set time periods; z ref (k+1) represents the control target command issued to the quadruped robot at time k+1, the translational velocity and angular velocity of which are given by the upper-level velocity command, the position command is obtained by integrating the translational velocity command, and the angle command is obtained by integrating the angular velocity command; f x 、f y 、f z are the x, y, and z components of the plantar force in the world coordinate system; μ is the friction coefficient of the ground; W and G are two different weight matrices.

[0143] Step 3.5: Solve the QP problem model to obtain the plantar force of the quadruped robot.

[0144] Furthermore, the mathematical solver of the QP problem in the prior art can be used to solve the above model, which will not be listed here in detail.

[0145] In this embodiment, the construction and training of the reasoning model in reinforcement learning reasoning include: building a quadruped robot training environment with neural network training capabilities on a simulation platform, constructing a reinforcement learning reward function and calculating the score based on the robot's action response; setting the training termination condition based on the score and the number of training times to obtain a pre-trained reasoning model; preferably, the training ends when the number of training times is set to >5000 times.

[0146] It should be noted that the training framework and process in the inference model of this embodiment can be provided by a simulation platform in the prior art, and the specific training process is not described in detail. The training improvement of the inference model of the present invention is that, in view of the fact that the existing reinforcement learning inference method only uses the fixed reward of the environment in the construction of the reward function and cannot solve the problem of sparse rewards, a reward function based on tasks and safety energy efficiency is set for model training to improve the training effect and enhance the prediction accuracy of the model;

[0147] As an embodiment of the present invention, the expression of the reward function based on task and safety energy efficiency is as follows:

[0148]

[0149]

[0150]

[0151] In the formula, the reward function r k Including task reward function and safety energy efficiency reward function α, β, γ, and λ are four different adjustable hyperparameters; is the linear speed instruction; v xy is the measured value of line speed; is the angular velocity command; ω z is the angular velocity measurement; τ is the joint torque; is the measured value of joint acceleration; IsColl(k) is the collision detection value of the quadruped robot at the current moment k.

[0152] Reference Figure 3 The control mode result diagram of the experiment using the quadruped robot motion control method of the present invention that integrates model predictive control and reinforcement learning reasoning; the experimental object is a quadruped inspection robot of a substation with 12 degrees of freedom, a standing height of 0.5m, and a fuselage length of 0.8m; it is equipped with encoders, IMU inertial measurement instruments, laser radars, visible light cameras and other sensors, positioning navigation and planning decision systems; it needs to pass through complex terrains such as cement pavement, grass, mud, cobblestone roads, single-layer steps, and continuous stairs in the substation to complete the inspection task; during the march, the quadruped robot switches to reinforcement learning reasoning in rugged terrains that require high passability, such as steps, continuous stairs, the intersection of cement pavement and grass, and switches to model predictive control near the target inspection point that requires precise control. The experimental results show that the posture of the method of the present invention accurately tracks the given instructions, accurately reaches the given coordinate point, and realizes unmanned automatic completion of the inspection task.

[0153] The beneficial effects of the present invention are as follows:

[0154] 1) The control process of the quadruped robot movement in the present invention integrates the two control methods of model predictive control and reinforcement learning reasoning. In the stage of traveling at a large distance from the destination point, the adaptive advantage of reinforcement learning reasoning is used to pass through complex terrain; in the stage of approaching the target point, the precise characteristics of model predictive control are used to achieve accurate control; in the model predictive control stage, since the quadruped robot is already close to the target point, a single flat ground model can be used to describe the terrain features, avoiding the problem of difficult modeling of terrain in model predictive control; the problem of difficulty in balancing adaptive passage through complex terrain and accurate arrival at the target position is solved.

[0155] 2) The present invention removes the plantar height observation term in the observation equation of the model predictive control state observation link, adds the angular velocity term in the physical equation of the plantar force optimization solution link, accurately models the simplified terms of conventional model predictive control, and further improves the accuracy of model predictive control.

[0156] 3) In the model training link of reinforcement learning reasoning, the present invention constructs a reward function based on tasks and safety and energy efficiency, which can solve the problem of not being able to obtain good training results when the rewards are sparse due to fixed rewards in the environment, and improves the control accuracy of reinforcement learning reasoning.

[0157] 4) The present invention maximizes the advantages of model predictive control and reinforcement learning. When one method of model predictive control or reinforcement learning reasoning is not selected as the output, it will track the output instructions of the other party in real time, and can perform undisturbed rapid switching. The switching process is completed automatically, thereby realizing intelligent control of the quadruped robot across different complex terrains.

[0158] Embodiment 2:

[0159] The present invention provides a quadruped robot motion control system, which is used to implement the steps of the method in the first embodiment. The system includes a control scheduler, a model predictive controller and a reinforcement learning inference device that are interconnected and communicate with each other; wherein:

[0160] A control scheduler is used to dynamically select the output instructions of the control modes in the model predictive controller and the reinforcement learning reasoner as the effective instructions for controlling the current trajectory of the robot according to the target task information and robot positioning and navigation data obtained in real time, and the other control mode executes and tracks the instructions output by the effective mode to achieve non-disturbance switching between the two control modes and complete the motion control of the quadruped robot;

[0161] A model predictive controller is used to establish a kinematic model of the quadruped robot based on the data collected by the encoder and the inertial measurement unit IMU and the robot speed command, and to perform state estimation in combination with the Kalman filter to obtain commands for controlling the current joint position, speed and torque of the quadruped robot; or to track the output command of the reinforcement learning reasoner;

[0162] The reinforcement learning reasoner is used to input the data collected by the encoder and the inertial measurement unit IMU and the robot speed command into the inference model trained based on the task and safety energy efficiency reward function for reasoning, so as to obtain the command for controlling the current joint positions of the quadruped robot; or to track the output command of the model prediction controller.

[0163] Furthermore, the system in the embodiment of the present invention further includes:

[0164] The joint controller is used to use the command output by the currently selected effective mode as the target value of the joint control in the robot, and obtain the motor torque command through joint control calculation;

[0165] The motor servo driver is used to execute the motor torque command to drive the motor to operate, so as to achieve the control of the movement of the quadruped robot.

[0166] The quadruped robot motion control system provided in the embodiment of the present invention and the quadruped robot motion control method provided in Example 1 are based on the same technical concept, and can produce the beneficial effects described in Example 1. For the contents not described in detail in this embodiment, please refer to Example 1.

[0167] Embodiment three:

[0168] An embodiment of the present invention provides a terminal, including a processor and a storage medium;

[0169] The storage medium is used to store instructions;

[0170] The processor is used to operate according to the instruction to execute the steps of the method according to any one of the embodiments.

[0171] Embodiment 4:

[0172] An embodiment of the present invention provides a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, the steps of any method described in Embodiment 1 are implemented.

[0173] The present disclosure may be a system, a method and / or a computer program product. The computer program product may include a computer-readable storage medium carrying computer-readable program instructions for causing a processor to implement various aspects of the present disclosure.

[0174] A computer-readable storage medium may be a tangible device that can hold and store instructions used by an instruction execution device. A computer-readable storage medium may be, for example, but not limited to, an electrical storage device, a magnetic storage device, an optical storage device, an electromagnetic storage device, a semiconductor storage device, or any suitable combination of the foregoing. More specific examples of computer-readable storage media (a non-exhaustive list) include: a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), a static random access memory (SRAM), a portable compact disk read-only memory (CD-ROM), a digital versatile disk (DVD), a memory stick, a floppy disk, a mechanical encoding device, such as a punch card or a raised structure in a groove on which instructions are stored, and any suitable combination of the foregoing. As used herein, a computer-readable storage medium is not to be interpreted as a transient signal per se, such as a radio wave or other freely propagating electromagnetic wave, an electromagnetic wave propagating through a waveguide or other transmission medium (e.g., a light pulse through a fiber optic cable), or an electrical signal transmitted through a wire.

[0175] The computer-readable program instructions described herein can be downloaded from a computer-readable storage medium to each computing / processing device, or downloaded to an external computer or external storage device via a network, such as the Internet, a local area network, a wide area network, and / or a wireless network. The network can include copper transmission cables, optical fiber transmissions, wireless transmissions, routers, firewalls, switches, gateway computers, and / or edge servers. The network adapter card or network interface in each computing / processing device receives the computer-readable program instructions from the network and forwards the computer-readable program instructions for storage in the computer-readable storage medium in each computing / processing device.

[0176] The computer program instructions for performing the operation of the present disclosure may be assembly instructions, instruction set architecture (ISA) instructions, machine instructions, machine-related instructions, microcode, firmware instructions, state setting data, or source code or object code written in any combination of one or more programming languages, including object-oriented programming languages, such as Smalltalk, C++, etc., and conventional procedural programming languages, such as "C" language or similar programming languages. Computer-readable program instructions may be executed completely on a user's computer, partially on a user's computer, as an independent software package, partially on a user's computer, partially on a remote computer, or completely on a remote computer or server. In the case of a remote computer, the remote computer may be connected to the user's computer via any type of network, including a local area network (LAN) or a wide area network (WAN), or may be connected to an external computer (e.g., using an Internet service provider to connect via the Internet). In some embodiments, an electronic circuit, such as a programmable logic circuit, a field programmable gate array (FPGA), or a programmable logic array (PLA), may be customized by utilizing the state information of the computer-readable program instructions, and the electronic circuit may execute the computer-readable program instructions, thereby realizing various aspects of the present disclosure.

[0177] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents, and any modifications or equivalent replacements that do not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A quadruped robot motion control method, characterized in that the method include: According to the target task information and robot positioning and navigation data acquired in real time, one of the control modes of model predictive control and reinforcement learning reasoning is dynamically selected as the effective mode for controlling the current trajectory of the robot, and the other control mode executes and tracks the instructions output by the effective mode to achieve disturbance-free switching between the two control modes and complete the motion control of the quadruped robot; Among them, when model predictive control is selected as the current control effectiveness mode, the kinematic model of the quadruped robot is established according to the data collected by the encoder and inertial measurement unit IMU and the robot speed command, and the state estimation is performed in combination with Kalman filtering to obtain the instructions for controlling the current joint position, speed and torque of the quadruped robot; When reinforcement learning reasoning is selected as the current control effectiveness mode, the data collected by the encoder and inertial measurement unit IMU and the robot speed command are input into the reasoning model trained based on the task and safety and energy efficiency reward function for reasoning, and the instructions for controlling the current joint positions of the quadruped robot are obtained.

2. The quadruped robot motion control method according to claim 1, characterized in that: The method further comprises the following steps: The command output by the currently selected effective mode is used as the target value of the joint control in the robot, and the motor torque command is obtained through joint control calculation; The motor torque command is input into the motor servo driver to drive the motor to operate, so as to achieve the control of the movement of the quadruped robot.

3. The quadruped robot motion control method according to claim 1, characterized in that: The step of selecting the validation mode comprises: The distance between the current position of the robot and the target point is obtained from the target task information. When the distance is not greater than the set threshold L, the model predictive control is selected as the currently effective control method. When the distance is greater than the threshold L, the maximum height difference between the current position and the target point terrain is obtained from the robot positioning and navigation data; when the height difference is greater than the set threshold H, reinforcement learning reasoning is selected as the currently effective control method; when the height difference is not greater than the threshold H, the current control method is maintained unchanged.

4. The quadruped robot motion control method according to claim 1, characterized in that: The steps of obtaining instructions for controlling the current joint positions, speeds and torques of the quadruped robot include: The kinematic model of the quadruped robot is established based on the speed measured by the encoder, the position of each joint, the acceleration, angular velocity, and rotation angle collected by the inertial measurement unit (IMU), and the geometric parameters of the quadruped robot. Combined with the established kinematic model, the Kalman filter is used to estimate the state and obtain the current state of the quadruped robot. Based on the current state of the quadruped robot, timing planning is performed for the movement of the sole of the foot in the contact phase and the swing phase respectively; wherein the timing planning of the swing phase includes planning the swing trajectory of the sole of the foot to the target position; the timing planning of the contact phase includes solving the sole force by establishing a single rigid body dynamics model; Performing inverse kinematics on the planned foot swing trajectory to obtain the joint position and speed of the swing foot of the quadruped robot; The plantar force is solved by inverse dynamics or inverse statics to obtain the joint torque of the ground contacting foot of the quadruped robot.

5. The quadruped robot motion control method according to claim 1 or 4, characterized in that: The steps of establishing a kinematic model of the quadruped robot and performing state estimation in combination with Kalman filtering include: According to the working condition that all four legs touch the ground, the state space equation describing the motion of the quadruped robot is constructed: x=[p b in b p0 p1 p2 p3] T Where x is the state vector of the quadruped robot, which includes the body position p in the global coordinate system b 、Body speed v b , the positions of the four foot ends p0, p1, p2, p3; the superscript T represents the transpose of the matrix; the superscript · represents the first-order derivative of the variable; a b is the acceleration output by the IMU, R sb is the body posture in the world coordinate system output by IMU; g is the gravitational acceleration; 0 3×1 is a 3×1 zero matrix; 0 3×3 is a 3×3 zero matrix; 0 3×12 is a 3×12 zero matrix; 0 3×18 is a 3×18 zero matrix; 0 12×18 is a 12×18 zero matrix; I3 is a 3×3 unit diagonal matrix; According to the joint angles and angular velocities measured by the robot's joint encoders, the position p of each foot end relative to the body in the body coordinate system is calculated by forward kinematics. bfB and speed v bfB , and then we get the observation equation: Where y is the observation matrix of the quadruped robot; ω b is the angular velocity output by the IMU; p sfB0～4 Respectively represent the positions of the four foot ends relative to the inertial system; v sfB0～4 Respectively represent the speed of the four foot ends relative to the inertial system; p bfB0～4 Respectively represent the positions of the four foot ends relative to the machine system; v bfB0～4 Respectively represent the speeds of the four foot ends relative to the machine system; [] × is the skewness function; I 12 is a 12×12 unit diagonal matrix; 0 12×12 is a 12×12 zero matrix; The confidence level of the stable contact between the swinging foot and the ground is set to untrustworthy, that is, the corresponding items of the process noise and measurement noise covariance matrices Q and R are set to +∞, and the state space equation and the observation equation are combined with the Kalman filter to recursively estimate the system to obtain the observed state of the state vector x of the quadruped robot 6. The quadruped robot motion control method according to claim 5, characterized in that: The state space equation and observation equation are combined into the Kalman filter to perform recursive estimation of the system as follows: In the formula, is the prior estimate of the robot state vector x at time k; and are the posterior estimates of the robot state vector x at time k and time k-1 respectively; u k-1 is the matrix u at time k-1 in the state space equation; y k is the observation matrix y at time k; is the prior estimate of the P matrix at time k; and The P matrix is ​​the posterior estimate at time k and time k-1 respectively; the P matrix is ​​the covariance matrix of the robot state vector x; Q and R are the process noise and measurement noise covariance matrices respectively.

7. The quadruped robot motion control method according to claim 4, characterized in that: The specific steps of the timing planning of the touchdown phase include: According to the geometric and physical parameters of the quadruped robot, a single rigid body dynamics model is established; The single rigid body dynamics model is converted into a state space form and discretized to obtain an equality constraint model retaining an angular velocity term; Inputting the current state and speed command into the equation constraint model for prediction recursion to obtain the predicted state within a future set time period; The QP problem model is constructed by taking the minimum deviation between the predicted state and the control target within the set time period as the solution goal, introducing friction constraints, and constructing the QP problem model; The QP problem model is solved to obtain the plantar force of the quadruped robot.

8. The quadruped robot motion control method according to claim 7, characterized in that: The expression for establishing the single rigid body dynamics model is as follows: In the formula, represents the fuselage position p in the global coordinate system b The second derivative of represents the first derivative of a variable; f i is the plantar force of the ith foot of the quadruped robot, m is the mass of the quadruped robot, g is the acceleration due to gravity, N is the moment of inertia of the quadruped robot, and r i is the position vector from the sole of the i-th foot of the quadruped robot to the center of mass; ω b is the angular velocity output by the IMU.

9. The quadruped robot motion control method according to claim 8, characterized in that: The expression of the equality constraint model retaining the angular velocity term is as follows: In the formula, z(k+1) and z(k) represent the values ​​of vector z at time k+1 and time k respectively; E k represents the value of vector E at time k; F k represents the value of vector F at time k; f(k) represents the value of vector f at time k; where, And the expressions of vectors z, f, E, and F are as follows: <h2 style=";text-align:left;direction:ltr">f=[f0 L f3]<h2 style=";text-align:left;direction:ltr"> T Where Δt is the discretized time; Θ is the Euler angle of the quadruped robot; R(Θ) is the conversion matrix between angular velocity and Euler angular rate; 1 3×3 is a 3×3 matrix whose elements are all 1; [] × is the skewness function; the superscript T indicates the transpose of the matrix; the superscript · indicates the first-order derivative of the variable; 0 1×3 is a 1×3 zero matrix; 0 3×3 is a 3×3 zero matrix.

10. The quadruped robot motion control method according to claim 9, characterized in that: The expression for constructing the QP problem model is as follows: |f x |≤μf z ,|f y |≤μf z ,f z >0 Where n represents the number of set time periods; z ref (k+1) represents the control target instruction given to the quadruped robot at time k+1; f x 、f y 、f z are the x, y, and z components of the plantar force in the world coordinate system; μ is the friction coefficient of the ground; W and G are two different weight matrices.

11. The quadruped robot motion control method according to claim 1, characterized in that: The expression of the reward function based on task and safety energy efficiency is as follows: In the formula, the reward function r k Including task reward function and safety energy efficiency reward function α, β, γ, and λ are four different adjustable hyperparameters; is the linear speed instruction; v xy is the measured value of line speed; is the angular velocity command; ω z is the angular velocity measurement; τ is the joint torque; is the measured value of joint acceleration; IsColl(k) is the collision detection value of the quadruped robot at time k.

12. The quadruped robot motion control method according to claim 2, characterized in that: The command output by the currently selected effective mode is used as the target value of the joint controller in the robot, and after the joint control operation, the calculation formula for obtaining the motor torque command is as follows: τ m =(τ cmd +K p (q cmd -q)+K d (qd cmd -qd)) / h In the formula, is τ m Motor torque command, τ cmd is the joint torque command, K p is the position stiffness coefficient, q cmd is the joint position command, q is the current position of the joint, K d is the velocity stiffness coefficient, qd cmd is the joint speed command, qd is the current joint speed, and h is the joint reduction ratio; Among them, in the instructions output by reinforcement learning reasoning as the effective mode, the joint torque instructions and the joint speed instructions are both zero.

13. A quadruped robot motion control system, running the quadruped robot motion control method according to any one of claims 1 to 12, characterized in that: The system includes a control scheduler, a model predictive controller, and a reinforcement learning reasoner that are communicatively connected to each other; wherein: A control scheduler is used to dynamically select the output instructions of the control modes in the model predictive controller and the reinforcement learning reasoner as the effective instructions for controlling the current trajectory of the robot according to the target task information and robot positioning and navigation data obtained in real time, and the other control mode executes and tracks the instructions output by the effective mode to achieve non-disturbance switching between the two control modes and complete the motion control of the quadruped robot; A model predictive controller is used to establish a kinematic model of the quadruped robot based on the data collected by the encoder and the inertial measurement unit IMU and the robot speed command, and to perform state estimation in combination with the Kalman filter to obtain commands for controlling the current joint position, speed and torque of the quadruped robot; or to track the output command of the reinforcement learning reasoner; The reinforcement learning reasoner is used to input the data collected by the encoder and the inertial measurement unit IMU and the robot speed command into the inference model trained based on the task and safety energy efficiency reward function for reasoning, so as to obtain the command for controlling the current joint positions of the quadruped robot; or to track the output command of the model prediction controller.

14. The quadruped robot motion control system according to claim 13, characterized in that: The system further comprises: The joint controller is used to use the command output by the currently selected effective mode as the target value of the joint control in the robot, and obtain the motor torque command through joint control calculation; The motor servo driver is used to execute the motor torque command to drive the motor to operate, so as to achieve the control of the movement of the quadruped robot.

15. A terminal comprising a processor and a storage medium; characterized in that: The storage medium is used to store instructions; The processor is configured to operate according to the instructions to execute the steps of the method according to any one of claims 1-12.

16. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps of the method according to any one of claims 1 to 12 are implemented.

Images