Locomotion Control of Quadruped Robots With Online Center of Mass Adaptation and Payload Identification

Most of the controllers for quadruped robots are based on the planning of their centers of mass (CoM), with the assumption that the CoM locates at the geometrical center of the robot trunk. However, this assumption rarely meets the fact of the robot prototype and introduces apparent influences to the system. This article proposes a CoM estimation and adaptation method for quadruped robots in dynamic trot gait based on the model predictive control (MPC) of the trunk. The influences of the CoM offset on the robot states are inherently analyzed. The CoM components on the horizontal plane are taken into consideration and estimated based on the speed errors and the contact force differences, which are compensated to the robot controller. In the case of payload changes, the identification method for the mass of payload is proposed based on the estimated CoM. Simulation results verify that with the proposed approach, the robot successfully adapts to the CoM payload changes while the locomotion is not interrupted, and the state errors are significantly reduced.


I. INTRODUCTION
Many controllers for legged robots [1], [2] are based on their dynamic models [3], to which the inertial parameters are of great importance, especially for those performing high dynamic locomotion [4]. For simplification, some motion controllers [5] of legged robots is based on the center of mass (CoM) trajectory planning of them [6], [7], with the assumption that the CoM locates at the geometrical center of the robot trunk [2], [8]. Although the hardware components are designed to distribute uniformly using the computer aided design (CAD) software in advance, additional factors, such as, different configurations of mission payloads, make it almost impossible for the CoM of the prototype to coincide with its geometrical center.
Inaccurate estimation of the CoM position is easy to cause the zero moment point (ZMP) running out of the support region for bipedal robots [9], [10], and thus, leads to instability. Although quadruped robots have larger stability margin than bipeds in most cases, the model errors introduce The associate editor coordinating the review of this manuscript and approving it for publication was Okyay Kaynak . heavy disturbances to the locomotion. The stability criteria can be severely affected by the CoM offset for quadruped robots in static walking gait [11], [12] and tumbling may happen. In some cases, the CoM error leads to asynchronous leg motions and impulsive contact forces, which result in attitude oscillation and even mechanical damages [13]. Quadratic optimization is commonly used to distribute contact forces based on the virtual model control (VMC) [14] or the model predictive control (MPC) [15], [16]. The CoM offset causes state tracking errors of the robot using this kind of controller, mainly including the lateral speed error and the attitude error.
There are many factors that can affect the CoM position of quadruped robots, such as the transformation of the robot configuration, the reconstitution of different function modules, and the operating motions of the mission payloads, in which case the total mass of the robot maintains constant. Other cases exist whenever the amount and loading position of the payloads change, which have effects both on the robot mass and CoM.
The errors between the CAD model and engineering prototype bring inertial parameter errors to the robots.
Stephens [17] designed a state estimator for the humanoid robot in the presence of unknown modelling errors, but did not allow the COM offset and the external force disturbances simultaneously. Muscolo et al. [18] presented an approach to minimize the CoM error between the ideal virtual model and the real platform of the humanoid robot using force/torque sensors mounted on the feet, but did not introduce the reflecting method for payload changes. Focchi et al. [19] designed a method for the quadruped robot HyQ [20] to estimate the CoM position offset from its CAD model by collecting numerous sets of sensor feedback in static poses. Alai et al. [21] designed an estimator based on Kalman Filter to estimate the CoM position and velocity of the humanoid robot. But this method is only valid when the angular acceleration and the speed of the robot are small.
In some cases, the quadruped robot suffers the time-varying CoM. This can be caused by the incidents such as the reconfiguration of hardware components, the changes of payloads, and the consumption of the fuel. When the robot stands statically, the support region surrounded by its four legs provides large stability margin, which results in the tolerance to CoM changes. The wheel-legged robot CENTAURO [22] carried 17 Kg payloads with its dual arms in static pose. But for robots running in dynamic gaits, the stability margin decreases and the CoM variations need to be taken into consideration and compensated into the controllers.
Some researchers focus on the adaptation to payload changes of legged robots. Kim et al. [23] developed a ZMP control methods for a passenger-carrying humanoid robot that coped with variable passenger weights. Tournois et al. [24] proposed an online payload identification method for quadruped robots walking in static gait. However, the forward locomotion of the robot was suspended in the case of payload changes and resumed after the adaptations and adjustments were accomplished.
To overcome the shortcomings above, this article devotes to the adaptive control method for quadruped robots with unknown CoM offsets and payload changes. The real CoM position is estimated based on the state deviation of the robot, which implements a MPC based controller. Further, the mass of payload is calculated in the case of payload changes. Both the mass and CoM offsets are compensated to the robot controller. The novelty of this work states that, it proposes a locomotion controller for the quadruped robot while taking the real CoM into consideration, with which the robot realizes online self-adaptation to real time CoM changes. Besides, the forward locomotion of the robot is not interrupted whenever the CoM changes.
This article is organized as follows. Section II briefly introduces the robot and its simulation model. Section III presents the model and control architecture for the robot with the compensation for CoM offsets. The CoM estimation and payload identification method is described in Section IV. Section V shows the effectiveness of the proposed algorithm through simulations. Section VI gives the conclusion.

II. SYSTEM OVERVIEW
The hydraulic actuated heavy-duty quadruped robot Scalf-III was released by our group in 2018, which was designed for payload transportation tasks and had interfaces for onboard mission units to execute specific tasks. The robot is mainly composed of the power unit, the mechanical unit, the electronic control unit, and the perception unit. There are three joints in each leg, one for adduction and abduction and two for flexion and extension. This work is based on the simulation model which has the same topological structure and parameters as the Scalf-III, as shown in Fig. 1. The main dimension and inertial parameters are listed in Table 1. In the mechanical structure design of legged robots, light-weight actuators and materials are usually used to construct the legs so that the legs are as light as possible. For Scalf-III, the mass of the legs is pretty small comparing to that of the trunk, and thus we neglect the mass of the legs in the simulation model.

III. MODELING WITH CoM ADAPTATION
Most controllers for quadruped robots regard the geometrical center of the trunk as the robot CoM, but it is far from the truth for the Scalf-III. As shown in Fig. 1(a), the components of the robot, especially the power unit, are asymmetric and unstructured, which makes it impossible that the CoM coincides with the robot center. For some quadruped robots used for goods transportation, the changes of the payload mass and position may cause variations of the CoM. In some cases, quadruped robots are used as mobile platforms with mission units such as detection equipments on them. The reconfiguration of VOLUME 8, 2020 mission payloads may cause sporadic CoM changes of the robot.
The CoM offset introduce disturbances to the system. Fig. 2 shows the mechanism of the effects. F i = [F i,x , F i,y , F i,z ] T is the contact force of the standing leg. The identifier i = f , h stands for the front leg and the hind leg. r com = [c x , c y , c z ] T is the position of the CoM with regard to the base frame, namely the CoM offset. The x diag -axis passes through the two diagonal contact points. When the robot knows nothing about the CoM offset, it regards the body center as the CoM, and regulates the locomotion while taking the body center as the force bearing point. In this case, the robot regards the condition in which the body center locates in the vertical plane passing through x diag -axis as the balancing state. However, once the true CoM deviates from the body center, the resultant moment generated by contact forces and the gravity around x diag -axis is nonzero since the arm of the gravity r g = 0. This nonzero resultant moment leads the robot to tip towards one corner, which conflicts with the requirement that the robot should maintain the horizontal body posture. In this case, the robot losses either the accuracy of its posture or the precision of other states, unless the controller compensates for the CoM offset based on the value of the CoM. An intuitive way to compensate for the CoM offset is to adjust the positions of the standing feet to make the gravity vector pass through the connecting line of the contact points. In this way, the tipping moment generated by the gravity becomes zero. The adjustments on the sagittal plane and coronal plane are shown in Fig. 3. c x and c y are the horizontal components of the CoM. Since the tipping moment does not vary when the CoM shifts in the vertical direction, the z component of the CoM offset is neglected in this work.
The adaptability to CoM variations without interrupting the robot locomotion is essential for quadruped robots. This section designs the trot gait controller for the robot while compensating the CoM offset to the standing phase and swing phase, respectively.

A. MODELING OF STANDING PHASE
For the quadruped robot running in trot gait with the duty factor of 0.5, the robot has two diagonal legs standing on the  ground which provide driving powers. The dynamic model of the robot trunk in standing phase is depicted in Fig. 4.
{O b } is the base frame locating at the center of the trunk.
the moment arm of the standing leg. g is the gravitational acceleration.
Different from most of the existing methods, this work builds the rigid body dynamic model of the robot trunk at the real CoM instead of at the origin of the base frame. The CoM offset is compensated to the arms of the contact forces as in where = [γ , θ, ϕ] T and P = [x, y, z] T are the attitude angle and the position of the trunk, respectively. I is the inertia tensor of the trunk. ω is the angular velocity. m pyl is the mass of payloads. The pseudo roll angle γ and pseudo pitch angle θ with respect to the base frame are used in the dynamic model, which helps avoid global state observations. To achieve rough terrain adaptation, the absolute roll angle α and pitch angle β, which represent the topography, are introduced into the gravity compensation The onboard manipulator in Fig.1(b) is mounted only for the purpose of simulating the situation in which the robot suffers time-varying CoM positions, and it is not involved in the dynamic model of the robot.
When the robot maintains the stable orientation, the term ω × (I ω) does not contribute significantly to the dynamics of the robot comparing to the term Iω [19], [25], [26], which means d dt (I ω) = ω×(I ω)+Iω ≈ Iω. Encapsulating the states and inputs with the state vector (1) can be rewritten in the state space formation and discretized with the extremely small time interval t. The iterative equation of the system state can be obtained as (2) The system matrix and the input matrix in (2) are calculated as where 3×3 is the identity matrix and [W ] × indicates the skew-symmetric matrix of the vector W . A(k) ∈ R 15×15 does not vary with the time. B(k) ∈ R 15×6 is determined by the desired state of the robot at each sampling instant. The MPC-based controller takes the state trajectory in the prediction horizon into consideration. The state trajectory is a sequence of the desired states of each iteration, which contains the desired motions of the robot in the prediction horizon, such as, the velocities and the attitude. The robot states can be predicted from the current one, as in where The force distribution of the standing legs are transferred into a quadratic programming (QP) problem with the constraints of the friction corn, as in where Q ∈ R 15l×15l and R ∈ R 6l×6l are diagonal positive definite matrices indicating the weights of each state and input. µ is the frictional coefficient between the foot and the ground, which depends on the material of the foot and the terrain. F zmax is the maximum output force of the leg in the z direction that the actuators can achieve.
The dimensions of A p and B p in (5) are determined by the length of the prediction horizon l that A p ∈ R 15l×15 and B p ∈ R 15l×6l . The larger value of l can improve the robustness of the system, since the controller finds input forces for the legs for more future steps. However, this will increase the scale of the QP problem, which leads to longer calculation time, and thus the lag of the system. Through each solving process of the QP problem, the controller gets a sequence of the input forces for the future l steps as and F h (0) are practically used as the desired forces at the current time and new prediction will be execute with the updated state X (0) and reference state trajectory X ref .

B. FOOT TRAJECTORY OF SWING PHASE
The landing point of the swing phase is also the starting point of the standing phase. In order to realize the adjustment shown in Fig. 3, the landing point is compensated in x and y directions, as in where x land and y land are the landing position and T is the gait cycle.ẋ d andẏ d are the desired velocities of the trunk and The foot trajectory is designed as a curve cluster that varies with the robot state, as in where t is the passed time of the current swing phase and t sw is the servo interval. H b and h step are the stand height and step height of the robot. The swing legs are controlled to follow the designed trajectory from the inspiration of virtual model control (VMC), in which a three dimensional virtual spring-damping model is fixed on the foot. The details are not elaborated in this work for the sake of brevity.

IV. CoM ESTIMATION AND PAYLOAD IDENTIFICATION
This section analyzes the influence of the CoM offset quantitatively and qualitatively, and then gives the estimation method. With the approximation that d dt (I ω) ≈ Iω, the Newton-Euler dynamic model of the trunk in (1) can be rewritten as where m = m r + m pyl . There are six variables with six force inputs in (12). However, the coefficient matrix D ∈ R 6×6 is not a full rank matrix. Generally, the rank of D is five. When z f = z h , the rank of D reduces to four, but this is practically impossible since the moment arms use the feedback positions of standing legs and have noises and errors. To solve this problem, the control of robot motions in a certain axis (usually the y-axis) is abandoned in some researches. For the MPC based control approach in this work, the solution of the QP problem finds a balance for the robot performance in all the six degrees of freedom through appropriate weight coefficient in the matrix Q = diag(q 1 (1), · · · , q 15 (1), q 1 (2), · · · , q 15 (2), · · · , q 1 (l), · · · , q 15 (l)).

A. ESTIMATION OF c y
The CoM is unknown for the controller at the initial moment, and is set to be zero. It is an ideal situation that when the robot maintains upright attitude, the torque applied by the contact forces around x b -axis satisfies However, as discussed above, the CoM offset is difficult to avoid. Once the CoM shifts from the trunk center, the additional torque applied to the x b -axis by the gravity is The intuitionistic influence of this extra torque is that the trunk tends to generate a nonzero angular acceleration to break the stable pose. The weigh matrix Q in the QP problem determines the priorities of each robot state. To ensure the safety and stability of the robot, the maintaining of upright attitude usually has higher priority than the speed tracking performance. That is, the attitude angles are given more weights than the velocities in the MPC controller. To remedy the deviation of the roll angle, the robot will apply additional forces to the standing feet to satisfy that where T x is the resultant moment applied on the x b -axis by the contact forces and the gravity. Being restricted to the limitation that F f ,z + F h,z = m r g, the adjustable range of the contact forces in z direction is very small ( F f ,z ≈ 0 and F h,z ≈ 0). Substituting (13) into (15), it can be obtained that During the locomotion of quadruped robots, the height of the trunk H b is commonly expected to be a constant value, which means z f = z h = −H b . Thus, an instantaneous acceleration of the trunk in the y b -axis appears with the value approximating to gc y /H b . Note that, as long as the controller is not completely compensated for the CoM, the lateral acceleration exists. Thus, the y component of the CoM position is estimated based on the errors between the feedback and desired lateral velocities, in a recursive fashion as where k c py and k c dy are the proportional coefficient and differential coefficient, respectively. The tilde on the top of a symbol means it is the estimated value in this work.
Compensating the controller for the estimated CoM following the way described in Fig. 3,c y is added to the level arms and (11) becomes Once the estimated CoM converges to its true value, the item (c y F f ,z +c y F h,z ) equals to m r gc y , which appropriately balances the extra gravitational moment in (14). In this case, the contact forces in y-axis F f ,y and F h,y resume their values when there is no CoM offset, namely F f ,z = F h,z = 0. In this approach, the acceleration and velocity error in y direction converge to zero as well as the estimated CoM converges to its true value.

B. ESTIMATION OF c x
Similar to the case of y direction, the CoM offset in x direction generates an unexpected torque around the y b -axis that is unable to be balanced by the contact forces. The quadruped robot has larger stability margin in its longitudinal dimension than that in the lateral direction, and thus lower sensitivity to the CoM error. In practical applications, the movements of quadruped robots prefer to fore-and-aft locomotion rather than sidewise. So, the robots require higher precision of speed tracking performance in the x b direction. With exorbitant large weight for the pitch angle, the robot tends to produce attitude oscillations when runs across rough terrains. Based on the considerations above, the controller assigns more weight for the tracking of roll angle than that for the y velocity in the lateral direction. But this emphasis is less obvious along the x b -axis. It leads to different effects on the robot by the CoM offset, and thus different appearance of the robot states. The longitudinal CoM offset leads the robot to nod and raise its head repeatedly rather than the speed error laterally.
From this inspiration, the real x position of the CoM is estimated based on the inconformity of the contact forces of the front and hind legs. The value of the identifier is determined by the difference between the contact forces as in where ξ > 0 and η < 0 are thresholds for judgements. The comparison of the contact forces is valid only when the two standing legs both touch the ground, which represents (20) where τ f and τ h are joint torques. F th f and F th h are thresholds. To avoid misjudgment caused by disturbances, a slide window is introduced, with short length to reduce the hysteresis. Along the window horizon, the identifiers are synthesized into the variable N , which is calculated as (21) where k stands for the sampling sequence. The recursive estimation of the CoM position in x direction is where k c px and k c dx are the proportional coefficient and differential coefficient, respectively. Similar to the case of the lateral direction,c x is also compensated into the level arms of the contact forces. Along with the convergence ofc x to its true value, the pitch angle oscillation of the robot trunk is weakened.

C. MASS OF PAYLOAD
There are two major situations that lead to the variation of the CoG position of quadruped robots. One is that the onboard manipulators execute motions while maintaining the total mass of the robot constant. Another case is that the payloads are added (loading) or cut down (unloading). Subjecting to the limitation of stable standing height, the variation of the robot mass has no obvious influence on the robot locomotion on flat terrain. However, when encountering slops, the inaccurate mass makes the reference values of contact forces cannot match their actual demands. The identification of the payload mass is essential. Based on the prior estimated CoMc x,y , once the payload is loaded or unloaded, the CoM position drifts tõ where c pyl is the position of the payload with respect to the base frame. The payloads are places in the cabin which is designed at a fixed place, and thus c pyl is known and maintains constant. m r can be obtained from the contact forces when the robot stands in a static pose initially, and updated with the payload mass afterwards. The variation tendency (load/unload) is detected based on the comparison of the contact forces between different sampling periods when the acceleration of the robot in the vertical direction closes to zero. By collecting a vast array of data sets, the payload mass is solved in the least square (LS) fashion, as in where the subscript j means the j-th sampling. n is the scale of the QP problem.

V. SIMULATIONS AND RESULTS
To verify the effectiveness of the proposed method, simulations are executed in the simulator Webots R2018a based on the robot model described in Section II. The robot runs in trot gait with the duty factor of 0.5 and the step frequency of 1.5 Hz, which means that there are two and only two diagonal legs in contact with the ground (the standing phase) at any moment, while the other two legs are in the air. Major situations that cause the CoM errors are considered, including the design error, the reconfiguration of mission units, and the change of payloads. Note that, in this section, the CoM with respect to the base frame is constant unless when the manipulator moves or when the payload changes. In all cases, the CoM is time-varying with respect to the world coordinate frame, because the robot keep moving.

A. CoM ESTIMATION WITH PREDEFINED REFERENCE VALUES
The CoM position of the robot can be assigned manually by users in Webots. The quadruped robot is ordered to walk in situ. In order to evaluate the accuracy of the CoM detection results, the CoM of the robot is set to be at the position r com = [0.15m, −0.08m, 0.0m] T with respect to the base frame, which is regarded as the real value. The estimation result is shown in Fig. 5. The estimated CoMc r converges to the real robot CoM within three seconds with the errors less than 0.6 cm in x direction and 0.5 cm in y direction. The step character of the x direction result is caused by the inherent dead zone of the estimation mechanism in (19) ∼ (22). To test the robustness of the proposed method to the external disturbances, both lateral and longitudinal impacts are exerted to the robot in this simulation. The robot is knocked by a 50 Kg pendulum bob, which falls from the horizontal position to FIGURE 5. CoM estimaiton results. VOLUME 8, 2020 the bottom with the pendulum length of 0.7 m. The estimated CoM returns to the real value within three seconds. The CoM adaptation method does not conflict with the path planning of the swing foot. As shown in Fig. 6, the oscillation of the roll and pitch angles of the robot trunk is remarkably decreased with the CoM adaptation approach.

C. SENSITIVITY ANALYSIS
The CoM in x direction is estimated based on the lateral velocity, which is obtained by the differential of the foot position, andc y is calculated relying on the force feedbacks.
To evaluate the sensitivity to sensor noises, Gaussian noises with different characteristics, namely mean (µ) and standard deviation (σ ) (listed in Table 2), are set to the foot positions and the contact forces. Since the sensors are usually calibrated offline, the mean of the noise is set fixed, and different standard deviations are considered. The estimation results in the convergence horizon are analyzed based on the five-number summary, as shown in Fig. 8. The length of whiskers are  set as 1.5 times the inter-quartile range. Table 2 reports the convergence errors and boundary errors in each cases. The convergence error is used to evaluate the accuracy of the results, defined as the distance between the median estimated value and the true value The dispersion of the results in the convergence horizon is evaluated by the boundary error, which is defined as the maximum of the distances from the upper and lower whisker edges to the true value, as in

D. ADAPTATION TO TIME-VARYING CoMs
Although the real value of the CoM can be designated manually in simulation, it is difficult to know the real CoM of a robot prototypes, which is also the necessity of this work.
In this section, we use a lightweight onboard manipulator which moves with the 50 Kg load at the end-effector to simulate the case in which the robot suffers time-varying CoMs, which may be caused by the reconfiguration of its hardware components, the changes of payloads and the consumption of the fuel. The end-effector is programmed to pass through five waypoints following the sequence of 0 -1 -2 -3 -4 -0 , as shown in Fig. 9. The robot is ordered to run in trot gait at different speeds, in both x and y directions. Once the load arrives at a new waypoint, the robot takes five seconds to detect and adapt to the shifted CoM, and then the load moves to the next waypoint taking three seconds. At each waypoint, the CAD software SolidWorks is used to simulate the CoM of the combination of the robot and the grasped goods, which is regarded as the true value. The trajectory of the end-effector is limited to the horizontal plane. The waypoint and the corresponding true CoM values are shown in Fig. 9. Fig. 10 shows the CoM estimation results during the entire process. The estimated CoM value converges to the CAD model value at each waypoint, with the error less than 15% in the x direction and less than 10% in the y direction.   as shown in Fig. 11(a) and 11(b), which denotes that the robot possesses higher stability. The velocity deviations in Fig. 11(c) and 11(d) are remarkably decreased and the robot runs following the desired commands more precisely with the proposed method. Fig. 12 shows the comparison of contact forces in the period of t ∈ [2.0 s ∼ 6.0 s], during which c x varies from zero to a positive value. The results show that when the proposed CoM adaptation method is applied, the force discrepancy between the two standing legs is remarkably decreased and the legs act more harmoniously. Fig. 13 consists of the snapshots of the robot during this simulation, showing that the locomotion is uninterrupted while adapting to the changed CoM, and the adjustments of legs can be seen. Since the dynamics of the manipulator is not involved in the robot controller, the movement velocity of the end-effector is limited to relatively low values, but it is enough for simulating the situation that the robot suffers  time-varying CoM. When the manipulator implements high dynamic motions, it is possible that the ZMP runs out of the support region of the robot, and thus causes tumbles.

E. MASS IDENTIFICATION OF PAYLOADS
Loading of the payload is simulated in this section, with different predefined CoM positions (listed in Table 3) of the quadruped robot. The robot starts to run in situ and the payload is loaded at the position of [0.51 m, 0.20 m] (coordinates in the horizontal plane with respect to the base frame) after the estimated CoMc r tends to be stable. Payloads with different mass (20 Kg,40 Kg, 60 Kg, 80 Kg and 100 Kg) are tested in each situation. Once the robot has already adapted to the newly changed CoM, data sets are collected by one thousand continuous sampling to constitute the over determined system of equations to identify the mass of payloads. The identification results are listed in Table 3. The errors between the estimated mass and the real mass are mostly within 8 %, and the biggest error is 12.23 %. Since the robot never knows the true value of the its CoM, once the estimated valuec x,y reaches its convergence horizon (the fluctuation maintains within a certain small range), it is adopted by the robot as the real CoM and employed by the controller. The mass estimation errors are mainly introduced by the error of the CoM estimation. The result shows that, the quadruped robot can accommodate to payload changes while its locomotion is not interrupted, as long as the actuators can provide sufficient forces.

VI. CONCLUSION
Most of the state-of-the-art CoM estimation methods for quadruped robot either are offline or require specific trunk poses, and thus lead to interruption of robot locomotion. This work proposed the online CoM adaptation and payload identification method for quadruped robots in trot gait based on the error of velocities and the discrepancy of contact forces. Multi-factors that may cause CoM offsets are involve in simulations. It is proved through the analysis of accuracy, reliability, and sensitivity, that the estimation results satisfy the demands of the state tracking performance of the robot. With the proposed method, the adaptation to payload changes is also realized. In all the cases above, the dynamic trot gait locomotion of the robot is not interrupted. CoM estimation approaches may vary with different robot controllers, and this work is conducted on the MPC-based one. However, it can be expanded to all controllers that are based on the rigid body Newton-Euler dynamic model of the trunk combining with the optimization theory, which has inherent under actuated characteristics.