# Suppression of Quadruped Robot Body Disturbance by Virtual Spring-Damping Model

Authors: JingYe He; JunPeng Shao; BingWei Gao; BingYi Miao; Xuan Shao
Journal: Complexity
 (2022)

---

## 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.


---

## Body

## 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 Section2, 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 Figure1. The body length, width, and height were 2a, 2b, and 2h, respectively. The length of the robot hip link, thigh link, and calf link is l1i, l2i, and l3i, respectively, where i=1,2,3,4 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 θ1i, θ2i, and θ3i 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 s at the CoM of the robot. At the ground projection of the initial CoM, establish an inertial coordinate system w and establish a reference coordinate system c at hip joint and foot coordinate system b. The parameters required for calculation are shown in Table 1.Figure 1
The quadruped robot PoE method establishes the coordinate system.Table 1
Calculation parameter.
Component nameParameterValue (m)Length2a1.1Body width2b0.4Height2h0.2Hip linkl10.06Thigh linkl20.34Calf linkl30.37First, determine the pose of the footAi in the reference coordinate system ci when joint angles θ1i=θ2i=θ3i=0(1)Mcbi=000−l1+l2+l30m00−m0000001,where m is(2)m=1,i=2,3,−1,i=1,4.According to theChasles−Mozzi theorem, any rigid link movement can be realized by moving around a fixed spiral axis S=ωs,vs in space ωs=ωsx,ωsy,ωsz is the direction vector of spiral axis, and vs=vsx,vsy,vsz is the linear velocity of the rigid link in space around the spiral axis S. The form of expressing the spiral axis as a matrix is(3)S=ωsvs00,where ωs is the skew-symmetric matrix of ωs(4)ωs=0−ωszωsyωsz0ωsxωsyωsx0.The forward kinematics of a single-leg needs to obtain the position of theAi pose in the reference system ci, so the spiral axis needs to be expressed in ci, and the relationship between ωs and vs is(5)vs=−ωs×qs,where qs is any point on the joint axis, measured in the reference coordinate system ci. 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.Table 2
The spiral axis of the leg joints in the reference coordinate system.
JointθiωciqciviHip joint0,0,10,0,00,0,0Thigh joint0,−β,0−l1,0,0β0,0,l1Calf joint0,−β,0−l1,−l2,0,0β0,0,l1+l2β in Table 2 is given by(6)β=1,i=1,2,−1,i=3,4.Take the right front leg as an example, the transformation matrixTcb1 of the foot A1 and the reference coordinate system c1 can be obtained as(7)Tcb1=eS11θ11eS21θ21eS31θ31Mcb1,where θ11, θ21, and θ31 are scalar quantities representing changes in hip joints, thigh joints, and calf joints.PoE can be expanded into a homogeneous transformation matrix as(8)T=esθ=eωIθ+1−cosθω+θ−sinθω2v01.The single-leg inverse motion can be simply described as knowing the coordinates of theAi in the reference coordinate system ci 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 leg(9)θ1i=atan2pcbyi,pcbxi,θ3i=π−arccosD2+pcbzi2−l22−l322l2l3,θ2i=atan2pcbzi,D−atan2l3s3,l3c3+l2,where pcbi=pcbxi,pcbyi,pcbzi is the position of the Ai measured in the reference coordinate system ci, and the expression of D is:(10)D=pcbxi2+pcbyi2−l1.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 Figure1, it can see that the position of the Ai in the body coordinate system s at the initial position is(11)Msbi=100ιa010−βb001−h+l1+l2+l30001,where ι is(12)ι=1,i=1,2,−1,i=3,4.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 systems, as shown in Table 3.Table 3
The coordinates of the leg joints spiral axis in the body coordinate system.
Leg jointsθωsiqsivsiHip jointm−1,0,00,−mb,−h0,mh,−bThigh jointι0,−1,0ιa,0,−h−l1−ιh+l1,0,−aCalf jointι0,−1,0ιa,0,−h−l1−l2−ιh+l1+l2,0,−aWhen the joint variables of legi are given, the pose of the foot end in the body coordinate system can be derived as(13)Tsbi=eSi1θi1eSi2θi2eSi3θ3iMsbi.


## 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 Figure1. The body length, width, and height were 2a, 2b, and 2h, respectively. The length of the robot hip link, thigh link, and calf link is l1i, l2i, and l3i, respectively, where i=1,2,3,4 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 θ1i, θ2i, and θ3i 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 s at the CoM of the robot. At the ground projection of the initial CoM, establish an inertial coordinate system w and establish a reference coordinate system c at hip joint and foot coordinate system b. The parameters required for calculation are shown in Table 1.Figure 1
The quadruped robot PoE method establishes the coordinate system.Table 1
Calculation parameter.
Component nameParameterValue (m)Length2a1.1Body width2b0.4Height2h0.2Hip linkl10.06Thigh linkl20.34Calf linkl30.37First, determine the pose of the footAi in the reference coordinate system ci when joint angles θ1i=θ2i=θ3i=0(1)Mcbi=000−l1+l2+l30m00−m0000001,where m is(2)m=1,i=2,3,−1,i=1,4.According to theChasles−Mozzi theorem, any rigid link movement can be realized by moving around a fixed spiral axis S=ωs,vs in space ωs=ωsx,ωsy,ωsz is the direction vector of spiral axis, and vs=vsx,vsy,vsz is the linear velocity of the rigid link in space around the spiral axis S. The form of expressing the spiral axis as a matrix is(3)S=ωsvs00,where ωs is the skew-symmetric matrix of ωs(4)ωs=0−ωszωsyωsz0ωsxωsyωsx0.The forward kinematics of a single-leg needs to obtain the position of theAi pose in the reference system ci, so the spiral axis needs to be expressed in ci, and the relationship between ωs and vs is(5)vs=−ωs×qs,where qs is any point on the joint axis, measured in the reference coordinate system ci. 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.Table 2
The spiral axis of the leg joints in the reference coordinate system.
JointθiωciqciviHip joint0,0,10,0,00,0,0Thigh joint0,−β,0−l1,0,0β0,0,l1Calf joint0,−β,0−l1,−l2,0,0β0,0,l1+l2β in Table 2 is given by(6)β=1,i=1,2,−1,i=3,4.Take the right front leg as an example, the transformation matrixTcb1 of the foot A1 and the reference coordinate system c1 can be obtained as(7)Tcb1=eS11θ11eS21θ21eS31θ31Mcb1,where θ11, θ21, and θ31 are scalar quantities representing changes in hip joints, thigh joints, and calf joints.PoE can be expanded into a homogeneous transformation matrix as(8)T=esθ=eωIθ+1−cosθω+θ−sinθω2v01.The single-leg inverse motion can be simply described as knowing the coordinates of theAi in the reference coordinate system ci 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 leg(9)θ1i=atan2pcbyi,pcbxi,θ3i=π−arccosD2+pcbzi2−l22−l322l2l3,θ2i=atan2pcbzi,D−atan2l3s3,l3c3+l2,where pcbi=pcbxi,pcbyi,pcbzi is the position of the Ai measured in the reference coordinate system ci, and the expression of D is:(10)D=pcbxi2+pcbyi2−l1.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 Figure1, it can see that the position of the Ai in the body coordinate system s at the initial position is(11)Msbi=100ιa010−βb001−h+l1+l2+l30001,where ι is(12)ι=1,i=1,2,−1,i=3,4.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 systems, as shown in Table 3.Table 3
The coordinates of the leg joints spiral axis in the body coordinate system.
Leg jointsθωsiqsivsiHip jointm−1,0,00,−mb,−h0,mh,−bThigh jointι0,−1,0ιa,0,−h−l1−ιh+l1,0,−aCalf jointι0,−1,0ιa,0,−h−l1−l2−ιh+l1+l2,0,−aWhen the joint variables of legi are given, the pose of the foot end in the body coordinate system can be derived as(13)Tsbi=eSi1θi1eSi2θi2eSi3θ3iMsbi.


## 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 velocityw=wx,wy,wz and linear velocity v=vx,vy,vz of the rigid body are combined into a six-dimensional vector form, denoted as V=w,vT, called the twist, write w in the form of an antisymmetric matrix, twist can be written in the form of a skew-symmetric matrix, and twist V can be written in the form of a matrix(14)V=0−wzwyvxwz0−wxvy−wywx0vz0001.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 Vb and the joint speed θ˙=θ˙1,θ˙2,θ˙3T is(15)Vb=Jcbθθ˙=Jcb1θ,Jcb2θ,Jcb3θθ˙1θ˙2θ˙3.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.Table 4
Jacobian matrix between the foot end and the hip joint.
JointθiωciqcivciHip joint0,0,10,0,00,0,0Thigh jointms1,−c1,0−c1l1,−s1l1,00,0,ml1Calf jointms1,−c1,0−c1l1+c2l2,−s1l1+c2l2,−βs2l2−ιc1s2l2,−ιs1s2l2,ml1+c2l2wherec1=cosθ1 and s1=sinθ1.Taking the RF leg as an example, the Jacobian matrix between the end of the foot and the hip joint can be written as(16)Jcb1=0s1s10−c1−c110000−c1s1l200−s1s2l20l1l1+c2l2.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,(17)τTθ˙=FTV,where F=m,fT is a six-dimensional wrench, and F and V 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 c can be obtained as(18)τci=JcbiTθFci.The Jacobian matrix is traditionally expressed in body coordinate systems, and it can be seen from Figure 1 that the homogeneous transformation matrix between the reference coordinate system ci and the body coordinate system s is(19)Tsci=00−mιa0m0−mb100−h0001.According to formula (19), the adjoint matrix between the reference coordinate system ci and the body coordinate system s can be derived as (20)AdTsci=Rsc0piRscRsc,where pi is the skew-symmetric matrix of the distance between the origin of the reference coordinate system and the origin of the body coordinate system(21)pi=0h−mb−h0−ιambιa0.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:(22)Jsbi=AdTsciJcbiθ.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(23)τsi=JsbiTθFs.


### 3.2. Dynamics Analysis

Robot dynamics is concerned with the forces and moments that cause the robot to move.(24)τ=Mθ+hθ,θ˙.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 legl21 as an example.The twist thatl21 moves is v=w,v, the time-varying position of any particle mi on it is pit, and the position measured in the l21 centroid coordinate system I is ri. It can be deduced that the velocity and acceleration of the particle are(25)p˙i˙=v+ω×ri,p¨i=v˙+ω˙×ri+ω×v+ω×ri.According to Newton’s second law, it can be deduced that the force and torque onl21 are(26)f=miv+ωv,m=Iω˙+ωIω,where I is l21 rotational inertia matrix at centroid, and the relationship with the other particles on l21 is(27)Iother=I+mqTqI−qqT,where m is the mass of l21, I is identity matrix and q=qx,qy,qz is the vector from the centroid of l21 to the particle.In order to use twist and wrench mentioned above, equation (26) is rewritten as a six-dimensional matrix(28)F=GV˙−adVTGV,where G is spatial inertia matrix of l21, and adV is the Lie bracket form of V, expressed as(29)G=I00mI,adV=ω0vω.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 systemi,i=1,2,3 at each leg link centroid, and foot inertial coordinate system is 4. The screw axis of joint i is expressed as Ai in the link coordinate system i, and the CoM of body wrench is F0. According to these definitions, the twist and acceleration of each link can be deduced from the base to the foot.The twist of linki is determined by the joint speed θ˙i and the twist of link i−1(30)Vi=Aiθ˙i+AdTi,i−1Vi−1.The acceleration of the linki can be obtained by the derivative of time on both sides of equation (30)(31)V˙i=Aiθ¨+AdTi,i−1V˙i−1+adViAiθ˙i.When determining the wrench on the footFi+1, according to formulas (31) and (28), each joint torque and body wrench can be reverse derivation(32)GiV˙i−adViTGiVi=Fi−AdTi+1,iTFi+1.Since jointi is a rotating pair and has only one degree of freedom, the torque of joint i is(33)τi=FiTAi.The wrench at the CoM is the vector sum of wrench applied to all foot(34)F0=F01+F02+F03+F04.


### 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.Figure 2
Virtual spring control diagram of body.Two independent spring-damping actuators are added at the CoM in Figure2. 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)(35)τ=JTθF,where τ=τ1,τ2,…τnT is the torque generated by the joints involved in the control of the body, and F=m,fT 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 wrenchFdistrub=mdistrub,fdistrubT, the CoM speed v˙=x˙,y˙,z˙T and position p=x,y,z will change. The virtual controller at the CoM needs to generate wrench Fcontrol=mcontrol,fcontrol resistance Fdistrub with the same size and opposite direction(36)Fcontrol=−Fdistrub.At this time, the wrench generated by the virtual controller of the front leg isFf=mf,ffT and hind leg is Fh=mh,fhT. The front and hind legs have the same structure, so the virtual controller generates the same wrench, Ff=Fh. The relationship between Ff, Fh, and Fcontrol is(37)Fcontrol=Ff+Fh=2Ff.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 is(38)τf=JcbiTFf,(39)τh=JcbiTFh,where τf=τf1,τf2,τf3T and τh=τh1,τh2,τh3T.Use speed control for thex and y direction of CoM, and use position control for the z direction of CoM. Set the spring coefficient as kcph and the damping coefficient as kcdh, the desired speed in the forward direction is x˙d, the desired speed in the y direction is y˙d, the desired position in the z direction is zd, the wrench generated by the virtual actuator at the CoM is(40)Fcontrol=τx=fcontrolz·y−fcontroly·z,τy=fcontrolz·x−fcontrolx·z,τz=fcontrolx·y−fcontroly·z,fcontrolx=cosθxkcdhx˙−x˙d,fcontroly=sinθxkcdhy−y˙d,fcontrolz=cosθzkcphz−zd+sinθxtanθzkcdhz−z˙d,where θx and θz are the angles between the body virtual controller and the x-axis and z-axis of the coordinate system b.According to formulas (37)–(40), in order to stabilize the body, the moment generated by the leg joints of the stance phase is(41)τf=τh=12JcbiTFcontrol.


## 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 velocityw=wx,wy,wz and linear velocity v=vx,vy,vz of the rigid body are combined into a six-dimensional vector form, denoted as V=w,vT, called the twist, write w in the form of an antisymmetric matrix, twist can be written in the form of a skew-symmetric matrix, and twist V can be written in the form of a matrix(14)V=0−wzwyvxwz0−wxvy−wywx0vz0001.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 Vb and the joint speed θ˙=θ˙1,θ˙2,θ˙3T is(15)Vb=Jcbθθ˙=Jcb1θ,Jcb2θ,Jcb3θθ˙1θ˙2θ˙3.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.Table 4
Jacobian matrix between the foot end and the hip joint.
JointθiωciqcivciHip joint0,0,10,0,00,0,0Thigh jointms1,−c1,0−c1l1,−s1l1,00,0,ml1Calf jointms1,−c1,0−c1l1+c2l2,−s1l1+c2l2,−βs2l2−ιc1s2l2,−ιs1s2l2,ml1+c2l2wherec1=cosθ1 and s1=sinθ1.Taking the RF leg as an example, the Jacobian matrix between the end of the foot and the hip joint can be written as(16)Jcb1=0s1s10−c1−c110000−c1s1l200−s1s2l20l1l1+c2l2.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,(17)τTθ˙=FTV,where F=m,fT is a six-dimensional wrench, and F and V 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 c can be obtained as(18)τci=JcbiTθFci.The Jacobian matrix is traditionally expressed in body coordinate systems, and it can be seen from Figure 1 that the homogeneous transformation matrix between the reference coordinate system ci and the body coordinate system s is(19)Tsci=00−mιa0m0−mb100−h0001.According to formula (19), the adjoint matrix between the reference coordinate system ci and the body coordinate system s can be derived as (20)AdTsci=Rsc0piRscRsc,where pi is the skew-symmetric matrix of the distance between the origin of the reference coordinate system and the origin of the body coordinate system(21)pi=0h−mb−h0−ιambιa0.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:(22)Jsbi=AdTsciJcbiθ.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(23)τsi=JsbiTθFs.


## 3.2. Dynamics Analysis

Robot dynamics is concerned with the forces and moments that cause the robot to move.(24)τ=Mθ+hθ,θ˙.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 legl21 as an example.The twist thatl21 moves is v=w,v, the time-varying position of any particle mi on it is pit, and the position measured in the l21 centroid coordinate system I is ri. It can be deduced that the velocity and acceleration of the particle are(25)p˙i˙=v+ω×ri,p¨i=v˙+ω˙×ri+ω×v+ω×ri.According to Newton’s second law, it can be deduced that the force and torque onl21 are(26)f=miv+ωv,m=Iω˙+ωIω,where I is l21 rotational inertia matrix at centroid, and the relationship with the other particles on l21 is(27)Iother=I+mqTqI−qqT,where m is the mass of l21, I is identity matrix and q=qx,qy,qz is the vector from the centroid of l21 to the particle.In order to use twist and wrench mentioned above, equation (26) is rewritten as a six-dimensional matrix(28)F=GV˙−adVTGV,where G is spatial inertia matrix of l21, and adV is the Lie bracket form of V, expressed as(29)G=I00mI,adV=ω0vω.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 systemi,i=1,2,3 at each leg link centroid, and foot inertial coordinate system is 4. The screw axis of joint i is expressed as Ai in the link coordinate system i, and the CoM of body wrench is F0. According to these definitions, the twist and acceleration of each link can be deduced from the base to the foot.The twist of linki is determined by the joint speed θ˙i and the twist of link i−1(30)Vi=Aiθ˙i+AdTi,i−1Vi−1.The acceleration of the linki can be obtained by the derivative of time on both sides of equation (30)(31)V˙i=Aiθ¨+AdTi,i−1V˙i−1+adViAiθ˙i.When determining the wrench on the footFi+1, according to formulas (31) and (28), each joint torque and body wrench can be reverse derivation(32)GiV˙i−adViTGiVi=Fi−AdTi+1,iTFi+1.Since jointi is a rotating pair and has only one degree of freedom, the torque of joint i is(33)τi=FiTAi.The wrench at the CoM is the vector sum of wrench applied to all foot(34)F0=F01+F02+F03+F04.


## 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 legl21 as an example.The twist thatl21 moves is v=w,v, the time-varying position of any particle mi on it is pit, and the position measured in the l21 centroid coordinate system I is ri. It can be deduced that the velocity and acceleration of the particle are(25)p˙i˙=v+ω×ri,p¨i=v˙+ω˙×ri+ω×v+ω×ri.According to Newton’s second law, it can be deduced that the force and torque onl21 are(26)f=miv+ωv,m=Iω˙+ωIω,where I is l21 rotational inertia matrix at centroid, and the relationship with the other particles on l21 is(27)Iother=I+mqTqI−qqT,where m is the mass of l21, I is identity matrix and q=qx,qy,qz is the vector from the centroid of l21 to the particle.In order to use twist and wrench mentioned above, equation (26) is rewritten as a six-dimensional matrix(28)F=GV˙−adVTGV,where G is spatial inertia matrix of l21, and adV is the Lie bracket form of V, expressed as(29)G=I00mI,adV=ω0vω.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 systemi,i=1,2,3 at each leg link centroid, and foot inertial coordinate system is 4. The screw axis of joint i is expressed as Ai in the link coordinate system i, and the CoM of body wrench is F0. According to these definitions, the twist and acceleration of each link can be deduced from the base to the foot.The twist of linki is determined by the joint speed θ˙i and the twist of link i−1(30)Vi=Aiθ˙i+AdTi,i−1Vi−1.The acceleration of the linki can be obtained by the derivative of time on both sides of equation (30)(31)V˙i=Aiθ¨+AdTi,i−1V˙i−1+adViAiθ˙i.When determining the wrench on the footFi+1, according to formulas (31) and (28), each joint torque and body wrench can be reverse derivation(32)GiV˙i−adViTGiVi=Fi−AdTi+1,iTFi+1.Since jointi is a rotating pair and has only one degree of freedom, the torque of joint i is(33)τi=FiTAi.The wrench at the CoM is the vector sum of wrench applied to all foot(34)F0=F01+F02+F03+F04.


## 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.Figure 2
Virtual spring control diagram of body.Two independent spring-damping actuators are added at the CoM in Figure2. 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)(35)τ=JTθF,where τ=τ1,τ2,…τnT is the torque generated by the joints involved in the control of the body, and F=m,fT 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 wrenchFdistrub=mdistrub,fdistrubT, the CoM speed v˙=x˙,y˙,z˙T and position p=x,y,z will change. The virtual controller at the CoM needs to generate wrench Fcontrol=mcontrol,fcontrol resistance Fdistrub with the same size and opposite direction(36)Fcontrol=−Fdistrub.At this time, the wrench generated by the virtual controller of the front leg isFf=mf,ffT and hind leg is Fh=mh,fhT. The front and hind legs have the same structure, so the virtual controller generates the same wrench, Ff=Fh. The relationship between Ff, Fh, and Fcontrol is(37)Fcontrol=Ff+Fh=2Ff.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 is(38)τf=JcbiTFf,(39)τh=JcbiTFh,where τf=τf1,τf2,τf3T and τh=τh1,τh2,τh3T.Use speed control for thex and y direction of CoM, and use position control for the z direction of CoM. Set the spring coefficient as kcph and the damping coefficient as kcdh, the desired speed in the forward direction is x˙d, the desired speed in the y direction is y˙d, the desired position in the z direction is zd, the wrench generated by the virtual actuator at the CoM is(40)Fcontrol=τx=fcontrolz·y−fcontroly·z,τy=fcontrolz·x−fcontrolx·z,τz=fcontrolx·y−fcontroly·z,fcontrolx=cosθxkcdhx˙−x˙d,fcontroly=sinθxkcdhy−y˙d,fcontrolz=cosθzkcphz−zd+sinθxtanθzkcdhz−z˙d,where θx and θz are the angles between the body virtual controller and the x-axis and z-axis of the coordinate system b.According to formulas (37)–(40), in order to stabilize the body, the moment generated by the leg joints of the stance phase is(41)τf=τh=12JcbiTFcontrol.


## 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.hen0≤t≤Tm/2(42)px=StTm−12πsin2πtTm−S2+a,py=−b,pz=2HtTm−14πsin4πtTm−S0.WhenTm/2≤t≤Tm(43)px=StTm−12π2πtTm−S2+a,py=−b,pz=2H1−tTm+14πsin4πt−TmT−Tm+S2+a,where p=px,py,pzT is the displacement of the foot in the inertial coordinate system w, S is the step length, H is the max foot lift height, T is the step cycle, Tm is the swing phase period, and t 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.WhenTm≤t≤T(44)px=−St−TmT−Tm−12πsin2πt−TmT−Tm+S2+a,py=−b,pz=−S0.When the legs are in the stance phase, the feet are in contact with the ground, and there is no displacement in thez direction and y direction.All the controls in this article are expressed in the coordinate systems, so the displacement of the foot in the inertial coordinate system w needs to be expressed in the coordinate system s. The posture between the coordinate system w and the coordinate system s can be expressed as(45)Rws=Rotω⌢,θ=cθ+ω⌢x21−cθω⌢xω⌢y1−cθ−ω⌢zsθω⌢xω⌢z1−cθ+ω⌢ysθω⌢xω⌢y1−cθ+ω⌢zsθcθ+ω⌢y21−cθω⌢yω⌢z1−cθ−ω⌢xsθω⌢xω⌢z1−cθω⌢ysθω⌢yω⌢z1−cθ+ω⌢xsθcθ+ω⌢z21−cθ,where ω⌢=ω⌢x,ω⌢y,ω⌢zT is the unit rotation axis between w and s and θ is a scalar, indicating the amount of rotation around ω⌢.The origin ofs at the position of w is pws, and the homogeneous transformation matrix between the s and w is(46)Tws=Rwspws01.The position of the foot in the body coordinate systemPs is(47)ps=Tws−1pw.


### 4.3. Design of Body Disturbance Suppressor

The disturbance suppression block diagram of the hydraulic quadruped robot is shown in Figure3. zd, x˙d, y˙d, and z˙d are, respectively, the expected height of the body and the expected CoM velocity of the x-axis, y-axis, and z-axis body. x, y, z, x˙, y˙, and z˙ are the displacement and velocity along the x-axis, y-axis, and z-axis of the coordinate system s 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.Figure 3
Body disturbance control block diagram.


## 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.hen0≤t≤Tm/2(42)px=StTm−12πsin2πtTm−S2+a,py=−b,pz=2HtTm−14πsin4πtTm−S0.WhenTm/2≤t≤Tm(43)px=StTm−12π2πtTm−S2+a,py=−b,pz=2H1−tTm+14πsin4πt−TmT−Tm+S2+a,where p=px,py,pzT is the displacement of the foot in the inertial coordinate system w, S is the step length, H is the max foot lift height, T is the step cycle, Tm is the swing phase period, and t 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.WhenTm≤t≤T(44)px=−St−TmT−Tm−12πsin2πt−TmT−Tm+S2+a,py=−b,pz=−S0.When the legs are in the stance phase, the feet are in contact with the ground, and there is no displacement in thez direction and y direction.All the controls in this article are expressed in the coordinate systems, so the displacement of the foot in the inertial coordinate system w needs to be expressed in the coordinate system s. The posture between the coordinate system w and the coordinate system s can be expressed as(45)Rws=Rotω⌢,θ=cθ+ω⌢x21−cθω⌢xω⌢y1−cθ−ω⌢zsθω⌢xω⌢z1−cθ+ω⌢ysθω⌢xω⌢y1−cθ+ω⌢zsθcθ+ω⌢y21−cθω⌢yω⌢z1−cθ−ω⌢xsθω⌢xω⌢z1−cθω⌢ysθω⌢yω⌢z1−cθ+ω⌢xsθcθ+ω⌢z21−cθ,where ω⌢=ω⌢x,ω⌢y,ω⌢zT is the unit rotation axis between w and s and θ is a scalar, indicating the amount of rotation around ω⌢.The origin ofs at the position of w is pws, and the homogeneous transformation matrix between the s and w is(46)Tws=Rwspws01.The position of the foot in the body coordinate systemPs is(47)ps=Tws−1pw.


## 4.3. Design of Body Disturbance Suppressor

The disturbance suppression block diagram of the hydraulic quadruped robot is shown in Figure3. zd, x˙d, y˙d, and z˙d are, respectively, the expected height of the body and the expected CoM velocity of the x-axis, y-axis, and z-axis body. x, y, z, x˙, y˙, and z˙ are the displacement and velocity along the x-axis, y-axis, and z-axis of the coordinate system s 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.Figure 3
Body disturbance control block diagram.


## 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 thex-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 Figure4.Figure 4
Robot tort gait experiment process. (a) Left hind leg lift stage. (b) The left hind leg reaches the highest point. (c) The left hind leg land stage. (d) The left hind leg landed.
(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 Figure5. 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.Figure 5
Posture angle of body. (a) The roll angle of the body during the movement of the robot. (b) The pitch angle of the body during the movement of the robot.
(a)(b)By comparing the two curves before optimization and optimization in Figure5, 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.Figures6(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.Figure 6
Body CoM displacement curve. (a) Forward curve; (b) lateral curve; (c) vertical curve.
(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. Figures7 and 8, respectively, show the change curve of attitude angle and CoM position under random external force.Figure 7
The attitude angle changes after the body under external force. (a) Roll angle of the body. (b) Pitch angle of the body.
(a)(b)Figure 8
Body CoM displacement curve. (a) Forward curve; (b) lateral curve; (c) vertical curve.
(a)(b)(c)It can be seen from Figure7(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 Figure8 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.


## 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 Figure4.Figure 4
Robot tort gait experiment process. (a) Left hind leg lift stage. (b) The left hind leg reaches the highest point. (c) The left hind leg land stage. (d) The left hind leg landed.
(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 Figure5. 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.Figure 5
Posture angle of body. (a) The roll angle of the body during the movement of the robot. (b) The pitch angle of the body during the movement of the robot.
(a)(b)By comparing the two curves before optimization and optimization in Figure5, 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.Figures6(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.Figure 6
Body CoM displacement curve. (a) Forward curve; (b) lateral curve; (c) vertical curve.
(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. Figures7 and 8, respectively, show the change curve of attitude angle and CoM position under random external force.Figure 7
The attitude angle changes after the body under external force. (a) Roll angle of the body. (b) Pitch angle of the body.
(a)(b)Figure 8
Body CoM displacement curve. (a) Forward curve; (b) lateral curve; (c) vertical curve.
(a)(b)(c)It can be seen from Figure7(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 Figure8 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.



---

*Source: 4510678-2022-03-16.xml*