A hydraulic quadruped robot touch-down detection method based on evidence theory

Abstract

The application provides a hydraulic quadruped robot touch ground detection method based on evidence theory, three different data sources are adopted to obtain joint torque characteristic values of joints, and then evidence theory is used to fuse the three joint torque characteristic values, compared with the single use of the force sensor and the single use of the displacement sensor, the application fuses the data information of the force sensor and the displacement sensor, the touch ground detection accuracy is higher, the real-time performance is better, and the robustness of the touch ground detection judgment can be greatly improved, thereby laying a good foundation for the gait planning and control of the hydraulic quadruped robot.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, and in particular relates to a method for detecting ground contact of a hydraulic quadruped robot based on evidence theory. Background Technology

[0002] Currently, in the field of hydraulic quadruped robots, there are two commonly used methods for ground contact detection. One direct method is to add pressure sensors or contact sensors to the ends of the hydraulic legs. The other indirect method relies on force sensors and displacement sensors fixed to the hydraulic cylinder to obtain force and displacement data from the hydraulic cylinder. By analyzing the data from the force and displacement sensors, the impact force on the hydraulic leg can be determined, thus indirectly determining the ground contact state. The basic principle is that the impact force on the hydraulic leg changes significantly before and after ground contact. By acquiring and fusing the data from the force and displacement sensors fixed to the hydraulic cylinder, and utilizing the dynamics and kinematic transformations of the hydraulic leg, the equivalent torque of each joint of the hydraulic leg can be calculated. The timing of ground contact is determined by the rapid change in the equivalent joint torque before and after ground contact.

[0003] There are currently two methods for detecting ground contact in hydraulic quadruped robots. The direct detection method is not suitable for heavy hydraulic quadruped robots because the impact force at the moment of contact is significant, far exceeding the measurement range of typical pressure sensors. While the indirect detection method can detect the ground contact status of the quadruped's legs, it still has the following problems that need to be addressed:

[0004] (1) When performing ground contact detection, the data output by the force sensor is delayed. When the hydraulic quadruped robot touches the ground at high frequency, the phase lag of the force sensor output data is more obvious. The force sensor is suitable for detecting the impact force changes caused by low-frequency foot movement.

[0005] (2) The displacement sensor has high detection sensitivity, but its detection of changes in foot impact force depends on the characteristic signals generated by inertial force and Coriolis force. It is suitable for detecting changes in impact force caused by high-frequency foot movement. The speed and acceleration changes of low-frequency foot movement are not obvious, and the torque changes and characteristic signal amplitudes generated are small. Summary of the Invention

[0006] To address the aforementioned problems, this invention provides a ground contact detection method for hydraulic quadruped robots based on evidence theory. By fusing data from force sensors and displacement sensors fixed to the hydraulic cylinders of the quadruped robot, the ground contact status of the hydraulic legs is determined, which can effectively improve the ground contact detection accuracy of the hydraulic quadruped robot.

[0007] A method for detecting ground contact in a hydraulic quadruped robot based on evidence theory includes the following steps:

[0008] S1: The equivalent joint torque difference of the three joints of the hydraulic leg before and after contact with the ground is obtained by using three different data sources, and the difference vectors corresponding to the different data sources are obtained. The three data sources are: using only the data of the force sensor fixed to the hydraulic leg as the data source, using only the data of the displacement sensor fixed to the hydraulic leg as the data source, and using both the data of the displacement sensor and the data of the force sensor as the data source; the three joints are the hip joint, hip joint, and knee joint.

[0009] S2: The least squares method is used to fuse the three equivalent joint torque differences in each difference vector to obtain the joint torque feature values ​​corresponding to the three difference vectors. When the joint torque feature values ​​take different values, the three independent probability events "not touching the ground", "touching the ground", and "uncertain touching the ground or not touching the ground" have different probabilities.

[0010] S3: Based on the principle of evidence theory, the three joint torque feature values ​​are integrated to obtain the probability that the probability event corresponding to each joint torque feature value is judged to have occurred and the probability that it is judged to be possible. Based on the magnitude between the two probabilities and the probability threshold, the actual ground contact state of the hydraulic leg is determined.

[0011] Furthermore, the method for obtaining the equivalent joint torque difference before and after the three joints of the hydraulic leg touch the ground using only force sensor data as the data source is as follows:

[0012] Data from the force sensor is acquired in real time, and the equivalent torque on the joint is calculated in real time based on the acquired data as follows:

[0013]

[0014] Where, τ f0 τ is the equivalent torque acting on the hip joint. f1 τ is the equivalent torque acting on the hip joint. f2 d is the equivalent torque acting on the knee joint. 00 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 11 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 21 It is the distance from the hydraulic cylinder installed in the knee joint to the knee joint, F s0 It is the reading of the hydraulic cylinder force sensor installed on the hip joint, F s1 It is the reading of the hydraulic cylinder force sensor installed on the hip joint, F s2 It is the reading from the hydraulic cylinder force sensor installed in the knee joint. It refers to the rotation angle of the hip joint. It refers to the rotation angle of the hip joint. It refers to the knee joint rotation angle;

[0015] The calculation method for the rotation angles of the three joints is as follows:

[0016]

[0017] Among them, L c0 It is the length of the hydraulic cylinder when it is fully retracted, d c0 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c1 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c2 It refers to the extension and retraction of the hydraulic cylinder installed in the knee joint, d 01 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 10 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 20 It is the distance from the fixing point of the hydraulic cylinder installed at the knee joint to the knee joint;

[0018] Based on the principle that the data from the force sensor will change abruptly after the hydraulic leg touches the ground, the equivalent torque of the three joints when the hydraulic leg touches the ground is extracted from the equivalent torque of the joints calculated in real time. The equivalent torque of the three joints before and after touching the ground is subtracted to obtain the equivalent joint torque difference vector of the three joints of the hydraulic leg before and after touching the ground when only the data from the force sensor is used as the data source.

[0019] Furthermore, the method for obtaining the equivalent joint torque difference before and after the three joints of the hydraulic leg touch the ground using only displacement sensor data as the data source is as follows:

[0020] By establishing a mapping relationship from the hydraulic cylinder motion space to the joint space, the displacement of the hydraulic cylinder is converted into the angle of joint rotation in real time. Then, the angle of joint rotation is obtained by first-order difference and second-order difference to obtain the angular velocity and angular acceleration of joint rotation.

[0021] By real-time inputting the joint rotation angle, angular velocity, and angular acceleration into the Lagrange dynamics equations, the real-time equivalent joint torque is obtained as follows:

[0022]

[0023] in,

[0024]

[0025]

[0026] Where τ is the column vector of equivalent moment forces acting on the three joints, and M(θ) is the mass matrix. G(θ) is the Coriolis matrix, G(θ) is the gravity matrix, θ0, θ1, and θ2 are the rotation angles of the hip, hip, and knee joints, respectively, and θ, These are the column vectors of rotational angle, rotational angular velocity, and rotational angular acceleration of the three joints, respectively, τ. f0 τ f1 τ f2 The equivalent torques acting on the hip, hip, and knee joints are respectively, d 00 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 10 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 20 It is the distance d from the fixing point of the hydraulic cylinder installed at the knee joint to the knee joint. 01 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 11 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 21 It is the distance L from the hydraulic cylinder installed in the knee joint to the knee joint. c0 It is the length of the hydraulic cylinder when it is fully retracted, d c0 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c1 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c2 It is the extension and retraction of the hydraulic cylinder installed in the knee joint, θ c0 θ is the initial angle of the hip joint. c1 θ is the initial angle of the hip joint. c2 This is the initial angle of the knee joint, and T represents transposition;

[0027] Based on the principle that the data from the displacement sensor will change abruptly after the hydraulic leg touches the ground, the equivalent torque of the three joints when the hydraulic leg touches the ground is extracted from the equivalent torque of the joints calculated in real time. The equivalent torque of the three joints before and after touching the ground is subtracted to obtain the equivalent joint torque difference vector of the three joints of the hydraulic leg before and after touching the ground when only the data from the displacement sensor is used as the data source.

[0028] Furthermore, the method of simultaneously using data from displacement sensors and force sensors as data sources to obtain the equivalent joint torque difference between the three joints of the hydraulic leg before and after contact with the ground is as follows:

[0029] Based on the principle that the data output by the force sensor lags behind the data output by the displacement sensor, when the data from the displacement sensor changes abruptly, the equivalent torque on the joint calculated from the displacement sensor data at the time of the change is taken as the equivalent torque on the joint after contact with the ground. The equivalent torque on the joint calculated from the force sensor data at the same moment of the change is taken as the equivalent torque on the joint before contact with the ground. The difference between the equivalent torque on the joint after contact with the ground obtained from the displacement sensor data and the equivalent torque on the joint before contact with the ground obtained from the force sensor data is calculated to obtain the equivalent joint torque difference vector of the three joints of the hydraulic leg before and after contact with the ground when both displacement sensor data and force sensor data are used as data sources.

[0030] Furthermore, the calculation method for the probability of each joint torque characteristic value being determined as having already occurred and the probability of being determined as being possible is as follows:

[0031] The joint torque characteristic values ​​corresponding to the first, second, and third data sources are represented as Γ0, Γ1, and Γ2, respectively. The probability events "not touching the ground" are denoted as event A, "touching the ground" as event B, and "uncertainty about touching the ground or not touching the ground" as event C.

[0032] According to the set segmentation value, the possible range of joint torque characteristic values ​​Γ0, Γ1, Γ2 is divided into more than three intervals, and reference probabilities corresponding to events A, B, and C are assigned to each interval.

[0033] According to the principles of evidence theory, the normalization constant K is calculated based on the reference probability:

[0034] K=[m1(A)+m1(C)][m2(A)+m2(C)][m3(A)+m3(C)]+[m1(B)+m1(C)][m2(B)+m2(C)][m3(B)+m3(C)]-m1(C)m2(C)m3(C)

[0035] Wherein, m1(A), m1(B), and m1(C) are the reference probabilities of events A, B, and C corresponding to the joint torque feature value Γ0 of the first data source under the current value; m2(A), m2(B), and m2(C) are the reference probabilities of events A, B, and C corresponding to the joint torque feature value Γ1 of the second data source under the current value; and m3(A), m3(B), and m3(C) are the reference probabilities of events A, B, and C corresponding to the joint torque feature value Γ2 of the third data source under the current value.

[0036] Calculate the combined probability P(A) of event A based on the normalization constant K and the reference probability:

[0037]

[0038] The combined probability P(B) of event B is calculated based on the normalization constant K and the reference probability:

[0039]

[0040] The combined probability P(C) of event C is calculated based on the normalization constant K and the reference probability:

[0041]

[0042] Based on the combined probabilities P(A), P(B), and P(C), the characteristic values ​​of joint torque Γ0, Γ1, and Γ2 are calculated. Under the current values, the probabilities that events A, B, and C are confirmed to have occurred are as follows:

[0043]

[0044] Where Bel(A), Bel(B), and Bel(C) are the probabilities that events A, B, and C are confirmed to have occurred, respectively.

[0045] Based on the combined probabilities P(A), P(B), and P(C), the characteristic values ​​of joint torque Γ0, Γ1, and Γ2 are calculated. Under the current values, the probabilities that events A and B are determined to be possible are as follows:

[0046]

[0047] Where Pl(A) and Pl(B) are the probabilities that events A and B are determined to be possible, respectively;

[0048] The actual ground contact state of the hydraulic leg is determined by the magnitude between the probability of the event corresponding to each joint torque characteristic value being determined as having occurred and the probability of being determined as possibly occurring, and the probability threshold.

[0049]

[0050] Where, p ct The set probability threshold.

[0051] Furthermore, the least squares method is used to fuse the three equivalent joint torque differences in each difference vector, specifically as follows:

[0052]

[0053] Among them, Λ i Let Γ denote a diagonal matrix, diag() denotes the diagonal matrix function, T denotes transpose, and Γ denotes the Γ function. i The characteristic value of joint torque, Δτ iLet Γ0, Γ1, Γ2 represent the difference vector, i = 0, 1, 2, where Γ0, Γ1, Γ2 represent the joint torque characteristic values ​​corresponding to the first, second, and third data sources, respectively, and k i0 ,k i1 ,k i2 This represents the difference between three equivalent joint torques in the difference vector corresponding to different data sources.

[0054] Beneficial effects:

[0055] 1. This invention provides a ground contact detection method for hydraulic quadruped robots based on evidence theory. After obtaining the joint torque characteristic values ​​of the joints using three different data sources, the method then fuses the three joint torque characteristic values ​​using evidence theory. Compared with using force sensors or displacement sensors alone, this invention integrates the data information from force sensors and displacement sensors, resulting in higher ground contact detection accuracy, better real-time performance, and significantly improved robustness of ground contact detection judgment. This lays a good foundation for gait planning and control of hydraulic quadruped robots.

[0056] 2. This invention provides a method for detecting the ground contact of a hydraulic quadruped robot based on evidence theory. It uses force sensors and displacement sensors fixed to the hydraulic cylinder to detect the force and displacement of the hydraulic cylinder. It obtains the equivalent joint torque of the hydraulic quadruped robot before and after ground contact through dynamic and kinematic transformation relationship, and then judges the ground contact state of the robot based on the equivalent joint torque. This method can reduce the structural design difficulty of the foot end of the hydraulic quadruped robot.

[0057] 3. This invention provides a ground contact detection method for hydraulic quadruped robots based on evidence theory. By using the least squares method, the method integrates the difference in joint torques of different joints of the hydraulic quadruped robot before and after ground contact obtained from a single data source to obtain ground contact feature information from the same source, which can improve the accuracy of ground contact detection. Attached Figure Description

[0058] Figure 1 A flowchart of a method for detecting ground contact in a hydraulic quadruped robot based on evidence theory, provided by the present invention;

[0059] Figure 2 This is a front view of the leg structure of the hydraulic quadruped robot of the present invention;

[0060] Figure 3 This is a right view of the leg structure of the hydraulic quadruped robot of the present invention;

[0061] Figure 4 This is a block diagram of the ground contact detection algorithm for a hydraulic quadruped robot based on evidence theory according to the present invention. Detailed Implementation

[0062] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings.

[0063] like Figure 1 As shown, a method for detecting ground contact in a hydraulic quadruped robot based on evidence theory is characterized by the following steps:

[0064] S1: The equivalent joint torque difference of the three joints of the hydraulic leg before and after contact with the ground is obtained by using three different data sources, and the difference vectors corresponding to the different data sources are obtained. The three data sources are: using only the data of the force sensor fixed to the hydraulic leg as the data source, using only the data of the displacement sensor fixed to the hydraulic leg as the data source, and using both the data of the displacement sensor and the data of the force sensor as the data source; the three joints are the hip joint, hip joint, and knee joint.

[0065] In other words, this invention first calculates the joint torque data obtained from force sensor data, then subtracts the joint torque before and after ground contact to extract the joint torque change characteristics before and after ground contact; then, it calculates the joint torque data obtained from displacement sensor data, then subtracts the joint torque before and after ground contact to extract the joint torque change characteristics before and after ground contact; finally, it combines the data from position sensor and force sensor to extract the joint torque change characteristics before and after ground contact. It should be noted that, since the data output by the force sensor is significantly lagging behind that of the displacement sensor, at the instant of ground contact, the joint torque calculated from the force sensor data is the joint torque at the instant before ground contact, while the joint torque calculated from the displacement sensor data is the joint torque after ground contact. Therefore, the joint torque obtained from the force sensor and displacement sensor can be subtracted to extract the joint torque change characteristics before and after ground contact.

[0066] S2: The least squares method is used to fuse the three equivalent joint torque differences in each difference vector to obtain the joint torque feature values ​​corresponding to the three difference vectors. When the joint torque feature values ​​take different values, the three independent probability events "not touching the ground", "touching the ground", and "uncertain touching the ground or not touching the ground" have different probabilities.

[0067] In step S1, based on the information from the force sensor and displacement sensor, three sets of joint torque change characteristic information before and after ground contact have been obtained. Each set of difference vectors reflecting joint torque change characteristics contains three components, which are the equivalent joint torque differences of the three joints of a single hydraulic leg of the hydraulic quadruped robot before and after ground contact. The three components in each difference vector are mutually sourced characteristic information, while the three difference vectors are mutually sourced characteristic information. For the mutually sourced characteristic information, this invention uses the least squares method for data fusion processing. The fusion processing formula is shown below, where Δτ iThis refers to the three-dimensional difference vector obtained in step S1, which reflects the characteristics of joint torque changes; in the formula, k i0 ,k i1 ,k i2 .

[0068]

[0069] Among them, Λ i Let Γ denote a diagonal matrix, diag() denotes the diagonal matrix function, T denotes transpose, and Γ denotes the Γ function. i The characteristic value of joint torque, Δτ i Let Γ0, Γ1, Γ2 represent the difference vector, i = 0, 1, 2, where Γ0, Γ1, Γ2 represent the joint torque characteristic values ​​corresponding to the first, second, and third data sources, respectively, and k i0 ,k i1 ,k i2 This represents the difference between three equivalent joint torques in the difference vector corresponding to different data sources. The parameters need to be adjusted according to the actual situation.

[0070] It should be noted that the torque differences of the three joints of a single hydraulic leg obtained from the same data source are mutually sourced feature information. The three joint torque differences are fused using the least squares method. The three data sources ultimately yield three fused joint torque feature values. However, the differences between the three fused joint torque feature values ​​are mutually sourced feature information. Evidence theory is needed to fuse the mutually sourced feature information. See step S3 for details.

[0071] S3: Based on the principle of evidence theory, the three joint torque feature values ​​are integrated to obtain the probability that the probability event corresponding to each joint torque feature value is judged to have occurred and the probability that it is judged to be possible. Based on the magnitude between the two probabilities and the probability threshold, the actual ground contact state of the hydraulic leg is determined.

[0072] like Figure 2 and Figure 3 As shown, the hydraulic leg device of the hydraulic quadruped robot consists of a hip joint hydraulic cylinder, a thigh hydraulic cylinder, a calf hydraulic cylinder, force sensors and displacement sensors fixed to the hydraulic cylinders, alloy structural components, etc. Among them, the force sensors and displacement sensors fixed to the hydraulic cylinders detect the force and displacement changes of the three hydraulic cylinders in real time, and input the sensor data into the controller through a digital-to-analog converter. The controller runs a ground contact detection algorithm based on evidence theory to determine the ground contact state of the hydraulic leg.

[0073] like Figure 4 As shown below, we will explain in detail how to use three different data sources to obtain the equivalent joint torque difference of the three joints of the hydraulic leg before and after they touch the ground, and then obtain the difference vector corresponding to different data sources.

[0074] The first data source, which uses only force sensor data to obtain the equivalent joint torque difference between the three joints of the hydraulic leg before and after contact with the ground, is as follows:

[0075] Data from the force sensor is acquired in real time, and the equivalent torque on the joint is calculated in real time based on the acquired data as follows:

[0076]

[0077] The left side of the equation represents three equivalent joint torque column vectors, while the right side consists of a first matrix representing the dynamic transformation matrix and a second matrix representing the force sensor data. Specifically, τ f0 τ is the equivalent torque acting on the hip joint. f1 τ is the equivalent torque acting on the hip joint. f2 d is the equivalent torque acting on the knee joint. 00 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 11 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 21 It is the distance from the hydraulic cylinder installed in the knee joint to the knee joint, F s0 It is the reading of the hydraulic cylinder force sensor installed on the hip joint, F s1 It is the reading of the hydraulic cylinder force sensor installed on the hip joint, F s2 It is the reading from the hydraulic cylinder force sensor installed in the knee joint. It refers to the rotation angle of the hip joint. It refers to the rotation angle of the hip joint. It refers to the knee joint rotation angle; that is, the present invention utilizes dynamic transformation relationships to transform the forces on the three force sensors into the torque on the joint.

[0078] The calculation method for the rotation angles of the three joints is as follows:

[0079]

[0080] Among them, L c0 It is the length of the hydraulic cylinder when it is fully retracted, d c0 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c1 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c2 It refers to the extension and retraction of the hydraulic cylinder installed in the knee joint, d 01 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 10 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 20 It is the distance from the fixing point of the hydraulic cylinder installed at the knee joint to the knee joint;

[0081] Based on the principle that the data from the force sensor will change abruptly after the hydraulic leg touches the ground, the equivalent torque of the three joints when the hydraulic leg touches the ground is extracted from the equivalent torque of the joints calculated in real time. The equivalent torque of the three joints before and after touching the ground is subtracted to obtain the equivalent joint torque difference vector of the three joints of the hydraulic leg before and after touching the ground when only the data from the force sensor is used as the data source.

[0082] The second data source, which uses only displacement sensor data to obtain the equivalent joint torque difference between the three joints of the hydraulic leg before and after contact with the ground, is as follows:

[0083] By establishing a mapping relationship from the hydraulic cylinder motion space to the joint space, the displacement of the hydraulic cylinder is converted into the angle of joint rotation in real time. Then, the angle of joint rotation is obtained by first-order difference and second-order difference to obtain the angular velocity and angular acceleration of joint rotation.

[0084] By real-time inputting the joint rotation angle, angular velocity, and angular acceleration into the Lagrange dynamics equations, the real-time equivalent joint torque is obtained as follows:

[0085]

[0086] in,

[0087] θ = [θ0, θ1, θ2] T , τ=[τ f0 ,τ f1 ,τ f2 ] T

[0088]

[0089] In this equation, the left side represents the rotation angles of the three joints of the hydraulic leg, and the first column on the right side is based on... Figure 1 The hydraulic leg structure utilizes the law of cosines to establish a mapping matrix from the hydraulic cylinder's motion space to the joint space. The second column on the right side of the equation represents the manually set zero points of the hydraulic leg joints. Specifically, τ is the column vector of equivalent moment forces acting on the three joints, and M(θ) is the mass matrix. G(θ) is the Coriolis matrix, G(θ) is the gravity matrix, θ0, θ1, and θ2 are the rotation angles of the hip, hip, and knee joints, respectively, and θ, These are the column vectors of rotational angle, rotational angular velocity, and rotational angular acceleration of the three joints, respectively, τ. f0 τ f1 τ f2 The equivalent torques acting on the hip, hip, and knee joints are respectively, d 00 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 10 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint.20 It is the distance d from the fixing point of the hydraulic cylinder installed at the knee joint to the knee joint. 01 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 11 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 21 It is the distance L from the hydraulic cylinder installed in the knee joint to the knee joint. c0 It is the length of the hydraulic cylinder when it is fully retracted, d c0 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c1 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c2 It is the extension and retraction of the hydraulic cylinder installed in the knee joint, θ c0 θ is the initial angle of the hip joint. c1 θ is the initial angle of the hip joint. c2 This is the initial angle of the knee joint, and T represents transposition;

[0090] Based on the principle that the data from the displacement sensor will change abruptly after the hydraulic leg touches the ground, the equivalent torque of the three joints when the hydraulic leg touches the ground is extracted from the equivalent torque of the joints calculated in real time. The equivalent torque of the three joints before and after touching the ground is subtracted to obtain the equivalent joint torque difference vector of the three joints of the hydraulic leg before and after touching the ground when only the data from the displacement sensor is used as the data source.

[0091] The third data source requires clarification. It's important to note that the equivalent joint torque calculated using force sensors exhibits a lag compared to that calculated using displacement sensors. This lag is inherent to the nature of force sensors. If the hydraulic leg is not in contact with the ground, the difference between the equivalent joint torque calculated using force sensors and displacement sensors is very small. However, if the hydraulic leg is in contact with the ground, the equivalent joint torque calculated by the force sensors reflects the torque before contact, while the equivalent joint torque calculated by the displacement sensors reflects the torque after contact. The difference between these two torques is substantial and can be used as a ground contact characteristic. Therefore, this invention employs a third data source: simultaneously using data from both displacement and force sensors to obtain the equivalent joint torque differences of the three joints of the hydraulic leg before and after contact with the ground. The specific method is as follows:

[0092] Based on the principle that the data output by the force sensor lags behind the data output by the displacement sensor, when the data from the displacement sensor changes abruptly, the equivalent torque on the joint calculated from the displacement sensor data at the time of the change is taken as the equivalent torque on the joint after contact with the ground. The equivalent torque on the joint calculated from the force sensor data at the same moment of the change is taken as the equivalent torque on the joint before contact with the ground. The difference between the equivalent torque on the joint after contact with the ground obtained from the displacement sensor data and the equivalent torque on the joint before contact with the ground obtained from the force sensor data is calculated to obtain the equivalent joint torque difference vector of the three joints of the hydraulic leg before and after contact with the ground when both displacement sensor data and force sensor data are used as data sources.

[0093] As mentioned above, by fusing information from the same source, the difference vectors obtained by each method are merged into a quantity representing the change in ground impact force, i.e., Γ. i By fusing the three difference vectors, three quantities Γ0, Γ1, and Γ2 representing the change in ground impact force can be obtained. These three quantities are heterogeneous feature information. This invention uses evidence theory to fuse heterogeneous feature information, thereby detecting and determining the ground impact state. The specific steps are as follows:

[0094] a. Probability event division. The probability event of ground contact detection is divided into three independent probability events: "no ground contact", "ground contact", and "uncertain ground contact or no ground contact", which is called the basic probability event set.

[0095]

[0096] b. Basic Probability Assignment. For the three quantities Γ0, Γ1, and Γ2 obtained in step four, representing the change in impact force, assign corresponding probabilities to the three probabilities of "no impact," "impact," and "uncertain impact or no impact." The probability assignment table used in this invention is shown in Table 1, where Γ... i Γ0, Γ1, and Γ2 can be chosen; for Γ i Different values ​​of the segmentation value τ g1 ,τ g2 ,τ g3 ,τ g4 The values ​​of are also different, and the parameter size needs to be determined based on experiments.

[0097] Table 1 Reference probability allocation table used in this invention

[0098]

[0099] As shown in Table 1, Γ i Different values ​​of Γ correspond to different probabilities of events A, B, and C. For example, Γ i Greater than 0 and less than τ g1When the event A occurs, the reference probability is 1, the reference probability of event B occurs is 0, and the reference probability of event C occurs is 0.

[0100] c. Calculate the normalization constant. Following the principles of evidence theory, calculate the normalization constant K, excluding cases where the probability of a ground contact is 1 and the probability of a non-ground contact is 0 simultaneously.

[0101]

[0102] in, E * Let m1(A), m1(B), and m1(C) represent any event in the set of basic probability events. m1(A), m1(B), and m1(C) are the reference probabilities of events A, B, and C corresponding to the joint torque feature value Γ0 of the first data source under the current value. m2(A), m2(B), and m2(C) are the reference probabilities of events A, B, and C corresponding to the joint torque feature value Γ1 of the second data source under the current value. m3(A), m3(B), and m3(C) are the reference probabilities of events A, B, and C corresponding to the joint torque feature value Γ2 of the third data source under the current value.

[0103] It should be noted that the normalization coefficient obtained from the above formula represents the probability that all three ground contact feature signals of the hydraulic leg are judged as ground contact or all are judged as non-ground contact, and the probability of ground contact feature signals obtained by different methods being judged as both ground contact and non-ground contact is removed.

[0104] d. Calculate the combined probability function of the three probability events "not touching the ground", "touching the ground", and "uncertain whether it touches the ground or not" based on the normalization constant K. The combined probability P(A) of event A not touching the ground is:

[0105]

[0106] The probability of the combination that hits the ground is P(B):

[0107]

[0108] The probability of a combination that is uncertain whether it will touch the ground is P(C):

[0109]

[0110] e. By combining probability functions, calculate the confidence function and likelihood function corresponding to the basic probability events. The confidence function represents the probability of the confirmed event occurring, and its calculation formula is:

[0111]

[0112] Here, Bel(A), Bel(B), and Bel(C) are the trust functions, which are the probabilities that events A, B, and C are determined to have occurred.

[0113] The likelihood function is the probability that an event may occur, and its formula is as follows:

[0114]

[0115] Here, Pl(A) and Pl(B) are the likelihood functions, which are the probabilities that events A and B are determined to be possible, respectively. It should be noted that it is meaningless to discuss the probability that uncertain events of touching the ground or not touching the ground may occur, so the likelihood function of event C is not calculated.

[0116] f. By comparing the relationship between the confidence function and likelihood function of the probability event and the set probability threshold, the ground contact state is determined. Ground contact determination is used to switch the phase state of the legs of the hydraulic quadruped robot in the control program. Timely switching can reduce the impact of leg ground contact on the stability of the robot body. Therefore, when determining ground contact, a higher probability of ground contact should be assumed, and then an adjustment strategy should be added when a false ground contact is detected. In view of this, the ground contact determination method adopted in this invention can be expressed as the following formula:

[0117]

[0118] Where, p ct The threshold for the probability of touching the ground and not touching the ground needs to be determined experimentally; for Bel(B) < p ct And Pl(B) > p ct In cases where additional strategies are needed to address misjudgments, this invention will not elaborate on these aspects.

[0119] By continuously reading data from force and displacement sensors and fusing homogeneous and heterogeneous feature information, the ground contact status of the hydraulic leg can be detected and determined in real time.

[0120] In summary, this invention utilizes the characteristics of force sensors and displacement sensors to calculate the equivalent joint torques of a hydraulic quadruped robot before and after it touches the ground. Then, using the least squares method, it fuses the differences in joint torques of different joints of the hydraulic quadruped robot before and after touching the ground obtained from a single data source to obtain homogeneous ground contact feature information. Finally, based on the principle of evidence theory, it fuses ground contact feature information obtained from three different data sources to obtain the probability of foot contact, and compares it with the ground contact threshold probability to determine the ground contact state of the robot's foot.

[0121] Therefore, firstly, compared to the direct measurement method of installing pressure sensors directly on the feet of a hydraulic quadruped robot, this invention utilizes force and displacement sensors fixed to the hydraulic cylinder to detect the force and displacement of the hydraulic cylinder. It obtains the equivalent joint torque through dynamic and kinematic transformations, thereby determining the ground contact state, thus reducing the structural design difficulty of the hydraulic quadruped robot's feet. Secondly, compared to using force and displacement sensors alone, this invention integrates the data from both sensors, resulting in higher accuracy and better real-time performance in ground contact detection. Finally, this invention employs evidence theory to integrate the information from force and displacement sensors, significantly improving the robustness of ground contact detection and laying a solid foundation for gait planning and control of hydraulic quadruped robots.

[0122] Of course, the present invention may have other various embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art can make various corresponding changes and modifications according to the present invention, but these corresponding changes and modifications should all fall within the protection scope of the appended claims.

Claims

1. A method for detecting ground contact in a hydraulic quadruped robot based on evidence theory, characterized in that, Includes the following steps: S1: The equivalent joint torque difference of the three joints of the hydraulic leg before and after contact with the ground is obtained by using three different data sources, and the difference vectors corresponding to the different data sources are obtained. The three data sources are: using only the data of the force sensor fixed to the hydraulic leg as the data source, using only the data of the displacement sensor fixed to the hydraulic leg as the data source, and using both the data of the displacement sensor and the data of the force sensor as the data source; the three joints are the hip joint, hip joint, and knee joint. S2: The least squares method is used to fuse the three equivalent joint torque differences in each difference vector to obtain the joint torque feature values ​​corresponding to the three difference vectors. When the joint torque feature values ​​take different values, the three independent probability events "not touching the ground", "touching the ground", and "uncertain touching the ground or not touching the ground" have different probabilities. S3: Based on the principle of evidence theory, the three joint torque feature values ​​are integrated to obtain the probability that the probability event corresponding to each joint torque feature value is judged to have occurred and the probability that it is judged to be possible. Based on the magnitude between the two probabilities and the probability threshold, the actual ground contact state of the hydraulic leg is determined.

2. The method for detecting ground contact of a hydraulic quadruped robot based on evidence theory as described in claim 1, characterized in that, The method for obtaining the equivalent joint torque difference before and after the three joints of the hydraulic leg touch the ground using only force sensor data as the data source is as follows: Data from the force sensor is acquired in real time, and the equivalent torque on the joint is calculated in real time based on the acquired data as follows: Where, τ f0 τ is the equivalent torque acting on the hip joint. f1 τ is the equivalent torque acting on the hip joint. f2 d is the equivalent torque acting on the knee joint. 00 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 11 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 21 It is the distance from the hydraulic cylinder installed in the knee joint to the knee joint, F s0 It is the reading of the hydraulic cylinder force sensor installed on the hip joint, F s1 It is the reading of the hydraulic cylinder force sensor installed on the hip joint, F s2 It is the reading from the hydraulic cylinder force sensor installed in the knee joint. It refers to the rotation angle of the hip joint. It refers to the rotation angle of the hip joint. It refers to the knee joint rotation angle; The calculation method for the rotation angles of the three joints is as follows: Among them, L c0 It is the length of the hydraulic cylinder when it is fully retracted, d c0 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c1 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c2 It refers to the extension and retraction of the hydraulic cylinder installed in the knee joint, d 01 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 10 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 20 It is the distance from the fixing point of the hydraulic cylinder installed at the knee joint to the knee joint; Based on the principle that the data from the force sensor will change abruptly after the hydraulic leg touches the ground, the equivalent torque of the three joints when the hydraulic leg touches the ground is extracted from the equivalent torque of the joints calculated in real time. The equivalent torque of the three joints before and after touching the ground is subtracted to obtain the equivalent joint torque difference vector of the three joints of the hydraulic leg before and after touching the ground when only the data from the force sensor is used as the data source.

3. The method for detecting ground contact of a hydraulic quadruped robot based on evidence theory as described in claim 1, characterized in that, The method for obtaining the equivalent joint torque difference before and after the three joints of the hydraulic leg touch the ground using only displacement sensor data as the data source is as follows: By establishing a mapping relationship from the hydraulic cylinder motion space to the joint space, the displacement of the hydraulic cylinder is converted into the angle of joint rotation in real time. Then, the angle of joint rotation is obtained by first-order difference and second-order difference to obtain the angular velocity and angular acceleration of joint rotation. By real-time inputting the joint rotation angle, angular velocity, and angular acceleration into the Lagrange dynamics equations, the real-time equivalent joint torque is obtained as follows: in, θ=[θ0,θ1,θ2] T ,τ=[τ f0 ,t f1 ,t f2 ] T Where τ is the column vector of equivalent moment forces acting on the three joints, and M(θ) is the mass matrix. G(θ) is the Coriolis matrix, G(θ) is the gravity matrix, θ0, θ1, and θ2 are the rotation angles of the hip, hip, and knee joints, respectively, and θ, These are the column vectors of rotational angle, rotational angular velocity, and rotational angular acceleration of the three joints, respectively, τ. f0 τ f1 τ f2 The equivalent torques acting on the hip, hip, and knee joints are respectively, d 00 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 10 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 20 It is the distance d from the fixing point of the hydraulic cylinder installed at the knee joint to the knee joint. 01 It is the distance d from the fixing point of the hydraulic cylinder installed at the hip joint to the hip joint. 11 It is the distance d from the output end of the hydraulic cylinder installed at the hip joint to the hip joint. 21 It is the distance L from the hydraulic cylinder installed in the knee joint to the knee joint. c0 It is the length of the hydraulic cylinder when it is fully retracted, d c0 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c1 It refers to the extension and retraction of the hydraulic cylinder installed at the hip joint, d c2 It is the extension and retraction of the hydraulic cylinder installed in the knee joint, θ c0 θ is the initial angle of the hip joint. c1 θ is the initial angle of the hip joint. c2 This is the initial angle of the knee joint, and T represents transposition; Based on the principle that the data from the displacement sensor will change abruptly after the hydraulic leg touches the ground, the equivalent torque of the three joints when the hydraulic leg touches the ground is extracted from the equivalent torque of the joints calculated in real time. The equivalent torque of the three joints before and after touching the ground is subtracted to obtain the equivalent joint torque difference vector of the three joints of the hydraulic leg before and after touching the ground when only the data from the displacement sensor is used as the data source.

4. The method for detecting ground contact of a hydraulic quadruped robot based on evidence theory as described in claim 1, characterized in that, The method for obtaining the equivalent joint torque difference before and after the three joints of the hydraulic leg touch the ground, using data from both displacement sensors and force sensors as data sources, is as follows: Based on the principle that the data output by the force sensor lags behind the data output by the displacement sensor, when the data from the displacement sensor changes abruptly, the equivalent torque on the joint calculated from the displacement sensor data at the time of the change is taken as the equivalent torque on the joint after contact with the ground. The equivalent torque on the joint calculated from the force sensor data at the same moment of the change is taken as the equivalent torque on the joint before contact with the ground. The difference between the equivalent torque on the joint after contact with the ground obtained from the displacement sensor data and the equivalent torque on the joint before contact with the ground obtained from the force sensor data is calculated to obtain the equivalent joint torque difference vector of the three joints of the hydraulic leg before and after contact with the ground when both displacement sensor data and force sensor data are used as data sources.

5. The method for detecting ground contact of a hydraulic quadruped robot based on evidence theory as described in claim 1, characterized in that, The methods for calculating the probability of an event corresponding to each joint moment characteristic value being determined as having occurred and the probability of it being determined as being possible are as follows: The joint torque characteristic values ​​corresponding to the first, second, and third data sources are represented as Γ0, Γ1, and Γ2, respectively. The probability events "not touching the ground" are denoted as event A, "touching the ground" as event B, and "uncertainty about touching the ground or not touching the ground" as event C. According to the set segmentation value, the possible range of joint torque characteristic values ​​Γ0, Γ1, Γ2 is divided into more than three intervals, and reference probabilities corresponding to events A, B, and C are assigned to each interval. According to the principles of evidence theory, the normalization constant K is calculated based on the reference probability: K=[m1(A)+m1(C)][m2(A)+m2(C)][m3(A)+m3(C)]+[m1(B)+m1(C)][m2(B)+m2(C)][m3(B)+m3(C)]-m1(C)m2(C)m3(C) Wherein, m1(A), m1(B), and m1(C) are the reference probabilities of events A, B, and C corresponding to the joint torque feature value Γ0 of the first data source under the current value; m2(A), m2(B), and m2(C) are the reference probabilities of events A, B, and C corresponding to the joint torque feature value Γ1 of the second data source under the current value; and m3(A), m3(B), and m3(C) are the reference probabilities of events A, B, and C corresponding to the joint torque feature value Γ2 of the third data source under the current value. Calculate the combined probability P(A) of event A based on the normalization constant K and the reference probability: The combined probability P(B) of event B is calculated based on the normalization constant K and the reference probability: The combined probability P(C) of event C is calculated based on the normalization constant K and the reference probability: Based on the combined probabilities P(A), P(B), and P(C), the characteristic values ​​of joint torque Γ0, Γ1, and Γ2 are calculated. Under the current values, the probabilities that events A, B, and C are confirmed to have occurred are as follows: Where Bel(A), Bel(B), and Bel(C) are the probabilities that events A, B, and C are confirmed to have occurred, respectively. Based on the combined probabilities P(A), P(B), and P(C), the characteristic values ​​of joint torque Γ0, Γ1, and Γ2 are calculated. Under the current values, the probabilities that events A and B are determined to be possible are as follows: Where Pl(A) and Pl(B) are the probabilities that events A and B are determined to be possible, respectively; The actual ground contact state of the hydraulic leg is determined by the magnitude between the probability of the event corresponding to each joint torque characteristic value being determined as having occurred and the probability of being determined as possibly occurring, and the probability threshold. Where, p ct The set probability threshold.

6. A method for detecting ground contact of a hydraulic quadruped robot based on evidence theory, as described in any one of claims 1 to 5, characterized in that, The least squares method is used to fuse the three equivalent joint moment differences in each difference vector, specifically as follows: Among them, Λ i Let Γ denote a diagonal matrix, diag() denotes the diagonal matrix function, T denotes transpose, and Γ denotes the Γ function. i The characteristic value of joint torque, Δτ i Let Γ0, Γ1, Γ2 represent the difference vector, i = 0, 1, 2, where Γ0, Γ1, Γ2 represent the joint torque characteristic values ​​corresponding to the first, second, and third data sources, respectively, and k i0 ,k i1 ,k i2 This represents the difference between three equivalent joint torques in the difference vector corresponding to different data sources.

Images