Path planning of a quadruped robot on a flat surface using ZMP for stability

In this paper a novel method for gait trajectory design of a point legged quadruped robot with 18 degree of freedom on a flat surface is proposed. The exact model of the mechanical parts of the robot including four legs and the body and all joints will be derived and used in gait trajectory planning. Inverse dynamics of the quadruped are formulized using Denavit-Hartenberg (DH) convention. Bezier curves are used for trajectory planning. Effects of body and legs on Zero Moment Point (ZMP) will be analyzed separately. By manipulating free parameters, a stable gait trajectory will be designed according to ZMP dynamic stability criterion. Simulations are done using Matlab and Mathematica software which show good performance of the novel method proposed.


Introduction
Mobile robots can be categorized into 4 different categories based of the method of moving: Wheeled robots, flying robots underwater robots and legged robots. Legged robots are capable of performing tasks in which other kinds of robots cannot perform. Many or all bipedal models are not practical as they are cumbersome and slow. Most successful legged robots have 4 or 6 legs for further stability. This legs-overwheels approach lends itself for use in all-terrain purposes seeing as legs are more effective in an uneven environment than wheels.
Study of walking robots started in the mid-60s when an initial prototype of a quadruped robot was built and tested by General Electric Corporation. Since then, lots of research has been done by many researchers but many problems still remain unsolved. On one side, these problems deal with low-level, but critically important issues such as gait control, force-feedback control for terrain-adapted foot-placement, stability, etc. On the other side, there are problems of dynamic control, the incorporation of environment sensing, collision avoidance, goal attainment, and others. This paper presents a forward gait for a quadruped footpoint walking robot. For a point-foot walker, there is no actuation at the ankle, therefore no direct control over the foot angle with the ground.
A moving robot by definition violates the assumptions of static stability. Zero moment point is a concept related with dynamics and control of legged locomotion. It specifies the point with respect to which dynamic reaction force at the contact of the foot with the ground does not produce any moment in the horizontal direction, i.e. the point where the total of vertical inertia and gravity forces equals zero [1]. The concept assumes the contact area is planar and has sufficiently high friction to keep the feet from sliding.
In [2] a gait planning and trajectory for a quadruped has been designed and optimized. A dynamic model has been derived and foot trajectory planning has been done. In [3] and [4] a gait trajectory using a simplified model of quadruped has been done with a force control approach. In [5] a rhythmic gait is designed and in [6] trajectory planning has been done so that the lost energy is minimized. The dynamic model of the robot is not incorporated in it. In this paper, a detailed dynamic and kinematic model of the quadruped is derived and the gait trajectory planning is done with consideration of ZMP criteria for dynamic stability. Point-foot walkers, have the advantages that it is easier to select placements for feet contacts on the terrain, and significantly simplifies simulation, as one does not have to worry about ground contacts along the surface of the foot. However, point-foot walkers exemplify the under-actuation problem. In addition, point-foot walkers have many redundant degrees of freedom (DOF); each leg can act to push the body in different directions, so care must be taken to coordinate the actuators of all legs. Foot planning and maintaining stability margins are crucial for the success of a walking vehicle, but lower level inverse kinematics algorithms must do the necessary work of coordinating available actuators while also maximizing stability. Further, the lower level control can be useful for reducing the dimensionality of the system. The quadruped in this paper has 18 DOF, including six unactuated degrees specifying body position and orientation, a two DOF universal joint at each hip and a hinge joint at each knee.
The organization of this paper is as follows: in section 2 dynamics and kinematics of the quadruped robot is discussed. Section 3 presents the implementation of the robot; section 4 discusses the Stable trajectory planning of the quadruped using ZMP criteria. Simulation results are presented in section 5 and finally conclusions close the paper.

Dynamics and Kinematics of a quadruped robot
In this section, inverse kinematics of a quadruped robot will be discussed. A mechanical arm consists of some links which are connected via joints. Joints could be of revolute or prismatic type. Each joint has one degree of freedom and for each joint an integer is assigned. A mechanical arm has n joints and n+1 links. Links are assigned integers from 0 to n and joints from 1 to n. This way, the i-th joint connects the (i-1)-th link to i-th link. When a joint is moved, the corresponding link also moves accordingly. Each joint is also with a variable .
To analyze the kinematics of the mechanical arm, coordination system is assigned to each link ( ). The coordination ( ) is assigned to the body of the quadruped. The matrix which is a function of and transforms the ( ) coordination to ( ) is as follows: (1) In which is the coordination of the origin of the ith coordination and is the rotation matrix of the ith coordination. By defining matrix H the coordination of end effector is found in a inertial framework as follows: (2) If the coordination of a point in n-th coordination system is multiplied by matrix H, the coordination of it will be found in the stiff framework. So if we have the joint variables the coordination of final points will be found in the reference framework.

2-2 Denavit-Hartenberg (DH) convention
To implement the forward kinetics in a systematic manner, DH method will be used. In this method, the coordination systems are chosen according to the following criteria [7] a) The axis is perpendicular to the axis b) The axis is intersects the axis These two criteria are sketched in figure (2). In fig (2), a is the distance between and measured along the , α is the angle between and which is measured in the plane perpendicular to , d is the distance from the origin measured along and θ is the angle between and measured in a plane perpendicular to the . Fig (3) shows an implementation of DH method for SILO4 [8] quadruped robot.

2-3. Dynamic modeling of a mechanical arm with n links.
In a robot arm with n links, the kinematic energy is calculated using the following equation: (4)(37-2) Which could be rewritten as: In which D is the inertia matrix which is a positive definite matrix that depends on mass configuration of the robot and J is the Jacobian matrix. The potential energy is calculated using the following equation: (7) (40-2) In which g is a vector that shows the direction of gravity in inertial framework and is center of mass of the link. Using Lagrangian method, the general mechanical equation of the robot arm is as follows: (8)(41-2) In which C could be calculated as follows:

Implementation of quadruped robot
Implementation of the quadruped robot is done based on equation (8) and (9) [7,9]. The movement of the robot is analyzed in two phases: steady and mobile [10,11]. In mobile phase due to similarity of legs, the equation for each leg is identical. To ignore the effects of impact of leg to the ground when changing phases, it is assumed that in the moment of changing phase from mobility to steady, the velocity of leg when hitting the ground is zero so there will be no need to model dampers and trots of ground.

3-1 derivation of equations
The quadruped has 18 degrees of freedom (DOF), 3 DOFs for center of mass of the quadruped, 3 DOFs for spatial angles of body, and 12 DOFs for four legs each has 3 DOFs. If λ represents the constraint forces we will have (10)(43-2) Here constraint is the immobility of legs while on the ground. With straight forward algebra the following equation will be derived which formulates the dynamics of the quadruped robot. ) (11)(47-2)

3-2 simulation of the model
Solving equation (10), by applying moments as an input for robot, motion of the robot could be simulated. MATLAB software has been used to perform the simulation. Proportional Derivative (PD) has been to control the applied moment and angle of the legs of the quadruped. 4. Trajectory planning In this section the trajectory planning of the quadruped will be discussed. The following procedure will be applied to plan the trajectory: 1. Position of key points of the robot will be determined. Key points include center of mass of body and tips of the legs. 2. Trajectory equations will be derived parametrically for key points so that the equations and their derivatives are continuous two times. Bezier Curves will be used for this purpose. 3. Using inverse kinematics and derived trajectories, angles for all joints are calculated.

3-3
Bezier curves The simplest form of Bezier curve is a third order function which is shown as follows: Derivatives of equation (12) with respect to u will give the slope of function in initial and final points.

Stable trajectory planning using ZMP criteria
In this section free parameters of trajectory will be determined using ZMP stability criteria so that the robot has dynamic stability. This approach is valid when three legs are on the ground at a time. Fig (5) shows a depiction of a biped robot. P is the linear momentum and H is the angular momentum with respect to origin of frame of reference. Center of mass of body and links are and respectively. We have: (14) (1)(2)(3)(4) (15)(2-4) In which and are angular inertia matrix and angular velocity respectively in inertial frame of reference. The following equation is valid for angular inertia matrix in inertial frame of reference and angular inertia matrix in the frame of reference attached to the body of the robot:

4-1 ZMP stability criterion
(16)(3-4) In which R is rotation matrix of attached frame of reference with respect to inertial frame of reference. Taking derivative of equation 15 yields: (17)(4-4) Coordination of point of ZMP is derived using following equations: If ZMP is inside the support polygon then the stability of the robot is guaranteed. If ZMP is outside the support polygon then by the parameters ZMP will be placed inside support polygon and stability is ensured.

4-2 Analysis of critical ZMP points
In this subsection critical ZMP points in which stability is deteriorating is analyzed. This is due to difficulty and in some cases impossibility of direct mathematical analysis of ZMP locus. So critical points in which ZMP falls out of support triangles are analyzed. Fig (5) shows the simulation of ZMP locus. Blue polygonal are support polygon and red curves are locus of ZMP. It can be seen in some points, ZMP falls out of polygonal of leverage.

4-3 Free Parameters
In each gait, step property information is acquired from the operator which could be a human operator or a program.
Step information includes length and height of step and speed of the robot. Using these information step parameters are step trajectory and value of all angles. Details of some of step parameters are shown in table (2). Movement of body along x axis in phase 1 xdh1 Velocity of body along x axis at the end of phase 1 xdd1 Acceleration of body along x axis at the end of phase 1 yh1 Movement of body along y axis in phase 1 yh2 Movement of body along y axis in phase 2 yh3 Movement of body along y axis in phase 3

xa1
Movement of tip of leg along x axis in phase 1 za1 Movement of tip of leg along z axis in phase 1 The movement of the robot is such that in phase 1 all legs are on the ground and the body of the robot moves; forward and lateral. The lateral movement is done so that the robot leans to a direction to be able to lift the leg on the other side. In phase 2 the robot lifts the leg and in phase 3 puts it back on the ground. Fig (6) shows the numbering of legs and x and y axis. To design a stable trajectory for the robot, yh1 and per are assigned as free parameters. In phase 2 and 3 in which legs are moving along with the body, the robot has no lateral movements. Fig (7) shows three different sequences for gait of quadruped and corresponding support triangle. As can be seen, in sequence B and C polygonal of leverage have more surfaces in comparison with sequence A. on the other hand, in sequence C, the quadruped has to lean to sides (lateral movements) in each step. So sequence B has been chosen for the purpose of this paper to avoid excessive lateral movements and accelerations.

Simulation results
In this section the quadruped robot is simulated and the trajectory planning according to ZMP criterion is done. Matlab and Mathematica software are used to run the simulations. ZMP criterion is analyzed separately for body and legs of the robot.

5-1 Effect of body movement on ZMP stability
As it has been mentioned in section (xxx), in each lateral movement of the robot there is a vertical peak (dip) in ZMP route. Consider a case in which the robot is on legs 3 and 4 and wants to lean toward minus part of y axis in order to move legs 1 and 2. According to Cart-table model [12], when the robot leans toward negative part of the y axis, it starts from zero acceleration and in one point it reaches the maximum acceleration (absolute value) with negative direction and using Cart-table model, the first peak is the result of maximum acceleration of toward legs 1 and 2. Using same logic, the second peak (dip) is the result of maximum acceleration with minus sign in order to be able to stop when reaches legs 1 and 2. Since we have a negative acceleration, according to Cart-table model, ZMP is in front of robot. Peak (dip) of ZMP will be formulized for body movement. Since there peak 1 and peak (dip) 2 are symmetrical, we will formulize peak 1 without losing generality. Using Mathematica software which is very powerful in symbolic computation, peak 1 will be formulated. The constraint to ensure ZMP stability is that the peak value of ZMP should not exceed y=25 which is the margin of support polygon. Trajectory procedure of section (3) will be used and this procedure uses Bezier curves to implement the body movement. Taking second time derivative of this trajectory and finding the maximum acceleration, the first peak will be calculated. In phase 1, the robot body will move from yh1 to -yh1 along the y axis. Two parameters, yh, per and t will be influential on lateral movement of the robot. Using analytical solution, time (t) could be derived from other two parameters (yh, per). (20)  Considering the stability constraint on the value of ZMP in lateral movement (max(ZMP)<25 ) the criterion for yh so that the lateral movement of the robot would be stable will be: (21)(13-4) In which is the ZMP value of the body in terms yh and a function of per. The value of yh will cancel out in eq. 21.

5-2 Effect of swinging leg on ZMP stability
In previous subsection, we studied the effect body movement on ZMP stability. In this subsection we will focus on horizontal peak of ZMP. According to fig (5), the horizontal peak occurs when the robot has no lateral movement and one leg swings. It could be inferred that horizontal peak of ZMP is the result of acceleration of the swinging leg. Fig (10) shows the value of ZMP in time with respect to different values of yh and per. The maximum of ZMP in fig (10) in a constant per, is somehow independent of yh. In other words, the time that ZMP reaches it maximum value is independent of how much the body of robot has moved in lateral direction but it will affect the value of ZMP. Fig (11) shows the value of ZMP as a function of time with yh=0 with different per values. The peak value is also shown in brackets.   (12) shows the geometrical relation between and and polygon of support. Using straight forward algebra the minimum value of yh will be found. Fig (13) gives the minimum value of yh with different per values for the robot to be stable. For example, for per=0.2, the minimum value of yh is equal to 7.6 and the maximum value is 2. So for per=0.2 we will not have any yh value to stabilize the robot using ZMP criterion. Fig (14) shows the ZMP route for per=0.2 For per=0.4 the maximum value of the yh is 8.43 and the minimum value is 8.252so we will choose 8. 3. Fig (15) shows the ZMP route for per=0.4 and yh=8.3. It can be seen that the horizontal protrusion is negligible and the robot is stabilized. If the value of per is set to 0.8, the maximum and minimum of yh are 15 and 11.1 respectively and fig (16) shows the ZMP route for yh=12 and 14.5.

Conclusions
In this paper a novel method for gait trajectory design of a point legged quadruped robot with 18 degree of freedom on a flat surface has been proposed. The exact model of the mechanical parts of the robot including four legs and the body and all joints have been derived and used in gait trajectory planning. Inverse dynamics of the quadruped were formulized using Denavit-Hartenberg (DH) convention. Bezier curves were used for trajectory planning. Effects of body and legs on Zero Moment Point (ZMP) have been analyzed separately. By manipulating free parameters, a stable gait trajectory was designed. Simulations were done using Matlab and Mathematica software.