Relative position and attitude coordinated control based on unit dual quaternion

Abstract

In traditional algorithms, relative pose controls of a leader–follower rigid body consider the position and attitude as two subsystems in designing control laws. Since relative pose of a leader–follower rigid body is coupled, having a pose coupling control is a smarter choice. For deficiencies in decentralized control of relative pose, this article establishes a coupled dynamic and kinematic model of a leader–follower rigid body based on the dual-number and dual-quaternion theory framework. Furthermore, based on the proposed model, the sliding mode control theory is used to conduct coordinated control for relative pose of a leader–follower rigid body. The coupled algorithm does not need to design two sets of control laws for relative pose of a leader–follower rigid body; therefore, the complexity of the control system is reduced. Simulation results show that this algorithm does not only demonstrate the coupled control law of the relative pose but also show good tracking control performance.

Introduction

Relative motion control of rigid bodies has become a popular topic in recent academic research fields. Many scholars have conducted large amounts of research on the relative motion control of rigid bodies. Examples can be found in research areas such as spacecraft formation systems, multi-robot systems, multi-unmanned aircraft systems, and distributed camera network systems. Many researchers have conducted studies on relative position control of rigid bodies.^1–7 For example, Chang et al. ¹ proposed using relative position control for a multi-robot dynamical system based on the adaptive fuzzy sliding-mode control algorithm; J Hu and Feng ² presented a tracking control algorithm of a multi-agent systems; Yun et al. ⁵ put forward a synchronized control method for a multi-unmanned aircraft system track; and Hu and Zhang ⁶ introduced a relative position control method to be used for spacecraft formation. Many researchers have also conducted work on relative attitude control of a rigid body.^8–16 For example, Dimarogonas et al. ⁸ addressed a control algorithm that exploits algebraic graph tools for coordinated control of multiple rigid bodies; Kristiansen et al. ⁹ developed a method to solve the problem of tracking relative rotation of a leader–follower spacecraft formation using relative attitude feedback; Zhao and Jia ¹² proposed an attitude synchronized control algorithm for a nonsingular fast terminal sliding mode controller; and Cai and Huang ¹⁶ presented an attitude feedback control algorithm for multiple rigid body systems based on unit quaternion. The above-mentioned algorithms describe relative position and attitude of multi-rigid-body system separately. Since translational and rotational motions of multi-rigid body and many shared resources of multi-rigid-body systems are coupled with each other, relative position and attitude systems for rigid bodies are not independent.

To resolve the discussed problems, some researchers have proposed using a dynamic model and a new control method for position and attitude integration of a rigid body. For example, Shan ¹⁷ developed a dynamic model of 6 degrees of freedom for spacecraft formation, and based on this, the researchers designed an adaptive synchronized controller that is based on the conception of cross-coupling. Segal and Gurfil ¹⁸ presented a coupled kinematic and dynamic model for the relative rotation and translation found for two spacecrafts rendezvous and docking (RVD). Wu et al. ¹⁹ proposed a nonlinear suboptimal synchronization control algorithm for relative pose tracking of space formation flying. Although above algorithms consider the coupling property of attitude and track, it is still necessary to design two sets of controllers, thus increasing the complexity of computation. In order to solve above problems, Hatanaka et al. ²⁰ addressed a passivity-based pose synchronization control law in the special Euclidean group SE(3), although the algorithm designed a pose-coordinated control law, it was far more complex than pure translational or rotational control and needed to combined with graph topology theory, which was more suitable for solving the coordinated control problem of multi-rigid-body formation.

Dual quaternion is the most compact and efficient tool to represent the pose of a rigid body. The 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 screw motion is the dual quaternion. It is an extension of classical quaternion. Recently, some scholars have applied dual quaternions in numerous fields.^22–24 For example, Daniilidis ²² applied a dual quaternion in robotic hand–eye calibration; Kavan et al. ²³ proposed a new skinning algorithm based on linear combination of dual quaternion; Wu et al. ²⁴ presented an algorithm to analyze the error characteristics of strapdown inertial navigation system using dual-quaternion algebra. In recent years, some scholars have proposed dynamics model or pose-coordinated control algorithm based on dual quaternions. For example, Wang et al. ²⁵ proposed a dual-quaternion solution to attitude and position control for multiple rigid body coordination, the distributed control strategy needed together with a specified rooted-tree structure and had been extended to the coordinations of multi-rigid-body systems, which was more suitable to solve the problem of multiple rigid body coordination, not suitable for the coordination control of relative position and attitude of leader–follower rigid body; Zhang et al. ²⁶ proposed a control law based on unit dual-quaternion lie group for the transformation of rigid bodies in configuration space and the algorithm using the dynamic model of a single-robot platform to validate the derived control law, and then, the expression of the control law in the plane configuration space was obtained; however, the algorithm was mainly applied to the configuration transformation of a single planar mobile robot; similarly, in Wang and Yu, ²⁷ an algorithm was proposed based on the dual-quaternion lie group and lie algebra; however, it provided a unified solution for the attitude and position tracking problem of a single rigid body, rather than the relative position and attitude coordinated control of leader–follower rigid body.

The rest of this article is organized as follows. Section “Definitions of dual number and dual quaternion” introduces the concepts of dual number and dual quaternion. Then, the relative kinematic and dynamic models for a leader–follower rigid body within the dual-quaternion theory framework are derived. In the next section, a coordinated controller for a leader–follower rigid body is deduced based on the dual-quaternion-based model proved to be almost globally asymptotically stable. Finally, a numerical example is used to verify the effectiveness of the proposed control law. Compared to traditional algorithms, where it is necessary to design two sets of control laws to control relative positions and attitudes of a leader–follower rigid body, the proposed algorithm for coordinated control law design based on a dual quaternion is much simpler and has good tracking control performance.

Definitions of dual number and dual quaternion

A dual number is defined as follows ²⁸

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

(1)

where a represents the real part of the dual number, b is the dual part, and $\epsilon$ is the dual unit, and the following operational rule is met

$${\epsilon }^2={\epsilon }^3={\epsilon }^4=\cdots =0$$

(2)

Quaternion is defined as follows

$$\mathit{q}=q_0+\mathbf{q}=q_0+q_1\mathit{i}+q_2\mathit{j}+q_3\mathit{k}$$

(3)

where $q_0$ is the scalar part and $\mathbf{q}=\left[q_1{q}_2{q}_3\right]$ is the vector part.

The dual quaternion can be observed as a dual number whose dual part and real part are both quaternions, as defined by

$$\hat{\mathit{q}}=\mathit{q}+\epsilon \mathit{q}\prime$$

(4)

where both $\mathit{q}$ and $\mathit{q}\prime$ are normal quaternions. The dual quaternion and quaternion have similar properties.

$\hat{\mathit{q}}$ can be written as two parts of dual number and dual vector

$$\hat{\mathit{q}}=\left[\begin{array}{c}\hat{s}\hfill \\ \multicolumn{1}{c}{\hat{\mathit{v}}}\end{array}\right]=\left[\begin{array}{c}s+\epsilon s\prime \hfill \\ \multicolumn{1}{c}{\mathit{v}+\epsilon \mathit{v}\prime }\end{array}\right]$$

(5)

According to the definition of dual quaternion, the following operation rules are satisfied

$$\{\begin{array}{c}{\hat{\mathit{q}}}_1+{\hat{\mathit{q}}}_2=\left[\begin{array}{c}{\hat{s}}_1+{\hat{s}}_2\hfill \\ \multicolumn{1}{c}{{\hat{\mathit{v}}}_1+{\hat{\mathit{v}}}_2}\end{array}\right]\hfill \\ \multicolumn{1}{c}{\lambda \hat{\mathit{q}}=\left[\begin{array}{c}\lambda \hat{s}\hfill \\ \multicolumn{1}{c}{\lambda \hat{\mathit{v}}}\end{array}\right]}\\ \multicolumn{1}{c}{{\hat{\mathit{q}}}_1°{\hat{\mathit{q}}}_2={\mathit{q}}_1°{\mathit{q}}_2+\epsilon ({\mathit{q}}_1°{\mathit{q}\prime }_2+{\mathit{q}}_2°{\mathit{q}\prime }_1)}\\ \multicolumn{1}{c}{=\left[\begin{array}{c}{\hat{s}}_1{\hat{s}}_2-{\hat{\mathit{v}}}_1·{\hat{\mathit{v}}}_2\hfill \\ \multicolumn{1}{c}{{\hat{s}}_1{\hat{\mathit{v}}}_2+{\hat{s}}_2{\hat{\mathit{v}}}_1+{\hat{\mathit{v}}}_1\times {\hat{\mathit{v}}}_2}\end{array}\right]}\end{array}$$

(6)

where the operator ° is called quaternion multiplication.

Chasles theorem shows that all rigid body motion can be achieved by rotating around a certain axis and translating along the axis. The rotation axis is called the screw axis, and the translation along the screw axis is called the pitch. According to the principle of transference, ²⁹ the unit dual quaternion can describe both the rotation and translation of the general motion.

As shown in Figure 1, the axis of rotation is a straight line with a direction $\mathit{l}$ , and passing through a point p, the unit dual quaternion can indicate that the coordinate system $o_1{x}_1{y}_1{z}_1$ moves d along the direction of the unit vector $\mathit{l}$ to the position of the coordinate system $o{\prime }_{1}x{\prime }_{1}y{\prime }_{1}z{\prime }_1$ and then rotates the angle $\theta$ around the unit vector $\mathit{l}$ to the position of the coordinate system $o_2{x}_2{y}_2{z}_2$ .

Figure 1.

Rotation and translation of coordinate system.

The dual quaternion $\hat{\mathit{q}}$ can also be written as follows

$$\hat{\mathit{q}}=(\cos \frac{\hat{\theta }}{2},\hat{\mathit{l}}\sin \frac{\hat{\theta }}{2})$$

(7)

In formula (7), $\hat{\mathit{l}}$ and $\hat{\theta }$ represent dual vector and dual angle, respectively, which expressions are as follows

$$\{\begin{array}{c}\hat{\mathit{l}}=\mathit{l}+\epsilon p\times l\hfill \\ \multicolumn{1}{c}{\hat{\theta }=\theta +\epsilon d}\end{array}$$

(8)

The general motion of a rigid body can be described by the change in the coordinate system that is attached to the rigid body. Therefore, the unit dual quaternion can be used to describe the screw motion of a rigid body around the unit vector $\mathit{l}$ . As shown in Figure 1, assuming that a rigid body is moving from the coordinate system $O_1$ to the coordinate system $O_2$ , unit dual quaternion $\hat{\mathit{q}}$ is a function of $\mathit{q}$ and ${\mathit{r}}^{O_1}$ (or $\mathit{q}$ and ${\mathit{r}}^{O_2}$ ), and the expression is as follows

$$\begin{array}{c}\hat{\mathit{q}}=\mathit{q}+\epsilon \mathit{q}\prime \hfill \\ =\mathit{q}+\epsilon \frac{1}{2}{\mathit{r}}^{O_1}°\mathit{q}\hfill \\ =\mathit{q}+\epsilon \frac{1}{2}\mathit{q}°{\mathit{r}}^{O_2}\hfill \end{array}$$

(9)

In this formula, the symbol ° represents the product of the quaternion and ${\mathit{r}}^{O_1}$ and ${\mathit{r}}^{O_2}$ represent the position quaternions of the vector r in the coordinate systems $O_1$ and $O_2$ , respectively.

Dual-quaternion-based kinematic and dynamic models

The kinematical equations of leader–follower rigid bodies are expressed as follows by the dual quaternion

$$\{\begin{array}{c}{\stackrel{·}{\hat{\mathit{q}}}}_{\mathrm{lI}}=\frac{1}{2}{\hat{\mathit{q}}}_{\mathrm{lI}}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l\hfill \\ \multicolumn{1}{c}{{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l={\mathbf{\omega }}_{\mathrm{lI}}^l+\epsilon ({\stackrel{·}{\mathbf{r}}}_{\mathrm{lI}}^l+{\mathbf{\omega }}_{\mathrm{lI}}^l\times {\mathbf{r}}_{\mathrm{lI}}^l)}\end{array}$$

(10)

$$\{\begin{array}{l}{\stackrel{.}{\hat{\mathit{q}}}}_{fI}=\frac{1}{2}{\hat{\mathit{q}}}_{fI}\circ {\hat{\mathbf{\omega }}}_{fI}^f\\ {\hat{\mathbf{\omega }}}_{fI}^f={\mathbf{\omega }}_{fI}^f+\epsilon \left({\stackrel{.}{\mathit{r}}}_{fI}^f+{\mathbf{\omega }}_{fI}^f\times {\mathbf{r}}_{fI}^f\right)\end{array}$$

(11)

In formula (10), ${\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l$ denotes the dual angular velocity of the leader rigid body relative to the inertial coordinate system, ${\mathbf{r}}_{\mathrm{lI}}^l$ denotes the position vector from the coordinate system of the leader rigid body relative to the inertial coordinate system, ${\stackrel{·}{\mathbf{r}}}_{\mathrm{lI}}^l$ is local derivative of the vector ${\mathbf{r}}_{\mathrm{lI}}^l$ , ${\mathbf{\omega }}_{\mathrm{lI}}^l$ is the angular velocity, and ${\stackrel{·}{\mathbf{r}}}_{\mathrm{lI}}^l+{\mathbf{\omega }}_{\mathrm{lI}}^l\times {\mathbf{r}}_{\mathrm{lI}}^l$ is the linear velocity. Similarly, the symbols in formula (11) are consistent with formula (10).

The dual quaternions of leader–follower rigid bodies are ${\hat{q}}_{\mathrm{lI}}$ and ${\hat{q}}_{\mathrm{fI}}$ , respectively, and the tracking error ${\hat{\mathit{q}}}_{\mathrm{fl}}$ of the follower rigid body relative to the leader rigid body is as follows

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

(12)

where ${\hat{\mathit{q}}}_{\mathrm{fl}}$ can be converted into the relative position and attitude information.

We can take the derivative of formula (12) and then substitute and simplify the quaternion kinematic equation as follows

$$\begin{array}{c}\multicolumn{1}{c}{{\stackrel{·}{\hat{\mathit{q}}}}_{\mathrm{fl}}=-\frac{1}{2}{{\hat{\mathbf{\omega }}}^l}_{\mathrm{lI}}°{\hat{\mathit{q}}}_{\mathrm{lI}}^{*}°{\hat{\mathit{q}}}_{\mathrm{fI}}+\frac{1}{2}{\hat{\mathit{q}}}_{\mathrm{lI}}^{*}°{\hat{\mathit{q}}}_{\mathrm{fI}}°{{\hat{\mathbf{\omega }}}^f}_{\mathrm{fI}}}\\ \multicolumn{1}{c}{=\frac{1}{2}{\hat{\mathit{q}}}_{\mathrm{lI}}^{*}°{\hat{\mathit{q}}}_{\mathrm{fI}}°({{\hat{\mathbf{\omega }}}^f}_{\mathrm{fI}}-{\hat{\mathit{q}}}_{\mathrm{fI}}^{*}°{\hat{\mathit{q}}}_{\mathrm{lI}}°{{\hat{\mathbf{\omega }}}^l}_{\mathrm{lI}}°{\hat{\mathit{q}}}_{\mathrm{lI}}^{*}°{\hat{\mathit{q}}}_{\mathrm{fI}})}\\ \multicolumn{1}{c}{=\frac{1}{2}{\hat{\mathit{q}}}_{\mathrm{fl}}°{\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f}\end{array}$$

(13)

where ${\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f$ represents the dual velocity of the follower rigid body with respect to the leader rigid body, which is expressed in the coordinate system of the follower rigid body. The dual velocity can be written as follows

$${\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f={\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f-{\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}}$$

(14)

where ${\hat{\mathit{q}}}_{\mathrm{fl}}^{*}$ is the conjugate of the dual quaternion ${\hat{\mathit{q}}}_{\mathrm{fl}}$ .

After taking the derivative of formula (14), we obtain formula (15) as follows

$$\begin{array}{c}\multicolumn{1}{c}{{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathrm{fl}}^f={\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathrm{fI}}^f-({\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}})\prime }\\ \multicolumn{1}{c}{={\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathrm{fI}}^f-{\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}}+{\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f\times ({\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}})}\end{array}$$

(15)

The dual inertia matrix is defined as follows ³⁰

$$\hat{\mathit{M}}=m\frac{d}{d\epsilon }{\mathit{I}}_{3\times 3}+\epsilon \mathit{J}$$

(16)

In this formula, m is the mass of follower rigid body, ${\mathit{I}}_{3\times 3}$ is the third-order unit matrix, and J is the rotational inertia of follower rigid body. The inverse ${\hat{M}}^{-1}$ of dual inertia matrix $\hat{M}$ is defined as follows

$${\hat{\mathit{M}}}^{-1}={\mathit{J}}^{-1}\frac{d}{d\epsilon }+\epsilon (1/m)\mathit{I}$$

(17)

The dual force acting on a rigid body can be written as follows

$$\hat{\mathit{F}}=\mathit{f}+\epsilon \mathbf{\tau }\mathbf{=}\hat{\mathit{M}}{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathbf{bI}}^{\mathbf{b}}\mathbf{+}{\hat{\mathbf{\omega }}}_{\mathbf{bI}}^{\mathbf{b}}\mathbf{\times }\hat{\mathit{M}}{\hat{\mathbf{\omega }}}_{\mathbf{bI}}^{\mathbf{b}}$$

(18)

where f and $\mathbf{\tau }$ are the force and torque, respectively, and ${\hat{\mathbf{\omega }}}_{\mathrm{bI}}^b$ is the dual angular velocity from the coordinate system of the rigid body relative to the inertial coordinate system.

The pose coupling dynamic models of the leader and follower rigid bodies in inertial frame are

$$\{\begin{array}{c}{\hat{M}}_l{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathbf{lI}}^{\mathbf{l}}\mathbf{+}{\mathbf{\omega }}_{\mathbf{lI}}^{\mathbf{l}}\mathbf{\times }{\hat{\mathbf{M}}}_{\mathbf{l}}{\hat{\mathbf{\omega }}}_{\mathbf{lI}}^{\mathbf{l}}\mathbf{=}{\hat{\mathbf{F}}}_{\mathbf{l}}\hfill \\ \multicolumn{1}{c}{{\hat{\mathbf{M}}}_{\mathbf{f}}{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathbf{fI}}^{\mathbf{f}}\mathbf{+}{\hat{\mathbf{\omega }}}_{\mathbf{fI}}^{\mathbf{f}}\mathbf{\times }{\hat{\mathbf{M}}}_{\mathbf{f}}{\hat{\mathbf{\omega }}}_{\mathbf{fI}}^{\mathbf{f}}\mathbf{=}{\hat{\mathbf{F}}}_{\mathbf{f}}}\end{array}$$

(19)

where ${\hat{\mathit{F}}}_l$ and ${\hat{\mathit{F}}}_f$ are the dual force acting on the leader and follower rigid bodies.

The following expression can be obtained by formula (19)

$$\{\begin{array}{c}{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathbf{lI}}^{\mathbf{l}}\mathbf{=}-{{\hat{\mathbf{M}}}_{\mathbf{l}}}^{-1}\mathbf{(}{\mathbf{\omega }}_{\mathbf{lI}}^{\mathbf{l}}\mathbf{\times }{\hat{\mathbf{M}}}_{\mathbf{l}}{\hat{\mathbf{\omega }}}_{\mathbf{lI}}^{\mathbf{l}}\mathbf{)}\mathbf{+}{{\hat{\mathbf{M}}}_{\mathbf{l}}}^{-1}\hat{\mathit{F}}\hfill \\ \multicolumn{1}{c}{{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathbf{fI}}^{\mathbf{f}}\mathbf{=}-{\hat{\mathbf{M}}}_{\mathbf{f}}^{-1}\mathbf{(}{\mathbf{\omega }}_{\mathbf{fI}}^{\mathbf{f}}\mathbf{\times }{\hat{\mathbf{M}}}_{\mathbf{f}}{\hat{\mathbf{\omega }}}_{\mathbf{fI}}^{\mathbf{f}}\mathbf{)}\mathbf{+}{{\hat{\mathbf{M}}}_{\mathbf{f}}}^{-1}\hat{\mathit{F}}}\end{array}$$

(20)

Substituting formula (20) into formula (15), we can obtain formula (21) as follows

$$\begin{array}{c}\multicolumn{1}{c}{{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathrm{fl}}^f=-{\hat{M}}_f^{-1}({\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f\times {\hat{M}}_f{\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f)+{\hat{M}}_f^{-1}{\hat{F}}_f-{\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}}}\\ \multicolumn{1}{c}{+{\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f\times ({\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}})}\end{array}$$

(21)

Given in the final stage, the dual velocity satisfies ${\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathrm{lI}}^l=0$ , and the above formula can be simplified to formula (22) as follows

$${\hat{F}}_f={\hat{M}}_f{\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathrm{fl}}^f+{\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f\times {\hat{M}}_f{\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f-{\hat{M}}_f[{\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f\times ({\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}})]$$

(22)

Formula (22) is the relative coupling dynamic model, where ${\hat{M}}_f$ is the dual inertia matrix of the follower rigid body and ${\hat{F}}_f$ is the dual force acting on a follower rigid body.

Sliding mode variable control law design

In the traditional sliding mode control law, Lyapunov’s stability theory is an important tool; the Lyapunov function is usually adopted as follows

$$\mathit{V}=\frac{1}{2}{\mathit{s}}^T\mathit{s}$$

(23)

where s is called sliding mode switching function. For control purposes, the function used to search for a suitable control law to make

$$\stackrel{·}{\mathit{V}}<0$$

(24)

refers to a reachable condition. From the foregoing, we can come to the conclusion that the two main problems of variable structure control are choosing suitable switching functions and determining the sliding mode control laws.

In leader–follower rigid body control, to make relative positions and attitudes between leader–follower rigid bodies reach desired values. The desired values of angular velocity and attitude are constant with respect to time. Therefore, a state variable is selected as follows

$$\hat{e}=\left[\begin{array}{c}{\hat{\mathit{q}}}_e\hfill \\ \multicolumn{1}{c}{{\hat{\mathbf{\omega }}}_e}\end{array}\right]=\left[\begin{array}{c}{({\hat{\mathit{q}}}_{\mathrm{fl},d}^{*}°{\hat{\mathit{q}}}_{\mathrm{fl}})}_v\hfill \\ \multicolumn{1}{c}{{\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f-{\hat{\mathbf{\omega }}}_{\mathrm{fl},d}^f}\end{array}\right]$$

(25)

where $\hat{e}$ represents the error information between true value and expected value of the relative position and attitude; ${\hat{\mathit{q}}}_{\mathrm{fl},d}$ represents the expected value of the dual quaternion; and ${\hat{\mathit{q}}}_{\mathrm{fl},d}^{*}$ is the conjugate of ${\hat{\mathit{q}}}_{\mathrm{fl},d}$ . ${\hat{\mathit{q}}}_e$ represents the error in the dual quaternion between true value and expected value; ${\left(\right)}_v$ refers to the vector part; ${\hat{\mathbf{\omega }}}_e$ represents the error in dual velocity, that is, the error information between the true value and expected value of the attitude angular velocity and linear velocity; and ${\hat{\mathbf{\omega }}}_{\mathrm{fl},d}^f$ represents the expected dual velocity value.

The switching function is defined as follows

$$\hat{\mathit{s}}={\hat{\mathbf{\omega }}}_e+k{\hat{\mathit{q}}}_e$$

(26)

In this formula, k is the positive definite constant diagonal matrix, that is $k=\mathrm{diag}\{k_1,k_2,k_3,k_4,k_5,k_6\}$ , $k>0$ .

The control law is designed to control the rigid body on the sliding mode vector, $\hat{s}=\hat{0}$ , and the following formula is acquired

$${\hat{\mathbf{\omega }}}_e=-k{\hat{\mathit{q}}}_e$$

(27)

Multiplying both sides by $(1/2){\hat{\mathit{q}}}_e$ of equation (27), we can obtain formula (28) as follows

$$\frac{1}{2}{\hat{\mathit{q}}}_e°{\hat{\mathbf{\omega }}}_e=-\frac{1}{2}{\hat{\mathit{q}}}_{e}k{\hat{\mathit{q}}}_e$$

(28)

So

$${\stackrel{·}{\hat{\mathit{q}}}}_e=-\frac{1}{2}{\hat{\mathit{q}}}_{e}k{\hat{\mathit{q}}}_e$$

(29)

Let us suppose the Lyapunov function is

$${\mathit{V}}_e=\frac{1}{2}{\hat{\mathit{q}}}_e^{T}k{\hat{\mathit{q}}}_e⩾0$$

(30)

We can take the derivative of formula (30), and then

$${\stackrel{·}{\mathit{V}}}_e={\hat{\mathit{q}}}_e^{T}k{\stackrel{·}{\hat{\mathit{q}}}}_e$$

(31)

Substituting formula (29) into formula (31), we can obtain formula (32) as follows

$${\stackrel{·}{\mathit{V}}}_e=-\frac{1}{2}{\hat{\mathit{q}}}_e^{T}k{\hat{\mathit{q}}}_{e}k{\hat{\mathit{q}}}_e=-\frac{1}{2}{\hat{\mathit{q}}}_e^T\Vert k{\hat{\mathit{q}}}_e{\Vert }^2$$

(32)

We can see from formula (32) that ${\stackrel{·}{\mathit{V}}}_e⩽0$ , and an equal sign is taken if and only if ${\hat{\mathit{q}}}_e=\hat{0}$ . In addition, it must satisfy the sliding mode reaching condition as follows

$$\hat{\mathit{s}}·\stackrel{·}{\hat{\mathit{s}}}<0$$

(33)

where $\stackrel{·}{\hat{\mathit{s}}}$ is the derivative of the switching function $\hat{\mathit{s}}$

$$\stackrel{·}{\hat{\mathit{s}}}={\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathbf{e}}+k{\stackrel{·}{\hat{\mathit{q}}}}_e={\stackrel{·}{\hat{\mathbf{\omega }}}}_{\mathbf{fl}}^{\mathbf{f}}\mathbf{+}\mathbf{k}{\stackrel{·}{\hat{\mathit{q}}}}_{\mathbf{e}}$$

(34)

Substituting formula (22) into formula (34), we can obtain formula (35) as follows

$$\stackrel{.}{\hat{s}}=-{\hat{M}}_f^{-1}\left({\hat{\mathbf{\omega }}}_{fI}^f\times {\hat{M}}_f{\hat{\mathbf{\omega }}}_{fI}^f\right)+{\hat{\mathbf{\omega }}}_{fl}^f\times \left({\hat{\mathit{q}}}_{fl}^{\ast }\circ {\hat{\mathbf{\omega }}}_{lI}^l\circ {\hat{\mathit{q}}}_{fl}\right)+M_f^{-1}{\hat{F}}_f+k{\stackrel{.}{\hat{q}}}_e$$

(35)

Formula (35) is expressed as a reaching condition of the Lyapunov function, that is, a control law is designed to guarantee that system states move to the sliding mode within a limited time. Let us suppose the Lyapunov function is

$${\mathit{V}}_s=\frac{1}{2}{\hat{\mathit{s}}}^T\hat{M}\hat{\mathit{s}}$$

(36)

Since $\hat{M}$ is a symmetrical positive determined matrix, ${\mathit{V}}_s⩾0$ is tenable for any $\hat{\mathit{s}}$ , and if and only if $\hat{\mathit{s}}=\hat{0}$ , there is ${\mathit{V}}_s=0$ . The Lyapunov function takes the derivative of the time t and the following formula is acquired

$$\begin{array}{c}\multicolumn{1}{c}{{\stackrel{·}{\mathit{V}}}_s={\hat{\mathit{s}}}^T\hat{M}\stackrel{·}{\hat{\mathit{s}}}={\hat{\mathit{s}}}^T\hat{M}({\stackrel{·}{\hat{\mathbf{\omega }}}}_e+k{\stackrel{·}{\hat{\mathit{q}}}}_e)={\hat{\mathit{s}}}^T(\hat{M}{\stackrel{·}{\hat{\mathbf{\omega }}}}_e+k\hat{M}{\stackrel{·}{\hat{\mathit{q}}}}_e)}\\ \multicolumn{1}{c}{={\hat{\mathit{s}}}^T[-({\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f\times {\hat{M}}_f{\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f)+\hat{M}[{\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f\times ({\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}})]}\\ \multicolumn{1}{c}{+{\hat{F}}_f+k\hat{M}{\stackrel{·}{\hat{\mathit{q}}}}_e]}\end{array}$$

(37)

Generally, when designing a control law for an uncertain system, we can add the equivalent control and the switching control, that is

$$\hat{\mathit{u}}={\hat{\mathit{u}}}_{\mathrm{eq}}+{\hat{\mathit{u}}}_{\mathrm{vss}}$$

(38)

In formula (38), ${\hat{\mathit{u}}}_{\mathrm{vss}}$ is a switching control, which is used for robust control of uncertainty and interference of a system, and ${\hat{\mathit{u}}}_{\mathrm{eq}}$ is the equivalent control, which should meet the condition: $\stackrel{·}{\hat{\mathit{s}}}=0$ . Then

$${\hat{\mathit{u}}}_{\mathrm{eq}}=({\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f\times {\hat{M}}_f{\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f)-{\hat{M}}_f[{\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f\times ({\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}})]-k{\hat{M}}_f{\stackrel{·}{\hat{\mathit{q}}}}_e$$

(39)

Suppose ${\hat{\mathit{u}}}_{\mathrm{vss}}=-\alpha \mathrm{sgn}(\hat{\mathit{s}})-\beta \hat{\mathit{s}}$ , where $\mathrm{sgn}(\hat{\mathit{s}})$ is the sign function and $\alpha$ and $\beta$ are the positive definite diagonal matrix. ${\hat{\mathit{u}}}_{\mathrm{vss}}$ contains the term $\beta \hat{\mathit{s}}$ , which can reduce chattering arising from the variable structure. Therefore, the control law is designed as follows

$$\begin{array}{c}\multicolumn{1}{c}{\hat{\mathit{u}}=({\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f\times {\hat{M}}_f{\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f)-{\hat{M}}_f[{\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f\times ({\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}})]}\\ \multicolumn{1}{c}{-k{\hat{M}}_f{\stackrel{·}{\hat{\mathit{q}}}}_e-\alpha \mathrm{sgn}(\hat{s})-\beta \hat{\mathit{s}}}\end{array}$$

(40)

Substituting formula (40) into formula (37), we can obtain formula (41) as follows

$${\stackrel{·}{\mathit{V}}}_s=-{\hat{s}}^T{\hat{M}}_f(\alpha \mathrm{sgn}(\hat{\mathit{s}})+\beta \hat{\mathit{s}})$$

(41)

We can see from formula (41) that when $\hat{\mathit{s}}=0$ , ${\stackrel{·}{\mathit{V}}}_s=0$ and when $\hat{\mathit{s}}\ne 0$ , ${\stackrel{·}{\mathit{V}}}_s<0$ .

Barbalat’s lemma is used to prove that the system state can converge to the expected value. Because ${\mathit{V}}_s⩾0$ , the differentiable function ${\mathit{V}}_s$ has a lower bound, and ${\stackrel{·}{\mathit{V}}}_s⩽0$ is tenable and $\underset{t\to \infty }{lim}{\mathit{V}}_s(t)$ exists and is bounded. In addition, the following formula is tenable, that is

$$\begin{array}{c}\multicolumn{1}{c}{\underset{t\to \infty }{lim}\underset{0}{\overset{t}{\int }}{\stackrel{·}{\mathit{V}}}_s(t)d\tau =-\frac{1}{2}{\hat{M}}_f\underset{t\to \infty }{lim}\underset{0}{\overset{t}{\int }}{\hat{\mathit{s}}}^T(\alpha \mathrm{sgn}(\hat{\mathit{s}})+\beta \hat{\mathit{s}})d\tau }\\ \multicolumn{1}{c}{=\underset{t\to \infty }{lim}{V}_s(t)-V_s(0)}\end{array}$$

(42)

The following formula is acquired

$$0⩽\frac{1}{2}{\hat{M}}_f\underset{t\to \infty }{lim}\underset{0}{\overset{t}{\int }}{\hat{\mathit{s}}}^T(\alpha \mathrm{sgn}(\hat{\mathit{s}})+\beta \hat{\mathit{s}})d\tau ⩽V_s(0)$$

(43)

Then

$$\underset{t\to \infty }{lim}\hat{\mathit{s}}=\hat{0}$$

(44)

It is obvious that the reachable condition of sliding mode is satisfied and the constructed closed-loop system is asymptotically stable, thus system states ${\hat{\mathbf{\omega }}}_e$ and ${\hat{\mathit{q}}}_e$ can reach the sliding mode surface $\hat{\mathit{s}}$ within a limited time.

The Lyapunov function ${\mathit{V}}_e⩾0$ , and its derivative ${\stackrel{·}{\mathit{V}}}_e⩽0$ , is tenable, so $\underset{t\to \infty }{lim}{\mathit{V}}_e(t)$ exists and it is the finite value; the following formula is acquired

$$\begin{array}{c}\multicolumn{1}{c}{\underset{t\to \infty }{lim}\underset{0}{\overset{t}{\int }}{\stackrel{·}{\mathit{V}}}_e(t)d\tau =-\frac{1}{2}\underset{t\to \infty }{lim}\underset{0}{\overset{t}{\int }}{\hat{\mathit{q}}}_e^T{\Vert k{\hat{\mathit{q}}}_e\Vert }^{2}d\tau }\\ \multicolumn{1}{c}{=\underset{t\to \infty }{lim}{V}_e(t)-V_e(0)}\end{array}$$

(45)

Since $k>0$ , the above equation is tenable if and only if $\underset{t\to \infty }{lim}{\hat{\mathit{q}}}_e^T=\hat{0}$ , that is ${\hat{\mathit{q}}}_e=\hat{0}$ . In addition, formula (44) can be written as follows

$$\underset{t\to \infty }{lim}\hat{\mathit{s}}=\underset{t\to \infty }{lim}({\hat{\mathbf{\omega }}}_e+k{\hat{\mathit{q}}}_e)=\hat{0}$$

(46)

Then, the following formula is acquired

$$\underset{t\to \infty }{lim}{\hat{\mathbf{\omega }}}_e=\hat{0}$$

(47)

When the condition $t\to \infty$ , the closed-loop system can converge to the desired value ${\hat{\mathit{q}}}_e=\hat{0}$ and ${\hat{\mathbf{\omega }}}_e=\hat{0}$ .

Simulation and discussion

To confirm the validity of the algorithm and its derivation, MATLAB (The MathWorks, Inc.) was employed to simulate and calculate the relative pose control accuracy of leader–follower rigid bodies. Let us suppose that the mass of follower rigid body is $m=20\mathrm{kg}$ , the moment of inertia is $\mathbf{J}=5{\mathbf{I}}_{3\times 3}$ , and the initial conditions are considered in the simulations, which are given as follows: $q_{\mathrm{fl}}^f(0)=(0.8005-0.1618-0.38550.4294{)}^T$ , $r_{\mathrm{fl}}^f(0)=(5050-50{)}^{T}m$ . Furthermore, the parameters are k = diag{0.1,0.1, 0.1,0.1,0.1,0.1}, $\alpha =diag\{0.02,0.02,0.02,0.02,0.02,0.02\}$ , and $\beta =\mathrm{diag}\{0.3,0.3,0.3,0.3,0.3,0.3\}$ .

To illustrate the performance of the proposed control algorithm, a proportional–derivative (PD) control algorithm based on dual quaternion was adopted in the simulation to compare with the proposed algorithm. We design the PD control law as follows

$$\begin{array}{c}\multicolumn{1}{c}{\hat{\mathit{u}}=-2k_p\ln {\hat{q}}_e-k_d{\hat{\mathbf{\omega }}}_e-\hat{f}}\\ \multicolumn{1}{c}{=-2k_p\ln {\hat{q}}_e-k_d{\hat{\mathbf{\omega }}}_e+{\hat{M}}_f^{-1}({\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f\times {\hat{M}}_f{\hat{\mathbf{\omega }}}_{\mathrm{fI}}^f)}\\ \multicolumn{1}{c}{-{\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f\times ({\hat{\mathit{q}}}_{\mathrm{fl}}^{*}°{\hat{\mathbf{\omega }}}_{\mathrm{lI}}^l°{\hat{\mathit{q}}}_{\mathrm{fl}})}\end{array}$$

where $k_p$ and $k_d$ are the control gain matrices of the PD controller and the parameters $k_p=\{0.5,0.5,0.5,0.1,0.1,0.1\}$ and $k_d=\{3,3,3,0.2,0.2,0.2\}$ . Furthermore, the initial value of the velocity is ${\hat{\mathbf{\omega }}}_{\mathrm{fl}}^f(0)=\hat{0}$ , the expected relative attitude is ${\mathit{q}}_{\mathrm{fl},d}={\left(\begin{array}{cccc}1\hfill & 0\hfill & 0\hfill & 0\hfill \end{array}\right)}^{\mathrm{T}}$ , and the simulation time $t=200\mathrm{s}$ .

Figures 2 and 3 are the curves for the relative position and velocity by using PD control algorithm and the proposed algorithm separately. By comparing Figures 2 and 3, the result suggests that the relative position and velocity of PD control algorithm converge to near zero at approximately 80 s, while relative position and velocity of the proposed algorithm are slightly faster than those of the PD control algorithm and about 40 s converge to near zero. Figures 4 and 5 are the curves for the relative attitude and angular velocity by using PD control algorithm and the proposed algorithm separately. By comparing Figures 4 and 5, the result suggests that the relative attitude and angular velocity of PD control algorithm converges to near zero at approximately 40 s, while relative attitude and angular velocity of the proposed algorithm are slightly faster than those of the PD control algorithm and about 20 s converge to near zero, which is similar to the relative position curve. It is observed from the curves for relative position and velocity that the convergence time for relative position and velocity of follower rigid body is slightly slower than that of the relative attitude.

Figure 2.

Curves for the relative position and velocity of PD control algorithm.

Figure 3.

Curves for the relative position and velocity of the proposed algorithm.

Figure 4.

Curves for the relative attitude and angular velocity of PD control algorithm.

Figure 5.

Curves for the relative attitude and angular velocity of the proposed algorithm.

It can be seen from the above results that if the parameters are designed reasonably, both the PD control algorithm and the proposed control algorithm can realize the coupling control of position and attitude and finally reach the given ideal state, but the PD control has small amplitude oscillation and the stability period is longer. The proposed algorithm can compensate for the shortcomings of PD control, shorten the stability period, and make the control process more stable. The simulation results show that the algorithm not only achieves unified control of relative position and attitude but also has good tracking control performance.

Conclusion

In this article, employing dual-number and dual-quaternion theory framework, the relative kinematic and dynamic model of leader–follower rigid body is established. On the basis of the coupled kinematic and dynamic model, a novel coordinated control law is designed to solve unified control of relative position and attitude of leader–follower rigid body. The proposed algorithm is proved to be able to make the relative position and attitude reach to the desired value in finite time. The simulation results show that the algorithm not only achieves unified control of relative position and attitude but also has a faster convergence time and good tracking control performance and verifies the scientific rationality and calculation quickness of the algorithm. In this article, we mainly discuss the design of the relative pose control law of the leader–follower rigid body, and in the following study, we need to further research on pose control law design of multi-rigid-body system.