Precise Calculation of Inverse Kinematics of the Center of Gravity for Bipedal Walking Robots

: The walking of humanoid robots is dependent on the precise tracking of their center of gravity and foot trajectories. Trajectory tracking is achieved by mobilizing their joints to achieve the correct trajectory. Errors occur because of assumptions on tracking the center of gravity and the foot trajectories. In this study, a numerical algorithm was developed that produces an exact and single kinematic solution in which the center of gravity and foot trajectories can be tracked with the desired precision. The effectiveness of this algorithm was examined with a dynamic simulation and compared with a method given in the literature. The main highlight of this study, using the presented algorithm, is that the robot could walk even if the position of its center of gravity was lower than its hips, resulting in a tracking error that was smaller than that reported in the literature.


Introduction
Humanoid robots have a similar joint structure to humans and are expected to perform actions usually performed by them.Robots with human-like mobility that do not require changes in work environments in the industry, space, service, and health sectors can increase the quality of life of people.Humanoid robots have advanced enough for use in industrial settings [1][2][3].These robots mainly consist of a bottom part, which consists of two legs connected to the pelvis that carry the trunk and is responsible for locomotion, and an upper body, which is connected to two arms and contains a central computer, sensors, and a power supply, to perform the tasks assigned to the robot.This study focuses on the locomotion of the bottom part, consisting of two legs and a pelvis piece.
Humanoid robots move via gravity, contact forces, and torques applied to their joints [4].In the first step, a plan is made for gait and steps in the process of walking [5][6][7].In this process, the trajectories of the center of gravity (CoG) of the robot and its feet are obtained.When it is expected to walk based on its CoG, the inverted pendulum method is used [8].The cart-table model is used for walking based on foot positions [9].In both methods, the zero-moment point (ZMP) is used as the point of contact between the fixed foot and the floor [10].Inverse kinematics methods are used to move the limbs to the desired position in the workspace, whereas forward kinematics methods are used to move them in the joint space [11].In the second step, inverse kinematics calculations are performed so that the robot can follow these trajectories.When the foot and pelvis positions are known, joint angles can be calculated using the inverse kinematics method, either analytically or numerically [12][13][14][15][16][17].
However, this does not guarantee the tracking of the CoG of the robot at the desired precision level.The CoG is in the middle of the connection points of the two legs of the robot or at a higher position [18][19][20][21][22][23][24][25].The CoG is calculated before the robot starts its motion.Throughout this motion, the difference between the positions of the pelvis and the CoG is assumed to be fixed [9].Another assumption is that the CoG of the robot is fixed in reference to the supporting leg [26].
When the CoG is not calculated precisely, the deviations in the CoG caused by contact forces prevent the locomotion of the robot [27].The methods used in the literature to solve this are listed in the following paragraph.
In a previous study, the feet of the robot were increased in size [28].To increase the accuracy of calculation, in addition to CoG and foot trajectories, analyses requiring the hip position and trajectory and orientation trajectories were carried out [20,29].When the weight of the foot constitutes a higher proportion of the total weight of the robot, the foot that is in motion has a greater influence on the locomotive dynamics of the robot.To mitigate this effect, the method of modeling the moving foot as a pendulum was used.Nevertheless, there were still errors in CoG tracking and the movements of the robot were restricted due to the aforementioned assumptions [30].
Applications aim to ensure that the controller moves the robot within the desired tolerances against these errors.The smallest achievable error was found to be 0.18 m with the inverse dynamics control method [31] and 0.03-0.4m with the non-linear optimization control method [32].
In this study, for the precise tracking of the CoG, an iterative method of calculating joint angles was developed, which can calculate the CoG at a precision of 0.001 m or lower, resulting in a single solution.There is no need to select the optimum solution among multiple; only the CoG and the trajectories of the feet are sufficient.
The novelty of this study is that the precision of the CoG calculation error can be determined before the robot starts moving.Rather than analyzing and eliminating the cause of errors in the CoG trajectory by looking at the trajectory resulting from the control cycle, it would be better to examine the error that occurs according to the predetermined error in establishing cause-effect relationships.
The rest of this paper is organized as follows.Section 2 gives information on the robot used in this study.The kinematics and dynamics of the robot are explained in Sections 2.2 and 2.3, respectively.The calculation of the CoG and zero-moment point equations is explained in Section 2.4.The main subject of the study, the algorithm, is explained in Section 2.5 with equations and pseudo-code.Initial values and result trajectories are also given.In Section 2.6, the simulation model and parameters of this study are presented.The results of this study are discussed in Section 3, and its conclusions are given in Section 4.

Robot Structure
The robot reported in this paper is the numerical model of the ITU Biped bipedal walking robot [33].The rotational axes of 3 harmonic geared (Harmonic Drive, Frankfurt, Germany) direct-current (DC) motors (Maxon, Sachseln, Switzerland) are placed at the hip joint so that they intersect at a single point, while the rotational axes of 2 DC motors (Maxon, Sachseln, Switzerland) connected to screw nuts (Rexroth, Istanbul, Türkiye) are placed at the ankle joint so that they intersect at a single point.The joint types and positions of the robot are shown in Figure 1.In Figure 1a,   is the inertial coordinate system,   is the base coordinate system,  0 is the starting point for movement,  0 is the pelvis length, and  0 is the pelvis height of the robot.The x axis is represented in red, and the y and z axes are represented in green and blue, respectively. 0 is the hip point of the left leg,  6 is the foot coordinate system, and   are the joint angles of the right leg.To increase readability, the hip point of the right leg and the joint angles of the left leg are not shown in Figure 1. 1 ,  2 , and  3 are the link lengths of the robot.Values of the length parameters are given in Table 1.
The approximate CoGs of the legs are given in Figure 1b.Index R represents the right leg, L is the left leg, and T is the torso.  is the CoG of the robot.  in Figure 1b is the weight of the torso, and   and   are the total weights of the right and left legs, respectively.

Parameter Symbol
The CoGs with respect to the link coordinate system and the masses of the body parts used in the simulation are given in Table 2.

Robot Kinematics
The method developed in this study requires the separate calculation of the CoG of the legs.Because each leg can be represented as a matrix structure, the Denavit-Hartenberg (DH) [34] method is used for kinematic calculations.The link transformations are presented in Table 3.
Equations ( 1)-( 4) present the link transformation matrices for an inertial reference frame.
The transformation matrix of  0 according to   is given in Equation ( 2).The calculation for the left foot is made by writing  0 in the second row and fourth column.
The joint transformation matrix can be written with Equation (3).

𝑇
After these expressions, the position and orientation of the foot can be calculated based on the 0th coordinate system as given in Equation ( 4).
where the left side of the equation is obtained by the placement of the values in the DH matrix.Its right side includes the position and orientation values of the foot obtained in the 0th coordinate system.When the reciprocal of the left side of the equation is taken, the chain from the foot to the hip is obtained.If both sides are multiplied by the matrix 5  6 −1 , the transformation matrix from the ankle to the hip can be obtained [35].As 2 coordinate systems intersect at the ankle, and 3 intersect at the hip, the value of the knee joint is obtained based on the matrix that is formed.According to this, inverse kinematic calculations can be made using Equations ( 5)- (10).
The forward kinematics of the robot can be calculated using Equation (11).
This expression denotes the orientation and position matrix of the robot in an inertial reference frame according to the joint angles used as inputs.

Robot Dynamics
The method developed in this study uses the reference CoG trajectory.Thus, the dynamics of the robot are obtained using the linear inverted pendulum method.In the 3dimensional linear inverted pendulum model (3DLIPM) shown in Figure 2, the desired surface equation is reached by setting   = 0 and   = 0.As the height of the inverted pendulum,   is taken as a constant.After making these arrangements, the differential equations that define the system can be given with Equations ( 12) and (13).
Step positions are set in a matrix , as given in Equation ( 14).The trajectory of the CoG is obtained as the output.
Using Equations ( 12)-( 14), for a step length of 0.3 m (  ), a step width of 0.065 m (  ), and a CoG height of 0.45 m (  ), the trajectory of the CoG is found, as shown in Figure 3.When the foot position is taken as a reference, the height of the hip of the robot in this case is 0.826 m.The step height is 0.075 m (ℎ  ).The foot trajectories are continuous up to the second derivation [36].In Figure 3, the robot starts its movement by taking a step with its right leg.It walks for a total of 6 steps and stops.

CoG and ZMP Calculations
The ITU Biped has a six-axis force-torque sensor at its ankle (ATI Mini85 F/T Transducer, ATI, Apex, NC, USA).ZMP is indirectly measured using the parameters shown in Figure 4.
Because no torque is generated in parallel with the foot plane according to the definition of the ZMP [10], the position of the ZMP based on the reaction on the sensor created by the force and moment at the ZMP is calculated using Equations ( 15) and ( 16).

𝑝 𝑧 =
+  3     (15) Individual CoGs of the robot bodies are calculated with Equation (18).Before using Equation (18), Equation ( 17) must be used for calculating positions of the bodies with respect to the inertial coordinate system.
In the algorithm, the CoG of the robot is calculated using the CoGs given in Figure 1b in Equation (19).

Converging Center of Gravity Algorithm
For the precise calculation of the CoG, joint angles are first calculated using the current positions.Using these angles, the CoG of the robot is calculated using Equation (19).The vector from the current to the reference CoG is found using Equation (20).
Using Equation ( 21), the hip connected to the foot that is in contact with the floor is moved to a new position by multiplying the unit vector by a certain coefficient  (0.001 in this study).
The hip connected to the foot in motion is also moved accordingly.The joint angles and the CoG of the robot are calculated again based on these new hip positions.This process continues until the difference between the reference and current CoGs becomes smaller than the desired value.In our study, the error margin is taken as 0.001 m.The calculation is first made for the fixed foot.Afterward, the indices of the fixed and moving feet are changed, the foot in motion is assumed to be fixed, and the hips are moved.In this way, the masses and positions of all robot components are utilized.
The pseudo-code of the process is given in Algorithm (Equation ( 11)) 20: Continue cycle 21: End if 22: transitory ⇐ j 23: j ⇐ i 24: i ⇐ transitory 25: End while 26: End for In this method, the error between the reference and current CoGs is first calculated.The result of running the algorithm given in Algorithm 1 is shown as the blue track in Figure 5.The walking trajectory is obtained by running the algorithm for all values on the trajectory.

Simulation Model
A simulation model is used to avoid mechanical rigidity, gearbox backlash, and signal delay issues and to obtain the effectiveness of the CCG algorithm.A dynamic model in which the limb angles calculated using this algorithm are used as inputs is obtained as in Figure 6 using MATLAB R2023b SimScape MultiBody [37].The foot positions in the gait of the robot are predetermined.To define contact with the floor, the "Spatial Contact Force" [38] block of SimScape MultiBody that defines contact between spherical and planar surfaces is used.A total of 4 contact spheres are added at the corners on both feet of the robot with a radius of 0.01 m.The "Inertia Sensor" component in MATLAB Multibody [39] is used to measure the actual CoG of the mechanism in the simulation.The parameters that are used in this study are presented in Table 4.

Results
Two simulations that use the same walking parameters are run.The CCG algorithm is used in the first simulation.Joint angles are calculated with the CCG algorithm for precise CoG tracking in each position in the trajectory.In the second simulation, the CoG is calculated before the movement starts.The vector is assumed to be constant between the pelvis and the CoG.The CCG algorithm is compared to the method given in [9].Other methods restrict the movement of the robot and require additional trajectories.Both simulations are run as an open loop.Neither a balancer nor a controller is used.
The timespan of the simulation is divided into 10 equal intervals, as shown in both Figure 7a,b.A screenshot of the simulation is taken and overlaid on the previous moment at the start of the intervals.The leftmost pose is the starting pose of the simulation in each panel in Figure 7.The robot moves forward in the positive x direction, which is represented by the red arrow in Figure 7.The poses of the robot with and without CCG are presented in Figure 7.As seen in (a), the robot is able to follow the trajectory without falling when CCG is used.When CCG is not used, as shown in (b), the robot is unable to preserve its stance and falls by losing its balance after taking its fourth step.The fall of the robot in the fourth step can be seen as an inverted robot pose in (b).
Figure 8 shows the reference ZMP and CoG trajectory, as well as a comparison of the trajectories of the CoG obtained with and without CCG.While the result of the simulation run with CCG is compatible with the reference CoG, the trajectory calculated without CCG starts to deviate after the second step and results in the robot falling in the fourth step.The starting points of motion are very close to each other.There is sliding in the feet during motion as only four points contact the floor.This explains the errors observed in the trajectories along the x and y axes.The calculation times of this method are also obtained.In a computer equipped with Windows 10 Pro 22H2, Core i7 2700, 16 GB of memory, and 240 GB SSD hardware, running MATLAB R2023b, the calculation time for the first value on the trajectory is 0.001662 s, while it is 0.000414 s for the following calculations.This shows that it is possible to make calculations 2415 times per second.Accordingly, the calculation method that is developed here can calculate joint angles according to a given CoG and foot trajectories at the desired level of precision in the real-time control of bipedal walking robots.
The results of the simulation with and without CCG are given in Figures 9 and 10, respectively.The ZMP values and CoG trajectory are in good agreement, as shown in Figure 9, which presents the result of the simulation run with CCG.Due to the hysteresis in contact forces, there are deviations in the ZMP values.However, because these values remain within the support geometry, the robot does not fall and can complete its walk.

Conclusions
The variation in limb positions using the unit vector of the vector between the target and reference centers of gravity, rather than joint angles, allows for the precise determination of the CoG.Using this method, the robot can walk on an even surface without needing any feedback or control.
There is no need for the CoG of the robot to be higher than the hip level.Therefore, there is no need to design the legs as very light components and the trunk as a heavy component.Using the CCG algorithm, the robot can walk even if its CoG is lower than its hips.
This method has advantages over those in the literature mentioned in this article.The model developed in the study allowed for the tracking of the CoG trajectory with a 0.0238 m error in the numerical simulation.This value is lower than 0.18 in [31] and lower than 0.03-0.4m in [32].
The CCG algorithm can be used with any position-controlled bipedal robot.The only inputs required are the reference feet and CoG trajectories.
The CCG algorithm can also be used in situations where the CoG trajectory needs to be regenerated precisely before the movement of the robot starts.

Figure 1 .
Figure 1.Joint angles and basic dimensions of the robot: (a) joint and length parameters; (b) leg indices and approximate CoGs.

Figure 3 .
Figure 3. Trajectory of the CoG and fixed foot positions.

Figure 5
displays the graphical representation of the legs of the robot.The right leg is shown in blue, and the left leg is shown in red.The CoG at the beginning of the trajectory calculations is called the current CoG and is shown as a red circle.The reference CoG is called the target CoG and is shown as a green circle.

Figure 6 .
Figure 6.Simulink model diagram.The joint angles of the left and right legs are input to the model.The model applies trajectories to the robot.ZMP and CoG are calculated in the model.Since the floor is flat and the CoG height of the robot is constant in the simulation, only the x and y components of the ZMP and CoG are plotted.

Figure 9 .
Figure 9. Result of simulation with CCG.

Figure 10 .
Figure 10.Result of simulation without CCG.On the other hand, in the result of the simulation run without CCG in Figure10, the CoG and ZMP values are compatible during the first two steps but deviate afterwards, and the trajectory showing the robot fall is seen in the fourth step as a result of the disruption of this compatibility in the third step.In addition, CoG tracking error values of the CCG algorithm are illustrated in Figure11.

Figure 11 .
Figure 11.CoG tracking error values of CCG algorithm.

Figure 11
Figure 11 displays the errors between the calculated and simulated CoG values.The greatest error value was 0.0559 m, the smallest error value was 9.6353 × 10 −4 m, and the average error value was 0.0238 m.

Table 2 .
CoGs and masses of the body parts.

Table 3 .
Link transformations according to the DH method. 1.