#
Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing^{ †}

^{*}

^{†}

## Abstract

**:**

## 1. Introduction

## 2. Notation

#### 2.1. Spatial Algebra Description

- ${\mathit{v}}_{i}$
- velocity of link i;
- ${\mathit{a}}_{i}$
- acceleration of link i;
- ${\mathit{f}}_{i}$
- force transmitted from $\lambda \left(i\right)$ to body i;
- ${\mathit{f}}_{\mathit{i}}^{B}$
- net force on link i;
- ${\mathit{f}}_{\mathit{i}}^{x}$
- external force acting on link i;
- ${\mathit{q}}_{i}$
- joint i position;
- $\dot{{\mathit{q}}_{i}}$
- joint i velocity;
- $\ddot{{\mathit{q}}_{i}}$
- joint i acceleration;
- ${\mathit{\tau}}_{i}$
- joint i torque;
- ${\mathit{v}}_{Ji}$
- velocity across joint i.

**x**in A coordinates.

#### 2.2. Stochastic Notation

**x**, we denote with $p\left(\mathit{x}\right)$ its probability density and with $p\left(\mathit{x}\right|\mathit{y})$ the conditional probability density of

**x**given the assumption that another stochastic variable

**has occurred. Since**

**y****is associated to a deterministic function $f\left(\mathit{x}\right)$, with ${E}_{\mathit{x}}[f\left(\mathit{x}\right)]$ we denote the expected value of $f\left(\mathit{x}\right)$ w.r.t. the probability distribution $p\left(\mathit{x}\right)$. With ${\mathit{\mu}}_{\mathit{x}}$, ${\mathbf{\Sigma}}_{\mathit{x}}$, we denote the mean and covariance of**

**y****x**, i.e., ${\mathbf{\mu}}_{\mathit{x}}=E[\mathit{x}]$ and ${\mathbf{\Sigma}}_{\mathit{x}}=E[\mathit{x}{\mathit{x}}^{\top}]$. The probability density function of a multivariate Gaussian distribution $\mathit{x}\in {\mathbb{R}}^{n}$ is $p\left(\mathit{x}\right)\sim \mathcal{N}({\mathbf{\mu}}_{\mathit{x}},{\mathbf{\Sigma}}_{\mathit{x}})$:

## 3. Problem Statement and Formulation

#### 3.1. Recursive Newton–Euler Algorithm

**q**and $\dot{\mathit{q}}$ have been precomputed, including the transformation matrices ${}^{j}{\mathit{X}}_{i}$, ${}^{j}{\mathit{X}}_{i}^{*}$ and the velocities ${\mathit{v}}_{Ji}$, ${\mathit{v}}_{i}$ which can be efficiently computed with the following recursive equation:

#### 3.2. RNEA Matrix Formulation and the Measurements Equation

**of dynamic variables as follows:**

**d****D**is a block matrix $\in {\mathbb{R}}^{(18{N}_{B}+n)\times d}$ and ${\mathit{b}}_{D}$ is a vector $\in {\mathbb{R}}^{18{N}_{B}+n}$. Let us define how to build

**D**matrix and ${b}_{D}$ vector:

**d**and, in a sense, inverse dynamic computation consists of computing

**d**given ${\mathit{f}}_{i}^{x}$ and ${\ddot{\mathit{q}}}_{i}$. Our contribution is in moving away from this classical approach replacing RNEA boundary conditions with measurements coming from sensors. For this purpose, let us also define an explicit equation for measurements by indicating with

**y**$\in {\mathbb{R}}^{y}$ the values vector measured by sensors:

**Y**matrix depends on the number of sensors ${N}_{S}$ used for each link i as follows:

#### 3.3. Considerations on the Representation

**d**and the columns of

**D**should be rearranged so that they respect the following order: ${\mathit{a}}_{1}..{\mathit{a}}_{{N}_{B}}$, ${\mathit{f}}_{1}^{B}..{\mathit{f}}_{{N}_{B}}^{B}$, ${\mathit{f}}_{1}..{\mathit{f}}_{{N}_{B}}$, ${\ddot{\mathit{q}}}_{1}..{\ddot{\mathit{q}}}_{{N}_{B}}$, ${\mathit{f}}_{1}^{x}..{\mathit{f}}_{{N}_{B}}^{x}$ and ${\mathbf{\tau}}_{1}..{\mathbf{\tau}}_{{N}_{B}}$. The resulting

**D**and

**b**are:

#### 3.4. Over Constrained RNEA

**d**given

**y**in a Gaussian framework by means of a minimum-variance estimator.

## 4. Maximum a Posteriori (MAP) Estimator

**d**and

**y**as stochastic variables with Gaussian distributions. Let us first define their suitable joint probability density using the factorization $p(\mathit{d},\mathit{y})\phantom{\rule{3.33333pt}{0ex}}=\phantom{\rule{3.33333pt}{0ex}}p\left(\mathit{d}\right)p\left(\mathit{y}\right|\mathit{d})$ being $p(\xb7)$ the probability density and $p(\xb7|\xb7)$ its conditioned version. Given $p(\mathit{d},\mathit{y})$, we can compute an estimation of

**d**using a MAP estimator (which, in Gaussian distributions, coincides with the mean of the distribution) as follows:

**d**and does not contribute to the optimization.

**d**. Pursuing the same methodology, we would like to have the following distribution $p\left(\mathit{d}\right)\sim \mathcal{N}\left({\mathbf{\mu}}_{D},{\mathbf{\Sigma}}_{D}\right)$ :

**d**in the form of $p\left(\mathit{d}\right)\sim \mathcal{N}\left({\mathbf{\mu}}_{d},{\mathbf{\Sigma}}_{d}\right)$ distribution, we can reformulate Equation (8) as $p\left(\mathit{d}\right)\sim \mathcal{N}\left({\overline{\mathbf{\mu}}}_{D},{\overline{\mathbf{\Sigma}}}_{D}\right)$:

#### On the Benefits of MAP Dynamics

## 5. Experimental Results

#### 5.1. Experimental Set-up

#### 5.2. Human Body Modeling

#### 5.3. Proof of Concept

#### 5.4. Method Robustness Test

**Y**, usually known as regressor, is the matrix function of joint positions, velocities and accelerations.

## 6. Conclusions

**d**and measurements

**y**coming from multiple sensors. The probability density $p(\mathit{d},\mathit{y})$ incorporates the dynamic constraints among variables. Preliminary results show that adding the i-th measurement induces changes in its covariance matrix according to Equation (13) for all variables estimated. We demonstrate that the variance of each estimated variable in ${\mathbf{\mu}}_{d|y}$ decreases as the number of considered measurements (sensors) increase. Moreover, it should be necessary to implement a procedure (Expectation-Maximization (EM) algorithm) to estimate sensor variance from data; in a sense, the goal is to build a data-driven variance estimation overcoming datasheet variance. While this paper presents a proof of concept of the theory behind WearDY, future works will be aimed at real-time implementation, at the design of the wearable garment and at the improvement of the sensor architecture in order to make the system reliable in situ. Computationally, it will be fundamental to integrate MAP dynamics with a state estimator such as an Extended Kalman filter combining obtained a posteriori estimates with a priori estimates of the filter state. From a design perspective, the wearable system will be comprised of a soft sensing garment with embedded sensors that facilitate free movement of the subject. The suit design will ideally exploit the material compliance in order to improve sensor reliability through elimination of the error introduced by the interface between the garment and the subject’s skin. Another important future objective concerns the possibility of using electromyography (EMG) analysis to provide information on muscular activity that is related to joint torques. Possible future applications of WearDY include rehabilitation monitoring where a wearable garment will be used for monitoring patients or creating more ergonomic and compliant prosthesis and exoskeleton systems. The proposed suit and the forthcoming improvements would permit substantial enhancements in the analysis of human movements in motion.

## Acknowledgments

## Author Contributions

## Conflicts of Interest

## Appendix A

**x**and ${\lambda}_{1}\left(A\right)$ the maximum eigenvalue of the matrix

**A**. The motivation behind this choice lies in the fact that the maximum eigenvalue bounds the variance of any other linear combination of

**d**. Let us assume that we are interested in estimating $\mathit{x}={\mathit{v}}^{\top}\mathit{d}$ by choosing $\widehat{\mathit{x}}={\mathit{v}}^{\top}\widehat{\mathit{d}}$. The estimator variance is given by:

**v**equals the eigenvector associated to the maximum eigenvalue.

## Appendix B

**A**and

**B**, respectively. Consider also an eigenvalue decreasing numbering for each matrix, from the largest to the smallest one. We can apply the Weyl inequality being k the k-th largest eigenvalue as follows:

**A**and

**B**are non-singular matrices and ${\lambda}_{1}\ne 0$, then $\frac{1}{{\lambda}_{1}}$ is an eigenvalue of ${\mathit{A}}^{-1}$. Thus, in Equation (B2):

## References

- Roetenberg, D.; Luinge, H.; Slycke, P. Xsens mvn: Full 6DOF human motion tracking using miniature inertial sensors. In Xsens Motion Technologies BV, Techology Report; Enschede, The Netherlands, 8 April 2009. [Google Scholar]
- Zhang, Z.Y. Microsoft Kinect sensor and its effect. IEEE MultiMed.
**2012**, 19, 4–12. [Google Scholar] [CrossRef][Green Version] - Mengüç, Y.; Park, Y.-L.; Martinez-Villalpando, E.; Aubin, P.; Zisook, M.; Stirling, L.; Wood, R.J.; Walsh, C.J. Soft wearable motion sensing suit for lower limb biomechanics measurements. In Proceedings of the 2013 IEEE International Conference on Robotics and Automation (ICRA), Karlsruhe, Germany, 6–10 May 2013; pp. 5309–5316.
- Mengüç, Y.; Park, Y.-L.; Pei, H.; Vogt, D.; Aubin, P.M.; Winchell, E.; Fluke, L.; Stirling, L.; Wood, R.J.; Walsh, C.J. Wearable soft sensing suit for human gait measurement. Robot. Res.
**2014**, 33, 1748–1764. [Google Scholar] [CrossRef] - Gallego, J.A.; Rocon, E.; Ibanez, J.; Dideriksen, J.L.; Koutsou, A.D.; Paradiso, R.; Popovic, M.B.; Belda-Lois, J.M.; Gianfelici, F.; Farina, D.; et al. A soft wearable robot for tremor assessment and suppression. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China, 9–13 May 2011; pp. 2249–2254.
- Wei, X.; Chai, J. Videomocap: Modelling phisically realistic human motion from monocular video sequences. ACM Trans. Graph.
**2010**, 29. [Google Scholar] [CrossRef] - Bonnet, V.; Mazza, C.; Fraisse, P.; Cappozzo, A. Real time estimate of body kinematics during a planar squat task using a single inertial measurement unit. IEEE Trans. Biomed. Eng.
**2013**, 60, 1920–1926. [Google Scholar] [CrossRef] [PubMed] - Zhang, P.; Siu, K.; Zhang, J.; Liu, C.K.; Chai, J. Leveraging depth cameras and wearable pressure sensors for full-body kinematics and dynamics capture. ACM Trans. Graph.
**2014**, 33, 221. [Google Scholar] [CrossRef] - Drillis, R.; Contini, R.; Bluestein, M. Body segment parameters; a survey of measurement techniques. Artif. Limbs
**1964**, 8, 44–66. [Google Scholar] [PubMed] - Nori, F.; Kuppuswamy, N.; Traversaro, S. Simultaneous state and dynamics estimation in articulated structures. In Proceedings of the 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Hamburg, Germany, 28 September–2 October 2015; pp. 3380–3386.
- Featherstone, R. Rigid Body Dynamics Algorithms; Springer: New York, NY, USA, 2008. [Google Scholar]
- Siciliano, B.; Sciavicco, L.; Villani, L.; Oriolo, G. Robotics: Modelling, Planning and Control. In Advanced Textbooks in Control and Signal Processing; Springer: London, UK, 2009. [Google Scholar]
- Guelton, K.; Delprat, S.; Guerra, T.-M. An alternative to inverse dynamics joint torques estimation in human stance based on takagi-sugeno unknown-inputs observer in the descriptor form. Control Eng. Pract.
**2008**, 16, 1414–1426. [Google Scholar] [CrossRef] - Lloyd, D.G.; Besier, T.F. An emg-driven musculoskeletal model to estimate muscle forces and knee joint moments in vivo. Biomechanics
**2003**, 36, 765–776. [Google Scholar] [CrossRef] - Hettich, G.; Fennel, L.; Mergner, T. Double inverted pendulum model of reactive human stance control. In Proceedings of the Multibody Dynamics 2011, ECCOMAS Thematic Conference, Brussels, Belgium, 4–7 July 2011.
- Featherstone, R. Spatial Vector and Rigid-Body Dynamics Software. Available online: http://royfeatherstone.org/spatial/v2/index.html (accessed on 18 May 2016).

**Figure 2.**Sensors used in the experiment: a force platform AMTI OR6 (

**a**) and an inertial sensor Xsens MTx comprising both accelerometer and gyroscope (

**b**).

**Figure 3.**General scheme of the experimental set-up including a motion capture system and a block of sensors. Data coming from the motion system were used as inputs for each Maximum-a-Posteriori (MAP) computation and, progressively, sensor measurements ($\ddot{\mathit{q}}$, force plate sensing and inertial sensor IMU) were added to compute a more complete version of MAP. Although only three sensors are used (the ‘multiple sensors’ block is not present in this version of experiment), in general, the set-up can embed n sensors and exploit their measurements to enhance the dynamic estimation of

**d**.

**Figure 4.**Human body modeling: generic representation of a marker-like model for a subject (

**a**); its representation with reference frames associated to points (P

_{0}, P

_{1}, P

_{2}, P

_{3}) and to sensors (

**b**). Reference frames are denoted by using the RGB (Red-Green-Blue) convention for x-y-z axis.

**Figure 5.**Representation of a trial composed of three repetitive bows (

**on top**). Joint quantities: angles ${\mathit{q}}_{1}$ and ${\mathit{q}}_{2}$ have been obtained from motion capture acquisition with a frequency filtering; velocities ${\dot{\mathit{q}}}_{1}$ and ${\dot{\mathit{q}}}_{2}$ have been computed using a third-order polynomial Savitzky–Golay filtering; torques have been computed using MAP estimation exploiting all sensors involved in the analysis (

**on bottom**).

**Figure 6.**Considering three MAP computation cases with one sensor ($\mathit{y}=\ddot{\mathit{q}}$), two sensors ($\mathit{y}=\ddot{\mathit{q}},{\mathit{y}}_{1}$), and all sensors ($\mathit{y}=\ddot{\mathit{q}},{\mathit{y}}_{1},{\mathit{y}}_{2}$), we computed standard deviation of (

**a**) ${\mathrm{\Sigma}}_{{\tau}_{1}|y}$ and ${\mathrm{\Sigma}}_{{\tau}_{2}|y}$ ; (

**b**,

**c**) ${\mathrm{\Sigma}}_{{f}_{1}|y}$ and ${\mathrm{\Sigma}}_{{f}_{2}|y}$ for both angular ($\mathbf{\mu}$) and linear (

**f**) components; (

**d**,

**e**) ${\mathrm{\Sigma}}_{{a}_{1}|y}$ and ${\mathrm{\Sigma}}_{{a}_{2}|y}$ for both angular ($\dot{\mathbf{\omega}}$) and linear (

**a**) components. These plots show that when increasing the number of sensors in the analysis, the standard deviation of each variable estimated in the vector ${\mathbf{\mu}}_{d|y}$ progressively decrease.

**Figure 7.**Mean (μ) and standard deviation (2σ) for the estimation of $\mathbf{\tau}$ increasing the number of sensors.

**Figure 8.**Error on torque estimation comparing RNEA and MAP methods on ${\mathbf{\tau}}_{1}$ (

**a**) and ${\mathbf{\tau}}_{2}$ (

**b**), (* p-value < 0.05).

Angular Part | Linear Part | |||||
---|---|---|---|---|---|---|

${\sigma}_{\ddot{q}}$ | $2.57\times {10}^{-3}\phantom{\rule{3.33333pt}{0ex}}\mathrm{rad}/{\mathrm{s}}^{2}$ | |||||

${\sigma}_{{y}_{1}}$ | 1.5 $\mathrm{Nm}$ | 1.5 $\mathrm{Nm}$ | 0.7483 $\mathrm{Nm}$ | 7.68 $\mathrm{N}$ | 7.68 $\mathrm{N}$ | 6 $\mathrm{N}$ |

${\sigma}_{{y}_{2}}$ | $7.1\times {10}^{-3}\phantom{\rule{3.33333pt}{0ex}}\mathrm{rad}/{\mathrm{s}}^{2}$ (gyro) | $3.33\times {10}^{-2}\phantom{\rule{3.33333pt}{0ex}}\mathrm{m}/{\mathrm{s}}^{2}$ (acc) |

${\mathbf{\sigma}}_{{\mathbf{\tau}}_{\mathbf{1}}}$ | ${\mathbf{\sigma}}_{{\mathbf{\mu}}_{\mathbf{1}}\left(\mathbf{1}\right)}$ | ${\mathbf{\sigma}}_{{\mathbf{\mu}}_{\mathbf{1}}\left(\mathbf{2}\right)}$ | ${\mathbf{\sigma}}_{{\mathbf{\mu}}_{\mathbf{1}}\left(\mathbf{3}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{f}}_{\mathbf{1}}\left(\mathbf{1}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{f}}_{\mathbf{1}}\left(\mathbf{2}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{f}}_{\mathbf{1}}\left(\mathbf{3}\right)}$ | ${\mathbf{\sigma}}_{{\dot{\mathbf{\omega}}}_{\mathbf{1}}\left(\mathbf{1}\right)}$ | ${\mathbf{\sigma}}_{{\dot{\mathbf{\omega}}}_{\mathbf{1}}\left(\mathbf{2}\right)}$ | ${\mathbf{\sigma}}_{{\dot{\mathbf{\omega}}}_{\mathbf{1}}\left(\mathbf{3}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{a}}_{\mathbf{1}}\left(\mathbf{1}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{a}}_{\mathbf{1}}\left(\mathbf{2}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{a}}_{\mathbf{1}}\left(\mathbf{3}\right)}$ | |
---|---|---|---|---|---|---|---|---|---|---|---|---|---|

$\ddot{q}$ | 4.990 | 2.075 | 1.153 | 2.088 | 2.423 | 2.279 | 2.371 | 0.028 | 0.027 | 0.029 | 0.025 | 0.027 | 0.028 |

$\ddot{q},{y}_{1}$ | 1.340 | 1.118 | 0.636 | 1.145 | 1.558 | 2.099 | 1.600 | 0.025 | 0.026 | 0.025 | 0.021 | 0.026 | 0.024 |

$\ddot{q},{y}_{1},{y}_{2}$ | 0.970 | 1.020 | 0.633 | 0.977 | 1.364 | 1.659 | 1.382 | 0.019 | 0.020 | 0.019 | 0.019 | 0.022 | 0.022 |

${\mathbf{\sigma}}_{{\mathbf{\tau}}_{\mathbf{2}}}$ | ${\mathbf{\sigma}}_{{\mathbf{\mu}}_{\mathbf{2}}\left(\mathbf{1}\right)}$ | ${\mathbf{\sigma}}_{{\mathbf{\mu}}_{\mathbf{2}}\left(\mathbf{2}\right)}$ | ${\mathbf{\sigma}}_{{\mathbf{\mu}}_{\mathbf{2}}\left(\mathbf{3}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{f}}_{\mathbf{2}}\left(\mathbf{1}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{f}}_{\mathbf{2}}\left(\mathbf{2}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{f}}_{\mathbf{2}}\left(\mathbf{3}\right)}$ | ${\mathbf{\sigma}}_{{\dot{\mathbf{\omega}}}_{\mathbf{2}}\left(\mathbf{1}\right)}$ | ${\mathbf{\sigma}}_{{\dot{\mathbf{\omega}}}_{\mathbf{2}}\left(\mathbf{2}\right)}$ | ${\mathbf{\sigma}}_{{\dot{\mathbf{\omega}}}_{\mathbf{2}}\left(\mathbf{3}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{a}}_{\mathbf{2}}\left(\mathbf{1}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{a}}_{\mathbf{2}}\left(\mathbf{2}\right)}$ | ${\mathbf{\sigma}}_{{\mathit{a}}_{\mathbf{2}}\left(\mathbf{3}\right)}$ | |
---|---|---|---|---|---|---|---|---|---|---|---|---|---|

$\ddot{q}$ | 0.510 | 0.848 | 0.758 | 0.758 | 1.817 | 1.544 | 1.696 | 0.040 | 0.037 | 0.041 | 0.043 | 0.040 | 0.042 |

$\ddot{q},{y}_{1}$ | 0.356 | 0.650 | 0.669 | 0.615 | 1.423 | 1.238 | 1.100 | 0.038 | 0.036 | 0.037 | 0.035 | 0.033 | 0.030 |

$\ddot{q},{y}_{1},{y}_{2}$ | 0.305 | 0.643 | 0.657 | 0.568 | 1.092 | 1.026 | 0.918 | 0.013 | 0.013 | 0.013 | 0.023 | 0.023 | 0.022 |

© 2016 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC-BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Latella, C.; Kuppuswamy, N.; Romano, F.; Traversaro, S.; Nori, F. Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing. *Sensors* **2016**, *16*, 727.
https://doi.org/10.3390/s16050727

**AMA Style**

Latella C, Kuppuswamy N, Romano F, Traversaro S, Nori F. Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing. *Sensors*. 2016; 16(5):727.
https://doi.org/10.3390/s16050727

**Chicago/Turabian Style**

Latella, Claudia, Naveen Kuppuswamy, Francesco Romano, Silvio Traversaro, and Francesco Nori. 2016. "Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing" *Sensors* 16, no. 5: 727.
https://doi.org/10.3390/s16050727