# Real-Time FPGA-Based Balance Control Method for a Humanoid Robot Pushed by External Forces
## 3 FPGA-Based Balance Control Method
### Inverse Kinematics  
The inverse kinematics module is proposed and implemented to determine the joint angles \(\theta\) based on the obtained stepping trajectories \(P=(P_{RA},P_{LA})\) by the trajectory planning module so that the robot can step out its foot to maintain balance. A humanoid robot has an ability to move with its feet to the next location. The stepping trajectories \(P{=(P_{RA},P_{LA})}\) are particular points of the right and left foot in the space. Therefore, the inverse kinematics are used to calculate the angle at which each joint should be rotated. The joint angles \(\theta\) can be solved through the relationship between links and joints of the robot to reach those particular points in the space.  
The link coordinate system of the humanoid robot is shown in Figure 11, where \(l_{t}\) and \(l_{c}\) are respectively the lengths of the robot's thigh link and calf link, and \(L_{R}^{x}(L_{L}^{x})\), \(L_{R}^{y}(L_{L}^{y})\), and \(L_{R}^{z}(L_{L}^{z})\) are respectively the step length, step width, and lift height of the right (left) foot. Observed from the front of the robot, \(\theta_{RH}^{rol}(\theta_{LH}^{rol})\) and \(\theta_{RA}^{rol}(\theta_{LA}^{rol})\) are the angles of the hip joint and ankle joint of the right (left) foot, respectively. Observed from the right side of the robot, \(\theta_{RH}^{pit}(\theta_{LH}^{pit})\), \(\theta_{RK}^{pit}(\theta_{LK}^{pit})\), and \(\theta_{RA}^{pit}(\theta_{LA}^{pit})\) are the angles of the hip joint, knee joint, and ankle joint of the right (left) foot, respectively.  
Based on the link coordinate system in Figure 11a and the inverse kinematics, \(\theta_{RH}^{rol}\), \(\theta_{RA}^{rol}\), \(\theta_{LH}^{rol}\), and \(\theta_{LA}^{rol}\) can be respectively obtained from \(L_{R}^{y},L_{R}^{z}\), \(L_{L}^{y}\), and \(L_{L}^{z}\). They are described as follows:  
\[\theta_{RH}^{rol}=\tan^{-1}(\frac{L_{R}^{y}}{L_{R}^{z}}) \tag{19}\]  
Figure 11: Link coordinate system of the humanoid robot. (**a**) Front view. (**b**) Right side view.  
Figure 10: The stepping trajectories of the forward balance strategy. (**a**) Trajectory oscillators on the left ankle. (**b**) Trajectory oscillators on the right ankle.  
\[\theta_{RA}^{rol}=\theta_{RH}^{rol} \tag{20}\]  
\[\theta_{LH}^{rol}=\tan^{-1}(\frac{L_{L}^{y}}{L_{L}^{z}}) \tag{21}\]  
and  
\[\theta_{LA}^{rol}=\theta_{LH}^{rol} \tag{22}\]  
Similarly, based on the link coordinate system in Figure 11b and the inverse kinematics, \(\theta_{RH}^{pit}\), \(\theta_{RK}^{pit}\), \(\theta_{RA}^{pit}\), \(\theta_{LH}^{pit}\), \(\theta_{LK}^{pit}\), and \(\theta_{LA}^{pit}\) can be respectively obtained from \(L_{R}^{x}\), \(L_{R}^{z}\), \(L_{L}^{x}\), and \(L_{L}^{z}\). They are described as follows:  
\[\theta_{RH}^{pit}=\cos^{-1}(\frac{L_{R}^{x}{}^{2}+L_{R}^{z}{}^{2}+l_{L}^{z}{} ^{2}-l_{c}{}^{2}}{2l_{t}\sqrt{L_{R}^{x}{}^{2}+L_{R}^{z}{}^{2}}})+\tan^{-1}( \frac{L_{R}^{x}}{L_{R}^{z}}) \tag{23}\]  
\[\theta_{RK}^{pit}=\pi-\tan^{-1}(\frac{l_{t}\cos(\theta_{RH}^{pit})}{l_{t}\sin (\theta_{RH}^{pit})})-\tan^{-1}(\frac{L_{R}^{z}-l_{t}\cos(\theta_{RH}^{pit})} {l_{t}\sin(\theta_{RH}^{pit})-L_{R}^{x}}) \tag{24}\]  
\[\theta_{RA}^{pit}=\tan^{-1}(\frac{l_{t}\sin(\theta_{RH}^{pit})-L_{R}^{x}}{L_{R }^{z}-l_{t}\cos(\theta_{RH}^{pit})}) \tag{25}\]  
\[\theta_{LH}^{pit}=\cos^{-1}(\frac{L_{L}^{x}{}^{2}+L_{L}^{z}{}^{2}+l_{L}{}^{2} -l_{c}{}^{2}}{2l_{t}\sqrt{L_{L}^{x}{}^{2}+L_{L}^{z}{}^{2}}})+\tan^{-1}(\frac{L _{L}^{x}}{L_{L}^{z}}) \tag{26}\]  
\[\theta_{LK}^{pit}=\pi-\tan^{-1}(\frac{l_{t}\cos(\theta_{LH}^{pit})}{l_{t}\sin (\theta_{LH}^{pit})})-\tan^{-1}(\frac{L_{L}^{z}-l_{t}\cos(\theta_{LH}^{pit})} {l_{t}\sin(\theta_{LH}^{pit})-L_{L}^{x}}) \tag{27}\]  
and  
\[\theta_{LA}^{pit}=\tan^{-1}(\frac{l_{t}\sin(\theta_{LH}^{pit})-L_{L}^{x}}{L_{L }^{z}-l_{t}\cos(\theta_{LH}^{pit})}) \tag{28}\]  
Thus, the joint angles described by Equations (19)-(28) are required for the humanoid robot to achieve stability through the proposed balance control method.