Model Predictive Control for Modeling Human Gait Motions Assisted by Vicon Technology

The mathematical model of 5-link mechanism is shown in Figure 1. This model is used for MPC controller to calculate the optimal controller torques for each joint. Five weighty links are OC, OB, OD, DE, and BE. Link OC is the body. ODE and OBA are feet. Each leg consists of the thigh and lower leg, so that the link OB and OD are hips and unit’s BA and DE are shins. Two legs are considered of the same weight and length.

1.jpg

Figure 1. Model of 5-link mechanism

Joint O connecting the body OC with hips OB and OD will be referred as hip joint, joints B and D, connecting the thigh OB and OD with shins BA and DE, will be referred as knee joints. All joints are assumed to be ideal, i.e. friction in their neglect. This mechanism has seven degrees of freedom. In describing the situation as six generalized coordinates, we choose the following works: coordinates x, y of hip О and five angles ψ, α_1, α_2, β_1, β_2, between the links and the vertical. These angles and directions of their reference are shown in Figure 1: ψ – the angle between the body and the vertical, α_1 and β_1 – the angles between the hips and the vertical, β_1, and β_2, – the angles formed by the vertical tibia. The angle is positive when the relevant unit deviates from the vertical direction in the opposite clockwise direction.

For this simplified model of 5-link, human feet have a small part of all mass, and we may assume that the feet do not have a strong influence on the movement of other parts of human. The following equations are used directly in the system for calculation.

Lagrange equation:

$\frac{d}{d t} \cdot \frac{\partial L}{\partial \dot{q}_{i}}-\frac{\partial L}{\partial q_{i}}=Q_{s}$  (1)

where, Qs - generalizes non conservative force, and

$L=T-V$  (2)

where, L - Lagrangian; T – kinetic energy; V - potential energy;

Kinetic energy:

$T=\frac{1}{2}\left(m v^{2}+2 m(v \omega) p+\Theta \omega^{2}\right)$  (3)

where, m - mass of link; v - absolute velocity; v - pole velocity; w - angular velocity; p - radius vector of the center of mass; $\Theta$  - Inertia moment relative to pole.

$v=\left(\begin{array}{c}\dot{x} \\ \dot{y} \\ 0\end{array}\right)$  (4)

$\omega={\psi}\left(\begin{array}{c}\sin (\psi) \\ \cos (\psi) \\ 0\end{array}\right)$  (5)

inetic energy of link OC point O is a pole, so:

$T_{O C}=\frac{1}{2}\left(\begin{array}{c}m_{k}\left(\dot{x}^{2}+\dot{y}^{2}\right)- \\ 2 K_{r} \dot{\psi}(\dot{x} \cos (\psi)+\dot{y} \sin (\psi))+J \dot{\psi}^{2}\end{array}\right)$  (6)

where, $K_{r}=m_{k} r$; $\mathrm{m}_{\mathrm{k}}$ - Mass OC; r  - distance from O to OC mass center; J - inertia moment OC relative point O.

In finding the kinetic energy for the body OC and hips OB and OD, a pole will take at the point O. Similarly, for parts of BA and DE, the pole point B and D are chosen. For kinetic energy of link OB, point O is chosen as a pole, so:

$T_{O B}=\frac{1}{2}\left(m_{a}\left(\dot{x}^{2}+\dot{y}^{2}\right)\right.$$+2 m_{a} a \dot{\alpha_{1}}\left(\dot{x} \cos \left(\alpha_{1}\right)\right.$$\left.\left.+\dot{y} \sin \left(\alpha_{1}\right)\right)+J_{a}^{0} \dot{\alpha_{1}}^{2}\right)$  (7)

where, $m_{a}$ - mass OB; a - distance from O to OB mass centre; $J_{a}^{0}$ - inertia moment OB relative point O.

Kinetic energy of link BA point B:

$T_{B A}=\frac{1}{2}\left(m_{b}\left(\dot{x}^{2}+\dot{y}^{2}\right.\right.$$+2 \dot{\alpha_{1}} L_{a}\left(\dot{x} \cos \left(\alpha_{1}\right)+\dot{y} \sin \left(\alpha_{1}\right)\right)$$\left.+L_{a}^{2} \dot{\alpha_{1}}^{2}\right)$$+2 K_{b} \dot{\beta}_{1}\left(\dot{x} \cos \left(\beta_{1}\right)+\dot{y} \sin \left(\beta_{1}\right)\right)$$\left.+J_{b} \dot{\beta}_{1}^{2}\right)$  (8)

where, $K_{b}=m_{b} b$; $\mathrm{m}_{\mathrm{b}}$ - mass BA; b - distance from B to BA mass center; $L_{a}$ - length of OB; J_b - inertia moment BA relative point B.

T_OD and T_DE can be taken from T_OB and T_BA by changing indexes from 1 to 2:

$\begin{aligned} T=T_{O C}+T_{O B}+& T_{B A}+T_{O D}+T_{D E} \\ &=\frac{1}{2} M\left(\dot{x}^{2}+\dot{y}^{2}\right)+\frac{1}{2} J {\psi}^{2} \\ &-K_{r} {\psi}(\dot{x} \cos (\psi)+\dot{y} \sin (\psi)) \\ &+\sum_{i=1}^{2}\left[\frac{1}{2} J_{a} \dot{\alpha}_{l}^{2}+\frac{1}{2} J_{b} \dot{\beta}_{l}^{2}\right.\\ &+K_{a} \dot{\alpha}_{\iota}\left(\dot{x} \cos \left(\alpha_{i}\right)+\dot{y} \sin \left(\alpha_{i}\right)\right) \\ &+K_{b} \dot{\beta}_{l}\left(\dot{x} \cos \left(\beta_{i}\right)+\dot{y} \sin \left(\beta_{i}\right)\right) \\ &\left.+J_{a b} \dot{\alpha}_{l} \dot{\beta}_{l} \cos \left(\alpha_{i}-\beta_{i}\right)\right] \end{aligned}$   (9)

where,

$M=m_{k}+2 m_{a}+2 m_{b}-$total mass   (10)

$K_{a}=m_{a} a+m_{b} L_{a}$  (11)

$J_{a}=J_{a}^{0}+m_{b} L_{a}^{2}$  (12)

$J_{a b}=K_{b} L_{a}=m_{b} b L_{a}$  (13)

Potential energy:

$V=g\left[m_{k}(y+r \cos (\psi))\right.$$+\sum_{i=1}^{2}\left(m_{a}\left(y-a \cos \left(\alpha_{i}\right)\right)\right.$$\left.\left.+m_{b}\left(y-L_{a} \cos \left(\alpha_{i}\right)-b \cos \left(\beta_{i}\right)\right)\right)\right]$$=g\left[M y+K_{r} \cos (\psi)\right.$$\left.-\sum_{i=1}^{2}\left(K_{a} \cos \left(\alpha_{i}\right)+K_{b} \cos \left(\beta_{i}\right)\right)\right]$   (14)

In this paper, MPC algorithms are used to find the optimal trajectory based on the minimization of an objective function and subject to constraints. Therefore, from Eq. (1), we find T and V, next we need to find Q_S by following some elementary expressions:

$\delta W=\left(R_{1 x}+R_{2 x}\right) \delta x+\left(R_{1 y}+R_{2 y}\right) \delta y$$-\left(q_{1}+q_{2}\right) \delta \psi$$\quad+\sum_{i=1}^{2}\left[\left(q_{i}-u_{i}\right) \delta \alpha_{i}\right.$$\quad+\left(u_{i}-P_{i}\right) \delta \beta_{i}$$\quad+R_{1 x} \delta\left(L_{a} \sin \left(\alpha_{i}\right)+L_{b} \sin \left(\beta_{i}\right)\right)$$\quad-R_{1 y} \delta\left(L_{a} \cos \left(\alpha_{i}\right)\right.$$\left.\left.\quad+L_{b} \cos \left(\beta_{i}\right)\right)\right]$

$=\sum_{i=1}^{2}\left[R_{i x} \delta x+R_{i y} \delta y-q_{i} \delta \psi\right.$$+\left(q_{i}-u_{i}\right.$$+R_{1 x} L_{a} \cos \left(\alpha_{i}\right)$$\left.+R_{1 y} L_{a} \sin \left(\alpha_{i}\right)\right) \delta \alpha_{i}$$+\left(u_{i}-P_{i}\right.$$+R_{1 x} L_{b} \cos \left(\beta_{i}\right)$$\left.\left.+R_{1 y} L_{b} \sin \left(\beta_{i}\right)\right) \delta \beta_{i}\right]$   (15)

where,

$Q_{x}=R_{1 x}+R_{2 x}$  (16)

$Q_{y}=R_{1 y}+R_{2 y}$  (17)

$Q_{\psi}=-q_{1}-q_{2}$   (18)

$Q_{\alpha_{1}}=q_{1}-u_{1}+R_{1 x} L_{a} \cos \left(\alpha_{1}\right)+R_{1 y} L_{a} \sin \left(\alpha_{1}\right)$  (19)

$Q_{\alpha_{2}}=q_{2}-u_{2}+R_{1 x} L_{a} \cos \left(\alpha_{2}\right)+R_{1 y} L_{a} \sin \left(\alpha_{2}\right)$  (20)

$Q_{\beta_{1}}=u_{1}-P_{1}+R_{1 x} L_{b} \cos \left(\beta_{1}\right)+R_{1 y} L_{b} \sin \left(\beta_{1}\right)$  (21)

$Q_{\beta_{2}}=u_{2}-P_{2}+R_{1 x} L_{b} \cos \left(\beta_{2}\right)+R_{1 y} L_{b} \sin \left(\beta_{2}\right)$  (22)

The derivatives of these variables can be described as follows:

Derivative $\frac{\partial \mathrm{L}}{\partial \mathrm{z}}$ :

$\frac{\partial L}{\partial x}=\frac{\partial(T-V)}{\partial x}=0$   (23)

$\frac{\partial L}{\partial y}=\frac{\partial(T-V)}{\partial y}=-g M$   (24)

$\frac{\partial L}{\partial \psi}=\frac{\partial(T-V)}{\partial \psi}=K_{r}(\dot{x} {\psi} \sin (\psi)-\dot{y} {\psi} \cos (\psi)$$-g \sin (\psi))$   (25)

$\frac{\partial L}{\partial \alpha_{i}}=\frac{\partial(T-V)}{\partial \alpha_{i}}=K_{a}\left(\dot{\alpha}_{l} \dot{x} \sin \left(\alpha_{i}\right)\right.$$\left.-\dot{\alpha}_{l} \dot{y} \cos \left(\alpha_{i}\right)+g \sin \left(\alpha_{i}\right)\right)$$-J_{a b} \dot{\alpha}_{l} \dot{\beta}_{l} \sin \left(\alpha_{i}-\beta_{i}\right)$    (26)

$\frac{\partial L}{\partial \beta_{i}}=\frac{\partial(T-V)}{\partial \beta_{i}}=K_{b}\left(\dot{\beta}_{l} \dot{x} \sin \left(\beta_{i}\right)\right.$$\left.-\dot{\beta}_{l} \dot{y} \cos \left(\beta_{i}\right)+g \sin \left(\beta_{i}\right)\right)$$-J_{a b} \dot{\alpha}_{l} \dot{\beta}_{l} \sin \left(\alpha_{i}-\beta_{i}\right)$    (27)

Derivative $\frac{\partial \mathrm{L}}{\partial \dot{\mathrm{z}}}$:

$\frac{\partial L}{\partial \dot{x}}=M \dot{x}-K_{r} \dot{\psi} \cos (\psi)+K_{a} \dot{\alpha}_{i} \cos \left(\alpha_{i}\right)+K_{b} \dot{\beta}_{i} \cos \left(\beta_{i}\right)$  (28)

$\frac{\partial L}{\partial \dot{y}}=M \dot{y}-K_{r} \dot{\psi} \sin (\psi)+K_{a} \dot{\alpha}_{i} \sin \left(\alpha_{i}\right)+K_{b} \dot{\beta}_{i} \sin \left(\beta_{i}\right)$  (29)

$\frac{\partial L}{\partial \dot{\psi}}=J \dot{\psi}-K_{r}(\dot{x} \cos (\psi)+\dot{y} \sin (\psi))$   (30)

$\frac{\partial L}{\partial \dot{\alpha}_{i}}=J_{a} \dot{\alpha}_{i}+K_{a}\left(\dot{x} \cos \left(\alpha_{i}\right)+\dot{y} \sin \left(\alpha_{i}\right)\right)+J_{a b} \dot{\beta}_{i} \cos \left(\alpha_{i}-\beta_{i}\right)$  (31)

$\frac{\partial L}{\partial \dot{\beta}_{i}}=J_{b} \dot{\beta}_{i}+K_{b}\left(\dot{x} \cos \left(\beta_{i}\right)+\dot{y} \sin \left(\beta_{i}\right)\right)+J_{a b} \dot{\alpha}_{i} \cos \left(\alpha_{i}-\beta_{i}\right)$  (32)

Derivatives: $\frac{d}{d t} \frac{\partial L}{\partial \dot{z}}$ :

$\begin{aligned} \frac{d}{d t} \frac{\partial L}{\partial \dot{x}}=M \ddot{x}-K_{r} \ddot{\psi} & \cos (\psi)+K_{r} \dot{\psi}^{2} \sin (\psi) \\ &+K_{a} \ddot{\alpha}_{l} \cos \left(\alpha_{i}\right)-K_{a} \dot{\alpha}_{l}^{2} \sin \left(\alpha_{i}\right) \\ &+K_{b} \ddot{\beta}_{l} \cos \left(\beta_{i}\right)-K_{b} \dot{\beta}_{l}^{2} \sin \left(\beta_{i}\right) \end{aligned}$  (33)

$\begin{aligned} \frac{d}{d t} \frac{\partial L}{\partial \dot{y}}=M \ddot{y}-K_{r} \ddot{\psi} & \sin (\psi)-K_{r} \dot{\psi}^{2} \cos (\psi) \\ &+K_{a} \ddot{\alpha}_{l} \sin \left(\alpha_{i}\right)+K_{a} \dot{\alpha}_{l}^{2} \cos \left(\alpha_{i}\right) \\ &+K_{b} \ddot{\beta}_{l} \sin \left(\beta_{i}\right)+K_{b} \dot{\beta}_{l}^{2} \cos \left(\beta_{i}\right) \end{aligned}$  (34)

$\begin{aligned} \frac{d}{d t} \frac{\partial L}{\partial \dot{\psi}}=J \ddot{\psi}-K_{r}(\ddot{x} \cos (\psi)\\ &+\ddot{y} \sin (\psi)-\dot{x} \dot{\psi} \sin (\psi) \\ &+\dot{y} \dot{\psi} \cos (\psi)) \end{aligned}$  (35)

$\frac{d}{d t} \frac{\partial L}{\partial \dot{\alpha}_{l}}=J_{a} \ddot{\alpha}_{l}+K_{a}\left(\ddot{x} \cos \left(\alpha_{i}\right)\right.$$+\ddot{y} \sin \left(\alpha_{i}\right)+\dot{x} \dot{\alpha}_{l} \sin \left(\alpha_{i}\right)$$\left.-\dot{y} \dot{\alpha}_{l} \cos \left(\alpha_{i}\right)\right)$$+J_{a b} \ddot{\beta}_{l} \cos \left(\alpha_{i}-\beta_{i}\right)-J_{a b} \dot{\beta}_{l}\left(\alpha_{i}\right.$$\left.-\beta_{i}\right) \sin \left(\alpha_{i}-\beta_{i}\right)$   (36)

$\frac{d}{d t} \frac{\partial L}{\partial \dot{\beta}_{l}}=J_{b} \ddot{\beta}_{l}+K_{b}\left(\ddot{x} \cos \left(\beta_{i}\right)\right.$$+\ddot{y} \sin \left(\beta_{i}\right)+\dot{x} \beta_{i} \sin \left(\beta_{i}\right)$$\left.-\dot{y} \beta_{i} \cos \left(\beta_{i}\right)\right)+J_{a b} \ddot{\alpha}_{l} \cos \left(\alpha_{i}-\beta_{i}\right)$$-J_{a b} \alpha_{l}\left(\alpha_{l}-\beta_{l}\right) \sin \left(\alpha_{i}-\beta_{i}\right)$   (37)

And finally, full Lagrange equations for this 5–link mechanism:

$\begin{aligned} M \ddot{x}-K_{r} \ddot{\psi} \cos (\psi) &+K_{r} \dot{\psi}^{2} \sin (\psi) \\ &+K_{a} \ddot{\alpha}_{l} \cos \left(\alpha_{i}\right)-K_{a} \dot{\alpha}_{l}^{2} \sin \left(\alpha_{i}\right) \\ &+K_{b} \ddot{\beta}_{l} \cos \left(\beta_{i}\right)-K_{b} \dot{\beta}_{l}^{2} \sin \left(\beta_{i}\right) \\ &=R_{1 x}+R_{2 x}, \quad(i=1,2) \end{aligned}$   (38)

$\begin{aligned} M \ddot{y}-K_{r} \ddot{\psi} \sin (\psi) &-K_{r} \dot{\psi}^{2} \cos (\psi)+K_{a} \ddot{\alpha}_{l} \sin \left(\alpha_{i}\right) \\ &+K_{a} \dot{\alpha}_{l}^{2} \cos \left(\alpha_{i}\right) \\ &+K_{b} \ddot{\beta}_{l} \sin \left(\beta_{i}\right)+K_{b} \dot{\beta}_{l}^{2} \cos \left(\beta_{i}\right) \\ &=R_{1 y}+R_{2 y}-M g, \quad(i=1,2) \end{aligned}$  (39)

$-K_{r} \ddot{x} \cos (\psi)-K_{r} \ddot{y} \sin (\psi)+J \ddot{\psi}-K_{r} g \sin (\psi)$$=-q_{1}-q_{2}, \quad(i=1,2)$   (40)

$\begin{aligned} K_{a} \ddot{x} \cos \left(\alpha_{i}\right)+K_\ddot{a} y & \sin \left(\alpha_{i}\right)+J_{a} \ddot{\alpha}_{l} \\ &+J_{a b} \ddot{\beta}_{l} \cos \left(\alpha_{i}-\beta_{i}\right) \\ &+J_{a b} \dot{\beta}_{l} \sin \left(\alpha_{i}-\beta_{i}\right) \\ &+K_{a} g \sin \left(\alpha_{i}\right) \\ &=q_{i}-u_{i} \\ &+R_{1 x} L_{a} \cos \left(\alpha_{i}\right) \\ &+R_{1 y} L_{a} \sin \left(\alpha_{i}\right),(i=1,2) ; \end{aligned}$  (41)

$K_{b} \ddot{x} \cos \left(\beta_{i}\right)+K_{b} \ddot{y} \sin \left(\beta_{i}\right)+J_{b} \ddot{\beta}_{l}+J_{a b} \ddot{\alpha}_{l} \cos \left(\alpha_{i}-\beta_{i}\right)$$-J_{a b} \dot{\alpha}_{l}^{2} \sin \left(\alpha_{i}-\beta_{i}\right)+K_{b} g \sin \left(\beta_{i}\right)$

$=u_{i}-P_{i}$$+R_{1 x} L_{b} \cos \left(\beta_{i}\right)$$+R_{1 y} L_{b} \sin \left(\beta_{i}\right), \quad(i=1,2)$   (42)

These equations describe the dynamics of 5-link model, but the model has limited movement as point A or E being fixed on the surface:

If point A fixed, we have kinematic equations for x and y:

$x=x_{A}-L_{a} \sin \left(\alpha_{1}\right)-L_{b} \sin \left(\beta_{1}\right)$   (43)

$y=y_{A}+L_{a} \cos \left(\alpha_{1}\right)+L_{b} \cos \left(\beta_{1}\right)$  (44)

$\dot{x}=-L_{a} \dot{\alpha}_{1} \cos \left(\alpha_{1}\right)-L_{b} \dot{\beta}_{1} \cos \left(\beta_{1}\right)$  (45)

$\dot{y}=-L_{a} \dot{\alpha_{1}} \sin \left(\alpha_{1}\right)-L_{b} \dot{\beta}_{1} \sin \left(\beta_{1}\right)$  (46)

New equations for T, V and $\delta W$ in (9, 14) for (43 to 46):

$T=\frac{1}{2} J \dot{\psi}^{2}+\frac{1}{2} M\left(J_{a}-2 L_{a} K_{a}+L_{a}^{2} M\right) \dot{\alpha_{1}}^{2}+\frac{1}{2} J_{a} \dot{\alpha_{2}}^{2}$$+\frac{1}{2} M\left(J_{b}-2 L_{b} K_{b}+L_{b}^{2} M\right) \dot{\beta_{1}}^{2}$$+\frac{1}{2} J_{a} \dot{\beta}_{2}^{2}+L_{a} K_{r} \cos \left(\psi-\alpha_{1}\right) \dot{\psi} \dot{\alpha}_{1}$

$+L_{b} K_{r} \cos \left(\psi-\beta_{1}\right) \dot{\psi} \dot{\beta}_{1}$$-L_{a} K_{a} \cos \left(\alpha_{1}-\alpha_{2}\right) \dot{\alpha}_{1} \dot\alpha_{2}$$+\left(J_{a b}-L_{a} K_{b}-L_{b} K_{a}\right.$$\left.+L_{a} L_{b} M\right) \cos \left(\alpha_{1}-\beta_{1}\right) \dot{\alpha}_{1} \dot{\beta}_{1}$     (47)

$-L_{a} K_{b} \cos \left(\alpha_{1}-\beta_{2}\right) \dot{\alpha}_{1} \dot{\beta}_{2}$$-L_{b} K_{a} \cos \left(\alpha_{2}-\beta_{1}\right) \dot{\alpha}_{2} \dot{\beta}_{1}$$+L_{b} K_{b} \cos \left(\beta_{1}-\beta_{2}\right) \dot{\beta}_{1} \dot{\beta}_{2}$

$V=g\left[M y_{A}+K_{r} \cos (\psi)+\left(L_{a} M-K_{a}\right) \cos \left(\alpha_{1}\right)\right.$$-K_{a} \cos \left(\alpha_{2}\right)$$+\left(L_{b} M-K_{b}\right) \cos \left(\beta_{1}\right)$$\left.-K_{b} \cos \left(\beta_{2}\right)\right]$  (48)

$\begin{aligned} \delta W=-\left(q_{1}+q_{2}\right) & \delta \psi \\+&\left(q_{1}-u_{1}-R_{2 x} L_{a} \cos \left(\alpha_{1}\right)\right.\\-&\left.R_{2 y} L_{a} \sin \left(\alpha_{1}\right)\right) \delta \alpha_{1} \\ &+\left(q_{2}-u_{2}-R_{2 x} L_{a} \cos \left(\alpha_{2}\right)\right.\\ &\left.-R_{2 y} L_{a} \sin \left(\alpha_{2}\right)\right) \delta \alpha_{2} \\ &+\left(u_{1}-P_{1}-R_{2 x} L_{b} \cos \left(\beta_{1}\right)\right.\\ &\left.-R_{2 y} L_{b} \sin \left(\beta_{1}\right)\right) \delta \beta_{1} \\ &+\left(u_{2}-P_{2}-R_{2 x} L_{b} \cos \left(\beta_{2}\right)\right.\\ &\left.-R_{2 y} L_{b} \sin \left(\beta_{2}\right)\right) \delta \beta_{2} \end{aligned}$  (49)

Above equations can be presented in the matrix forms:

$B_{l} \ddot{z}_{l}+g A \sin \left(z_{i}\right)+D(z) \dot{z}_{l}^{2}=C(z) \omega$  (50)

where,

$Z_{\mathrm{i}}=\left[\begin{array}{c}\varphi \\ \alpha_{1} \\ \alpha_{2} \\ \beta_{1} \\ \beta_{2}\end{array}\right]$, $\sin \left(Z_{i}\right)=\left[\begin{array}{c}\sin \varphi \\ \sin \alpha_{1} \\ \sin \alpha_{2} \\ \sin \beta_{1} \\ \sin \beta_{2}\end{array}\right], \dot{\mathrm{z}_{1}}^{2}=\left[\begin{array}{c}\dot{\varphi}^{2} \\ \dot{\alpha_{1}}^{2} \\ \dot{\alpha_{2}}^{2} \\ \dot{\beta_{1}}^{2} \\ \dot{\beta_{2}}^{2}\end{array}\right]$, $\omega=\left[\begin{array}{c}\mathrm{u}_{1} \\ \mathrm{u}_{2} \\ \mathrm{q}_{1} \\ \mathrm{q}_{2} \\ \mathrm{P}_{1} \\ \mathrm{P}_{2} \\ \mathrm{R}_{2 \mathrm{x}} \\ \mathrm{R}_{2 \mathrm{y}}\end{array}\right]$.

$T=T(z, \dot{z})=\frac{1}{2} \dot{z} B(z) \dot{z}$  (51)

where,

$\mathrm{B}(\mathrm{z})=\left[\begin{array}{ccccc}J & L_{a} K_{r} \cos \left(\varphi-\alpha_{1}\right) & 0 & L_{b} K_{r} \cos \left(\varphi-\beta_{1}\right) & 0 \\ L_{a} K_{r} \cos \left(\varphi-\alpha_{1}\right) & J_{a}-2 L_{a} K_{a}+L_{a}^{2} M & -L_{a} K_{a} \cos \left(\alpha_{1}-\alpha_{2}\right) & \left(J_{a b}-L_{a} K_{b}-L_{b} K_{a}+L_{a} L_{b} M\right) \cos \left(\alpha_{1}-\beta_{1}\right) & -L_{a} K_{b} \cos \left(\alpha_{1}-\beta_{2}\right) \\ 0 & -L_{a} K_{a} \cos \left(\alpha_{1}-\alpha_{2}\right) & J_{a} & -L_{b} K_{a} \cos \left(\alpha_{2}-\beta_{1}\right) & J_{a b} \cos \left(\alpha_{2}-\beta_{2}\right) \\ L_{b} K_{r} \cos \left(\varphi-\beta_{1}\right) & \left(J_{a b}-L_{a} K_{b}-L_{b} K_{a}+L_{a} L_{b} M\right) \cos \left(\alpha_{1}-\beta_{1}\right) & -L_{b} K_{a} \cos \left(\alpha_{2}-\beta_{1}\right) & J_{b}-2 L_{b} K_{b}+L_{b}^{2} M & -L_{b} K_{b} \cos \left(\beta_{1}-\beta_{2}\right) \\ 0 & -L_{a} K_{b} \cos \left(\alpha_{1}-\beta_{2}\right) & J_{a b} \cos \left(\alpha_{2}-\beta_{2}\right) & -L_{b} K_{b} \cos \left(\beta_{1}-\beta_{2}\right) & J_{b}\end{array}\right]$

$V=V(z)=\dot{g}\left(M y_{A}-\sum_{i=1}^{5} a_{i i} \cos z_{i}\right.$)  (52)

where, $a_{i i}$ are diagonal elements of A:

$A=\left[\begin{array}{ccccc}-K_{r} & 0 & 0 & 0 & 0 \\ 0 & K_{a}-L_{a} M & 0 & 0 & 0 \\ 0 & 0 & K_{a} & 0 & 0 \\ 0 & 0 & 0 & K_{b}-L_{b} M & 0 \\ 0 & 0 & 0 & 0 & K_{b}\end{array}\right]$

Matrix D(z) is skew-symmetric matrix $d_{i j}(z)=-d_{j i}(z)$. Elements of matrix D(z) is Christoffel symbols of the first kind for matrix B(z).

$\mathrm{D}(z)=\left[\begin{array}{ccccc}0 & L_{a} K_{r} \sin \left(\varphi-\alpha_{l}\right) & 0 & L_{b} K_{r} \sin \left(\varphi-\beta_{1}\right) & 0 \\ -L_{a} K_{r} \sin \left(\varphi-\alpha_{l}\right) & 0 & -L_{a} K_{a} \sin \left(\alpha_{1}-\alpha_{2}\right) & \left(J_{a b}-L_{a} K_{b}-L_{b} K_{a}+L_{a} L_{b} M\right) \sin \left(\alpha_{1}-\beta_{1}\right) & -L_{a} K_{b} \sin \left(\alpha_{1}-\beta_{2}\right) \\ 0 & L_{a} K_{a} \sin \left(\alpha_{1}-\alpha_{2}\right) & 0 & -L_{b} K_{a} \sin \left(\alpha_{2}-\beta_{1}\right) & J_{a b} \sin \left(\alpha_{2}-\beta_{2}\right) \\ -L_{b} K_{r} \sin \left(\varphi-\beta_{1}\right) & -\left(J_{a b}-L_{a} K_{b}-L_{b} K_{a}+L_{a} L_{b} M\right) \sin \left(\alpha_{1}-\beta_{1}\right) & L_{b} K_{a} \sin \left(\alpha_{2}-\beta_{1}\right) & 0 & -L_{b} K_{b} \sin \left(\beta_{1}-\beta_{2}\right) \\ 0 & L_{a} K_{b} \sin \left(\alpha_{1}-\beta_{2}\right) & -J_{a b} \sin \left(\alpha_{2}-\beta_{2}\right) & L_{b} K_{b} \sin \left(\beta_{1}-\beta_{2}\right) & 0\end{array}\right]$

and,

$\mathrm{C}(z)=\left[\begin{array}{cccccccc}0 & 0 & -1 & -1 & 0 & 0 & 0 & 0 \\ -1 & 0 & 1 & 0 & 0 & 0 & -L_{a} \cos \alpha_{1} & -L_{a} \sin \alpha_{1} \\ 0 & -1 & 0 & 1 & 0 & 0 & L_{a} \cos \alpha_{2} & L_{a} \sin \alpha_{2} \\ 1 & 0 & 0 & 0 & -1 & 0 & -L_{b} \cos \beta_{1} & -L_{b} \sin \beta_{1} \\ 0 & 1 & 0 & 0 & 0 & -1 & L_{b} \cos \beta_{2} & L_{b} \sin \beta_{2}\end{array}\right]$

Next, the above system is linearized at $\dot{y_{A}}=\dot{x_{A}}=0$; $\dot{y_{E}} \neq 0 ; \quad \dot{x_{E}} \neq 0$. If movement $Z_{i}$ and $\dot{Z}_{1}$ are small, we can linearize movement equations around point $Z_{\mathrm{i}}=0, \dot{z}_{1}=0,(\mathrm{i}=1, \ldots, 5)$. These equations describe the state of equilibrium when w(t)=0. This state corresponds to the vertical arrangement of all parts of the mechanism (5-link stands on one leg). Note that reporting a five-link mechanism is pivotally mounted end of the supporting leg can have 2⁵=32 the equilibrium position. This follows from the fact that the mechanism of equilibrium each of the links can be a vertical angle equal to zero or 180°.

Then, the movement equations have the form:

$\ddot{Z}_{l} B_{l}+g A z_{i}=C_{l} \omega$  (53)

From matrix B(z) and C(z) we can get B₁ and C₁:

$B_{1}=\left[\begin{array}{ccccc}J & L_{a} K_{r} & 0 & L_{b} K_{r} & 0 \\ L_{a} K_{r} & J_{a^{-}} 2 L_{a} K_{a}+L_{a}^{2} M & -L_{a} \cdot K_{a} & \left(J_{a b}-L_{a} K_{b}-L_{b} K_{a}+L_{a} L_{b} M\right) & -L_{a} K_{b} \\ 0 & -L_{a} K_{a} & J_{a} & -L_{b} K_{a} & J_{a b} \\ L_{b} K_{r} & \left(J_{a b}-L_{a} K_{b}-L_{b} K_{a}+L_{a} L_{b} M\right) & -L_{b} \cdot K_{a} & J_{b^{-}} 2 L_{b} K_{b}+L_{b}^{2} M & -L_{b} K_{b} \\ 0 & -L_{a} K_{b} & J_{a b} & -L_{b} K_{b} & J_{b}\end{array}\right]$

$C_{1}=\left[\begin{array}{cccccccc}0 & 0 & -1 & -1 & 0 & 0 & 0 & 0 \\ -1 & 0 & 1 & 0 & 0 & 0 & -L_{a} & -L_{a} \alpha_{1} \\ 0 & -1 & 0 & 1 & 0 & 0 & L_{a} & L_{a} \alpha_{2} \\ 1 & 0 & 0 & 0 & -1 & 0 & -L_{b} & -L_{b} \beta_{1} \\ 0 & 1 & 0 & 0 & 0 & -1 & L_{b} & L_{b} \beta_{2}\end{array}\right]$

Note that the matrix B under the sign of the cosine of the included angle difference. While the absolute value of each of the corners may be small, for example, may not exceed 30°, angle difference may be large. The validity of such a linearization depends on the stride length. For large steps linearization is not applicable.

With w(t)=0 we can get equations of linearized motion for 5-link model:

$B_{l} \ddot{Z}_{l}+g A Z_{i}=0$  (54)

$\ddot{z}_{1}+g B_{l}^{-1} A Z_{i}=0$  (55)

A boundary value problem for the system (54) or (55) is formulated as follows: find a solution z(t)=0 of the system (54), which at the time t=0 and t=T passes through specified in the configuration space of the point z(0) and z(T). We can use the linear non-singular transformation with constant coefficients:

$T=T(z, \dot{z})=\frac{1}{2} \dot{Z} B(z) \dot{z}$  (56)

In normal coordinates at (55) after transformation at (56), we have the form:

$T=T(z, \dot{z})=\frac{1}{2} \dot{z} B(z) \dot{z}$  (57)

where, $\Omega$ is diagonal 5×5 matrix:

$\Omega=\left[\begin{array}{ccccc}\lambda_{1} & 0 & 0 & 0 & 0 \\ 0 & \lambda_{2} & 0 & 0 & 0 \\ 0 & 0 & \lambda_{3} & 0 & 0 \\ 0 & 0 & 0 & \lambda_{4} & 0 \\ 0 & 0 & 0 & 0 & \lambda_{5}\end{array}\right],$

and $\lambda_{i}$ are roots of characteristic equation of $\Omega$:

$\operatorname{det}\left(B_{l} \lambda+g A\right)=0$  (58)

The matrix R is known resulting in a symmetric positive definite matrix B_l to the unit and the symmetric matrix A - to the diagonal. So for matrix R we have equations:

$R^{T} B_{l} R=E$  (59)

$R^{T} g A R=\Omega$  (60)

From the law of inertia of quadratic forms, we have among the numbers $\lambda_{\mathrm{i}}$ as positive (negative) as positive (negative) eigenvalues of a matrix A.

So, in matrix A we have 3 negative and 2 positive elements and denote:

$\lambda_{\mathrm{i}}=\omega_{\mathrm{i}}^{2}>0$ for $2 \lambda_{i}$ and $\lambda_{i}=-\omega_{\mathrm{i}}^{2}<0$ for $3 \lambda_{\mathrm{i}}$

It means we have 2 equations:

$\ddot{x}_{i}+\omega_{i}^{2} \cdot x_{i}=0$ for $i=3,5$  (61)

$\ddot{x}_{i}-\omega_{i}^{2} \cdot x_{i}=0$ for $i=1,2,4$  (62)

Vectors of the initial and final conditions:

$x(0)=R^{-1} Z(0), x(T)=R^{-1} Z(T)$  (63)

And decision of this Eq. (9):

$x_{i}(t)=\frac{\dot{x_{l}}(0)}{\omega_{i}} \sin \left(\omega_{i} t\right)+x_{i}(0) \cos \left(\omega_{i} t\right)$  (64)

$\dot{x_{\imath}}(0)=\omega_{i} \frac{x_{i}(T)-x_{i}(0) \cos \left(\omega_{i} T\right)}{\sin \left(\omega_{i} t\right)}$  (65)

After substitution (13) in (12) we have:

$x_{i}(t)=\frac{x_{i}(T) \sin \left(\omega_{i} T\right)+x_{i}(0) \sin \left(\omega_{i}(T-t)\right)}{\sin \left(\omega_{i} t\right)}$ i=3,5  (66)

For (10) we have analogical decision:

$x_{i}(t)=\frac{x_{i}(T) \operatorname{sh}\left(\omega_{i} T\right)+x_{i}(0) \operatorname{sh}\left(\omega_{i}(T-t)\right)}{\operatorname{sh}\left(\omega_{i} t\right)}$ i=1,2,4  (67)

Eqns. (66) and (67) are used to test the mathematical model in Figure 1 and compared to the experimental data obtained by the real motion capture cameras. Experimental data for real human movement is setup in the next section.

The MPC algorithms for nonlinear (NMPC) are referred to [10]. The general expression for this linearized continuous time in state space is:

$\dot{x}(t)=\mathbb{A}_{C} x(t)+\mathbb{B}_{C} u(t)$  (68)

$y(t)=\mathbb{C}_{C} x(t)+\mathbb{D}_{C} u(t)$  (69)

where, x(t) represents the states, u(t) represents the inputs, y(t) represents the output, $\mathbb{A}_{C}$, $\mathbb{B}_{C}$, $\mathbb{C}_{C}$, $\mathbb{D}_{C}$ are the model state matrices in continuous time.

For computer calculation, the above continuous time system can be discretized (sampling interval T₀= 0.01 sec) as:

$x(k+1)=\mathbb{A}_{D} x(k)+\mathbb{B}_{D} u(k)$  (70)

$y(k)=\mathbb{C}_{D} x(k)+\mathbb{D}_{D} u(k)$  (71)

where, x(k) represents the discrete states, u(k) represents the discrete inputs, y(k) represents the discrete output, $\mathbb{A}_{D}$, $\mathbb{B}_{D}$, $\mathbb{C}_{D}$, $\mathbb{D}_{D}$ are the state matrices in discrete form.

For simplification, we assign the state prediction, x(k), equal to the input prediction horizon, u(k), or $N_{x(k)}=N_{u(k)}=N$. Then, the objective function of this MPC is:

$J(x(k), u(k))=$$\frac{1}{2} \sum_{k=N_{0}}^{N-1}\left[x(k)^{T} Q x(k)+u(k)^{T} R u(k)\right]$$+\frac{1}{2} x(N)^{T} Q_{f} x(N)$  (72)

where, Q is the weighting matrix for the predicted states along the prediction horizon, R is the weighting matrix for the control inputs, and Q_f is the weighting matrix for the final predicted states at the final time step. N is the horizon prediction length for both inputs and states.

Constraints for inputs are setup such as the maximum input torques and the limited joint angle at ankles, knees, hips:

$\min (u(k))<u(k)<\max (u(k))$  (73)

Similarly, constraints for states are also setup as:

$\min (x(k))<x(k)<\max (x(k))$  (74)

Example of constraints for hip of a healthy human is illustrated in Figure 4.

For the simplification, we set the horizon prediction length as N=50 for all simulations. A design MPC blocks in Matlab Simulink is developed and shown in Figure 5.

MPC designs and calculations are referred in the paper [9]. This MPC block is used as the internal model to control the external human plant model. The external human plant model is developed and presented in the next part.

4.jpg

Figure 4. Human hip constrains

Human gait motions based on MPC are simulated and compared to the experimental data obtained by the Vicon motion capture cameras in ITMO University and partly in Technical University of Liberec. Mass units, moments of inertia, and the relative location of the centers of mass are estimated by the empirical equation (75) depending on the total mass (M) and the human height (H). The lengths of the links, the start and end positions are calculated with MPC controller.

$Y=B_{0}+B_{1} M+B_{2} H$  (75)

where, Y - segment mass, $B_{0}, B_{1}, B_{2}$ are coefficients given in Table 1.

Table 1. Coefficients of mass

As referred in the paper [9], the first MPC is tested with zero terminals, x(N)=0. The MPC objective function in (72) becomes:

$J(x(0), u)=\frac{1}{2} \sum_{k=N_{0}}^{N-1}\left[x(k)^{T} Q x(k)+u(k)^{T} R u(k)\right]$  (76)

Simulation results of (76) are shown in Figure 7.

71.jpg

7.1 Sagittal plane right hip angle

72.jpg

7.2 Sagittal plane left hip angle

73.jpg

7.3 Sagittal plane right shin angle

74.jpg

7.4 Sagittal plane left shin angle

Figure 7. Model Predictive Controller with zero terminals

81.jpg

8.1 Sagittal plane right hip angle

82.jpg

8.2 Sagittal plane left hip angle

83.jpg

8.3 Sagittal plane right shin angle

84.jpg

8.4 Sagittal plane left shin angle

Figure 8. Model Predictive Controller with softened state constraints

MPC with zero terminals, x(N)=0, shows that, the models in Figure 7.1 and 7.3 do not follow the experimental motions. The model motions in Figure 7.2 and 7.4 have two peaks while the real experimental data has only one. Figures from 7.1 to 7.4 show that the mean of angle errors for the right and left hip is 6.2546° and 7.5277°, correspondingly. The mean of angle errors for right and left shin is 8.3327° and 7.9761°, correspondingly.

Next, another MPC controller with softened state constraints is developed:

$J(x(0), u)=$$\frac{1}{2} \sum_{k=N_{0}}^{N-1}\left[x(k)^{T} Q x(k)+u(k)^{T} R u(k)\right]+$$\sum_{k=N_{0}}^{N-1}\left[\varepsilon(k)^{T} M \varepsilon(k)+2 \mu(k)^{T} \varepsilon(k)\right]$  (77)

In (77), a penalty term of softened state constraints, $\sum_{k=N_{0}}^{N-1}\left[\varepsilon(\boldsymbol{k})^{T} \boldsymbol{M} \varepsilon(\boldsymbol{k})+2 \mu(\boldsymbol{k})^{T} \varepsilon(\boldsymbol{k})\right]$, is added with a positive definite and symmetric matrix, M, and usually large values, $\varepsilon(\boldsymbol{k})$. These terms help to penalize the violations of the state constraints, as $\mu(\boldsymbol{k})$ are the state violation values. Simulation results of MPC with softened constraints are shown in Figure 8.

Figure 8.1 to 8.4 show that the performances of the MPC with softened state constraints are better than the MPC with zero terminals. The mean of angle errors for the right and left hip is 4.8226° and 4.6601°, correspondingly. The mean of angle errors for the right and left shin is 3.95° and 4.145°. These values are much smaller than the MPC with zero terminals. Therefore, MPC with softened state constraints can be used well to predict the human gait motions. MPC with softened state constraints can also maintain the stability of the system and always keep the tracking errors at low levels.