# Whole-Body Motion Generation for Balancing of Biped Robot
## 3 Generation of COM Trajectory
### COM Trajectory Generation Using MPC  
In this paper, linear MPC was used to generate COM trajectory for tracking the modified ZMP. Assuming that the jerk is constant over the interval \(0\leq t<T\) and given the control step time \(T\), the state at any discrete time step \(k+1\) can be expressed:  
\[\mathbf{x}_{k+1}=\mathbf{Ax}_{k}+\mathbf{B}\,\vec{x}_{k} \tag{16}\]  
Figure 3: Scheme of CP control for stabilizing the divergent dynamics of the CP. where  
\[x_{k}=\left[ \begin{array}{c}x_{k}\\ \dot{x}_{k}\\ \dot{x}_{k}\end{array} \right]\,\ \mathbf{A}=\left[ \begin{array}{ccc}1&T&T^{2}/2\\ 0&1&T\\ 0&0&1\end{array} \right]\,\ \mathbf{B}=\left[ \begin{array}{c}T^{3}/6\\ T^{2}/2\\ T\end{array} \right]\]  
Equation (2a) with discrete time can be expressed:  
\[\mathbf{P}_{k}=x_{k}-\frac{1}{\omega^{2}}\ddot{x}_{k}=\left[ \begin{array}{cc}1&0 &-1/\omega^{2}\end{array} \right]\mathbf{x}_{k}=\mathbf{C}\mathbf{x}_{k} \tag{17}\]  
With a prediction horizon of \(n\) steps,  
\[\mathbf{X}_{k+1} =\widehat{\mathbf{A}}\mathbf{x}_{k}+\widehat{\mathbf{B}}\ddot{\mathbf{X}}_{k} \tag{18}\] \[\mathbf{P}_{k+1} =\mathbf{P}_{ps}\mathbf{x}_{k}+\mathbf{P}_{pu}\dddot{\mathbf{X}}_{k} \tag{19}\]  
where  
\[\mathbf{X}_{k+1}=\left[ \begin{array}{c}\mathbf{x}_{k+1}\\ \mathbf{x}_{k+2}\\ \vdots\\ \mathbf{x}_{k+n}\end{array} \right]\,\ \widehat{\mathbf{A}}=\left[ \begin{array}{c}\mathbf{A}\\ \mathbf{A}^{2}\\ \vdots\\ \mathbf{A}^{n}\end{array} \right]\,\ \widehat{\mathbf{B}}=\left[ \begin{array}{cccc} \mathbf{B}&\mathbf{0}&\cdots&\mathbf{0}\\ \mathbf{AB}&\mathbf{B}&\cdots&\mathbf{0}\\ \vdots&\vdots&\ddots&\vdots\\ \mathbf{A}^{n-1}\mathbf{B}&\mathbf{A}^{n-2}\mathbf{B}&\cdots&\mathbf{B}\end{array} \right]\,\ \ddot{\mathbf{X}}_{k}=\left[ \begin{array}{c}\ddot{x}_{k}\\ \dddot{x}_{k+1}\\ \vdots\\ \ddot{x}_{k+n-1}\end{array} \right]\]  
At any time instance, the velocity state can be expressed:  
\[\dot{\mathbf{X}}_{k+1}=\mathbf{P}_{ps}\mathbf{x}_{k}+\mathbf{P}_{pu}\dddot{\mathbf{X}}_{k} \tag{20}\]  
where  
\[\mathbf{P}_{ps}=\left[ \begin{array}{ccc}0&1&T\\ 0&1&2T\\ \vdots&\vdots&\vdots\\ 0&1&nT\end{array} \right]\,\mathbf{P}_{pu}=\left[ \begin{array}{cccc}T^{2}/2&1& \cdots&T\\ T^{2}/2+T^{2}&T^{2}/2&\cdots&2T\\ \vdots&\vdots&\ddots&\vdots\\ T^{2}/2+(n-1)T^{2}&T^{2}+(n-2)T^{2}&\cdots&T^{2}/2\end{array} \right]\]  
The objective function was designed to achieve ZMP tracking and generate stable COM motions. The objective function is formulated as:  
\[\min_{\mathbf{P}_{k+1}}\frac{\alpha}{2}\abs{\ddot{\mathbf{X}}_{k}}^{2}+\frac{\beta}{ 2}\abs{\dot{\mathbf{X}}_{k+1}}^{2}+\frac{\gamma}{2}\abs{\mathbf{P}_{k+1}-\mathbf{P}_{k+1}^{ des}}^{2} \tag{21}\]  
This can be expressed in a quadratic form as:  
\[\min_{\mathbf{P}_{k+1}}\frac{1}{2}\mathbf{P}_{k+1}^{T}\mathbf{Q}\mathbf{P}_{k+1}+f^{T}\mathbf{P}_{k+1} \tag{22}\]  
where  
\[\mathbf{Q}=\alpha\mathbf{P}_{pu}^{-T}\mathbf{P}_{pu}^{-1}+\beta\mathbf{P}_{pu}^{-T}\mathbf{P}_{pu} ^{T}\mathbf{P}_{eu}\mathbf{P}_{pu}^{-1}+\gamma\mathbf{E}\]  
\[f=\left[\beta\mathbf{P}_{pu}^{-T}\mathbf{P}_{pu}^{T}\mathbf{\left(}\mathbf{P}_{ps}-\mathbf{P}_{pu} \mathbf{P}_{pu}^{-1}\mathbf{P}_{ps}\right)-\alpha\mathbf{P}_{pu}^{-T}\mathbf{P}_{pu}\mathbf{P}_{ps} \right]\mathbf{x}_{k}-\gamma\mathbf{P}_{k+1}^{des}\]where \(\alpha\) is the jerk gain, \(\beta\) is the velocity gain, and \(\gamma\) is the ZMP tracking gain. \(\mathbf{E}\in\mathbb{R}^{n\times n}\) is the identity matrix. The objective function in Equation (21) is designed to achieve several critical goals for stable and smooth locomotion. Specifically, the term penalizing jerk promotes smoother COM trajectories by minimizing rapid changes in acceleration. The velocity term helps to prevent excessively fast COM movements that could lead to instability or violate physical limits. The ZMP tracking term is crucial for maintaining the balance of the robot by ensuring the ZMP follows its desired reference.