Behavior-driven fine-grained evolutionary optimization model for the servo layer of quadruped robots

Abstract

The present invention belongs to the field of robot motion control technology, and specifically relates to a behavior-driven fine-grained evolutionary optimization model of the servo layer of a quadruped robot, including: combining the Jacobian dynamics of the robot to estimate the plantar force online; using a reverse Smith trigger to distinguish support and swing behaviors online and switch the behavior of the control channel; based on the switching result, using position + velocity for the swing behavior control channel to track the desired joint position and force command; using position + velocity + force feedforward for the support behavior control channel to track the desired joint position and force command; using an ADRC controller to estimate the system disturbance online, and fine-grained optimization of the channel control parameters. This solution can improve the reliability of the foot-ground contact and the robustness of the force control of the quadruped robot in an unknown environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot motion control, and in particular relates to a behavior-driven quadruped robot servo layer fine-grained evolutionary optimization model. Background Art

[0002] The control system of the quadruped robot can be deconstructed into a hierarchical real-time control system composed of a "brain layer", a "central layer" and a "peripheral layer" based on a humanoid control mechanism. The "brain layer" completes the deep perception of the environmental situation and generates corresponding coarse-grained trajectory planning control commands to the "central layer"; the "central layer" generates corresponding fine-grained actions through the VMC virtual model control method under the drive of the above instructions; finally, the "peripheral layer" tracks the fine-grained action instructions by driving the servo actuator, thereby realizing the "coarse to fine" control of the entire quadruped robot.

[0003] For quadruped robots, the "servo layer" control mainly completes the tracking of fine-grained swing and support position-speed and force instructions, and realizes the switching of the above three channels according to different compliance requirements. The use of fixed impedance and stiffness coefficients in traditional control methods will cause problems such as large impact, slippage and slow tracking under some working conditions. Summary of the invention

[0004] 1. Technical issues to be resolved

[0005] The technical problem to be solved by the present invention is: how to propose a behavior-driven fine-grained evolution optimization model for the servo layer of a quadruped robot, how to realize online switching of the servo layer impedance channel and online optimization of fine-grained impedance parameters, so as to improve the robustness and reliability of the servo control of the quadruped robot.

[0006] (II) Technical solution

[0007] In order to solve the above technical problems, the present invention provides a behavior-driven fine-grained evolution optimization model for the servo layer of a quadruped robot. The implementation process of the fine-grained evolution optimization model for the servo layer of a quadruped robot specifically includes the following steps:

[0008] Step 1: Combined with the Jacobian dynamics of the robot, the plantar force is estimated online;

[0009] Step 2: Use the reverse Smith trigger to perform online discrimination between support and swing behaviors and control channel behavior switching;

[0010] Step 3: Based on the switching result of step 2,

[0011] For the swing behavior control channel, position + velocity is used to track the desired joint position and force command;

[0012] Position + velocity + force feedforward is used for the support behavior control channel to track the desired joint position and force command;

[0013] Step 4: Use the active disturbance rejection (ADRC) controller to estimate the system disturbance online and perform fine-grained optimization of the channel control parameters.

[0014] Wherein, in said step 1:

[0015] Firstly, based on force perception and combined with the Jacobi dynamics of the robot, the plantar force is estimated online, and the robot leg joint angle and posture sensor data are collected;

[0016] Based on the robot structural parameters and single-leg configuration, the joint angle feedback value is used to calculate the Jacobian matrix J of the i-th leg using the kinematics forward solution. i , based on the principle of virtual work, the plantar force in the body coordinate system {B} is calculated in is the measured value of the motor torque of the leg;

[0017] Based on the attitude quaternions q0, q1, q2, q3 measured by the robot's onboard IMU, the transformation matrix between the global coordinate system {N} and the body coordinate system {B} is calculated:

[0018]

[0019] in, is the transformation matrix between the global coordinate system {N} and the body coordinate system {B}; the plantar force of the foot end of the i-th leg in the body coordinate system {B} is converted to the plantar force in the global coordinate system {N} according to the following formula:

[0020]

[0021] Wherein, in step 2: based on the global plantar force estimation result, a reverse Smith trigger is used to determine whether the current i-th leg is in a supporting behavior or a swinging behavior. The mathematical description of the reverse Smith trigger is as follows:

[0022]

[0023] The reverse Smith trigger is set by setting the rising threshold T up And the falling threshold T down And the corresponding judgment condition, i.e., the counter time t, is used to construct a reverse trigger loop with good fault tolerance. When in the support behavior, if the input plantar force estimation result is greater than the set descending threshold T down , the corresponding counter time t increases, otherwise it is cleared; when the counter time t is greater than the set time threshold t down, the behavior switches, and the same logic applies to the swing behavior.

[0024] Among them, in step 3: based on the behavior switching result in step 2, different impedance control channels are enabled, the position-speed control channel is used for the swing behavior, and the position-speed-force control channel is used for the supporting behavior, and finally the tracking of the desired joint position and force command is achieved.

[0025] Wherein, the step 3 specifically includes:

[0026] Step 31: Construct the position control channel design and calculate the current end position x and speed based on the robot joint angle and kinematic inverse solution and acceleration

[0027]

[0028] Among them, q is the current joint angle, is the current joint angular velocity, IK{} is the kinematic mapping function; based on the expected end planning position result x d Construct impedance control rate to generate virtual force f p Change of traction end position:

[0029]

[0030] Among them, K p ,K d are stiffness and damping coefficients;

[0031] Step 32: Design a velocity impedance loop, whose input is the fine-grained angular velocity instruction planned by the central layer, and the feedback data is the angular velocity result calculated by the differential calculation of the terminal layer. Stiffness + damping is used, based on the expected terminal angular velocity Construct feedback impedance control rate and integrate feedforward proportional control to generate virtual force:

[0032]

[0033] The virtual traction force calculated in step 31 and step 32 is mapped into a corresponding torque command using a Jacobian matrix;

[0034]

[0035] Step 33: Design a force-impedance controlled loop based on the desired force F d Construct feedback impedance control rate and integrate feedforward proportional control to generate virtual traction:

[0036]

[0037] Step 34: Use Jacobi dynamics to map the virtual traction in step 33 into torque, and mix and superimpose the outputs:

[0038]

[0039] Step 35: Combine the torque commands in steps 32 and 34 and output them to the physical actuator:

[0040] τ=τ pv +τ F .

[0041] Wherein, in the step 4: construct an ADRC controller to estimate the disturbance in the control channel and adjust the fine-grained control parameters.

[0042] Wherein, the step 4 specifically includes:

[0043] First, an extended state observation is established to estimate the disturbance in each control channel online, and the states of each order of the control error are used for tracking processing:

[0044]

[0045] Where b is the model gain coefficient, u is the control output of the current channel, is the estimated value of the system state, L1, L2, L3 are the observer gain coefficients, which are calculated using the observer bandwidth w:

[0046] L1=3w,L2=3w 2 ,L3=w 3

[0047] The current total disturbance of the system is calculated by the ADRC controller as z 13 , then it is used to compensate the control output of each channel to realize the fine-grained evolution of servo control, and the control amount correction value is:

[0048]

[0049] Among them, in the step 4, in the case of the position control channel, the control amount correction value is set to The corrected traction force is:

[0050]

[0051] Among them, it realizes fine-grained evolutionary optimization of servo layer behavior based on the idea of ​​multi-modal switching control, thereby improving the reliability of force servo control of quadruped robots in unknown terrains.

[0052] Among them, it can improve the reliability of foot-ground contact and the robustness of force control of quadruped robots in unknown environments.

[0053] (III) Beneficial effects

[0054] The present invention adopts the self-disturbance rejection theory to realize the self-regulation of the above control parameters and to compensate for unknown disturbances in advance, thereby realizing the fine-grained evolution of the "peripheral layer" control model.

[0055] Compared with the prior art, the present invention proposes a behavior-driven fine-grained evolution optimization model for the servo layer of a quadruped robot, which adopts a multi-modal switching control strategy to improve the intelligence level of the servo layer behavior of the quadruped robot, realizes force perception of ground contact through body kinematics and IMU and other perception data, and further switches the servo layer impedance control channel online. In order to improve the tracking reliability of force servo instructions, ADRC is used to perform online estimation of the foot-ground contact disturbance, and further compensate in advance, thereby realizing the fine-grained evolution of the servo layer parameters. The present invention can improve the reliability of foot-ground contact and the robustness of force control of the quadruped robot in an unknown environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 Flowchart of the fine-grained evolutionary optimization process for the servo layer of a behavior-driven quadruped robot.

[0057] Figure 2 Schematic diagram of the reverse Smith trigger structure principle for switching between support and swing behavior. DETAILED DESCRIPTION

[0058] In order to make the purpose, content, and advantages of the present invention more clear, the specific implementation methods of the present invention are further described in detail below in conjunction with the accompanying drawings and examples.

[0059] In order to solve the above technical problems, the present invention provides a behavior-driven quadruped robot servo layer fine-grained evolution optimization model, such as Figure 1 As shown, the implementation process of the fine-grained evolution optimization model of the quadruped robot servo layer specifically includes the following steps:

[0060] Step 1: Combined with the Jacobian dynamics of the robot, the plantar force is estimated online;

[0061] Step 2: Use the reverse Smith trigger to perform online discrimination between support and swing behaviors and control channel behavior switching;

[0062] Step 3: Based on the switching result of step 2,

[0063] For the swing behavior control channel, position + velocity is used to track the desired joint position and force command;

[0064] Position + velocity + force feedforward is used for the support behavior control channel to track the desired joint position and force command;

[0065] Step 4: Use the active disturbance rejection (ADRC) controller to estimate the system disturbance online and perform fine-grained optimization of the channel control parameters.

[0066] Wherein, in said step 1:

[0067] Firstly, based on force perception and combined with the Jacobi dynamics of the robot, the plantar force is estimated online, and the robot leg joint angle and posture sensor data are collected;

[0068] Based on the robot structural parameters and single-leg configuration, the joint angle feedback value is used to calculate the Jacobian matrix J of the i-th leg using the kinematics forward solution. i , based on the principle of virtual work, the plantar force in the body coordinate system {B} is calculated in is the measured value of the motor torque of the leg;

[0069] Based on the attitude quaternions q0, q1, q2, q3 measured by the robot's onboard IMU, the transformation matrix between the global coordinate system {N} and the body coordinate system {B} is calculated:

[0070]

[0071] in, is the transformation matrix between the global coordinate system {N} and the body coordinate system {B}; the plantar force of the foot end of the i-th leg in the body coordinate system {B} is converted to the plantar force in the global coordinate system {N} according to the following formula:

[0072]

[0073] Wherein, in step 2: based on the global plantar force estimation result, a reverse Smith trigger is used to determine whether the current i-th leg is in a supporting behavior or a swinging behavior. The mathematical description of the reverse Smith trigger is as follows:

[0074]

[0075] The reverse Smith trigger is set by setting the rising threshold T up And the falling threshold T down And the corresponding judgment condition, i.e., the counter time t, is used to construct a reverse trigger loop with good fault tolerance. When in the support behavior, if the input plantar force estimation result is greater than the set descending threshold T down , the corresponding counter time t increases, otherwise it is cleared; when the counter time t is greater than the set time threshold t downWhen , the behavior switches. Similarly, the same logic is used for the swing behavior. The switching process principle is as follows Figure 2 shown.

[0076] Among them, in step 3: based on the behavior switching result in step 2, different impedance control channels are enabled, the position-speed control channel is used for the swing behavior, and the position-speed-force control channel is used for the supporting behavior, and finally the tracking of the desired joint position and force command is achieved.

[0077] Wherein, the step 3 specifically includes:

[0078] Step 31: Construct the position control channel design and calculate the current end position x and speed based on the robot joint angle and kinematic inverse solution and acceleration

[0079] x:

[0080]

[0081] Among them, q is the current joint angle, is the current joint angular velocity, IK{} is the kinematic mapping function; based on the expected end planning position result x d Construct impedance control rate to generate virtual force f p Change of traction end position:

[0082]

[0083] Among them, K p ,K d are stiffness and damping coefficients;

[0084] Step 32: Design a velocity impedance loop, whose input is the fine-grained angular velocity instruction planned by the central layer, and the feedback data is the angular velocity result calculated by the differential calculation of the terminal layer. Stiffness + damping is used, based on the expected terminal angular velocity Construct feedback impedance control rate and integrate feedforward proportional control to generate virtual force:

[0085]

[0086] The virtual traction force calculated in step 31 and step 32 is mapped into a corresponding torque command using a Jacobian matrix;

[0087]

[0088] Step 33: Design a force-impedance controlled loop based on the desired force F d Construct feedback impedance control rate and integrate feedforward proportional control to generate virtual traction:

[0089]

[0090] Step 34: Use Jacobi dynamics to map the virtual traction in step 33 into torque, and mix and superimpose the outputs:

[0091]

[0092] Step 35: Combine the torque commands in steps 32 and 34 and output them to the physical actuator:

[0093] τ=τ pv +τ F .

[0094] Wherein, in the step 4: construct an ADRC controller to estimate the disturbance in the control channel and adjust the fine-grained control parameters.

[0095] Wherein, the step 4 specifically includes:

[0096] First, an extended state observation is established to estimate the disturbance in each control channel online, and the states of each order of the control error are used for tracking processing:

[0097]

[0098] Where b is the model gain coefficient, u is the control output of the current channel, is the estimated value of the system state, L1, L2, L3 are the observer gain coefficients, which are calculated using the observer bandwidth w:

[0099] L1=3w,L2=3w 2 ,L3=w 3

[0100] The current total disturbance of the system is calculated by the ADRC controller as z 13 , then it is used to compensate the control output of each channel to realize the fine-grained evolution of servo control, and the control amount correction value is:

[0101]

[0102] Among them, in the step 4, in the case of the position control channel, the control amount correction value is set to The corrected traction force is:

[0103]

[0104] Among them, it realizes fine-grained evolutionary optimization of servo layer behavior based on the idea of ​​multi-modal switching control, thereby improving the reliability of force servo control of quadruped robots in unknown terrains.

[0105] Among them, it can improve the reliability of foot-ground contact and the robustness of force control of quadruped robots in unknown environments.

[0106] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the technical principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A behavior-driven fine-grained evolutionary optimization model for the servo layer of a quadruped robot, characterized in that: The implementation process of the fine-grained evolution optimization model of the quadruped robot servo layer specifically includes the following steps: Step 1: Combined with the Jacobian dynamics of the robot, the plantar force is estimated online; Step 2: Use the reverse Smith trigger to perform online discrimination between support and swing behaviors and control channel behavior switching; Step 3: Based on the switching result of step 2, For the swing behavior control channel, position + velocity is used to track the desired joint position and force command; Position + velocity + force feedforward is used for the support behavior control channel to track the desired joint position and force command; Step 4: Use the ADRC controller to estimate the system disturbance online and perform fine-grained optimization of the channel control parameters; In step 1: Firstly, based on force perception and combined with the Jacobi dynamics of the robot, the plantar force is estimated online, and the robot leg joint angle and posture sensor data are collected; Based on the robot structural parameters and single-leg configuration, the joint angle feedback value is used to calculate the Jacobian matrix J of the i-th leg using the kinematics forward solution. i , based on the principle of virtual work, the plantar force in the body coordinate system {B} is calculated in is the measured value of the motor torque of the leg; Based on the attitude quaternions q0, q1, q2, q3 measured by the robot's onboard IMU, the transformation matrix between the global coordinate system {N} and the body coordinate system {B} is calculated: in, is the transformation matrix between the global coordinate system {N} and the body coordinate system {B}; the plantar force of the foot end of the i-th leg in the body coordinate system {B} is converted to the plantar force in the global coordinate system {N} according to the following formula: In step 2: based on the global plantar force estimation result, a reverse Smith trigger is used to determine whether the current i-th leg is in a support behavior or a swing behavior. The mathematical description of the reverse Smith trigger is as follows: The reverse Smith trigger is set by setting the rising threshold T up And the falling threshold T down And the corresponding judgment condition, i.e., the counter time t, is used to construct a reverse trigger loop with good fault tolerance. When in the support behavior, if the input plantar force estimation result is greater than the set descending threshold T down , the corresponding counter time t increases, otherwise it is cleared; when the counter time t is greater than the set time threshold t down , the behavior switches, and the same logic applies to the swing behavior.

2. The behavior-driven quadruped robot servo layer fine-grained evolution optimization model as claimed in claim 1, characterized in that: In step 3: based on the behavior switching result in step 2, different impedance control channels are enabled, the position-speed control channel is used for the swing behavior, and the position-speed-force control channel is used for the supporting behavior, and finally the tracking of the desired joint position and force command is achieved.

3. The behavior-driven quadruped robot servo layer fine-grained evolution optimization model as described in claim 2, characterized in that: The step 3 specifically includes: Step 31: Construct the position control channel design and calculate the current end position x and speed based on the robot joint angle and kinematic inverse solution and acceleration Among them, q is the current joint angle, is the current joint angular velocity, IK{} is the kinematic mapping function; based on the expected end planning position result x d Construct impedance control rate to generate virtual force f p Change of traction end position: Among them, K p ,K d are stiffness and damping coefficients; Step 32: Design a velocity impedance loop, whose input is the fine-grained angular velocity command planned by the central layer, and the feedback data is the angular velocity result calculated by the differential calculation of the terminal layer. Stiffness + damping is used, based on the expected terminal angular velocity Construct feedback impedance control rate and integrate feedforward proportional control to generate virtual force: The virtual traction force calculated in step 31 and step 32 is mapped into a corresponding torque command using a Jacobian matrix; Step 33: Design a force-impedance controlled loop based on the desired force F d Construct feedback impedance control rate and integrate feedforward proportional control to generate virtual traction: Step 34: Use Jacobi dynamics to map the virtual traction in step 33 into torque, and mix and superimpose the outputs: Step 35: Combine the torque commands in steps 32 and 34 and output them to the physical actuator: τ=τ pv +t F 。 4. The behavior-driven quadruped robot servo layer fine-grained evolution optimization model as described in claim 3, characterized in that: In the step 4: an ADRC controller is constructed to estimate the disturbance in the control channel and adjust the fine-grained control parameters.

5. The behavior-driven quadruped robot servo layer fine-grained evolutionary optimization model as described in claim 4, characterized in that: The step 4 specifically includes: First, an extended state observation is established to estimate the disturbance in each control channel online, and the control error states of each order are used for tracking processing: Where b is the model gain coefficient, u is the control output of the current channel, is the estimated value of the system state, L1, L2, L3 are the observer gain coefficients, which are calculated using the observer bandwidth w: <h2 style=";text-align:left;direction:ltr">L1 = 3w, L2 = 3w<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> L3 = w<h2 style=";text-align:left;direction:ltr"> 3 The current total disturbance of the system is calculated by the ADRC controller as z 13 , then it is used to compensate the control output of each channel to realize the fine-grained evolution of servo control, and the control amount correction value is:

6. The behavior-driven quadruped robot servo layer fine-grained evolution optimization model as described in claim 5, characterized in that: In step 4, in the case of the position control channel, the control amount correction value is set to The corrected traction force is:

7. The behavior-driven quadruped robot servo layer fine-grained evolution optimization model as claimed in claim 6, characterized in that: It realizes fine-grained evolutionary optimization of servo layer behavior based on the idea of ​​multi-modal switching control, thereby improving the reliability of force servo control of quadruped robots in unknown terrains.

8. The behavior-driven quadruped robot servo layer fine-grained evolution optimization model as claimed in claim 7, characterized in that: It can improve the reliability of foot-ground contact and the robustness of force control of quadruped robots in unknown environments.

Images