# Model Predictive Control of Running Biped Robot
## 4 Hierarchical Motion Control
### COM Trajectory Generation Based on MPC
This optimization problem can be expressed as a canonical quadratic problem:  
\[\min_{U_{z}} \frac{1}{2}U_{z}^{T}Q_{z}U_{z}+P_{z}^{T}U_{z}, \tag{23}\]  
subject to  
\[\underline{U}_{z}<U_{z}<\overline{U}_{z},\]  
where  
\[Q_{z} = \alpha^{Z}_{COM}I_{N}+\beta^{Z}_{COM}\bar{B}^{T}\hat{B}+\gamma^{ Z}_{COM}(I_{N}-\lambda^{2}\hat{S}_{P}\hat{B})^{T}(I_{N}-\lambda^{2}\hat{S}_{P} \hat{B})\in\mathbb{R}^{N\times N},\] \[P_{z} = (\hat{A}\hat{z}(k)-\hat{Z}^{ref})^{T}\bar{B}+(\bar{g}-\lambda^{2} \hat{S}_{P}\hat{A}\hat{z}(k))^{T}(I_{N}-\lambda^{2}\hat{S}_{P}\hat{B})\in \mathbb{R}^{N},\] \[\underline{U}_{z} = -\bar{g}\in\mathbb{R}^{N},\] \[\overline{U}_{z} = U_{z,max}\in\mathbb{R}^{N},\]  
\(I_{N}\in\mathbb{R}^{N\times N}\) is identity matrix. The objective function of horizontal motion is designed to follow the reference position and velocity of the COM and keep the ZMP close to the center of the supporting foot for stability based on Equation (8). The minimized problem for horizontal motion can be expressed as  
\[\min_{U_{x}} \alpha^{X}_{COM}||U_{x}||^{2} + \beta^{X}_{COM}||\hat{X}^{ref}-\hat{X}(k+1)||^{2} \tag{24}\] \[+ \gamma^{Z}_{COM}||U_{z}+\bar{g}-\lambda^{2}\hat{Z}_{P}(k+1)||^{2},\]0  
where \(\alpha^{X}_{COM}\), \(\beta^{X}_{COM}\) and \(\gamma^{X}_{COM}\) respectively denote the weights of optimization for control input, the position of the COM, and ZMP. \(\hat{X}^{ref}\) denotes reference position and speed in the horizontal direction at the end of each support phase, i.e., \(t_{i}=T_{s}^{j}\), which can be calculated based on Equations (9) and (10) with calculated foot placement. For the stable dynamic motion of the COM, constraints based on the friction cone are applied [38]. The contact force in the horizontal plane is constrained to lie in the friction cone defined by  
\[+ \gamma^{Z}_{COM}||U_{z}+\bar{g}-\lambda^{2}\hat{Z}_{P}(k+1)||^{2},\]1  
where \(\bar{\mu}\) denotes the friction coefficient between the ground and the foot of the robot. These minimized problems can be also expressed as a canonical quadratic form:  
\[+ \gamma^{Z}_{COM}||U_{z}+\bar{g}-\lambda^{2}\hat{Z}_{P}(k+1)||^{2},\]2  
subject to  
\[+ \gamma^{Z}_{COM}||U_{z}+\bar{g}-\lambda^{2}\hat{Z}_{P}(k+1)||^{2},\]3  
where  
\[+ \gamma^{Z}_{COM}||U_{z}+\bar{g}-\lambda^{2}\hat{Z}_{P}(k+1)||^{2},\]4 \[+ \gamma^{Z}_{COM}||U_{z}+\bar{g}-\lambda^{2}\hat{Z}_{P}(k+1)||^{2},\]5 \[+ \gamma^{Z}_{COM}||U_{z}+\bar{g}-\lambda^{2}\hat{Z}_{P}(k+1)||^{2},\]6 \[+ \gamma^{Z}_{COM}||U_{z}+\bar{g}-\lambda^{2}\hat{Z}_{P}(k+1)||^{2},\]7  
From the object functions, the control inputs in the horizontal and vertical direction, i.e., \(U_{x}\) and \(U_{z}\), are obtained, which are used to generate the COM trajectory of the biped robot based on Equations (17) and (18). Typically, a biped robot has several joints, and the motion of each joint affects its COM. To follow the generated COM trajectories, for this reason, trajectory generation of the joints which realize the desired COM trajectory is needed [18; 26; 39]. To generate the trajectory of joints to follow the COM trajectory, in this paper, momentum control is applied, which is presented in Section 4.2.