Abstract
The quadruped robot is subject to self and external interference during the running process. In this paper, in order to improve the stability of the quadruped robot, a disturbance suppression strategy based on the kinematic model and the virtual model is proposed. Through the whole-body kinematics modeling of the body, the cause of the disturbance is analyzed. At the same time, two spring-damping virtual elements are introduced on the body. The virtual force generated by the spring-damping model is according to the displacement and speed of the CoM (center of mass) when the robot is moving. By simulating the tort gait of the robot, sorting out the data obtained from the experiment, and drawing a comparison curve with the experiment results of the no spring-damping virtual element under the same conditions, it is proved that the method proposed in this paper has a significant effect on maintaining the dynamic stability of the body, and the actual trajectory is not different from the theoretical trajectory.
1. Introduction
In the field of mobile robots, legged robots have great advantages in flexibility compared to crawler mobile robots and wheeled mobile robots [1]. This is reflected in two aspects: crossing obstacles and passing through complex terrain more easily [2–5]. According to the number of legs, legged robots can be roughly divided into three types: hexapod, quadruped, and bipedal. Compared with the other two types of legged robots, quadruped robots have the advantages of fewer motion mechanisms, strong body load capacity, and better dynamic stability of the body. Therefore, the actual working environment of quadruped robots is mostly in unknown terrain exploration search, the rescue of people in the disaster area after the earthquake, and other complex environments [6–8].
During the walking process of a quadruped robot, the foot trajectory planning is relatively mature, including single gait and multiple gait transitions [9]. In addition, the stability of the robot during walking also needs to be considered [10–12]. Without considering the contact force generated when the foot is in contact with the ground and the force exerted by the external environment on the robot body, the motion trajectory of the center of mass (CoM) of the body is consistent with the robot motion trajectory. However, in the actual working environment of a quadruped robot, the impact force generated when the foot contacted the ground, or the force exerted on the robot by the external environment will cause the CoM trajectory to deviate from the motion trajectory and float within a certain range. As the impact force generated by the foot contacts the ground or the external force exerted on the body increases, the degree to which the CoM deviates from the motion trajectory increases, which will damage the stability of the robot when it moves. Generally, the situation that causes the CoM to deviate from the motion trajectory is called interference. The reasons for the interference are as follows: the sudden increase or decrease of the body load, the excessive acceleration of the foot when landing, the sudden force exerted by the external environment on the body, and the decrease of the friction force between the foot and the ground due to the wet. [13–15]. When the interference is beyond the endurance range of the robot, the robot will lose stability or even cause the robot falling down. In this case, the robot will not be able to complete the specified task, and even cause hardware damage. [16]. In order to enable the quadruped robot to complete the task of motion planning smoothly, the control system of the quadruped robot should have the ability to recognize the disturbance during the process of moving and compensate for the changes in the posture of the body caused by the interference. The current quadruped robot motion planning papers have proposed different solutions to this problem [17–19].
Scholars have conducted research on body disturbance. The quadruped robot is a very complex system. Due to the error of the mathematical model, it generates self-disturbance during the walking process. In order to improve the accuracy of the control model, Martinez et al. [20] proposed a linear robot model with a gravity term, which can be used to determine the controllability and observability of the robot. This method avoids the requirement of structural conditions, reduces the approximate error in the control model, and can be used for any robot with traditional structure. The simulation results show that this method has higher accuracy in obtaining the control model. When a quadruped robot traverses a complex environment without relying on visual sensors, it is necessary to plan the movement trajectory of the CoM in advance and consider the stability of the body [21]. Park et al. [22] utilized the redundant degrees of freedom of the quadruped robot legs to complete the motion and manipulation tasks of the quadruped robot, and improved the overall stability by changing the body posture. The experimental results show the effectiveness of the method. In order to identify external disturbances, Tian et al. [23] used the leg joint driving force and the inertial measurement unit (IMU) parameters as input quantities, through the dynamic model calculation, the external disturbance is expressed as a 6-DOF wrench exerted on the CoM. Floriano et al. [24] introduced a multi-objective genetic algorithm into gait planning. The main function of this algorithm is to optimize the motion parameters in gait planning. At the same time, using kinematics and empirical methods to control the balance of the body, the external disturbance caused by the motion can be well solved. Khorram and Moosavian [25, 26] proposed a framework for restoring robot balance against unknown external disturbances. The premise of constructing this framework is that the quadruped robot is a multi-degree-of-freedom system. By enhancing the whole-body dynamics model, the balance controller is defined. After the robot is disturbed, the controller calculates the acceleration needed to restore the body to a steady state and then makes full use of the multi-degree-of-freedom characteristics of the quadruped robot to keep the robot in a stable state. The simulation results show that when the robot is disturbed, the frame can adjust the position of the body to maintain the stable state. Edrisi et al. [27] changed the posture of the body to resist interference by fusing sensor feedback information. In the experiment, the stance leg uses these fusion data to compensate the body posture deviation and achieve the purpose of stabilizing the body. Although the above methods can achieve effective suppression of body disturbances, they are mostly based on joint force control modes. Because hydraulic robots mostly use flow servo valves as joint control components, the pressure gain is relatively large, making it difficult for quadruped robots to use joint force control. Therefore, existing hydraulic quadruped robots mostly use joint position control mode [28]. In the position control mode, the change of the body posture of the robot during the walk is not considered. At the same time, the realization of the above method also depends on the high-configuration platform, which cannot be realized on the low-configuration platform.
In order to be suitable for the low-configuration hardware platform, this paper uses the Jacobian matrix derived from kinematics and uses the displacement and velocity of the CoM as the control parameters of the virtual spring-damping model. A simple mathematical model can achieve significant control effects. In Section 2, the product of exponential (PoE) method is used to build the whole-body kinematic model. Section 3, through the first-order kinematics derivation and dynamics derivation of the robot, explains the reasons for using the virtual control model and analyzes the wrench generated by the virtual controller when dealing with interference. Section 4 describes trot gait planning. Section 5 describes a dynamic experiment on trot gait. In Section 6, the conclusion of this work is drawn.
2. Kinematic Modeling
This article chooses the PoE formula method [29–31]. Compared with the Denavit–Hartenberg (D-H) parameter method [32–34], although there are more parameters, but the link coordinate system can be chosen at will. Once the robot’s zero position is given, and the base coordinate system and the end coordinate system are established (unlimited), the PoE formula is completely determined, and its physical meaning is more intuitive.
2.1. Building a Kinematic Model for a Single Leg
The robot kinematic model is constructed using the PoE method, the coordinate system is established as shown in Figure 1. The body length, width, and height were , , and , respectively. The length of the robot hip link, thigh link, and calf link is , , and , respectively, where is the number of each leg corresponding to right front leg (RF), left front leg (LF), left hind leg (LH), and right hind leg (RH). The joint angle , , and corresponds to the hip joint, thigh joint, and calf joint. All coordinate systems in Figure 1 are established according to the right-hand rule. Establish the body coordinate system at the CoM of the robot. At the ground projection of the initial CoM, establish an inertial coordinate system and establish a reference coordinate system at hip joint and foot coordinate system . The parameters required for calculation are shown in Table 1.
First, determine the pose of the foot in the reference coordinate system when joint angles where is
According to the theorem, any rigid link movement can be realized by moving around a fixed spiral axis in space is the direction vector of spiral axis, and is the linear velocity of the rigid link in space around the spiral axis . The form of expressing the spiral axis as a matrix iswhere is the skew-symmetric matrix of
The forward kinematics of a single-leg needs to obtain the position of the pose in the reference system , so the spiral axis needs to be expressed in , and the relationship between and iswhere is any point on the joint axis, measured in the reference coordinate system . This article is aimed at a quadruped robot with a front elbow and hind knee structure for the legs, so the joint axis vectors of the thigh and calf in the front and hind legs are opposite. Table 2 lists the spiral axis of each joint in the reference coordinate system.
in Table 2 is given by
Take the right front leg as an example, the transformation matrix of the foot and the reference coordinate system can be obtained aswhere , , and are scalar quantities representing changes in hip joints, thigh joints, and calf joints.
PoE can be expanded into a homogeneous transformation matrix as
The single-leg inverse motion can be simply described as knowing the coordinates of the in the reference coordinate system and finding the corresponding joint angle. By observing Figure 1, the analytical solution of the joint angle can be derived from the geometric relationship of the legwhere is the position of the measured in the reference coordinate system , and the expression of is:
According to the analysis of single-leg inverse kinematics, when the position of the foot in the transition coordinate system is determined, the rotation angle of the three joints of the leg can be calculated at this time.
2.2. Whole-Body Kinematics Analysis
The single-leg kinematics analysis only considers the influence of foot position changes on the angle of each joint but does not consider the influence of body posture changes on the robot. Therefore, it is necessary to establish whole-body kinematics analysis. Observing Figure 1, it can see that the position of the in the body coordinate system at the initial position iswhere is
Different from single-leg kinematics, the spiral axis of the leg joints in the kinematics of the whole-body is described in the body coordinate system , as shown in Table 3.
When the joint variables of leg are given, the pose of the foot end in the body coordinate system can be derived as
3. Body Disturbance Suppression Strategy
3.1. First-Order Kinematics and Statics
The first-order dynamics analysis of the quadruped robot can get the relationship between the foot speed and the joint speed at a certain time. In this paper, the angular velocity and linear velocity of the rigid body are combined into a six-dimensional vector form, denoted as , called the twist, write in the form of an antisymmetric matrix, twist can be written in the form of a skew-symmetric matrix, and twist can be written in the form of a matrix
The Jacobian matrix can express the linear sensitivity of the end effector speed relative to the joint speed, which is a function of joint variable . The mapping relationship between the foot end twist and the joint speed is
The method of deriving the Jacobian matrix and the twist in the next part is the same as the method of PoE positive kinematics. The only difference is the joint variable in forward kinematics, which is zero, and is the variable in the Jacobian matrix. The Jacobian matrix between the foot end and the hip joint is described in Table 4.
Taking the RF leg as an example, the Jacobian matrix between the end of the foot and the hip joint can be written as
According to the principle of virtual work, the power consumption at the joints is equal to the power consumption performed at the end (legs are in a state of static balance, and there is no power consumption when the robot moves), that is,where is a six-dimensional wrench, and and must be expressed in the same coordinate system.
According to formulas (15) and (17), the relationship between wrench and joint torque in the reference coordinate system can be obtained as
The Jacobian matrix is traditionally expressed in body coordinate system , and it can be seen from Figure 1 that the homogeneous transformation matrix between the reference coordinate system and the body coordinate system is
According to formula (19), the adjoint matrix between the reference coordinate system and the body coordinate system can be derived as where is the skew-symmetric matrix of the distance between the origin of the reference coordinate system and the origin of the body coordinate system
According to formula (20), the relationship between the Jacobian matrix of the reference coordinate system and the Jacobian matrix of the body can be obtained as follows:
According to formulas (18) and (22), the relationship between the foot end force and the joint torque expressed in the body coordinate system can be obtained as
3.2. Dynamics Analysis
Robot dynamics is concerned with the forces and moments that cause the robot to move.
It is divided into two parts, forward dynamics and inverse dynamics, as in robot kinematics. This article uses Newton–Euler formula to derive robot dynamics.
3.2.1. Forward Dynamics
First, consider the forward dynamics of a single member of the robot leg and take the thigh member of the right front leg as an example.
The twist that moves is , the time-varying position of any particle on it is , and the position measured in the centroid coordinate system is . It can be deduced that the velocity and acceleration of the particle are
According to Newton’s second law, it can be deduced that the force and torque on arewhere is rotational inertia matrix at centroid, and the relationship with the other particles on iswhere is the mass of , is identity matrix and is the vector from the centroid of to the particle.
In order to use twist and wrench mentioned above, equation (26) is rewritten as a six-dimensional matrixwhere is spatial inertia matrix of , and is the Lie bracket form of , expressed as
According to formula (28), the acceleration of the component can be calculated under the condition that the wrench of the component and the joint state variables are determined. The derivation of the wrench of the component requires the derivation of inverse dynamics.
3.2.2. Inverse Dynamics
Inverse dynamics is the process of deriving body wrench and joint torque with known joint position , joint velocity , and acceleration .
Take the right front leg as an example. Frist, establish the inertial coordinate system at each leg link centroid, and foot inertial coordinate system is . The screw axis of joint is expressed as in the link coordinate system , and the CoM of body wrench is . According to these definitions, the twist and acceleration of each link can be deduced from the base to the foot.
The twist of link is determined by the joint speed and the twist of link
The acceleration of the link can be obtained by the derivative of time on both sides of equation (30)
When determining the wrench on the foot , according to formulas (31) and (28), each joint torque and body wrench can be reverse derivation
Since joint is a rotating pair and has only one degree of freedom, the torque of joint is
The wrench at the CoM is the vector sum of wrench applied to all foot
3.3. Virtual Spring Control Model
Dynamic analysis can obtain the wrench, joint torque, and component acceleration when the robot is moving. However, due to the large number of quadruped robot components and each joint has an independent actuator, body stability control using dynamics is not intuitive and difficult. Virtual model control is a way to use virtual actuators to replace real actuators [35], which can make control simple. In this paper, the wrench generated by the virtual actuator at the CoM is decomposed into the wrench generated by the virtual actuator on the support leg, and the torque generated by the joint on the support leg is obtained through the Jacobian. Add virtual springs and damping components to the body, as shown in Figure 2.
Two independent spring-damping actuators are added at the CoM in Figure 2. The Jacobian matrix can be used to obtain the mapping relationship between the wrench generated by the virtual controller and the joint torque. It should be noted that they must be expressed in the same coordinate system, and the expression form is similar to formula (23)where is the torque generated by the joints involved in the control of the body, and is the wrench acting on CoM.
In the process of quadruped robot walking, the legs in the swing phase are regarded as the disturbance factor of the body, and the legs in the stance phase are the control to the body steady by joint torque. In order to use the virtual control model, the following assumptions need to be made: (1) there is no flying phase in the tort gait, and (2) there is no slip when the foot is in contact with the ground.
After the body is interfered by the wrench , the CoM speed and position will change. The virtual controller at the CoM needs to generate wrench resistance with the same size and opposite direction
At this time, the wrench generated by the virtual controller of the front leg is and hind leg is . The front and hind legs have the same structure, so the virtual controller generates the same wrench, . The relationship between , , and is
According to the principle of virtual power control, the relationship between the torque generated by the stance leg joints and the wrench generated by the virtual controller iswhere and .
Use speed control for the and direction of CoM, and use position control for the direction of CoM. Set the spring coefficient as and the damping coefficient as , the desired speed in the forward direction is , the desired speed in the direction is , the desired position in the direction is , the wrench generated by the virtual actuator at the CoM iswhere and are the angles between the body virtual controller and the x-axis and z-axis of the coordinate system .
According to formulas (37)–(40), in order to stabilize the body, the moment generated by the leg joints of the stance phase is
4. Tort Gait Planning
The existing robot motion planning is divided into two types: model control and central pattern generator (CPG) [36]. Because CPG has many parameters, this paper uses model control. The current swing phase planning methods mostly use polynomial, ellipse method, compound cycloid method, Bezier curve method, and so on. The compound cycloid method is improved on the basis of cycloid, and the height of the middle section can be adjusted according to actual needs, which can ensure that the displacement, speed, and acceleration of the foot remain smooth during the swing process and avoid the contact of the foot with the ground.
4.1. Swing Phase Planning
During the movement of the robot, the legs have to go through the swing phase consisting of three parts: lifting, flying, and landing. If a large impact force is generated when the foot falls on the ground, the stability of the body will be affected. In this paper, the zero-impact trajectory planning method in reference [37] is adopted to reduce the impact force generated when the foot contacts the ground on the body.
hen
When where is the displacement of the foot in the inertial coordinate system , is the step length, is the max foot lift height, is the step cycle, is the swing phase period, and is the time of foot movement.
4.2. Stance Phase Planning
The horizontal trajectory of the swing phase is applied to the stance phase [38] as follows.
When
When the legs are in the stance phase, the feet are in contact with the ground, and there is no displacement in the direction and direction.
All the controls in this article are expressed in the coordinate system , so the displacement of the foot in the inertial coordinate system needs to be expressed in the coordinate system . The posture between the coordinate system and the coordinate system can be expressed aswhere is the unit rotation axis between and and is a scalar, indicating the amount of rotation around .
The origin of at the position of is , and the homogeneous transformation matrix between the and is
The position of the foot in the body coordinate system is
4.3. Design of Body Disturbance Suppressor
The disturbance suppression block diagram of the hydraulic quadruped robot is shown in Figure 3. , , , and are, respectively, the expected height of the body and the expected CoM velocity of the x-axis, y-axis, and z-axis body. , , , , , and are the displacement and velocity along the x-axis, y-axis, and z-axis of the coordinate system when robot moving. The robot starts to move from a stationary state according to the swing phase trajectory and stance phase trajectory planned by formulas (42)–(44). The IMU (inertial measurement unit) element on the body measures the movement speed and displacement of the CoM when the robot moves in real time. Determine which set of virtual control model to choose according to the difference between the expected displacement and the actual displacement. If the robot deviates from the desired motion trajectory, the control loop based on the virtual spring model adjusts the motion of the robot according to the real-time attitude feedback of the quadruped robot. The virtual spring generates additional acceleration that can obtain the acceleration required to stabilize the body and then obtain the torque required to support the leg joints. Determine the wrench generated by the virtual control model at CoM according to formula (40). Considering the stability of the closed-loop controller, this paper makes the following assumptions for the leg in the stance phase: (1) ignoring the influence of the swinging leg on the robot, in the actual motion process of the robot, the movement of the swinging leg can be regarded as disturbance; (2) assuming that the quadruped robot moves without the quadruped vacant state, the robot always has two legs in the same phase; and (3) assuming that the robot foot end is in contact with the ground without deformation or slippage.
5. Performing Simulation on Adams
In order to verify the effectiveness of the control method against body interference proposed in Chapter 4, a tort gait experiment was performed on a virtual machine. The experiment is divided into two parts: 1. suppress the robot tort gait disturbance; 2. in the process of robot tort, restrain the interference of external lateral force. The experiment platform is a joint experiment of Adams and MATLAB. The experiment setting time of the swing phase and the stance phase is equal, both are 0.5 s, the step length is 250 mm, and the leg lift height is 100 mm. The forward direction is along the x-axis. The static friction coefficient between the foot end and the ground is 0.6, the dynamic friction coefficient is 0.1, and the applied external force is 100 N.
5.1. Experiment of Restraining Robot Tort Gait Disturbance
In one walking cycle, the hydraulic quadruped robot’s diagonal legs went through the stages of lifting, falling, and four-legged support from the initial state. Take the state of the right front leg and left hind leg of the hydraulic quadruped robot as an example, and the experiment process is shown in Figure 4.
(a)
(b)
(c)
(d)
The change of the posture angle of the body during the movement can well reflect the stability of the robot during the movement. The change of the attitude angle of the body during movement is shown in Figure 5. The curve before optimization is the experimental data of the robot in the process of motion without using the scheme of resisting body interference proposed in this paper. The optimization curve represents the data obtained by using the body interference control method proposed in this paper.
(a)
(b)
By comparing the two curves before optimization and optimization in Figure 5, the effectiveness of the body disturbance suppression method proposed in this paper can be proved when the robot is motion. It can be seen from Figure 5(a) that using the interference suppression strategy, the variation range of roll angle after 2.3 s is −1.4°∼1.3°. It can be seen from Figure 5(b) that using the interference suppression strategy, the variation range of pitch angle is −10.5°∼−3.5°. In Figure 5, it can also be found that the pitch angle of the robot body during the movement process is much larger than the roll angle. This is because when the robot uses the tort gait to walk, the two sets of diagonal legs move alternately so that the fuselage will produce a large swing in the pitch direction.
Figures 6(a)–6(c) are the displacement distance of the CoM of robot body in the inertial coordinate system in the X, Y, and Z direction. It can be seen from Figure 6(a), when lateral displacement of the body is suppressed, the forward direction of the robot is more stable, and the walking distance is increased by 500 mm. Figure 6(b) shows a large lateral deviation of the body CoM when the robot does not use the body disturbance suppression scheme proposed in this paper. Using the body disturbance suppression method can effectively reduce the side-to-side shaking of the body, the range of change is 7.0∼10.2 mm. It can be seen from Figure 6(c) that the use of the body disturbance strategy can make the variation range of CoM in the vertical direction smaller, and the range of change is −1.6∼1.2 mm.
(a)
(b)
(c)
5.2. Suppressing the Disturbance of Lateral Force during Tort Gait
After the robot tort gait runs stably for about 4 cycles, in order to verify the effectiveness of the method proposed in this paper to suppress the disturbance of the body under external forces, an external force in a random direction is applied to the body. The magnitude of the external force is sufficient to cause the change of the posture angle and CoM position during the movement. Figures 7 and 8, respectively, show the change curve of attitude angle and CoM position under random external force.
(a)
(b)
(a)
(b)
(c)
It can be seen from Figure 7(a), using the interference suppression strategy the roll angle range of the body is reduced from −10°∼9° to −6.4°∼6.2°, range reduced by 33%. By comparing the roll angle change range of the body before optimization and optimization in Figure 7(b), body pitch change from −15.1°∼3.4° to −13°∼2.9°, range reduced by 14%.
It can be seen from Figure 8 that after the robot body is affected by external force, the change of the CoM displacement in the forward direction and the vertical direction is small, but the change of the lateral displacement is larger. It can be seen from Figure 8(b) that after the body disturbance suppression method is used, the change of the lateral displacement of the CoM is smaller. It takes about 6s to recover the lateral displacement, and the range of change during the period is 0.2∼14 mm. It can be seen from Figure 8(c) that it takes about 5.6 s to recover in the vertical direction, and the range of change during this period is −5∼−0.2 mm. The robot moves smoothly, which proves the effectiveness of the robot disturbance suppression strategy.
6. Conclusions
Based on the analysis of the whole-body dynamic, the robot can better grasp the influence of interference on the body during the movement process. However, applying dynamics model to body stability control will make the control model very complicated. The virtual control method proposed in this paper has the advantages of simple control model and clear mathematical structure. Through the analysis of the results of Adams experiment, it can be proved that the use of virtual spring-damping controller proposed in this paper can effectively suppress the disturbance during the robot movement. However, the experimental environment in this article is limited to flat ground and lacks the need for more complex working environments, and subsequent studies will focus on large disturbances, such as foot slip, body fall caused by external forces, and stability of walking on unstructured terrain.
Data Availability
Data used to support the findings of this study are included in the article.
Conflicts of Interest
The authors declare no conflicts of interest.
Authors’ Contributions
J.H. investigated the study; J.S. and X.S. contributed to resources; J.H. and B.M. contributed to data curation; J.S. and B.G. supervised the study; J.H. prepared the original draft; J.H. reviewed and edited the document; and J.H. was responsible for software.