Whole-body kinematic and dynamic modeling for quadruped robot under different gaits and mechanism topologies

Dynamic locomotion plays a crucial role for legged robots to fulfill tasks in unstructured environments. This paper proposes whole-body kinematic and dynamic modeling method s based on screw theory for a quadruped robot using different gaits and mechanism topologies. Unlike simplified models such as centroid or inverse pendulum models, the methods proposed here can handle 10-dimensional mass and inertia for each part. The only simplification is that foot contact models are treated as spherical joints. Models of three different mechanism topologies are formulated: (1) Standing phase: a system consisting of one end-effector, the body, and four limbs, the legs; (2) Walking phase: a system consisting of one or two lifting legs (depending on the chosen gait), two or three supporting legs; (3) Floating phase: a system in which all legs detach from the ground. Control strategies based on our models are also introduced, which includes walk and trot gait plans. In our control system, two additional types of information are provided: (1) contacting forces are given by force sensors installed under feet; (2) body poses are determined by an inertial measurement unit (IMU). Combined with the sensor data and calibrated mass, inertia, and friction, the joint torque can be estimated accurately in simulation and experiment. Our prototype, the “XiLing” robot, is built to verify the methods proposed in this paper, and the results show that the models can be solved quickly and leads to steady locomotions.


INTRODUCTION
Compared with wheeled and tracked mobile machines, legged robots have apparent advantages when working in unstructured environments. In the past few decades, remarkable improvements have been witnessed in the agility and mobility of legged robots. For instance, the biped Atlas (Kuindersma et al., 2016) and quadruped BigDog (Raibert et al., 2008), both actuated by hydraulic systems, and the quadruped spot mini driven by electric motors have demonstrated their capabilities in highly complex motions. In addition, the quadruped HyQ designed by Semini et al. (2011), which was further improved by Hutter et al. (2016) manifested itself in terms of excellent dynamic Delossantos-Lara, 2018; Du, Fnadi & Benamar, 2020), especially for those involving parallel mechanisms. One advantage of the screw theory is that it can simplify the robot coordinate system and make the solution faster. In light of the screw theory, the authors in Chen et al. (2015) proposed the fault-tolerant gait to deal with the kinematics problem containing mechanical faults.
In this paper, a whole-body dynamic modeling method for quadruped robots is proposed, which can make a quadruped robot realize real-time motions of a loop in 1 ms and generate more stable movements. This paper mainly contributed as follows: 1. A novel modeling method for quadruped robots is proposed, and based on screw theory, both kinematic and dynamic models can be formulated elegantly. 2. A model-based control strategy is proposed, which can improve the dynamic response performance of the robot.
3. We integrate the proposed model and plan on the "XiLing" robot, which has high dynamic response performance in a complex environment. Various simulations and experiments are carried out to validate the method's effectiveness.

SYSTEM OVERVIEW
This paper introduces a new modeling method for quadruped robots based on screw theory to improve the dynamic performance in a complex ground environment. Our design, "XiLing" is shown in Fig. 1. To increase the carrying capacity of the robot, we use carbon fiber and aluminum alloy to reduce the overall weight without losing strength. As shown in Fig. 1, each leg has three degrees of freedom. The abduction/adduction joint contributes to the leg motion in the frontal plane. The hip and knee joint commonly relate to the leg motion in the sagittal plane. The shank uses a pulley to drive, which the reduction ratio is 1:2. The robot can move in all directions with a walk or trot gait. The whole-body dynamics model is built to analyze the dynamics characteristics during walking. In addition, we design and assemble the motor drive module and add the brake system to ensure the safety of the robot itself and the operator. Moreover, force sensors are placed on the toes of each leg, which can sense the ground reaction in real-time. Thus, we can analyze the changes in external terrain and the position and posture of the robot body in combination with the IMU. Based on the perception of the external environment and itself, the current state can be comprehensively estimated. The corresponding dynamic model will be selected according to the topological structure at this time. To calculate the torque of the joint at this time. Then the MPC controller will re-plan the walking gait and trajectory so that the robot can walk smoothly. The control framework is shown in Fig. 2.
As we know, the legged robot has to face the extraordinarily complex and changeable external environment in walking, and it needs to respond to the changes of the external environment. Otherwise, the feet may have landed, and the algorithm has not been solved yet; thus, the algorithm has lost its due function, which is also one reason why no one has established the whole-body dynamics model of the quadruped robot at present. We use the screw theory to develop the dynamic quadruped model. It will significantly simplify the establishment of the coordinate system and improve the model calculation speed and efficiency. The following several sections will detail the modeling process of kinematic and inverse dynamic.

KINEMATICS
Solving kinematics is the foundation of dynamic calculation. Before solving the dynamics, we need to finish the kinematics to obtain the position and velocity of each link. This section will introduce the definition of the coordinate system, inverse kinematic, and forward kinematic. It will provide the theoretical basis for subsequent modeling and planning.

Definition of coordinate system
The link's rotation axis, velocity, and inertia are expressed differently in different coordinates. So, it is essential to define a unified coordinate system. Figure 3 shows the definition of the home position and the coordinate of body center, leg, and ground. The leg frame is defined at the intersection of the joint HAA (hip abduction/adduction) and HFE (Hip flexion/extension) axes in the same direction as the body frame.

Inverse kinematic
The inverse kinematics is given based on the leg frame's representationx in the leg frame to calculate the joint rotation angleq. Figure 3 shows the coordinates and steering definitions for each joint of the leg. Before solving the problem, we simplify the calculation by limiting the actual walking condition of the robot. Assume that the robot's toes will not reach the top of the body, that is: First, q 1 is calculated by projecting onto the YOZ plane. In the YOZ plane, the projection of point C is D. We can obtain Eq. (2) by considering the geometrical relationship: a ¼ arc cos Here α is the angle between DO ! and Z, and β is the angle between AO ! and DO ! . Figure 4 shows the geometric relationship of the ends under different positions. We can use this to calculate q 1 according to Eq. (3).
During the robot locomotion, the upper and lower links are always in the same plane. This plane is used as a new study plane to calculate q 2 and q 3 . The new origin of the coordinate system was transferred to joint HFE, as shown in Fig. 5. The conversion formula of the old and new coordinate systems is Eq. (4): Then the problem was transformed into solving the inverse kinematics of the planar two-link mechanism. According to the trigonometric relationship, we know that: Here ϕ is the angle between AC ! and the x-axis and φ is the angle between AC ! and AB ! . As shown in Fig. 6, when the end effectors are in a specific position, there will be different solutions due to different ways to bend the legs.

Forward kinematic
Forward kinematics is to calculate the expression of all links relative to the ground frame given the initial position and joint rotation angle. Because the robot is a floating base system, we cannot directly find the expression of the relevant parameters of the robot in the ground frame. So we first take the center of the robot body as the reference frame to solve the presentation of all links. And then, we transfer them to the ground coordinate according to the conversion formula of the body and the ground coordinate system. Screw theory is used to build a kinematics model. See Appendix A for the meaning of all symbols used in this paper.
In the screw theory, the velocity screw V and screw axis S of the rigid body is defined by a pair of vectors that is: The screw axis S defines the position and positive rotation direction of the revolute joint. The first vector is the position on the ground coordinate frame, and the second vector is the direction of rotation. V represents the velocity and angular velocity of the rigid body.
In this paper, all the joint and link parameters are transferred to the body frame B for analysis. So, we assume that the body coordinate frame is relatively stationary when we solve the forward kinematic. The forward position model solves the end-effector position on the frame B from the inputs angle of every joint. Before computing the angle, the initial position of the robot should be determined to determine the initial velocity screw as follows: We can obtain the initial velocity screw of the joint computed according to the screw axis S ij using Eq. (9). Whereŝ is the unit velocity screw. Such as the joint HAA of leg1 iŝ Then we can get the end-effector position as Eq. (10) and the Jacobian matrix in Eq. (11).
where i = 1, 2, 3, 4 is the leg number of the robot; P is the homogeneous transformation matrix when the joint rotation θ. S ij is the initial velocity screw of the joint computed according to the screw axis in home position.

INVERSE DYNAMICS
Dynamics is to solve the joint torque when the current motion state and external force are known. The position and velocity of all the links and joints are required before the dynamics. The previous section addresses these issues. Additionally, the inertia and the constraint matrix are also known because these two parameters are only related to the robot's position.
In this paper, all the joints are revolute joints. Each joint has five dimensions of constraint and one dimension of motion. Therefore, we can define the constraint matrix and moment matrix according to the Plucker basis coordinate system. At the home position, it is expressed as Eq. (12).
At any given time, the constraint matrix depends only on the position of the joint. According to the forward kinematics, the homogeneous transformation matrix can be obtained according to Eq. (10). And then the constraint matrix at any time can be obtained by Eq. (13): For the inertia of the linkage, we can first get the inertia of the linkage at the center of mass. Then, according to the position relationship between the center of mass and the ground, Eq. (14) is used to solve it.
where T f and P are isomorphic and only depends on the position of the links. I ijo is the initial inertia details in Appendix B. Moreover, we can combine G I ij to a big matrix: The solution of velocity is related to the model of dynamics. During the walking phase, quadruped robots mainly have three situations: four-leg landing on the ground, two or three landing on the ground, and four suspended in the air. Respectively corresponding to the stand, walk and bound. In this paper, different states of robots are divided into different topological for analysis. Furthermore, the dynamics model is different under different topological mechanisms. In the state of stand and walk, the robot is a fixed base system, and the velocity of all the links can be obtained by establishing a constraint matrix. Then it can be substituted into the dynamic model to calculate the joint torque; in the bound gaits, the robot is a floating base system, which the constraint matrix is singular. Here, we first get the acceleration according to the last time velocity, then use the integral method to get the next velocity. As shown in Fig. 7, the topology is constructed for these three cases. Figure 7A shows that the robot jumps and is free from the ground reaction. Figures 7B and 7C show the external forces on the robot when it walks in the trot or walking gaits. Figure 7D shows the force analysis of the robot when it stands, and all four legs are in the support phase. Next, we will introduce how to build the constraint matrix and solve the dynamic model. Fig. 7D, When the robot stand, there is only one end-effector, the body. It can realize the action of squatting and standing up and the three-axis attitude transformation of roll, pitch, and yaw. In this case, each robot's leg is connected to the ground as a passive spherical joint, and the three-dimensional translation is constrained. The body is the only end effector, and the robot can be regarded as a parallel mechanism. The force of each joint on its parent connecting link is defined as negative, and the force on the child connecting link is defined as positive so that the constraint matrix can be established.

Show in
Once we have the constraint matrix for each joint, write the constraint matrix for all joints and links as a larger matrix C.  According to the formula of constraint matrix, C ij represents the force of joint j on link i. For constraint: C ij ¼ G J ij cmo ; For motion: C ij ¼ G M ij cmo . Each of these rows is a linkage, there are 14 of them, and each of these columns is a joint. For the same joint, constraints and motions are considered separately. We knew that C is a sparse matrix, and we can use unique algorithms to calculate it to improve the computational efficiency of dynamics. We know that by conservation of energy in Eq. (16).
where, C v represents the power of joint rotation, and v represents the speed of all the links. Take the derivative of both sides: For any linkage, the force equilibrium condition satisfies Eq. (18): By combining the equilibrium equations of all the links and Eq. (17), we can take the dynamic Eq. (19) of the whole-body dynamic: Here a is the acceleration of all links and η is the forces of all joints, including constraint and driving force.

Walk
Quadruped robots can walk in various gaits, such as walk, trot, pace, gallop. The main differences between gaits are the order and the time of the stride and the duty cycle of the swing phase. Regardless of the gait, there always exists a supporting phase and a swinging phase at any given moment. The dynamic model was constructed with the contact point between the support and ground as a spherical joint and the swinging leg and body as the end-effectors. This section introduces how to build a constraint matrix by taking Walk-gait as an example. The methods are the same for other gaits. We need to analyze which leg is in the swing and which portion supports and modifies the constraints.
When the robot walks in Walk-gait, at any time, there are three legs in contact with the ground and one leg in the air. There are four different topologies. Figure 7B shows the third leg in the air. This section analyzes only this case, and other issues are similar. Among them, the 1st, 2nd, and 4th legs contribute to the support contact with the ground and support body movement. The contact points between the toe and the ground can be regarded as spherical joints, which constraining three-dimensional translation, but three-dimensional rotation is not restricted. Therefore, the constraint of a spherical joint can be obtained as follows: Furthermore, the constraint matrices of all the joints can be combined into a larger matrix C to represent the robot's force. The establishment and solution of the dynamic model are consistent with the above Stand topology, which will not be described in detail.

Bound
The Bound-gait differs from the above two. It is a floating base system, including five end effectors. The robot belongs to an unconstrained mechanism at this moment. Note that, the constraint matrix corresponding to the force of the robot is a singular matrix. That makes it impossible to calculate the velocities of all the links by calculating the constraint matrix.
And then, in the next loop, we can obtain the spatial velocity by numerical integration. Although the acceleration of the link changes from time to time, we use high-frequency real-time control, which can be calculated at the frequency of 1,000 Hz. The speed can be refreshed quickly according to the change of the link acceleration. The control effect has been achieved continuously and steadily.

VERIFICATION OF KINEMATIC AND DYNAMIC MODELS
The above section introduces the whole body kinematic and dynamic models of a quadruped robot. In our control framework, the kinematics model will be used to transform the trajectory of end-effectors from Cartesian space to axial space. Dynamic models will be used for control and planning, making the robot regenerate the control parameters of the next real-time cycle according to the feedback information. This section will introduce how to generate the trajectory in walk and trot gait, and verify the models in simulation and experiment.

Motion plan
According to bionics research from Polet & Bertram (2019), for tetrapod, there are different gaits at different speeds. The walking gait is used when motion at a slow pace, using the trot gait when moving faster. Furthermore, use the bound gait when chasing prey or running away. The main difference between different gaits is the order of the stride and the time cycle of the support and swing legs. This paper has mainly introduced the planning of the walk and trot gaits at a slow speed to verify the correctness and feasibility of the model. Figure 8 shows the stride order, stride length, and time to duty ratio in these two gaits. An elliptical trajectory transition is used between two steps, where step length, step height, and step time are adjustable. During the acceleration and deceleration, the displacement of the body is as half as the length of other times. So, we set the step length at the beginning and the end to be half a step, but the duration is the same.
To make the robot walk smoothly and stably, the trajectory needs to satisfy certain constraints, such as the position and velocity should be continuously differentiable. For this purpose, the planning uses the trapezoidal curve, making each step of walking experience a process of acceleration, uniform speed, and deceleration. Equation (22) is the functional expression of the trapezoidal curve, and all following trajectories are planned on this basis. In addition, different curves can be generated by setting different velocity and acceleration.
A legged robot is a floating base system that needs to plan its four toes and body simultaneously. First, as shown in Eq. (23), is the trajectory planning of feet. Between every two footholds, the robot use elliptical trajectory transitions. Second, the body's trajectory is determined by the number of steps and the position of the foothold shown in Eq. (24). Here L is the step length of forwarding direction; W is the step length of right and left directions.
x body ¼ T 2 þ L n À 1 2 8 > > > > > > > > > < > > > > > > > > > : Figure 9 shows the trajectory of leg and body in Cartesian space when robot walking with trot and walk gait. These trajectories are expressed in-ground coordinate systems. We can use Eq. (25) to transfer it from G frame to Leg frame. Then the joint rotation angle was calculated by the inverse kinematics. Sent the angle to the corresponding motor can make the robot walking with a planned gait.

Kinematic simulation
This paper use ADAMS to verify the kinematic and dynamic models. The physics engine of ADAMS has high computational precision. It can accurately reflect the ground reaction force of the robot and joint output torque. It can provide a theoretical basis for the mechanism design and hardware selection. The body and links are made of aluminum alloy in the simulation, close to the prototype used. The only difference is that the prototype has actuators, IMU, force sensors, batteries but look at them as a whole with the body in the simulation. We simplified the model and equivalently added all of these masses to body mass, ignoring the influence of its inertia. Figure 10 shows the schematic diagram of a quadruped robot walking in the trot and walk gait.

Dynamic simulation
When the robot is in motion with a walking gait, three legs are always in the support phase and another leg in the swing phase. At this time, the body is stationary relative to the ground. Figure 11 shows the torque curve of the knee joint of the swing leg during the switching between the swing phase and the support phase. It can be seen that the calculation results of the dynamic model are the same as the simulation results. It is shown that the dynamic model has high computational accuracy and veracity under this topology structure. In addition, the torque of the swing leg's joint will suddenly change at the moment of switching. That is due to the disappearance and appearance of external forces, leading to a sudden change in joint acceleration.

Experiment in prototype
Our experiments were conducted on "XiLing", a high dynamic response quadrupedal robot used an embedded PC with an Intel Core i7 4500U, running Ubuntu 16.04 (Linux-4.9.90 kernel) with Xenomai3 patch. The level communicates at 1 kHz over EtherCAT and control signals are generated in a 1 ms control loop that runs on a dedicated onboard industrial PC. The robot is driven by 12 identical motors, which are actuated by Elmo driver and are powered by a 48 V lithium battery. Table 1 shows motor performance parameters, with a continuous torque of 40 N·M and maximum torque of 108 N·M. In the prototype, the shank is driven by a gear belt with a reduction ratio of 1:2. The torque bearing capacity of the knee joint can reach 216 N·M, which completely satisfied the maximum torque requirements of the simulation. In other periods, the joint force is below 40 NM, which is lower than the continuous output torque of the motor. The robot can quickly switch any posture in its workspace while standing. Figure 12 shows the basic movements such as roll, pitch, yaw. Respectively rotation angles of them are ±5°, ±10°, ±20°. The trapezoidal curve is used to complete the planning during the movement, and the time of the switching process can be adjusted by modifying the acceleration and the maximum speed. Figure 13 illustrates a snapshot of the yaw angle changing from −15°to 15°within 0.9 s. Figure 14 shows the tracking curve of knee position and velocity during this movement. With 1,000 points interpolated per second, both  allow for fast and accurate tracking. It is worth noting that the measured velocity curve has burrs because of the gear backlash and the friction between the belt and pulley. Burrs can be eliminated by choosing more precise gears and optimizing the planning control algorithm. We will prioritize this problem in the next step.

CONCLUSION AND FUTURE WORK
This paper presented whole-body kinematic and dynamic modeling methods for quadruped robots based on screw theory. Compared with traditional LIP or centroidal models, 10-dimensional mass and inertia of all parts are considered, which means higher precision. We divided the model into three phases: stand, walk, bound. Controller and plan strategies based on these models are proposed under each state. The motor torque curves of different gaits are calculated and compared to simulating software, which shows that computing and simulating results are identified. Prototype experiments of the standing phase are provided, and it turned out that the measured curves are very close to theoretical ones. In the future, we will focus on dynamic parameter identification. Contact models should also be improved via considering friction to get better results under slipping conditions.