A novel kinematics and statics correction algorithm of semi-cylindrical foot end structure for 3-DOF LHDS of legged robots

In this paper, aiming at the problem that the ideal modeling point of the foot end deviates from the actual contact point due to the rolling effect of the semi-cylindrical foot end in leg hydraulics drive system (LHDS), a novel and relatively simple kinematics and statics correction algorithm is proposed. This algorithm can effectively improve the accuracy of LHDS kinematics and statics, and it is still applicable when the body and the contact surface are at different angles. Firstly, this paper deduces the kinematics and statics when the foot end of LHDS is regarded as the point foot, analyzes and calculates the deviation under common working conditions. Secondly, in view of this phenomenon, a kinematics correction algorithm based on virtual DOF and a statics correction algorithm based on translation theorem of forces are proposed respectively. Finally, the proposed algorithm is verified by LHDS performance test platform under various working conditions. The results show that after applying the kinematics correction algorithm, the trajectory deviation reduction rate of the hip joint is over 65%; after applying the statics correction algorithm, the deviation reduction rate of foot end contact force is over 43%. The related research results can be applied to any similar foot end, which has engineering application value.


Introduction
Robot can walk in a variety of ways. Up to now, the movement forms can be roughly divided into wheeled [1], tracked [2], wheel-foot compound [3], snake-like [4] and bionic leg [5], etc. Compared with other types of robots, bionic legged robot adopts the similar structure with the creatures which have legs, so, it has the characteristics of discontinuous support. Especially when combined with hydraulic drive which has a high power to weight ratio, it not only adapts well to unknown, unstructured environments, but also can pass through the barrier. Therefore, this type of robot is particularly suitable to use in the complex environment in the field [6]. In order to improve the adaptability of legged robot to complex environment, the foot end is generally designed as semi-cylindrical structure [7,8]. However, in the process of robot walking, the semi-cylindrical foot end will lead to the real-time change of the contact point between the foot end and ground, which will lead to a certain deviation in the kinematics based on D-H coordinate transformation and the statics based on virtual work principle. This will cause the robot to produce the wrong posture and the wrong calculated force, thus affecting the movement control of the whole robot.
Regarding the phenomenon that the robot has wrong posture in the process of movement, Guardabrazo et al. [9] quantitatively analyzed the influence of the hemispherical foot end on the hip joint trajectory of Silo6 walking robot, aiming at the problem of hip joint trajectory deviation. Meanwhile, the kinematics was modified by adding nonlinear equations, and the validity of the proposed method was verified by using a two-dimensional insect-like leg test platform. Based on the inverse velocity kinematics of the legged robot, Chen Gang [10] divided the position-posture closed-loop control into position closed-loop control and posture closedloop control to realize the precise position-posture control of the robot. Through co-simulation, the validity of using the closed-loop control method based on position-posture to solve the position-posture deviation problem was verified. Chen Weihai et al. [11] proposed a method to correct the trajectory of robot foot end through force feedback information, and verified the validity of this method by testing hexapod robots. Zhu Yaguang et al. [12] proposed a semi-cylindrical foot end trajectory correction algorithm based on LS-SVM, and verified the validity of the method on a hexapod robot prototype. To solve the problem of robot motion parameter deviation, Lin Peng et al. [13] firstly established the kinematics model of the mobile robot, and then estimated the motion parameters by using the multiple linear regression mathematical model based on least squares. Finally, the validity of the proposed method was verified by the omnidirectional robot. Li Jing et al. [14] proposed a numerical method for solving the inverse kinematics of a six degree of freedoms (6-DOF) robot with a non-spherical wrist. By compensating the end position deviation between the actual robot end posture and the target posture calculated by the simplified inverse kinematics solution, the inverse solution of the simplified robot gradually approached the expected solution. Finally, the validity of the proposed algorithm was verified by a robot prototype.
Regarding the phenomenon that the robot has the wrong calculated force in the process of movement, Hawley Louis et al. [15] proposed an external force observer construction method based on Kalman filtering equation by using the force sensing resistance (FSR) under the foot of humanoid robot and the robot inertial measurement unit (IMU), which could keep the deviation range of external force estimation within a reasonable range. Kresimir Turkovic et al. [16] proposed an end-force estimation method based on the manipulator dynamics model, Jacobian matrix and joint torques, which assumed that there was a connection between the forces acting on the end-effector and the torques of the manipulator joints. The validity of the method was verified by the manipulator arm experiments on the aircraft. Li Dongyi et al. [17] proposed a calculation method of equivalent Jacobian inverse matrix, aiming at the phenomenon that the traditional virtual work principle in 3-RPS parallel robot could not solve the driving force because the Jacobian matrix was not full rank matrix, and the validity of the method was verified by simulation model.
In the above control algorithm of robot body posture deviation compensation and foot end force deviation compensation, although the adjustment of robot body posture and foot end force deviation is realized, most of the algorithms are too complex and do not take into account the situation that body and contact surface are at different angles. However, the situation that body and contact surface are at different angles always occurs in the actual movement process of the robot.
Based on the above situation, this paper mainly carries out the following research work: The kinematics and statics correction algorithm proposed in this paper is based on the control algorithm as shown in Fig. 1. As can be seen from Fig. 1, the implementation of the control algorithm requires the following two steps: Step 1 is environmental parameters identification algorithm for the whole leg. This step is mainly used to solve the equivalent angle ϑ between the robot body and the contact surface.
Step 2 is kinematics and statics correction algorithm. This step is the core research content of this paper and the main contribution of this paper. As long as the equivalent angle ϑ between the robot body and the contact surface can be accurately identified, the control algorithm proposed in this paper can effectively reduce the calculation deviation of kinematics and statics, thus improve the motion control precision of the whole robot. Regarding Step 1, environmental parameter identification has been extensively studied in the field of robot control, and relevant algorithms have been relatively mature. For example, methods such as foot end six-dimensional force sensor, robot body IMU [18], machine vision [19] and lidar [20] can be used for environmental identification. The identification of complex environment, such as beach, jungle, snow and grass, is also the difficulty and hotspot of current research.
Regarding Step 2, the control algorithm shown in Fig. 1 is to set the equivalent angle ϑ identified in Step 1 as the input quantity, and on this basis, to correct the kinematics and statics algorithms, effectively making up for the fact that the existing research correction algorithms in this field seldom take into account the different angles (take ϑ as different equivalent values) between the robot body and the ground.
Based on the above ideas, the organizations of the paper are organized as follows: "Sampling system" describes the sampling system in this paper, namely the composition of the 3-DOF leg hydraulic drive system (LHDS). In "Mathematical modeling of kinematics and statics of LHDS", the kinematics of leg based on D-H coordinate transformation and the statics calculation method based on virtual work principle are derived. "Deviation analysis of kinematics and statics" analyzes the reasons for the calculation deviation caused by the rolling effect of the semi-cylindrical foot end, and calculates the kinematics and statics deviation respectively under common working conditions. In "Kinematics and statics correction algorithm", the semi-cylindrical foot end is treated as a rod perpendicular to the ground, and a kinematics correction algorithm based on virtual DOF is proposed. The actual contact point between the foot end and the ground is translated to the ideal modeling point, and a statics correction method based on the translation theorem of forces is proposed. The statics correction algorithm is the main contribution of this paper, and kinematics correction algorithm is the supplement of sampling system 2 in Ref. [21]. In addition, these two algorithms are general and can be extended to different leg structure with similar foot end. In "Experiment", the validity of the proposed kinematics and statics correction algorithms are verified experimentally by using the LHDS performance test platform under different working conditions.

Sampling system
The study content of this paper is a sampling system based on 3-DOF LHDS, and its real model and three-dimensional model are shown in Fig. 2.
LHDS is mainly composed of the following five parts: Base (For connecting leg with aluminum alloy guide rail), hip joint, knee joint, ankle joint, and three sets of hydraulic drive unit (HDU). The LHDS can move up and down along aluminum alloy support through guide rail.
As can be seen from Fig. 2b, LHDS contains a total of 3-DOF, which are represented by O, E and G successively.  The HDU of the leg is a highly integrated valve-controlled cylinder independently designed and developed by the authors' team, as shown in Fig. 3. It is mainly composed of valve block, servo cylinder, servo valve, position sensor and one-dimensional force sensor. The one-dimensional force sensor is installed on the end of the piston rod of the servo cylinder, and the position sensor is installed on the side of the servo cylinder, which is used for the force closed-loop control and position closed-loop control of the HDU. Servo valve and valve block are installed on the upper part of the servo cylinder. The valve-controlled cylinder has the advantages of compact structure and large power-to-weight ratio, which reduces the connection pipeline between the servo valve and the servo cylinder. Besides, it can effectively reduce the probability of leakage and pipe joint damage and improve its dynamic performance.

Kinematics modeling
The forward kinematics solution is to use the known angles of each joint to solve the position of the foot end relative to the hip joint. D-H coordinate transformation is a commonly used kinematics modeling method of robot. During the D-H transformation, the length of each rod should be determined. However, due to the special structure of the semi-cylindrical foot end, the length of the foot is uncertain; therefore, the line between the mid-point I of the semi-cylindrical foot end and the ankle joint G is selected as the length of the foot in this paper. Considering the complexity of the mechanical structure of the leg and for the convenience of modeling and analysis, the three-dimensional LHDS diagram in Fig. 2 is simplified to Fig. 4 in this paper.
In Fig. 4, O is the origin of the coordinates of the hip joint, x 0 is the positive direction of the horizontal axis, and y 0 is the positive direction of the vertical axis, l 1 is the OE length of thigh, l 2 is the EG length of shank, and l 3 is the GI length of foot Specify the direction of joint rotation counterclockwise to be positive. θ 1 is the included angle between thigh and the positive direction of x 0 axis, which is set as the rotation angle of hip joint. θ 2 is the included angle between EG and OE extension line, which is set as the rotation angle of knee joint. θ 3 is the included angle between EG extension line and GI, which is set as the rotation angle of ankle joint. α is the included angle between EG and ED, β is the included angle between OA and the positive direction of x 0 axis. AB, CD, and FH are the total length of the hip, knee, and ankle joint HDUs, respectively.
The main structure dimension parameters of LHDS are shown in Table 1.
In Table 2, a i−1 is the measured distance from z i−1 to z i (the z i axis is perpendicular to the x i and y i axis, the positive direction of the z i axis satisfies the Cartesian right-handed coordinate system) along In the D-H coordinate system as shown in Fig. 4, the relative position relation can be expressed by the general transformation equation i−1 i T of connecting rod.
By substituting the parameters in Table 2 into Eq. (1), the transformation matrix between adjacent connecting rods can be obtained as follows: By multiplying Eqs. (2), (3), (4), and (5) in turn, 0 4 T can be obtained as follows: Then the relative position relationship between the foot end position I X X foot , X Y foot and the hip joint O can be obtained from Eq. (6), so the forward kinematics solution is obtained as follows: The inverse kinematics solution is to find the angle of each joint by the position of the foot end relative to the hip joint. Because LHDS is 3-DOF and has a redundant DOF, if only the position of the foot end to the hip joint is given, it will become an indefinite solution problem because use two equations solve for three unknown numbers when analyzing the inverse kinematics. Therefore, a constraint should be added to the inverse kinematics solution of this kind of 3-DOF leg structure. There are many forms of such constraints; the constraint used in this paper is when LHDS at stance phase, making the hip joint O, ankle joint G, and foot end I collinear, as shown in Fig. 5. The above method can effectively solve the problem of indeterminate solution in inverse kinematics analysis.
In Fig. 5, the included angle θ 0 between the line where OGI is located and the positive direction of x 0 axis is as follows: In O EG, E OG is: According to Fig. 5, the rotation angle of hip joint θ 1 is as follows: In O EG, O EG is as follows: According to Fig. 5, the rotation angle of knee joint θ 2 is as follows: In O EG, O EG is as follows: According to Fig. 5, the rotation of angle ankle joint θ 3 is as follows: During LHDS actual motion control, the variable that can be directly controlled are the elongation of the hip, knee, and ankle joint HDUs. It is necessary to obtain the relation between angle of each joint and elongation of HDU. The triangular relationship of each joint is shown in Fig. 6.
The total elongation of the hip joint HDU AB, the knee joint HDU CD, and the ankle joint HDU FH respectively is as follows: where l 01 is the initial length of the hip joint HDU, l 02 is the initial length of the knee joint HDU, and l 03 is the initial length of the ankle joint HDU.
In AO B of Fig. 6a, AO B can be obtained by the law of cosines as follows: According to the geometric relation in Fig. 6a, the rotation angle θ 1 of hip joint can be obtained as follows: According to the above equation, the extension length x p1 of the hip joint is as follows: In C DE of Fig. 6b, C E D can be obtained by the law of cosines as follows: According to the geometric relation in Fig. 6b, the rotation angle θ 2 of knee joint can be obtained as follows: According to the above equation, the extension length x p2 of the knee joint is as follows: In FG H of Fig. 6c, FG H can be obtained by the law of cosines as follows: According to the geometric relation in Fig. 6c, the rotation angle θ 3 of ankle joint can be obtained as follows: According to the above equation, the extension length x p3 of the ankle joint is as follows:
(29) Fig. 8 Relationship between each joint torque and each joint HDU force The forward statics solution is to use the known torque of each joint to solve out the foot end contact force. According to Eq. (29), it is only needed to know the torque on any two joints to obtain the contact force on the foot end. In this paper, the contact force on the foot end can be obtained through the torque on the knee and ankle joints, and the forward statics solution can be obtained as follows: During LHDS actual motion control, the variables that can be directly collected are measured signals of hip, knee, and ankle joint HDUs' one-dimensional force sensors. Therefore, it is necessary to clarify the relationship between each joint torque and the measured signal of each joint HDU onedimensional force sensor. The relationship between torque of each joint and the force of each joint HDU is shown in Fig. 8.
In AO B of Fig. 8a, AB O can be obtained by using the law of cosines and Eq. (15) as follows: According to Eq. (31), the mapping relationship between the signal F s1 detected by the one-dimensional force sensor of the hip joint HDU and the torque τ 1 of the hip joint is as follows: In C DE of Fig. 8b, DC E can be obtained by using the following law of cosines: According to Eq. (33), the mapping relationship between the signal F s2 detected by the one-dimensional force sensor of the knee joint HDU and the torque τ 2 of the knee joint is as follows: In FG H of Fig. 8c, G F H can be obtained by using the following law of cosines: According to Eq. (35), the mapping relationship between the signal F s3 detected by the one-dimensional force sensor of the ankle joint HDU and the torque τ 3 of the ankle joint is as follows:

Quantitative analysis of kinematics solving deviation
When the leg of the robot in stance phase, the phenomenon of hip joint deviation caused by the rolling of the semicylindrical foot end is shown in Fig. 9. The dotted line in Fig. 9 is the position of the leg when the foot end is regarded as the point, and the solid line is the position of the leg caused by the rolling of the semi-cylindrical foot end. In Fig. 9, the hip joint trajectory deviation − − → O O ( x, y) is as follows: − → I G, and included angle ϑ between the robot body and contact surface are parallel transported to the G of the ankle joint in this paper, as shown in Fig. 10.
In Fig. 10, the included angle ϕ between − − → O r P and − − → O r I is as follows: According to Eq. (39), the hip joint trajectory deviation is not only related to the rotation angle θ 1 , θ 2 , θ 3 of hip, knee, and ankle joints, but also related to the included angle ϑ between the robot body and the contact surface.

Quantitative analysis of statics solving deviation
In the movement process of the robot, the contact force between the semi-cylindrical foot end and ground is shown in Fig. 11. In Fig. 11, F x foot and F y foot are the components in the horizontal and vertical directions of contact forces between the foot end and ground (The positive direction of F x foot is vertical upward, the positive direction of F y foot is horizontal to the right), and δ is the angle formed by − − → I O r and − → I P. From Fig. 11, under ideal circumstances, there is a rigid contact between the foot end and the ground, and the contact point is P. However, the statics modeling point based on virtual work principle is I, so it is necessary to simplify the contact point from P to I. According to the translation theorem of forces, the contact point can be directly translated to point I, but additional torque will be generated. The distance P I is as follows: In Fig. 11, the torque τ y generated by F y foot on point I is as follows: In Fig. 11, the torque τ x generated by F x foot on point I is as follows: Combined with Eq. (38) and Fig. 11, angle δ can be obtained as as follows: As can be seen from Fig. 11, the contact point P may be located to the left or right of ideal contact point I. Different positions will produce completely opposite torque effect, so it needs to be discussed in two cases. The position of contact point P can be determined by angle ϕ. When angle ϕ is greater than zero, contact point P is located to the left of ideal contact point I. When angle ϕ is less than zero, contact point P is located to the right of ideal contact point I.
When angle ϕ is greater than zero, the actual torque of each joint is as follows: When angle ϕ is less than zero, the actual torque of each joint is as follows: It can be seen from Eqs. (44) and (45) that the expression of the actual torque of each joint will be appended with a correction term. These correction terms are the root causes of deviations in the statics calculation method based on the virtual work principle. The expression of the correction terms of each joint is as follows: According to Eq. (46), it can be known that Substituting the Eqs. (46) and (47) where Also select the included angle ϑ between the robot body and the contact surface are − 10°, 0°and 10°, control the foot end of the robot body to remain stationary at the initial position. F x f oot in the x direction changes within the range of ± 500 N, and F y foot end in the y direction changes within the range of ± 1000 N. Then, the statics calculation deviation F F x , F y T caused by the semi-cylindrical foot end is shown in Fig. 12. It can be seen from Fig. 12 that the statics calculation deviation caused by the semi-cylindrical foot end exists in the direction perpendicular and parallel to the contact surface. When the included angle between the robot body and the contact surface are 10°, 0°, and 10°, the maximum absolute values of the statics calculation deviations in the positive direction of x axis are 52.89, 37.72, and 21.09 N, respectively, the maximum absolute values of the statics calculation deviations in the positive direction of y axis are 86.92, 63.48, and 36.37 N, respectively. These deviations will cause the foot end contact force detected by the sensor to deviate from the actual contact force, thus weakening the control effect of control algorithms such as impedance control and contact force control. Therefore, it is very necessary to propose a statics correction algorithm for the special structure of semicylindrical foot end.

Kinematics correction algorithm based on virtual DOF
The kinematics correction algorithm proposed in this paper is shown in Fig. 13. Considering that the semi-cylindrical foot end is always tangent to the contact surface, the method of virtual DOF is adopted in this paper. The semi-cylindrical foot end is virtualized as a rod O r P which is always perpendicular to the contact surface.

Statics correction algorithm for semi-cylindrical foot end
From Eq. (46), it can be seen that the additional correction term in the joint moment expression is the root cause of the statics calculation deviation. Therefore, the core idea of the statics correction algorithm proposed in this paper is to compensate the torque of each joint by using the additional correction term of each joint. By substituting Eqs. (44) and (45) into Eq. (29), the inverse statics solution of the robot leg with a semi-cylindrical foot end can be obtained as follows: By substituting Eq. (55) into Eq. (30), the forward statics solution of the robot leg with a semi-cylindrical foot end can be obtained as follows:

Introduction of the experimental system
The validity of the kinematics and statics correction algorithm proposed in this paper was verified experimentally by using the LHDS performance test platform. The experimental test site is shown in Fig. 14.
By controlling the load simulation experimental platform to perform impedance control in the vertical direction, the load simulation experimental platform is guaranteed to be in close contact with the foot end during the verification process of the kinematics correction algorithm, and constantly moves with the deviation of the foot end in the vertical direction. During the validation of the statics correction algorithm, the foot end position of the LHDS keeps unchanged. By changing the extension length of the load simulation experimental platform, different contact forces can be loaded on the foot end. The specific experimental principle is shown in Fig. 15.

Kinematics correction experiment
From Eq. (39), it can be concluded that if the vertical deviation of the hip joint is improved, then the horizontal displacement also can be improved. Therefore, the vertical deviation of the hip joint trajectory was used as the evaluation index to test the validity of the proposed kinematics correction algorithm. In this paper, the sinusoidal signal input from the foot end parallel to the contact surface is 150 sin(0.5t). The position sensor in the vertical movement plane of the load simulation platform detects the deviation in the vertical direction during the movement of the foot end. The specific experimental plan is shown in Table 3.
In Table 3, plans 1 and 2 aim to test the correction effect of kinematics correction algorithm when the robot body at the same angle with the contact surface and the foot end is at different positions. Plans 2 and 3 aim to test the correction effect of kinematics correction algorithm when the robot body at the different angles with the contact surface and the foot end at the same position.
The relationship between theoretical deviation, uncorrected deviation, corrected deviation of hip joint trajectory is, respectively, shown in Figs. 16, 17, 18 [21]. The theoretical deviation curves are the deviation in the positive direction of the y 0 axis of the hip joint obtained by inserting the real-time extension length of each hydraulic driving unit into Eq. (39). The uncorrected deviation curves are the deviation in the positive direction of the y 0 axis of the hip joint obtained by inverse kinematics solution when the foot end is regarded as a point. The corrected deviation curves are the deviation in the positive direction of the y 0 axis of the hip joint obtained by using the kinematics correction algorithm proposed in this paper.
In Figs. 16,17,18, it can be seen that the theoretical deviation of the hip joint trajectory and the uncorrected deviation almost coincide, indicating that the theoretical calculation  In plans 1 to 3, the trajectory deviation of the hip joint is greatly reduced by adding the correction algorithm proposed in this paper, which indicates that the kinematics algorithm proposed in this paper is applicable when the foot end is located at different positions and the robot body is at different angles to the contact surface. However, it can be seen from the experimental curves of plan 1 to 3 that the corrected deviation of hip joint has been stable within the range of about 2 mm. It is assumed that the deviation is caused by the compression deformation of the foot end rubber, the wear of the foot end rubber, the measurement deviation, the deviation of the position control system of the HDU, and the installation deviation of the experimental platform. In order to quantitatively analyze the experimental results, the experimental curves obtained in Figs. 16a, 17a, and 18a are processed in this paper, as shown in Table 4, where R represents the maximum deviation rate of hip trajectory in the whole period, and R a represents the average deviation rate of hip trajectory in the whole period. The equations are as follows: where E 1 represents the absolute value of the maximum deviation of hip joint trajectory under ideal kinematics, and E a1 represents the average value of absolute deviation of hip joint trajectory under ideal kinematics. E 2 represents the absolute value of the maximum deviation of hip trajectory by using the kinematics correction algorithm, and E a2 represents the average value of absolute deviation of hip trajectory by using the kinematics correction algorithm. The larger the value of R, the better the reduction effect of the correction algorithm on the maximum deviation. The larger the value of R a , the better the reduction effect of the correction algorithm on the average deviation in the whole movement period.
From Table 4, the following conclusions can be drawn: both the maximum and average deviation rates of hip trajectory in the whole period are more than 60%, indicating that the hip trajectory deviation phenomenon has been significantly reduced after the application of the correction algorithm proposed in this paper. Because the maximum deviation and average deviation of the ideal kinematics in plan 3 are small, and the correction effect is always affected

Statics correction experiments
In order to directly measure the contact force between the foot end and the load simulation test platform, considering that the LHDS only moves in the vertical plane, this paper fixed the two-dimensional force sensor on the load simulation test platform to realize the accurate measurement of the contact force. The experimental equipment is shown in Fig. 19. The LHDS is fixed on the base and controlled by the upper computer to keep the initial position unchanged during the experiment. The load simulation test platform at the bottom is used for impedance control. By changing the impedance input position and placement angle of the load simulation test platform, the force loading of different positions and angles of the foot end can be realized. Since the LHDS remains stationary during the experiment, no inertial forces are generated. When the statics correction algorithm proposed in this paper is used to solve the foot end force, the detection value of the one-dimensional force sensor of each joint is directly subtracted from the initial value of the one-dimensional force sensor when the foot end is not loaded, so as to eliminate the influence of the gravity term in dynamics on the experimental results.
The specific experimental plans are shown in Table 5: The purpose of plans 1, 3 and 5 is to test the validity of correction algorithm when the body of the LHDS is at different angles to the contact surface and the foot end is at the initial position. The purpose of plans 2, 4 and 6 is to test the validity of correction algorithm when the body of the LHDS is at different angles to the contact surface and the foot end is at other positions. The purpose of plans 1 and 2, plans In plans 1-6, both the foot end contact force and the solution deviation related to the time are shown in Figs. 20, 21, 22, 23, 24, 25. The force sensor measured value is the actual contact force detected by the two-dimensional force sensor. The uncorrected statics solution force is the value of the statics solution when the foot end is regarded as a point. The corrected statics solution force is the calculated value of the foot end contact force after applying the statics correction algorithm. The theoretical deviation is the deviation in x/y direction of the foot end contact force obtained by substituting the real-time detection value of position sensor and one-dimensional force sensor of each joint HDU into Eq. (48). The uncorrected deviation is the difference between the calculated value of statics and the detected value of twodimensional force sensor in the x/y direction when the foot end is regarded as a point. The corrected deviation is the difference between the calculated value of the foot end contact force and the detected value of the two-dimensional force sensor in x/y direction when applying the correction algorithm.
From Figs. 20, 21, 22, 23, 24, 25, it can be seen that the theoretical deviation and the uncorrected deviation of the foot end contact force almost coincide, indicating that the theoretical calculation of the foot end contact force deviation in Eq. (48) is correct. In plans 1-6, after adding the correction algorithm proposed in this paper, the foot end contact force deviation is greatly reduced, indicating that the statics correction algorithm proposed in this paper is applicable when the foot end is located at different positions and the robot body is at different angles with the contact surface.
This paper uses the same evaluation index as Eqs. (57) and (58) to evaluate the effect of statics correction. Then, the meanings of each symbol are as follows: R is the maximum deviation rate of foot end contact force in the whole period, R a is the average deviation rate of foot end contact force in the whole period, E 1 is the absolute value of the maximum deviation of foot end contact force under ideal statics calculation, E a1 is the average value of the absolute value of foot end contact force deviation under ideal statics calculation, E 2 is the absolute value of the maximum deviation of foot end contact force after using the correction  Table 6 Deviation of foot end contact force in the direction of x axis algorithm, E a2 is the average value of the absolute value of the foot end contact force deviation after using the correction algorithm; the data processing results are shown in Tables 6  and 7 as follows.  From Tables 6, 7, the following conclusions can be drawn: In the direction of x/y axis, the maximum deviation rate and average deviation rate of foot end contact force in the whole cycle reached more than 42%. The results show that the foot end contact force deviation is obviously reduced by using the correction algorithm proposed in this paper. In plan 3, the maximum deviation and average deviation of ideal statics along the y axis are small, and the deviation of foot end contact force after correction along the y axis is always maintained at about 11 N, so the correction effect is the worst.

Conclusion
First, because the kinematics based on D-H coordinate transformation and the static based on virtual work principle are simplified in the modeling process, the actual contact point of the foot end deviates from the ideal modeling point. So, it will inevitably produce a certain deviation, which will affect the motion control performance of the whole robot.
Second, after applying the kinematics correction algorithm proposed in this paper, the trajectory deviation of the hip joint is greatly reduced. The experimental results show that, in plan 1 to 3, after applying the kinematics correction algorithm proposed in this paper, the maximum deviation reduction rate reaches 71.69%, 87.16%, and 65.00%, respectively. And the average deviation reduction rate in the whole motion period reaches 80.56%, 90.99%, and 67.68%, respectively.
Future work: although the kinematics and statics correction algorithm proposed in this paper can achieve obvious correction effect, they cannot completely eliminate the deviation. It is assumed that this is due to the wear of the rubber particles wrapped around the semi-cylindrical foot end and the deformation of the rubber during contact with the foot end. In future, it is planned to design the corresponding compensation algorithm according to the above phenomenon to further improve the control accuracy of the robot leg.