Dynamic analysis and decoupled control of a heavy-duty walking robot with flexible feet based on super twisting algorithm

In this paper, the dynamics and control strategies of a biped robot with 6-DOF parallel leg mechanism are studied. Firstly, the multi-body kinematic model and dynamic model of the robot are established. Secondly, the insufficient stiffness of robot’s feet and the coupling effect between the kinematic chains are considered in dynamics modeling, and the rigid-flexible coupling model is established by using ADAMS and finite element method. Finally, aiming at the position deviation and system vibration caused by the flexible foot, a strategy based on the combination of a computed torque controller (CTC) and a second-order sliding-mode super twisting algorithm (STA) is designed. At the same time, the control strategy based on CTC and PID and the control strategy based on CTC and sliding mode control (SMC) are designed to compare with CTC-STA. The results show that CTC-STA has faster regulation ability and stronger robustness than CTC-SMC and CTC-PID.


Introduction
Mobile robots are more and more widely used in many fields and the analysis of function and stability is essential in robot research and application. 1,2 At present, the legs of most mobile robots are connected in series by a group of open-loop kinematic chains, which have the characteristics of large-scale workspace, poor stiffness, and simple dynamics. [3][4][5] In order to enhance the loadcarrying capacity and stiffness, more and more robots with parallel leg mechanism are being developed and studied. [6][7][8] In theory, since the load is supported by multiple chains, parallel leg mechanism tends to have a large load-carrying capacity and high structural stiffness. 9 However, this kind of leg mechanism has complex dynamics and control algorithm problems 10,11 as well as difficult forward kinematics. 12 The kinematic model can reflect the mapping relationship between joint motion and spatial motion of the mechanism without considering the effect of force, while the dynamic model is to study the relationship between mechanical motion and force. 13,14 In the dynamic model, using CTC as dynamic compensation can make the robot become a more easily controlled system. Zhang et al. 15 designed an adaptive robust decoupling controller based on time-delay estimation (TDE) and sliding mode control (SMC) for a multiarm space robot. The dynamic coupling of the robot was compensated by TDE technology, then SMC complemented and reinforced the robustness of the TED. Van et al. 16 proposed a new method based on the combination of CTC, high-order SMC and fuzzy compensator to control uncertain robot manipulators by using only position measurements. A third-order SMC observer was used to observe the speed and identify the uncertainty, and fuzzy compensator was proposed to enhance capability of the tracking performance.
The flexible joints and components have great influence on the kinematic and dynamic performance of the robot, and the uncertain factors not considered in the model will be enlarged. Slender rods and joints in robot structures are generally considered to be flexible. 17,18 Yang et al. 17 established the rigid-flexible coupling model of a 6-DOF Stewart platform with flexible joints in ADAMS and MATLAB, and the explicit dynamic equations are established based on the pseudo-rigidbody model and the principle of virtual power. Elghoul 19 proposed a fault-tolerant control method of a flexible joint robot to compensate for the lost performance due to the uncertainty. The singular perturbation method was used to reduce the dynamic model of the flexible joint robot in a fast and slow subsystem.
In this paper, a biped robot with 6-DOF parallel leg mechanism is studied. The insufficient stiffness of the foot structure in space leads to large trajectory deviation and even vibration, and the coupling effect between the kinematic chains is strengthened. In order to improve the dynamic performance and the robustness of the control system, a strategy based on the combination of CTC and STA is designed to adjust the system uncertainty caused by the flexible foot. CTC-PID and CTC-SMC strategies are designed for comparison. The contribution of this paper is to provide a dynamic analysis method of the flexible parts of the robot system, and then take the flexible deformation into account in the dynamic control, and provide a feasible control strategy.

Robot structure
The schematic diagram of the robot prototype and the kinematic chain are shown in Figures 1 and 2, respectively. Each leg has six kinematic chains (P i A i =P i 0 B i i = 1 . . . 6) that are driven by a servo motor to realize two-way linear motion. And each leg can be modeled as a closed-loop kinematic system.
In order to simplify the robot model and facilitate the analysis, the following provisions are made: (a) The center of mass (COM) of the upper platform (P) is located in the center of all the joints (P i , P i 0 ), correspondingly, the COM of foot A is located in the center of all the joints (A i ) of leg A, and the COM of foot B is located in the center of all the joints (B i ) of leg B; (b) The friction and clearance in the system can be ignored; (c) The motion process of foot A relative to P is taken as the research object.
As shown in Figure 1, the upper platform coordinate is defined as {P: O-XYZ}, and the two foot coordinate systems are {A: O-XYZ} and {B: O-XYZ}, respectively. In the initial configuration, coordinate system {A} and {B} are coincident. In Figure 2, L i represents the length of the kinematic chain i, and e i represents the unit vector of the position vector from A i to P i in coordinate system {P}.

Kinematic model
The robot's motion is realized by changing the relative position among the coordinate systems {P}, {A}, and {B}. It is stipulated that ZYX Euler angle is used to    (1): where T is the rotation matrix, A G is the vector in {A}, G is the position vector of {A} in {P}, and T can be expressed by equation (2): cacb casbsg À sacg casbcg + sasg sacb sasbsg + cacg sasbcg À casg Àsb cbsg cbcg where ca = cos(a), sa = sin(a), a, b, g are rotation angles around Z, Y, X axis of {P}. According to Figure 2, the closed loop of each kinematic chain of leg A can be established as equation (3): It can be obtained that the vector L i e i (P i A i ) is calculated as equation (4): l i and e i of the ith kinematic chain are calculated as equation (5a) and (5b), respectively: Dynamic model q = ½x, y, z, a, b, g T is used to represent the position and direction of {A} relative to {P} and Lagrange equation (6) is used to model dynamics: where K = K q, _ q ð Þ represents the kinetic energy, P = P q ð Þ represents the potential energy , F represents the generalized force acting on q. The kinetic energy and potential energy of leg A can be obtained from equation (7).
where K 1 is the kinetic energy of foot A, P 1 is the gravitational potential energy of foot A, K 2 is the total kinetic energy of six kinematic chains, P 2 is the total potential energy of six kinematic chains. K 1 , K 2 , P 1 , P 2 are calculated as follows: where M q is the mass matrix of leg A, M L is the mass matrix of six kinematic chains, Z i is the position of COM(G i ) of ith kinematic chain in Z axis of {P}. According to Lagrange theory, 20 the dynamic equation can be presented as follows: where q, _ q, € q 2 < n are the vectors of generalized position, generalized velocity and generalized acceleration, respectively; M q ð Þ 2 < n3n is a bounded, symmetric and positive definite inertia matrix; V q, _ q ð Þ 2 < n represents the Coriolis force and centripetal force; G q ð Þ 2 < n and F 2 < n represent the conservative force and dissipative forces, respectively. M q ð Þ, V q, _ q ð Þ, G q ð Þ can be rewritten as: where I n is the unit matrix, is Koditschek symbol. The relationship between F in equation (9) and driving force f of kinematic chains is Þ , therefore, equation (9) can be written as equation (11): whereĴ = ½e 1 T r A1 3e 1 ð Þ T ; :::; e 6 T r A6 3e 6 ð Þ T is the mapping matrix between space velocity and joint velocity, and r Ai is the position vector of A i in {P}. Equation (11) can reflect the relationship between f and q when the deformation of flexible parts, the uncertainty and unknown load are not considered.

Flexible component analysis
As shown in Figure 1, the stiffness of foot A in space is not high, which easily leads to large position deviation and even system vibration in the process of robot motion. In order to calculate the influence of elastic deformation on the large-scale movement of foot A, a hybrid coordinate is proposed to describe the displacement. X = x, y, z ½ , c = f, u, u ½ represent the position and direction, respectively; q i = q 1 , q 2 . . . q n f g represents the modal coordinate; then the generalized coordinate of the flexible body can be expressed as equation (12): The kinetic energy equation T K of a flexible body can be calculated as equation (13): where m i represents mass of node i, I i represents moment of inertia, M z ð Þ represents inertia matrix. The potential energy equation U of the flexible body can be calculated as equation (14): where C is the stiffness matrix; U g z ð Þ is the gravitational potential energy term. The equation of flexible body motion can be derived by using Lagrange equation, as shown in equation (15): where L = TÀU, Q is dissipative energy, C is the constraint equation, l is the Lagrange generator corresponding to the constraint, F is the generalized external force. By substituting equations (12), (13), and (14) into equation (15), the dynamic differential equation of flexible body follows: where G is the derivative of U, D is the damping coefficient. When the mass, COM, moment of inertia, frequency, mode shape and other information of the flexible body are obtained, the reasonable deformation can be calculated in real time by using mode superposition theory in ADAMS. The deformation u of flexible body module in ADAMS is obtained by calculating mode superposition equation (17) u = where F i is the ith mode vector, q i is the coordinate value of displacement u in the ith mode. The first six modes of foot A are shown in Figure 3.
The rigid-flexible coupling model in ADAMS is shown in Figure 4.
The X-axis is selected as the forward direction of the robot, and the trajectory is planned according to equation (18a): x t ð Þ = 0:5926t 3 À 0:5926t 4 + 0:1580t 5 The speed and acceleration of the swinging leg at the beginning (t = 0 s) and the end (t = 1.5 s) are both 0, the single step distance is 0.2 m.
According to equations 18(a) and 5(a), the displacement of each kinematic chain can be obtained.    (18). P1, V1, and A1 respectively represent the displacement, velocity and acceleration when foot A is a rigid body, P2, V2, and A2 respectively represent the displacement, velocity and acceleration when foot A is flexible, e1, e2, and e3 are position error, velocity error and acceleration error between flexible model and rigid model, respectively. In Figure 5(b), J i and J' i represent the main force of A i in Z direction (counteracting the gravity) when foot A is rigid and flexible, respectively. Figure 5 shows that foot A can track the planned trajectory whether it is flexible or rigid. Compared with the rigid body, when foot A is a flexible body, the acceleration jitters and changes in a large range. In order to obtain reliable simulation results, the effect of foot A's deformation is treated as position disturbance, as shown in equation 18(b): where the first term is Gauss function, which represents the position change caused by rebound deformation when foot A leaves the ground; the second term is trigonometric function, which represents the swing caused by the coupling action from foot A's flexibility. a= 0.005 mm, b = 0.25 s, c = 0.2, d = 0.003 mm, w = 15 rad/s, t 1 = 0-0.5 s, t 2 = 1-1.5 s.

CTC-based PID
The calculated torque controller (CTC) is widely used in the controller design of industrial robot and manipulator. 15 An internal control loop is introduced into the dynamic control of the robot, whose function is to dynamically compensate the system. As shown in Figure 6, the traditional CTC combines the calculation of nonlinear force term V q, _ q ð Þ and gravity term G q ð Þ in equation (11) and PD control into the controller to realize the decoupling and linearization of the system.
The CTC law can be expressed as: where q d , _ q d , € q d are the ideal position, velocity and acceleration signals, respectively; u is the controller output; K P , K D are the proportional and derivative gain matrices, respectively, which are diagonal and positive definite. Equation (20) can be obtained by substituting equation (19) in to equation (9): Equation (20) can be expressed in the form of state space: where X = e _ e T Â Ã , A = 0 I; ÀK P À K D ½ .According to the exponential algorithm, equation (22) can be obtained: It is obvious that by choosing K P and K D reasonably, lim t!' e t ð Þ = 0 and the closed-loop performance of the system can be adjusted by K P and K D .

CTC-based STA
When the system model is accurate enough, the CTC-PD control strategy can achieve good effect by selecting K P and K D . However, PD control is difficult to deal with the disturbance and uncertainty, especially when there are flexible parts in the system, there is a large perturbation of system parameters. The super twisting sliding mode control is considered to deal with the disturbance caused by the flexible parts in this section, so as to maintain the robustness and tracking accuracy. The controller is designed based on the combination of CTC and super twisting algorithm (STA), as shown in Figure 7. STA sliding surface vector is designed as equation (23): where L . 0, e and _ e are position error and velocity error, respectively. Equation (24) can be obtained by deriving equation (23): where € e is acceleration error. Substituting equation (9) into equation (24): The sliding mode control (SMC) can be expressed by equation (26): where u STA is STA and can be defined by equation 27: where k 1 , k 2 . 0, sign() is sign function, w is the intermediate variable. When the system moves near the sliding surface , s'0, so the step part s j j The closed-loop system's stability can be proven by Lyapunov function: where X T = ½ s j j 1 2 sign s ð Þ, w = X 1 , X 2 ½ T , P is the positive definite symmetric matrix and satisfies equation (30): where A = À0:5k 1 0:5; Àk 2 0 ½ is Hurwitz matrix, Q is any positive definite symmetric matrix. Equation (31) can be obtained: The derivative of equation (29) with respect to time is shown as equation (32).
According to equation (29), equation (33) can be obtained: where l min P ð Þ and l max P ð Þ represent the minimum and maximum eigenvalues of matrix P, respectively. Next, we can get equation (34): Equation (35) can be obtained from equation (33): Substituting equation (34) into equation (35) yields: It can be obtained that the closed-loop system equation (28) tends to 0 in finite time, and e ! 0, _ e ! 0 when t ! ', so the closed-loop system is asymptotically stable. To compare the control effect of STA and common SMC, a SMC based on proportional switching function is designed as equation (37).
Numerical simulation and analysis In this section, in order to verify the influence of flexible parts on the system and the performance of CTCbased PID and CTC-based STA, the simulation model is built in Matlab/Simulink and ADAMS / flex, as shown in Figure 8. When leg A moves as equation 18(a), the driving force f = f 1 , f 2 . . . f 6 ð Þof six kinematic chains is obtained according to dynamic equation (9), and f is taken as the input parameter of Adams model. Parameters of the controllers are shown in Table 1.
The simulation work is divided into two parts. The first part is to compare the effects of CTC-PD, CTC-SMC, and CTC-STA on the nominal model, and obtain the trajectory tracking performance of foot A. In the second part, the performance of the three methods are compared in the case that foot A is flexible.
The performance of the controllers to the nominal model is shown in Figure 9, where IT represents the ideal trajectory, CT represents the common PID control, ST represents the STA control, CST represents the common SMC control; CE, SE and CSE are the errors between CTC-PID, CTC-STA, CTC-SMC and the ideal trajectory respectively. In Figure 9(b), CF is the resultant force of A i (i = 1..6) in Z direction with CTC-PID control, SF is the resultant force of A i in Z direction with STA control.
The simulation results show that all the three kinds of controllers can make the nominal model track the ideal trajectory accurately. Compared with CTC-STA, the tracking errors of CTC-PID and CTC-SMC method are larger. From Figure 9(b), it can be seen that when leg A moves as equation (18), the A i 's resultant force in Z direction is used to offset the gravity of foot A. And due to the sliding surface, the resultant force presents buffeting.
In accordance to the analysis in Section 2.4, simplify the effect of foot A's flexibility and apply it to {A} as equation (18b) when foot A is considered as a flexible part. The performance of the three controllers is shown in Figure 10, FCT, FST and FCST are the trajectories of CTC-PD, CTC-STA, and CTC-SMC, respectively.
It can be seen from Figure 10(a) and (b) that although the position of foot A is greatly deviated due to rebound motion in the early stage (0-0.5 s), all the three controllers can quickly adjust the position to make foot A accurately track the ideal trajectory, and CTC-STA and CTC-SMC show better adjustment performance than CTC-PD. When foot A vibrates (1-1.5 s), the trajectory tracking error of CTC-PD increases gradually. CTC-STA and CTC-SMC still have strong adjustment ability, which makes the trajectory close to the ideal trajectory, and CTC-STA has the best tracking performance. Common SMC has properties such as robustness, parameter variations, reduced order dynamics, while STA retains most of the properties and its higher order derivative is activated on the sliding variable instead of first derivative. As shown in   Figure 10, the output of CTC-STA controller is continuous and gentle, which is beneficial to the system.

Conclusion
In this paper, a super twisting sliding mode controller based on computational torque control is proposed for the control of a heavy-duty walking robot with flexible structure, the PID control based on CTC and the SMC control based on CTC are designed for comparison. CTC-PID, CTC-SMC and CTC-based STA are model-based decoupling control strategies, but CTC-PID is more dependent on the model accuracy. When there are flexible components or large model errors and external interference in the system, the CTC-PID maybe not able to achieve satisfactory performance. By introducing the STA strategy, the trajectory can be accurately tracked, which can effectively reduce the coupling effect and improve the robot's performance and robustness. The combination of CTC and STA improves the potential of robot dynamics in practical application.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) received no financial support for the research, authorship, and/or publication of this article.