Motion Tracking of a High-Speed Multilink System Using Dynamic Measurements Fusion

Zhang, Wei; Steckenrider, J. Josiah; Furukawa, Tomonari

doi:10.1115/1.4063798

Abstract

This paper presents a technique for tracking the high-speed motion of a multilink system using inertial measurement units (IMUs) in a new sensor arrangement, an approach which is referred to as dynamic measurements fusion. The proposed technique incorporates accelerometers with traditional gyroscopes to measure joint angular velocities, while joint angles are measured with magnetometers. Comparative studies with conventional techniques show that the proposed technique tracks the motion of a multilink system accurately at both low (0.5 m/s) and high (5 m/s) speeds. Further analysis with different levels of measurement noise demonstrates the robustness of the proposed technique and its overall capability for tracking joint angular velocities and angles.

1 Introduction

1.1 Background.

Biological and biologically inspired systems can often be modeled as multilink systems since they commonly consist of links and joints for mobility. Jointed multilink systems are common in both nature and the engineered world because they allow for the flexible configuration of sensors and end effectors [1]. Industrial and humanoid robots all fall into this category, and nonrobotic systems like dummies and humans can also be classified as such. The motion tracking of multilink systems has found broad applications and interest in robotics [2–7], human health monitoring and therapeutics [8–11], and the like. Recently, there has been increasing interest in the motion tracking of highly dynamic multilink systems, particularly in the field of sports [12–14] and vehicular safety [15–18]. Such systems are subject to significant accelerations, so additional thought is necessary to handle the tracking of high-speed motion. This paper focuses on the motion tracking of such highly dynamic multilink systems.

1.2 Related Work.

Past work in the area of multilink system motion tracking can be classified into two categories. The first, commonly known as motion capture, uses a set of cameras installed in known locations to identify the three-dimensional (3D) positions and orientations of all links in a system. This typically requires the placement of point markers onto the links and the triangulation of these markers via fixed external cameras [19]. Such technology is widely available commercially, e.g., Vicon (Vicon Motion Systems Ltd, UK) and OptiTrack (NaturalPoint, Inc., Hillsboro, OR). These vision-based techniques have been widely applied to analyze human gait for clinical analysis [20–22]. Recently, a depth-sensing technique combining cameras, infrared projectors, and detectors has provided an inexpensive and portable alternative to capture the motion of multilink systems without the need for point markers. For instance, Pfister et al. [23] and Segura et al. [24] performed human gait analyses using the Microsoft Kinect (Microsoft). By using multiple optical sensors, these techniques can achieve high precision in motion measurement. However, motion capture is only possible in the space specified by the cameras, and each link must be clearly visible by at least two cameras at all times. This limits its application in complex or confined spaces.

The second type of approach recursively estimates the state of links using IMUs, which generally consist of a gyroscope, an accelerometer, and sometimes a magnetometer. If an IMU is attached to each link, the three-axis gyroscope measures the angular velocity of the link whereas the linear acceleration of the link is measured by a three-axis accelerometer. The additional three-axis magnetometer measures geomagnetism and provides the direction of magnetic north. Jung et al. [25] developed a mobile motion capture system using IMU and force sensors in the shoes to monitor human activity and health conditions. A different combination of these sensors leads to the use of different motion capture techniques. Taetz et al. [26] and Kok et al. [27] developed computationally efficient multilink motion capture systems and tracked the linear and angular motions correspondingly using only accelerometers and gyroscopes. With the deployment of single-axis accelerometers, Bagalà et al. [28] and Caroselli et al. [29] estimated the planar angular motion of a chained multilink system, such as human motion in the sagittal plane. A three-dimensional angular velocity estimation using only accelerometers was also developed by He and Cardou [30]. Some commercially available suits, such as Xsens (Xsens, Enschede, Netherlands) [31] and Smartsuit Pro (Rokoko, Copenhagen, Denmark), track 3D human motion using inexpensive micro-electro-mechanical system (MEMS) IMU sensors. This type of approach is critical when the working environment lacks observability or when joint encoders are unavailable [15].

While motion tracking by IMUs is free from the camera's issue of limited observability, one of the major limitations of IMUs is the drift that occurs when noisy acceleration and angular velocity signals are integrated. This drift is unavoidable even if a state estimator such as a Kalman filter (KF) is employed, though it can be significantly reduced if global sensors such as GPS [32,33], visual devices [34,35], or magnetometers [36–38] are added as correctors. Magnetometers are the most common correctors used in IMU-based multilink motion tracking since the attitude of each link can be fully observed and oriented in a global east-north-up (ENU) coordinate system [39].

So far, IMU-based motion tracking with global correction has been successfully applied only to systems which experience relatively low linear accelerations [36,39–41]. Such approaches rely on both magnetic fields and gravity measured by an IMU to estimate the attitude. These approaches are subject to potential issues such as sensor bias, which is well studied in the Refs. [42,43], and magnetic field attitude, which is often recorded, and the resulting magnetic map is often used to aid the attitude estimation [40,44]. In this paper, we consider a different problem of using IMU to estimate the attitude. That is, accelerometer-based gravity measurement is only accurate at near-constant velocities in inertial reference frames. When the system is highly dynamic, accelerometer measurements deliver not only information about the gravity vector but also information coming from system motion (i.e., linear and centrifugal accelerations) [45]. Since high-speed motion is important to accurately quantify in many applications, linear and angular accelerations must be modeled and handled properly such that motion is accurately tracked.

1.3 Objectives and Outline.

This paper presents a technique called dynamic measurements fusion that tracks the high-speed motion of a multilink system using measurements from IMUs in a new sensor arrangement. Unlike conventional approaches, the proposed technique uses accelerometers to measure the joint angular velocities of the multilink system together with gyroscopes whereas only the magnetometers are used to measure joint angles. The proposed arrangement of the magnetometers and the accelerometers enables the measurement of both linear and angular accelerations and thus leads to the accurate tracking of highly dynamic motion. The technique further formulates the sensor models as well as the motion model in the framework of an extended Kalman filter (EKF), which additionally enhances the motion tracking accuracy [46]. The motion model is described dynamically with the connection and inertial motion of the links.

This paper is organized as follows. Section 2 defines the motion tracking problem of concern. Section 3 presents the IMU-based technique for tracking the highly dynamic motion of multilink systems. Numerical analyses are conducted in Sec. 4 to investigate the performance of the proposed technique, and Sec. 5 summarizes conclusions and future work.

2 Motion Tracking of a Dynamic Multilink System

2.1 Problem Formulation.

Figure 1 shows the motion tracking problem of concern in this paper. Since drift caused by net system translation is well studied and can be corrected by conventional techniques, only drift in angular motion, which is the concern of this paper, is considered. Thus, the multilink system is assumed to be fixed to the ground by a single revolute joint at the first link. In the absence of joint encoders and external camera systems, the goal of this problem is to estimate the motion of the multilink system by attaching IMUs to the surface of each link, each of which consists of a 3-axis magnetometer, a 3-axis gyroscope, and a 3-axis accelerometer.

Fig. 1

View large Download slide

Schematic of motion tracking of a multilink system using IMU sensors

The problem is mathematically formulated as follows. Consider a multilink system with n joints where the joint angles

q \in ℝ^{n}

⁠, are measured with respect to the global frame. The dynamics of the multilink system are known to be

τ = M (q) \ddot{q} + V (q, \dot{q}) + F (q, \dot{q}) + G (q) - \sum_{i} J_{i}^{⊤} f_{i}

(1)

where $M \in ℝ^{n \times n}$ is the inertial matrix, and V, F, and $G \in ℝ^{n}$ are the Coriolis, friction and gravity forces , respectively, and $τ \in ℝ^{n}$ are joint torques ([1], Chap. 6.10). Each $f_{i}$ is a wrench vector of forces and torques corresponding to contacts with external bodies at links i with corresponding Jacobians $J_{i}$ ⁠.

2.2 Motion and Sensor Models.

The solution process of the state estimation problem most commonly starts by describing motion and sensor models in the state-space form. Let the two sets of state variables and the control variable be defined as

\begin{matrix} x_{1} = q (\in ℝ^{n}) \\ x_{2} = \dot{q} (\in ℝ^{n}) \\ u = τ (\in ℝ^{n}) \end{matrix}

The continuous state-space motion model is given by

{\dot{x}}_{1} = x_{2}

(2a)

{\dot{x}}_{2} = M {(x_{1})}^{- 1} [u - V (x_{1}, x_{2}) - F (x_{1}, x_{2}) - G (x_{1}) + \sum_{i} J_{i}^{⊤} f_{i}] + w_{2}

(2b)

where

w_{2} \in ℝ^{n}

is the motion noise. It is to be noted that the motion noise can be more structured than what we present here, i.e., dependent on the position and/or dependent on the fluctuations in applied torques and forces. For the sake of generality we chose a simpler additive model. Meanwhile, there is no motion noise for

{\dot{x}}_{1}

as its equity to

x_{2}

is exact. The motion of the multilink system can be simulated iteratively by specifying the initial joint angles and angular velocities

\begin{matrix} x_{1, 0} = q_{0} \\ x_{2, 0} = {\dot{q}}_{0} \end{matrix}

To overcome the uncertainty due to motion noise

w

⁠, state estimation is achieved by IMU measurements. Since the magnetometer, gyroscope, and accelerometer of an IMU each measure in three axes, let the measurements of the magnetometer, gyroscope and accelerometer be

z_{m}, z_{g}, z_{a} \in ℝ^{3 n}

⁠. The measurements are related to the state of the multilink system through

z_{m} = h_{m} (x) + v_{m}

(3a)

z_{g} = h_{g} (x) + v_{g}

(3b)

z_{a} = h_{a} (x) + v_{a}

(3c)

where $x = {[x_{1}^{⊤} x_{2}^{⊤}]}^{⊤}$ is the state to estimate, and $h_{m}, h_{g}, h_{a} : ℝ^{2 n} \to ℝ^{3 n}$ map the state the measurement of the magnetometer, gyroscope and accelerometer and are the sensor models for them , respectively, and $v_{m}, v_{g}, v_{a} \in ℝ^{3 n}$ is the noise associated with measurement [47].

2.3 Conventional Quasi-Steady Motion Tracking.

The conventional quasi-steady motion tracking orients each link in the global frame using magnetometers and accelerometers as follows. The accelerometer and magnetometer attached to a link measure the gravity vector

g^{b} = {[g_{x}^{b}, g_{y}^{b}, g_{z}^{b}]}^{⊤}

(when static) and magnetic field vector

m^{b} = {[m_{x}^{b}, m_{y}^{b}, m_{z}^{b}]}^{⊤}

in the body frame b, which are assumed to be constant. The following equations govern the transformation of the gravity and magnetometer vectors from the global frame into the body frame

g^{b} = R (ϕ, ψ, θ) g

(4)

m^{b} = R (ϕ, ψ, θ) m

(5)

where the Euler angles

ϕ

⁠, θ, and ψ are the roll, pitch, and yaw angles with respect to the global ENU coordinate frame, and

g = {[0, 0, - g]}^{⊤}

and

m = {[0, m_{y}, m_{z}]}^{⊤}

are the corresponding gravity and the earth's magnetic field vector in the ENU frame. R denotes the direction cosine matrix (DCM) given by

R (ϕ, θ, ψ) = [\begin{matrix} c_{θ} c_{ψ} & c_{θ} s_{ψ} & - s_{θ} \\ s_{ϕ} s_{θ} c_{ψ} - c_{ϕ} s_{ψ} & s_{ϕ} s_{θ} s_{ψ} + c_{ϕ} c_{ψ} & c_{θ} s_{ϕ} \\ c_{ϕ} s_{θ} c_{ψ} + s_{ϕ} s_{ψ} & c_{ϕ} s_{θ} s_{ψ} - s_{ϕ} c_{ψ} & c_{θ} c_{ϕ} \end{matrix}]

(6)

where $c_{(\cdot)} = cos (\cdot)$ and $s_{(\cdot)} = sin (\cdot)$ ⁠.

The goal of motion tracking is to identify the roll, pitch and yaw angles for each link. The roll and pitch angles can be obtained by substituting g into Eq. while the yaw angle can be determined by substituting m into Eq. . An explicit solution to these angles is therefore obtained as

ϕ = \arctan (g_{y}^{b} / g_{z}^{b})

(7a)

θ = \arctan [- g_{x}^{b} / (g_{y}^{b} s in ϕ + g_{z}^{b} c os ϕ)]

(7b)

ψ = \arctan [(m_{x}^{b} c_{θ} + m_{y}^{b} s_{ϕ} c_{θ} + m_{z}^{b} c_{ϕ} c_{θ}) / (m_{y}^{b} c_{ϕ} - m_{z}^{b} s_{ϕ})]

(7c)

where $g^{b} = {[g_{x}^{b}, g_{y}^{b}, g_{z}^{b}]}^{⊤}$ and $m^{b} = {[m_{x}^{b}, m_{y}^{b}, m_{z}^{b}]}^{⊤}$ are the measured gravity and magnetic vector [48]. Motion tracking using IMU measurements and Euler angles can be summarized in Fig. 2. We refer to the techniques based on Eqs. (4), (5), and (7) as conventional approaches against which we compare our proposed technique in Sec. 3. With multiple simultaneous measurements of the gravity and the magnetic field, the orientation can be determined with high accuracy using the TRIAD or the QUEST algorithms [39,41,49].

Fig. 2

View large Download slide

Conventional motion tracking with IMU-determined attitude

The fundamental problem of conventional motion tracking is that the computed orientation is accurate only when the multilink system moves at a constant velocity. This is because the dynamics of each link will introduce accelerations in the measured $g^{b}$ which obscure the true (constant) gravity vector. In high-speed motion, these aliasing accelerations are non-negligible, so all the roll, pitch, and yaw computations through Eq. (7) become inaccurate. Since the links are connected, these inaccuracies compound from one link to the next. The next section presents the proposed technique which alters the conventional usage of IMU sensors and incorporates the accelerations caused by the high-speed motion of the links to track their movement.

3 Inertial Measurement Unit-Based Motion Tracking Using Kalman Filter

3.1 Overview.

Figure 3 provides the architecture of the proposed motion tracking technique for a multilink system using dynamic measurements fusion. To incorporate the motion model of the link system with inertial sensors and recursively estimate the state of the dynamical system (joint angles and angular velocities) in the presence of noise, the proposed motion tracking technique uses the framework of an EKF [50]. The state of the dynamical system, defined with the mean $x_{k | k}$ and the covariance $Σ_{k | k}$ at time-step k, is predicted to $x_{k + 1 | k}$ and $Σ_{k + 1 | k}$ using the dynamics of a multilink system as a motion model. As an IMU-based motion tracking method, the proposed technique utilizes an IMU comprised of a 3-axis magnetometer, 3-axis accelerometer, and 3-axis gyroscope attached to each link for motion tracking. The predicted $x_{k + 1 | k}$ and $Σ_{k + 1 | k}$ ⁠, are corrected to $x_{k + 1 | k + 1}$ and $Σ_{k + 1 | k + 1}$ using and fuzing the measurements of the sensors (m, a, and $ω$ ⁠) as well as their Kalman gains (⁠ $K_{k}^{M}, K_{k}^{A}$ ⁠, and $K_{k}^{G}$ ⁠).

Fig. 3

View large Download slide

Proposed motion tracking technique using dynamic measurements fusion

The novelty of the proposed technique lies in the rearrangement of the magnetometers, accelerometers, and gyroscopes and the subsequent new sensor models in the EKF estimation framework, which are shown in gray in Fig. 3. While the conventional technique uses accelerometer measurements with magnetometer measurements to correct joint angles as in Eq. (7) for slow motion, the proposed technique uses the magnetometer measurements to correct joint angles and uses the accelerometer and gyroscope measurements to estimate the joint angular velocities. The elimination of the erroneous gravity measurements in motion with varying acceleration improves the estimation accuracy of the joint angles. The addition of the acceleration measurements improves the estimation accuracy of the joint angular velocities again in motion with varying acceleration. The proposed technique is thus expected to track high-speed motion even in the presence of high accelerations.

3.2 Probabilistic Motion and Sensor Models

3.2.1 Motion Model.

In order to predict the motion of the multilink system using the EKF, the motion model of Eq. is discretized as

x_{1, k} = x_{1, k - 1} + δ t x_{2, k - 1}

(8a)

x_{2, k} = x_{2, k - 1} + δ t [a (x_{1, k - 1}, x_{2, k - 1}) + b (x_{1, k - 1}) + c (x_{1, k - 1}, f) + w_{2, k}]

(8b)

where

a (x_{1, k - 1}, x_{2, k - 1}) = M {(x_{1, k - 1})}^{- 1} [- V (x_{1, k - 1}, x_{2, k - 1}) - F (x_{1, k - 1}, x_{2, k - 1})]

(9a)

b (x_{1, k - 1}) = M {(x_{1, k - 1})}^{- 1} [u_{k} - G (x_{1, k - 1})]

(9b)

c (x_{1, k - 1}, f) = M {(x_{1, k - 1})}^{- 1} \sum_{i} J_{i}^{⊤} f_{i}

(9c)

Collect the forces associated with joint angles and angular velocities. The Jacobian of the motion model, denoted as

A_{k - 1}

(⁠

\in ℝ^{2 n \times 2 n}

⁠), is derived by taking the derivative of the first-order differential equation of Eq. with respect to the given state

x_{k} = {[x_{1, k}^{⊤}, x_{2, k}^{⊤}]}^{⊤}

as

A_{k - 1} = [\begin{matrix} \nabla_{x_{1}} (x_{1, k}) & \nabla_{x_{2}} (x_{1, k}) \\ \nabla_{x_{1}} (x_{2, k}) & \nabla_{x_{2}} (x_{2, k}) \end{matrix}] = [\begin{matrix} I & δ t I \\ δ t \nabla_{x_{1}} (a + b + c) & I + δ t \nabla_{x_{2}} a \end{matrix}] |_{x_{k - 1}}

(10)

where $\nabla_{x_{1}}$ and $\nabla_{x_{2}}$ stand for the matrix derivatives with respect to $x_{1}$ and $x_{2}$ ⁠, respectively.

3.2.2 Sensor Model for Magnetometers.

The magnetometers observe the orientation of the whole multilink system if equipped on all links. Assuming the magnetic field is constant in the global frame, following the notation of frames defined in the Appendix, the measurements of magnetometers yield the following equation:

Q {(x_{1, k})}^{⊤} (z_{m, k} - v_{m, k}) = Q {(x_{1, 0})}^{⊤} (z_{m, 0} - v_{m, 0})

(11)

where

z_{m, k} \in ℝ^{3 n}

and

v_{m, k} \in ℝ^{3 n}

are the magnetometer measurement and its noise at step k , respectively, and

Q (x_{1, k}) \in ℝ^{3 n \times 3 n}

is a block diagonal matrix that is given by

Q (x_{1, k}) = diag (\begin{matrix} T_{1} (x_{1, k}), & \dots, & T_{n} (x_{1, k}) \end{matrix})

(12)

where

T_{i} (x_{1, k}) \in ℝ^{3 \times 3}

⁠, defined in Eq. (in the Appendix), is the rotation matrix of the ith link. The multiplication of

Q (x_{1, k})

on both sides and rearrangement of the

v_{m, k}

to the right-hand side yield the sensor model for the magnetometers as

\begin{matrix} z_{m, k} = h_{m} (x_{1, k}) + v_{m, k}^{'} \equiv Q (x_{1, k}) Q {(x_{1, 0})}^{⊤} z_{m, 0} + (v_{m, k} - Q (x_{1, k}) Q {(x_{1, 0})}^{⊤} v_{m, 0}) \end{matrix}

(13)

where $v_{m, k}^{'} = v_{m, k} - Q (x_{1, k}) Q {(x_{1, 0})}^{⊤} v_{m, 0} \in ℝ^{3 n}$ is the sensor model noise of which the covariance is updated at every time-step k with a given state $x_{1, k}$ ⁠. The formula has the advantage that no exact magnetic north vector is measured.

Since this is nonlinear with respect to

x_{1, k}

⁠, the Jacobian of the sensor model is needed for covariance correction in EKF,

H_{k}^{M} \in ℝ^{3 n \times 2 n}

⁠. The Jacobian is derived by taking the derivative with respect to the state variable as

H_{k}^{M} = [\begin{matrix} \nabla_{x_{1}} h_{m} (x_{1, k}) & 0_{3 n \times n} \end{matrix}] |_{x_{1, k}}

(14)

3.2.3 Sensor Model for Gyroscopes.

The sensor model of a gyroscope can be derived by equating the angular velocity measured by the gyroscope to the system state in a global frame. It is written as

T_{i}^{⊤} (x_{1, k}) (z_{g i, k} - v_{g i, k}) = \sum_{l = 1}^{i} T_{l}^{⊤} (x_{1, k}) w_{l, k} (x_{2, k})

(15)

where $z_{g i, k} and v_{g i, k} \in ℝ^{3}$ are the angular velocity measurements of ith gyroscope and its noise, and $w_{l, k} (x_{2, k}) = {[0, 0, {\dot{q}}_{l, k}]}^{⊤}$ is the angular velocity of ith link in its body frame. In the equation, $T_{l}^{⊤} (x_{1, k}) w_{l, k} (x_{2, k})$ converts the angular velocity from the body frame to the global frame while the summation transfers the relative angular velocity to an absolute one in the global frame.

For the configuration that gyroscopes are installed on all links, the sensor model is written as

\begin{matrix} z_{g, k} = h_{g} (x_{1, k}, x_{2, k}) + v_{g, k} \equiv Q (x_{1, k}) [\begin{matrix} T_{1}^{⊤} (x_{1, k}) & 0 \\ T_{1}^{⊤} (x_{1, k}) & T_{2}^{⊤} (x_{1, k}) \\ ⋮ & ⋮ & ⋱ \\ T_{1}^{⊤} (x_{1, k}) & T_{2}^{⊤} (x_{1, k}) & \dots & T_{n}^{⊤} (x_{1, k}) \end{matrix}] [\begin{matrix} c w_{1} (x_{2, k}) \\ w_{2} (x_{2, k}) \\ ⋮ \\ w_{n} (x_{2, k}) \end{matrix}] + v_{g, k} \end{matrix}

(16)

The Jacobian of the corresponding sensor model

H_{k}^{G} \in ℝ^{3 n \times 2 n}

is derived by taking the derivative

H_{k}^{G} = [\begin{matrix} \nabla_{x_{1}} h_{g} (x_{1, k}, x_{2, k}) & \nabla_{x_{2}} h_{g} (x_{1, k}, x_{2, k}) \end{matrix}] |_{x_{1, k}, x_{2, k}}

(17)

3.2.4 Sensor Model for Accelerometers.

The proposed technique additionally measures angular velocities from accelerometers using the inertial sensor arrays method. The theory and application have been explored in the Refs. [51–54]. For the extended applications, the reader can refer to the survey of [55,56]. For completeness, we briefly describe the theory and derive the sensor model for the later application. The sensor model can be derived using the relation of accelerations at differential positions on a rigid link, that is

a_{p 2} = a_{p 1} + \dot{ω} \times r + ω \times (ω \times r)

(18)

where

a_{p 1} \in ℝ^{3}

and

a_{p 2} \in ℝ^{3}

are the accelerations at point p1 and point p2 on the same link , respectively, and

r \in ℝ^{3}

is the vector from p1 to p2.

ω

and

\dot{ω}

are the rigid body's angular velocity and angular acceleration. The multiplication of Eq. by r and its rearrangement yields a relation between acceleration and angular velocity as

(a_{p 2} - a_{p 1}) \cdot r = (ω \times (ω \times r)) \cdot r = {(ω \cdot r)}^{2} - {‖ r ‖}^{2} {‖ ω ‖}^{2}

(19)

As the dot product is invariant when performing frame rotation, the equation can accommodate quantities in not only the global frame but also the body frame with respect to which the accelerometers measure acceleration. The accelerometer measurement

{\tilde{a}}^{b}

in the body frame is given by

{\tilde{a}}^{b} = a^{b} + g^{b} + v_{a}

(20)

where

a^{b}

is the true acceleration of motion excluding the gravity

g^{b}

⁠, and both are in the body frame.

v_{a}

is the acceleration measurement noise. The substitution of the equation into Eq. yields

\begin{matrix} [({\tilde{a}}_{2}^{b} - g^{b} - v_{a 2}) - ({\tilde{a}}_{1}^{b} - g^{b} - v_{a 1})] \cdot r^{b} & = ({\tilde{a}}_{2}^{b} - {\tilde{a}}_{1}^{b}) \cdot r^{b} - (v_{a 2} - v_{a 1}) \cdot r^{b} \\ = {(ω^{b} \cdot r^{b})}^{2} - {‖ r^{b} ‖}^{2} {‖ ω^{b} ‖}^{2} \end{matrix}

(21)

Now that the relation between accelerometer measurements and angular velocities has been described at two points on a rigid link, the sensor model for accelerometers can be derived by first defining two new variables e_i and v_i from the measurement and the noise respectively and a variable

h_{i} (x_{1, k}, x_{2, k})

from the state variable as

e_{i} = ({\tilde{a}}_{2}^{b} - {\tilde{a}}_{1}^{b}) \cdot r^{b}

(22a)

v_{i} = (v_{a 2} - v_{a 1}) \cdot r^{b}

(22b)

h_{i} (x_{1, k}, x_{2, k}) = {(ω_{i}^{b} (x_{1, k}, x_{2, k}) \cdot r^{b})}^{2} - {‖ r^{b} ‖}^{2} {‖ ω_{i}^{b} (x_{1, k}, x_{2, k}) ‖}^{2}

(22c)

where the angular velocities

ω_{i}^{b} (x_{1, k}, x_{2, k})

are given by

ω_{i}^{b} (x_{1, k}, x_{2, k}) = T_{i} (x_{1, k}) \sum_{l = 1}^{i} T_{l}^{⊤} (x_{1, k}) w_{l, k} (x_{2, k})

(23)

The sensor model is then given by substituting e_i, v_i, and h_i into Eq. and rearranging as

z_{a, k} = h_{a} (x_{1, k}, x_{2, k}) + v_{a, k}

(24)

where

z_{a, k} \in ℝ^{n}, v_{a, k} \in ℝ^{n}

⁠, and

h_{a} (x_{1, k}, x_{2, k}) \in ℝ^{n}

and are given by

z_{a, k} = [\begin{matrix} e_{1} \\ ⋮ \\ e_{n} \end{matrix}], v_{a, k} = [\begin{matrix} v_{1} \\ ⋮ \\ v_{n} \end{matrix}], h_{a} (x_{1, k}, x_{2, k}) = [\begin{matrix} h_{1} \\ ⋮ \\ h_{n} \end{matrix}]

(25)

The Jacobian of the corresponding sensor model

H_{k}^{A} \in ℝ^{n \times 2 n}

is derived by taking the derivative

H_{k}^{A} = [\begin{matrix} \nabla_{x_{1}} h_{a} (x_{1, k}, x_{2, k}) & \nabla_{x_{2}} h_{a} (x_{1, k}, x_{2, k}) \end{matrix}] |_{x_{1, k}, x_{2, k}}

(26)

As the term of $h_{a} (x_{1, k}, x_{2, k})$ contains second-order polynomials of the angular velocities, the sensor model can be more effective to correct the estimated state when the angular velocities are large as the Jacobian matrix has a large weight.

Finally, as sensors are installed at joints as shown in Fig. 1, the left-hand side of Eq. is reformulated by rotating values from the body frame of link i + 1 to that of i by the matrix

{[R (0, 0, x_{1 i + 1}) R_{i}^{i + 1}]}^{⊤}

that has been defined in Eq. in the Appendix, and thus yields

\begin{matrix} {{[R (0, 0, x_{1 i + 1}) R_{i}^{i + 1}]}^{⊤} ({\tilde{a}}_{i + 1}^{b} - g^{b, i + 1} - v_{a i + 1}) - ({\tilde{a}}_{i}^{b} - g^{b, i} - v_{a i})} \cdot r^{b} = \\ {{[R (0, 0, x_{1 i + 1}) R_{i}^{i + 1}]}^{⊤} {\tilde{a}}_{i + 1}^{b} - {\tilde{a}}_{i}^{b}} - {{[R (0, 0, x_{1 i + 1}) R_{i}^{i + 1}]}^{⊤} v_{a i + 1} - v_{a i}} \\ - {{[R (0, 0, x_{1 i + 1}) R_{i}^{i + 1}]}^{⊤} g^{b, i + 1} - g^{b, i}} \end{matrix}

(27)

where

{\tilde{a}}_{i}^{b}

is the measurement of the ith accelerometer, and

g^{b, i}

denotes the gravity in the ith link's body frame. When the motion is accurately tracked, e.g., using the EKF presented in § 2.3, the estimated orientation is close to the true value. For each link i, the term of

{[R (0, 0, x_{1 i + 1}) R_{i}^{i + 1}]}^{⊤} g^{b, i + 1} - g^{b, i}

is negligible because both terms represent the gravity in the body from of link i. Hence, the sensor model for accelerometer is finalized by redefining e_i and v_i as

e_{i} = {{[R (0, 0, {\hat{x}}_{1 i + 1}) R_{i}^{i + 1}]}^{⊤} {\tilde{a}}_{i + 1}^{b} - {\tilde{a}}_{i}^{b}} \cdot r^{b}

(28a)

v_{i} = {{[R (0, 0, {\hat{x}}_{1 i + 1}) R_{i}^{i + 1}]}^{⊤} v_{a i + 1} - v_{a i}} \cdot r^{b}

(28b)

and substituting them into the sensor model of Eq. (24), where ${\hat{x}}_{1 i + 1}$ denotes numerical estimation of $x_{1 i + 1}$ obtained from the proposed Kalman filter.

3.3 Extended Kalman Filter Based State Estimation.

Since the probabilistic motion model is defined in Eq. , the prediction process using the EKF can be formulated straightforwardly. The predicted mean of the state at step k, i.e.,

{\hat{x}}_{(\cdot), k | k - 1}

⁠, is derived from the motion model as

{\hat{x}}_{1, k | k - 1} = {\hat{x}}_{1, k - 1 | k - 1} + δ t {\hat{x}}_{2, k - 1 | k - 1}

(29a)

{\hat{x}}_{2, k | k - 1} = {\hat{x}}_{2, k - 1 | k - 1} + δ t [a ({\hat{x}}_{1, k - 1 | k - 1}, {\hat{x}}_{2, k - 1 | k - 1}) + b ({\hat{x}}_{1, k - 1 | k - 1})]

(29b)

where

{\hat{x}}_{(\cdot), k - 1 | k - 1}

is the state estimated at previous step. The covariance

Σ_{k | k - 1} \in ℝ^{2 n \times 2 n}

is predicted using the following equation

Σ_{k | k - 1} = A_{k - 1} Σ_{k - 1 | k - 1} A_{k - 1}^{⊤} + Σ_{w, k}

(30)

where

A_{k - 1} \in ℝ^{n \times 2 n}

is the Jacobian of the motion model, and

Σ_{w, k} \in ℝ^{2 n \times 2 n}

is the covariance of the motion noise with the form

Σ_{w, k} = [\begin{matrix} 0 & 0 \\ 0 & Σ_{w_{2}, k} \end{matrix}]

(31)

Similarly to the motion model, the sensor models, Eqs. , , and , also formulate the correction process straightforward with parallel sensor fusion. Since the magnetometers correct the joint angles whereas the gyroscopes and accelerometers correct the joint angluar velocities, the means of the joint angles and the joint angular velocities are updated respectively by

{\hat{x}}_{1, k | k} = {\hat{x}}_{1, k | k - 1} + K_{k}^{M} [z_{m, k} - h_{m} ({\hat{x}}_{1, k | k - 1})]

(32a)

{\hat{x}}_{2, k | k} = {\hat{x}}_{2, k | k - 1} + K_{k}^{G} [z_{g, k} - h_{g} ({\hat{x}}_{1, k | k - 1}, {\hat{x}}_{2, k | k - 1})] + K_{k}^{A} [z_{a, k} - h_{a} ({\hat{x}}_{1, k | k - 1}, {\hat{x}}_{2, k | k - 1})]

(32b)

where

K_{k}^{M} \in ℝ^{n \times 3 n}, K_{k}^{G} \in ℝ^{n \times 3 n}

⁠, and

K_{k}^{A} \in ℝ^{n \times n}

are the Kalman gains for the magnetometer, gyroscope, and accelerometer and are given by

K_{k}^{M} = Σ_{k | k} {(H_{k}^{M})}^{⊤} {(Σ_{k}^{M})}^{- 1}

(33a)

K_{k}^{G} = Σ_{k | k} {(H_{k}^{G})}^{⊤} {(Σ_{k}^{G})}^{- 1}

(33b)

K_{k}^{A} = Σ_{k | k} {(H_{k}^{A})}^{⊤} {(Σ_{k}^{A})}^{- 1}

(33c)

Here

Σ_{k | k} \in ℝ^{2 n \times 2 n}

is the covariance of the state estimation which is updated by

Σ_{k | k}^{- 1} = Σ_{k | k - 1}^{- 1} + \sum_{i} {(H_{k}^{i})}^{⊤} {(Σ_{k}^{i})}^{- 1} H_{k}^{i}

(34)

where $Σ_{k}^{(\cdot)}$ represents the covariance of sensor noises in Eqs. (13), (16), and (24).

Now that the EKF estimation formulation is complete with the proper arrangement of motion and sensor models for high-speed motion tracking, the proposed technique is expected to show its efficacy.

4 Numerical Validation

Having its novelty and strength identified, it is essential to validate the effectiveness of the proposed technique for motion tracking. The following investigation aimed to identify the capability and limitations of the proposed technique through the parametric studies of a two-arm link system in simulation. The ability of the proposed IMU-based motion tracking technique with the magnetometer was then investigated.

4.1 Two-Arm Link System for Motion Tracking.

Figure 4 shows a two-arm link system that was used for the proof of concept and parametric study. The two-link system is a simplified problem of a crash vehicle project to investigate the motion of human body during the crash. The time duration is short during the experiment and is often 100 ms $\sim$ 200 ms. The working plane is vertical and aligned with magnetic north so the orientations estimated using both the magnetic vector and gravity can be compared. The arm was initially stretched perpendicular to the direction of the motion at t = t_init, as shown in Fig. 4. The base of the arm then moved along the rail at a constant speed v and hit a stopper to create a swinging motion.

Fig. 4

View large Download slide

Schematic of a two-arm link system

4.1.1 Motion Model for the Planar Two-Arm Link System.

Derived from Lagrangian mechanics, as shown in Ref. ([1], Chap. 6.10), the inertial, gravity, and Coriolis matrices of the dynamic Eq. are given by

\begin{matrix} M (q) & = [\begin{matrix} m_{2} + \frac{m_{1}}{s_{2}^{2}} & 0 \\ 0 & m_{2} \end{matrix}], G (q) = g [\begin{matrix} m_{1} \frac{c_{1}}{s_{2}} + m_{2} s_{12} \\ m_{2} c_{12} \end{matrix}], \\ V (q, \dot{q}) & = [\begin{matrix} - (m_{2} l_{1} c_{2} + m_{2} l_{2}) {\dot{q}}_{1}^{2} - m_{2} l_{2} {\dot{q}}_{2}^{2} - (2 m_{2} l_{2} + m_{2} l_{1} c_{2} + m_{1} l_{1} \frac{c_{2}}{s_{2}^{2}}) {\dot{q}}_{1} {\dot{q}}_{2} \\ m_{2} l_{1} s_{2} {\dot{q}}_{1}^{2} + l_{1} m_{2} s_{2} {\dot{q}}_{1} {\dot{q}}_{2} \end{matrix}] \end{matrix}

(35)

where m₁ and m₂ are the masses of two links, and l₁ and l₂ are the corresponding lengths. Variables with c and s are

\begin{matrix} c_{1} & = cos (q_{1}), s_{2} = sin (q_{2}), c_{2} = cos (q_{2}), \\ c_{12} & = cos (q_{1} + q_{2}), s_{12} = sin (q_{1} + q_{2}) \end{matrix}

In the dynamic equation, the torques and friction forces on the joints were neglected.

4.1.2 Sensor Models for the Planar Two-Arm Link System.

When all links move in one plane, the sensor models can be greatly simplified because of the reduced dimensions of measurements. One of the simplifications is made on orientation, which is to be measured by the magnetometer only in two axes, i.e.,

z_{m, k} and v_{m, k} \in ℝ^{2 n}

⁠. The sensor model for magnetometer of Eq. with the reduced magnetometer measurements is accomplished with a reduced rotation matrix

Q (x_{1, k}) = diag (T_{1} (x_{1, k}), \dots, T_{n} (x_{1, k})) \in ℝ^{2 n \times 2 n}

(36)

where

T_{i} (x_{1, k}) = R (\sum_{l = 1}^{i} x_{1 l, k}), and R (θ) = [\begin{matrix} cos (θ) & sin (θ) \\ - sin (θ) & cos (θ) \end{matrix}]

(37)

Similarly, only 1 deg of rotation is measured for the planar motion. Hence, the sensor model for a gyroscope, i.e., Eq. , is reduced to

z_{g, k} = L x_{2, k} + v_{g, k}

(38)

where

z_{g, k}, v_{g, k} \in ℝ^{n}

⁠, and

L \in ℝ^{n \times n}

is a lower triangular matrix with element l_ij = 1 for

i \geq j

⁠. The Jacobian of the planar sensor model is thus

H_{k}^{G} = [\begin{matrix} 0_{n \times n} & L \end{matrix}]

(39)

As

ω^{b} \cdot r^{b}

equals zero for the planar link motion, the sensor model for the accelerometers, i.e., Eq. , is simplified as

z_{a, k} = [\begin{matrix} e_{1, k} \\ ⋮ \\ e_{n, k} \end{matrix}] = h_{a} (x_{2, k}) \equiv - [\begin{matrix} l_{1}^{2} x_{21, k}^{2} \\ ⋮ \\ l_{n}^{2} x_{2 n, k}^{2} \end{matrix}] + [\begin{matrix} v_{e 1, k} \\ ⋮ \\ v_{e n, k} \end{matrix}]

(40)

where l_i is the length of ith link. The corresponding Jacobian is given by

H_{k}^{A} = [\begin{matrix} 0_{n \times n} & \nabla_{x_{2}} h_{a} (x_{2, k}) \end{matrix}] |_{x_{2, k}}

(41)

where $\nabla_{x_{2}} h_{a} (x_{2, k}) = - 2 diag (l_{1}^{2} x_{21, k}, \dots, l_{n}^{2} x_{2 n, k}) \in R^{n \times n}$ ⁠.

For the particular two-link system, n equals 2 in the sensor models of Eqs. (36), (38), and (40).

4.1.3 Parameters of the Two-Arm Link System.

To test the proposed motion tracking technique, the motion of the two-link system was integrated numerically and defined the ground truth. Meanwhile, the measurements of IMU sensors were obtained by adding some white noises to Eqs. (35)–(41) as many sensors can be characterized as having white noise. The noise distribution is assumed known and its statistics properties, listed in Table 1, are used in the proposed state estimation technique.

Table 1

Parameters of the two-link system and simulation

Parameter	Value
Link lengths $l_{1} = l_{2} = l$ (m)	0.25
Link mass $m_{1} = m_{2} = m$ (kg)	0.5
Gyroscope sample rate (Hz)	1,000
Accelerometer sample rate (Hz)	1,000
Magnetometer sample rate (Hz)	1,000
Initial pose $q_{0}$ (rad)	${[- π / 2, 0]}^{⊤}$
Initial linear velocity $v_{i, 0}$ (m/s)	0.5, 1, or 5
Motion noise covariance $Σ_{w_{2}, k}$ (rad²/s²)	$diag (25, 25)$
Gyroscope noise deviation σ_g (rad/s)	20
Accelerometer noise deviation σ_a (m/s²)	50
Magnetometer noise deviation σ_m (μT)	2.5

Parameter	Value
Link lengths $l_{1} = l_{2} = l$ (m)	0.25
Link mass $m_{1} = m_{2} = m$ (kg)	0.5
Gyroscope sample rate (Hz)	1,000
Accelerometer sample rate (Hz)	1,000
Magnetometer sample rate (Hz)	1,000
Initial pose $q_{0}$ (rad)	${[- π / 2, 0]}^{⊤}$
Initial linear velocity $v_{i, 0}$ (m/s)	0.5, 1, or 5
Motion noise covariance $Σ_{w_{2}, k}$ (rad²/s²)	$diag (25, 25)$
Gyroscope noise deviation σ_g (rad/s)	20
Accelerometer noise deviation σ_a (m/s²)	50
Magnetometer noise deviation σ_m (μT)	2.5

The numerical simulation was first performed with an initial linear velocity $v_{i, 0} =$ 5 m/s. The acceleration on the joints was proportional to $v_{i, 0}^{2} / l =$ 100 m/s² and was much larger than the gravitational acceleration of 9.8 m/s². It is therefore ideal to demonstrate the effectiveness of the proposed technique. In the test, the signal-to-noise ratio (SNR) was chosen around 10 to approximate the presence of moderate noises in the measurements. Hence, the deviations were $σ_{a} =$ 50m/s², $σ_{m} =$ 2.5μT, and $σ_{g} =$ 20 rad/s, respectively for the accelerometer, magnetometer, and gyroscope. As the motion tracking technique is expected to work for a wide range of dynamic motions, the efficacy under different initial linear speeds of 0.5 m/s, 1 m/s, and 5 m/s or a wide range of measurement noises is investigated on the two-link system. The rate of IMU measurement is assumed to be 1000 Hz as numerical investigation shows that a sampling rate of 1000 Hz is enough to capture the motion of the link system.

4.2 Effect of Initial Linear Velocity.

As the conventional technique requires gravity to be measured, which is often subject to the centrifugal acceleration due to the link motion, the initial linear velocity is hence examined as the acceleration is proportional to $v^{2} / l$ ⁠, where l is the length of the link.

4.2.1 Results With High Initial Linear Velocity.

Figure 5 shows the first set of motion-tracking simulations conducted at the initial linear velocity of $v_{i, 0}$ = 5 m/s. For comparison, the results of a conventional technique, which uses a gravity sensor for orientation estimation, are presented in addition to the ground truth. Figure 5 shows the results of motion tracking with solid lines while the ground truth is presented with dotted lines. The estimated lines and the ground truth lines are indicated by $[{\hat{q}}_{1, k | k}, {\hat{q}}_{2, k | k}]$ and $[q_{1}^{*}, q_{2}^{*}]$ ⁠, respectively. In Figs. 5(a) and 5(b), the means of the joint angles and the joint angular velocities estimated respectively by the proposed technique are shown together with its magnetometer measurements $[z_{m 1, k}, z_{m 2, k}]$ marked by circle (⁠ $○$ ⁠) and gyroscope measurements $[z_{g 1, k}, z_{g 2, k}]$ marked by triangle (⁠ $△$ ⁠). While the measurements are and remain considerably noisy, the result shows that the proposed technique nearly coincides with the ground truth. This indicates the ability of the proposed technique is not only filter noises but also estimate the motion accurately. Figures 5(c) and 5(d) comparatively show the joint angles and the joint angular velocities estimated respectively by the conventional technique, which also uses the EKF as a framework but incorporates Eq. (7) for measurements. The accelerometers' gravity measurements are marked by a cross (×). It is seen that the estimated joint angles have a significant difference from the ground truth while the estimated joint angular velocities also have some deviation. The difference in the estimated joint angles is because Eq. (7) is not valid in high-speed motion.

Fig. 5

Motion tracking of a two-arm link system using the proposed and conventional techniques when the initial linear speed is 5 m/s. q̂(·) and q(·)* denote the estimated results and the ground truth respectively and z(·) represents the corresponding measured value: (a) Joint angles, proposed, (b) Joint angular velocity, proposed, (c) Joint angles, conventional, and (d) Joint angular velocity, conventional.

View large Download slide

Motion tracking of a two-arm link system using the proposed and conventional techniques when the initial linear speed is 5 m/s. ${\hat{q}}_{(\cdot)}$ and $q_{(\cdot)}^{*}$ denote the estimated results and the ground truth respectively and $z_{(\cdot)}$ represents the corresponding measured value: (a) Joint angles, proposed, (b) Joint angular velocity, proposed, (c) Joint angles, conventional, and (d) Joint angular velocity, conventional.

Figure 6 shows the transition of the differential entropy of the proposed technique and the conventional technique. The differential entropy, defined as

entropy = \frac{1}{2} l og [{(2 π e)}^{n} | Σ_{k | k} |]

(42)

Represents the uncertainty of the estimation, where e is the exponential constant (⁠ $e \approx 2.7182818$ ⁠). The result shows that the proposed technique keeps the differential entropy much lower than the conventional technique. Unlike the conventional technique that assumes gravity dominates the bodies' accelerations and thus increases the uncertainty, the uncertainty of the proposed technique is low because the sensor measurements are fed to formulas properly.

Fig. 6

View large Download slide

The estimation differential entropy

4.2.2 Results With Different Initial Linear Velocities.

Having the effectiveness of the proposed technique at the high initial linear velocity of $v_{i, 0}$ = 0.5 m/s understood, this subsection analyzes its effectiveness with different initial linear velocities of 0.5 m/s, 1 m/s, and 5 m/s. Figures 7(a), 7(b), and 7(c) show the motion of the arm (solid lines) tracked by the proposed technique together with the ground truth (dotted lines) when the initial linear velocity was 0.5 m/s, 1 m/s, and 5 m/s, respectively. The motion is captured every 0.01 s up to 0.2 s. The black and gray colors are used for the first and second links, respectively. Figure 7 show that the proposed technique tracks the arm motion nearly perfectly regardless of the initial linear velocity. These indicate the efficacy of the proposed technique in a wide range of dynamic motions including low-speed motion. For comparison, Figs. 7(d), 7(e), and 7(f) show the results of the conventional motion-tracking technique with the initial linear velocities of 0.5 m/s, 1 m/s, and 5 m/s. The results show that the tracked motion is wrong when the initial linear velocity is 5 m/s whereas the motion is close to the ground truth when the initial linear velocity is 0.5 m/s. The comparative study endorses the deficiency of the conventional technique and the applicability of the proposed technique to high-speed motion tracking.

Fig. 7

View large Download slide

Motion tracking of a two-arm link system with the initial linear speeds of 0.5 m/s, 1 m/s, and 5 m/s during 0 ∼ 0.2 s: (a)v_i,0 = 0.5 m/s, proposed, (b) v_i,0 = 1 m/s, proposed, (c) v_i,0 = 5 m/s, proposed, (d) v_i,0 = 0.5 m/s, conventional, (e) v_i,0 = 1 m/s, conventional, and (f) v_i,0 = 5 m/s conventional

Table 2 lists the mean and the root-mean-square (RMS) of the error of the estimation. We define the mean and the RMS of the joint angles and the angular velocities as

\begin{matrix} {\bar{e}}_{‖ q ‖} & = \frac{1}{N} \sum_{k = 1}^{N} ‖ {\hat{q}}_{k | k} - q_{k}^{*} ‖, {\bar{e}}_{‖ \dot{q} ‖} = \frac{1}{N} \sum_{k = 1}^{N} ‖ {\hat{\dot{q}}}_{k | k} - {\dot{q}}_{k}^{*} ‖ \\ e_{‖ q ‖}^{RMS} & = \sqrt{\frac{\sum_{k = 1}^{N} {‖ {\hat{q}}_{k | k} - q_{k}^{*} ‖}^{2}}{N}}, e_{‖ \dot{q} ‖}^{RMS} = \sqrt{\frac{\sum_{k = 1}^{N} {‖ {\hat{\dot{q}}}_{k | k} - {\dot{q}}_{k}^{*} ‖}^{2}}{N}} \end{matrix}

(43)

where $q_{k | k}, {\dot{q}}_{k | k} \in ℝ^{2}$ and $q_{k}^{*}, {\dot{q}}_{k}^{*} \in ℝ^{2}$ are the tracked states and the corresponding ground truths , respectively, and N is the total number of steps of the estimation. The table shows that besides the small error in the mean value, the RMS of the error of the proposed technique is also much smaller than that of the conventional technique. This is because that the proposed technique avoids the pitfall of gravity measurement at the high-speed link motion. Besides, the proposed technique uses the new sensor model of the accelerometer to additionally measure the angular velocity.

Table 2

The error of the proposed motion-tracking technique compared with the conventional technique

		Proposed		Conventional
		Mean	RMS	Mean	RMS
$v = 0.5 m / s$	$e_{‖ q ‖}$	0.0166	0.0172	0.0583	0.0654
$v = 0.5 m / s$	$e_{‖ \dot{q} ‖}$	0.0204	0.0286	0.0427	0.1088
$v = 1.0 m / s$	$e_{‖ q ‖}$	0.0141	0.0151	0.3863	0.4092
$v = 1.0 m / s$	$e_{‖ \dot{q} ‖}$	0.0186	0.0278	0.2945	0.1904
$v = 5.0 m / s$	$e_{‖ q ‖}$	0.0048	0.0078	1.1881	1.3503
$v = 5.0 m / s$	$e_{‖ \dot{q} ‖}$	0.0117	0.0321	10.5931	8.1147

		Proposed		Conventional
		Mean	RMS	Mean	RMS
$v = 0.5 m / s$	$e_{‖ q ‖}$	0.0166	0.0172	0.0583	0.0654
$v = 0.5 m / s$	$e_{‖ \dot{q} ‖}$	0.0204	0.0286	0.0427	0.1088
$v = 1.0 m / s$	$e_{‖ q ‖}$	0.0141	0.0151	0.3863	0.4092
$v = 1.0 m / s$	$e_{‖ \dot{q} ‖}$	0.0186	0.0278	0.2945	0.1904
$v = 5.0 m / s$	$e_{‖ q ‖}$	0.0048	0.0078	1.1881	1.3503
$v = 5.0 m / s$	$e_{‖ \dot{q} ‖}$	0.0117	0.0321	10.5931	8.1147

4.3 Effect of Accelerometer for Joint Angular Velocity Correction.

Since the usage of accelerometers is different between the proposed and conventional techniques, the effect of the accelerometer arrangement was further investigated.

Figure 8 shows the average errors of joint angles (Fig. 8(a)) and joint angular velocities (Fig. 8(b)) subject to different levels of accelerometer measurement noise σ_a in addition to different initial linear velocities $v_{i, 0}$ ⁠. The averaged errors are plotted in Fig. 8. The errors of the proposed technique, shown by the green surfaces Fig. 8, are seen to be consistently low (⁠ $10^{- 2}$ rad and $10^{- 2}$ rad/s) for $v_{i, 0}$ and σ_a ranging from 0.1 m/s to 10 m/s and 0.1 m/s² to 10 m/s², respectively, and are of notable values only when both $v_{i, 0}$ and σ_a are large (10∼100). The error of joint angular velocity stays low when the initial velocities are low and grows proportionally with increased initial velocities until it reaches a peak and decreases significantly at high initial speeds. The decrease in error is because the accelerometers correct the joint angular velocities and the correction is more effective at high angular velocities as indicated by the sensor model for accelerometers. The gray surface in Fig. 8, which did not use accelerometers for the joint angular velocity correction, endorses the effect of the accelerometers. To validate the superiority of the proposed technique, the error of the conventional technique is comparatively shown by the red surface. Since the conventional technique does not use accelerometers for the angular velocity correction, the red surface of ${\bar{e}}_{‖ \dot{q} ‖}$ nearly coincides with the gray surface. On the other hand, the red surface of ${\bar{e}}_{‖ \dot{q} ‖}$ becomes significantly larger than the gray surface, particularly with large $v_{i, 0}$ and small accelerometer measurement noises. This is due to the notable discrepancy of joint angles shown by the red surface in Fig. 8(a) and the subsequent outcome of the angular velocities through the sensor model for the gyroscope.

Fig. 8

View large Download slide

Motion tracking errors of the two-link system with different measurement noises of the accelerometer: (a) error of joint angles and (b) error of angular velocities

4.4 Effect of Magnetometer and Gyroscope in the Proposed Technique.

With the comprehensive analysis of the accelerometer arrangement, the proposed technique was finally investigated in its arrangement of magnetometers and gyroscopes. The errors of motion tracking were derived from different measurement noise levels of magnetometers and gyroscopes. As the noise level should be defined with respect to the size of the measurement, those of the magnetometers and gyroscopes were nondimensionalized as $σ_{m} / ‖ m ‖$ and $σ_{g} l / v_{i, 0}$ where $‖ m ‖$ is the strength of the magnetic field and l is the length of a link. The initial linear velocity $v_{i, 0}$ was set to 5 m/s as the proposed technique has its strength at high motion. The accelerometer measurement noise was also set high to 10 m/s².

Figure 9 shows average errors of joint angles (a) and joint angular velocities (b) subject to different levels of magnetometer and gyroscope measurement noise. Although there are some variations, the tracking error of the proposed technique remains small (less than $10^{- 2}$ rad and 0.2 rad/s²); while ${\bar{e}}_{‖ \dot{q} ‖}$ is randomly small, ${\bar{e}}_{‖ q ‖}$ has been seen to increase slightly with increase in the measurement noise. As the error is expected to be more when there are more noises, the result indicates the robustness of the proposed technique in measurement noise.

Fig. 9

View large Download slide

Motion tracking errors of the two-link system with different measurement noises of the magnetometer and gyroscope: (a)error of joint angles and (b) error of angular velocities

5 Conclusions

This paper has presented a motion tracking technique for high-speed multilink systems using the dynamic measurements fusion. In the proposed technique, accelerometers are combined with gyroscopes to track joint angular velocities whereas magnetometers are used to measure joint angles. The use of EKF as a framework further enhances the accuracy of motion tracking. Numerical studies first show that the proposed technique tracks the high-speed motion of a two-arm link system with nearly no error while the conventional technique estimated the motion fully wrongly. It was further found that the proposed technique tracks the motion regardless of the speed. Analysis of the effect of accelerometers on the joint angular velocity correction shows that the angular velocity error is significantly low and exhibits an increased value only when both the initial linear velocity and the accelerometer measurement noise are high. This accurate angular velocity measurement, together with the accurate magnetometer measurement for the joint angles makes the proposed technique effective. The proposed technique has also been found to be robust against magnetometer and gyroscope measurement noises at high speed, indicating its efficacy for motion tracking of a highly dynamical multilink system.

This paper shows the first set of results for high-speed motion tracking, and many extensive studies and developments are possible. Ongoing work includes and is not limited to (1) experimental validation by a real system, (2) extension to a higher-DOF multilink system, and (3) treatment for singularities. For experimental validation, a 2DOF arm system is being developed to prove the effectiveness of the proposed technique for real-world systems. Since it is useful for human motion tracking, the proposed technique and system are ready for human motion tracking. Because there exist singular conditions where the joint angles and angular velocities cannot be estimated, the proposed technique is extensively formulated to handle the singularities.

Funding Data

US Office of Naval Research (N00014-20-1-2468; Funder ID: 10.13039/100000006).
Honda Motor Co., Ltd.

Global and Body Frames for Motion Tracking of a Multilink System

To describe the measurements of IMU sensors and the motion of a multilink system, a global frame is introduced as the ENU frame, and the body frame for each link is defined such that the z-axis is along the rotation axis and the x-axis is in the plane consisting of the rotation axis and the link and points to the end of the link, as shown in Fig. 10. For simplicity, IMUs are assumed to be installed so their sensor frames coincide with the body frames of the links. The reference position for the zero rotation angle (q_i = 0) is defined as the x-axis of the ith link's body frame in the x–z plane of the $i - 1 t h$ link's body frame.

Fig. 10

View large Download slide

Schematic of the global and body frames for tracking the motion of a multilink system

To transfer the measurement of a vector from a body frame to the global frame or vice versa, the measurement is rotated from the successive links into the global frame via

v^{b, i} = R (0, 0, q_{i}) R_{i - 1}^{i} v^{b, i - 1}

(A1)

where

v^{b, i}

is the measurement vector in the body frame of ith link, and

R (0, 0, q_{i}) \in ℝ^{3 \times 3}

is the rotation matrix describing the rotation of angle q_i and is defined in Eq. , and

R_{i - 1}^{i} \in ℝ^{3 \times 3}

defines the initial rotation matrix from body frame of link i–1 to that of link i. Successive application of the coordinate transformations transfers a vector from a body frame, say of link i, to the global frame as

v^{b, i} = T_{i} (q) v^{G}

(A2)

where the vector

v^{G} \in ℝ^{3}

is in the global frame, and

T_{i} (q) \in ℝ^{3 \times 3}

is recursively defined as

T_{i} (q) = (R (0, 0, q_{i}) R_{i - 1}^{i}) T_{i - 1} (q), for i \geq 1

(A3)

where $T_{0} (q) = R_{G}^{1} \in ℝ^{3 \times 3}$ is the rotation matrix from the global frame to that of the first link when $q_{1} = 0$ ⁠. $T_{i} (q)$ is a rotation matrix since $T_{i} {(q)}^{⊤} T_{i} (q) = I .$

References

1.

Craig

,

J. J.

,

1989

,

Introduction to Robotics: Mechanics and Control

,

Welsey Publishing Company

,

Reading, MA

.

2.

Xu

,

J.-X.

,

Pan

,

Y.-J.

, and

Lee

,

T.-H.

,

2000

, “

A Gain Scheduled Sliding Mode Control Scheme Using Filtering Techniques With Applications to Multilink Robotic Manipulators

,”

ASME J. Dyn. Syst. Meas. Control

,

122

(

4

), pp.

641

–

649

.10.1115/1.1318352

Google Scholar

Crossref

3.

Sentis

,

L.

, and

Khatib

,

O.

,

2005

, “

Synthesis of Whole-Body Behaviors Through Hierarchical Control of Behavioral Primitives

,”

Int. J. Humanoid Rob.

,

2

(

4

), pp.

505

–

518

.10.1142/S0219843605000594

Google Scholar

Crossref

4.

Gawronski

,

W.

,

Ih

,

C.-H.

, and

Wang

,

S.

,

1995

, “

On Dynamics and Control of Multilink Flexible Manipulators

,”

ASME J. Dyn. Syst., Meas., Control

,

117

(

2

), pp.

134

–

142

.10.1115/1.2835173

Google Scholar

Crossref

5.

Jimenez-Cano

,

A. E.

,

Martin

,

J.

,

Heredia

,

G.

,

Ollero

,

A.

, and

Cano

,

R.

,

2013

, “

Control of an Aerial Robot With Multi-Link Arm for Assembly Tasks

,”

IEEE International Conference on Robotics and Automation

, Karlsruhe, Germany, May 6–10, pp.

4916

–

4921

.10.1109/ICRA.2013.6631279

Google Scholar

Crossref

6.

Griffin

,

R. J.

,

Wiedebach

,

G.

,

McCrory

,

S.

,

Bertrand

,

S.

,

Lee

,

I.

, and

Pratt

,

J.

,

2019

, “

Footstep Planning for Autonomous Walking Over Rough Terrain

,” IEEE-RAS 19th International Conference on Humanoid Robots (

Humanoids

), Toronto, ON, Canada, Oct. 15-17, pp.

9

–

16

.10.1109/Humanoids43949.2019.9035046

Google Scholar

Crossref

7.

Steckenrider

,

J. J.

, and

Furukawa

,

T.

,

2020

, “

A Probabilistic Model-Adaptive Approach for Tracking of Motion With Heightened Uncertainty

,”

Int. J. Control, Autom. Syst.

,

18

(

10

), pp.

2687

–

2698

.10.1007/s12555-019-0697-x

Google Scholar

Crossref

8.

Cotton

,

S.

,

Murray

,

A. P.

, and

Fraisse

,

P.

,

2009

, “

Estimation of the Center of Mass: From Humanoid Robots to Human Beings

,”

IEEE/ASME Trans. Mech.

,

14

(

6

), pp.

707

–

712

.10.1109/TMECH.2009.2032687

Google Scholar

Crossref

9.

Mirek

,

E.

,

Kubica

,

J.

,

Szymura

,

J.

,

Pasiut

,

S.

,

Rudzińska

,

M.

, and

Chwała

,

W.

,

2016

, “

Assessment of Gait Therapy Effectiveness in Patients With Parkinson's Disease on the Basis of Three-Dimensional Movement Analysis

,”

Front. Neurol.

,

7

, pp.

102

–

102

.10.3389/fneur.2016.00102

Google Scholar

Crossref

PubMed

10.

Müller

,

B.

,

Ilg

,

W.

,

Giese

,

M. A.

, and

Ludolph

,

N.

,

2017

, “

Validation of Enhanced Kinect Sensor Based Motion Capturing for Gait Assessment

,”

PLoS One

,

12

(

4

), p.

e0175813

.10.1371/journal.pone.0175813

Google Scholar

Crossref

PubMed

11.

Paez-Granados

,

D. F.

,

Kadone

,

H.

,

Hassan

,

M.

,

Chen

,

Y.

, and

Suzuki

,

K.

,

2022

, “

Personal Mobility With Synchronous Trunk–Knee Passive Exoskeleton: Optimizing Human–Robot Energy Transfer

,”

IEEE/ASME Trans. Mech.

,

27

(

5

), pp.

3613

–

3623

.10.1109/TMECH.2021.3135453

Google Scholar

Crossref

12.

Guihard

,

M.

, and

Gorce

,

P.

,

2001

, “

Tracking of High Acceleration Movements With a Pneumatic Impedance Controller

,”

ASME J. Dyn. Syst. Meas. Control

,

123

(

3

), pp.

549

–

551

.10.1115/1.1371773

Google Scholar

Crossref

13.

Pueo

,

B.

, and

Jimenez-Olmedo

,

J. M.

,

2017

, “

Application of Motion Capture Technology for Sport Performance Analysis

,”

Retos

,

32

(

32

), pp.

241

–

247

.10.47197/retos.v0i32.56072

Google Scholar

Crossref

14.

Macias

,

M. E. G.

,

Angulo

,

C. V.

,

Millan

,

E. M. A.

, and

Gonzalez

,

K. R. K.

,

2022

, “

Biomechanics Assessment of Kinematic Parameters of Low-Sprint Start in High-Performance Athletes Using Three Dimensional Motion Capture System

,”

Mexican J. Biomed. Eng.

,

43

(

1

), pp.

52

–

64

.10.17488/RMIB.43.1.5

15.

Furukawa

,

T.

,

Steckenrider

,

J. J.

, and

Dissanayake

,

G.

,

2021

, “

State Estimation of a Partially Observable Multi-Link System With No Joint Encoders Incorporating External Dead-Reckoning

,” IEEE/RSJ International Conference on Intelligent Robots and Systems (

IROS

), Prague, Czech Republic, Sept. 27–Oct. 1, pp.

7342

–

7348

.10.1109/IROS51168.2021.9636132

Google Scholar

Crossref

16.

Zaseck

,

L. W.

,

Bonifas

,

A. C.

,

Miller

,

C. S.

,

Orton

,

N. R.

,

Reed

,

M. P.

,

Demetropoulos

,

C. K.

,

Ott

,

K. A.

,

Dooley

,

C. J.

,

Kuo

,

N. P.

,

Strohsnitter

,

L. M.

,

Andrist

,

J. R.

,

Luongo

,

M. E.

,

Drewry

,

D. G.

,

Merkle

,

A. C.

, and

Rupp

,

J. D.

,

2019

, “

Kinematic and Biomechanical Response of Post-Mortem Human Subjects Under Various Pre-Impact Postures to High-Rate Vertical Loading Conditions

,”

Stapp Car Crash J.

,

63

, pp.

235

–

266

.10.4271/2019-22-0010

Google Scholar

PubMed

17.

Xu

,

T.

,

Sheng

,

X.

,

Zhang

,

T.

,

Liu

,

H.

,

Liang

,

X.

, and

Ding

,

A.

,

2018

, “

Development and Validation of Dummies and Human Models Used in Crash Test

,”

Appl. Bionics Biomech.

,

2018

, pp.

1

–

12

.10.1155/2018/3832850

18.

Zhang

,

W.

,

Furukawa

,

T.

,

Nakata

,

A.

, and

Hashimoto

,

T.

,

2022

, “

Localization of Stereovision for Measuring in-Crash Toeboard Deformation

,”

Sensors

,

22

(

8

), p.

2962

19.

Hartley

,

R.

, and

Zisserman

,

A.

,

2003

,

Multiple View Geometry in Computer Vision

,

Cambridge University Press

,

Cambridge, UK

.

20.

van der Krogt

,

M. M.

,

Delp

,

S. L.

, and

Schwartz

,

M. H.

,

2012

, “

How Robust is Human Gait to Muscle Weakness?

,”

Gait Posture

,

36

(

1

), pp.

113

–

119

.10.1016/j.gaitpost.2012.01.017

Google Scholar

Crossref

PubMed

21.

Minh

,

V. T.

,

Tamre

,

M.

,

Musalimov

,

V.

,

Kovalenko

,

P.

,

Rubinshtein

,

I.

,

Ovchinnikov

,

I.

,

Krcmarik

,

D.

,

Moezzi

,

R.

, and

Hlava

,

J.

,

2020

, “

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

,”

J. Européen Des Systèmes Automatisés (JESA)

,

53

(

5

), pp.

589

–

600

.10.18280/jesa.530501

Google Scholar

Crossref

22.

Begon

,

M.

,

Andersen

,

M. S.

, and

Dumas

,

R.

,

2018

, “

Multibody Kinematics Optimization for the Estimation of Upper and Lower Limb Human Joint Kinematics: A Systematized Methodological Review

,”

ASME J. Biomech. Eng.

,

140

(

3

), p.

030801

.10.1115/1.4038741

Google Scholar

Crossref

23.

Pfister

,

A.

,

West

,

A. M.

,

Bronner

,

S.

, and

Noah

,

J. A.

,

2014

, “

Comparative Abilities of Microsoft Kinect and Vicon 3D Motion Capture for Gait Analysis

,”

J. Med. Eng. Technol.

,

38

(

5

), pp.

274

–

280

.10.3109/03091902.2014.909540

Google Scholar

Crossref

PubMed

24.

Segura

,

M. E.

,

Coronado

,

E.

,

Maya

,

M.

,

Cardenas

,

A.

, and

Piovesan

,

D.

,

2016

, “

Analysis of Recoverable Falls Via Microsoft Kinect: Identification of Third-Order Ankle Dynamics

,”

ASME J. Dyn. Syst. Meas. Control

,

138

(

9

), p. 091006.10.1115/1.4032878

25.

Jung

,

P.-G.

,

Oh

,

S.

,

Lim

,

G.

, and

Kong

,

K.

,

2014

, “

A Mobile Motion Capture System Based on Inertial Sensors and Smart Shoes

,”

ASME J. Dyn. Syst. Meas. Control

,

136

(

1

), p. 011002.10.1115/1.4025207

26.

Taetz

,

B.

,

Bleser

,

G.

, and

Miezal

,

M.

,

2016

, “

Towards Self-Calibrating Inertial Body Motion Capture

,” 19th International Conference on Information Fusion (

FUSION

), Heidelberg, Germany, pp.

1751

–

1759

.10.48550/arXiv.1606.03754

27.

Kok

,

M.

,

Hol

,

J. D.

, and

Schön

,

T. B.

,

2014

, “

An Optimization-Based Approach to Human Body Motion Capture Using Inertial Sensors

,”

IFAC Proc. Vol.

,

47

(

3

), pp.

79

–

85

.10.3182/20140824-6-ZA-1003.02252

Google Scholar

Crossref

28.

Bagalà

,

F.

,

Fuschillo

,

V. L.

,

Chiari

,

L.

, and

Cappello

,

A.

,

2012

, “

Calibrated 2D Angular Kinematics by Single-Axis Accelerometers: From Inverted Pendulum to N-Link Chain

,”

IEEE Sens. J.

,

12

(

3

), pp.

479

–

486

.10.1109/JSEN.2011.2107897

Google Scholar

Crossref

29.

Caroselli

,

A.

,

Bagalà

,

F.

, and

Cappello

,

A.

,

2013

, “

Quasi-Real Time Estimation of Angular Kinematics Using Single-Axis Accelerometers

,”

Sensors

,

13

(

1

), pp.

918

–

937

30.

He

,

P.

, and

Cardou

,

P.

,

2012

, “

Estimating the Angular Velocity From Body-Fixed Accelerometers

,”

ASME J. Dyn. Syst. Meas. Control

,

134

(

6

), p.

061015

.10.1115/1.4006364

Google Scholar

Crossref

31.

Roetenberg

,

D.

,

Luinge

,

H.

, and

Slycke

,

P.

,

2009

,

Xsens MVN: Full 6DOF Human Motion Tracking Using Miniature Inertial Sensors

,

Xsens Motion Technology

, Xsens Motion Technologies BV, Report No. 1.

32.

Bevly

,

D. M.

,

2004

, “

Global Positioning System (GPS): A Low-Cost Velocity Sensor for Correcting Inertial Sensor Errors on Ground Vehicles

,”

ASME J. Dyn. Syst. Meas. Control

,

126

(

2

), pp.

255

–

264

.10.1115/1.1766027

Google Scholar

Crossref

33.

Steckenrider

,

J. J.

,

Crawford

,

B.

, and

Zheng

,

P.

,

2021

, “

GPS and IMU Fusion for Human Gait Estimation

,” IEEE 24th International Conference on Information Fusion (

FUSION

), Sun City, South Africa, Nov. 1–4, pp.

1

–

7

.10.23919/FUSION49465.2021.9627008

Google Scholar

Crossref

34.

Nguyen

,

T.

,

Mann

,

G. K.

,

Vardy

,

A.

, and

Gosine

,

R. G.

,

2019

, “

Developing Computationally Efficient Nonlinear Cubature Kalman Filtering for Visual Inertial Odometry

,”

ASME J. Dyn. Syst. Meas. Control

,

141

(

8

), p. 081012.10.1115/1.4042951

35.

Deilamsalehy

,

H.

,

Havens

,

T. C.

, and

Manela

,

J.

,

2017

, “

Heterogeneous Multisensor Fusion for Mobile Platform Three-Dimensional Pose Estimation

,”

ASME J. Dyn. Syst. Meas. Control

,

139

(

7

), p.

071002

.10.1115/1.4035452

Google Scholar

Crossref

36.

Hellmers

,

H.

,

Norrdine

,

A.

,

Blankenbach

,

J.

, and

Eichhorn

,

A.

,

2013

, “

An IMU/Magnetometer-Based Indoor Positioning System Using Kalman Filtering

,”

International Conference on Indoor Positioning and Indoor Navigation

, Montbeliard, France, Oct. 28–31, pp.

1

–

9

.10.1109/IP IN.2013.6817887

Google Scholar

Crossref

37.

Psiaki

,

M. L.

,

Martel

,

F.

, and

Pal

,

P. K.

,

1990

, “

Three-Axis Attitude Determination Via Kalman Filtering of Magnetometer Data

,”

J. Guid. Control, Dyn.

,

13

(

3

), pp.

506

–

514

.10.2514/3.25364

Google Scholar

Crossref

38.

Poddar

,

S.

,

Kumar

,

V.

, and

Kumar

,

A.

,

2017

, “

A Comprehensive Overview of Inertial Sensor Calibration Techniques

,”

ASME J. Dyn. Syst. Meas. Control

,

139

(

1

), p. 011006.10.1115/1.4034419

39.

Shuster

,

M. D.

, and

Oh

,

S. D.

,

1981

, “

Three-Axis Attitude Determination From Vector Observations

,”

J. Guid. Control

,

4

(

1

), pp.

70

–

77

.10.2514/3.19717

Google Scholar

Crossref

40.

Chung

,

J.

,

Donahoe

,

M.

,

Schmandt

,

C.

,

Kim

,

I.-J.

,

Razavai

,

P.

, and

Wiseman

,

M.

,

2011

, “

Indoor Location Sensing Using Geo-Magnetism

,”

Proceedings of Ninth International Conference on Mobile Systems, Applications, and Services

, Bethesda, MD, pp.

141

–

154

.10.1145/1999995.2000010

Google Scholar

Crossref

41.

Yun

,

X.

,

Bachmann

,

E. R.

, and

McGhee

,

R. B.

,

2008

, “

A Simplified Quaternion-Based Algorithm for Orientation Estimation From Earth Gravity and Magnetic Field Measurements

,”

IEEE Trans. Instrument. Meas.

,

57

(

3

), pp.

638

–

650

.10.1109/TIM.2007.911646

42.

Kang

,

D.

,

Jang

,

C.

, and

Park

,

F. C.

,

2019

, “

Unscented Kalman Filtering for Simultaneous Estimation of Attitude and Gyroscope Bias

,”

IEEE/ASME Trans. Mech.

,

24

(

1

), pp.

350

–

360

.10.1109/TMECH.2019.2891776

Google Scholar

Crossref

43.

Troni

,

G.

, and

Whitcomb

,

L. L.

,

2019

, “

Field Sensor Bias Calibration With Angular-Rate Sensors: Theory and Experimental Evaluation With Application to Magnetometer Calibration

,”

IEEE/ASME Trans. Mech.

,

24

(

4

), pp.

1698

–

1710

.10.1109/TMECH.2019.2920367

Google Scholar

Crossref

44.

Lee

,

T. N.

, and

Canciani

,

A. J.

,

2020

, “

MagSLAM: Aerial Simultaneous Localization and Mapping Using Earth's Magnetic Anomaly Field

,”

Navigation

,

67

(

1

), pp.

95

–

107

.10.1002/navi.352

Google Scholar

Crossref

45.

Yuan

,

Q.

,

Asadi

,

E.

,

Lu

,

Q.

,

Yang

,

G.

, and

Chen

,

I.-M.

,

2019

, “

Uncertainty-Based IMU Orientation Tracking Algorithm for Dynamic Motions

,”

IEEE/ASME Trans. Mech.

,

24

(

2

), pp.

872

–

882

.10.1109/TMECH.2019.2892069

Google Scholar

Crossref

46.

Steckenrider

,

J. J.

,

2022

, “

Adaptive Aerial Localization Using Lissajous Search Patterns

,”

IEEE Trans. Rob.

,

38

(

4

), pp.

2094

–

2113

.10.1109/TRO.2021.3126225

Google Scholar

Crossref

47.

Welch

,

G.

, and

Bishop

,

G.

,

1995

,

An Introduction to the Kalman Filter

, University of North Carolina,

Chapel Hill, NC

.

48.

Zhao

,

H.

, and

Wang

,

Z.

,

2012

, “

Motion Measurement Using Inertial Sensors, Ultrasonic Sensors, and Magnetometers With Extended Kalman Filter for Data Fusion

,”

IEEE Sens. J.

,

12

(

5

), pp.

943

–

953

.10.1109/JSEN.2011.2166066

Google Scholar

Crossref

49.

Schiefer

,

C.

,

Ellegast

,

R. P.

,

Hermanns

,

I.

,

Kraus

,

T.

,

Ochsmann

,

E.

,

Larue

,

C.

, and

Plamondon

,

A.

,

2014

, “

Optimization of Inertial Sensor-Based Motion Capturing for Magnetically Distorted Field Applications

,”

ASME J. Biomech. Eng.

,

136

(

12

), p.

121008

.10.1115/1.4028822

Google Scholar

Crossref

50.

Steckenrider

,

J. J.

, and

Furukawa

,

T.

,

2020

, “

Multi-Dimensional Belief Fusion of multi-Gaussian Structures

,”

Inf. Fusion

,

57

, pp.

71

–

88

.10.1016/j.inffus.2019.12.006

Google Scholar

Crossref

51.

Skog

,

I.

,

Nilsson

,

J.-O.

,

Händel

,

P.

, and

Nehorai

,

A.

,

2016

, “

Inertial Sensor Arrays, Maximum Likelihood, and Cramér–Rao Bound

,”

IEEE Trans. Signal Process.

,

64

(

16

), pp.

4218

–

4227

.10.1109/TSP.2016.2560136

Google Scholar

Crossref

52.

Vikas

,

V.

, and

Crane

,

C. D.

, III ,

2016

, “

Joint Angle Measurement Using Strategically Placed Accelerometers and Gyroscope

,”

ASME J. Mech. Rob.

,

8

(

2

), p.

021003

.10.1115/1.4031299

Google Scholar

Crossref

53.

Weygers

,

I.

,

Kok

,

M.

,

De Vroey

,

H.

,

Verbeerst

,

T.

,

Versteyhe

,

M.

,

Hallez

,

H.

, and

Claeys

,

K.

,

2020

, “

Drift-Free Inertial Sensor-Based Joint Kinematics for Long-Term Arbitrary Movements

,”

IEEE Sens. J.

,

20

(

14

), pp.

7969

–

7979

.10.1109/JSEN.2020.2982459

Google Scholar

Crossref

54.

Woods

,

C.

, and

Vikas

,

V.

,

2022

, “

Joint Angle Estimation Using Accelerometer Arrays and Model-Based Filtering

,”

IEEE Sens. J.

,

22

(

20

), pp.

19786

–

19796

.10.1109/JSEN.2022.3200251

Google Scholar

Crossref

55.

Nilsson

,

J.-O.

, and

Skog

,

I.

,

2016

, “

Inertial Sensor Arrays-A Literature Review

,” European Navigation Conference (

ENC

), Helsinki, Finland, May 30–June 2, pp.

1

–

10

.10.1109/EURONAV.2016.7530551

Google Scholar

Crossref

56.

Cheng

,

P.

, and

Oelmann

,

B.

,

2010

, “

Joint-Angle Measurement Using Accelerometers and Gyroscopes-a Survey

,”

IEEE Trans. Instrum. Meas.

,

59

(

2

), pp.

404

–

414

.10.1109/TIM.2009.2024367

Google Scholar

Crossref

Motion Tracking of a High-Speed Multilink System Using Dynamic Measurements Fusion

Abstract

1 Introduction

1.1 Background.

1.2 Related Work.

1.3 Objectives and Outline.

2 Motion Tracking of a Dynamic Multilink System

2.1 Problem Formulation.

2.2 Motion and Sensor Models.

2.3 Conventional Quasi-Steady Motion Tracking.

3 Inertial Measurement Unit-Based Motion Tracking Using Kalman Filter

3.1 Overview.

3.2 Probabilistic Motion and Sensor Models

3.2.1 Motion Model.

3.2.2 Sensor Model for Magnetometers.

3.2.3 Sensor Model for Gyroscopes.

3.2.4 Sensor Model for Accelerometers.

3.3 Extended Kalman Filter Based State Estimation.

4 Numerical Validation

4.1 Two-Arm Link System for Motion Tracking.

4.1.1 Motion Model for the Planar Two-Arm Link System.

4.1.2 Sensor Models for the Planar Two-Arm Link System.

4.1.3 Parameters of the Two-Arm Link System.

4.2 Effect of Initial Linear Velocity.

4.2.1 Results With High Initial Linear Velocity.

4.2.2 Results With Different Initial Linear Velocities.

4.3 Effect of Accelerometer for Joint Angular Velocity Correction.

4.4 Effect of Magnetometer and Gyroscope in the Proposed Technique.

5 Conclusions

Funding Data

Global and Body Frames for Motion Tracking of a Multilink System

References

Get Email Alerts

Cited By

ASME Journals

ASME Conference Proceedings

ASME eBooks

Resources

Opportunities

Motion Tracking of a High-Speed Multilink System Using Dynamic Measurements Fusion

Abstract

1 Introduction

1.1 Background.

1.2 Related Work.

1.3 Objectives and Outline.

2 Motion Tracking of a Dynamic Multilink System

2.1 Problem Formulation.

2.2 Motion and Sensor Models.

2.3 Conventional Quasi-Steady Motion Tracking.

3 Inertial Measurement Unit-Based Motion Tracking Using Kalman Filter

3.1 Overview.

3.2 Probabilistic Motion and Sensor Models

3.2.1 Motion Model.

3.2.2 Sensor Model for Magnetometers.

3.2.3 Sensor Model for Gyroscopes.

3.2.4 Sensor Model for Accelerometers.

3.3 Extended Kalman Filter Based State Estimation.

4 Numerical Validation

4.1 Two-Arm Link System for Motion Tracking.

4.1.1 Motion Model for the Planar Two-Arm Link System.

4.1.2 Sensor Models for the Planar Two-Arm Link System.

4.1.3 Parameters of the Two-Arm Link System.

4.2 Effect of Initial Linear Velocity.

4.2.1 Results With High Initial Linear Velocity.

4.2.2 Results With Different Initial Linear Velocities.

4.3 Effect of Accelerometer for Joint Angular Velocity Correction.

4.4 Effect of Magnetometer and Gyroscope in the Proposed Technique.

5 Conclusions

Funding Data

Global and Body Frames for Motion Tracking of a Multilink System

References

Get Email Alerts

Cited By

Related Articles

Related Proceedings Papers

Related Chapters

ASME Journals

ASME Conference Proceedings

ASME eBooks

Resources

Opportunities

This Feature Is Available To Subscribers Only