Leg Locomotion Adaption for Quadruped Robots with Ground Compliance Estimation

Locomotion control for quadruped robots is commonly applied on rigid terrains with modelled contact dynamics. However, the robot traversing different terrains is more important for real application. In this paper, a single-leg prototype and a test platform are built. The Cartesian coordinates of the foot-end are obtained through trajectory planning, and then, the virtual polar coordinates in the impedance control are obtained through geometric transformation. The deviation from the planned and actual virtual polar coordinates and the expected force recognized by the ground compliance identification system are sent to the impedance controller for different compliances. At last, several experiments are carried out for evaluating the performance including the ground compliance identification, the foot-end trajectory control, and the comparison between pure position control and impedance control.


Introduction
Currently, the main forms of locomotion robots include legged robots and tracked/wheeled robots. Compared with a tracked or wheeled robot, legged robots can easily adapt to and walk on rough terrain [1]. Legged robots can choose contact points with environment for overcoming obstacles and finding the feasible stable region [2]. For example, Boston Dynamics developed a hydraulic driven BigDog robot, which aimed at building an unmanned legged robot with roughterrain mobility [3]. MIT Cheetah was designed with proprioceptive actuator for impact mitigation and high-bandwidth physical interaction [4]. In our previous research, inspired by the proprioceptive actuator [5], we designed a single-leg platform for high-speed locomotion. ANYmal used the complicated actuator which makes the robot impact with high torque control accuracy [6]. Overall, the hydraulic actuator has the feature of naturally robust against impulsive loads with a high-power density [7,8]. However, legged robots with hydraulic actuators such as Bigdog developed by Boston Dynamics are difficult to be scaled down and will generate large noise [9]. Different from that, the electric actuator with proprioceptive design allows the force proprioception which can deliver the desired force with motor current sensing [10].
For quadruped robot to achieve better performance, foot trajectory planning is crucial. Two factors should be considered in the design of the foot trajectory. One is that the great impact should be avoided when the feet land on the ground; another is that the leg structure should have enough ground clearance for avoiding obstacles [11]. The foot trajectory also requires continuous velocity and acceleration for stability. There are three main methods to plan the trajectory of a quadruped robot, i.e., Bézier curve trajectory, cubic trajectory, and sinusoidal trajectory [11]. Among them, the Bézier curve meets the above requirements perfectly. After the foot trajectory planning, the behaviour that the robot interacts with irregular ground should be considered. Quadruped robots which can adjust the foot-end dynamic behaviour to deal with the unstructured terrains are important on real application. For example, Semini et al. implemented an impedance controller to control the electrohydraulically driven leg of HyQ [12]. Hyun et al. realized the virtual leg compliance of the MIT Cheetah with proprioceptive impedance control to deal with the external disturbance. After knowing the coordinates in the Cartesian coordinate system, the joint coordinates are obtained by inverse kinematic transformation, and the joint motion trajectory is obtained. Moreover, the whole leg movement of quadruped robots will be divided into stance phase with impedance control and swing phase with position control [13]. However, rigid ground assumption is applied for most researches. The different contact dynamics between the rigid assumption and soft contact will affect the performance and stability of robots. In contrast to that, Kim et al. penalizing the contact interaction in the cost function during the design of the whole-body controller [14]. Neunert et al. used the soft contact model combined with MPC controller [15]. Bosworth et al. designed a controller which can be tuned for different ground types [16]. Different from that, in our study, the least-squares method is applied to estimate the ground compliance parameters. The method is a soft terrain adaption algorithm which can achieve a transition between hard and soft ground with a real-time terrain-aware. For real leg locomotion adaption, the estimated stiffness of ground will be used for adjusting the parameters of impedances controller. This paper is structured as follows: Section 2 introduces the kinematic modelling method for the leg. In Section 2, the foot trajectory of the leg is designed utilizing the Bézier curve and sinusoidal waves. The detailed implementation of impedance control and ground compliance identification will be introduced in Section 2. The experimental results are provided in Section 3. At last, the conclusions are given.

Materials and Methods
The detailed leg design with three-joint leg structure can be found in our previous research [5]. The overall of the control diagram is shown in Figure 1. During the movement of robots, the Cartesian coordinates of the foot-end are obtained through trajectory planning, and then, the virtual polar coordinates in the impedance control are obtained   2 Applied Bionics and Biomechanics through geometric transformation. The deviation from the planned and actual virtual polar coordinates and the expected force recognized by the ground identification system is sent to the impedance controller. Finally, the desired foot-end force is obtained and then, the joint torque can be calculated by the change of the Jacobian matrix and sent to the robot system. The status of the robot system is also fed back to the previous process.
2.1. One Leg Kinematic Analysis. For realizing the foot trajectory control of the leg, the kinematics formula should be derived first. To simplify the subsequent analysis of the motion control problems, the first step is to establish a complete coordinate system for a quadruped robot. Since the configuration of the four legs of the robot is identical, the only difference is the position of the hip joint relative to the centre of mass (COM) of the robot. Therefore, the kinematics analysis of the foot-end with the hip coordinate system is consistent for all four legs. Here, only the right front leg is selected for the analysis in this section. The following joint coordinate systems are established according to the Denavit-Hartenberg (D-H) method: the hip rolling coordinate system ∑ HipR , the hip pitch coordinate system ∑ HipP , the knee pitch coordinate system ∑ Knee , the ankle pitch coordinate system ∑ Ankle , and the foot-end coordinate system ∑ Foot are shown in Figure 2. The blue line in the figure shows the three links of the leg; the red line shows the axes of the leg rotating joints.
The coordinate system of each link, as well as four joint angles, is shown in Figure 3 in detail. The left picture is the front view, where the X BH axis of the hip torso coordinate system is perpendicular to the paper surface. The right picture is a schematic plan view of the right front leg, the rotation axes of the hip pitch, knee pitch, and ankle pitch joints which are oriented perpendicular to the paper in. In addition, according to the right-hand rule, from the foot-end to the hip joint, the ankle pitch joint θ 4 , knee pitch joint θ 3 , hip pitch joint θ 2 , and hip roll joint θ 1 are defined.
According to the given kinematic model of the quadruped robot, from the hip pitch coordinate system ∑ BH to the foot-end coordinate system ∑ Foot , the homogeneous transformation matrix between the neighbouring link coordinate systems can be derived. Firstly, the homogeneous transformation matrix of the hip rolling coordinate system relative to the hip torso coordinate system is : ð1Þ The homogeneous transformation matrix of the hip pitch coordinate system relative to the hip rolling coordinate system is : ð2Þ The homogeneous transformation matrix of the knee pitch coordinate system relative to the hip pitch coordinate system is The homogeneous transformation matrix of the ankle pitch coordinate system relative to the knee pitch coordinate system is , ð4Þ The homogeneous transformation matrix of the foot-end coordinate system relative to the ankle pitch coordinate system is At last, by integrating these transformation matrixes, the (6) and (7) can be derived

Applied Bionics and Biomechanics
where c 1 represents cos θ 1 , s 1 represents sin θ 1 , c 2 represents cos θ 2 ,c 2−3 represents cos ðθ 2 − θ 3 Þ,s 2−3 represents sin ðθ 2 − θ 3 Þ,c 2−3+4 represents cos ðθ 2 − θ 3 + θ 4 Þ, and s 2−3+4 represents sin ðθ 2 − θ 3 + θ 4 Þ. Therefore, according to the positive kinematic analysis, in the case of the known joint angle θ 1 , θ 2 , θ 3 , θ 4 , the position and orientation of the foot-end relative to the hip torso coordinate system can be obtained. For calculating the inverse solution, the solvability should be considered for avoiding the no solution or multiple solutions. Usually, solving robot kinematic equations by the inverse operation is a nonlinear problem, and solving forward kinematic problems is to check whether the target point is in the working space. Therefore, for deciding the existence of the robot inverse kinematics solution, the robot leg's workspace should be calculated. For ensuring that the inverse kinematic is solvable, the foot of the quadruped robot must be within the workspace of the leg joint. According to the design index For the solution of inverse kinematic problems, there are mainly two types of closed-form solutions (analytic solutions) and numerical solutions. In this paper, the closed solution method is used to solve the analytical solution. Because the quadruped robot has the same leg configuration, only the right front leg is considered to establish its inverse kinematics model. From equation (7), we can get Because of the parallelogram structure, it is structurally guaranteed that the (8) simplifies to So, we can obtain From equation (7), we get BH P y + L 1 cos θ 1 Further, So, From equation (7), we can also get Since θ 3 has been given by equation (10), so Formula (10), formula (13), and formula (15) are the inverse kinematic equations of the leg, and the inverse solution is the only solution. Thus, if we know the coordinates of the foot-end of any leg in the hip joint body coordinate system, we can solve the requirement of each joint angle.

Velocity Jacobian Matrix.
In this section, the velocity Jacobian matrix is derived which is useful for further leg control such as the transformation of forces and torques from the foot-end to the joints. The definition of the Jacobian matrix is as follows: 2.3. Foot-End Trajectory Planning. For obtaining better performance of the robot, we should design the foot-end trajectory properly. During the swing phase, the leg is not affected by contact force, so a higher speed and acceleration can be achieved. During the stance phase, the exten-sive loadings will be added on the legs; it will cause a great rigid impact with only position control. Since the swing phase and the stance phase have different dynamic characteristics, the trajectories of the two phases are designed individually. Later, for tracking the trajectory, two control methods will be compared which are position control and impedance control.
The swing-phase trajectories are designed from a Bézier curve defined by twelve control points, and stance-phase trajectories are designed as part of sinusoidal wave which has a good performance in smoothness and was also used in other robots [17].
2.3.1. Trajectory Design for the Swing Phase. The swingphase trajectory design should guarantee enough ground clearance for avoiding obstacles and reduce energy losses   [18]. The design of the swingphase trajectory should not only approximate the natural behaviour of the leg but also satisfy ground clearance, which can avoid obstacles in the swing phase. The Bézier curve formula determined by the normalization parameter S SW ðtÞ ∈ ½0, 1 is where C k n represents the number of unordered collection in which k elements are taken from n elements, (n + 1) is the number of control points, c k is a kth twodimensional control point where k ∈ f0, ⋯ ⋯ , 11g. The curve can be generated by twelve control points as shown in Table 1, and the trajectory shape is shown in Figure 6. 2.3.2. Trajectory Design for the Stance Phase. The stancephase control of each leg will affect the performance of quadruped locomotion via interaction with the ground. Therefore, the planned trajectory should not only consider the motion requirements but also the interactive force requirements.
The stance-phase trajectory is proposed to simply as a sinusoidal wave with two parameters: the half of the stroke length L span , and the amplitude variable δ. As with the swing   Applied Bionics and Biomechanics phase, the stance-phase trajectory equation is also determined by the normalized parameters, S st ∈[0,1] Considering that the robot's leg will touch the ground and bear an impact, therefore, an impedance control should be used to resist the impact during the stance phase and will be introduced in the next part.

Impedance Controller Design.
In the previous section, we discussed the planning for robot's foot-end trajectory. If the robot has no contact with the external environment, pure motion control is enough for trajectory tracking. However, quadruped robots are high dynamic robots that their feet will contact the ground frequently during the movement. In this case, the space constraint brought by the environment will hinder the tracking movement of the robot end effector. Therefore, for ensuring the compliance during the movement, the impedance control is used where the leg will be imitating mass, spring, and damper properties. Based on this, the robot will present virtual mass, stiffness, and damping characteristics during movement [4].
A schematic diagram of a one-dimensional massdamping-spring model is shown in Figure 7. Impedance is used to describe the behavior of a robot. Different impedance parameters can be set to give different dynamic characteristics of the robot.
The dynamics for a one-dof robot rendering an impedance can be written where x is the position, m is the mass, b is the damping, k is the stiffness, and f is the force applied by the user. If b or k in the impedance coefficient is set to be large, it is called high impedance; if b or k is set small, it becomes low impedance. In this paper, virtual leg impedance is created in the polar coordinate as shown in Figure 7. The control formula can be derived as where f control is the control force sent to the controller; f est is the estimated ground reaction force; € x d , _ x d , x d are desired acceleration, velocity, and position; and € x, _ x, x are actual acceleration, velocity, and position. Considering that the virtual mass has no significant effect on the impedance effect of the robot's legs, no virtual mass term is added to the control algorithm in this paper.
The Jacobian from the hip/shoulder to foot-end in the polar coordinate system is obtained by the transformation. The position relationship between the Cartesian coordinate system and the polar coordinate system is

Applied Bionics and Biomechanics
So the Jacobian from the Cartesian coordinate system to the polar coordinate system is Then, impedance control law can be derived as where e ρ , _ e ρ , e θ , and _ e θ are radial position error, radial velocity error, angular position error, and angular velocity error between the actual trajectory with designed trajectory, respectively. J polar ðqÞ is the Jacobian from hip/shoulder to foot-end in the polar coordinate system, and J polar ðqÞ = J polar ðx, yÞJ Cartesian ðqÞ. And the block diagram for the impedance controller can be shown in Figure 8.

Ground Compliance Estimation.
For the above analysis, we assumed that the ground is completely rigid, but for a real application, the assumption is limited. For example, if the robot is moving on a concrete floor or an asphalt road, we can think that the assumption is completely valid. But when the robot moves in an environment like grass, marshes, and snow, there will be a big difference. Therefore, the identification of ground compliance is important. Through the system identification method, we can get the stiffness and damping characteristics of the contact ground, and then, further operations to achieve the corresponding impedance characteristics can be implemented.
Among the system identification theory, the least-squares method is widely used, and the effect is also excellent. Therefore, we use the least-squares method to estimate the ground compliance parameters.

Applied Bionics and Biomechanics
Taking into account the storage performance and computational performance of industrial controllers, we estimate the ground parameters using the least squares method in limited memory, which limit the estimated range. The ground reaction force (GRF) is described as where k G and b G are ground stiffness and damping coefficients; that is, the values we need to identify by the system identification x f and f G are the depth and the ground reaction force. Then, we can estimate the ground parameters by the least squares method in limited memory. At every time instant n, we gather samples from the previous k time instances and compute the ground parameters. By choosing the appropriate k value, i.e., the defined range, a good parameter estimate can be obtained and the data saturation phenomenon can be effectively improved.
To implement the estimation of parameters, we construct the following dataset By estimating b θ andF G as the optimal estimate of θ = ½k G b G T and F G , we can get Then, by defining e = F G −F G and J = e T e, and from that, the least-squares estimation requires that the sum of the squares of the residuals be the smallest; we can obtain the parameters as shown in Figure 9.
Moreover, considering that recent samples make more great impact on the results, we use a weighting matrix Q ∈ ℝ k×k to penalize the error on most recent sample compared to the less recent ones and thus, giving more importance to the most recent samples. And the result is According to the estimates b θ = k∧ G b∧ G ½ T , we can acquiref

Applied Bionics and Biomechanics
Then, in the stance phase, for getting better impedance characteristics for the robot, we apply this force to the impedance control formula (21), and we can get By the least-squares estimation method with limited memory and fading memory for ground parameters, we reasonably introduced the estimated value of GRF. Applying the estimated GRF to the f desired impedance controller, the value will change with different ground conditions, allowing the robot to adapt to different ground conditions with suitable compliance.

Foot-End Trajectory Experiment.
For ensuring that the robot can well perform the planned motion characteristics, an experiment was carried out to verify the robot's performance of foot-end trajectory tracking.
The experimental platform was constructed as shown in Figure 10. The NDI Optotrak Certus was used to trace the presticked marker on the endpoint of the leg, and the actual trajectories can be obtained. The accuracy that NDI Optotrak Certus can achieve is 0.01 mm and its sampling frequency is 100 Hz, which is sufficient for our trajectory following the acquisition.  Because the data collected by the NDI Optotrak Certus is three-dimensional coordinates of a point on its body, and the trajectory points we plan are the two-dimensional coordinates with the axis of the hip joint motor as the origin; it is necessary to compare the trajectories after the coordinate transformation.
The first step is to find the homogeneous transformation matrix O T A from the NDI Optotrak Certus coordinate system A to the robot coordinate system O.
In the coordinate system O, four scattered coordinate points were selected that the foot-end of the robot can reach. Suppose the leg motion is strictly in a plane, and z = 0, then assume that the coordinates of the selected four points are Simultaneously, the four points are collected by the NDI Optotrak Certus, and the coordinates under frame A are So : ð31Þ After making the same transformation of all four coordinates and transforming the final matrix equation appropriately, we can get After finding the homogeneous transformation matrix O T A , the points collected by the NDI Optotrak Certus are homogeneously transformed to obtain the actual trajectory in the O coordinate system and compared with the planned trajectory.
3.2. Impedance Control Experiment. For the experiment, the impedance control is based on the control in the torque mode. Therefore, the controller should first be switched to the torque mode at first. In our study, the controller (C6015, Beckhoff) was chosen with EtherCAT connection to each motor.
We observed the relationship between the real-time position, the real-time torque, and time by applying external torque to the hip joint, as shown in Figure 11. Then, we fit the torque position and get the result as shown in Figure 12.
And the fitting equation is 3.3. Comparison between Pure Position Control and Impedance Control. As we know, the essence of impedance control is to control the dynamic relationship between force and position. The pure position control is mainly to achieve an accurate position and does not consider the interaction with the outside world. To better understand impedance control and pure position control, we have done experiments to compare the characteristics of the two control methods. According to the characteristics of the two control methods, we can determine which way is more suitable in the locomotion of the quadruped robot.
In the previous analysis, we have also seen that in the swing phase, since the foot-end point of the single-leg prototype has no contact with the ground, the effects of impedance control and pure position control are similar. But in the stance phase, the foot-end point of the single-leg prototype contacts the ground, and there will be a large impact at the moment of contact. If the control strategy does not have a certain cushioning effect, it will be harmful to the motor. Therefore, it is necessary to test and analyse the control effect of pure position control and impedance control.
As shown in Figure 13, the experimental procedure is to set a fixed desired position through the impedance control and pure position control in the single-leg prototype in the suspended state (here, the desired force in the impedance control is set to 0). Then, at a certain height, the single-leg prototype was dropped freely, and the current and position change of the single-leg prototype motor were observed.

12
Applied Bionics and Biomechanics It should be noted that the drop height under the impedance control is 44 mm. Therefore, in the pure position control, the drop height was also set to 44 mm at first, but it is impossible to collect appropriate experimental data with high impact. At last, the drop height was determined to be 10 mm. In addition, through the data measurement and analysis of the two joint motors, the variation of current and position values of the hip motor during the falling process is not particularly obvious. Therefore, we did not collect the data on hip joint motor, but only current and position acquisition of the knee motor.
By analyzing the experimental data in Figure 14, we can know that in the pure position control, even if it drops from a height of 10 mm, the position and current value of the motor show a tendency to disperse, that is, an unstable state. And the peak current during the whole process is 11.03 A as shown in Table 2. In the final stage, to avoid serious damage to the motor and the outside world, it is necessary to add external force to force the single-leg prototype to stop beating.
In the impedance control as shown in Figure 15, we can see that although the drop height is 44 mm, which is 4.4 times in the above example, the peak current during the whole drop process is only 4.309 A as shown in Table 2, which effectively solves the impact on the motor during the dropping process.

Ground Compliance Identification.
Through the previous analysis, we set up the corresponding experiments to identify the stiffness of the ground of various materials.
A portion of the experimental setup is similar to the previous motion control experiment where the motion capture system is used to capture the depth of the foot into the material. Since the stiffness is defined as the force acting on the unit displacement, it is also necessary to place a force sensor 13 Applied Bionics and Biomechanics as shown in Figure 16 at the foot-end to measure the ground reaction force when the foot-end is in contact with the ground.
The experimental procedure consists of designing a control program that allows the foot-end point of the single-leg prototype to move vertically downward from above the ground material and penetrate the material, and the NDI Optotrak Certus and the force sensor collect the displacement and force information of the foot-end in real-time. Then, by observing the data of the collected foot-end force, we can judge the moment when the foot-end is in contact with the material, and then based on the displacement information at that moment, the penetration value data of the foot-end is obtained. With the penetration value and the foot-end force information, the stiffness of various ground materials can be identified by the recursive least squares method.
In the experiment, we selected five different materials, namely, material 1 is medium hardness rubber, material 2 is reinforced rubber, material 3 is low hardness sponge, material 4 is medium hardness sponge, and material 5 is high hardness sponge. The stiffness of the above five materials was obtained by the above experimental method, as shown in Figure 17.

Conclusion
Locomotion adaption for quadruped robots with ground compliance estimation is important, especially for highspeed locomotion. In this paper, the Cartesian coordinates of the foot-end are obtained through trajectory planning, and then the virtual polar coordinates in the impedance control are obtained through geometric transformation. The deviation from the planned and actual virtual polar coordinates and the expected force recognized by the ground identification system is sent to the impedance controller. The desired foot-end force is obtained and then the joint torque is obtained by the change of the Jacobian matrix and sent to the robot system. The status of the robot system is also fed back to the previous process. At last, several experiments are carried out for evaluating the performance including the ground compliance identification, the motion control and the comparison between pure position control and impedance control. In the future, the prosed control framework will be used on our quadruped robots for testing the robust when traversing on a wide variety of terrains.

Data Availability
All data were experimentally obtained.

Conflicts of Interest
The authors declare that there is no conflict of interest regarding the publication of this paper.