Fault-tolerant motion planning and generation of quadruped robots synthesised by posture optimization and whole body control

Quadruped robots are likely to fall into the fault joint state in outdoor explorations. The unexpected joint lock may suddenly happen when quadruped robots are implementing normal gaits, and maintaining the primal movement patterns to finish targeted tasks could be disastrous. In this paper, a fault-tolerant motion planning and generation method for quadruped robots with joint lock is proposed. Fault-tolerant cases on three types of joints on legs are investigated, and equivalent geometric models are proposed to reconstruct kinematics. To make unnecessary deformation of gait patterns as small as possible, the body posture and standing height of quadruped robots are to be optimized based on the nonlinear equivalent geometry models with constraints. The proposed fault-tolerant method is applicable to constructing a quasi-static whole-body controller, and it does not require additional operations and constraints of the fault leg. To validate the consistency and stability of the proposed fault-tolerant method, the experiments are implemented on the three joint lock failure scenarios for quadruped robots.


Introduction
Quadruped robots may encounter unexpected joint failure when performing locomotion in a complex outdoor environment. Once quadruped robots sustain joint failure, such as joint lock phenomena, they might lose the ability to continue locomotion to execute tasks or return. Thus, quadruped robots must possess motion robustness by fault-tolerant motion planning when a joint occurs locked failure.
As one type of multiple-legged robots, quadruped robots can take advantage of multi-degree-of-freedom joints with redundancy to generate fruitful and effective motion patterns for locomotion. However, when the quadrupeds suffer joint or actuator damage, their walking gait rhythm might become discordant and finally cause a locomotion failure. In this case, fault-tolerant motion planning and generation schemes are required to continue to maintain normal walking. As joints failure may occur in several joints on one leg or multiple legs, the fault-tolerant schemes can be usually considered in two directions: utilizing the leg with fault joints or/and renounce of the fault leg completely.
Some researchers have proposed various strategies of still utilizing the leg with one fault joint for locomotion of quadruped robots. Yang investigated the gait fault-tolerant ability of the sprawling-type quadruped robot with a single fault joint and analyzed the workspace of one leg with different locked joints to design the quadruped robot's gait patterns and parameters for different walking situations [1,2]. Furthermore, a gait planning method was exploited to avoid dead-lock caused by kinematic constraints, ensuring the effectiveness of the crab gait [3]. Likewise, Pana et al. also adopted a straight line and crab strategy for fault-tolerant gait and extended it to slope terrain scenarios [4]. The common contributive point of the aforementioned works is that the body posture is set to be maintained in a two-dimensional plane and to scheme the sequence and parameters of gaits.
The fault leg only plays a temporary supporting role without excavating the potential of the fault leg by using the movement space of the fault leg.
To still use the fault leg in planning and control for locomotion, Chen et al. proposed to ensure the quadruped robot's displacement and get a kinematics solution of attitude to utilize the fault leg's workspace without changing the gait pattern [5]. The concept of this fault-tolerant method is that the body posture can adapt to the workspace of the fault leg. Christensen et al. applied a stochastic optimization algorithm to optimize the parameters of a central pattern generator of quadrupeds that can re-adapt when the actuators fail [6]. Gor et al. proposed to change the center of mass (CoM) position and combine gait's coordination information to achieve fault-tolerant performance, which requires a pair of moving appendages mechanism [7].
For severe faults on more joints of one leg, an alternative tactic is to abandon the participation of the fault legs for the gait generation. Lee and Hirose applied minimum energy gait planning and active shock absorption to achieve a fault-tolerant gait without one leg [8]. Completely giving up the fault legs for multi-legged robots' locomotion can be regarded as an extreme scheme for dealing with harsh situations. However, it may cause losing the static support phase in the gait cycle for quadruped robots, and it should not be adopted unless in extreme fault situations which sacrifices locomotion stability.
To the best of our knowledge, there are few studies on fault-tolerant planning and generation of mammal-type quadruped robots [9]. Moreover, currently fault-tolerant strategies mainly focus on developing fault-tolerant gaits or inverse kinematics solutions that do not take advantage of the fault leg's potential or are designed specifically for specialstructured legged robots. And up to now, many quadruped robots have achieved successful performances by synthesizing the whole body control (WBC) [10][11][12][13][14][15][16] method, but the combination between the fault-tolerant method and the WBC method is not reported yet. Motivated by these points, we propose a method of fault-tolerant motion planning and control for quadruped robots in this paper. Our main contributions can be summarized as follows.
(1) A new paradigm is proposed to optimize the quadruped robot's torso posture and standing height by sequential quadratic programming (SQP). And the proposed objective function equivalently transforms the motion model of the fault leg into a manageable geometric model. (2) When fault-tolerant area is sure and further integrated with gait constraints, the motion planning of the quadruped robot is realized. (3) The WBC method is employed before and after the joint failure occurs. Hence the fault leg can be utilized as a The proposed method ensures that the quadruped robot's gait pattern can remain the original as much as possible and only modify it when some extreme circumstances occur. The body posture can be steady after optimization and execution that assures the low variation of energy. And, the consistency and stability of this method are proved by the experiment of quadrupeds under unexpected failures.
The organization of this work is outlined as follows. The fault-tolerant optimization method and gait planning of quadrupeds are proposed in the next section. The WBC and the adjustment design of posture and position are presented in the third section. The experimental results and analysis are addressed in the fourth section. Finally, we conclude in the last section.

Fault-tolerant method
There are various types of causes of joint failure situations in quadruped robots with the leg model shown in Fig. 1. The failure may come from these sources: damage to the mechanical structure, interruption of communication with the hardware, breakdown of the driver, and so on [2]. We assume that the robot fails accidentally with one joint during the whole locomotion and the joint still sends the feedback π rad, respectively of position precisely [1], which can be diagnosed through fault diagnosis algorithms [17]. For simplicity and without loss of generality, the fault joint will be discussed on the right front (RF) leg. The method can be applied likewise for the other legs due to the quadruped robots' symmetrical mechanical structure. The fault joint would reduce the range or dimension of workspace of cascade joints [1,18], which may require some specific methods to solve the kinematics [19]. For our leg model, the workspace with a fault joint may have three situations shown in Fig. 2 and corresponded as: Case (1) The fault HAA joint: It will cause the workspace to be reduced to a boundary plane, which makes the workspace intersect the ground plane to be a straight line segment parallel to the sagittal plane; Case (2) The fault HFE joint: It will distort the workspace into a part of a torus surface, the HAA joint is the axis and the KFE joint is the circle center. The torus intersects with the even ground like a closed curve; Case (3) The fault KFE joint: The residual joints in good conditions will form a spheroidal joint, and the workspace is a part of a sphere which intersects with the horizon plane as a regular circle.
The intersection area formed by the fault leg's workspace and the even ground plane can be employed by the ensuing fault-tolerant method with proper configuration.

Equivalent geometric model
If the fault/locked joint angle has no limits, the workspace of the fault leg can be regarded as an equivalent geometric model. And the kinematics can be redescribed through the geometric formula. Case (1) is straightforward to deal with by directly restricting the locomotion trajectory without an equivalent model for further optimization. For Cases (2) and (3), the geometric is a generalized torus and sphere.
The equivalent geometric model that intersects the even ground plane that forms the closed curve can be termed the fault-tolerant work area (FWA). In normal static walking, the end of the leg will move within a range around the hip respecting to the CoM frame as denote the rotation matrix, roll, pitch, the hip position with respect to the CoM frame and the hip position with respect to the base frame respectively. We assume that this is an ideal closed circle termed normal work area (NWA) with diameter being the leg step length L step , which can be formulated as where (x, y) denotes the coordinate of the NWA with coef- As the FWA in Case (2) is the generalized torus intersects the ground, it hard to build an optimization object. We consider the KFE joint as an imagined sphere's center to form a sphere, so Cases (2) and (3) unify the optimization basis. Both are a sphere that intersects the even ground. The center of the sphere concerning the CoM frame can be expressed as the joint angles and the position of the sphere's center respectively. The sphere equation with radius H r can be depicted as (1) and (2), the difference in coefficients lies in the difference in geometric parameters between FWA and NWA, which can be minimized to reduce the loss caused by the joint failure. Thus, the FWA can probably adapt to the original gait pattern.
The original idea is to change the body posture and standing height to make the FWA and NWA being similar as much as possible in terms of location and effective region. Define the body posture and standing height as q = [φ θ h] T for the variable to be optimized, and we have the following optimization paradigm where P −P = [A −Ā B −B D−D] T denotes the position and area differences between FWA and NWA, Q, H ∈ R 3×3 both are positive-definite weight, q d − q 2 H is the quadratic adjustment cost, which is a regularization term to ensure that the solution with a boundary, A = [I 3×3 −I 3×3 ] T denotes the constrained inequality coefficient,q = [q T max −q T min ] T ∈ R 6 denotes the body posture boundaries. The position boundaries of the imagined sphere center of the fault leg are x l and x r .
The optimization problem above can be solved with SQP with gradients and Hessians to get the optimized solution q * . The obtained q * alters the position and posture of the equivalent geometric models. And the models can intersect with the even ground as closed curves formulated as x − x c,B where (·) i, j denotes the effective element at row i and row j.
The optimized FWA described in Eq. (5) is the function that torus intersects the ground in the CoM frame. And the Eq. (6) is the function that the sphere intersects the ground. Based on the optimized FWA, the motion trajectory of the CoM can be planned, and this also requires additional restrictions that come from gait.

Motion planning
The static walking gait [20] can be considered a primary and preferred gait to be modified and implemented for the faulttolerant planning [1], and we apply it or its variant according to the FWA. The optimized FWA is the region schemed for the supporting phase, denoted as g(q * ). For static walking patterns, the constraints in support locomotion can be indicated as , and it comes from the feasible range of movement of the CoM on the y-axis shown as the gait constraints boundaries in Fig. 3. The FWA can be restricted as g w (q * ), and the appearances are the black curve shown in Fig. 3. Within the g w (q * ) space, a sub-trajectory can be planned for moving the center of gravity (CoG) into the supporting polygonal area that is the phase of propelling the body horizontally in the gait cycle [20]. The end position of the sub-trajectory can be obtained by where x e,C represents the CoM moving target in the supporting triangle [21,22]. x s,leg and x d,leg denote the start and end point of the foot. The locomotion trajectories of the CoM and the supporting feet are oblique symmetrical to each other. Thus, taking the oblique symmetry point sets of g w (q * ) with x s,leg and x d,leg as the boundary and applying the least-squares method to fit the cubic polynomial curve h(t) can be the translation reference of CoM.
When that g w (q * ) is a completed closed curve, the gait pattern can remain steady, provided that the swing trajectory can scheme entirely. However, if g w (q * ) is a segmented curve, the contribution to the support displacement in the x-axis dimension will be minimal, and a gait fault-tolerant strategy will be required. For the fault HAA joint case, the FWA is a line segment and suitable to the original gait. For some HFE and KFE locked/fault joint states, the y-axis space range of FWA is tremendous, and the CoG may run out of the support area, so the gait pattern necessitates being changed. The g w (q * ) of the mentioned situations that can adopt normal static walking gait [21,22] and two-phase discontinuous gaits [23] together with the corresponding gait cycle and motion planning diagram are shown in Fig. 3.

Whole body control
The WBC applied is a quasi-static controller that is very suitable for normal static walking given the appropriate trajectory, including position and posture. After the joint failure occurs, the schemed body translation trajectory is the g w (q * ) in oblique symmetry, and the WBC would drift the foot of the fault leg in the space of FWA. For body orientation maintenance, the fault leg's planned ground reaction forces (GRFs) will be compensated by passive ground reaction force and internal stress caused by normal legs. Thus, the controller can not be modified or require additional constraints and still have reliable performance with fault/locked joint(s). The diagram of the fault-tolerant method with WBC is shown in Fig. 4.

Model description
The quadruped robot can be considered a trunk floating in space [10], with distributed limbs providing body support. Since the inertia of the limbs of the quadrupeds is much smaller than the trunk, it can simplify the controller model and consider the GRFs as the main factor affecting the CoM translation accelerationẍ d,C and angular accelerationẇ d,B [11,12], which can be expressed as where denotes GRFs of the legs and n denotes the number of legs contacted including the fault leg. p i,C × is the skew-symmetric matrix of the contacted foot position respect to the CoM frame. m, I g and g are the robot's mass, centroidal rotational inertia and gravity vector. A virtual model [24][25][26] applied to the center can be employed to track the reference trajectory [14] by the PD controller as follows: where log(·) denotes the exponential map of rotation [11,27]. R d and R denote the desired and actual body rotation matrices respectively. The solution of (8) can be obtained by the following constrained QP problem [13][14][15][16]: where Q w and R w respectively represent the weights of the accuracy of trajectory tracking and the output cost. CF ≤ ensures that the GRFs are confined within the friction cone [13][14][15][16], and can be expressed in detail as where μ is the friction coefficient, f z,max and f z,min denote the limit boundaries of the end vector force on the z-axis.
Ultimately, if all the joints number is k, the joints torque τ ∈ R k including the no executed fault/locked joint is determined by where J f ∈ R 3n×(k+6) denotes the stacked jacobian of the contact feet, F * denotes the optimized GRFs, and S = [0 k×6 I k×k ] denotes the selection matrix.

Design adjustment of position and posture
We utilize the minimum jerk trajectory [28,29] to plan body position and posture in the time domain before the failure. When the locked fault occurs, the execution of q * is also with the minimum jerk trajectory. After position and posture adjustment, the translation of CoM is substituted by the fitted curve h(t), and the posture is still by the minimum jerk trajectory. From point to point in one dimension, the position, velocity, and acceleration variables of the start point p s = [p sṗsps ] T ∈ R 3 at time t k and the endpoint p e = [p eṗepe ] T ∈ R 3 at time t k+1 can be specified as the function as The ε ∈ R 6 is the spline coefficients need to be solved. We utilize the fifth-order polynomial function in the three dimensions of translation as The translation trajectory can be planned as a long-term trajectory based on the gait planning in the normal state in the world frame [22]. And we define the one dimension time function with range [0, 1] as b r (t) to plan the orientation. The desired body orientation with the start and endpoint rotation matrices can be formulated as At the HFE joint failure case, the WBC performs the optimized q * that will cause the imagined sphere's center position to change in the CoM frame. We iteratively optimize and execute to approach the minimize the differences between FWA and NWA. As for the KFE joint failure, the equivalent model does not change with the WBC process in our leg model.

Swing leg planning
The trajectory of the endpoint of the swing leg in task space is also devised by Eq. (14). However, the workspace is compressed in the curved surface when one joint occurs locked failure, and whether the end of the leg can swing forward, landing on the support point depends on the level and case of the failure. Specifically, the planar workspace formed by the HAA joint failure is still effective of a swing leg trajectory for the gait pattern. The toroidal space by the HFE joint failure can not produce an effective swing leg trajectory, but the leg can still be lifted. For the formation of the KFE failure, its spherical workspace can develop an effective trajectory, which is manifested as an arc movement on the surface. We obtain the inverse kinematics solution or approximate solution [30] as where λ > 0 denotes a regularization factor, and α > 0 denotes the step length. After the joint failure occurs, it is necessary to replan the reference swing leg trajectory according to the resultant workspace features. The HAA joint failure case can directly use the original trajectory to form an approximate motion, assuming no special requirements for the track. The HFE joint failure situation can directly plan the point-to-point

Experimental results
The validation experiment is performed in the Webots platform simulated by Open Dynamics Engine [31,32] with the quadruped robot prototype as shown in Fig. 5, and its main physical parameters are presented in Table 1.

Optimization of planning and control loop with different fault cases in different levels
In the experiments, we set desired body state q d = [0 0 0.453] T , the SQP optimization initial value as q 0 = [0 0 0.2] T , the step length L step = 0.1 m, and the moving direction is 0 rad. To explain the level of failure in different cases, we use some parameters to characterize them. As the HAA fault joint does not participate in the optimization process, and we do not discuss it here since it is easy to handle in general. For the HFE joint failure, the HFE joint angle is the variable that directly impacts the level of failure, which changes the section circle center position of the torus relative to the CoM frame. The corresponding geometric representations of FWA and NWA are shown in Fig. 6a. It can be seen that the optimized position difference between FWA and NWA gradually decreases as the HFE joint angle increases. Further, the area difference between FWA and NWA decreases, and the failed leg's workspace shifts backward. For more detailed investigations, Table 2 shows the comparison of absolute center distance (CD) and area radius difference (ARD) of FWA and NWA under the equivalent geometric model without and with optimization. The equivalent models may not contact (NC) with the ground are not in comparison. And it is evident that the different levels of difference between FWA and NWA can be decreased mostly.
For the KFE joint failure, we use the distance from the foot to the center of the hip for characterization, and the specific range is [0.25, 0.55] m. The geometry of the optimized FWA and NWA is shown in Fig. 6b. We can see the optimized FWA center almost coincides with the NWA. And the FWA edge concentrates on the edge of the NWA inside and outside. For more details, the optimized FWA compare with not are presented in Table 3, which elaborates that the main factor of the optimization is the standing height. And the optimization can prevent the failed leg from hanging in the air to facilitate locomotion.
We use the standard of |q * − q d | for comparison of q * and q d , as shown in Fig. 6c and d. From a macro point of view, the foremost content of the two fault scenarios' optimization is the standing height and pitch angle. The fault situations of the HFE are more complicated than the KFE, leading to the complexity of the combination among the optimized quantities. And it can be easily seen that the KFE joint failure has an ideal fault state that minimizes changes in the body pose.
With a loop of SQP optimization and the WBC process, the final convergence results are shown in Fig. 7. It can be seen that the results are almost with no change with the iteration of the KFE joint fault case. And it can achieve the steadystate without the loop. On the contrary, the HFE joint failure case needs to be optimized and comply iteratively until the convergence.
It is worth mentioning that the permissible range of fault tolerance depends on many factors, such as the distance between hip joints, gait parameters, leg length, etc. In our model and test parameters, we consider the tolerable failure angle of the HAA joint as (−0.78, 0.78) rad. Comprehensively, using CD and ARD to indicate, the tolerance in the HFE case are CD ∈ (0, 0.2) m and ARD ∈ (0, 0.2) m. The tolerance in the KFE case is ARD ∈ (0, 0.05) m.

HAA joint failure
When the HAA joint occurs failure, the quadruped will overturn if the original motion mode is maintained by the WBC without the proposed fault-tolerant method, and the instant    walking frame is shown in Fig. 8a. Figures 9a and 10a show the results by the proposed fault-tolerant method with the WBC, and we can see that the quadrupeds can work normally with the fault HAA joint. Figure 11a shows the corresponding planned and actual translation trajectory of CoM which is a nearly straight line. It indicates that the actual CoM trajectory can track the expected one well with HAA joint failure by the proposed method. Figure 12a shows the passive motion performance of the end of the fault leg with respect to the CoM frame, and it reveals that the fault leg can still work passively under the planned CoM trajectory.

HFE joint failure
Under the standard of similarity between FWA and NWA, the HFE joint failure is more serious in general as compared with the other types of joint failure (i.e., HAA and KFE), it is unlikely to plan the swing leg trajectory to make stepping.  Thus, a two-phase discontinuous gait is adopted as described in Fig. 3.
For the fault scenario of the HFE joint without using the fault-tolerant method, the robot will drag the failed leg compulsorily but not overturn, and the leg cannot track the planned swing leg trajectory, as described in Fig. 8b. Figures  9b and 10b show the stable overall coherent locomotion, and it can be seen that the quadrupeds act with a head-up attitude. During the body forward translation phase, the distance is also taken at a ratio of 0.7 of the extreme described in [23], which naturally leads to regular and stable CoM locomotion and is confirmed as shown in Fig. 11b. To have a sufficient stability margin, the actual constraint boundary of FWA here is 0.6 of the extreme boundary. Figure 12b records the repetitive motion trajectory under the actual gait constraint boundary, which appears as the central part of an ellipse-like arc that approves the effectiveness of the passive motion. The passive motion trajectory is mostly on the inner side, which may be the influence of the spherical foot.

KFE joint failure
The quadrupeds can do normal static walking and the twophase discontinuous gait according to the fault level and distances among hips. Unexpected failure in the experiment appears in the swing phase, and the optimization results lead to the initial gait cycle requiring trajectory constraints and planning. Furthermore, the swing step length needs to be reset as the diameter of optimized FWA to unified gait standards of each leg in the CoM frame.
When the KFE joint failure occurs, the whole robot will fall since no support comes from the failed leg as shown in Fig. 8c. From Figs. 9c and 10c, we can see the forward locomotion can be ensured and sustained. The failed leg swing similar to an arc of a circle. Since the angle between the tangent plane of the failed leg's motion space and the even ground is too small, the foot of failed leg would be easily disturbed to slip. We apply the failed leg to perform the position control independently and do not implement passive movement under the WBC.
It is obvious to get the planned trajectory of the body and the trajectory at the failed leg's end as part of a circle, and the actual execution trajectory will be near the coveted arc, which is compared in Figs. 11c and 12c. In the gait cycle, the trajectories combined into an approximate half of the circle schemed twice. Carefully, a stagnant point can be vaguely seen in the arc. Since the failure occurs at a particular phase in the gait cycle, failed leg movements will repeat this stage Fig. 11 The fault-tolerant locomotion trajectory in the x-y plane of CoM after failure. a The HAA joint failure. b The HFE joint failure. c The KFE joint failure Fig. 12 The optimized FWA and the repeated trajectory of the failed leg's end relative to the CoM frame. a The HAA joint failure. b The HFE joint failure with cut FWA. c The KFE joint failure with complete FWA of action, causing the arc appears periodically on one side. And from the perspective of body motion trajectory, the faulttolerant motion planning and the actual WBC operation are stable and rhythmic.

Conclusions and future work
The study of the fault-tolerant ability of quadruped robots can play an essential role in quadrupeds' vigorous future development. This paper proposes a fault-tolerant method through the equivalent geometric model to reconstruct the failed leg's workspace when one joint occurs locked/failure and constructs a nonlinear description approximation formula that uses the equivalent model to optimize the body posture and standing height. The proposed approach can be compatible with the quasi-static controller (WBC) to achieve continuous and stable forward motion. The failed leg can be operated passively or in active position control mode, depending on its fault workspace. In all, we have discussed three joint fault situations, analyzed each one's characteristic, and compare it with normal walking cases.

Conflict of interest
The authors declare that they have no conflict of interest.
Open Access This article is licensed under a Creative Commons Attribution 4.0 International License, which permits use, sharing, adaptation, distribution and reproduction in any medium or format, as long as you give appropriate credit to the original author(s) and the source, provide a link to the Creative Commons licence, and indicate if changes were made. The images or other third party material in this article are included in the article's Creative Commons licence, unless indicated otherwise in a credit line to the material. If material is not included in the article's Creative Commons licence and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder. To view a copy of this licence, visit http://creativecomm ons.org/licenses/by/4.0/.