Estimation of Cartesian Space Robot Trajectories Using Unit Quaternion Space

Abstract

The ability to estimate Cartesian space trajectories that include orientation is of great importance for many practical applications. While it is becoming easier to acquire trajectory data by computer vision methods, data measured by general-purpose vision or depth sensors are often rather noisy. Appropriate smoothing methods are thus needed in order to reconstruct smooth Cartesian space trajectories given noisy measurements. In this paper, we propose an optimality criterion for the problem of the smooth estimation of Cartesian space trajectories that include the end-effector orientation. Based on this criterion, we develop an optimization method for trajectory estimation which takes into account the special properties of the orientation space, which we represent by unit quaternions. The efficiency of the developed approach is discussed and experimental results are presented.

1. Introduction

The estimation of human hand motion is an important problem for many applications. Our interest stems from programming by demonstration [1] (also called ‘imitation learning’) in robotics. Figure 1 shows an example of a programming by demonstration system where a robot is guided to perform a classic peg-in-hole task [2]. The goal of imitation learning is to provide robotic systems with the ability to relate perceived human actions to their own embodiment in order to learn - and later perform - the demonstrated actions [3]. In imitation learning, knowledge about the demonstrated human hand motion is often essential for the understanding of the demonstrated behaviour.

Figure 1.

Demonstration of a peg-in-hole operation. The human demonstrator observes the performance of the robot and adapts his hand motion so that the robot successfully executes the task. The demonstrator's hand motion is measured by Kinect.

Ignoring the fingers, we normally encode hand motion as a rigid body motion. It is well known that rigid body motion consists of a translational and a rotational part [4]. The reconstruction of a pure translational motion can be accomplished by standard optimization methods, because the set of all translations forms a three-dimensional (3D) vector space. On the other hand, the set of all rotations in the 3D Cartesian space, which we denote by SO(3), forms only a group and not a vector space. The special Euclidean group SE(3) is defined as a semi-direct product of ℝ³ and SO(3). It represents the Euclidean transformation of rotation followed by translation. Unfortunately, there exists no representational scheme for rotations that would be simultaneously non-redundant, continuous and free of singularities [4]. This causes problems when solving optimization problems for SO(3) because representations free of singularities (e.g., rotation matrices and quaternions) contain more than the minimal number of parameters. The resulting parameters are therefore not independent of each other.

One possibility for measuring human arm and hand motion is to use RGB-D sensors, e.g., Kinect. The Kinect sensor uses the principle of structured light and captures depth and colour images simultaneously at a frame rate of about 30 Hz [5]. Together with the appropriate software, Kinect enables the tracking of several joints on the human body, including hand position and orientation [6, 7]. No special markers are needed. This results in a sequence of noisy measurements of the form:

$\left(t_k\mathrm{,}{p}_k^{*}\mathrm{,}{q}_k^{*}\mathrm{,}{\Sigma }_k\right)\mathrm{,}\hspace{0.17em}k=\mathrm{1,}\dots \mathrm{,}n\mathrm{,}$

(1)

where t_k are the measurement times, p^*_k ∈ ℝ³ the measured positions, q^*_k ∈ S³ the measured orientations represented by unit quaternions, and σ _k ∈ ℝ^6times6 the covariance matrices describing the uncertainties in the measured position and orientations (see Section 3). Alternatively, such data can be obtained from marker-based systems, e.g., Optotrak. While the unit quaternion space S³ does not uniquely represent SO(3) (i.e., for every orientation there are two equivalent unit quaternions), this duality does not represent a practical problem because the two unit quaternions representing the same orientation are well separated and there are no singularities in this representation. Our goal is to find a sequence of positions and orientations (p_k, q_k) that approximate measurements (1) well and at the same time encode a smooth rigid body motion. Note that, in this paper, we consider the problem of batch (offline) processing, i.e., all of the data are available at the time of estimation. This is different from online filtering, which can only use past measurements to smooth the incoming data. Online filters on SE(3) and SO(3) were considered, e.g., in [8–12]. Methods that take into account the properties of the special Euclidean group have also been considered in the context of pose estimation in computer vision [13–16] and in control [17].

The problem of smoothing on general Riemannian manifolds has been considered in general mathematical texts [18–20]. The approach proposed in this paper is a special case of smoothing on Riemannian manifolds. Unlike these more general papers, in this paper we focus on a specific problem of smoothing in ℝ³ × S³ and address many particular issues relevant to this problem, e.g., the definition of tangential space and metrics using an exponential and logarithmic map and how to use them within the framework of the Gauss-Newton and Levenberg-Marquardt methods on R³ × S³.

As mentioned above, SO(3) cannot be globally embedded in the 3D Euclidean space. This means that if the rotation group is represented by three real parameters (e.g., as in the case of Euler angles), the Euclidean metric topology in ℝ³ does not induce a global topology or metric structure in SO(3). This is the main motivation for selecting unit quaternions to represent rotations - the spherical metric of S³ corresponds to the angular metric of SO(3) [21]. We would, however, obtain similar results if we used a different representation free of singularities, e.g., rotation matrices. In the following, we first formulate the problem of estimating motion trajectories on ℝ³ × S³ and then propose an optimization method that can be applied to solve it. The main feature of our approach is that we exploit the properties of the exponential map to calculate new estimates at each step of the iterative optimization process, which enables us to formulate the optimization process directly on ℝ³ × S³.

2. Preliminaries

Formally, a quaternion q = (w, u₁, u₂, u₃) is a vector quantity, where w is the scalar component of q and u = [u₁,u₂, u₃]T is the vector component. The quaternion multiplication is defined by:

$q\ast q\prime =\left(w{w}^{\prime }-u^{T}u\prime \mathrm{,}wu\prime +w^{\prime }u+u\times u\prime \right)\mathrm{.}$

(2)

Quaternions form a non-commutative group with respect to the above multiplication. The magnitude of a quaternion is defined as:

$\left|q\right|=\sqrt{q\ast \bar{q}}=\sqrt{w^2+u^{T}u}\mathrm{,}$

(3)

$\bar{q}=\left(w\mathrm{,}-u\right)\mathrm{,}$

(4)

where ̄ is the conjugate of q. Given a rotation by V about a unit axis vector n, we define the associated unit quaternion as:

$q(\vartheta \mathrm{,}n)=\left(\cos \left(\frac{\vartheta }{2}\right)\mathrm{,}\hspace{0.17em}\hspace{0.17em}\sin \left(\frac{\vartheta }{2}\right)n\right)\mathrm{.}$

(5)

There is a 2-1 mapping between unit quaternions and the rotation group [21]. Each rotation from SO(3) can thus be represented by two quaternions belonging to the unit sphere S³ ℝ⁴. However, the two unit quaternions representing the same rotation are well separated because they lie on different sides of the unit sphere. It can be shown that a vector v′ ∈ ℝ³, rotated from a vector v ∈ ℝ³ by a rotation represented by a unit quaternion q, can be calculated by a simple quaternion multiplication:

$v\prime =q\ast v\ast \bar{q}\mathrm{.}$

(6)

In this multiplication, the 3D vector v is treated as a quaternion with a zero scalar component. It is easy to see that the resulting quaternion v′ has a zero scalar component as well.

In the following, we will need the exponential map exp : ℝ³ ŕ S³, which is given by:

$\exp (r)=\{\begin{array}{l}\left(\cos (\parallel r\parallel )\mathrm{,}\sin (\parallel r\parallel )\frac{r}{\parallel r\parallel }\right)\mathrm{,}\hspace{0.17em}\parallel r\parallel \ne 0\hfill \\ \left(\mathrm{1,0,0,0}\right)\mathrm{,}\hspace{0.17em}r=0\hfill \end{array}\mathrm{.}$

(7)

We denote by T _q (S³) C ℝ⁴ the tangent space of S³ at unit quaternion q. It can be shown that the exponential map transforms a tangent vector r ∈ T₁ (S³) = ℝ³ into q ∈ S³, where q is a quaternion at distance ||r|| from the identity quaternion 1 (a unit quaternion with a zero vector component) along the geodesic curve, which is given by q(t) = exp(t log(q)), starting from quaternion 1 in the direction of r. The geodesic curve represents the shortest path from 1 to q on S³. The logarithmic map log : S³ → ℝ³ is defined as:

$\log (q)=\log \left(w\mathrm{,}u\right)=\{\begin{array}{l}\arccos (w)\frac{u}{\Vert u\Vert }\mathrm{,}\hspace{0.17em}u\ne 0\hfill \\ \left(\mathrm{0,0,0}\right)\mathrm{,}\hspace{0.17em}q=\left(\mathrm{1,0,0,0}\right)\hfill \end{array}\mathrm{.}$

(8)

If we limit the domain of the exponential map to ||r|| < π and the domain of the logarithmic map to S³/(– 1,0,0,0), then both mappings become one-to-one, continuously differentiable and inverse to each other. It can be shown that the expression:

$\mathrm{d}(q_1\mathrm{,}{q}_2)=\{\begin{array}{l}2\Vert \log \left(q_1\ast \overline{q_2}\right)\Vert \mathrm{,}\hspace{0.17em}{q}_1\ast \overline{q_2}\ne \left(-\mathrm{1,0,0,0}\right)\hfill \\ 2\pi \mathrm{,}\hspace{0.17em}\text{otherwise}\hfill \end{array}$

(9)

is a metric on S³. This metric is usually called the ‘angular metric’ of S³.

3. Formulation of the Problem

The estimation of noisy vector-valued measurements with non-diagonal covariance matrices has been considered by [22, 23], who developed an iterative algorithm for the nonlinear estimation of a smooth vector-valued function based on a non-parametric optimality criterion. In our case, the problem is more complicated because the space of all orientations is not a vector space. While the difference between the measured and the true position can be modelled as additive, namely:

$p_k=p_{k^{\prime }}+e_k^p\mathrm{,}$

(10)

this is not the case for the difference between the measured orientation q_k and the true orientation q_k‘. This error can be modelled as [8]:

$q_k=\exp \left(e_k^q\right)\ast {q^{\prime }}_k\mathrm{,}$

(11)

where e^q_k ∈ ℝ³ is an error vector.

We assume that the error in the position and orientation is Gaussian with zero mean and a covariance matrixσ _k . The changing of the current position P¹_k and orientation q¹_k by a deterministic displacement (ΔP_k, Δq_k) results in:

$\left({p^{″}}_k\mathrm{,}{q^{″}}_k\right)=\left(p_{k^{\prime }}+\mathrm{\Delta }{p}_k\mathrm{,}\mathrm{\Delta }{q}_k\ast {q^{\prime }}_k\right).$

(12)

We note that the position error vector remains unchanged under transformation (12). To find the relationship between the old and the transformed rotation error vector, we make the following observation:

$\begin{array}{l}{q}_k^{\text{new}}=\mathrm{\Delta }{q}_k\ast q_k=\mathrm{\Delta }{q}_k\ast \exp (e_k^q)\ast {q^{\prime }}_k\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}=\mathrm{\Delta }{q}_k\ast \exp (e_k^q)\ast \overline{\mathrm{\Delta }{q}_k}\ast \mathrm{\Delta }{q}_k\ast {q^{\prime }}_k\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}=\exp \left(\mathrm{\Delta }{q}_k\ast e_k^q\ast \overline{\mathrm{\Delta }{q}_k}\right)\ast {q^{\prime }}_k\mathrm{.}\end{array}$

(13)

Hence, there exists the following relationship between the two error vectors:

$e_k^{q\hspace{0.17em}\text{new}}=\mathrm{\Delta }{q}_k\ast e_k^q\ast \overline{\mathrm{\Delta }{q}_k}=R\left(\mathrm{\Delta }{q}_k\right)e_k^q\mathrm{,}$

(14)

where the rotation matrix R is given by the formula:

$\begin{array}{c}R(q)=R(w\mathrm{,}u)=[\begin{array}{c}{w}^2+u_1^2-u_2^2-u_3^2\\ 2\left(u_1{u}_2+w{u}_3\right)\\ 2\left(u_1{u}_3-w{u}_2\right)\end{array}\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\begin{array}{cc}2\left(u_1{u}_2-w{u}_3\right)& 2\left(u_1{u}_3+w{u}_2\right)\\ w^2-u_1^2+u_2^2-u_3^2& 2\left(u_2{u}_3-w{u}_1\right)\\ 2\left(u_2{u}_3+w{u}_1\right)& w^2-u_1^2-u_2^2+u_3^2\end{array}].\end{array}$

The new error vector is obtained by rotating the old error vector into a new orientation. Writing the covariance matrix σ _k as:

${\Sigma }_k=\left[\begin{array}{cc}{\Sigma }_k^p& {\Sigma }_k^{pq}\\ {\Sigma }_k^{pq}& {\Sigma }_k^q\end{array}\right]\mathrm{,}$

(15)

the covariance matrix describing the uncertainties in the new pose can be calculated by:

${\Sigma }_k^{\text{new}}=\left[\begin{array}{cc}{\Sigma }_k^p& {\Sigma }_k^{pq}R{(\mathrm{\Delta }{q}_k)}^T\\ R(\mathrm{\Delta }{q}_k)({\Sigma }_k^{pq}{)}^T& R(\mathrm{\Delta }{q}_k){\Sigma }_k^{q}R{(\mathrm{\Delta }{q}_k)}^T\end{array}\right]\mathrm{.}$

(16)

The aim of reconstruction is to find a trajectory that not only approximates the measurements well but is also smooth. If the measurements were simply interpolated, the reconstructed trajectory would not be smooth enough. Hence, we must search for a compromise between smoothness and goodness of fit. Writing P = [p^T₁,…,p^T_n and q = [q^T₁,…, q^T_n] ^T , the goodness of fit can be evaluated by:

$\begin{array}{c}{\mathrm{F}}_0\left(p\mathrm{,}q\right)=\\ {\displaystyle {\sum }_{k=1}^n}{\left[\begin{array}{c}{p}_k-p_k^{*}\\ \log \left(q_k*\overline{q_k^{*}}\right)\end{array}\right]}^T{\Sigma }_k^{-1}\left[\begin{array}{c}{p}_k-p_k^{*}\\ \log \left(q_k*\overline{q_k^{*}}\right)\end{array}\right]\mathrm{,}\end{array}$

(17)

where (p^*_k, q^*_k), k = 1,…,n, are the measured poses and σ _k are the positive definite covariance matrices with respect to the zero mean error vector [ ^p _k ^T , e^q_k ^T ] ^T defined in (10) and (11). It is easy to show that each of the summed terms in (17) is a metric on ℝ³ × S³ if ∈k are positive definite matrices.

A good measure of the smoothness of trajectories is given by the amount of linear and angular acceleration. The linear acceleration a_k, k = 2,…, n – 1, can be estimated by:

$\begin{array}{l}{v}_k(p)=\frac{p_{k+1}-p_k}{\mathrm{\Delta }{t}_k}\mathrm{,}\\ a_k(p)=\frac{v_k(p)-v_{k-1}(p)}{\mathrm{\Delta }{t}_{k-1}}\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}=\frac{p_{k+1}-p_k}{\mathrm{\Delta }{t}_{k-1}\mathrm{\Delta }{t}_k}-\frac{p_k-p_{k-1}}{\mathrm{\Delta }{t}_{k-1}\mathrm{\Delta }{t}_{k-1}}\mathrm{,}\end{array}$

where Δt_k = t_k+1 – t_k. Similarly the angular acceleration α_k, k = 2,…, n – 1, can be estimated by:

$\begin{array}{l}{\omega }_k(q)=\frac{2}{\mathrm{\Delta }{t}_k}\log \left(q_{k+1}\ast \overline{q_k}\right)\\ {\alpha }_k(q)=\frac{{\omega }_k(q)-{\omega }_{k-1}(q)}{\mathrm{\Delta }{t}_{k-1}}\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}=\frac{2\log \left(q_{k+1}\ast \overline{q_k}\right)}{\mathrm{\Delta }{t}_{k-1}\mathrm{\Delta }{t}_k}-\frac{2\log \left(q_k\ast \overline{q_{k-1}}\right)}{\mathrm{\Delta }{t}_{k-1}\mathrm{\Delta }{t}_{k-1}}\end{array}$

Writing:

${\mathrm{g}}_1(p)={\displaystyle \sum _{k=2}^{n-1}}{\Vert a_k(p)\Vert }^2\mathrm{,}$

(18)

${\mathrm{g}}_2(q)={\displaystyle \sum _{k=2}^{n-1}}{\Vert {\alpha }_k(q)\Vert }^2\mathrm{,}$

(19)

we can formulate the following criterion, which should be minimized by a rigid body motion that exhibits good balance between smoothness and goodness of fit:

$\mathrm{F}\left(p\mathrm{,}q\right)=\frac{1}{2}\left({\mathrm{F}}_0\left(p\mathrm{,}q\right)+{\lambda }_1{\mathrm{g}}_1\left(p\right)+{\lambda }_2{\mathrm{g}}_2\left(q\right)\right)\mathrm{.}$

(20)

The parameters λ₁ and λ₂ govern the trade-off between the two criteria.

Since the criterion function (20) is nonlinear, we must apply nonlinear optimization techniques to find the optimal sequence of poses (P_k, q_k). The minimization of (20) over p_k, q_k would be a classic nonlinear least squares optimization problem if we could treat unit quaternions q_k as elements of ℝ⁴ and not of S³. Since this is not the case, the classic approach would be to add the constraints q_k = 1 to the optimization criterion. However, such constraints make the optimization problem significantly harder. In the following, we propose a technique that can be used to optimize the criterion (20) without specifying additional constraints.

4. Optimization in ℝ³ × S³ × … × ℝ³ × S³

The tangent space T _q (S³) C ℝ⁴ is defined as a space that contains the directions of all paths on S³ passing through the quaternion q. As mentioned in Section 2, the exponential map exp transforms a tangent vector r ∈ T₁(S³) = ℝ³ into a point $\tilde{q}\in {\mathrm{S}}^3$ that lies on the geodesic curve corresponding to the tangent vector r. It turns out that for any q ∈ S³, the exponential map exp _q that transforms each tangent vector x ∈ T _q (S³) into a point $\tilde{q}$ that lies on S³ along the geodesic curve starting at q in the direction of x at the distance ||x|| is given by:

${{\displaystyle \exp }}_q(x)=\exp \left(x\ast \bar{q}\right)\ast q\mathrm{.}$

(21)

It can be shown [24] that x ^* ̄ ∈ T₁ (S₃) or - in other words - x ^* ̄ is a quaternion with a zero scalar component, for anyx ∈ T _q (S³), q ∈ S³. Thus, the above mapping is well-defined for all x ∈ T _q (S³). As the mapping x ^* ̄ is an isomorphism from T _q (S³) to ℝ³, all the unit quaternions in the neighbourhood of q can be represented by exp(r) ^* q, r ∈ ℝ³.

Taking (pⁱ_k, qⁱ_k) ∈ ℝ³ × S³, k = 1,…n, to be the i-th estimate for the optimal sequence of positions and orientations, it is appropriate to calculate the next sequence of positions and orientations as follows:

$p_k^{i+1}=p_k^i+d_k^i\mathrm{,}$

(22)

$q_k^{i+1}=\exp (r_k^i)\ast q_k^i\mathrm{.}$

(23)

where dⁱ = [dⁱ₁ T,…,dⁱ_n] T and rⁱ = [rⁱ₁ T,…,rⁱ_n T]T should be obtained by approximating the minimum of the objective function:

${\mathrm{F}}_i\left(d\mathrm{,}r\right)=\frac{1}{2}\mathrm{F}\left(p^i+d\mathrm{,}\exp (r)\ast q^i\right)\mathrm{.}$

(24)

where:

$\exp (r)\ast q^i=\left[\begin{array}{c}\exp (r_1)\ast q_1^i\\ ⋮\\ \exp (r_n)\ast q_n^i\end{array}\right]\mathrm{.}$

The above criterion can be rewritten as:

${\mathrm{F}}_i\left(d\mathrm{,}r\right)=\frac{1}{2}{f}_i{\left(d\mathrm{,}r\right)}^T{f}_i\left(d\mathrm{,}r\right)\mathrm{,}$

(25)

where:

$f_i(d\mathrm{,}r)=\left[\begin{array}{c}{\Sigma }_1^{-1/2}\left[\begin{array}{c}{d}_1+p_1^i-p_1^{*}\\ \log \left(\exp (r_1)\ast q_1^i\ast \overline{q_1^{*}}\right)\end{array}\right]\\ \sqrt{{\lambda }_1}\hspace{0.17em}{a}_2\left(d+p^i\right)\\ \sqrt{{\lambda }_2}\hspace{0.17em}{\alpha }_2\left(\exp (r)*q^i\right)\\ {\Sigma }_2^{-1/2}\left[\begin{array}{c}{d}_2+p_2^i-p_2^{*}\\ \log \left(\exp (r_2)\ast q_2^i\ast \overline{q_2^{*}}\right)\end{array}\right]\\ ⋮\\ \sqrt{{\lambda }_1}\hspace{0.17em}{a}_{n-1}\left(d+p^i\right)\\ \sqrt{{\lambda }_2}\hspace{0.17em}{\alpha }_{n-1}\left(\exp (r)\ast q^i\right)\\ {\Sigma }_{n-1}^{-1/2}\left[\begin{array}{c}{d}_{n-1}+p_{n-1}^i-p_{n-1}^{*}\\ \log \left(\exp (r_{n-1})\ast q_{n-1}^i\ast \overline{q_{n-1}^{*}}\right)\end{array}\right]\\ {\Sigma }_n^{-1/2}\left[\begin{array}{c}{d}_n+p_n^i-p_n^{*}\\ \log \left(\exp (r_n)\ast q_n^i\ast \overline{q_n^{*}}\right)\end{array}\right]\end{array}\right]$

is a vector function from ℝ ⁶ⁿ to ℝ¹²ⁿ–12. The gradient and the Hessian of Fi are given by:

$\nabla {\mathrm{F}}_i\left(d\mathrm{,}r\right)=J_i{\left(d\mathrm{,}r\right)}^T{f}_i\left(d\mathrm{,}r\right)\mathrm{,}$

(26)

$\begin{array}{l}{\nabla }^2{\mathrm{F}}_i\left(d\mathrm{,}r\right)=J_i{\left(d\mathrm{,}r\right)}^T{J}_i\left(d\mathrm{,}r\right)+\\ \hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}\hspace{0.17em}{\displaystyle \sum _{k=1}^n}{f}_i^k\left(d\mathrm{,}r\right){\nabla }^2{f}_i^k\left(d\mathrm{,}r\right)\mathrm{,}\end{array}$

(27)

where J_i(d, r) is the Jacobian of f at (d, r) and ^k _i are the component functions of f_i.

The Taylor series expansion for the vector function ▿F _i around ▿F _i (0,0) is given by:

$\nabla {\mathrm{F}}_i\left(d\mathrm{,}r\right)\approx \nabla {\mathrm{F}}_i\left(\mathrm{0,0}\right)+{\nabla }^2{\mathrm{F}}_i\left(\mathrm{0,0}\right){\left[d^T\mathrm{,}{r}^T\right]}^T$

(28)

If we assume that the value of F _i is small for all q belonging to the neighbourhood of the solution (i.e., f^k_i(d, r) ≈ 0 for all k), we obtain from Eq. (27) the following approximation for the Hessian in the neighbourhood of the solution:

${\nabla }^2{\mathrm{F}}_i\left(\mathrm{0,0}\right)\approx J_i^T{J}_i\mathrm{,}$

(29)

where J_i∈ ℝ(^12n–12)x(⁶ⁿ) is the Jacobian of f_i at d = r = 0. Using the Taylor series expansion (28) and the fact that ▿F _i (d, r) = 0 at the minimum of F _i , we can calculate the appropriate modification (dⁱ, rⁱ) as follows:

$\left[\begin{array}{c}{d}^i\\ r^i\end{array}\right]=-{\left(J_i^T{J}_i\right)}^{-1}{J}_i^T{f}_i\left(\mathrm{0,0}\right)\mathrm{,}$

(30)

The next sequence of poses can then be calculated using Eq. (22) and (23). The iteration is stopped when:

$\Vert \nabla {\mathrm{F}}_i\left(\mathrm{0,0}\right)\Vert =\Vert J_i^T{f}_i\left(\mathrm{0,0}\right)\Vert <\epsilon \mathrm{.}$

(31)

Note that J^T_i J_i ∈ ℝ^6nx6n is a symmetric band matrix with bandwidth 35 (17+1+17). Thus, the number of arithmetic operations needed to solve the resulting linear system of equations is linear with respect to the number of measurements. Note that this is by far the most computationally expensive part of our system. Since we have a good initial approximation for our optimization problem (the measurements themselves are used to initialize optimization), there are not too many iterations that need to be performed in order to find the optimal solution (see also Table 1). Hence, our approach can easily deal with thousands of measurements, which is the order of magnitude for the number of data points we normally acquire when measuring demonstrated trajectories. Demonstrated trajectories are usually acquired at 30 Hz, and up to 120 Hz with marker-based systems, and take from a few seconds to tens of seconds. Note also that batch optimization is by definition offline; hence, real-time operation is not an issue.

Table 1.

Convergence of the Gauss-Newton method for different smoothing parameters.

${\lambda }_1={\lambda }_2=1$		${\lambda }_1={\lambda }_2{=10}^5$
$\mathrm{F}\left(p^i\mathrm{,}{q}^i\right)$	$\Vert \nabla {\mathrm{F}}_i(\mathrm{0,0})\Vert$	$\mathrm{F}\left(p^i\mathrm{,}{q}^i\right)$	$\Vert \nabla {\mathrm{F}}_i(\mathrm{0,0})\Vert$
.8854645e+04	7.6323626e+03	1.8799571e+09	7.6100687e+08
.4852889e+03	2.1105371e+01	5.2411567e+05	1.4645490e+07
.4852889e+03	1.2726109e-04	3.4902288e+05	7.6035127e+04
.4852889e+03	2.2911019e-06	3.4901791e+05	8.8733884e+01
.4852889e+03	1.1957504e-07	3.4901790e+05	2.3231791e+00
.4852889e+03	1.8406582e-09	3.4901790e+05	2.7915866e-01
.4852889e+03	3.6851655e-10	3.4901790e+05	1.6016155e-02
		3.4901790e+05	2.1152017e-03
		3.4901790e+05	1.2825353e-04
		3.4901790e+05	1.7447888e-05
		3.4901790e+05	4.6314701e-06

The counterpart of the derived iteration in real spaces is the Gauss-Newton iteration. Actually, we have shown above how to carry out the Gauss-Newton iteration on ℝ³ × S³ × … × ℝ³ × S³. However, the classic Gauss-Newton method can encounter problems when the second-order term in Eq. (27) is significant. While for small smoothing parameters λ₁, λ₂ the criterion functions f^k_i are also small in the neighbourhood of the minimum, this is not the case for larger smoothing parameters. Therefore, we can expect slower convergence when the smoothing parameter becomes large.

We can overcome this problem by applying the Levenberg-Marquardt method, in which the search direction is calculated as follows:

$\left[\begin{array}{c}{d}^i\\ r^i\end{array}\right]=-{\left(J_i^T{J}_i+{\mu }_{i}I\right)}^{-1}{J}_i^T{f}_i\left(\mathrm{0,0}\right)\mathrm{.}$

(32)

Note that the system matrix (J^T_iJ_i + μ_iI) is positive definite for every μ_i > 0. When μ_i is equal to zero, the search direction becomes identical to that of the Gauss-Newton method. As μ_i tends towards infinity, (dⁱ, rⁱ) tends towards a vector of zeros and a steepest descent direction. This implies that, for some sufficiently large μ_i, the value F _i (dⁱ, rⁱ) is smaller than F _i (0,0) = F(pⁱ, qⁱ). Thus, the Levenberg-Marquardt method uses a search direction that is a cross between the Gauss-Newton direction and the steepest descent.

It remains to show how to determine the smoothing parameters λ₁ and λ₂. It is important to properly select the degree of smoothing to find a proper balance between smoothness and goodness of fit. Often, methods like cross-validation are used, but in general cross-validation is computationally expensive because it requires that the data be partitioned into two sets: one used to learn or train a model, and the other used to validate the model. These sets need to be changed many times so that all the data can be validated. For this reason, we prefer to use an approach proposed in [25] for the case when the amount of noise associated with the data is known. With this method, we can determine the optimal values for λ₁ and λ₂ by solving the following nonlinear systems of equations:

${\displaystyle \sum _{k=1}^n}{\left(p_k({\lambda }_1)-p_k^{*}\right)}^T{\left({\Sigma }_k^p\right)}^{-1}\left(p_k({\lambda }_1)-p_k^{*}\right)={\mathrm{S}}_1\mathrm{,}$

(33)

${\displaystyle \sum _{k=1}^n}\log {\left(q_k({\lambda }_2)*\overline{q_k^{*}}\right)}^T{\left({\Sigma }_k^q\right)}^{-1}\log \left(q_k({\lambda }_2)\ast \overline{q_k^{*}}\right)={\mathrm{S}}_2\mathrm{.}$

(34)

Unlike in (20), where the calculation of P_k,q_k is coupled through the covariance matrices, here, the smoothed positions and orientations P_k (λ₁), q_k (λ₂) are calculated in a decoupled way by solving:

$\frac{1}{2}{\displaystyle \sum _{k=1}^n}{\left(p_k-p_k^{*}\right)}^T{\left({\Sigma }_k^p\right)}^{-1}\left(p_k-p_k^{*}\right)+{\lambda }_1{g}_1(p)\mathrm{,}$

(35)

$\frac{1}{2}{\displaystyle \sum _{k=1}^n}\log {\left(q_k\ast \overline{q_k^{*}}\right)}^T{\left({\Sigma }_k^q\right)}^{-1}\log \left(q_k\ast \overline{q_k^{*}}\right)+{\lambda }_2{g}_2(q)\mathrm{.}$

(36)

Assuming that σ ^p _k and σ ^q _k are the covariance matrices of the measurements, the acceptable values for S₁ and S₂ are within the range $N-\sqrt{2N}\le {\mathrm{S}}_1\mathrm{,}{\mathrm{S}}_2\le N+\sqrt{2N}\mathrm{,}N=n+1$ . This approach requires that we solve two additional nonlinear zero finding problems (33) and (34), but these are single variable, scalar, nonlinear equations, and can therefore easily be solved with standard solvers for scalar functions, such as, e.g., fzero available in MATLAB. Every time we need to compute the value of (33) or (34), we must first solve the decoupled systems (35) and (36) to obtain P_k(λ₁) and q_k(λ₂), which are needed for the calculation of (33) and (34) at the given λ₁λ₁ and λ₂, respectively. While the problems of determining λ₁ and λ₂ are not totally independent of each other, the approach of decoupling the calculation of both smoothing parameters worked well in practice.

The described method finds a smooth sequence of hand postures that approximate the measurements well. To generate a continuous motion trajectory that can be used for robot control, one must interpolate the smoothed postures. While standard techniques for interpolation in ℝ ⁿ [26] can be utilized for the interpolation of position vectors, more specialized methods are needed for smooth interpolation on SO(3). The most commonly used quaternion interpolation method is spherical linear interpolation (Slerp), proposed by [21], but more advanced methods that can ensure higher-order smoothness also exist [27].

5. Experimental Results and Conclusions

We applied the developed method for the reconstruction of 15 real hand motions. In these experiments, the Gauss-Newton method always converged. As expected, the convergence was slower for larger values of smoothing parameters (see Tab. 1). The measured poses were used as a starting point in iteration (30) or (32). One example smoothed trajectory, which was calculated at the optimal values of the smoothing parameters, is depicted in Figures 2, 3 and 4. In this way, smoother trajectories were estimated that resulted in less jerky robot movements.

Figure 2.

The translational part (x, y and z components) of the reconstructed trajectory (in centimetres) and a sample of measurements. Not all measurements are shown for better visualization.

Figure 3.

The orientational part (x, y andz components of u(t), q(t) = (w(t), u(t))) of the reconstructed trajectory and a sample of measurements. Not all measurements are shown for better visualization.

Figure 4.

The orientational part (w component of q(t) = (w(t),u(t))) of the reconstructed trajectory and a sample of measurements. Not all measurements are shown for better visualization.

To show the benefit of the proposed approach, which considers full covariance matrices σ _k and computes positions and orientations simultaneously, we compared it to scalar spline smoothing, where each component is evaluated separately. Decoupled smoothing of separate components of unit quaternions has an additional disadvantage that the smoothed quaternions are not unit quaternions. They should, therefore, be normalized after smoothing, which introduces additional errors. Note that scalar spline smoothing also requires the determination of an optimal smoothing parameter λ. The comparison was done in a simulation experiment where the correct position p(t) ∈ ℝ³ and orientation trajectories q(t) ∈ S³ were known. To the simulated trajectories, we added Gaussian noise using preselected covariance matrices σ _k . The quality of approximation was evaluated using the mean error, namely:

${\text{ME}}_p=\frac{1}{n}{\displaystyle \sum _{k=1}^n}\Vert p_k-p(t_k)\Vert \mathrm{,}$

(37)

${\text{ME}}_q=\frac{1}{n}{\displaystyle \sum _{k=1}^n}\mathrm{d}\left(q_k\mathrm{,}q(t_k)\right)\mathrm{,}$

(38)

where d is the metric on S³ defined in Eq. (9). Results for a typical trajectory are shown in Tab. 2. Significant improvement could be achieved both in position and the orientation trajectory.

In summary, in this paper we developed two trajectory estimation methods on ℝ³ × S³ - one based on Gauss-Newton iteration and the other on Levenberg-Marquardt iteration. We have also shown how to treat the measurement errors and suggested an approach for the automatic calculation of smoothing parameters. The method based on Gauss-Newton iteration turned out to be sufficient in our experiments and converged faster. However, it might become necessary to apply the method based on Levenberg-Marquardt iteration for the data containing more noise. With our approach, we were able to reconstruct smooth trajectories on ℝ³ × S³ using real data obtained by measurement systems with or without markers. In this way, we can provide a high quality input for imitation learning systems.

Table 2.

Comparison of smoothing with the proposed approach and with scalar spline smoothing, where each position and quaternion dimension is smoothed separately.

	ME p (mm)	ME p (deg)
Proposed approach	1.04	1.39
Scalar spline smoothing	1.81	2.07