METHOD FOR GENERATING NOVEL IMPEDANCE CONFIGURATION FOR THREE-DEGREE-OF-FREEDOM (3DOF) ROBOTIC LEG

Abstract

The present disclosure relates to a method for generating a novel impedance configuration for a three-degree-of-freedom (3DOF) leg of a hydraulically-driven legged robot. The method includes: separately determining variations of input signals of an inner position-based control loop and an inner force-based control loop of a hydraulic drive unit of each joint based on an obtained mathematical model; generating a novel impedance configuration in which position-based control is performed on a hydraulic drive unit of a hip joint, and force-based control is performed on hydraulic drive units of a knee joint and an ankle joint in a hydraulic drive system of the leg of a to-be-controlled robot; and performing forward calculation by using the leg mathematical model, to obtain an actual position and a force variation of the foot of the leg of the to-be-controlled robot to control motion of the foot of the to-be-controlled robot within motion space.

Description

CROSS-REFERENCE TO RELATED APPLICATION

This application claims the benefit of Chinese Patent Application No. 202011179099.8, filed Oct. 29, 2020, which is hereby incorporated by reference in its entirety.

TECHNICAL FIELD

The present disclosure relates to the field of robot control, and in particular, to a method for generating a novel impedance configuration for a three-degree-of-freedom (3DOF) leg of a hydraulically-driven legged robot.

BACKGROUND

In recent years, robots have been widely applied in all walks of life. As an important part of the robot assembly, mobile robots are usually classified into the following types based on movement: wheeled robots, legged robots, spherical robots, tracked robots, and the like. Relative to robots moving in other manners, legged biomimetic robots can better adapt to unknown and unstructured environments. Particularly, after being integrated with a hydraulic drive with a high power-to-weight ratio, the legged biomimetic robots can be improved greatly in motion performance and load capabilities. The legged biomimetic robots can better complete tasks such as material transportation, rescue, detection, and military assistance in complicated environments (such as ice surfaces, snowy lands, sandy lands, and rubble), and gain close attention from scholars all over the world.

The overall gait control performance of a hydraulically-driven legged robot is determined by motion control performance of each leg, and the motion control performance of the leg is determined by swing phase trajectory tracking performance and stance phase compliance control performance of the leg. Impedance control is an effective control method that can implement trajectory tracking and compliance for the leg. In impedance control, an inner hydraulic control loop is used as a core, and an outer impedance control loop is added, so that the system implements an output correspondence between a position and force. Frequently-used inner hydraulic control loops include an inner position-based control loop and an inner force-based control loop, and corresponding hydraulic control methods are referred to as position-based impedance control and force-based impedance control. The two impedance control methods are obviously different in construction of a mathematical model, design of a core controller, parameter selection, and experiment system construction, and therefore have different dynamic compliance. At present, the two impedance control methods have been widely used in all kinds of legged robots. Although mature optimized control methods have been provided, due to restrictions of an impedance control mechanism, there is limited space for improving control precision and response capabilities of the two impedance control methods. How to further improve the motion control performance of the leg of this kind of robot has become one of challenges in this field. Therefore, it is necessary to preferably select different inner-loop core control modes for various joints of the leg, to give full play to advantages of the two impedance control methods.

In conclusion, in leg compliance control of the hydraulically-driven legged robot, a novel impedance configuration needs to be urgently designed for a 3DOF leg.

SUMMARY

An objective of the present disclosure is to provide a method for generating a novel impedance configuration for a 3DOF robotic leg, to further improve motion control performance of a robotic leg.

To achieve the above purpose, the present disclosure provides the following technical solutions.

A method for generating a novel impedance configuration for a three-degree-of-freedom (3DOF) robotic leg includes:

obtaining a mathematical model of a hydraulic drive system of the leg of a to-be-controlled robot, where the mathematical model includes a leg kinematics model, a leg statics model, a leg dynamics model, a leg impedance characteristic model, a mapping between an elongation of a hydraulic drive unit of each joint of the leg and a rotation angle of each joint of the leg, and a mapping among force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and a torque of each joint; and a hip joint, a knee joint, and an ankle joint are included;

determining, when the foot of the to-be-controlled robot suffers disturbing force, a variation of an input signal of an inner position-based control loop of a hydraulic drive unit of each joint based on the mathematical model;

determining, when the foot suffers a disturbing position, a variation of an input signal of an inner force-based control loop of the hydraulic drive unit of each joint based on the mathematical model;

generating a novel impedance configuration in which position-based control is performed on an inner control loop of a hydraulic drive unit of the hip joint, and force-based control is performed on inner control loops of hydraulic drive units of the knee joint and the ankle joint, and using a servo valve to obtain output positions and output force of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint; and

based on the output positions and the output force of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint, performing forward calculation by using the leg kinematics model, the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and the torque of each joint, to obtain an actual position and a force variation of the foot of the leg.

Preferably, before the obtaining a mathematical model of a hydraulic drive system of the leg of a to-be-controlled robot, the method further includes:

obtaining a length of each member in the hydraulic drive system of the leg of the to-be-controlled robot, the rotation angle of each joint, and the torque of each joint, where a thigh member, a crus member, and a foot member are included;

constructing the leg kinematics model based on the length of each member and the rotation angle of each joint;

constructing the leg statics model based on the length of each member, the rotation angle of each joint, and the torque of each joint; and

constructing the leg dynamics model based on a Lagrangian dynamics equation.

Preferably, the determining, when the foot of the to-be-controlled robot suffers disturbing force, a variation of an input signal of an inner position-based control loop of the hydraulic drive unit of each joint based on the mathematical model specifically includes:

when the foot of the to-be-controlled robot suffers the disturbing force, detecting, by a force sensor of the hydraulic drive unit of each joint, a force signal, performing inverse calculation by using the leg dynamics model, to obtain a disturbing force component of the foot in the signal detected by the force sensor of the hydraulic drive unit of each point, and performing forward calculation by using the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and the torque of each joint, to obtain the disturbing force that the foot suffers;

using a leg impedance characteristic solver to obtain a foot position variation corresponding to the disturbing force that the foot suffers; and

performing inverse calculation based on the foot position variation by using the leg kinematics model and the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, to obtain the variation of the input signal of the inner position-based control loop of the hydraulic drive unit of each joint.

Preferably, the determining, when the foot suffers a disturbing position, a variation of an input signal of an inner force-based control loop of the hydraulic drive unit of each joint based on the mathematical model specifically includes:

when the foot suffers the disturbing position, detecting, by a position sensor of the hydraulic drive unit of each joint, a position signal, and then performing forward calculation by using the leg kinematics model and the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, to obtain a position signal of the foot, where the position signal of the foot includes an input position of the foot and the actual position of the foot;

obtaining a desired outer-loop position of the foot based on the position signal of the foot; and

converting the desired outer-loop position of the foot into a corresponding desired force signal of foot impedance by using a leg impedance characteristic solver, and performing inverse calculation on the desired force signal of the foot impedance by using the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each point, the rotation angle of each joint, and the torque of each joint, to obtain the variation of the input signal of the inner force-based control loop of the hydraulic drive unit of each joint.

According to specific embodiments provided in the present disclosure, the present disclosure has the following two technical effects.

According to the method for generating a novel impedance configuration for a 3DOF leg of a hydraulically-driven legged robot in the present disclosure, a novel impedance configuration in which position-based control is performed on a hydraulic drive unit of a hip joint, and force-based control is performed on hydraulic drive units of a knee joint and an ankle joint in a hydraulic drive system of a leg is generated, to control motion of the foot of a to-be-controlled robot within motion space. This method not only considers impact of different load mass and installation positions on control performance of each joint, but also gives full play to an advantage of position-based control in precision and an advantage of force-based control in response speed, thereby increasing its applicability.

In addition, when kinematics calculation is performed on the 3DOF leg, a same foot position is obtained based on different combinations of angle variations of the three joints. As a result, in a process of performing traditional force-based impedance control (force-based control is performed on all of the hip, knee, and ankle joints), the foot position keeps unchanged, but positions of the joints are constantly changing and cannot be controlled. Consequently, traditional force-based impedance control cannot be applied directly, and other algorithms are required, thereby greatly increasing the control difficulty. The novel impedance configuration proposed in the present disclosure can be used to effectively resolve the foregoing problem in traditional force-based impedance control, and ensure motion control performance of the foot of the robot. In addition, the novel impedance configuration in which position-based control is performed on the hydraulic drive unit of the hip joint, and force-based control is performed on the hydraulic drive units of the knee joint and the ankle joint can be used to effectively resolve the following problem that there are multiple solutions in statics when impedance control is performed on the 3DOF leg.

BRIEF DESCRIPTION OF DRAWINGS

To describe the technical solutions in the embodiments of the present disclosure or in the prior art more clearly, the following briefly describes the accompanying drawings required for describing the embodiments. Apparently, the accompanying drawings in the following description show merely some embodiments of the present disclosure, and a person of ordinary skill in the art may still derive other drawings from these accompanying drawings without creative efforts.

FIG. 1 is a flowchart of a method for generating a novel impedance configuration for a 3DOF robotic leg in the present disclosure;

FIG. 2 shows a model of a hydraulic drive system of a 3DOF leg of a legged robot in an embodiment of the present disclosure;

FIG. 3 is a schematic diagram of a forward leg kinematics model in an embodiment of the present disclosure;

FIG. 4 is a schematic diagram of an inverse leg kinematics model in an embodiment of the present disclosure;

FIG. 5 shows a relationship between an angle of a joint and an elongation of a hydraulic drive unit of the joint in an embodiment of the present disclosure;

FIG. 6 is a schematic diagram of a leg statics model in an embodiment of the present disclosure;

FIG. 7 shows a relationship between a torque of each joint and force applied to a drive unit of each joint in an embodiment of the present disclosure;

FIG. 8 is a schematic diagram of a leg dynamics model in an embodiment of the present disclosure;

FIG. 9 is a schematic diagram of an impedance characteristic of a hydraulic drive system of a leg in an embodiment of the present disclosure;

FIG. 10 shows a principle of implementing position-based impedance control for a leg in an embodiment of the present disclosure;

FIG. 11 shows a principle of implementing force-based impedance control for a leg in an embodiment of the present disclosure;

FIG. 12 is a schematic diagram of an impedance configuration scheme for a hydraulic drive system of a leg in an embodiment of the present disclosure;

FIG. 13 shows a principle of implementing an impedance configuration scheme for a hydraulic drive system of a leg in an embodiment of the present disclosure;

FIG. 14 is a schematic structural diagram of a hydraulic drive system of a leg in a swing phase state in an embodiment of the present disclosure;

FIG. 15 is a schematic structural diagram of a hydraulic drive system of a leg in a stance phase state in an embodiment of the present disclosure;

FIG. 16 shows equivalent mass of a hydraulic drive unit of each joint of a leg in an embodiment of the present disclosure;

FIG. 17 shows an equivalent mass curve of a hydraulic drive unit of each joint of a leg when a centroid position of a base member keeps unchanged and mass of the base member changes in an embodiment of the present disclosure;

FIG. 18 shows an equivalent mass curve of a hydraulic drive unit of each joint of a leg when mass of a base member keeps unchanged and a centroid height of the base member continuously increases in an embodiment of the present disclosure;

FIG. 19 is a schematic structural diagram of a hydraulic drive system of a leg in a stance phase state when objects with different heights and a same density are loaded in an embodiment of the present disclosure; and

FIG. 20 shows an equivalent mass curve of a hydraulic drive unit of each joint of a leg when heavy objects are loaded in an embodiment of the present disclosure.

Meanings of parameters in the accompanying drawings:

O: hip joint in a hydraulic drive system of a leg; E: knee joint in the hydraulic drive system of the leg; G: ankle joint in the hydraulic drive system of the leg; A: position at which a hydraulic drive unit of the hip joint is connected to a base; B: position at which the hydraulic drive unit of the hip joint is connected to a thigh: C: position at which a hydraulic drive unit of the knee joint is connected to the thigh; D: position at which the hydraulic drive unit of the knee joint is connected to a crus; F: position at which a hydraulic drive unit of the ankle joint is connected to the crus; H: position at which the hydraulic drive unit of the ankle joint is connected to a foot member; I: vertex of a semicylindrical foot; OE: length from the hip joint to the knee joint; EG: length from the knee joint to the ankle joint; GI: length from the ankle joint to the foot; θ₁: rotation angle of the hip joint, namely, an included angle formed between the thigh member and a positive direction of an axis x₀; θ₂: rotation angle of the knee joint, namely, an included angle formed between the crus member EG and an extension line of the thigh member OE; θ₃: rotation angle of the ankle joint, namely, an included angle formed between an extension line of the crus member EG and the foot member GI; α: included angle formed between the EG and an ED; β: included angle formed between an OA and the positive direction of the axis x₀; AB: total elongation of the hydraulic drive unit of the hip joint; CD: total elongation of the hydraulic drive unit of the knee joint; FH: total elongation of the hydraulic drive unit of the ankle joint; ΔF_s1: detection signal of a force sensor of the hydraulic drive unit of the hip joint; ΔF_s2: detection signal of a force sensor of the hydraulic drive unit of the knee joint; ΔF_s3: detection signal of a force sensor of the hydraulic drive unit of the ankle joint; X_foot^x: actual position applied to the foot of the leg in an x-axis direction; X_foot^y: actual position applied to the foot of the leg in a y-axis direction; F_foot^x: force applied to the foot of the leg in the x-axis direction; F_foot^y: force applied to the foot of the leg in the y-axis direction; τ₁: torque of the hip joint; ρ₂: torque of the knee joint; τ₃: torque of the ankle joint; m₁: mass of the thigh member; m₂: mass of the crus member; m₃: mass of the foot member; α₁: an included angle formed between the OE and a line connecting a centroid of the thigh member and the point O; α₂: an included angle formed between the EG and a line connecting a centroid of the crus member and the point E; α₃: an included angle formed between the GI and a line connecting a centroid of the foot member and the point G; M_Dx: desired mass in the x-axis direction; M_Dy: desired mass in the y-axis direction; B_Dx: desired damping in the x-axis direction; B_Dy: desired damping in the y-axis direction; K_Dx: desired stiffness in the x-axis direction; K_Dy: desired stiffness in the y-axis direction; ΔX_Dx^leg: actual position variation relative to an initial position in the x-axis direction; ΔX_Dy^leg: actual position variation relative to the initial position in the y-axis direction; ΔX_rx^leg: input position variation relative to the initial position in the x-axis direction; ΔX_ry^leg: input position variation relative to the initial position in the y-axis direction; ΔF_sx^foot: actual force variation relative to initial force in the x-axis direction; ΔF_sy^foot: actual force variation relative to the initial force in the y-axis direction; ΔF_kx^foor: resultant force, excluding disturbing force, calculated for the foot in the x-axis direction based on inverse dynamics; ΔF_ky^foot: resultant force, excluding disturbing force, calculated for the foot in the y-axis direction based on inverse dynamics; Δx_p1: detection signal of a position sensor of the hydraulic drive unit of the hip joint; Δx_p2: detection signal of a position sensor of the hydraulic drive unit of the knee joint; Δx_p3: detection signal of a position sensor of the hydraulic drive unit of the ankle joint; m₄: total mass of the base member and a mechanical structure assembled for the base member; β₁: an included angle between the thigh member OE and a line connecting the centroid of the thigh member and the knee joint; β₂: an included angle between the crus member EG and a line connecting the centroid of the crus member and the ankle joint; β₃: an included angle between the positive direction of the axis x₀ and a line connecting a centroid of the base member and the hip joint;

Reference numerals in FIG. 13:

101: Input position of cylinder 1

102: Inner position-based control loop of cylinder 1

103: Inner position-based control loop of cylinder 1

104: Outer impedance control loop

105: Force sensor

106: Position controller

107: Hydraulic drive unit 1

108: Displacement sensor

109: Output position of cylinder 1

110: Output position of cylinder 1

111: Input position of cylinder 2

112: Inner position-based control loop of cylinder 2

113: Load capability of cylinder 2

114: Force sensor

115: Inverse kinematics of a mechanical structure of a robotic leg

116: Position controller

117: Hydraulic drive unit 2

118: Displacement sensor

119: Output position of cylinder 2

120: Forward kinematics of the mechanical structure of the robotic leg

121: Output position of a foot of the leg

122: Inverse leg dynamics calculation

123: Output position of cylinder 2

124: Input position of cylinder 3

125: Inner position-based control loop of cylinder 3

126: Load capability of cylinder 3

127: Position controller

128: Force sensor

129: Hydraulic drive unit 3

130: Displacement sensor

131: Output position of cylinder 3

132: Output position of cylinder 3

133: Switching switch of cylinder 1

134: Switching switch of cylinder 2

135: Switching switch of cylinder 3

136: Input position of the foot of the leg

137: Leg impedance characteristic solver

138: Input force of cylinder 1

139: Inner force-based control loop of cylinder 1

140: Displacement sensor

141: Force controller

142: Hydraulic drive unit 1

143: Force sensor

144: Output force of cylinder 1

145: Input force of cylinder 2

146: Inverse statics of the mechanical structure of the robotic leg

147: Inner force-based control loop of cylinder 2

148: Displacement sensor

149: Force controller

150: Hydraulic drive unit 2

151: Force sensor

152: Forward statics of the mechanical structure of the robotic leg

153: Leg impedance characteristic solver

154: Input position of the foot of the leg

155: Output force of cylinder 2

156: Inner force-based control loop of cylinder 3

157: Displacement sensor

158: Force controller

159: Hydraulic drive unit 3

160: Input force of cylinder 3

161: Output force of cylinder 3

162: Force sensor.

DETAILED DESCRIPTION

The following clearly and completely describes the technical solutions in the embodiments of the present disclosure with reference to accompanying drawings in the embodiments of the present disclosure. Apparently, the described embodiments are merely a part rather than all of the embodiments of the present disclosure. All other embodiments obtained by a person of ordinary skill in the art based on the embodiments of the present disclosure without creative efforts shall fall within the protection scope of the present disclosure.

An objective of the present disclosure is to provide a method for generating a novel impedance configuration for a 3DOF robotic leg, to further improve motion control performance of a robotic leg.

To make the foregoing objective, features, and advantages of the present disclosure clearer and more comprehensible, the present disclosure is further described in detail below with reference to the accompanying drawings and specific implementations.

To overcome a limitation of a control mechanism in an impedance control method in the prior art, and give full play to advantages of the control method in control precision, response speed, and the like, a novel impedance configuration is finally generated for a leg. In the novel impedance configuration, for a hydraulic drive system of a 3DOF leg, position-based control is performed on a hydraulic drive unit of a hip joint, and force-based control is performed on hydraulic drive units of a knee joint and an ankle joint. The novel configuration can be compatible with swing phase trajectory tracking control performance and stance phase compliance control performance of the hydraulic drive system of the leg within an entire motion period, and provide important support for high-performance motion control and leg structure design of a hydraulically-driven legged robot.

Based on the novel configuration, the present disclosure provides a method for generating a novel impedance configuration for a 3DOF robotic leg. As shown in FIG. 1, the method includes the following steps.

Step 100: Obtain a mathematical model of a hydraulic drive system of the leg of a to-be-controlled robot. The mathematical model includes a leg kinematics model, a leg statics model, a leg dynamics model, a leg impedance characteristic model, a mapping between an elongation of a hydraulic drive unit of each joint of the leg and a rotation angle of each joint of the leg, and a mapping among force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and a torque of each joint. A hip joint, a knee joint, and an ankle joint are included.

Step 101: When the foot of the to-be-controlled robot suffers disturbing force, determine a variation of an input signal of an inner position-based control loop of the hydraulic drive unit of each joint based on the mathematical model. This step specifically includes:

when the foot of the to-be-controlled robot suffers the disturbing force, detecting, by a force sensor of the hydraulic drive unit of each joint, a force signal, performing inverse calculation by using the leg dynamics model, to obtain a disturbing force component of the foot in the signal detected by the force sensor of the hydraulic drive unit of each point, and performing forward calculation by using the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and the torque of each joint, to obtain the disturbing force that the foot suffers;

using a leg impedance characteristic solver to obtain a foot position variation corresponding to the disturbing force that the foot suffers; and

performing inverse calculation based on the foot position variation by using the leg kinematics model and the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, to obtain the variation of the input signal of the inner position-based control loop of the hydraulic drive unit of each joint.

Step 102: When the foot suffers a disturbing position, determine a variation of an input signal of an inner force-based control loop of the hydraulic drive unit of each joint based on the mathematical model. This step specifically includes:

when the foot suffers the disturbing position, detecting, by a position sensor of the hydraulic drive unit of each joint, a position signal, and then performing forward calculation by using the leg kinematics model and the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, to obtain a position signal of the foot, where the position signal of the foot includes an input position of the foot and an actual position of the foot;

obtaining a desired outer-loop position of the foot based on the position signal of the foot; and

converting the desired outer-loop position of the foot into a corresponding desired force signal of foot impedance by using a leg impedance characteristic solver, and performing inverse calculation on the desired force signal of the foot impedance by using the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each point, the rotation angle of each joint, and the torque of each joint, to obtain the variation of the input signal of the inner force-based control loop of the hydraulic drive unit of each joint.

Step 103: Generate a novel impedance configuration in which position-based control is performed on an inner control loop of a hydraulic drive unit of the hip joint, and force-based control is performed on inner control loops of hydraulic drive units of the knee joint and the ankle joint, and use a servo valve to obtain output positions and output force of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint.

Step 104: Based on the output positions and the output force of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint, perform forward calculation by using the leg kinematics model, the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and the torque of each joint, to obtain the actual position and a force variation of the foot of the leg.

Preferably, before the step 100, the method further includes:

obtaining a length of each member in the hydraulic drive system of the leg of the to-be-controlled robot, the rotation angle of each joint, and the torque of each joint, where a thigh member, a crus member, and a foot member are included;

constructing the leg kinematics model based on the length of each member and the rotation angle of each joint;

constructing the leg statics model based on the length of each member, the rotation angle of each joint, and the torque of each joint; and

constructing the leg dynamics model based on a Lagrangian dynamics equation.

The following further describes the solutions in the present disclosure by using a specific embodiment. The specific embodiment of the present disclosure is described by using an example in which the to-be-controlled robot is a hydraulically-driven legged robot. In specific application, the solutions in the present disclosure are also applicable to another type of legged robot.

Step 1: Simplify a mechanical structure of a hydraulic drive system of a leg, to obtain a leg kinematics model, a leg statics model, a leg dynamics model, a mapping between an elongation of a hydraulic drive unit of each joint and a rotation angle of each joint, and a mapping among force applied to each hydraulic drive unit, the rotation angle of each joint, and a torque of each joint.

FIG. 2 shows a three-dimensional model of a hydraulic drive system of a 3DOF leg of a legged robot. As shown in FIG. 2, a hip joint is connected to a base and a thigh, and a hydraulic drive unit of the hip joint is used to control an angle of the hip joint. A knee joint is connected to the thigh and a crus, and a hydraulic drive unit of the knee joint is used to control an angle of the knee joint. An ankle joint is connected to the crus and a foot member, and a hydraulic drive unit of the ankle joint is used to control an angle of the ankle joint. A foot position of the leg is controlled by controlling the angles of the hip joint, the knee joint, and the ankle joint.

The hydraulic drive unit mainly includes a servo cylinder, a servo valve, a position sensor, and a force sensor. The force sensor and the position sensor are used for force closed-loop control and position closed-loop control of the hydraulic drive unit.

The mechanical structure of the hydraulic drive system of the leg is complicated. For convenience of kinematic analysis, the model of the hydraulic drive system of the leg of the legged robot in FIG. 2 is simplified into a model in FIG. 3. FIG. 3 is a schematic diagram of the forward leg kinematic model.

D-H parameters of the mechanical structure can be obtained based on FIG. 3, and are shown in Table 1.

TABLE 1
D-H parameters of the mechanical structure
of the hydraulic drive system of the leg
Link serial number	ai−1	αi−1	di	θi
1	0	0	0	θ1
2	l1	0	0	θ2
3	l2	0	0	θ3
4	l3	0	0	—

In Table 1, α_i−1 represents a distance that is between z_i−1 and z_i and that is measured along x_i−1. α_i−1 represents an angle that is between z_i−1 and z_i and that is rotated around x_i−1. d_i represents a distance that is between x_i−1 and x_i and that is measured along z_i. θ_i represents an angle that is between x_i−1 and x_i and that is rotated around z_i.

In a single-leg D-H coordinate system shown in FIG. 3, a pose relationship may be expressed by a link transformation formula _iⁱ⁻¹T.

$\begin{array}{cc}{\hspace{0.17em}}_i^{i-1}T=\left[\begin{array}{cccc}\cos {\theta }_i& -s\mathrm{in}{\theta }_i& 0& a_{i-1}\\ \sin {\theta }_i\cos {\alpha }_{i-1}& \cos {\theta }_i\cos {\alpha }_{i-1}& -s\mathrm{in}{\alpha }_{i-1}& -d_i\sin {\alpha }_{i-1}\\ \sin {\theta }_i\sin {\alpha }_{i-1}& \cos {\theta }_i\sin {\alpha }_{i-1}& \cos {\alpha }_{i-1}& d_i\cos {\alpha }_{i-1}\\ 0& 0& 0& 1\end{array}\right]& \left(1\right)\end{array}$

₄⁰T can be obtained by multiplying ₁⁰T, ₂¹T, ₃²T, ₄³T in sequence.

$\begin{array}{cc}{\hspace{0.17em}}_4^{0}T={\hspace{0.17em}}_1^{0}T{\hspace{0.17em}}_2^{1}T{\hspace{0.17em}}_3^{2}T{\hspace{0.17em}}_4^{3}T=\left[\begin{array}{cccc}\cos \left({\theta }_1+{\theta }_2+{\theta }_3+{\theta }_4\right)& -\sin \left({\theta }_1+{\theta }_2+{\theta }_3+{\theta }_4\right)& 0& \begin{array}{c}{l}_1\cos {\theta }_1+l_2\cos \left({\theta }_1+{\theta }_2\right)+\\ l_3\cos ({\theta }_1+{\theta }_2+{\theta }_3)\end{array}\\ \sin \left({\theta }_1+{\theta }_2+{\theta }_3+{\theta }_4\right)& \cos \left({\theta }_1+{\theta }_2+{\theta }_3+{\theta }_4\right)& 0& \begin{array}{c}{l}_1\sin {\theta }_1+l_2\sin \left({\theta }_1+{\theta }_2\right)+\\ l_3\sin ({\theta }_1+{\theta }_2+{\theta }_3)\end{array}\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]& \left(2\right)\end{array}$

Therefore, the kinematics model (forward kinematics) can be obtained according to the formula (2):

$\{\begin{array}{c}{X}_{\mathrm{foot}}^X=l_1\cos {\theta }_1+l_2\cos \left({\theta }_1+{\theta }_2\right)+l_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)\\ X_{\mathrm{foot}}^Y=l_1\sin {\theta }_1+l_2\sin \left({\theta }_1+{\theta }_2\right)+l_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3\right)\end{array}\hspace{1em}$

As described above, l₁ represents a length from the hip joint to the knee joint, l₂ represents a length from the knee joint to the ankle joint, and l₃ represents a length from the ankle joint to the foot, θ₁ represents a rotation angle of the hip joint, θ₂ represents a rotation angle of the knee joint, and θ₃ represents a rotation angle of the ankle joint.

FIG. 4 is a schematic diagram of an inverse leg kinematics model. The model is used to avoid indefinite solutions in inverse kinematics. In the present disclosure, the hydraulic drive system of the leg is equivalent to that shown in FIG. 4, so that the hip joint O, the ankle joint G, and the foot I are collinear.

Based on a geometrical relationship in FIG. 4, the rotation angle θ₁ of the hip joint, the rotation angle θ₂ of the knee joint, and the rotation angle θ₃ of the ankle joint can be obtained as follows:

$\begin{array}{cc}{\theta }_1={\theta }_0+\angle EOG=\arctan 2\left(X_{foot}^Y,X_{foot}^X\right)+\arccos \left(\frac{O{E}^2+{\left(\sqrt{X_{foot}^{Y2}+X_{foot}^{X2}}-\mathrm{GI}\right)}^2-E{G}^2}{2·\mathrm{OE}·\mathrm{OG}}\right){\theta }_2=\angle \mathrm{OEG}-\pi =\arccos \left(\frac{O{E}^2+E{G}^2-{\left(\sqrt{X_{foot}^{\mathrm{Y2}}+X_{foot}^{X2}}-\mathrm{GI}\right)}^2}{2·\mathrm{OE}·\mathrm{EG}}\right)-\pi {\theta }_3=\angle OGE=\arccos \left(\frac{{\left(\sqrt{X_{\mathrm{foot}}^{Y2}+X_{foot}^{X2}}-\mathrm{GI}\right)}^2+E{G}^2-O{E}^2}{2·\left(\sqrt{X_{foot}^{Y2}+X_{foot}^{X2}}-\mathrm{GI}\right)·\mathrm{EG}}\right)& \left(4\right)\end{array}$

In FIG. 5, a) to c) each show a relationship between the angle of each joint and the elongation of the hydraulic drive unit of each joint.

A total elongation AB of the hydraulic drive unit of the hip joint, a total elongation CD of the hydraulic drive unit of the knee joint, and a total elongation FH of the hydraulic drive unit of the ankle joint are obtained as follows:

AB=l₀₁+Δx_p1

CD=l₀₂+Δx_p2

FH=l₀₃+Δx_p3   (5)

As described above, l₀₁ represents an initial length of the hydraulic drive unit of the hip joint, l₀₂ represents an initial length of the hydraulic drive unit of the knee joint, and l₀₃ represents an initial length of the hydraulic drive unit of the ankle joint.

Based on a geometrical relationship in FIG. 5, the rotation angle θ₁ of the hip joint, the rotation angle θ₂ of the knee joint, and the rotation angle θ₃ of the ankle joint can be obtained as follows:

$\begin{array}{cc}{\theta }_1=\beta -\angle A\mathrm{OB}-\angle \mathrm{BOE}=\beta -\arccos \left(\frac{O{A}^2+O{B}^2-{\left(l_{01}+\Delta x_{p1}\right)}^2}{2·\mathrm{OA}·\mathrm{OB}}\right)-\angle BOE{\theta }_2=\angle \mathrm{CED}-\pi -\alpha +\angle \mathrm{OEC}=\arccos \left(\frac{E{C}^2+E{D}^2-{\left(l_{02}+\Delta x_{p2}\right)}^2}{2·\mathrm{EC}·\mathrm{ED}}\right)-\pi -\alpha +\angle \mathrm{OEC}\begin{array}{c}{\theta }_3=\pi -\angle \mathrm{FGH}-\angle \mathrm{IGH}-\angle \mathrm{EGF}\\ =\pi -\arccos \left(\frac{G{F}^2+G{H}^2-{\left(l_{03}+\Delta x_{p3}\right)}^2}{2·\mathrm{GF}·\mathrm{GH}}\right)-\angle \mathrm{IGH}-\angle \mathrm{EGF}\end{array}& \left(6\right)\end{array}$

According to the foregoing formulas, an elongation Δx_p1 of the hydraulic drive unit of the hip joint, an elongation Δx_p2 of the hydraulic drive unit of the knee joint, and an elongation Δx_p3 of the hydraulic drive unit of the ankle joint can be obtained as follows:

$\begin{array}{cc}\Delta x_{p1}=\sqrt{{\mathrm{OA}}^2+O{B}^2-2·\mathrm{OA}·\mathrm{OB}·\cos \left(\beta -\angle \mathrm{BOE}-{\theta }_1\right)}-l_{01}\Delta x_{p2}=\sqrt{E{C}^2+E{D}^2-2·\mathrm{EC}·\mathrm{ED}·\cos \left(\angle \mathrm{OEC}-{\theta }_2-\pi -\alpha \right)}-l_{02}\Delta x_{p3}=\sqrt{G{F}^2+G{H}^2-2·\mathrm{GF}·\mathrm{GH}·\cos \left({\theta }_3-\pi +\angle \mathrm{IGH}+\angle \mathrm{EGF}\right)}-l_{03}& \left(7\right)\end{array}$

FIG. 6 is a schematic diagram of the leg statics model.

According to the principle of virtual work, a torque vector τ of each joint can be obtained as follows:

τ=J^T(q)F   (8)

As described above, F represents force (N) applied to the foot of the leg, and J^T(q) represents a force Jacobian of the leg.

In the formula (8), the force Jacobian J^T(q) of the leg is as follows:

$\begin{array}{cc}{J}^T\left(q\right)=\left[\begin{array}{cc}\frac{\partial X_{\mathrm{foot}}^X}{\partial {\theta }_1}& \frac{\partial X_{\mathrm{foot}}^Y}{\partial {\theta }_1}\\ \frac{\partial X_{\mathrm{foot}}^X}{\partial {\theta }_2}& \frac{\partial X_{\mathrm{foot}}^Y}{\partial {\theta }_2}\\ \frac{\partial X_{\mathrm{foot}}^X}{\partial {\theta }_3}& \frac{\partial X_{\mathrm{foot}}^Y}{\partial {\theta }_3}\end{array}\right]=[\begin{array}{cc}-l_1\sin \left({\theta }_1\right)-l_2\sin \left({\theta }_1+{\theta }_2\right)-& l_1\cos \left({\theta }_1\right)+l_2\cos \left({\theta }_1+{\theta }_2\right)+\\ l_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3\right)& l_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)\\ -l_2\sin \left({\theta }_1+{\theta }_2\right)-& l_2\cos \left({\theta }_1+{\theta }_2\right)+\\ l_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3\right)& l_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)\\ -l_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3\right)& l_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)\end{array}]& \left(9\right)\end{array}$

The torque vector τ of each joint that is obtained according to the formulas (8) and (9) is as follows:

$\begin{array}{cc}{\tau }_1=-F_{foot}^x\left(\begin{array}{c}{l}_1\sin \left({\theta }_1\right)+l_2\sin \left({\theta }_1+{\theta }_2\right)+\\ l_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3\right)\end{array}\right)+F_{foot}^y\left(\begin{array}{c}{l}_1\cos \left({\theta }_1\right)+l_2\cos \left({\theta }_1+{\theta }_2\right)+\\ l_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)\end{array}\right){\tau }_2=-F_{foot}^x\left(l_2\sin \left({\theta }_1+{\theta }_2\right)+l_3\sin \left(\begin{array}{c}{\theta }_1+\\ {\theta }_2+{\theta }_3\end{array}\right)\right)+F_{foot}^y\left(l_2\cos \left({\theta }_1+{\theta }_2\right)+l_3\cos \left(\begin{array}{c}{\theta }_1+\\ {\theta }_2+{\theta }_3\end{array}\right)\right){\tau }_3=-F_{foot}^x{l}_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3\right)+F_{foot}^y{l}_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)& \left(10\right)\end{array}$

As described above, τ₁ represents a torque of the hip joint, τ₂ represents a torque of the knee joint, and τ₃ represents a torque of the ankle joint.

Forward statics is to obtain, based on the torque of each joint, the force applied to the foot. In the present disclosure, the force applied to the foot is obtained based on the torques of the knee joint and the ankle joint. Then, the leg statics model (forward leg statics) can be obtained as follows:

$\begin{array}{cc}\{\begin{array}{c}{F}_{foot}^x=\frac{{\tau }_2{l}_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)-{\tau }_3\left(l_2\cos \left({\theta }_1+{\theta }_2\right)+l_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3\right)\right)}{l_2{l}_3\sin \left({\theta }_3\right)}\\ F_{foot}^y=\frac{{\tau }_2{l}_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3\right)-{\tau }_3\left(l_2\sin \left({\theta }_1+{\theta }_2\right)+l_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3\right)\right)}{l_2{l}_3\sin \left({\theta }_3\right)}\end{array}& \left(11\right)\end{array}$

In FIG. 7, a) to c) each show a relationship between the torque of each joint and the force applied to the drive unit of each joint.

Based on a geometrical relationship in FIG. 7, it can be learned that a mapping between a detection value ΔF_s1 of the force sensor of the hip joint and the torque τ₁ of the hip joint, a mapping between a detection value ΔF_s2 of the force sensor of the knee joint and the torque τ₂ of the knee joint, and a mapping between a detection value ΔF_s3 of the force sensor of the ankle joint and the torque τ₃ of the ankle joint are as follows:

$\begin{array}{cc}{\tau }_1=\Delta F_{s1}·\mathrm{OB}·\sin \left(\arccos \left(\frac{{\left(l_{01}+\Delta x_{p1}\right)}^2+O{B}^2-O{A}^2}{2·\left(l_{01}+\Delta x_{p1}\right)·\mathrm{OB}}\right)\right){\tau }_2=-\Delta F_{s2}·\mathrm{EC}·\sin \left(\arccos \left(\frac{{\left(l_{02}+\Delta x_{p2}\right)}^2+E{C}^2-E{D}^2}{2·\left(l_{02}+\Delta x_{p2}\right)·\mathrm{EC}}\right)\right){\tau }_3=\Delta F_{s3}·\mathrm{GF}·\sin \left(\arccos \left(\frac{{\left(l_{03}+\Delta x_{p3}\right)}^2+G{F}^2-G{H}^2}{2·\left(l_{03}+\Delta x_{p3}\right)·\mathrm{GF}}\right)\right)& \left(12\right)\end{array}$

In FIG. 7, a solid line between a hydraulic cylinder and a joint represents an entity. A dashed line between a hydraulic cylinder and a joint is an auxiliary line for formula derivation.

FIG. 8 is a schematic diagram of the leg dynamics model. In a motion process of the leg of the robot, a centroid position of the hydraulic drive unit of each joint does not change greatly. Therefore, a change of the centroid position in the motion process is ignored in the present disclosure.

Based on a geometrical relationship in FIG. 3, coordinates (x₁, y₁) of a centroid of the thigh member OE can be obtained as follows:

$\begin{array}{cc}\{\begin{array}{c}{x}_1=p_1\cos \left({\theta }_1+{\alpha }_1\right)\\ y_1=p_1\sin \left({\theta }_1+{\alpha }_1\right)\end{array}& \left(13\right)\end{array}$

A moment of inertia of the thigh member OE is defined as J₁. Kinetic energy E_k1 of the thigh member OE and potential energy E_p1 of the thigh member OE are defined as follows:

E_k1=½m₁v₁²+½J₁{dot over (θ)}₁²=½m₁(p₁{dot over (θ)}₁)²+½J₁{dot over (θ)}₁²

E_p1=m₁gp₁ sin(θ₁+α₁)   (14)

As described above, g represents an acceleration of gravity (9.8 m/s²).

Based on the geometrical relationship in FIG. 3, coordinates (x₂,y₂) of a centroid of the crus member EG can be obtained as follows:

$\begin{array}{cc}\{\begin{array}{c}{x}_2=l_1\cos \left({\theta }_1\right)+p_2\cos \left({\theta }_1+{\theta }_2+{\alpha }_2\right)\\ y_2=l_1\sin \left({\theta }_1\right)+p_2\sin \left({\theta }_1+{\theta }_2+{\alpha }_2\right)\end{array}& \left(15\right)\end{array}$

A moment of inertia of the crus member EG is defined as J₂. Kinetic energy E_k2 of the crus member EG and potential energy E_p2 of the crus member EG are defined as follows:

E_k2=½m₂(l₁{dot over (θ)}₁)²+½m₂p₂²({dot over (θ)}₁+{dot over (θ)}₂)²+m₂l₁p₂({dot over (θ)}₁²+{dot over (θ)}₁{dot over (θ)}₂)cos(θ₂+α₂)+½J₂({dot over (θ)}₁+{dot over (θ)}₂)²

E_p2=m₂gl₁ sin(θ₁)+m₂gp₂ sin(θ₁+θ₂+α₂)   (16)

Based on the geometrical relationship in FIG. 3, coordinates (x₃, y₃) of a centroid of the foot member GI can be obtained as follows:

$\begin{array}{cc}\{\begin{array}{c}{x}_3=l_1\cos {\theta }_1+l_2\cos \left({\theta }_1+{\theta }_2\right)+p_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3+{\alpha }_3\right)\\ y_3=l_1\sin {\theta }_1+l_2\sin \left({\theta }_1+{\theta }_2\right)+p_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3+{\alpha }_3\right)\end{array}& \left(17\right)\end{array}$

A moment of inertia of the foot member GI is defined as J₃. Kinetic energy E_k3 of the foot member GI and potential energy E_p3 of the foot member GI are defined as follows:

$\begin{array}{cc}{E}_{k3}=\frac{1}{2}{m}_3\left(\begin{array}{c}{\left(\begin{array}{c}{\stackrel{.}{\theta }}_1{l}_1\cos {\theta }_1+l_2\left({\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2\right)\cos \left({\theta }_1+{\theta }_2\right)+\\ p_3\left({\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2+{\stackrel{.}{\theta }}_3\right)\cos \left({\theta }_1+{\theta }_2+{\theta }_3+{\alpha }_3\right)\end{array}\right)}^2+\\ {\left(\begin{array}{c}{\stackrel{.}{\theta }}_1{l}_1\sin {\theta }_1+l_2\left({\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2\right)\sin \left({\theta }_1+{\theta }_2\right)+\\ p_3\left({\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2+{\stackrel{.}{\theta }}_3\right)\sin \left({\theta }_1+{\theta }_2+{\theta }_3+{\alpha }_3\right)\end{array}\right)}^2\end{array}\right)+\frac{1}{2}{J_3\left({\stackrel{.}{\theta }}_1+{\stackrel{.}{\theta }}_2+{\stackrel{.}{\theta }}_3\right)}^2{E}_{p3}=m_{3}g\left(l_1\sin {\theta }_1+l_2\sin \left({\theta }_1+{\theta }_2\right)+p_3\sin \left({\theta }_1+{\theta }_2+{\theta }_3+{\alpha }_3\right)\right)& \left(18\right)\end{array}$

A Lagrangian dynamics equation is as follows:

$\begin{array}{cc}{Q}_j=\frac{d}{\mathrm{dt}}\left(\frac{\partial L}{\partial {\stackrel{.}{q}}_j}\right)-\frac{\partial L}{\partial q_j}& \left(19\right)\end{array}$

As described above, Q_j represents generalized force in generalized coordinates, L represents a Lagrangian function, and q_j represents a joint variable.

An expression of the Lagrangian function L in the equation (19) is as follows:

L=E_k−E_p=E_k1+E_k2+E_k3−E_p1−E_p2−E_p3   (20)

Based on the Lagrangian dynamics equation, a mathematical relationship of inverse dynamics of a leg structure of the robot is obtained as follows:

$\begin{array}{cc}\left[\begin{array}{c}{\tau }_1\\ {\tau }_2\\ {\tau }_3\end{array}\right]=\left[\begin{array}{ccc}{D}_{111}& D_{112}& D_{113}\\ D_{121}& D_{122}& D_{123}\\ D_{131}& D_{132}& D_{133}\end{array}\right]\left[\begin{array}{c}{\ddot{\theta }}_1\\ {\ddot{\theta }}_2\\ {\ddot{\theta }}_3\end{array}\right]+\left[\begin{array}{ccc}0& D_{212}& D_{213}\\ D_{221}& 0& D_{223}\\ D_{231}& D_{232}& 0\end{array}\right]\left[\begin{array}{c}{\stackrel{.}{\theta }}_1\\ {\stackrel{.}{\theta }}_2\\ {\stackrel{.}{\theta }}_3\end{array}\right]+\left[\begin{array}{ccc}{D}_{311}& D_{312}& D_{313}\\ 0& D_{322}& D_{323}\\ D_{331}& 0& 0\end{array}\right]\left[\begin{array}{c}{\stackrel{.}{\theta }}_1{\stackrel{.}{\theta }}_2\\ {\stackrel{.}{\theta }}_1{\stackrel{.}{\theta }}_3\\ {\stackrel{.}{\theta }}_2{\stackrel{.}{\theta }}_3\end{array}\right]+\left[\begin{array}{c}{D}_1\\ D_2\\ D_3\end{array}\right]& \left(21\right)\end{array}$

In the formula (21):

$\begin{array}{cc}\{\begin{array}{c}{D}_{131}=J_3+m_3{p}_3^2+m_3{l}_1{p}_3\cos \left({\theta }_2+{\theta }_3+{\alpha }_3\right)+\\ m_3{l}_2{p}_3\cos \left({\theta }_3+{\alpha }_3\right)\\ D_{132}=J_3+m_3{p}_3^2+m_3{l}_2{p}_3\cos \left({\theta }_3+{\alpha }_3\right)\\ D_{133}=J_3+m_3{p}_3^2\\ D_{231}=m_3{l}_2{p}_3\sin \left({\theta }_3+{\alpha }_3\right)+m_3{l}_1{p}_3\sin \left({\theta }_2+{\theta }_3+{\alpha }_3\right)\\ D_{232}=m_3{l}_2{p}_3\sin \left({\theta }_3+{\alpha }_3\right)\\ D_{331}=2m_3{l}_2{p}_3\sin \left({\theta }_3+{\alpha }_3\right)\\ D_3=m_3{\mathrm{gp}}_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3+{\alpha }_3\right)\end{array}& \left(22\right)\end{array}$ $\begin{array}{cc}\{\begin{array}{c}{D}_{121}=J_2+J_3+m_2{p}_2^2+m_3{p}_3^2+m_3{l}_2^2+m_2{l}_1{p}_2\\ \cos \left({\theta }_2+{\alpha }_2\right)+m_3{l}_1{l}_2\cos {\theta }_2+2m_3{l}_2{p}_3\cos ({\theta }_3+{\alpha }_3+m_3{l}_1{p}_3\\ \cos \left({\theta }_2+{\theta }_3+{\alpha }_3\right)\\ D_{122}=J_2+J_3+m_2{p}_2^2+m_3{l}_2^2+m_3{p}_3^2+2m_3{l}_2{p}_3\cos \left({\theta }_3+{\alpha }_3\right)\\ D_{123}=J_3+m_3{p}_3^2+m_3{l}_2{p}_3\cos \left({\theta }_3+{\alpha }_3\right)\\ D_{221}=m_2{l}_1{p}_2\sin \left({\theta }_2+{\alpha }_2\right)+m_3{l}_1{l}_2\sin {\theta }_2+\\ m_3{l}_1{p}_3\sin \left({\theta }_2+{\theta }_3+{\alpha }_3\right)\\ D_{223}=-m_3{l}_2{p}_3\sin \left({\theta }_3+{\alpha }_3\right)\\ D_{322}=-2m_3{l}_2{p}_3\sin \left({\theta }_3+{\alpha }_3\right)\\ D_{323}=-2m_3{l}_2{p}_3\sin \left({\theta }_3+{\alpha }_3\right)\\ D_2-m_3{\mathrm{gp}}_3\cos \left({\theta }_1+{\theta }_2+{\theta }_3+{\alpha }_3\right)+m_3{\mathrm{gl}}_2\cos \left({\theta }_1+{\theta }_2\right)+\\ m_2{\mathrm{gp}}_2\cos \left({\theta }_1+{\theta }_2+{\alpha }_2\right)\end{array}& \end{array}$ $\begin{array}{cc}\{\begin{array}{c}{D}_{111}=J_1+J_2+J_3+m_1{p}_1^2+m_2{l}_1^2+m_2{p}_2^2+m_3{p}_3^2+m_3{l}_1^2+\\ m_3{l}_2^2+2m_2{l}_1{p}_2\cos \left({\theta }_2+{\alpha }_2\right)+2m_3{l}_2\cos \left({\theta }_3+{\alpha }_3\right)+\\ 2m_3{l}_1{l}_2\cos {\theta }_2+2m_3{l}_1{p}_3\cos \left({\theta }_2+{\theta }_3+{\alpha }_3\right)\\ D_{112}=J_2+J_3+m_2{p}_2^2+m_3{p}_3^2+m_3{l}_2^2+m_2{l}_1{p}_2\cos \left({\theta }_2+{\alpha }_2\right)+\\ m_3{l}_1{l}_2\cos {\theta }_2+2l_2{m}_3{p}_3\cos \left({\theta }_3+{\alpha }_3\right)+l_1{m}_3{p}_3\cos \left({\theta }_2+{\theta }_3+{\alpha }_3\right)\\ D_{113}=J_1+m_3{p}_3^2+m_3{l}_2{p}_3\cos \left({\theta }_3+{\alpha }_3\right)\\ +m_3{l}_1{p}_3\cos \left({\theta }_2+{\theta }_3+{\alpha }_3\right)\\ D_{212}=-(m_3{l}_1{l}_2\sin {\theta }_2+m_2{l}_1{p}_2\sin \left({\theta }_2+{\alpha }_2\right)+m_3{l}_1{p}_3\\ \sin \left({\theta }_2+{\theta }_3+{\alpha }_3\right))\\ D_{213}=-\left(m_3{l}_2{p}_3\sin \left({\theta }_3+{\alpha }_3\right)+m_3{l}_1{p}_3\sin \left({\theta }_2+{\theta }_3+{\alpha }_3\right)\right)\\ D_{311}=-(2m_2{l}_1{p}_2\sin \left({\theta }_2+{\alpha }_2\right)+2m_3{l}_1{l}_2\sin {\theta }_2+2m_3{l}_1{p}_3\\ \sin \left({\theta }_2+{\theta }_3+{\alpha }_3\right))\\ D_{312}=-\left(2m_3{l}_2{p}_3\sin \left({\theta }_3+{\alpha }_2\right)+2m_3{l}_1{p}_3\sin \left({\theta }_2+{\theta }_3+{\alpha }_3\right)\right)\\ D_{313}=-(2m_3{l}_2{p}_3\sin \left({\theta }_3+{\alpha }_3\right)+m_3{l}_1{p}_3\sin \left({\theta }_2+{\theta }_3+{\alpha }_3\right)+\\ m_3{l}_1{p}_3\sin \left({\theta }_2+{\theta }_3+{\alpha }_3\right))\\ D_1=m_1{\mathrm{gp}}_1\cos \left({\theta }_1+{\alpha }_1\right)+m_{2}g\left(l_1\cos {\theta }_1+p_2\cos \left({\theta }_1+{\theta }_2+{\alpha }_2\right)\right)+\\ m_{3}g\left(l_1\cos {\theta }_1+l_2\cos \left({\theta }_1+{\theta }_2\right)+p_3\cos \left(\begin{array}{c}{\theta }_1+{\theta }_2+\\ {\theta }_3+{\alpha }_3\end{array}\right)\right)+\\ m_3{l}_1{l}_2\cos \left({\theta }_2\right)\end{array}& \end{array}$

Step 2: Describe the hydraulic drive system of the leg and an impedance control principle of the hydraulic drive system. Based on the leg kinematics model, the leg statics model, and the leg dynamics model, select different impedance control methods for different joints to generate multiple impedance configuration schemes and obtain an implementation principle of each impedance configuration.

FIG. 9 is a schematic diagram of an impedance characteristic of the hydraulic drive system of the leg. Impedance control is a main method for implementing compliance control of the robot. Impedance control is implemented for the hydraulic drive system of the leg by adjusting an impedance characteristic Z_D (including desired mass M_D, a desired damping B_D, and desired stiffness K_D) of the leg, to enable the system to have the desired impedance characteristic shown in FIG. 9.

It can be learned from FIG. 9 that a relational expression of acting force between the foot position of the robot and an environment is as follows:

$\begin{array}{cc}\{\begin{array}{c}{M}_{\mathrm{Dx}}\left(\Delta {\ddot{X}}_{\mathrm{Dx}}^{\mathrm{leg}}-\Delta {\ddot{X}}_{\mathrm{rx}}^{\mathrm{leg}}\right)+B_{\mathrm{Dx}}\left(\Delta {\stackrel{.}{X}}_{\mathrm{Dx}}^{\mathrm{leg}}-\Delta {\stackrel{.}{X}}_{\mathrm{rx}}^{\mathrm{leg}}\right)+K_{\mathrm{Dr}}\left(\Delta X_{\mathrm{Dx}}^{\mathrm{leg}}-\Delta X_{\mathrm{rx}}^{\mathrm{leg}}\right)=\\ \Delta F_{\mathrm{sx}}^{\mathrm{foot}}-\Delta F_{\mathrm{kx}}^{\mathrm{foot}}\\ \begin{array}{c}{M}_{\mathrm{Dy}}\left(\Delta {\ddot{X}}_{\mathrm{Dx}}^{\mathrm{leg}}-\Delta {\ddot{X}}_{\mathrm{rx}}^{\mathrm{leg}}\right)+B_{\mathrm{Dy}}\left(\Delta {\stackrel{.}{X}}_{\mathrm{Dx}}^{\mathrm{leg}}-\Delta {\stackrel{.}{X}}_{\mathrm{rx}}^{\mathrm{leg}}\right)+K_{\mathrm{Dy}}\left(\Delta X_{\mathrm{Dx}}^{\mathrm{leg}}-\Delta X_{\mathrm{rx}}^{\mathrm{leg}}\right)\\ \Delta F_{\mathrm{sy}}^{\mathrm{foot}}-\Delta F_{\mathrm{ky}}^{\mathrm{foot}}\end{array}\end{array}& \left(23\right)\end{array}$

Laplace transform is performed on the relational expression (23) to obtain the following:

$\begin{array}{cc}\{\begin{array}{c}\Delta X_{\mathrm{Dx}}^{\mathrm{leg}}-\Delta X_{\mathrm{rx}}^{\mathrm{leg}}=\frac{\Delta F_{\mathrm{sx}}^{\mathrm{foot}}-\Delta F_{\mathrm{kx}}^{\mathrm{foot}}}{M_{\mathrm{Dx}}{s}^2+B_{\mathrm{Dx}}s+K_{\mathrm{Dx}}}\\ \Delta X_{\mathrm{Dy}}^{\mathrm{leg}}-\Delta X_{\mathrm{ry}}^{\mathrm{leg}}=\frac{\Delta F_{\mathrm{sy}}^{\mathrm{foot}}-\Delta F_{\mathrm{ky}}^{\mathrm{foot}}}{M_{\mathrm{Dy}}{s}^2+B_{\mathrm{Dy}}s+K_{\mathrm{Dy}}}\end{array}\mathrm{or}& \left(24\right)\end{array}$ $\begin{array}{cc}\{\begin{array}{c}\Delta F_{\mathrm{sx}}^{\mathrm{foot}}-\Delta F_{\mathrm{kx}}^{\mathrm{foot}}=\left(M_{\mathrm{Dx}}{s}^2+B_{\mathrm{Dx}}s+K_{\mathrm{Dx}}\right)\left(\Delta X_{\mathrm{Dx}}^{\mathrm{leg}}-\Delta X_{\mathrm{rx}}^{\mathrm{leg}}\right)\\ \Delta F_{\mathrm{sy}}^{\mathrm{foot}}-\Delta F_{\mathrm{ky}}^{\mathrm{foot}}=\left(M_{\mathrm{Dy}}{s}^2+B_{\mathrm{Dy}}s+K_{\mathrm{Dy}}\right)\left(\Delta X_{\mathrm{Dy}}^{\mathrm{leg}}-\Delta X_{\mathrm{ry}}^{\mathrm{leg}}\right)\end{array}& \left(25\right)\end{array}$

The formulas (24) and (25) have the same leg impedance characteristic, but are applicable to different systems. The formula (24) is applicable to position-based impedance control. That is, when disturbing force ΔF_sx^foot−ΔF_kx^foot and disturbing force ΔF_sy^foot−ΔF_ky^foot are applied to the foot, position errors ΔX_Dx^leg−ΔX_rx^leg and ΔX_Dy^leg−ΔX_ry^leg are generated for the foot due to the desired impedance characteristic of the leg, to balance the disturbing force ΔF_sx^foot−ΔF_kx^foot and the disturbing force ΔF_sy^foot−ΔF_ky^foot. The formula (25) is applicable to force-based impedance control. That is, when disturbing positions ΔX_Dx^leg−ΔX_rx^leg and ΔX_Dy^leg−ΔX_ry^leg are applied to the foot, force errors ΔF_sx^foot−ΔF_kx^foor and ΔF_sy^foot−ΔF_ky^foot are generated for the foot due to the desired impedance characteristic of the leg, to balance the disturbing positions ΔX_Dx^leg−ΔX_rx^leg and ΔX_Dy^leg−ΔX_ry^leg.

FIG. 10 and FIG. 11 show principles of implementing position-based/force-based impedance control for the leg. Eight impedance configuration schemes are obtained for the hydraulic drive system of the leg based on FIG. 12. For details, refer to Table 2.

TABLE 2
Impedance configuration schemes for the hydraulic drive system of the leg
	Hip joint	Knee joint	Ankle joint
Scheme	Position-base	Force-base	Position-base	Force-base	Position-base	Force-base
1	•		•		•
2		•		•		•
3	•		•			•
4	•			•	•
5		•	•		•
6	•			•		•
7		•		•	•
8		•	•			•

FIG. 13 shows a principle of implementing an impedance configuration scheme for the hydraulic drive system of the leg. As shown in FIG. 13, the implementation of the impedance configuration scheme for the hydraulic drive system of the leg includes three parts: an outer impedance control loop, an inner impedance control loop, and a module for switching between inner-loop position-based impedance control and inner-loop force-based impedance control. An implementation principle of a module for switching between inner-loop position-based impedance control and inner-loop force-based impedance control in cylinder 1 is used as an example. When C₁ is set to a value greater than 0, a switching switch sets S₁ to 1. Therefore, |S₁−1|=0. In this case, position-based control is performed on the inner impedance control loop. When C₁ is set to a value equal to or less than 0, the switching switch sets S₁ to 0. Therefore, |S₁−1|=1. In this case, force-based control is performed on the inner impedance control loop.

As shown in FIG. 13, the impedance configuration scheme for the hydraulic drive system of the leg is implemented according to the following four steps:

(1) In the outer impedance control loop, convert a disturbing force signal of the foot into a variation of an input signal of an inner position-based control loop of each joint. When the foot suffers disturbing force, a force sensor of the hydraulic drive unit of each joint detects the force signal. A disturbing force component of the foot in the signal detected by the force sensor of the hydraulic drive unit of each point is obtained by using inverse leg dynamics. Forward calculation is performed by using leg statics, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and the torque of each joint, to obtain the disturbing force (ΔF_sx^foot−ΔF_kx^foot, ΔF_sy^foot−ΔF_ky^foot) that the foot suffers. Then, a leg impedance characteristic solver (Z_Dx=M_Dxs²+B_Dxs+K_Dx, Z_Dy=M_Dys²+B_Dys+K_Dy) is used to obtain a foot position variation (ΔX_Dx^leg−ΔX_rx^leg, ΔX_Dy^leg−ΔX_ry^leg) corresponding to the disturbing force that the foot suffers. Finally, variations of input signals of inner position-based control loops of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint are obtained by using inverse leg kinematics and the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint.

(2) In the outer impedance control loop, convert a position signal of an external load into a variation of an input signal of an inner force-based control loop of each joint. When the foot suffers a disturbing position, position sensors of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint each detect a position signal. Forward calculation is performed by using the leg kinematics model and the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, to obtain a position signal of the foot. An error between an input position of the foot and an actual position of the foot is used as a desired outer-loop position of the foot. Then, the leg impedance characteristic solver is used to convert the desired outer-loop position of the foot into a corresponding desired force signal of foot impedance. Inverse calculation is performed on the desired force signal of the foot impedance by using the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each point, the rotation angle of each joint, and the torque of each joint, to obtain variations of input signals of inner force-based control loops of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint.

(3) Set values of C₁, C₂, and C₃ in advance to switch between inner-loop position-based impedance control and inner-loop force-based impedance control for cylinders 1, 2, and 3.

(4) Based on the signals detected by the position sensors of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint, and the signals detected by the force sensors of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint, obtain the actual position and a force variation of the foot of the leg by using forward leg structure kinematics and forward leg structure statics, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and the torque of each joint.

Step 3: Analyze distribution of equivalent mass of the hydraulic drive unit of each joint within motion space of the foot when the hydraulic drive system of the leg is in a swing phase state and a stance phase state. An optimal inner-loop impedance control mode is selected for each joint of the leg based on different distribution of the equipment mass and different external disturbance characteristics of the hydraulic drive unit of each joint within the motion space of the foot in the swing phase state and the stance phase state, and influence of the distribution of the equivalent mass on impedance control performance of the hydraulic drive unit of the joint. Finally, a novel impedance configuration is obtained for the 3DOF leg of the hydraulically-driven legged robot.

In the motion process of the robot, the hydraulic drive system of the leg may be in the swing phase state and the stance phase state. Therefore, in the present disclosure, swing phase equivalent mass and stance phase equivalent mass are separately calculated for the hydraulic drive unit of each joint in the hydraulic drive system of the leg.

FIG. 14 is a schematic structural diagram of the hydraulic drive system of the leg in the swing phase state.

According to the formula (6), a relationship between rotation speed of the hip joint and motion speed of the hydraulic drive unit of the hip joint is as follows:

$\begin{array}{cc}\frac{{\stackrel{.}{\theta }}_1}{v_1}=\frac{\frac{l_{01}+\Delta x_{p1}}{\mathrm{OA}·\mathrm{OB}}}{\sqrt{1-{\left(\frac{{\mathrm{OA}}^2+{\mathrm{OB}}^2-{\left(l_{01}+\Delta x_{p1}\right)}^2}{2·\mathrm{OA}·\mathrm{OB}}\right)}^2}}& \left(26\right)\end{array}$

Kinetic energy E_kt1 of rotation of the thigh member, the crus member, and the foot member around the hip joint is as follows:

E_kt1=½(J₁+J₂+J₃+m₁p₁²+m₂(l₁²+p₂²+2l₁p₂ cos(θ₂+α₂))+m₃(l₁²+l₂²+p₃²+2l₁l₂ cos (θ₂)+2l₁p₃ cos(θ₂+θ₃+α₃)+2l₂p₃ cos (θ₃+α₃))){dot over (θ)}₁²   (27)

According to the formula (27) and a principle of constant kinetic energy, the following can be obtained:

E_kt1=½m_Lt1v₁²   (28)

As described above, m_Lt1 represents equivalent mass of rotation of the thigh member, the crus member, and the foot member around the hip joint.

According to the formulas (26), (27), and (28), the equivalent mass m_Lt1 of rotation of the thigh member, the crus member, and the foot member around the hip joint is as follows:

$\begin{array}{cc}{m}_{\mathrm{Lt}1}=\frac{\left(J_1+J_2+J_3+m_1{p}_1^2+m_2{l}_{12}^2+m_3{l}_{13}^2\right){\left(l_{01}+\Delta x_{p1}\right)}^2}{{\left(\mathrm{OA}·\mathrm{OB}\right)}^2\left(1-{\left(\frac{{\mathrm{OA}}^2+{\mathrm{OB}}^2-{\left(l_{01}+\Delta x_{p1}\right)}^2}{2·\mathrm{OA}·\mathrm{OB}}\right)}^2\right)}& \left(29\right)\end{array}$

According to the formula (6), a relationship between rotation speed of the knee joint and motion speed of the hydraulic drive unit of the knee joint is as follows:

$\begin{array}{cc}\frac{{\stackrel{.}{\theta }}_2}{v_2}=\frac{\frac{l_{02}+\Delta x_{p2}}{\mathrm{EC}·\mathrm{ED}}}{\sqrt{1-{\left(\frac{{\mathrm{EC}}^2+{\mathrm{ED}}^2-{\left(l_{02}+\Delta x_{p2}\right)}^2}{2·\mathrm{EC}·\mathrm{ED}}\right)}^2}}& \left(30\right)\end{array}$

Kinetic energy E_Kt2 of rotation of the crus member and the foot member around the knee joint is as follows:

E_kt1=½(J₂+J₃+m₂p₂²+m₃(l₂²+p₃²+2l₂p₃ cos(θ₃+α₃))){dot over (θ)}₂²   (31)

According to the formula (31) and the principle of constant kinetic energy, the following can be obtained:

E_kt2=½m_Lt2v₂²   (32)

In the formula (32), m_Lt2 represents equivalent mass of rotation of the crus member and the foot member around the knee joint.

According to the formulas (30), (31), and (32), the equivalent mass m_Lt2 of rotation of the crus member and the foot member around the knee joint is as follows:

$\begin{array}{cc}{m}_{\mathrm{Lt}2}=\frac{\left(J_2+J_3+m_2{p}_2^2+m_3{l}_{23}^2\right){\left(l_{02}+\Delta x_{p2}\right)}^2}{{\left(\mathrm{EC}·\mathrm{ED}\right)}^2\left(1-{\left(\frac{{\mathrm{EC}}^2+{\mathrm{ED}}^2-{\left(l_{02}+\Delta x_{p2}\right)}^2}{2·\mathrm{EC}·\mathrm{ED}}\right)}^2\right)}& \left(33\right)\end{array}$

According to the formula (6), a relationship between rotation speed of the ankle joint and motion speed of the hydraulic drive unit of the ankle joint is as follows:

$\begin{array}{cc}\frac{{\stackrel{.}{\theta }}_3}{v_3}=\frac{\frac{l_{03}+\Delta x_{p3}}{\mathrm{GF}·\mathrm{GH}}}{\sqrt{1-{\left(\frac{{\mathrm{GF}}^2+{\mathrm{GH}}^2-{\left(l_{03}+\Delta x_{p3}\right)}^2}{2·\mathrm{GF}·\mathrm{GH}}\right)}^2}}& \left(34\right)\end{array}$

Kinetic energy E_kt3 of rotation of the foot member around the ankle joint is as follows:

E_kt3=½(J₃+m₃p₃²){dot over (θ)}₃²   (35)

According to the formula (35) and the principle of constant kinetic energy, the following can be obtained:

E_kt3=½m_Lt3v₃²   (36)

In the formula (36), m_Lt3 represents equivalent mass of rotation of the foot member around the ankle joint.

According to the formulas (34), (35), and (36), the equivalent mass m_Lt3 of rotation of the foot member around the ankle joint is as follows:

$\begin{array}{cc}{m}_{\mathrm{Lt}3}=\frac{\left(J_3+m_3{p}_3^2\right){\left(l_{03}+\Delta x_{p3}\right)}^2}{{\left(\mathrm{GF}·\mathrm{GH}\right)}^2\left(1-{\left(\frac{{\mathrm{GF}}^2+{\mathrm{GH}}^2-{\left(l_{03}+\Delta x_{p3}\right)}^2}{2·\mathrm{GF}·\mathrm{GH}}\right)}^2\right)}& \left(37\right)\end{array}$

As shown in FIG. 15, kinetic energy E_kz3 of rotation of a base member, the thigh member, and the crus member around the ankle joint is as follows:

E_kz3=½(J₁+J₂+J₄+m₁l₃₁²+m₂l₃₂²+m₄l₃₄²){dot over (θ)}₃²   (38)

In the formula (38), J₄ represents a moment of inertia of the base member, l₃₁ represents a distance between the centroid of the thigh member and the ankle joint, l₃₂ represents a distance between the centroid of the crus member and the ankle joint, and l₃₄ represents a distance between a centroid of the base member and the ankle joint.

Based on a geometrical relationship in FIG. 15, l₃₁ and l₃₄ in the formula (38) are as follows:

$\begin{array}{cc}{l}_{31}=\sqrt{\begin{array}{c}{l}_1^2+l_2^2+p_1^2+2l_1{l}_2\cos \left({\theta }_2\right)-2l_1{p}_1\cos \left({\alpha }_1\right)-\\ 2l_2{p}_1\cos \left({\theta }_2-{\alpha }_1\right)\end{array}}{l}_{34}=\sqrt{\begin{array}{c}{l}_1^2+l_2^2+p_4^2+2l_1{l}_2\cos \left({\theta }_2\right)-2l_1{p}_4\cos \left({\theta }_1-{\beta }_3\right)-\\ 2l_2{p}_4\cos \left({\theta }_1+{\theta }_2-{\beta }_3\right)\end{array}}& \left(39\right)\end{array}$

According to the formula (38) and the principle of constant kinetic energy, the following can be obtained:

E_kz3=½m_Lz3v₃²   (40)

In the formula (40), m_Lz3 represents equivalent mass of rotation of the base member, the thigh member, and the crus member around the ankle joint.

According to the formulas (38), (39), and (40), the equivalent mass m_Lz3 of rotation of the base member, the thigh member, and the crus member around the ankle joint is as follows:

$\begin{array}{cc}{m}_{\mathrm{Lz}3}=\frac{\begin{array}{c}\left(J_1+J_2+J_4+m_1{l}_{31}^2+m_2{l}_{32}^2+m_4{l}_{34}^2\right)\\ {\left(l_{03}+\Delta x_{p3}\right)}^2\end{array}}{{\left(\mathrm{GF}·\mathrm{GH}\right)}^2\left(1-{\left(\frac{{\mathrm{GF}}^2+{\mathrm{GH}}^2-{\left(l_{03}+\Delta x_{p3}\right)}^2}{2·\mathrm{GF}·\mathrm{GH}}\right)}^2\right)}& \left(41\right)\end{array}$

Kinetic energy E_kz2 of rotation of the base member and the thigh member around the knee joint is as follows:

E_kz2=½(J₁+J₄+m₁l₂₁²+m₄l₂₄²){dot over (θ)}₂²   (42)

In the formula (42), l₂₁ represents a distance between the centroid of the thigh member and the knee joint, and l₂₄ represents a distance between the centroid of the base member and the knee joint.

Based on the geometrical relationship in FIG. 15 l₂₄ in the formula (42) is as follows:

l₂₄=√{square root over (l₁²+p₄²+2l₁p₄ cos(β₃−θ₁))}  (43)

According to the formula (42) and the principle of constant kinetic energy, the following can be obtained:

E_kz2=½m_Lz2v₂²   (44)

In the formula (44), m_Lz2 represents equivalent mass of rotation of the base member and the thigh member around the knee joint.

According to the formulas (42), (43), and (44), the equivalent mass m_Lz2 of rotation of the base member and the thigh member around the knee joint is as follows:

$\begin{array}{cc}{m}_{\mathrm{Lz}2}=\frac{\left(J_1+J_4+m_1{l}_{21}^2+m_4{l}_{24}^2\right){\left(l_{02}+\Delta x_{p2}\right)}^2}{{\left(\mathrm{EC}·\mathrm{ED}\right)}^2\left(1-{\left(\frac{{\mathrm{EC}}^2+{\mathrm{ED}}^2-{\left(l_{02}+\Delta x_{p2}\right)}^2}{2·\mathrm{EC}·\mathrm{ED}}\right)}^2\right)}& \left(45\right)\end{array}$

Kinetic energy E_kz1 of rotation of the base member around the hip joint is as follows:

E_kz1=½(J₄+m₄p₄²){dot over (θ)}₁²   (46)

According to the formula (46) and the principle of constant kinetic energy, the following can be obtained:

E_kz1=½m_Lz1v₁^t   (47)

In the formula (47), m_Lz1 represents equivalent mass of rotation of the base member around the hip joint.

According to the formulas (46) and (47), the equivalent mass m_Lz1 of rotation of the base member around the hip joint is as follows:

$\begin{array}{cc}{m}_{\mathrm{Lz}1}=\frac{\left(J_4+m_4{p}_4^2\right){\left(l_{01}+\Delta x_{p1}\right)}^2}{{\left(\mathrm{OA}·\mathrm{OB}\right)}^2\left(1-{\left(\frac{{\mathrm{OA}}^2+{\mathrm{OB}}^2-{\left(l_{01}+\Delta x_{p1}\right)}^2}{2·\mathrm{OA}·\mathrm{OB}}\right)}^2\right)}& \left(48\right)\end{array}$

Table 3 lists specific parameters of leg structures in FIG. 14 and FIG. 15.

TABLE 3
Specific parameters in a schematic diagram of the mechanical
structure of the hydraulic drive system of the leg
	Length		Angle		Mass		Moment of inertia
Parameter	(mm)	Parameter	(°)	Parameter	(kg)	Parameter	(kg · m2)
p1	155.7751	α1	−5.7591	m1	6.9082	J1	0.0526
p2	138.7153	α2	16.7343	m2	6.5005	J2	0.0452
p3	179.9112	α3	−3.9971	m3	4.9153	J3	0.0835
p4	127.4765	β1	6.1524	m4	3.1498	J4	0.0339
L32	181.6057	β2	−12.7050
L21	145.8512	β3	38.3134

In the present disclosure, MATLAB is used to obtain the equivalent mass of the hydraulic drive unit of each joint within the motion space of the foot when the hydraulic drive system of the leg is in the swing phase state and the stance phase state, as shown in a) and b) in FIG. 16.

When the hydraulic drive system of the leg is in the stance phase state, only mass of the mechanical structure of the leg is considered. Equivalent mass of the hydraulic drive unit of the knee joint and equivalent mass of the hydraulic drive unit of the ankle joint partially overlap. In an actual working process of the leg of the robot, a body of the robot and the external load are loaded onto the base member. Therefore, a centroid position and mass of the base member are ever-changing. In the present disclosure, three schemes are designed to study a change of the equivalent mass of the hydraulic drive unit of each joint when the centroid position and the mass of the base member are ever-changing.

Scheme 1: When the centroid position of the base member keeps unchanged, and the mass m₄ of the base member is 10 kg, 50 kg, 100 kg, and 300 kg in sequence, the equivalent mass of the hydraulic drive unit of each joint of the leg is shown in FIG. 17. As shown in FIG. 17, a) to c) show longitudinal change curves of the equivalent mass of each joint when the mass of the base member increases continuously, and d) to g) show horizontal change curves of the equivalent mass of each joint when the mass of the base member increases continuously.

Scheme 2: When the mass m₄ of the base member is fixedly 3.1498 kg, a horizontal distance between the centroid of the base member and the origin of the coordinates is fixedly 100.0957 mm, and a vertical distance H between the centroid of the base member and the origin of the coordinates is 100 mm, 200 mm, 300 mm, and 400 mm in sequence, the equivalent mass of the hydraulic drive unit of each joint of the leg is shown in FIG. 18. As shown in FIG. 18, a) to c) show longitudinal change curves of the equivalent mass of each joint when a centroid height of the base member increases continuously, and d) to g) show horizontal change curves of the equivalent mass of each joint when the centroid height of the base member increases continuously.

Scheme 3: It is assumed that a cuboid with a length of 400 mm, a width of 400 mm, a height of h, and a density of 2700 kg/m³ is loaded onto the base member, as shown in FIG. 19. It is assumed that total mass of the base member and the cuboid is centered on a centroid of the cuboid. When the height h of the cuboid is 100 mm, 200 mm, 300 mm, and 400 mm, parameters of the base member are listed in Table 4.

TABLE 4
Specific parameters of the
base member loaded with another member
Height h	Angle	Mass m4	Length p4	Moment of inertia J4
(mm)	β3 (°)	(kg)	(mm)	(kg · m2)
100	55.8610	44.0341	196.0395	0.6276
200	62.5929	87.2341	239.5273	1.4654
300	67.2226	130.4341	284.9976	2.7393
400	70.5657	173.6341	331.7387	4.6653

Based on Table 4, MATLAB is used to obtain a solution. The equivalent mass of the hydraulic drive unit of each joint of the leg is shown in a) to g) in FIG. 20.

In the present disclosure, the following inclusions can be obtained through analysis based on theoretical formulas and curves of the distribution of the equivalent mass of the hydraulic drive unit of each joint when the hydraulic drive system of the leg is in the swing phase state and the stance phase state:

(1) When the hydraulic drive system of the leg is in the swing phase state, the equivalent mass of the hydraulic drive unit of each joint of the leg within the motion space of the foot satisfies the following relationship: equivalent mass of the hydraulic drive unit of the knee joint>equivalent mass of the hydraulic drive unit of the hip joint>equivalent mass of the hydraulic drive unit of the ankle joint.

(2) When the hydraulic drive system of the leg is in the stance phase state, no matter how the centroid position and the mass of the base member change, the equivalent mass of the hydraulic drive unit of each joint within most of the motion space of the foot satisfies the following relationship: equivalent mass of the hydraulic drive unit of the ankle joint>equivalent mass of the hydraulic drive unit of the knee j oint>equivalent mass of the hydraulic drive unit of the hip joint.

A main objective of the present disclosure is to use a different inner-loop core control mode for each joint of the leg based on different equivalent load mass and different external disturbance characteristics of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint in an actual motion process of the hydraulic drive system of the leg, to finally design a novel impedance control configuration for multiple joints of a leg. The equivalent mass has the following influence on the inner impedance control loop:

1. When position-based control is performed on the inner impedance control loop:

Load mass greatly affects performance of a position-based control system of the hydraulic drive unit. Higher inertia usually leads to a lower natural frequency of the hydraulic system. Larger equivalent mass leads to a lower natural frequency of the hydraulic system and poorer control performance of the system. In addition, a drastic change of the equivalent mass leads to poor control performance of the system because a natural frequency and a damping of the hydraulic system greatly change accordingly.

2. When force-based control is performed on the inner impedance control loop:

Load mass plays an important role in performance of a force-based control system of the hydraulic drive unit. Higher inertia usually leads to higher control bandwidth. When the hydraulic drive system of the leg of the robot is in the swing phase state, a joint close to the base usually has a heavier link than a joint far away from the base. Therefore, the joint close to the base usually has better force tracking performance than the joint far away from the base.

Based on the distribution of the equivalent mass of the hydraulic drive unit of each joint within the motion space of the foot in the swing phase state and the stance phase state, and the influence of the distribution of the equivalent mass on the impedance control performance of the hydraulic drive unit of the joint, the following inner-loop control modes is selected for various joints in the hydraulic drive system of the leg to improve impedance control performance of the hydraulic drive system of the leg:

1. The hip joint is closer to the base member relative to the other joints. According to a kinematics formula and a simulation analysis result, relative to the other joints, a position change of the hydraulic drive unit of the hip joint results in a larger foot variation. Therefore, to improve control precision of the foot, it is more important to ensure position-based control precision of the hip joint. In addition, when the hydraulic drive system of the leg is in the stance phase state, the hip joint has minimum equivalent mass relative to the other joints, and as a result, has poorer force-based control performance than the other joints. Based on the foregoing two aspects, it is reasonable to select position-based control as an inner-loop impedance control mode for the hydraulic drive unit of the hip joint.

2. When the hydraulic drive system of the leg is in the swing phase state, maximum equivalent mass of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint is 896.1710 kg, 1907.1834 kg, and 162.3234 kg respectively. The knee joint has maximum equivalent mass and best force-based control performance relative to the other joints. In addition, when the hydraulic drive system of the leg is in the stance phase state, the equivalent mass of the hydraulic drive unit of the knee joint is slightly less than that of the ankle joint and is remarkably greater than that of the hip joint. In addition, within a small part of motion space, load mass of the knee joint is greater than that of the ankle joint. Based on the foregoing two aspects, it is reasonable to select force-based control as an inner-loop impedance control mode for the hydraulic drive unit of the knee joint.

3. When the hydraulic drive system of the leg is in the stance phase state, the foot is in contact with the ground, and the base member rotates relative to the foot to push the robot to move forward. In this case, the ankle joint is closer to the foot relative to the other joints. Therefore, tracking precision of the hydraulic drive unit of the ankle joint plays a more important role in improving motion precision of the robot when the leg of the robot is in a support-phase state. In addition, within most of the motion space of the foot (including motion trajectories of most gaits of the robot), the ankle joint has maximum equivalent mass and best force-based control performance relative to the other joints. Based on the foregoing two aspects, it is reasonable to select force-based control as an inner-loop impedance control mode for the hydraulic drive unit of the ankle joint.

In conclusion, in Table 2, the sixth scheme (performing position-based control on the hip joint, and force-based control on the knee joint and the ankle joint) has better control performance than the other seven schemes, and is an optimal impedance configuration for the hydraulic drive system of the leg.

Each embodiment of this specification is described in a progressive manner, each embodiment focuses on the difference from other embodiments, and the same and similar parts between the embodiments may refer to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description of the system is relatively simple, and the relevant information can be referred to the description of the method.

In this specification, one example is used for illustration of the principles and implementations of the present disclosure. The description of the foregoing embodiments is merely used to help illustrate the method of the present disclosure and the core principles thereof. In addition, those of ordinary skill in the art can make various modifications in terms of specific implementations and a scope of application in accordance with the teachings of the present disclosure. In conclusion, the content of this specification shall not be construed as a limitation to the present disclosure.

Claims

1. A method for generating a novel impedance configuration for a three-degree-of-freedom (3DOF) robotic leg, comprising:

obtaining a mathematical model of a hydraulic drive system of the leg of a to-be-controlled robot, wherein the mathematical model comprises a leg kinematics model, a leg statics model, a leg dynamics model, a leg impedance characteristic model, a mapping between an elongation of a hydraulic drive unit of each joint of the leg and a rotation angle of each joint of the leg, and a mapping among force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and a torque of each joint; and a hip joint, a knee joint, and an ankle joint are comprised;

determining, when the foot of the to-be-controlled robot suffers disturbing force, a variation of an input signal of an inner position-based control loop of a hydraulic drive unit of each joint based on the mathematical model;

determining, when the foot suffers a disturbing position, a variation of an input signal of an inner force-based control loop of the hydraulic drive unit of each joint based on the mathematical model;

generating a novel impedance configuration in which position-based control is perfoli ied on an inner control loop of a hydraulic drive unit of the hip joint, and force-based control is performed on inner control loops of hydraulic drive units of the knee joint and the ankle joint, and using a servo valve to obtain output positions and output force of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint; and

based on the output positions and the output force of the hydraulic drive units of the hip joint, the knee joint, and the ankle joint, performing forward calculation by using the leg kinematics model, the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and the torque of each joint, to obtain an actual position and a force variation of the foot of the leg.

2. The method for generating a novel impedance configuration for a 3DOF robotic leg according to claim 1, wherein before the obtaining a mathematical model of a hydraulic drive system of the leg of a to-be-controlled robot, the method further comprises:

obtaining a length of each member in the hydraulic drive system of the leg of the to-be-controlled robot, the rotation angle of each joint, and the torque of each joint, wherein a thigh member, a crus member, and a foot member are comprised;

constructing the leg kinematics model based on the length of each member and the rotation angle of each joint;

constructing the leg statics model based on the length of each member, the rotation angle of each joint, and the torque of each joint; and

constructing the leg dynamics model based on a Lagrangian dynamics equation.

3. The method for generating a novel impedance configuration for a 3DOF robotic leg according to claim 1, wherein the determining, when the foot of the to-be-controlled robot suffers disturbing force, a variation of an input signal of an inner position-based control loop of the hydraulic drive unit of each joint based on the mathematical model specifically comprises:

when the foot of the to-be-controlled robot suffers the disturbing force, detecting, by a force sensor of the hydraulic drive unit of each joint, a force signal, performing inverse calculation by using the leg dynamics model, to obtain a disturbing force component of the foot in the signal detected by the force sensor of the hydraulic drive unit of each point, and performing forward calculation by using the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each joint, the rotation angle of each joint, and the torque of each joint, to obtain the disturbing force that the foot suffers;

using a leg impedance characteristic solver to obtain a foot position variation corresponding to the disturbing force that the foot suffers; and

performing inverse calculation based on the foot position variation by using the leg kinematics model and the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, to obtain the variation of the input signal of the inner position-based control loop of the hydraulic drive unit of each joint.

4. The method for generating a novel impedance configuration for a 3DOF robotic leg according to claim 1, wherein the determining, when the foot suffers a disturbing position, a variation of an input signal of an inner force-based control loop of the hydraulic drive unit of each joint based on the mathematical model specifically comprises:

when the foot suffers the disturbing position, detecting, by a position sensor of the hydraulic drive unit of each joint, a position signal, and then performing forward calculation by using the leg kinematics model and the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, to obtain a position signal of the foot, wherein the position signal of the foot comprises an input position of the foot and the actual position of the foot;

obtaining a desired outer-loop position of the foot based on the position signal of the foot; and

converting the desired outer-loop position of the foot into a corresponding desired force signal of foot impedance by using a leg impedance characteristic solver, and performing inverse calculation on the desired force signal of the foot impedance by using the leg statics model, the mapping between the elongation of the hydraulic drive unit of each joint and the rotation angle of each joint, and the mapping among the force applied to the hydraulic drive unit of each point, the rotation angle of each joint, and the torque of each joint, to obtain the variation of the input signal of the inner force-based control loop of the hydraulic drive unit of each joint.

5.-8. (canceled)