# Real-Time FPGA-Based Balance Control Method for a Humanoid Robot Pushed by External Forces
## 3 FPGA-Based Balance Control Method
### Push Recovery Balance Control  
The push recovery balance control module is proposed and implemented to determine the capture point \(x_{Sup}\) based on the inclination angle \(\phi\) obtained by the external force detection module to let the humanoid robot maintain balance under an external force. When a human is suddenly hit by a large external force, it is natural for this human to immediately swing the leg to maintain balance. Therefore, a linear inverted pendulum with a flywheel model, as shown in Figure 7, is used to calculate the capture point \(x_{Sup}\). The motion equations are defined as follows:  
\[\ddot{x}_{COM}=\frac{g}{h}\cdot x_{COM}-\frac{1}{mh}\cdot\tau \tag{3}\]  
\[\ddot{\theta}_{b}=\frac{1}{J}\cdot\tau \tag{4}\]  
where \(m\) is the center of mass (COM), \(h\) is the height of mass, \(g\) is the gravitational acceleration constant, \(l\) is the length of the leg, \(f\) is the force at the support point, \(\tau\) is the motor torque on the flywheel, and \(J\) is the rotational inertia of the flywheel. \(x_{COM}\) is the COM of the robot and \(\theta_{b}\) is the flywheel angle with respect to the vertical direction.  
When a humanoid robot is under an external force, suppose the upper body of the robot does not rotate. Therefore, the motor torque on the flywheel \(\tau\) is set as 0 to maintain the original pose of the upper body and find the capture point. An orbital energy \(E_{LIP}\) is defined to represent the torque \(\tau\)[19; 20] and it can be expressed by  
\[E_{LIP}=\frac{1}{2}{x_{COM}}^{2}-\frac{g}{2h}\cdot(x_{COM}-x_{Sup})^{2} \tag{5}\]  
when \(E_{LIP}=0\), the robot will keep its balance. Thus, the capture point \(x_{Sup}\) can be computed by  
\[x_{Sup}=x_{COM}+\dot{x}_{COM}\sqrt{\frac{h}{g}} \tag{6}\]  
A linear inverted pendulum model is also applied to require the COM state of the robot which would be generated based on the obtained inclination angle \(\phi\). Therefore, \(x_{COM}\) and \(\dot{x}_{COM}\) can be described by  
\[x_{COM}=l\sin(\phi) \tag{7}\]  
and  
\[\dot{x}_{COM}=l\cos(\phi)\dot{\phi} \tag{8}\]  
Figure 7: Graphical description of a linear inverted pendulum with a flywheel model.  
According to the energy conservation law, the kinetic energy can be represented by  
\[\frac{1}{2}{m}{l}^{2}{\dot{\phi}}^{2}=mgl(1-\cos(\phi)) \tag{9}\]  
The velocity of the inclination angle \(\dot{\phi}\) of the robot can be required as follows:  
\[\dot{\phi}=\sqrt{2\frac{g}{l}(1-\cos(\phi))} \tag{10}\]  
Thus, the capture point \(x_{Sup}\) can be determined by substituting \(x_{COM}\) and \(\dot{x}_{COM}\) into Equation (6). The capture point \(x_{Sup}\) is used to calculate the step length based on the strength of the external force which is presented by the inclination angle \(\phi\). The safety thresholds, \(\varepsilon_{1}\) and \(\varepsilon_{2}\), are set to compare with the external force which is detected from the COM of the humanoid robot through a gyroscope and an accelerometer. If the detected external force is more than \(\varepsilon_{1}\) or less than \(-\varepsilon_{2}\), the positive or negative value of \(x_{Sup}\) will be obtained to execute the forward or backward balance strategies, and then the robot will take a step to resist the external force. In contrast, if the detected external force is between \(\varepsilon_{1}\) and \(-\varepsilon_{2}\), the robot will just maintain its standing posture. The pseudo code of the proposed push recovery balance control is shown in Algorithm 1.  
```
Method: Push Recovery Balance Control.  Initialize the safety thresholds \(\varepsilon_{1}\) and \(\varepsilon_{2}\) \(\phi\leftarrow\) Update from a gyroscope and an accelerometer if(\(\phi>\varepsilon_{1}\)) or (\(\phi<-\varepsilon_{2}\)) then \(\dot{\phi}=\sqrt{2g(1-\cos(\phi))/l}\) \(x_{COM}=l\sin(\phi)\) \(\dot{x}_{COM}=l\cos(\phi)\dot{\phi}\) \(x_{Sup}=x_{COM}+\dot{x}_{COM}\sqrt{h/g}\) else \(x_{Sup}=0\) end
```  
**Algorithm 1** Pseudo code of the proposed push recovery balance control.