Estimation and Control of Cooperative Aerial Manipulators for a Payload with an Arbitrary Center-of-Mass

Abstract

This paper presents an integrated framework that integrates the kinematic and dynamic parameter estimation of an irregular object with non-uniform mass distribution for cooperative aerial manipulators. Unlike existing approaches, including impedance-based control which requires expensive force/torque sensors or the first-order-momentum-based estimator which is weak to noise, this paper suggests a method without such sensor and strong to noise by exploiting the decentralized dynamics and sliding-mode-momentum observer. First, the kinematic estimator estimates the relative distances of multiple aerial manipulators by using translational and angular velocities between aerial robots. By exploiting the distance estimation, the desired trajectories for each aerial manipulator are set. Second, the dynamic parameter estimation is performed for the mass of the common object and the vector between the end-effector frame and the center of mass of the object. Finally, the proposed framework is validated with simulations using aerial manipulators combined with two degrees-of-freedom robotic arms using a noisy measurement. Throughout the simulation, we can decrease the mass estimation error by 60% compared to the existing first-order momentum-based method. In addition, a comparison study shows that the proposed method satisfactorily estimates an arbitrary center-of-mass of an unknown payload in noisy environments.

1. Introduction

Multirotor UAVs (unmanned aerial vehicles), which are highly maneuverable and can be made small, are gaining popularity as a major air transportation platform [1]. Among them, cooperative UAVs are widely exploited to handle a heavy or large payload [2] beyond the limits of a robot’s transportation capabilities. Recently, researchers have developed cooperative mobile manipulators [2,3,4,5,6,7,8,9] by exploiting grasping capability. However, due to several issues including the complexity associated with multiple aerial robots, they have focused on solving a control and coordination problem. To make the problem simple, they have assumed that a relative distance from the payload frame can be easily calculated because the regular payload has a uniform mass distribution [2,4,5,6,7,8,9]. For these reasons, cooperative aerial manipulations to handle a payload with the non-uniform mass distribution (i.e., the geometry of the payload and center of mass (COM) of the payload are different) are quite complicated.

1.1. Contribution

This paper is interested in resolving the aforementioned problem of handling a payload with an arbitrary center-of-mass for cooperative aerial manipulators. The contributions of this paper can be summarized as follows. First, we propose a kinematic estimation algorithm for unknown kinematics of the payload and the trajectory generation algorithm using the estimated kinematic parameter. The existing method in [10] estimates the relative kinematic parameter between each end-effector. However, the proposed algorithm can estimate not only the kinematic parameters, but also the the coordinates of COM of the payload, which is crucial for handling a payload with an arbitrary COM. By exploiting the distance estimation, the desired trajectories for each aerial manipulator are set. Second, we propose a dynamic parameter estimation for the mass and COM of a payload by using the external wrench estimation algorithm. While existing approaches require a force/torque sensor [11] or a vision sensor [12], the proposed algorithm does not require such sensors. Unlike the existing estimation algorithms for estimating unknown kinematic and dynamic parameters [9,13], the proposed algorithm does not require an assumption of uniform mass distribution or contact-force control. In addition, the proposed algorithm shows better estimation performance of dynamic parameters even with a noisy measurement than the first-order momentum-based approach [14,15,16] or our previous research [4]. Finally, the numerical simulation results show that the feasibility of the proposed algorithm is capable of allowing cooperative aerial manipulators to jointly carry a payload with an arbitrary center-of-mass.

1.2. Related Works

Early works on the cooperative aerial manipulation assumed that the dynamic and kinematic information of the payload is known [2]. They designed a fully centralized controller and planning algorithm with a known payload model. Unlike these centralized approaches, in [11], the decentralized impedance controller was applied for cooperative aerial manipulators. The mass and COM of the payload were estimated by the external wrench, which is measured by the force-torque sensor. In [17], the authors designed an optimization-based force decomposition algorithm and decentralized force controller with a payload stiffness model. However, these methods require expensive force-torque sensors, which are difficult to be equipped in aerial robots because of a payload limitation. Contact force estimation algorithm for dual-arm aerial manipulator was developed using the Cartesian deflection of the end-effector measured by a stereo vision sensor in [12]. Although the method shows precise estimation performance of the payload mass, the stereo camera always watches the end-effector for the deflection measurement.

Another approach is to design a controller that is robust/adaptive to parameter changes in the kinematic or dynamic payload model [4,6,7,8,18]. A robust optimal control is designed for dual arm manipulator in [18]. In [4,8], decentralized adaptive controllers were designed based on a consensus algorithm to ensure that the estimated mass from every agent gets an equal share of the mass of the payload. In [6], the robust controller with a disturbance observer is used for each aerial manipulator to carry a common payload. In [7], the effects of parametric uncertainties for each aerial manipulator were removed by designing a robust controller without using explicit communication between agents. While these methods [4,6,7,8] can easily handle the mass changes of the payload, the kinematic parameters should be known.

Kinematic parameter estimation for cooperative manipulations is used for ground manipulators [10] or human–robot interaction [19]. As the exact position of human or other manipulator is difficult to be identified in both applications, the kinematic parameter estimation is essential to complete the cooperative tasks. In [10], the relative kinematics between the robotic end-effectors can be identified based on the motion signals of the end-effector. In [19], the unknown kinematic and dynamic model of a payload are estimated for the human–robot manipulation tasks. For the aerial robots, this relative position may easily be estimated when they have an accurate global positioning sensor (GPS). However, the GPS sensor cannot be applied to GPS-denied environments. To resolve this issue, in [20], the relative position and orientation for rigidly-attached quadrotors were estimated without using an external force-torque sensor. However, this method only estimates kinematic parameters including the relative length and orientation of each quadrotor. Therefore, it is hard to apply for cooperative aerial manipulators which carry a payload with an arbitrary COM.

There exists some research for handling both the dynamic and kinematic uncertainties [9,13]. In [13], a distributed algorithm for estimating the kinematic and dynamic parameters of a common payload was proposed for cooperative ground mobile manipulators. However, this method assumes that each ground mobile manipulator can control the contact force, which is difficult to use for the aerial robots. In addition, they do not perform estimation and control simultaneously. In [9], they developed a two-stage approach: at the first stage, each robot estimates the payload kinematic and dynamic parameters using external wrenches, while, in the second stage, the estimated parameters are used for a distributed cooperative controller. Although the proposed algorithm can handle a payload with a arbitrary COM, this method [9] is only applicable to a payload with a uniform mass distribution.

2. Dynamics

It is necessary to analyze the dynamics of aerial manipulators when they transport a payload with an arbitrary COM as shown in Figure 1. In this figure, the distance ${\mathit{\rho }}_j^i$ is the estimated relative distance to j-th end-effector by i-th aerial manipulator and ${\mathit{\tau }}_i^b$ is the external wrench. In general, we will use bold letters (e.g., ${\mathit{\tau }}_i^b$) to indicate vector quantities and the upper manuscript (e.g., $e,b$ etc.) to indicate the frame.

The dynamics for cooperative aerial manipulators is addressed. Figure 1 shows that two cooperative aerial manipulators transport a payload with non-uniform mass distribution. The coordination ${\Sigma }_{e,i},{\Sigma }_{b,i}$, and ${\Sigma }_o$ are the end-effector frame, the body frame of each aerial manipulator, and the common payload frame, respectively. For better understanding, the definition of the used terms in this paper is summarized in

Table 1. The major terms used in this paper.

Term	Definition
${\mathbf{q}}_i\in {\mathbb{R}}^8$	a state vector for i-th aerial manipulator
${\mathit{\tau }}_i\in {\mathbb{R}}^8$	a control input
${\mathit{\lambda }}_i\in {\mathbb{R}}^8$	an applied force at the i-th end-effector
${\mathit{\tau }}_i^b\in {\mathbb{R}}^8$	an external wrench applied to ${\Sigma }_{b,i}$
$c_i\in \mathbb{R}$	a force balance term
${\mathbf{v}}_i^e\in {\mathbb{R}}^3$	a linear velocity at ${\Sigma }_{e,i}$
${\mathit{\omega }}_i^e\in {\mathbb{R}}^3$	an angular velocity at ${\Sigma }_{e,i}$
$m_o\in \mathbb{R}$	a mass of a payload
${\psi }_j^i\in \mathbb{R}$	a relative heading angle between ${\Sigma }_{b,i}$ and ${\Sigma }_{b,j}$
${\mathbf{r}}_i\in {\mathbb{R}}^3$	a vector from ${\Sigma }_o$ to ${\Sigma }_{e,i}$
$E_i\in {\mathbb{R}}^{6\times 6}$	a grasp matrix
${\mathbf{r}}_g\in {\mathbb{R}}^3$	the geometric centroid of the payload wr.t. the leader robot
${\mathit{\rho }}_j^i\in {\mathbb{R}}^3$	a relative distance from ${\Sigma }_{e,i}$ to ${\Sigma }_{e,j}$
${\mathbf{p}}_i\in {\mathbb{R}}^8$	a generalized momentum
${\mathit{\sigma }}_i\in {\mathbb{R}}^8$	an estimated external wrench
$o_{x,y}\in \mathbb{R}$	a vector from the geometric centroid to ${\Sigma }_o$

.

2.1. Aerial Manipulator and Payload Dynamics

Using the position of the COM of the multirotor in ${\Sigma }_I$, we define the states of the i-th aerial manipulator ${\mathbf{q}}_i\in {\mathbb{R}}^{6+n}$ ($i=1,...,N)$ in terms of the position of the multirotor UAV, Euler angle of the multirotor, and joint angles of a robotic arm. Here, $n=2$ is the number of degrees of freedom (DOFs) of a robotic arm and N represents the total number of aerial manipulators.

While carrying a common payload, the dynamics of the i-th the aerial manipulator can be obtained as

$$\begin{array}{c}\hfill M_i\left({\mathbf{q}}_i\right){\ddot{\mathbf{q}}}_i+Q_i({\mathbf{q}}_i,{\dot{\mathbf{q}}}_i){\dot{\mathbf{q}}}_i+W_i\left({\mathbf{q}}_i\right)={\mathit{\tau }}_i+{\mathit{\tau }}_i^b,\end{array}$$

(1)

where $M_i\in {\mathbb{R}}^{8\times 8}$ is the inertia matrix, $Q_i\in {\mathbb{R}}^{8\times 8}$ is the Coriolis matrix, and $W_i\in {\mathbb{R}}^8$ is the gravity term. ${\mathit{\tau }}_i$ is the control input in the inertial frame ${\Sigma }_I$ and ${\mathit{\tau }}_i^b$ is the external wrench. In this study, we can set

$$\begin{array}{c}\hfill {\mathit{\tau }}_i^b=-J_i^T\left({\mathbf{q}}_i\right){\mathit{\lambda }}_i\end{array}$$

(2)

where ${\mathit{\lambda }}_i\in {\mathbb{R}}^6$ is the wrench applied to the end-effector of the i-th aerial manipulator in ${\Sigma }_{e,i}$. $J_i\left({\mathbf{q}}_i\right)\in {\mathbb{R}}^{6\times 8}$ means the Jacobian matrix from ${\Sigma }_{b,i}$ to ${\Sigma }_{e,i}$.

Then, the dynamics of a common payload can be defined as

$$\begin{array}{c}\hfill H_o{\ddot{\mathbf{q}}}_o+{\mu }_o{\dot{\mathbf{q}}}_o+G_o={\sum }_{i=1}^N{E}_i{\mathit{\lambda }}_i,\end{array}$$

(3)

where ${\dot{\mathbf{q}}}_o\in {\mathbb{R}}^6$ is the state of a payload. The state consists of a six-dimensional vector including the translational velocity and rotational velocity of the payload in ${\Sigma }_I$. We use $H_o$ = diag$(m_o{\mathrm{I}}_3,{\mathrm{J}}_o)\in {\mathbb{R}}^{6\times 6}$, where $m_o$ is the mass of the payload, ${\mathrm{J}}_o\in {\mathbb{R}}^{3\times 3}$ is the inertia, and ${\mathrm{I}}_3$ is $3\times 3$ identity matrix. The parameter ${\mu }_o\in {\mathbb{R}}^{6\times 6}$ is a matrix containing centripetal terms and the Coriolis. $G_o\in {\mathbb{R}}^6$ is a gravity matrix. $E_i\in {\mathbb{R}}^{6\times 6}$ is a grasp matrix and is given by

$$\begin{array}{c}\hfill E_i=\left[\begin{array}{cc}{\mathrm{I}}_3& 0_3\\ S\left({\mathbf{r}}_i\right)& {\mathrm{I}}_3\end{array}\right],\end{array}$$

(4)

where $0_3$ is a $3\times 3$ zero matrix and $S\left({\mathbf{r}}_i\right)$ is the skew-symmetric matrix expressing the cross-product from the position of ${\Sigma }_o$ for ${\Sigma }_{e,i}$.

2.2. Combined Dynamics

Using the assumption of a rigid grasp, the wrench ${\mathit{\lambda }}_i$ can be computed using the force distribution solution [21] as

$$\begin{array}{c}\hfill {\mathit{\lambda }}_i=c_i{E}_i^{†}\left(H_o{\ddot{\mathbf{q}}}_o+{\mu }_o{\dot{\mathbf{q}}}_o+G_o\right),\end{array}$$

(5)

where $c_i$ is the weight satisfying ${\sum }_{i=1}^N$ and $E_i^{†}$ can be obtained using the Moore–Penrose pseudo-inverse solution as

$$\begin{array}{c}\hfill c_i{E}_i^{†}=\left[\begin{array}{cc}{c}_i{\mathrm{I}}_3& -c_{i}S\left({\mathbf{r}}_i\right){\mathrm{\Pi }}^{-1}\\ 0_3& c_i{\mathrm{\Pi }}^{-1}\end{array}\right],\end{array}$$

(6)

with $\mathrm{\Pi }={\mathrm{I}}_3+{\sum }_{i=1}^N{c}_{i}S\left({\mathbf{r}}_i\right)S^T\left({\mathbf{r}}_i\right)$. To compute $\mathrm{\Pi }$, the position of all end-effector with respect to ${\mathrm{\Sigma }}_o$ is required. In our previous work [22], we set $c_i=1/N$ and ${\mathbf{r}}_i$ is computed based on the assumption of uniform mass distribution. However, in this paper, we estimate ${\mathbf{r}}_i$ using the relative distance $\mathit{\rho }$ and the estimated force applied to the end-effector. Using the estimated ${\mathbf{r}}_i$, the weight value $c_i$ can be calculated.

As the manipulator grasps the paylod rigidly, all positions and orientations (i.e., ${\mathbf{q}}_o$) of the payload and the end-effectors can be expressed using the state variable of the aerial manipulator itself (i.e., ${\mathbf{q}}_i$). We can obtain the following equation as

$$\begin{array}{cc}\hfill {\dot{\mathbf{q}}}_o& =E_i^{-T}{J}_i{\dot{\mathbf{q}}}_i.\hfill \end{array}$$

(7)

By substituting (5) and (7) into (1), we can obtain the decentralized motion equation for the i-th aerial manipulator as

$$\begin{array}{c}\hfill D_i\left({\mathbf{q}}_i\right){\ddot{\mathbf{q}}}_i+C_i({\mathbf{q}}_i,{\dot{\mathbf{q}}}_i){\dot{\mathbf{q}}}_i+G_i\left({\mathbf{q}}_i\right)={\mathit{\tau }}_i.\end{array}$$

(8)

Here, the matrices are calculated as

$$\begin{array}{cc}\hfill D_i& =M_i\left({\mathbf{q}}_i\right)+c_i{M}_o\left({\mathbf{q}}_i\right),G_i=W_i+c_i{W}_o\left({\mathbf{q}}_i\right)\hfill \\ \hfill C_i& =Q_i({\mathbf{q}}_i,{\dot{\mathbf{q}}}_i)+c_i{Q}_o({\mathbf{q}}_i,{\dot{\mathbf{q}}}_i)+c_i{J}_i^T\left(E_i^{†}{H}_o{E}_i^{-T}\right){\dot{J}}_i\hfill \end{array}$$

(9)

where the following representations hold

$$\begin{array}{cc}\hfill M_o\left({\mathbf{q}}_i\right)& =J_i^T\left(E_i^{†}{H}_o{E}_i^{-T}\right)J_i\in {\mathbb{R}}^{8\times 8},\hfill \\ \hfill Q_o({\mathbf{q}}_i,{\dot{\mathbf{q}}}_i)& =J_i^T\left(E_i^{†}{\mu }_o{E}_i^{-T}\right)J_i\in {\mathbb{R}}^{8\times 8},\hfill \\ \hfill W_o\left({\mathbf{q}}_i\right)& =J_i^T{E}_i^{†}{G}_o\in {\mathbb{R}}^8.\hfill \end{array}$$

(10)

The detailed process for (10) are described in the previous work of this research [22]. Thanks to (8), we can control the aerial manipulator separately.

3. Kinematic Parameter Estimation and Path Planning

In this section, we address the kinematic parameter estimation and coordination algorithm for cooperative aerial manipulations. The overall structure is shown in Figure 2. The distance ${\mathit{\rho }}_j^i$, heading angle ${\psi }_j^i$ between i-th and j-th robots, and mass $m_o$ are estimated by an online parameter estimator by receiving the state ${\mathbf{q}}_i$ and the estimated external wrench ${\mathit{\tau }}_i^b$. The estimated kinematic parameters are exploited to calculate the geometry centroid of the payload. With the given desired trajectory of the common payload, the desired trajectory for each aerial manipulator can be computed using the geometry centroid. After the estimation of the dynamic parameters ${\mathbf{r}}_i$ and $m_o$ in (5), the controller generates the control input.

Estimation of the relative distance ${\mathit{\rho }}_j^i$ can be done with a vision sensor [23] for the indoor environment or RTK-GPS sensor [24] for the outdoor environment. However, the method [23] assumed that the vision sensor always watches the plane to compute the homography matrix. In the outdoor environment, the position can be measured by RTK-GPS with a few centimeters accuracies [24], but this system cannot be applied in indoor environments or near buildings. To overcome this issue, this paper proposes the kinematic parameter estimation algorithm only using a state variable itself.

3.1. Kinematic Parameter Estimation

To achieve this goal, we first make the following assumptions on the kinematic model of cooperative manipulators.

(1)

The position/orientation of the end effector grasping on to the payload in the global frame are not known.

(2)

At each grasping point, the roll and pitch angle of the end effector is the same for the cooperative aerial manipulators.

(3)

Configurations of the payload are previously given, so aerial manipulators know the grasping point of the payload. However, the exact relative distances and heading angles between each robot are unknown.

For the ground manipulator, it is necessary to estimate the six DOF kinematic model (i.e., three DOF for the distance and three DOF for the relative attitude). However, for an aerial manipulator, roll (i.e., ${\varphi }_i$) and pitch (i.e., ${\theta }_i$) angles can be computed without estimation because those angles are close to 0 degrees in the hovering state, or easily obtained on the IMU sensor. Despite the inertial measurement unit (IMU) sensor, it is impossible to accurately estimate the heading angle in the global frame even with the magnetometer sensor. Because of these issues, unlike approaches for ground manipulators [10,19], we only estimate the relative heading angle. Therefore, our approach is divided into two steps: (1) heading angle ${\psi }_j^i$, and (2) the relative distance ${\mathit{\rho }}_j^i$.

The relative heading angle is estimated from the measurement of the IMU sensor around the z-axis. From the assumption of rigid grasping, the module pair $[{\varphi }_i,{\theta }_i]$ has to be the same for both i and j-th aerial manipulator in the same time step k as

$$\begin{array}{c}\hfill \frac{{}^k{\theta }_i}{{}^k{\varphi }_i}=\frac{{}^k{\theta }_j}{{}^k{\varphi }_j}tan\left({\psi }_i^i\right).\end{array}$$

(11)

The above equation should be satisfied for the whole process, so we can augment the Equation (11) as

$$\begin{array}{c}\hfill {\widehat{\psi }}_j^i=\frac{{\sum }_{k=1}^K\left({tan}^{-1}\left(\frac{{}^k{\theta }_i}{{}^k{\varphi }_i}\right)-{tan}^{-1}\left(\frac{{}^k{\theta }_j}{{}^k{\varphi }_j}\right)\right)}{K},\end{array}$$

(12)

where K is the total number of time steps. It is impossible to estimate the relative heading angle when the roll and pitch angles are exactly 0 degrees, but in the case of drones, this is not the case, so the algorithm can run.

For the relative distance estimation, we analyze the kinematics of the payload. Let ${\mathbf{v}}_i^e$ and ${\mathit{\omega }}_i^e$ be the linear and angular velocities of the end-effector of the i-th aerial manipulator in ${\Sigma }_{e,i}$. These terms can be computed as

$$\left[\begin{array}{c}{\mathbf{v}}_i^e\\ {\mathit{\omega }}_i^e\end{array}\right]=\left[\begin{array}{cc}{R}_i^T& 0\\ 0& T_i\end{array}\right]J_i{\dot{\mathbf{q}}}_i$$

(13)

where ${\mathbf{R}}_i$ is the rotation matrix, which represents the orientation of ${\Sigma }_{b,i}$ for the inertial frame. $T_i$ is a mapping matrix between the angular rate of an aerial robot in ${\Sigma }_I$ and the body angular rate ${\mathit{\omega }}_i^e$ in ${\Sigma }_{e,i}$.

The end-effector velocities at the i-th and j-th aerial manipulator should satisfy the following constraints:

$$\begin{array}{cc}\hfill {\mathit{\omega }}_i^e& =\mathbf{R}\left({\widehat{\psi }}_j^i\right){\mathit{\omega }}_j^e\hfill \\ \hfill {\mathbf{v}}_i^e& =\mathbf{R}\left({\widehat{\psi }}_j^i\right){\mathbf{v}}_j^e-{\mathit{\rho }}_j^i\times {\mathit{\omega }}_i^e\hfill \end{array}$$

(14)

where $\mathbf{R}\left({\widehat{\psi }}_j^i\right)$ is the rotation matrix using ${\widehat{\psi }}_j^i$, which is the orientation of ${\Sigma }_{e,j}$ for ${\Sigma }_{e,i}$.

The proposed method for parameter estimation ${\mathit{\rho }}_{ij}$ is updated based on the following error terms

$$\begin{array}{cc}\hfill {\mathbf{e}}_{{\mathbf{v}}_i}& ={\mathbf{v}}_i^e-\mathbf{R}\left({\widehat{\psi }}_j^i\right){\mathbf{v}}_j^e+{\mathit{\rho }}_j^i\times {\mathit{\omega }}_i^e.\hfill \end{array}$$

(15)

To minimize the prediction error in (14), we now define the cost function as

$$\begin{array}{c}\hfill J_{{\mathbf{v}}_i}=\sum _{i=1}^n\frac{a_e}{2}\parallel {\mathbf{e}}_{{\mathbf{v}}_i}{\parallel }^2+\sum _{\begin{array}{c}j=1\\ (i\ne j)\end{array}}^N\frac{a_{\mathbf{r}}}{2}(\parallel {\widehat{\mathit{\rho }}}_j^i\parallel -\parallel {\widehat{\mathit{\rho }}}_i^j{\parallel )}^2,\end{array}$$

(16)

where $a_e$ and $a_r$ are non-negative weights. Recalling that ${\widehat{\mathit{\rho }}}_j^i$ is the distance to from the i-th end-effector to the j-th end-effector estimated by i-th aerial manipulator. Therefore, $|{\mathit{\rho }}_j^i|=|{\mathit{\rho }}_i^j|=\mathit{\rho }$ in the true value, which means that the relative distance between i and j-th end-effector should be the same.

In the cost function, the error term $\parallel {\mathbf{e}}_{{\mathbf{v}}_i}{\parallel }^2$ is designed to minimize the error and estimate ${\widehat{\mathit{\rho }}}_j^i$. The term $(\parallel {\widehat{\mathit{\rho }}}_j^i\parallel -\parallel {\widehat{\mathit{\rho }}}_i^j{\parallel )}^2$ is the consensus term to share the same estimation results. Here, we apply the gradient rule to obtain the update rule for ${\mathit{\rho }}_j^i$ as

$$\begin{array}{cc}\hfill {}^{k+1}{\widehat{\mathit{\rho }}}_j^i=& {}^k{\widehat{\mathit{\rho }}}_j^i-\alpha \frac{\partial J_{{\mathbf{v}}_i}}{\partial {\widehat{\mathit{\rho }}}_j^i}\hfill \\ \hfill =& {}^k{\widehat{\mathit{\rho }}}_j^i-\alpha ·a_e({\mathbf{v}}_i^e-R\left({\widehat{\psi }}_j^i\right){\mathbf{v}}_j^e-{\mathit{\omega }}_i^e{\times }^k{\widehat{\mathit{\rho }}}_j^i)\hfill \\ \hfill & -\alpha ·a_r\sum _{\begin{array}{c}j=1\\ (i\ne j)\end{array}}^N{(\parallel }^k{\widehat{\mathit{\rho }}}_j^i{\parallel -\parallel }^k{\widehat{\mathit{\rho }}}_i^j\parallel )\frac{{}^k{\widehat{\mathit{\rho }}}_j^i}{{\parallel }^k{\widehat{\mathit{\rho }}}_j^i\parallel },\hfill \end{array}$$

(17)

where $\alpha$ is the learning rate. Note that the kinematic estimation of ${\widehat{\mathit{\rho }}}_{ij}$ converges to the true value $\mathit{\rho }$ if the following PE (persistence of excitation) condition is satisfied as

$$\begin{array}{c}\hfill \sum _k^{k+T}S{\left({\mathit{\omega }}_i^e\right)}^{T}S\left({\mathit{\omega }}_i^e\right)>0,\end{array}$$

(18)

for the constant time T. Here, $S\left({\mathit{\omega }}_i^e\right)$ is the skew-symmetric matrix of ${\mathit{\omega }}_i^e$. Equation (18) implies the condition of persistent excitation (PE) and the PE term should be positive definite. The estimation performance comparison will be shown in Section 5.

3.2. Path Planning

As we discussed the assumption in Section 3, we assume that cooperative robots know the configuration of the payload. For example, as shown in Figure 3, aerial manipulators know the basic configuration between each robot. In this case, geometric centroid can be estimated using the estimated distance ${\mathit{\rho }}_j^i$. For simplicity of the path planning, the i-th robot can be considered a leader robot and the rest of the robots can be considered as follower robots. The geometric centroid ${\mathbf{r}}_g$ can be calculated as

$$\begin{array}{c}\hfill {\mathbf{r}}_g=\sum _{\begin{array}{c}j=1\\ i\ne j\end{array}}^N{\mathit{\rho }}_j^i/N.\end{array}$$

(19)

Given the desired trajectory of the payload ${\mathbf{q}}_o^d$, the desired trajectory for each end-effector (i.e., ${\mathbf{q}}_{e,i}^d\in {\mathbb{R}}^6$) is derived as

$$\begin{array}{cc}\hfill {\mathbf{q}}_{e,i}^d& ={\mathbf{q}}_o^d-{\mathbf{r}}_g\hfill \\ \hfill {\mathbf{q}}_{e,j}^d& ={\mathbf{q}}_o^d+({\mathit{\rho }}_j^i-{\mathbf{r}}_g)(j=1,...,N,i\ne j)\hfill \end{array}$$

(20)

where j is a randomly selected j-th aerial robot frame to unify and express it in one coordinate system among several robots (See Figure 3 for the details). Using ${\mathbf{q}}_{e,i}^d$, the desired trajectory for each aerial manipulator ${\mathbf{q}}_{e,i}^d\in {\mathbb{R}}^8$ is obtained based on the inverse kinematics solution. The details of inverse kinematics are described in [4].

4. Dynamic Parameter Estimation

External wrench estimation to handle unexpected collisions of robots has begun with early works on stationary ground manipulators [25]. To increase the speed and accuracy of the calculation, the wrench estimation algorithm is newly developed for a dual-arm manipulator [26] and now it is also applied to an aerial robot [15,16]. The hybrid estimation algorithm based on a generalized momentum and acceleration was satisfactorily applied for an aerial robot to detect collisions [15,16]. However, these algorithms [15,16] are applied only for a single aerial robot and the estimation performance was not verified in the presence of sensor noise. Research has been studied to solve the sensor noise problem of the wrench estimation algorithm [27], but it was applied only to the collision problem of a single ground manipulator.

To handle the aforementioned issue, in this section, we propose the dynamic parameter estimator for cooperative aerial manipulators to obtain the payload mass ${\widehat{m}}_o$ and ${\mathbf{r}}_i$. If a payload has uniform mass distribution and the aerial manipulators exert the same force on the grasping point of the payload, then the ${\mathbf{r}}_i$ is easy to compute, and the mass ${\widehat{m}}_o$ can be estimated only using only the state variable of a robot itself as described in our previous work [4]. However, for the payload with non-uniform mass distribution, previous works cannot be applied because the geometry centroid and COM of the payload are different. To overcome this issue, in this paper, we design the dynamic-parameter-updates rule for the cooperative aerial manipulator by exploiting the external wrench estimation algorithm.

In this paper, as configuration of a payload is given as described in Section 3.1, we can assume that the moment of inertia is approximated using the kinematic (i.e., ${\mathit{\rho }}_j^i$) and dynamic parameter (i.e., $m_o$ and ${\mathbf{r}}_i$) of a payload [28]. Therefore, we focus on the estimation of $m_o$ and ${\mathbf{r}}_i$.

4.1. First-Order Momentum Observer

Before addressing the proposed parameter estimation algorithm, we briefly explain the classical first-order momentum-based wrench estimator for the performance comparison used in simulations as used in [14].

The momentum-based estimator is designed based on the generalized momentum of the robot as

$$\begin{array}{cc}\hfill {\mathbf{p}}_i& =D_i{\dot{\mathbf{q}}}_i.\hfill \end{array}$$

(21)

Using (21), the time derivative of ${\mathbf{p}}_i$ is given as

$$\begin{array}{cc}\hfill {\dot{\mathbf{p}}}_i& ={\mathit{\tau }}_i+{\mathit{\tau }}_i^b-C_i{\dot{\mathbf{q}}}_i-G_i+\dot{D}{\dot{\mathbf{q}}}_i\hfill \\ \hfill & ={\mathit{\tau }}_i+{\mathit{\tau }}_i^b+C_i^T{\dot{\mathbf{q}}}_i-G_i.\hfill \end{array}$$

(22)

Here, the passivity property (i.e., $\dot{D}\left({\mathbf{q}}_i\right)=C({\mathbf{q}}_i,{\dot{\mathbf{q}}}_i)+C^T({\mathbf{q}}_i,{\dot{\mathbf{q}}}_i)$) was used. ${\mathit{\tau }}_i^b$ and is the external wrench that we want to estimate.

The derivation of classic momentum-based estimator is designed based on the following residual vector as

$$\begin{array}{c}\hfill {\mathbf{m}}_i=K_o\left({\mathbf{p}}_i-\int ({\mathit{\tau }}_i+C_i^T{\dot{\mathbf{q}}}_i-G_i+{\mathbf{m}}_i)ds-{\mathbf{p}}_i\left(0\right)\right),\end{array}$$

(23)

where $\mathbf{p}\left(0\right)$ is the initial value of p and $K_o$ is the positive gain. By using (22), the time derivative of the residual r can be obtained as

$$\begin{array}{c}\hfill {\dot{\mathbf{m}}}_i=-K_o({\mathbf{m}}_i-{\mathit{\tau }}_i^b).\end{array}$$

(24)

This equation means that ${\mathbf{m}}_i$ is the first-order filtered value of ${\mathit{\tau }}_i^b$; therefore, it can be used as an estimation of the external wrench.

4.2. Dynamic Parameter Estimation with Sliding Mode Momentum Observer

In the classical momentum-based observer, to achieve ${\mathbf{m}}_i\approx {\mathit{\tau }}_i^b$, $K_o$ should be large enough. However, a large value of $K_o$ results in higher noise amplification in the estimated wrench. In this case, the estimation performance of dynamic parameters may deteriorate. This issue can be solved by exploiting the second-order sliding mode (SOSM) momentum observer which was introduced in [29].

SOSM observer, the so-called super twisting algorithm, is widely used in sliding-mode literature to design controllers, observers, and exact differentiators. Using the generalized momentum in (21), the momentum observer can be designed

$$\begin{array}{cc}\hfill {\dot{\widehat{\mathbf{p}}}}_i& ={\mathit{\tau }}_i+C_i^T{\dot{\mathbf{q}}}_i-G_i-T_1\left|{\tilde{\mathbf{p}}}_i\right|-T_2{\tilde{\mathbf{p}}}_i+{\mathit{\sigma }}_i\hfill \end{array}$$

(25)

$$\begin{array}{cc}\hfill {\dot{\mathit{\sigma }}}_i& =-S_1\mathrm{sgn}\left({\tilde{\mathbf{p}}}_i\right)-S_2{\tilde{\mathbf{p}}}_i,\hfill \end{array}$$

(26)

where ${\widehat{\mathbf{p}}}_i$ is the estimated momentum of i-th aerial manipulator and ${\tilde{\mathbf{p}}}_i={\widehat{\mathbf{p}}}_i-{\mathbf{p}}_i$. $S_i$ and $T_i$ are the user-defined gain.

Using (22) and the definition of new variable ${\mathbf{s}}_i={\mathit{\sigma }}_i-{\mathit{\tau }}_i^b$, we can obtain the observer error dynamics as

$$\begin{array}{cc}\hfill {\dot{\tilde{\mathbf{p}}}}_i& =-T_1{\left|{\tilde{\mathbf{p}}}_i\right|}^{1/2}\mathrm{sgn}\left({\tilde{\mathbf{p}}}_i\right)-T_2{\tilde{\mathbf{p}}}_i+{\mathit{\sigma }}_i\hfill \\ \hfill {\dot{\mathbf{s}}}_i& =-S_1\mathrm{sgn}\left({\tilde{\mathbf{p}}}_i\right)-S_2{\tilde{\mathbf{p}}}_i-{\dot{\mathit{\tau }}}_i^b.\hfill \end{array}$$

(27)

Assuming that ${\mathit{\tau }}_i^b$ and ${\mathit{\sigma }}_i$ are globally bounded by some known constant and $S_i$ and $T_i$ are selected high enough, then the origin $({\tilde{\mathbf{p}}}_i,{\mathbf{s}}_i)=(\mathbf{0},\mathbf{0})$ is an equilibrium point that is globally asymptotically stable. Here, we can say that ${\mathit{\sigma }}_i$ becomes an estimation of ${\mathit{\tau }}_i^b$. The detailed proofs are described in [29]. The detailed process for dynamic parameter estimation is described in Figure 4.

As ${\mathit{\tau }}_i^b\in {\mathbb{R}}^8$ is the external wrench applied to the aerial manipulator in ${\Sigma }_{b,i}$, we have to compute the applied force at the end-effector ${\mathit{\lambda }}_i$. Following the description in [30], ${\mathit{\lambda }}_i$ can be derived as

$$\begin{array}{cc}\hfill {\widehat{\mathit{\lambda }}}_i& ={\left(J_i^T\right)}^{†}{\widehat{\mathit{\tau }}}_i^b\hfill \\ \hfill [{\widehat{f}}_{i,x}^e,{\widehat{f}}_{i,y}^e,{\widehat{f}}_{i,z}^e{]}^T:& =\left[\begin{array}{cc}{\mathrm{I}}_3& 0_3\end{array}\right]{\widehat{\mathit{\lambda }}}_i\hfill \end{array}$$

(28)

where ${\mathrm{I}}_3$ and $0_3$ are $3\times 3$ identity and zero matrix, respectively. ${\left(J_i^T\right)}^{†}={\left(J_i{J}_i^T\right)}^{-1}{J}_i$ is the pseudo inverse of $J_i^T$. Here, we only need the force at $x,y$ and z direction, so $\left[\begin{array}{cc}{\mathrm{I}}_3& 0_3\end{array}\right]$ is added.

Now, the dynamic parameters (i.e., ${\widehat{m}}_o$ and ${\mathbf{r}}_i$) in Figure 1 are estimated using the estimated wrench in (). The mass is updated as

$$\begin{array}{c}\hfill {\widehat{m}}_o=\frac{{\sum }_{i=1}^N{\widehat{f}}_{i,z}^e}{N\times g},\end{array}$$

(29)

where g is gravitational constant and ${\widehat{f}}_{i,z}^e$ is the estimated force by i-th aerial manipulator at z-direction.

The momentum generated by the effective force at the end-effector should be zero, so the following equation should be satisfied:

$$\begin{array}{c}\hfill \sum _{j=1}^N({\mathit{\rho }}_j^i-{\mathbf{r}}_g-{\mathbf{o}}_g)\times {\widehat{f}}_j^e=0.\end{array}$$

(30)

Note that i in (30) refers to the leader robot or the reference frame. As this paper considers the flat-surface payload as shown in Figure 5, Equation (30) can be rewritten as

$$\begin{array}{cc}\hfill & \sum _{j=1}^N{\widehat{f}}_{j,z}^e(r_{j,y}^g-o_{g,y})=0,\sum _{j=1}^N{\widehat{f}}_{i,z}^e(r_{j,x}^g-o_{g,x})=0\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill & \sum _{j=1}^N{\widehat{f}}_{j,y}^e(r_{j,x}^g-o_{g,x})-{\widehat{f}}_{j,x}^e(r_{j,y}^g-o_{g,y})=0\hfill \end{array}$$

(32)

Finally, $o_x$ and $o_y$ are computed as

$$\begin{array}{c}\hfill o_x=\frac{\sum {\widehat{f}}_{j,z}^e{r}_{j,x}^g}{\sum {\widehat{f}}_{j,z}^e},o_y=\frac{\sum {\widehat{f}}_{j,z}^e{r}_{j,y}^g}{\sum {\widehat{f}}_{i,z}^e}.\end{array}$$

(33)

Now, we can compute ${\mathbf{r}}_i$ as ${\mathbf{r}}_i=({\mathit{\rho }}_j^i-{\mathbf{r}}_g-{\mathbf{o}}_g)$. Equation (32) may not be necessary for the flat-surface payload considered in this paper, but it is necessary for verification of the estimated value. In addition, if the surface of the payload is not flat, ${\mathbf{o}}_g$ can be estimated by solving (30) directly.

5. Simulations

To evaluate the performance of the proposed algorithm, we present several simulation results to compare the performances of the proposed algorithm with the conventional algorithm. As the conventional estimation algorithm for cooperative robots [4,9,13,14] is difficult to handle the payload with an arbitrary COM well, it is hard to compare the estimation performance with the proposed algorithm. For this reason, we divide performance comparison into two fields, mass estimation, and kinematic parameter estimation, and compare them with existing algorithms. The mass estimation performance is compared with first-order-based momentum observer [14] and our previous parameter estimation algorithm [4]. For the kinematic parameter estimation, the adaptive control for an inaccurate kinematic model [10] is used. For detailed analysis, two scenarios (two drones or three drones) as shown in Figure 5 are considered in the simulation.

5.1. Simulation Environment

In the simulations, each aerial manipulator consists of a quadrotor and a 2-DOF arm. For the two aerial manipulators in Figure 5a, the mission consists of manipulating an unknown payload from the initial position ${[0,0.6,0.6]}^T$ m to ${[1.2,1.6,0.6]}^T$ m. The payload is characterized as $m_o=0.5$, $o_x=0$ m, and $o_y=-0.2$ m. The total length of the payload is $l:=0.8$ m. For the three aerial manipulators in Figure 5b, the initial position of the payload is ${[0.23,0.4,0.6]}^T$ m and the final position of the payload is ${[1.73,1.9,0.6]}^T$ m. In this mission, the payload is characterized as $m_o=0.5$, $o_x=0.1$ m, and $o_y=0.1$ m. In the triangular payload, the length of each side is set to $0.8$ m.

In both scenarios, for the property of the persistence of excitation (PE) in (18), two aerial manipulators rotate for 5 s. After the PE process is finished, the cooperative aerial manipulator follows the desired trajectory.

The control input in (8) is calculated as

$$\begin{array}{c}\hfill {\mathit{\tau }}_i={\widehat{D}}_i{\ddot{\mathbf{q}}}_{\mathbf{r},i}+{\widehat{C}}_i{\dot{\mathbf{q}}}_{\mathbf{r},i}+{\widehat{G}}_i-({\kappa }_{s,i}+{\delta }_i){\mathbf{s}}_i\end{array}$$

(34)

where ${\widehat{D}}_i$, ${\widehat{C}}_i$, and ${\widehat{G}}_i$ include the estimated payload mass ${\widehat{m}}_o$. ${\kappa }_{s,i}$ is the positive-definite gain matrix. Here, ${\delta }_{\mathit{\tau }}$ is the auxiliary control input for handling properties and defined as

$$\begin{array}{c}\hfill {\delta }_i:=\frac{c_i}{2}{J}_{e,i}^T{E}_i^{†}({\dot{H}}_o-2\mu )E_i^{-T}{J}_{e,i}.\end{array}$$

(35)

The sliding surface variable ${\mathbf{s}}_i$ can be defined as

$$\begin{array}{cc}\hfill {\dot{\mathbf{q}}}_{\mathbf{r},i}& ={\dot{\mathbf{q}}}_i^d-{\Lambda }_i{\mathbf{e}}_i\hfill \\ \hfill {\mathbf{s}}_i& ={\dot{\mathbf{q}}}_i-{\dot{\mathbf{q}}}_{\mathbf{r},i}={\dot{\mathbf{e}}}_i+{\Lambda }_i{\mathbf{e}}_i\hfill \end{array}$$

(36)

where ${\Lambda }_i$ is a positive diagonal matrix and ${\mathbf{q}}_i^d$ is the desired state of the i-th aerial manipulator. The detailed process for the controller is described in our previous research [4].

For the performance analysis, we assumed that

(1)

The main uncertainty is caused by the measurement noise, ${\dot{\mathbf{q}}}_i^{mes}={\dot{\mathbf{q}}}_i+{\mathbf{ϵ}}_v,{\mathbf{q}}_i^{mes}={\mathbf{q}}_i+{\mathbf{ϵ}}_p$.

(2)

The noise in the measured state ${\mathbf{ϵ}}_v$ and ${\mathbf{ϵ}}_p$ has a high frequency and is zero mean.

In the simulations, the standard deviation of the measurement noise ${\mathbf{ϵ}}_v$ and ${\mathbf{ϵ}}_p$ are set as

• 0.2 m in x and y direction and 0.1 m in the z direction
• 0.01 rad in attitude and 0.1 rad in joint angles.

Let ${\widehat{ϵ}}_k$ is the estimated parameter of $ϵ$ at the time step k and $ϵ$ is the true value. The performance of the payload mass (i.e., ${\widehat{m}}_o$) and kinematic parameter estimation (i.e., ${\widehat{\rho }}_{ij}$) is evaluated by considering the average error ($E_{ae}$) and the maximum error $E_{me}$:

$$\begin{array}{c}\hfill E_{ae}=\frac{1}{N}\sum _{k=1}^T\parallel {\widehat{ϵ}}_k-ϵ\parallel ,E_{me}=\underset{k}{max}\parallel {\widehat{ϵ}}_k-ϵ\parallel .\end{array}$$

(37)

To analyze the above performance, Monte Carlo simulations are also performed using 30 sample runs.

Based on our proposed framework discussed in the previous section, we first conducted a simulation study with two aerial manipulators carrying an irregular payload, which are shown in Figure 6a. During the phase to satisfy PE condition (18), the desired state for the first aerial manipulator is fixed and the desired state for the second aerial manipulator is set as

$$\begin{array}{c}\hfill {\mathbf{q}}_2^d={[0.05\times t,0.05\times t,0.6,0,0,-30sin(2\pi /5t),-90,0]}^T,\end{array}$$

(38)

where unit of the attitude and joint angles is written in degrees. After the kinematic estimation phase, the desired trajectory for the payload is set as

$$\begin{array}{c}\hfill {\mathbf{q}}_o^d={[0.1\times t,0.1\times t,0.6,0,0,0]}^T.\end{array}$$

(39)

We assume that the noises for the translational and angular velocities have a value that is twice as large as the position noise.

5.2. Simulation Results

The simulation data are shown in Figure 6 and Figure 7. In Figure 6, the comparison results including kinematic parameter estimation and trajectory tracking performance are shown. In Figure 6b, the red dotted line is the true value and the blue line is the estimated results by the proposed algorithm, and the green dotted line is the estimated results by [10]. The estimation error between the proposed method and conventional method in Figure 6b is also caused by the fact that the algorithm proposed in [10] estimates six-DOF poses of a payload while the proposed algorithm in this paper estimates only four-DOF poses of the payload considering the characteristics of the cooperative manipulator. For this reason, we can accurately estimate the location of COM (i.e., $o_x$ and $o_y$) in the noisy measurement as shown in Figure 6e. In Figure 6c,d, the red-dotted line is the desired state for the aerial manipulator and the blue line means the actual states. As the conventional estimation algorithm for the relative distance ${\mathit{\rho }}_j^i$ contains a relatively large error, the desired trajectory in the x and z direction may incorrectly be adjusted. The tracking performance in x and z direction also affects the joint angle of the arm. So our proposed algorithm shows better tracking performance as shown in Figure 6f.

The dynamic parameter estimation is addressed in Figure 7. In Figure 7, the blue and green line is the estimated value by the proposed algorithm and the first-order-based momentum observer used in [14]. The orange line in Figure 7b is the estimated mass by our previous research in [4]. Our previous algorithm [4] for the mass estimation shows better performance compared with the first-order momentum observer [14], but the proposed method in this paper shows the best performance. This is because our algorithm with second-order sliding-mode observer accurately estimates the external wrench applied to the end-effector than the first-order-based momentum observer as shown in Figure 7a. Although showing better performance than the momentum-based observer, our previous work cannot handle the payload with an arbitrary COM because of the assumption of the uniform-mass distribution. In addition, as the force distribution $c_i$ in (5) can be calculated using the estimated force on z axis (i.e., $c_i={\widehat{f}}_{i,z}^e/{\sum }_{j=1}^N{\widehat{f}}_{j,z}^e$), the estimation of $c_i$ in Figure 7b describes that our proposed algorithm estimates the applied wrench precisely. The detailed estimation performance for the dynamic and kinematic parameters is presented in

Table 2. Mass and kinematic parameter estimation performance.

	Proposed Algorithm		Comparison Algorithm
Parameter	${\mathit{E}}_{\mathit{ae}}$(std)	${\mathbf{E}}_{\mathbf{me}}$	${\mathit{E}}_{\mathit{ae}}$(std)	${\mathit{E}}_{\mathit{me}}$	Ref
${\widehat{m}}_o$ [kg]	0.1968 (0.0094)	0.1883	0.4859 (0.0134)	1.0196	by [14]
			0.3275 (0.0104)	0.4604	by [4]
${\widehat{\rho }}_j^i\left(x\right)$ [m]	0.1265 (0.0107)	0.0242	0.2015 (0.0479)	0.1120
${\widehat{\rho }}_j^i\left(y\right)$ [m]	0.2456 (0.0463)	0.0363	0.2550 (0.0680)	0.1004	by [10]
${\widehat{\rho }}_j^i\left(z\right)$ [m]	0.0549 (0.0055)	0.0048	0.4081 (0.1073)	0.3757

. In this table, the average error $E_{ae}$ and the maximum error $E_{me}$ are calculated after parameter converges. $std$ in Table 2 means standard deviation. As described in Table 2, the proposed algorithm has the smallest value in both $E_{ae}$ and $E_{me}$.

The second simulation using three aerial manipulators is described in Figure 8 and Figure 9. Unlike the first scenario with two aerial manipulators, the leader or reference aerial manipulator in the second scenario estimates ${\mathit{\rho }}_j^i$ (e.g., $i=1$, and $j=2,3$) simultaneously using their velocity measurement. In this case, it is difficult to distinguish whether the effect in (14) is caused by the robot with $j=2$ or the robot with $j=3$. To overcome this issue, the consensus parameter $a_r$ in (17) for the reference or leader robot is set to 0. For the follower robot, we set $a_e\ne 0$. Figure 8b shows the estimation performance. The relative distances between i-th and j-th aerial manipulators are estimated precisely even in the complex-shaped payload. Figure 9 shows the estimation of the mass and the COM of the payload. Our algorithms estimate the COM of the payload satisfactorily with or without the noisy measurement.

6. Conclusions

This paper presents a cooperative aerial manipulation algorithm to handle a payload with an arbitrary center-of-mass. The proposed framework consists of two estimation algorithms: relative distance estimation and dynamic parameter estimation. The relative distances are estimated using the translational and angular velocities between aerial robots. By exploiting the distance estimation, the desired trajectories for each aerial manipulator are set. After the kinematic estimation, the dynamic parameter estimation is performed for the mass of the common object and the vector between the end-effector frame and the COM of the payload. We performed flight simulations using multiple aerial manipulators and compared the proposed algorithm with the conventional method involving our previous work or the first-order momentum-based algorithm. The simulation results showed that the proposed algorithm achieved the best performance in parameter estimation and trajectory tracking.

Our future works include relaxation of PE (persistence of excitation) conditions and taking into account complex payload types. The relaxation of PE is required for the safety of drones because it can be dangerous when the drone rotates to satisfy the PE condition. In addition, the proposed method assumes that the COM and the geometric centroid are on the same horizontal plane and the configuration of a payload should be known previously, otherwise it may be difficult to estimate. Experimental validation of the proposed method on a real setup is needed to address the above-mentioned challenges.