Research on the rigid body pose estimation using dual quaternions

Abstract

The dual quaternion is the simplest and most effective mathematical tool to describe the translational and rotational motion of a general rigid body. Its computation and updating require screw vector. The relative pose information need to be updated when measuring the pose of the rigid body. In the traditional algorithm, it generally focuses on attitude updating, but less research on position updating. The rotation vector algorithm is used to represent the quaternion to update the attitude of the rigid body, but it cannot update the position. Because any general rigid body motion can be realized by rotation about a certain axis and translation along this axis, this article proposes an algorithm to update the position and attitude of the rigid body’s relative motion based on the screw vector. The rotation vector and screw vector are introduced in the rigid body motion and update the quaternion and dual quaternion, respectively; then, the relative pose information of the leader–follower rigid body based on the screw vector algorithm is deduced. The single-sample, two-sample, and three-sample algorithms are compared and simulated, and the simulation results show that this method not only overcomes the deficiencies associated with the separate updating of position and attitude using a traditional algorithm but also has higher precision than the traditional algorithm.

Introduction

Chasles’ theorem shows that any general rigid body motion can be achieved by rotation about a certain axis and translation along that axis, which is also called screw motion. ¹ The simplest and most effective mathematical tool to describe the screw motion is the dual quaternion which is developed on the basis of dual number. Dual number was invented by Clifford ² and further developed by Kotelnikov ³ and Study. ⁴ Kotelnikov and Study proposed the “principle of transference”; in accordance with this principle, all the vector or quaternion formulas and equations of the theory of finite rotations and the kinematics of the rotational motion of a rigid body can be replaced by bivector or biquaternion formulas and equations of the theory of finite displacements and the kinematics of the motion of a free rigid body. For example, in the study of Chelnokov, ⁵ the integration of kinematic equations of a rigid body’s screw motion was proposed. The dual quaternions have important applications in many fields. In robotics field,^6–12 Daniilidis ⁶ applied a dual quaternion in the robotic hand–eye calibration, and Antonio and Renata ⁷ presented a dual quaternion methodology to kinematically synthesize constrained robotic systems. In the study of Zhou et al., ¹⁰ a new forward kinematic algorithm based on dual quaternion for a 6-degree-of-freedom (DOF) general Stewart platform is proposed. In the study of Yang et al., ¹¹ a dual quaternion solution to the forward kinematics of a class of 6-DOF parallel robots with full or reductant actuation is presented. In Chelnokov’s study, ¹² a biquaternion solution of the kinematic control problem for the motion of a rigid body was addressed. In aerospace field,^13–18 Wang et al. ¹³ established the relative coupled dynamic model based on the dual quaternion, where the coupling effect between translational and rotational motion was indicated. Wu et al. ¹⁴ addressed the synchronized control problem of relative position and attitude for spacecraft with input constraint. In the work of Gui and Vukovich, ¹⁷ an adaptive controller was developed by combining the proportional–derivative (PD) controller with an adaptive algorithm, which provided estimations of unknown parameters and disturbances. In addition, in the application of human motion, Ji et al. ¹⁹ presented an algorithm for human motion capture of the real-time motion trajectory of human arms based on the dual quaternion with motion tracker, and Kavan et al. ²⁰ presented a new skinning algorithm based on a linear combination of dual quaternions. In strapdown inertial navigation field, in 1992, Branets and Shmyglevsky ²¹ devised a Picard-type recursive integration algorithm based on dual quaternion to solve the differential kinematic equations. Panov ²² devoted to mathematical foundations of the theory of inertial attitude control. Chelnokov^23–26 presented the equations and algorithms of inertial navigation based on quaternion and biquaternion. Wu et al.^27,28 applied the dual quaternion to sins and updated the attitude and velocity of the carrier using the dual quaternion updating algorithm; the algorithm is an extension of the traditional rotation vector method. Wang et al. ²⁹ proved the superiority of the attitude and gravitational velocity algorithm based on the dual quaternion compared to that based on the rotation vector in accuracy. However, the dual quaternion had fewer applications in updating the relative position and attitude of the rigid body.

Using the rotation vector to derive the differential equation of rigid body rotation, the angular increment method can cause a non-commutativity error. To reduce this error, the equivalent rotation vector algorithm is commonly used to correct the attitude to improve the attitude angle resolution, ³⁰ which is based on the idea of rotation vector theory. When more angular increment sub-samples are included in an update cycle, the attitude calculation is more accurate. The equivalent rotation vector method can be divided into two steps: rotation vector calculation and quaternion update. In the traditional algorithm, the quaternion and rotation vector algorithm are generally used in the rigid body attitude estimation.^31–33 However, the method can update only the attitude of the rigid body but not the position of the rigid body. The dual quaternion is the simplest and most effective mathematical tool to describe the general rigid body motion, and its updation and calculation require the screw vector method. When there is a continuous finite rotation of a rigid body around the axis, the angular displacement of the rigid body is related to the order of rotation; then, there is a non-commutativity of rotation, which is consistent with the non-commutative multiplication of the dual quaternion. The dual quaternion can well describe the line movement of a rigid body, so the screw vector algorithm can be applied to update the relative position and attitude of the rigid body. The screw vector algorithm is introduced in rigid body motion. When the algorithm is used to update the dual quaternion, the relative position and relative attitude updating algorithms based on the screw vector are deduced.

In this article, the rotation vector based on the quaternion and the screw vector based on the dual quaternion are combined. First, the rotation vector method is used to update the attitude quaternion of the leader rigid body relative to the inertial coordinate system. Then, the screw vector algorithm is used to update the dual quaternion of the follower rigid body relative to the inertial coordinate system. Finally, the relative position and relative attitude updating algorithms of the leader–follower rigid body are established based on the dual quaternion.

Mathematical preliminaries

Dual number

The concept of dual number was first proposed by Clifford ² as follows

$$\hat{z}=a+\epsilon b$$

(1)

where the two parts of a dual scalar are termed as the “primal” and the “dual,” the latter being multiplied by the dual unit $\epsilon$ ; and both the primal and the dual parts of a dual number are real numbers.

Screw vector

The axis of rotation in the screw motion is called the screw axis. As shown in Figure 1, the rigid body moves from position $O_1$ to $O_2$ by translating and rotating: when the rigid body turns angle $\theta$ around point $\mathit{c}$ on the screw axis $\hat{\mathit{l}}$ and translates distance $d$ along $\hat{\mathit{l}}$ .

Figure 1.

Screw transformation of a rigid body.

The motion of a rigid body is similar to the motion of a bolt and a nut, so it is called the screw motion. It can be described by a screw vector, which is defined as

$$\hat{\mathbf{\Phi }}=\hat{\mathrm{\Phi }}\hat{\mathit{n}}$$

(2)

where $\hat{\mathit{n}}=\mathit{l}+\epsilon \mathit{c}\times \mathit{l}$ is the unit screw axis, $\mathit{l}$ is the unit direction vector, and $\hat{\mathrm{\Phi }}=\Vert \hat{\mathbf{\Phi }}\Vert =\theta +\epsilon d$ is the dual angle, where ‖ ‖ represents the norm of the vector.

By formula $\hat{\mathrm{\Phi }}=(\hat{\mathbf{\Phi }}·\hat{\mathbf{\Phi }}{)}^{\frac{1}{2}}$ , we can get the screw vector which has the following rules

$${\hat{\mathrm{\Phi }}}^2={\mathrm{\Phi }}^2+\epsilon 2\mathrm{\Phi }·\mathrm{\Phi }\prime =\hat{\mathbf{\Phi }}·\hat{\mathbf{\Phi }}=\mathbf{\Phi }\mathbf{·}\mathbf{\Phi }+\mathbf{\epsilon }2\mathbf{\Phi }\mathbf{·}\mathbf{\Phi }\prime$$

(3)

So it is satisfied

$$\begin{array}{c}\multicolumn{1}{c}{\mathbf{\Phi }\mathbf{·}\mathbf{\Phi }={\Phi }^2}\\ \multicolumn{1}{c}{\mathbf{\Phi }\mathbf{·}\mathbf{\Phi }\prime =\Phi \Phi \prime }\end{array}$$

(4)

Dual quaternion

According to the principle of transference,^3,4 a dual quaternion can be described as dual numbers, as defined by

$$\hat{\mathit{q}}=\mathit{q}+\epsilon \mathit{q}\prime ={\hat{q}}_0+{\hat{q}}_1\mathit{i}+{\hat{q}}_2\mathit{j}+{\hat{q}}_3\mathit{k}$$

(5)

In equation (5), both the real part $\mathit{q}$ and the dual part $\mathit{q}\prime$ are quaternions, and ${\hat{q}}_i(i=0,\dots ,3)$ are dual numbers.

The quaternion can be represented by a rotation vector; similarly, the dual quaternion can be expressed by a screw vector as follows

$$\hat{\mathit{q}}=\mathit{q}+\epsilon \mathit{q}\prime =\cos \frac{\hat{\mathrm{\Phi }}}{2}+\frac{\hat{\mathbf{\Phi }}}{\hat{\mathrm{\Phi }}}\sin \frac{\hat{\mathrm{\Phi }}}{2}$$

(6)

When the scalar numbers of dual quaternion $\hat{p}$ and $\hat{q}$ are zero ( ${\hat{p}}_0={\hat{q}}_0=0$ ), there are

$$\hat{p}°\hat{q}=\hat{\mathit{p}}°\hat{\mathit{q}}=-\hat{\mathit{p}}·\hat{\mathit{q}}+\hat{\mathit{p}}\times \hat{\mathit{q}}$$

(7)

The relationship between translation and rotation in Figure 1 is as follows

$$\begin{array}{c}\multicolumn{1}{c}{{\hat{\mathit{l}}}^{O_2}={\hat{\mathit{q}}}^{*}°{\hat{\mathit{l}}}^{O_1}°\hat{\mathit{q}}}\\ \multicolumn{1}{c}{{\hat{\mathit{l}}}^{O_1}=\hat{\mathit{q}}°{\hat{\mathit{l}}}^{O_2}°{\hat{\mathit{q}}}^{*}}\end{array}$$

(8)

where ${\hat{\mathit{q}}}^{*}$ is the conjugate of $\hat{\mathit{q}}$ , and ${\hat{\mathit{l}}}^{O_1}$ and ${\hat{\mathit{l}}}^{O_2}$ are the representations of $\hat{\mathit{l}}$ in the coordinate systems $O_1$ and $O_2$ , respectively.

Relative position and relative attitude update of the leader–follower rigid body

Model establishment

Because the function of the model is to describe the relative position and attitude, it is necessary to select a coordinate system as the reference. Therefore, the primary task of modeling is to determine the reference coordinate system of the model. It is necessary to obtain the position increment and the angle increment of the rigid body when calculating the position and attitude of the follower rigid body relative to the leader rigid body, while the relative position increment is the centroid of the follower rigid body relative to the centroid of the leader rigid body; and the description of angle increment of the rigid body is the simplest relative to the inertial coordinate system. Therefore, as shown in Figure 2, we choose the inertial coordinate system I whose origin coincides with the leader rigid body as the reference coordinate system to model.

Figure 2.

Transformation between coordinate systems.

It is assumed that the angular velocity vector of the leader rigid body relative to the inertial system I is represented in the leader rigid body’s coordinate system as ${\mathbf{\omega }}_{\mathrm{lI}}^l$ ; and the angular velocity vector of the follower rigid body relative to the inertial system I is represented in the follower rigid body’s coordinate system as ${\mathbf{\omega }}_{\mathrm{fI}}^f$ . Since the centroid of the leader rigid body coincides with the origin of the inertial system I, there is no translation of the leader rigid body relative to the inertial system, only rotation. At the initial time $t_0$ , the attitude quaternion of follower rigid body relative to the leader rigid body is

$${\mathit{q}}_{\mathrm{fl}{t}_0}={\mathit{q}}_{\mathrm{lI}{t}_0}^{*}°{\mathit{q}}_{\mathrm{fI}{t}_0}$$

(9)

At the initial time, the dual quaternion of the position and attitude of the follower rigid body relative to the inertial system is as follows

$${\hat{\mathit{q}}}_{\mathrm{fI}{t}_0}={\mathit{q}}_{\mathrm{fI}{t}_0}+\frac{\epsilon }{2}{\mathbf{r}}_{\mathrm{fI}{t}_0}^I°{\mathit{q}}_{\mathrm{fI}{t}_0}$$

(10)

Considering that the coordinate system of leader rigid body has no translation relative to the inertial system, only the relative attitude, the dual quaternion of position and attitude of the follower rigid body relative to the leader rigid body is

$${\hat{\mathit{q}}}_{\mathrm{fl}{t}_0}={\mathit{q}}_{\mathrm{lI}{t}_0}^{*}°{\hat{\mathit{q}}}_{\mathrm{fI}{t}_0}={\mathit{q}}_{\mathrm{fl}{t}_0}+\frac{\epsilon }{2}{\mathbf{r}}_{\mathrm{fl}{t}_0}^l°{\mathit{q}}_{\mathrm{fl}{t}_0}$$

(11)

In order to obtain the dual quaternion ${\hat{\mathit{q}}}_{\mathrm{fl}{t}_k}$ of the follower rigid body relative to the leader rigid body at any time, it is necessary to solve the kinematic equation of the dual quaternion of the follower rigid body relative to the inertial system I. The kinematic equation is as follows ⁵

$${\stackrel{·}{\hat{\mathit{q}}}}_{\mathrm{fI}}=\frac{1}{2}{\hat{\mathit{q}}}_{\mathrm{fI}}°{\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f$$

(12)

where ${\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f$ is the dual angular velocity of the follower rigid body.

And the kinematics equation of leader rigid body relative to the inertial system I is as follows

$${\stackrel{·}{\mathit{q}}}_{\mathrm{lI}}=\frac{1}{2}{\mathit{q}}_{\mathrm{lI}}°{\mathbf{\omega }}_{\mathrm{lI}}^l$$

(13)

Attitude update of the leader rigid body

Because the centroid of the leader rigid body coincides with the origin of the inertial coordinate system I, as shown in Figure 2, the leader rigid body coordinate system has no translation relative to the inertial coordinate system (only the rotation). The leader rigid body has only attitude updating and no position updating. The attitude quaternion of the leader rigid body in the update period is

$${\mathit{q}}_{\mathrm{lI}}\left(t_{k+1}\right)={\mathit{q}}_{\mathrm{lI}}\left(t_k\right)°{\mathit{q}}_{\mathrm{lI}}\left(\Delta t\right)$$

(14)

where ${\mathit{q}}_{\mathrm{lI}}(t_k)$ and ${\mathit{q}}_{\mathrm{lI}}(t_{k+1})$ are the quaternions of the previous time and present time, respectively; and ${\mathit{q}}_{\mathrm{lI}}(\Delta t)$ is the updated quaternion. The corresponding rotation vector is ${\mathbf{\Phi }}_{t_k}$ in the time period $(t_k,t_{k+1})$ . Then, the updated quaternion represented by the rotation vector algorithm is

$${\mathit{q}}_{\mathrm{lI}}\left(\Delta t\right)=\cos \frac{{\mathrm{\Phi }}_{t_k}}{2}+\frac{{\mathbf{\Phi }}_{t_k}}{{\mathrm{\Phi }}_{t_k}}\sin \frac{{\mathrm{\Phi }}_{t_k}}{2}$$

(15)

The rotation vector ${\mathbf{\Phi }}_{t_k}$ is determined by angular velocity $\mathit{\omega }$ as follows

$${\mathbf{\Phi }}_{t_k}=\mathit{\Delta }{\mathit{\theta }}_{{\mathbf{t}}_{\mathbf{k}}}=\underset{t_k}{\overset{t_{k+1}}{\int }}\mathbf{\omega }\left(\tau \right)d\tau$$

(16)

where $\Delta {\mathit{\theta }}_{{\mathbf{t}}_{\mathbf{k}}}$ is the angular increment in the time period $(t_k,t_{k+1})$ .

If a constant is used to fit the angular velocity, a single-sample algorithm for the rotation vector is obtained, and the corresponding rotation vector is

$$\{\begin{array}{c}\begin{array}{c}\mathbf{\omega }(t_k+\tau )=\mathit{a},0⩽\tau ⩽\Delta t\hfill \end{array}\hfill \\ \multicolumn{1}{c}{{\mathbf{\Phi }}_{t_k}=\Delta \mathit{\theta }}\end{array}$$

(17)

If a straight line is used to fit the angular velocity, a two-sample algorithm for the rotation vector is obtained, and the corresponding rotation vector is ²²

$$\{\begin{array}{c}\begin{array}{c}\mathit{\omega }\mathbf{(}{\mathbf{t}}_{\mathbf{k}}+\mathbf{\tau }\mathbf{)}=\mathit{a}+2\mathit{b}\mathbf{\tau }\mathbf{,}0⩽\tau ⩽\Delta t\hfill \end{array}\hfill \\ \multicolumn{1}{c}{{\mathbf{\Phi }}_{t_k}=\Delta {\mathit{\theta }}_1+\Delta {\mathit{\theta }}_2+\frac{2}{3}(\Delta {\mathit{\theta }}_1\times \Delta {\mathit{\theta }}_2)}\end{array}$$

(18)

where $\Delta {\mathit{\theta }}_{\mathbf{j}}={\int }_{\frac{j-1}{2}h}^{\frac{j}{2}h}\mathbf{\omega }\mathbf{(}{\mathbf{t}}_{\mathbf{k}}+\mathbf{\tau }\mathbf{)}\mathbf{d}\mathbf{\tau }$ , j = 1, 2; $\frac{2}{3}\Delta {\mathit{\theta }}_1\times \Delta {\mathit{\theta }}_2$ is the non-commutativity error.

If a parabola is used to fit the angular velocity, the angular velocity is sampled three times in one update period. Then, a three-sample algorithm for the rotation vector is obtained, and the corresponding rotation vector is ²²

$$\{\begin{array}{c}\multicolumn{1}{c}{\begin{array}{c}\multicolumn{1}{c}{\mathit{\omega }\mathbf{(}{\mathbf{t}}_{\mathbf{k}}+\mathbf{\tau }\mathbf{)}=\mathit{a}+2\mathit{b}\mathbf{\tau }+3\mathit{c}{\mathbf{\tau }}^2\mathbf{,}0⩽\tau ⩽\Delta t}\end{array}}\\ \multicolumn{1}{c}{{\mathbf{\Phi }}_{t_k}=\Delta {\mathit{\theta }}_1+\Delta {\mathit{\theta }}_2+\Delta {\mathit{\theta }}_3+\frac{9}{20}\Delta {\mathit{\theta }}_1\times \Delta {\mathit{\theta }}_3}\\ \multicolumn{1}{c}{+\frac{27}{40}\Delta {\mathit{\theta }}_2\times (\Delta {\mathit{\theta }}_3-\Delta {\mathit{\theta }}_1)}\end{array}$$

(19)

where $\Delta {\mathit{\theta }}_{\mathbf{j}}={\int }_{\frac{j-1}{3}h}^{\frac{j}{3}h}\mathbf{\omega }\mathbf{(}{\mathbf{t}}_{\mathbf{k}}+\mathbf{\tau }\mathbf{)}\mathbf{d}\mathbf{\tau }$ , j = 1, 2, 3; $\frac{9}{20}\Delta {\mathit{\theta }}_1\times \Delta {\mathit{\theta }}_3+\frac{27}{40}\Delta {\mathit{\theta }}_2\times (\Delta {\mathit{\theta }}_3-\Delta {\mathit{\theta }}_1)$ is the non-commutativity error. $\Delta {\mathit{\theta }}_1$ , $\Delta {\mathit{\theta }}_2$ , and $\Delta {\mathit{\theta }}_3$ are the angular increments in time periods $[t_k,t_k+\frac{h}{3}]$ , $[t_k+\frac{h}{3},t_k+\frac{2h}{3}]$ , and $[t_k+\frac{2h}{3},t_{k+1}]$ , respectively.

The solution procedure is as follows: first, substitute $\Delta {\mathit{\theta }}_{\mathbf{j}}(t)$ into formulas (17)–(19) to obtain the rotation vector $\mathbf{\Phi }$ of several sub-samples; then, substitute $\mathbf{\Phi }$ into formula (15) to obtain the updated quaternion ${\mathit{q}}_{\mathrm{lI}}(\Delta t)$ ; combining this with formula (14), we obtain the attitude quaternion of the leader rigid body relative to the inertial coordinate system at every updating time.

Position and attitude update of the followerrigid body

Because the follower rigid body coordinate system relative to the leader rigid body coordinate system has both translation and rotation, as shown in Figure 2, the follower rigid body has both an updated position and an updated attitude relative to the leader rigid body. Therefore, using the analogy principle, similar to the rotation vector method, the position and attitude dual quaternion is updated using the screw vector algorithm.

The dual quaternion update formula is

$${\hat{\mathit{q}}}_{\mathrm{fI}}\left(t_{k+1}\right)={\hat{\mathit{q}}}_{\mathrm{fI}}\left(t_k\right)°{\hat{\mathit{q}}}_{\mathrm{fI}}\left(\Delta t\right)$$

(20)

where $\hat{\mathit{q}}(\Delta t)$ is a function of the screw vector

$${\hat{\mathit{q}}}_{\mathrm{fI}}\left(\Delta t\right)=\cos \frac{\hat{\sigma }(t_k)}{2}+\frac{\hat{\mathit{\sigma }}(t_k)}{\hat{\sigma }(t_k)}\sin \frac{\hat{\sigma }(t_k)}{2}$$

(21)

where $\hat{\mathit{\sigma }}$ is the screw vector along the screw axis and is determined by the angular velocity of the rigid body, and $\hat{\sigma }=(\hat{\mathit{\sigma }}·\hat{\mathit{\sigma }}{)}^{1/2}$ .

The calculation shows that the screw vector algorithm is consistent with the rotation vector algorithm, so the formula for the single-sample screw vector algorithm is

$$\hat{\mathit{\sigma }}=\hat{\mathit{\alpha }}\left(t\right)=\underset{t}{\overset{t+\Delta t}{\int }}{\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f\left(t\right)\mathrm{dt}$$

(22)

The formula for the two-sample screw vector algorithm is

$$\hat{\mathit{\sigma }}={\hat{\mathit{\alpha }}}_1(t)+{\hat{\mathit{\alpha }}}_2(t)+\frac{2}{3}{\hat{\mathit{\alpha }}}_1\left(t\right)\times {\hat{\mathit{\alpha }}}_2\left(t\right)$$

(23)

The formula for the dual angular increment is

$${\hat{\mathbf{\alpha }}}_i\left(t\right)=\underset{t+\frac{i-1}{2}\Delta t}{\overset{t+\frac{i}{2}\Delta t}{\int }}{\hat{\mathit{\omega }}}_{\mathrm{fI}}^f\left(t\right)\mathrm{dt},i=1,2$$

(24)

where ${\hat{\mathit{\omega }}}_{\mathrm{fI}}^f$ is the dual angular velocity of the follower rigid body ³⁴

$${\hat{\mathit{\omega }}}_{\mathrm{fI}}^f={\mathbf{\omega }}_{\mathrm{fI}}^f+\epsilon ({\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^f+{\mathbf{\omega }}_{\mathrm{fI}}^f\times {\mathbf{r}}_{\mathrm{fI}}^f)$$

(25)

where ${\mathbf{r}}_{\mathrm{fI}}^f$ denotes the position from the coordinate system of the follower rigid body relative to the inertial coordinate system, ${\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^f$ is local derivative of the vector ${\mathbf{r}}_{\mathrm{fI}}^f$ , ${\mathbf{\omega }}_{\mathrm{fI}}^f$ is the angular velocity, and ${\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^f+{\mathbf{\omega }}_{\mathrm{fI}}^f\times {\mathbf{r}}_{\mathrm{fI}}^f$ is the linear velocity.

The algorithm for the three-sample screw vector is

$$\begin{array}{c}\multicolumn{1}{c}{\hat{\mathit{\sigma }}={\hat{\mathit{\alpha }}}_1\left(t\right)+{\hat{\mathit{\alpha }}}_2\left(t\right)+{\hat{\mathit{\alpha }}}_3\left(t\right)+\frac{9}{20}{\hat{\mathit{\alpha }}}_1\left(t\right)\times {\hat{\mathit{\alpha }}}_3\left(t\right)}\\ \multicolumn{1}{c}{+\frac{27}{40}{\hat{\mathit{\alpha }}}_2\left(t\right)\times ({\hat{\mathit{\alpha }}}_3\left(t\right)-{\hat{\mathit{\alpha }}}_1\left(t\right))}\end{array}$$

(26)

The dual angle increment is

$${\hat{\mathit{\alpha }}}_i\left(t\right)=\underset{t+\frac{i-1}{3}\Delta t}{\overset{t+\frac{i}{3}\Delta t}{\int }}{\hat{\mathit{\omega }}}_{\mathrm{fI}}^f\left(t\right)\mathrm{dt},i=1,2,3$$

(27)

From formula (27), it can be seen that the updating of the leader rigid body needs to calculate the dual angular velocity ${\hat{\mathit{\omega }}}_{\mathrm{fI}}^f$ at each time. In formula (25), ${\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^f$ can be obtained by the following formula

$$\begin{array}{c}\multicolumn{1}{c}{{\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^f=\frac{\partial ({\mathit{q}}_{\mathrm{fI}}^{*}°{\mathbf{r}}_{\mathrm{fI}}^I°{\mathit{q}}_{\mathrm{fI}})}{\partial t}}\\ \multicolumn{1}{c}{={\stackrel{·}{\mathit{q}}}_{\mathrm{fI}}^{*}°{\mathbf{r}}_{\mathrm{fI}}^I°{\mathit{q}}_{\mathrm{fI}}+{\mathit{q}}_{\mathrm{fI}}^{*}°{\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^I°{\mathit{q}}_{\mathrm{fI}}+{\mathit{q}}_{\mathrm{fI}}^{*}°{\mathbf{r}}_{\mathrm{fI}}^I°{\stackrel{·}{\mathit{q}}}_{\mathrm{fI}}}\\ \multicolumn{1}{c}{=-{\mathit{q}}_{\mathrm{fI}}^{*}°{\stackrel{·}{\mathit{q}}}_{\mathrm{fI}}°{\mathit{q}}_{\mathrm{fI}}^{*}°{\mathbf{r}}_{\mathrm{fI}}^I°{\mathit{q}}_{\mathrm{fI}}+{\mathit{q}}_{\mathrm{fI}}^{*}°{\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^I°{\mathit{q}}_{\mathrm{fI}}+{\mathit{q}}_{\mathrm{fI}}^{*}°{\mathbf{r}}_{\mathrm{fI}}^I°{\stackrel{·}{\mathit{q}}}_{\mathrm{fI}}}\end{array}$$

(28)

From formula (28), we can see that the calculation of ${\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^f$ requires multiple calculation of quaternion multiplication, which is a more time-consuming process. In order to improve the real time of algorithm, we propose a simplified algorithm. The following expression can be obtained

$${\mathit{r}}_{\mathrm{fI}}^I=q_{\mathrm{fI}}^{*}°{\mathit{r}}_{\mathrm{fI}}^f°q_{\mathrm{fI}}$$

(29)

Formula (29) takes the derivative of the time, and the following formula is acquired

$${\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^I=q_{\mathrm{fI}}°({\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^f+{\mathbf{\omega }}_{\mathrm{fI}}^f\times {\mathbf{r}}_{\mathrm{fI}}^f)°q_{\mathrm{fI}}^{*}$$

(30)

If formula (30) is substituted into formula (25), the following expression is acquired

$${\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f={\mathbf{\omega }}_{\mathrm{fI}}^f+\epsilon ({\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^f+{\mathbf{\omega }}_{\mathrm{fI}}^f\times {\mathbf{r}}_{\mathrm{fI}}^f)={\mathbf{\omega }}_{\mathrm{fI}}^f+\epsilon ({\mathit{q}}_{\mathrm{fI}}^{*}°{\stackrel{·}{\mathbf{r}}}_{\mathrm{fI}}^I°{\mathit{q}}_{\mathrm{fI}})$$

(31)

Using formulas (22)–(31), the screw vector $\hat{\mathit{\sigma }}$ for several sub-samples can be obtained. Then, we substitute $\hat{\mathit{\sigma }}$ into formula (21) to obtain the updated quaternion $\hat{\mathit{q}}(\Delta t)$ and combine with formula (20) to obtain the position and attitude dual quaternion of the follower rigid body relative to the inertial coordinate system.

Relative position and relative attitude updating algorithm of the leader–follower rigid body

When $q_{\mathrm{fI}}(t_k)$ and ${\hat{q}}_{\mathrm{lI}}(t_k)$ are obtained at each updating time, the dual quaternion of the follower rigid body relative to the leader rigid body can be written as

$$\begin{array}{c}\multicolumn{1}{c}{{\hat{\mathit{q}}}_{\mathrm{fl}}(t_k)={\mathit{q}}_{\mathrm{lI}}^{*}(t_k)°{\hat{\mathit{q}}}_{\mathrm{fI}}(t_k)}\\ \multicolumn{1}{c}{={\mathit{q}}_{\mathrm{lI}}^{*}(t_k)°({\mathit{q}}_{\mathrm{fI}}(t_k)+\epsilon \frac{1}{2}{\mathbf{r}}_{\mathrm{fI}}^I(t_k)°{\mathit{q}}_{\mathrm{fI}}(t_k))}\\ \multicolumn{1}{c}{={\mathit{q}}_{\mathrm{lI}}^{*}(t_k)°{\mathit{q}}_{\mathrm{fI}}(t_k)+\epsilon \frac{1}{2}{\mathit{q}}_{\mathrm{lI}}^{*}(t_k)°{\mathbf{r}}_{\mathrm{fI}}^I(t_k)°{\mathit{q}}_{\mathrm{fI}}(t_k)}\\ \multicolumn{1}{c}{={\mathit{q}}_{\mathrm{fl}}(t_k)+\epsilon \frac{1}{2}{\mathit{q}}_{\mathrm{lI}}^{*}(t_k)°{\mathbf{r}}_{\mathrm{fI}}^I(t_k)°{\mathit{q}}_{\mathrm{lI}}(t_k)°{\mathit{q}}_{\mathrm{lI}}^{*}(t_k)°{\mathit{q}}_{\mathrm{fI}}(t_k)}\\ \multicolumn{1}{c}{={\mathit{q}}_{\mathrm{fl}}(t_k)+\epsilon \frac{1}{2}{\mathbf{r}}_{\mathrm{fl}}^f(t_k)°{\mathit{q}}_{\mathrm{fl}}(t_k)}\end{array}$$

(32)

where ${\mathbf{r}}_{\mathrm{fl}}^f(t_k)$ and ${\mathit{q}}_{\mathrm{fl}}(t_k)$ are the position vector and attitude quaternion of the follower rigid body relative to the leader rigid body, respectively.

The position vector ${\mathbf{r}}_{\mathrm{fl}}^f(t_k)$ can be written as ³

$${\mathbf{r}}_{\mathrm{fl}}^f(t_k)=2{\mathit{q}\prime }_{\mathrm{fl}}\left(t_k\right)°{\mathit{q}}_{\mathrm{fl}}^{*}\left(t_k\right)$$

(33)

In this article, the relative position and attitude model of the leader–follower rigid body based on the dual quaternion and the position and attitude update using the screw vector algorithm are established. Then, the relative position and attitude information of the follower rigid body relative to the leader rigid body is obtained by calculation.

Simulation results

In this article, taking the moving rigid body in three-dimensional space as an example, according to the formulas (22)–(31), the screw vector for single-sample, two-sample, and three-sample can be obtained, then the updated dual quaternion ${\hat{\mathit{q}}}_{\mathrm{fI}}(\Delta t)$ can be calculated by formula (21); finally, the dual quaternion of the follower rigid body relative to the leader rigid body ${\hat{\mathit{q}}}_{\mathrm{fl}}(t_k)$ can be determined. The relative pose information of the leader–follower rigid body using the dual quaternion model and the screw vector algorithm is compared with the real values to obtain the relative position and attitude error. The position error is ${\mathbf{r}}_{\mathrm{fl}}^f(t_k)-{\overline{\mathbf{r}}}_{\mathrm{fl}}^f(t_k)$ ; the attitude error is ${{\overline{\mathit{q}}}^{*}}_{\mathrm{fl}}(t_k)°{\mathit{q}}_{\mathrm{fl}}(t_k)$ , where $\mathbf{r}$ and $\mathit{q}$ are the true values, and $\overline{\mathbf{r}}$ and $\overline{\mathit{q}}$ are the estimated values obtained by the algorithm in this article. The calculation of the relative attitude is a product (instead of a subtraction) of two attitude quaternions. If there is no attitude error, the quaternion error is ${\left[\begin{array}{cccc}1\hfill & 0\hfill & 0\hfill & 0\hfill \end{array}\right]}^{\mathrm{T}}$ . Assume that the attitude quaternion of the leader rigid body relative to the inertial coordinate system is $q_{\mathrm{lI}}(t_0)={\left[\begin{array}{c}0\frac{\sqrt{2}}{2}0\frac{\sqrt{2}}{2}\hfill \end{array}\right]}^{\mathrm{T}}$ , and the attitude quaternion of the follower rigid body relative to the inertial coordinate system is $q_{\mathrm{fI}}(t_0)={\left[\begin{array}{c}\frac{1}{2}-\frac{1}{2}-\frac{1}{2}\frac{1}{2}\hfill \end{array}\right]}^{\mathrm{T}}$ . The initial relative attitude angular velocity is ${[000.001]}^{\mathrm{T}}\mathrm{rad}/\mathrm{s}$ , the update period is $\mathrm{P}=0.2s$ , and the simulation time $t=3000s$ . The following are the use of single-sample, two-sample, and three-sample screw vector algorithm for simulation.

Figures 3 and 4 show the relative attitude and position error curves when the single-sample screw vector algorithm is used. Figures 3 and 4 show that the position error of the single-sample screw vector algorithm is 10⁻² m, the attitude error of $q_0$ is 1, and the errors of $q_1$ , $q_2$ , and $q_3$ are in the order of 10⁻¹. Figures 5 and 6 show the relative attitude and position error curves when the two-sample screw vector algorithm is used. Figures 5 and 6 show that the two-sample screw vector algorithm is better than the single-sample screw vector algorithm, and the accuracy is significantly improved. Figures 7 and 8 show the relative attitude and position error curves that were obtained using the three-sample screw vector algorithm. Figures 7 and 8 show that the position error of the three-sample screw vector algorithm is 10⁻⁴ m, the attitude error of $q_0$ is 1, and the errors of $q_1$ , $q_2$ , and $q_3$ are in the order of 10⁻⁵. From this, we can see that the accuracy of three-sample screw vector algorithm is higher than those of the other sub-samples.

Figure 3.

Single-sample attitude error of screw vector algorithm.

Figure 4.

Single-sample position error of screw vector algorithm.

Figure 5.

Two-sample attitude error of screw vector algorithm.

Figure 6.

Two-sample position error of screw vector algorithm.

Figure 7.

Three-sample attitude error of screw vector algorithm.

Figure 8.

Three-sample position error of screw vector algorithm.

It can be seen from Figures 7 and 8 that although the three-sample screw vector algorithm satisfies the accuracy requirements, the position and attitude error curves are divergent. Taking the position error modulus curve as an example, the simulation time is increased to 1.0 × 10⁵ s. The simulation figure is as follows.

As can be seen from Figure 9, the position error diverges very slowly. After 1.0 × 10⁵ s, the error does not exceed 5 × 10⁻³ m, so the error is completely acceptable. It can meet the requirements of pose estimation accuracy.

Figure 9.

Curves of three-sample position error norm.

To further confirm the validity of the algorithm and its derivation, MATLAB (MathWorks Inc.) was employed to simulate and calculate the relative attitude calculated by the quaternion attitude model, and the three-sample rotation vector algorithm in the study of Pittelkau ³² was adopted in the simulation to compare with the algorithm in this article. We assume that the same initial value of the attitude quaternion is used, and the following results were achieved.

Figure 10 shows the relative attitude error curves that were obtained using the three-sample rotation vector algorithm. The figure shows that the attitude error of $q_0$ is 1, the errors of $q_1$ and $q_2$ are in the order of 10⁻³, and the error of $q_3$ is in the order of 10⁻⁴.

Figure 10.

Three-sample attitude error of rotation vector algorithm.

It can be seen from the above simulation that the accuracy of three-sample screw vector algorithm is not only higher in the pose accuracy than those of the other sub-samples but also has higher accuracy than the rotation vector algorithm in the attitude accuracy. The single-sample screw vector algorithm does not consider the non-commutativity error of the rigid body, so the divergence speed is faster. Therefore, it is hardly used in practical engineering applications, even though its computation cost is very small. The accuracy of the two-sample screw vector algorithm is obviously improved and can be applied to some engineering practices, but the divergence speed is faster and it is not suitable for long-term operation. Compared to the two-sample screw vector algorithm, the three-sample screw vector algorithm can better deal with the influence of rigid body non-commutativity error on position and attitude, so it is more accurate than other sub-sample screw vector algorithm. Compared to the single-sample and two-sample screw algorithms, although the position and attitude errors of the three-sample screw vector algorithm increased, the divergence is very slow. Even after a long period of operation, it can meet the requirements of pose estimation accuracy. However, compared to the rotation vector algorithm, the screw vector algorithm can consider the updating of position and attitude uniformly, and the attitude accuracy is higher than that of the rotation vector algorithm, but the computation amount is also correspondingly larger, which will affect the fastness and real-time performance of the algorithm. In practical engineering applications, the appropriate sub-sample and algorithms should be selected according to the accuracy, fastness, and real-time performance of the algorithm.

Conclusion

In this article, the screw vector algorithm, which is applied in the strapdown inertial navigation system in the traditional algorithm, is used to update the relative position and attitude of the moving rigid body. The rotation vector and screw vector are introduced in the rigid body motion to update the quaternion and dual quaternion, respectively; then, the relative position and attitude information of the leader–follower rigid body based on the screw vector algorithm is deduced. First, the rotation vector method is used to update the attitude quaternion of the leader rigid body relative to the inertial coordinate system. Then, the screw vector algorithm is used to update the dual quaternion of the follower rigid body relative to the inertial coordinate system. Finally, the relative position and attitude updating algorithm of the leader–follower rigid body is established based on the dual quaternion. The simulation results show that the three-sample algorithm not only overcomes the deficiencies associated with the separate updating of position and attitude but also improves the non-commutativity error compared to the single-sample and two-sample algorithms. At the same time, it has higher accuracy than the rotation vector algorithm in the attitude accuracy. In the next study, we need further research on the real-time and validity performance of the algorithm.