## 4 The Control Algorithm for a Hydraulic Quadruped Robot
### Centroid Control in Touchdown Stage
\begin{array}{l}F_{bx}=F_{Fx}+F_{Hx}\\ F_{by}=F_{Fy}+F_{Hy}\\ F_{bz}=F_{Fz}+F_{Hz}\\ T_{bx}=T_{Fx}+T_{Hx}+2b(F_{Fz}-F_{Hz})\\ T_{by}=T_{Fy}+T_{Hy}+2a(F_{Fz}-F_{Hz})\\ T_{bz}=T_{Fz}+T_{Hz}-2b(F_{Fy}-F_{Hy})+2a(F_{Fx}-F_{Hx})\end{array} \right. \tag{25}\]  
Let  
\[\mathbf{Q}=\left[ \begin{array}{cccccccccccc}1&0&0&0&0&0&1&0&0&0&0&0\\ 0&1&0&0&0&0&0&1&0&0&0&0\\ 0&0&1&0&0&0&0&0&1&0&0&0\\ 0&0&2b&1&0&0&0&0&-2b&1&0&0\\ 0&0&2a&0&1&0&0&0&-2a&0&1&0\\ 2a&2b&0&0&1&-2a&-2b&0&0&0&1\end{array} \right] \tag{26}\]  
We can substitute \(\mathbf{Q}\) back into Equation (25) to get a set of simplified equations:  
\[\mathbf{F}\mathbf{T}_{b}=\mathbf{Q}\cdot\mathbf{F}\mathbf{T}_{FH} \tag{27}\]  
So that we can rearrange Equation (24) into the following intuitive form:  
\[\mathbf{F}\mathbf{T}_{b}=\mathbf{Q}\cdot\left[ \begin{array}{cc}\mathbf{J}_{FT_{F}}&\mathbf{0} _{6\times 3}\\ \mathbf{0}_{6\times 3}&\mathbf{J}_{FT_{H}}\end{array} \right]\left[ \begin{array}{c}\mathbf{ \tau}_{F}\\ \mathbf{\tau}_{H}\end{array} \right] \tag{28}\]  
As shown in Figure 4, the resultant forces and moments at the centroid of the trunk are as follows:  
\[\left\{ \begin{array}{l}F_{bz}=k_{ph}(h-h^{d})+k_{dh}\dot{h}\\ T_{bx}=k_{px}(q_{1}-q_{1}q^{d})+k_{dx}\dot{q}_{1}\\ T_{by}=k_{py}(q_{2}-q_{2}q^{d})+k_{dy}\dot{q}_{2}\end{array} \right. \tag{29}\]  
where \(k_{ph}\), \(k_{px}\) and \(k_{py}\) are the elastic coefficients of the virtual components; \(k_{dh}\), \(k_{dx}\) and \(k_{dy}\) are the damping coefficients of the virtual components; \(h^{d}\) is the desired height of the trunk; \(h\) is the actual height of the trunk; and \(q^{1d}\) and \(q^{2d}\) are the desired roll angle and pitch angle of the robot trunk. \(T_{bx}\), \(T_{by}\) and \(T_{bz}\) represent the torque of the landing leg acting on the robot trunk along \(x\), \(y\) and \(z\) axes. The actual attitude angles and the height of the robot trunk are close to their theoretical values when choosing reasonable elastic coefficients and damping coefficients. By substituting Equation (29) into Equation (28), the following results can be obtained:  
\[\left[ \begin{array}{c}F_{tx}\\ T_{tx}\\ T_{by}\end{array} \right]=\left[ \begin{array}{cccccccccccccccc}0&0&1&0&0&0&0&0&1&0&0 &0\\ 0&0&2b&1&0&0&0&0&-2b&1&0&0\\ 0&0&2a&0&1&0&0&0&-2a&0&1&0\end{array} \right]\left[ \begin{array}{c}I_{FT_{F}} &0_{6\times 3}\\ 0_{6\times 3}&J_{FT_{H}}\end{array} \right]\left[ \begin{array}{c}\tau_{F}\\ \tau_{H}\end{array} \right] \tag{30}\]  
Let be  
\[S_{3\times 6}=\left[ \begin{array}{cccccccccccc}0&0&1&0&0&0&0&0&1&0&0&0\\ 0&0&2b&1&0&0&0&0&-2b&1&0&0\\ 0&0&2a&0&1&0&0&0&-2a&0&1&0\end{array} \right]_{3\times 12}\left[ \begin{array}{ c}J_{FT_{F}}&\mathbf{0}_{6\times 3}\\ \mathbf{0}_{6\times 3}&J_{FT_{H}}\end{array} \right]_{12\times 6} \tag{31}\]  
However, it is impossible to solve all six joint torques of two landing legs according to Equation (29). Our solution allows for the output requirements of actuators so as to derive the joint torques. Therefore, the following equations are added:  
\[\left\{ \begin{array}{c}\tau_{F1}=\tau_{H1}\\ \tau_{F2}=\tau_{H2}\\ \tau_{F3}=\tau_{H3}\end{array} \right. \tag{32}\]  
From Equations (25), and (29) to (32), the joint torques of the two landing legs can be concluded:  
\[\left[ \begin{array}{c}\tau_{F1}\\ \tau_{F2}\\ \tau_{F3}\\ \tau_{H1}\\ \tau_{H2}\\ \tau_{H3}\end{array} \right]=\left[ \begin{array}{c}\mathbf{S}\\ \mathbf{P}\end{array} \right]^{-1}\left[ \begin{array}{c}k_{ph}(h-h^{d})+k_{dh} \dot{h}\\ k_{px}(q_{1}-q_{1}^{d})+k_{dx}\dot{q}_{1}\\ k_{py}(q_{2}-q_{2}^{d})+k_{dy}\dot{q}_{2}\\ 0\\ 0\\ 0\end{array} \right] \tag{33}\]  
where  
\[\mathbf{P}=\left[ \begin{array}{cccccccc}1&0&0&-1&0&0\\ 0&1&0&0&-1&0\\ 0&0&1&0&0&-1\end{array} \right] \tag{34}\]  
As shown in Equation (33), the roll angle, pitch angle and height of the trunk can be controlled with reference to the joint torques of the landing leg of the hydraulic quadruped robot. Figure 4: Centroid control model of hydraulic quadruped robot.