Implementation of Push Recovery Strategy Using Triple Linear Inverted Pendulum Model in “T-FloW” Humanoid Robot

Push recovery is one of humanbehaviorwhich is a strategy to defend the body from anexternal force in any environment. This paper describes push recovery strategy which usesMIMO decoupled control system method. The dynamics system uses aquasi-dynamic system based on triple linear inverted pendulum model (TLIPM). The analysis of TLIPMuses zero moment point (ZMP) calculation from ZMP simplification in last research. By using this simplification of dynamics system, the control design can be simplified into 3 serial SISOwith known and uncertain disturbance models in each inverted pendulum. Each pendulum has different plan to damp the external force effect. In this experiment, PID controller (closed- loop)is used to arrange the damp characteristic.The experiment result shows thatwhen using push recovery control strategy (closed-loop control) is about 85.71% whilewithout using push recovery control strategy (open-loop control) it is about 28.57%.


Introduction
Push recovery challenge hasthe main goal, which withstands a strong push and keeps standing. The robot receives a push from an object that swings against its body. The robot receives one or more pushes and withstand successfully to fulfil the push recovery challenge [1]. The humanoid robot in this discussion is called "T-FLoW". In this discussion, we use aT-FLoW humanoid robot with 28 Degrees of Freedom (DoF) version. To complete the push recovery challenge, the MIMO decoupled control systemanalysisis usedto design the control system [2][3]. Another research about push recovery is also using similar action [11][12][13][14][15][16][17][18][19][20][21], but the difference in this discussion is the dynamics system approach and control system. With a lot of DoF, the dynamics model of T-FLoWisapproached by using Triple Linear Inverted Pendulum Model (TLIPM) [4][5].
With this approach, the dynamic system model of T-FLoW will be quasi-dynamics system model and itis analyzed by using thecalculation of zero moment point (ZMP) analysisas in the previous research [6][7][8]. By using this simplification of dynamics system, the control design can be simplified into 3 serial SISO with known and uncertain disturbance (mass and external force) in each inverted pendulum (IP).Each pendulum has different plan to damp the external force effect (uncertain disturbance). In this plan, PID controller is used to arrange the damping characteristic [9][10]. In the experiment result will explainingabout thecomparison between open-loop push recovery control strategy (without PID controller) and closed-loop (3 serial SISO) push recovery control strategy (with PID controller).

Originality
The differences with previous research about push recovery strategyarein the dynamics system approach and control system usage. As mentioned before, the dynamics system approach is using Triple Linear Inverted Pendulum Model (TLIPM). In previous research in reference [11][12][13][14][15][16][17][18][19][20][21], they are using Double Linear Inverted Pendulum Model (DLIPM) or SingleLinear Inverted Pendulum Model (SLIPM). The control system design is using MIMO decoupled control system analysis with PID controller. This proposed control system is used to smoothing the movement of each IP. In another research in reference [11][12][13][14][15][16][17][18][19][20][21], they are not using MIMO control analysis. The last difference is in the domain input for thecontrol system. In our control system is using cartesian space input for the control system and another research is using joint space input for their control system.

Mechanical Design (T-FLoW)
Mechanical design of T-FLoW humanoid robot is shownin figure 1. T-FLOW humanoid robot has 2 version of mechanical design which is using 28 Degree of Freedom (DoF) and using 38-DoF. But, in this discussion, 28-DoF version of T-FLoW humanoid robot will be used. The configuration of 28-DoF is 16-DoF in the left and right leg, 3-DoF in the waist, 6-DoF in the left and right arm, and 3-DoF in the head. The total height and width of T-FLoW humanoid robot are about 112.67cm and 27.54cm. The configuration of each joint and number of actuators are shown in table 1.  The lower body is a left leg, right leg, and hip. T-FLoW humanoid robot using parallel link mechanical system called parallelogram mechanism in the left and right legs. The parallelogram mechanism is used to add more active torque in the legs. The upper body is a left arm, right arm, and waist. The upper body is using serial link mechanical system.

Quasi Dynamics System Model
In the last research of T-FLoW humanoid robot, ZMP classic calculation was used to obtain the dynamics characteristic. Where In which ,( , , ) is the position vector of CoM.This position can be obtained from position calculation of each joint, which is used in the robot.
, ( , , ) is the position vector of ZMP.This position is the dynamics characteristic of the system. is the mass total from the system. ℒ ( , , ) and ( , , ) is the angular acceleration (torque) and the linier acceleration (force). isthe linier acceleration of gravitation in Z axis. When discussing control system, the dynamics system must be calculated in real time condition. Because of it, the dynamic system model of T-FLoW humanoid robot is approached by using Triple Linear Inverted Pendulum Model (TLIPM). The simplification design of dynamic system model based on TLIPMis shown in figure 2. The dynamics calculation of T-FLoW humanoid robot is divided into 3 parts which are Head part (IP 1), Trunk part (IP_2), and Legs part (IP_3). As mentioned before, the dynamics system is simplified from multi-body analysis into single body analysis based on the parts. Because of it, the position vector of ZMP for multi-body in Equation 1 from previous research is developed and simplified to obtain dynamics system based on Inverted Pendulum (IP) model.

Control Design based on System Model
As mentioned in theintroduction, T-FLoW humanoid robot is doing a push recovery challenge. The push recovery strategy is how to maintain the robot's posture during an external force without moving the legs (step/walk).The Push recovery in this challenge hasthe main goal, which withstands a strong push and keeps standing.Therobotreceives a push from an object that swings against its body.
The push recovery strategy is adapting the human behavior. When human receives a small external force, they are only correcting the position of their body. The position correction is happening based on the force direction vector. When an external force has X direction (forward direction) and the external force is directly contacting to the upper body of the human, t he human reaction is moving the upper body (trunk) into the same direction vector of external force and the legs are moving into the opposite direction vector of external force. This reaction is adapted by T-FLoW humanoid robot. As seen in figure 6, the T-FLoW humanoid robot is receiving an external force through the upper body (trunk). The upper body (trunk) of the robot is following the direction vector of external force. But, the legs and the head body is following the opposite direction vector of external force. The strategy to damp the external force is how to arrange the position vector of CoM in each IP to not be outside of support polygon (SP). For the first IP, the strategy is how to maintain the condition at the angular position reference which is the first IP is remaining in upright condition ( , , = 0). For second IP, the strategy is how to damp the external force disturbance because the external force is directly contacted with the second IP. In the second IP (Trunk), the arms are used to shift the position vector of CoM in the Trunk. For the third IP (Legs), the strategy is how to compensate the second IP during external force. The compensation is from direction of the angular position in second and third IP which is when second IP is moved into forward direction because the external force, so the third IP is moved into opposite direction (backward direction). This compensation is happened because the third IP is the most important part of the robot which is the main base of the robot body.
The MIMO decoupled control design based on system model is used in push recovery control strategy. As mention it before, the dynamic system of T-FLoW humanoid robot is using quasidynamic system model approach based on TLIPM.
With this approach, the MIMO decoupled control design is simplified into 3 serial SISO control design when implemented into the system. Basically, the MIMO control system example is described in Equation 13 and MIMO decoupled control system which is multi SISO control system example is described in the Equation 14.
Based on equation 12, The design of the control system (closed-loop) in the T-FLoW humanoid robot can be seen in figure 6 and the strategy to damping the external force is shown in figure 5. From , and , . But, these 6 parts of SISO control system is categorized into 3 IP parts which is first IP (Head) is , and , , second IP (Trunk) is , and , , and third IP (Legs) is , and , . The control system is work based on cartesian data input from dynamics system calculation.
The FK process is similar to IK process, just calculating the local transformation calculation in each IP. From anangular position in each IP, the real position vector in each IP isobtained by using Equation 16 as follows.
The value of , , , , , , , , , , , , and , , , , , in the PID controller will be tune by using Ziegler-Nichols tuning methods. This method is used as one of the way to tune the PID controller. This method is work based on step response requirement of the system (figure 8) [22]. The tuning formulas can be seen in table 2.

Experiment Result
The experiment of push recovery in this section is using pendulum model to generate an external force. The robot is receiving the external force from the backward direction (X-axis).Before the experiment, the PID controller (closed-loop) is tuned by using Ziegler-Nichols tuning methods and the result of this tune can be seen in table 3. The pendulum (force generator) is drawn to certain angles so the pendulum will swing and causing an external force against the robot's body. The pendulum is swinging directly into trunk part of the robot body. Because in this experiment is only focus on the implementation of push recovery strategy during external force directly into the trunk with the only From table5 can be seen the experiment result of push recovery strategy implementation comparison between open-loop control system and closed-loop control system. When the robot doing push recovery challenge using open-loop control system, the percentage of successful withstand is about 28.57% and robot falls is about 71.43%. When the robot doing push recovery challenge using closed-loop control system, the percentage of successful withstand is about 85.71% and robot falls is just about 14.29%. From the experiment also known the maximum value of external force that can be handled by push recovery strategy is about 30 degrees of pendulum angle. In figure 9 shows the push recovery strategy at a maximum angle about 30 degrees of pendulum angle and the comparison in each IP can be seen in figure 10, figure 11, and figure 12. From the position vector comparison shows when using closed-loop control system, the robot withstands and keeps standing (red line) and when using open-loop control system, the robot is fall (blue line). Table 3.Tuned Constanta of PID controller using Ziegler-Nichols tuning methods.   Figure 9. Implementation sample of push recovery strategy from table 5 during maximum external force (30 degrees of pendulum angle). In this figure shows, the robot controlsystemreached the critical condition because of theposition vector of ZMP almost outside SP.
In figure 10, figure 11, and figure 12, there are Fall area and Safe area. Fall area is area when the robot is impossible to maintain the balance. When the robot reaches this condition even in short time, the robot will fall. The safe area is area when therobot is still possible to balancing the body. When the robot reaches this condition, the robot has a chance to maintain the body balance to avoid fall condition.
The Inverted Pendulum 1 is shown in figure10. In data without PID controller (open-loop), the standard deviation (SD) of data is about 14.12 and the maximal minimal angle of data is about (46 ~ -1) degree. In data with PID controller (closed-loop), the SD of data is about 5.163 and the maximal minimal angle of data is about (29 ~ -18) degree  The Inverted Pendulum 2 is shown in figure11. In data without PID controller (open-loop), the standard deviation (SD) of data is about 15 and the maximal minimal angle of data is about (50 ~ 1) degree. In data with PID controller (closed-loop), the SD of data is about 9.738 and the maximal minimal angle of data is about (33 ~ -10) degree

Conclusion and Discussion
The main result of the experiment when receiving maximum external force at 30 degrees of pendulum angle is arobotwith open-loop control systemhas been falling and arobot with closed-loop control systemhas been successfully (withstanding) from external force and keeps standing.The difference in percentage of successful withstand is about 85.71% when using push recovery control strategy (closed-loop control system) and about 28.57% without using push recovery control strategy (openloop control system). But a new problem is appeared, that is, push recovery control strategy (closed-loop control system) cannot handle and damp the external force up to 30 degrees of pendulum angle. It isbecausethe position vector of ZMP in the critical condition (the position vector of ZMP almost outside SP). With this problem, the future research will be focusedon a strategy deals with the walking stepsoff whenreaching the critical condition and try touse another controller such as FUZZY, Neural Network, and etc to improve and compare the differences result from thepresentresearch.