Shared Control of an Aerial Cooperative Transportation System with a Cable-suspended Payload

Abstract

This paper presents a novel bilateral shared framework for a cooperative aerial transportation and manipulation system composed by a team of micro aerial vehicles with a cable-suspended payload. The human operator is in charge of steering the payload and he/she can also change online the desired shape of the formation of robots. At the same time, an obstacle avoidance algorithm is in charge of avoiding collisions with the static environment. The signals from the user and from the obstacle avoidance are blended together in the trajectory generation module, by means of a tracking controller and a filter called dynamic input boundary (DIB). The DIB filters out the directions of motions that would bring the system too close to singularities, according to a suitable metric. The loop with the user is finally closed with a force feedback that is informative of the mismatch between the operator’s commands and the trajectory of the payload. This feedback intuitively increases the user’s awareness of obstacles or configurations of the system that are close to singularities. The proposed framework is validated by means of realistic hardware-in-the-loop simulations with a person operating the system via a force-feedback haptic interface.

Appendices

Appendix A: Input Shaping

Many applications generally require a robot, be it a manipulator or an autonomous mobile vehicle, to track a certain trajectory. In most cases this reference trajectory is not known a priori but it is generated online from a discontinuous input such as a sequence of waypoints. The transformation from the input to a smooth enough trajectory is achieved using a variety of filters or input shaping methods [3, 31]. The aim of all these approaches is not only to approximate the input signals with smooth functions but also to guarantee that the output trajectory complies with the constraints imposed by the robot, e.g. velocity and acceleration limits.

The practical solution used in our framework to achieve this goal consists of a combination of low-pass filters together with a controller that ensures tracking of the input signals subordinate to the constraints (see general architecture in Fig. 21). The smoothing is realized by a double low-pass filter to produce a $\mathcal {C}^{2}$ signal denoted by the subscript ∙_LP. The tracking is achieved by two different designs, one for the DIB output and one for the input ${\!}^{P}\dot {\eta }_{hmn}$ (see Fig. 5). The difference in the two designs is that the first only imposes limits on the maximum velocity and acceleration of the reference trajectory whereas the second also imposes limits on the range.

Fig. 21

Block diagm of an input shaping solution for a generic piecewise continuous velocity-like input $\dot {\boldsymbol {v}}$

Consider first the DIB filtering, restricting our discussion to the translational for the sake of simplicity. The tracking controller is defined as

$$ \ddot{\boldsymbol{p}}_{ref} = \text{sat}\left( \ddot{\boldsymbol{p}}_{LP} + k_{r} \left( \dot{\boldsymbol{p}}_{LP} - \dot{\boldsymbol{p}}_{ref} \right)\right) $$

(48)

where k_r > 0 and sat(∙) is a function that imposes the limits

$$ \begin{aligned} \| \ddot{\boldsymbol{p}}_{ref}\| \leq A \\ \| \dot{\boldsymbol{p}}_{ref}\| \leq V \end{aligned} $$

From a practical point of view, Eq. 48 is implemented on a computer with a fixed discrete sample time T. Using the Euler forward method $\dot {\boldsymbol {p}}_{ref}(k+1) = \dot {\boldsymbol {p}}_{ref}(k) + T\ddot {\boldsymbol {p}}_{ref}(k)$ the velocity constraint is easily transformed into an acceleration constraint

$$ \begin{aligned} \|\ddot{\boldsymbol{p}}_{ref}(k)\| \leq & \small{\frac{ -\dot{\boldsymbol{p}}_{ref}(k)^{T} {\boldsymbol{v}}(k)}{T}} + \\ &\small{\frac{\sqrt{(\dot{\boldsymbol{p}}_{ref}(k)^{T} {\boldsymbol{v}}(k))^{2} - \|\dot{\boldsymbol{p}}_{ref}(k)\|^{2} + V^{2} } }{T}} \end{aligned} $$

where ${\boldsymbol {v}}(k) = \frac {\ddot {\boldsymbol {p}}_{LP} + k_{r} (\dot {\boldsymbol {p}}_{LP} - \dot {\boldsymbol {p}}_{ref})}{\|\ddot {\boldsymbol {p}}_{LP} + k_{r} (\dot {\boldsymbol {p}}_{LP} - \dot {\boldsymbol {p}}_{ref})\|}$ is the direction of the unsaturated command Eq. 48.

In regards to ${\!}^{P}\dot \eta _{LP}$, the controller must additionally satisfy a limit on the maximum angle, i.e. ${\!}^{P}\eta _{min}\leq {\!}^{P}\eta _{des} \leq {\!}^{P}\eta _{max}$. For this purpose, a standard bang-bang strategy is used, i.e.

$$ {\!}^{P}\ddot\eta_{des} = \begin{cases} \small{-{\!}^{P}\ddot\eta_{max}} & \text{if } {\!}^{P}\dot\eta_{des}>0 \\ & \text{and } \scriptstyle{{\!}^{P}\eta_{des} \geq {\!}^{P}\eta_{\min} - \frac{{\!}^{P}\dot\eta_{des}^{2}}{2{\!}^{P}\eta_{\min}}} \\ \small{{\!}^{P}\ddot\eta_{max}} & \text{if } {\!}^{P}\dot\eta_{des}<0 \\ & \text{and }\scriptstyle{ {\!}^{P}\eta_{des} \leq {\!}^{P}\eta_{\max} + \frac{{\!}^{P}\dot\eta_{des}^{2}}{2{\!}^{P}\eta_{\max}}} \\ \scriptstyle{\text{sat}\left( {\!}^{P}\ddot\eta_{LP} + k_{\eta} \left( {\!}^{P}\dot\eta_{LP} - {\!}^{P}\dot\eta_{des}\right)\right)} & \text{otherwise} \end{cases} $$

(49)

where k_η > 0 and sat(∙) is a function that imposes the limits

$$ \begin{aligned} \| {\!}^{P}\ddot\eta_{des}\| \leq {\!}^{P}\ddot\eta_{max} \\ \| {\!}^{P}\dot\eta_{des}\| \leq {\!}^{P}\dot\eta_{max} \end{aligned} $$

Appendix B: Tension Distribution

The problem of tension distribution, i.e., solving Eq. 36 when m > n, does not have a unique solution. Rather, if the wrench matrix has full rank¹, the solution to Eq. 36 lies in the m − n dimensional null space of the wrench matrix $J_{\mathcal {R}}^{T} N$. In view of this property of the solution manifold, the general solution Eq. 36 can be expressed in the form

$$ {\boldsymbol{t}}_{r} = {\boldsymbol{t}}_{p} + {\boldsymbol{t}}_{h} $$

where t_p is a particular solution of Eq. 36 and t_h is the homogeneous solution, i.e., a vector that belongs to the null space of the wrench matrix. This decomposition can be used to reformulate the optimization problem Eqs. 38 to 42. Consider the general solution of the problem expressed in the following form

$$ {\boldsymbol{t}}_{r} = \underbrace{\left( J_{\mathcal{R}}^{T} N\right)^{\dagger}{\boldsymbol{w}}_{r}}_{{\boldsymbol{t}}_{p}} + \underbrace{Q {\boldsymbol{\lambda}}}_{{\boldsymbol{t}}_{h}} $$

(50)

where (⋅)^‡ indicates the pseudo-inverse, $Q \in {\mathbb {R}}^{m\times m-n}$ is a matrix whose column form a basis of the null space of the wrench matrix, and ${\boldsymbol {\lambda }}\in {\mathbb {R}}^{m-n}$ is a vector of coordinates that indicates how we move on the manifold of the solution starting from t_p. It is important to observe that the particular solution obtained with the pseudo-inverse is the one with the minimum Euclidean norm, however it may not be feasible because it does not necessarily verifies the constraint on the cable tensions. Nevertheless, by imposing that t has the structure Eq. 50, we can rewrite the optimization problem Eq. 38 to 42 as

$$ \begin{array}{@{}rcl@{}}& \!\!{\boldsymbol{\lambda}}^{\star} =& \arg\min \frac{1}{2} \left( N{\boldsymbol{\lambda}} - {\boldsymbol{n}}(\alpha) \right)^{T} \left( N{\boldsymbol{\lambda}} - {\boldsymbol{n}}(\alpha) \right) \end{array} $$

(51)

$$ \begin{array}{@{}rcl@{}} \text{subject to}& &\!\!\!\!\!\!\!\!\!\!\!\!\!\underline{\boldsymbol{n}}- N{\boldsymbol{\lambda}} \leq {\boldsymbol{0}} \end{array} $$

(52)

$$ \begin{array}{@{}rcl@{}} &&\!\!\!\!\!\!\!\!\!\!\!\!\!\!\!N{\boldsymbol{\lambda}} - \overline{\boldsymbol{n}} \leq v \end{array} $$

(53)

where

$$ \begin{array}{@{}rcl@{}} {\boldsymbol{n}}(\alpha) &=& \underline{{\boldsymbol{t}}} + \alpha\left( \overline{{\boldsymbol{t}}} - \underline{{\boldsymbol{t}}} \right) - \left( J_{\mathcal{R}}^{T} N\right)^{\dagger}{\boldsymbol{w}}_{r}, \alpha \in [0, 1] \end{array} $$

(54)

$$ \begin{array}{@{}rcl@{}} \underline{\boldsymbol{n}} &=& \underline{{\boldsymbol{t}}} - \left( J_{\mathcal{R}}^{T} N\right)^{\dagger}{\boldsymbol{w}}_{r} \end{array} $$

(55)

$$ \begin{array}{@{}rcl@{}} \overline{\boldsymbol{n}} &=& \overline{{\boldsymbol{t}}} - \left( J_{\mathcal{R}}^{T} N\right)^{\dagger}{\boldsymbol{w}}_{r} \end{array} $$

(56)

The main advantage of this reformulation of the optimization problem is that the dimension of the search space is decreased to m − n. Given the solution λ^⋆ of Eq. 39, the optimal cable tension is simply ${\boldsymbol {t}}_{r}^{\star } = \left (J_{\mathcal {R}}^{T} N\right )^{\dagger } {\boldsymbol {w}}_{r} + N{\boldsymbol {\lambda }}^{\star }$. In the simulations presented in Section 5 we solve the tension distribution problem using the reduced formulation Eqs. 51 and 53. In particular we impose Lagrange optimality conditions which, being the problem convex, ensure that a candidate point is a global minima.