Generation of human walking paths

Abstract

This work investigates the way humans plan their paths in a goal-directed motion, assuming that a person acts as an optimal controller that plans the path minimizing a certain (unknown) cost function. Taking this viewpoint, the problem can be formulated as an inverse optimal control one, i.e., starting from control and state trajectories one wants to figure out the cost function used by a person while planning the path. The so-obtained model can be used to support the design of safe human–robot interaction systems, as well as to plan human-like paths for humanoid robots. To test the envisaged ideas, a set of walking paths of different volunteers were recorded using a motion capture facility. The collected data were used to compare two solutions to the inverse optimal control problem coming from the literature to a novel one. The obtained results, ranked using the discrete Fréchet distance, show the effectiveness of the proposed approach.

Appendix

This appendix reports the calculation of the matrices required to setup the least-squares optimization problem (10), for each of the cost functions introduced in Sect. 5.2.

1.1 Energy-based cost function

First of all, the unicycle time model in (1) can be discretized yielding

$$\begin{aligned} {\left\{ \begin{array}{ll} x_1(k+1) = x_1(k) + \varDelta t(k)v(k)\cos \left( x_3(k)\right) \\ x_2(k+1) = x_2(k) + \varDelta t(k)v(k)\sin \left( x_3(k)\right) \\ x_3(k+1) = x_3(k) + \varDelta t(k)\omega (k) \end{array}\right. } \end{aligned}$$

(12)

where $\varDelta t$ is the discrete time step, and $x_1$, $x_2$, $x_3$ are the Cartesian coordinates of point P and the orientation, respectively.

Then, considering a discretised version of the cost function (3) and the model in (12), the inverse optimal control problem can be formulated as follows

$$\begin{aligned} \begin{aligned} \underset{\mathbf {x}(k),v(k),\omega (k)}{\min }&\quad \dfrac{1}{2} \sum _{k=0}^{N-1} \left( \alpha v(k)^2+\omega (k)^2\right) \varDelta t(k)\\ \text {s.t.}&\quad \mathbf {x}(0) - \mathbf {x}_{\text {s}} = 0\\&\quad \mathbf {x}(N-1) - \mathbf {x}_{\text {g}} = 0\\&\quad x_1(k+1) - \left[ x_1(k) + \varDelta t(k)v(k)\cos \left( x_3(k)\right) \right] = 0\\&\quad x_2(k+1) - \left[ x_2(k) + \varDelta t(k)v(k)\sin \left( x_3(k)\right) \right] = 0\\&\quad x_3(k+1) - \left[ x_3(k) + \varDelta t(k)\omega (k)\right] = 0\\&\quad \qquad \forall k=0,\ldots ,N-1 \end{aligned} \end{aligned}$$

(13)

where $\mathbf {x}=\begin{bmatrix}x_1&x_2&x_3\end{bmatrix}^T$ is the state vector, $\mathbf {x}_{\text {s}}$ and $\mathbf {x}_{\text {g}}$ are the initial and the final states, respectively, and N is the number of samples.

Writing, now, the Lagrangian associated with (13), as described in Sect. 6, the residual functions matrices in (10) become where

$$\begin{aligned} z = \begin{bmatrix} \alpha&\lambda _{1}^{0}&\lambda _{2}^{0}&\lambda _{3}^{0}&\cdots&\lambda _{1}^{N-1}&\lambda _{2}^{N-1}&\lambda _{3}^{N-1} \end{bmatrix}^{T}\quad b = \begin{bmatrix} \zeta (0)\\ \zeta (1) \\ \vdots \\ \zeta (N-1) \end{bmatrix} \end{aligned}$$

and

$$\begin{aligned} J= \begin{bmatrix} \psi (0)&I_{5\times 3}&M(0)&\mathbf {0}_{5\times 3}&\mathbf {0}_{5\times 3}&\cdots&\mathbf {0}_{5\times 3}\\ \psi (1)&\mathbf {0}_{5\times 3}&-I_{5\times 3}&M(1)&\mathbf {0}_{5\times 3}&\cdots&\mathbf {0}_{5\times 3}\\ \psi (2)&\mathbf {0}_{5\times 3}&\mathbf {0}_{5\times 3}&-I_{5\times 3}&M(2)&\cdots&\mathbf {0}_{5\times 3}\\ \vdots&\vdots&\vdots&\vdots&\vdots&\ddots&\vdots \\ \psi (N-1)&\mathbf {0}_{5\times 3}&\mathbf {0}_{5\times 3}&\mathbf {0}_{5\times 3}&\mathbf {0}_{5\times 3}&\cdots&-I_{5\times 3} \end{bmatrix} \end{aligned}$$

with

$$\begin{aligned}&\zeta (k) = \begin{bmatrix} 0\\0\\0\\0\\ \varDelta t(k) \omega (k) \end{bmatrix},\quad I_{5\times 3} = \begin{bmatrix} I_{3\times 3}\\ \mathbf {0}_{2\times 3} \end{bmatrix},\quad \psi (k) = \begin{bmatrix} 0 \\ 0 \\ 0 \\ \varDelta t(k) v(k) \\0 \end{bmatrix},\\&M(k) = \begin{bmatrix} 1&0&0\\ 0&1&0\\ -\varDelta t(k) v(k) \sin \left( x_3(k)\right)&\varDelta t(k) v(k) \cos \left( x_3(k)\right)&1\\ \varDelta t(k) \cos \left( x_3(k)\right)&\varDelta t(k) \cos \left( x_3(k)\right)&0\\ 0&0&\varDelta t(k) \end{bmatrix}. \end{aligned}$$

1.2 Hybrid energy/goal-based cost function

Considering a discretised version of the cost function (4) and the discretized unicycle space model in (6), the inverse optimal control problem can be formulated as follows

$$\begin{aligned} \begin{aligned} \underset{\mathbf {x}(k),\sigma (k)}{\min }&\quad \dfrac{1}{2} \sum _{k=0}^{N-1} \sigma (k)^2 \left( 1+\beta ^T\varGamma ^2\right) \varDelta s(k)\\ \text {s.t.}&\quad \mathbf {x}(0) - \mathbf {x}_{\text {s}} = 0\\&\quad \mathbf {x}(N-1) - \mathbf {x}_{\text {g}} = 0\\&\quad x_1(k+1) - \left[ x_1(k) + \varDelta s(k) \cos \left( x_3(k)\right) \right] = 0\\&\quad x_2(k+1) - \left[ x_2(k) + \varDelta s(k) \sin \left( x_3(k)\right) \right] = 0\\&\quad x_3(k+1) - \left[ x_3(k) + \varDelta s(k) \sigma (k)\right] = 0\\&\quad \qquad \forall k=0,\ldots ,N-1 \end{aligned} \end{aligned}$$

(14)

where $\mathbf {x}=\begin{bmatrix}x_1&x_2&x_3\end{bmatrix}^T$ is the state vector, $\mathbf {x}_{\text {s}}$ and $\mathbf {x}_{\text {g}}$ are the initial and the final states, respectively, and N is the number of samples.

Writing, now, the Lagrangian associated with (14), as described in Sect. 6, the residual functions matrices in (10) become

$$\begin{aligned} \begin{aligned} z =&\begin{bmatrix} \beta ^T&\lambda _{1}^{0}&\lambda _{2}^{0}&\lambda _{3}^{0}&\cdots&\lambda _{1}^{N-1}&\lambda _{2}^{N-1}&\lambda _{3}^{N-1} \end{bmatrix}^{T}\\ b =&\begin{bmatrix} \zeta (0)^T&\zeta (1)^T&\cdots&\zeta (N-1)^T \end{bmatrix}^T \end{aligned} \end{aligned}$$

and

$$\begin{aligned} J= \begin{bmatrix} \psi (0)&I_{4\times 3}&M(0)&\mathbf {0}_{4\times 3}&\cdots&\mathbf {0}_{4\times 3}\\ \psi (1)&\mathbf {0}_{4\times 3}&-I_{4\times 3}&M(1)&\cdots&\mathbf {0}_{4\times 3}\\ \psi (2)&\mathbf {0}_{4\times 3}&\mathbf {0}_{4\times 3}&-I_{4\times 3}&\cdots&\mathbf {0}_{4\times 3}\\ \vdots&\vdots&\vdots&\vdots&\ddots&\vdots \\ \psi (N-1)&\mathbf {0}_{4\times 3}&\mathbf {0}_{4\times 3}&\mathbf {0}_{4\times 3}&\cdots&-I_{4\times 3} \end{bmatrix} \end{aligned}$$

with

$$\begin{aligned}&\zeta (k) = \begin{bmatrix} 0\\0\\0\\ \varDelta s(k) \sigma (k) \end{bmatrix},\qquad I_{4\times 3} = \begin{bmatrix} I_{3\times 3}\\ \mathbf {0}_{1\times 3} \end{bmatrix}\\&\begin{aligned} \psi (k) =&\varDelta s(k) \sigma (k) \cdot \\&\begin{bmatrix} \sigma (k) \left( x_1(k)-x_{1_g}\right)&0&0\\ 0&\sigma (k) \left( x_2(k)-x_{2_g}\right)&0\\ 0&0&\sigma (k) \left( x_3(k)-x_{3_g}\right) \\ \left( x_1(k)-x_{1_g}\right) ^2&\left( x_2(k)-x_{2_g}\right) ^2&\left( x_3(k)-x_{3_g}\right) ^2 \end{bmatrix} \end{aligned}\\&M(k) = \begin{bmatrix} 1&0&0\\ 0&1&0\\ -\varDelta s(k) \sin \left( x_3(k)\right)&\varDelta s(k) \cos \left( x_3(k)\right)&1\\ 0&0&\varDelta s(k) \end{bmatrix} \end{aligned}$$

1.3 Normalized hybrid energy/goal-based cost function

Considering a discretised version of the cost function (5) and the discretized unicycle space model in (6), the inverse optimal control problem can be formulated as follows

$$\begin{aligned} \begin{aligned} \underset{\mathbf {x}(k),\sigma (k)}{\min }&\quad \dfrac{1}{2} \sum _{k=0}^{N-1} \sigma (k)^2 \left( 1+\gamma ^T\tilde{\varGamma }^2\right) \varDelta s(k)\\ \text {s.t.}&\quad \mathbf {x}(0) - \mathbf {x}_{\text {s}} = 0\\&\quad \mathbf {x}(N-1) - \mathbf {x}_{\text {g}} = 0\\&\quad x_1(k+1) - \left[ x_1(k) + \varDelta s(k) \cos \left( x_3(k)\right) \right] = 0\\&\quad x_2(k+1) - \left[ x_2(k) + \varDelta s(k) \sin \left( x_3(k)\right) \right] = 0\\&\quad x_3(k+1) - \left[ x_3(k) + \varDelta s(k) \sigma (k)\right] = 0\\&\quad \qquad \forall k=0,\ldots ,N-1 \end{aligned} \end{aligned}$$

(15)

where $\mathbf {x}=\begin{bmatrix}x_1&x_2&x_3\end{bmatrix}^T$ is the state vector, $\mathbf {x}_{\text {s}}$ and $\mathbf {x}_{\text {g}}$ are the initial and the final states, respectively, and N is the number of samples.

Writing, now, the Lagrangian associated with (15), as described in Sect. 6, the residual functions matrices in (10) become

$$\begin{aligned} \begin{aligned} z =&\begin{bmatrix} \gamma ^T&\lambda _{1}^{0}&\lambda _{2}^{0}&\lambda _{3}^{0}&\cdots&\lambda _{1}^{N-1}&\lambda _{2}^{N-1}&\lambda _{3}^{N-1} \end{bmatrix}^{T}\\ b =&\begin{bmatrix} \zeta (0)^T&\zeta (1)^T&\cdots&\zeta (N-1)^T \end{bmatrix}^T \end{aligned} \end{aligned}$$

and

$$\begin{aligned} J= \begin{bmatrix} \psi (0)&I_{4\times 3}&M(0)&\mathbf {0}_{4\times 3}&\cdots&\mathbf {0}_{4\times 3}\\ \psi (1)&\mathbf {0}_{4\times 3}&-I_{4\times 3}&M(1)&\cdots&\mathbf {0}_{4\times 3}\\ \psi (2)&\mathbf {0}_{4\times 3}&\mathbf {0}_{4\times 3}&-I_{4\times 3}&\cdots&\mathbf {0}_{4\times 3}\\ \vdots&\vdots&\vdots&\vdots&\ddots&\vdots \\ \psi (N-1)&\mathbf {0}_{4\times 3}&\mathbf {0}_{4\times 3}&\mathbf {0}_{4\times 3}&\cdots&-I_{4\times 3} \end{bmatrix} \end{aligned}$$

with

$$\begin{aligned}&\zeta (k) = \begin{bmatrix} 0\\0\\0\\ \varDelta s(k) \sigma (k) \end{bmatrix},\qquad I_{4\times 3} = \begin{bmatrix} I_{3\times 3}\\ \mathbf {0}_{1\times 3} \end{bmatrix}, \end{aligned}$$

and, letting $\delta _{sg,i} = x_{i_s} - x_{i_g}$, $i\in \{1,2,3\}$ to lighten the notation, the remaining matrices become

$$\begin{aligned}&\begin{aligned} \psi (k) =&\varDelta s(k) \sigma (k)\\&\quad \times \begin{bmatrix} \sigma (k) \dfrac{x_1(k)-x_{1_g}}{\delta _{sg,1}^2 + \delta _{sg,2}^2}&0\\ \sigma (k) \dfrac{x_2(k)-x_{2_g}}{\delta _{sg,1}^2 + \delta _{sg,2}^2}&0\\ 0&\sigma (k) \dfrac{x_3(k)-x_{3_g}}{\delta _{sg,3}^2}\\ \dfrac{\left( x_1(k)-x_{1_g}\right) ^2 + \left( x_2(k)-x_{2_g}\right) ^2}{\delta _{sg,1}^2 + \delta _{sg,2}^2}&\dfrac{\left( x_3(k)-x_{3_g}\right) ^2}{\delta _{sg,3}^2} \end{bmatrix} \end{aligned}\\&M(k) = \begin{bmatrix} 1&0&0\\ 0&1&0\\ -\sigma (k) \sin \left( x_3(k)\right)&\sigma (k) \cos \left( x_3(k)\right)&1\\ 0&0&\sigma (k) \end{bmatrix} \end{aligned}$$