I Introduction
Robot swarms are envisioned to assist humans in logistics operations with better cost-effective approaches [1]. Rotorcraft are an essential part of this vision due to their mechanical simplicity and relative expendable nature. In this paper, we demonstrate a systematic method to deal with the transport of objects that are too heavy for a single rotorcraft, but not for a team of them. This added complexity is supported by the fact that rotorcraft do not scale up well aerodynamically speaking, and they become dangerous to operate if their size is big enough. In particular, we focus on carrying suspended loads. In this way, we give flexibility to the team to change its shape and distribute the load among the vehicles efficiently.
Formation control has been employed in the cooperative transportation of objects on the ground [2]. In our approach, we use rigidity theory [3] to describe the geometrical shape of the team so that we can design the desired load distribution among the rotorcraft in their steady-state, even while moving. The coordinated motion of the team is induced by injecting disagreements into the inter-robot distances of the formation. The superposition of specific sets of disagreements creates translational, rotational, and scaling motions of a distributed rigid formation [4]. We combine the team motion controller with Incremental Nonlinear Dynamic Inversion (INDI) to track desired accelerations in the individual vehicles [5]
. By having prior knowledge about the created forces and moments of the actuators and their delays, the INDI tracks the commanded accelerations coming from the formation controller by using acceleration measurements, which includes the tension force from the load. This measurement allows the INDI controller to calculate the necessary increment in the previous control action for the motors.
Our proposed method brings substantial positive differences with respect to the current approaches in the literature on collaborative transportation with aerial vehicles. For example, we do not need centralized calculations or global positioning, nor do we require previously calculated paths to be tracked by the vehicles [6, 7, 8]. Therefore, the usage of accurate motion capture systems is not a crucial requirement for our approach. We will see that while we employ them in our demonstration, we do not require global positioning but we fake relative measurements that could be provided by an onboard system [9]. Another practical positive aspect of our approach is that we promote scalability. The algorithm can be executed in a distributed way, i.e., robots can calculate their own desired accelerations based only on local information, and the calculations can be done in simple microcontrollers. Therefore we do not require heavy computers for the practical task like in [10]. We do not require the individual vehicles to track any trajectories either. For example, such an approach, by a centralized motion planning, seems pivotal for the motion of the rotorcraft team in many works whose implementation is shown only in simulations [11, 10, 12]. Nevertheless, they consider active control over the payload.
The main contribution of this paper focuses on the practical demonstration of the combined approaches of the INDI controller together with the motion of the formation by disagreements for the collaborative transportation of objects. In particular, we will show how these two techniques can predict results on the deformation of the shape together with the control of the velocity of the formation.
This paper is divided as follows. In Section II we introduce the guidance system for the formation and motion of the rotorcraft. The controller responsible for tracking the guidance signals is described in III. We continue by presenting in Section IV a qualitative description of the forces involved in the system and how to quantitatively calculate tolerances in the formation and control gains from a worst case condition. We show the performance of our system in Section V with a team of four rotorcraft transporting a heavy object for an individual vehicle. We finally end the paper in Section VI with some conclusions.
Ii Guidance system for the formation-motion control of second-order robots
Ii-a Undirected rigid formations
Consider a team of robots and denote by their 2D positions in the horizontal plane parallel to the ground with respect to a fixed navigation frame of coordinates. The guidance system for the formation-motion of the team generates an acceleration signal to be tracked by the robots. In particular, the guidance will assume that the vehicles can generate such acceleration sufficiently fast in the fixed navigation frame as it is a common assumption in the literature [12]. Although our rotorcraft (quadcopter) can only generate a force along one axis in its body frame, we will see that the INDI controller adapts the attitude and the thrust of the vehicle fast enough so that the vehicle can track a sufficiently slow varying acceleration signal effectively, while it rejects disturbances such as the non-constant tension from the ropes. In particular, as we will see, the exponential nature of the proposed guidance and the INDI control will allow us to set the time constants for the exponential decay of their signals by appropriately selecting their gains. The guidance system considers the following dynamical model for the formation-motion task of the vehicles
(1) |
where
are the stacked vector of positions and velocities in the plane parallel to the ground respectively, and
is the stacked vector of accelerations generated by the guidance system.A robot does not need to measure its relative position with respect to all the robots in the team, but only with respect to its neighbors. The neighbors’ relationships are described by an undirected graph with the vertex set and the ordered edge set . The set of the neighbors of robot is defined by . We define the elements of the incidence matrix for by if or otherwise, where and denote the tail and head nodes, respectively, of the edge , i.e., . For undirected graphs, how one sets the direction of the edges is not relevant for the stability results or the practical implementation of the algorithm [13]. The proposed formation control algorithm is based on the distance-based approach, i.e., we are defining shapes by only controlling distances between neighboring robots. These shapes are based on the rigidity graph theory [3]. One of the advantages of controlling distances instead of relative positions is the freedom of the shape to be rotated and translated without modifying the controller. This fact will give us enough freedom to design both the motion and formation controller at once.
The stacked vector of the sensed relative positions by the robots can be calculated as
(2) |
where is the identity matrix, and the operator denotes the Kronecker product. Note that each vector stacked in corresponds to the relative position associated with the edge . The introduced concepts and notations are illustrated in Figure 1. As an example, in this paper we will focus on a regular square with the neighbors defined by the incidence matrix
(3) |
Let be the stacked column vector of fixed distances, associated to their corresponding edges, which defines the desired regular square. Then, the error signals to be minimized are given by
(4) |
The control action for each robot in order to stabilize the regular square can be derived from the gradient descent of the potential function involving all the error distances to be minimized and the kinetic energy of the agents
(5) |
where , which leads to the following control action [14] for each robot
(6) |
where each desired distance is associated with its corresponding , and the superscript over the vectorial quantities is used for the representation of a vector with respect to the local frame of coordinates of robot .
Ii-B Motion control
The control action (6) only achieves the task of converging (exponentially fast) to the desired shape where all the agents will be stopped [14]. In order to create motion we ask the agents to disagree on the distances , i.e., the second term in (6) is written as
(7) |
where is a designed disagreement and it is equal to zero in (6). Note that the second term in (7) will be the responsible of the desired motion of the agent during its steady state [14], e.g., once the regular square is achieved. In particular, these acceleration signals are linear combinations of the unit relative positions where the coefficients are the disagreements .
We design three rigid motions for the team: two orthogonal translations and one rotational following the systematic method explained in [14]. For each of these motions, each rotorcraft must follow a specific velocity with respect to the body frame as illustrated in Figure 1. We first write the desired velocities for each motion as linear combinations of so we can construct the following matrix with disagreements based on the incidence matrix (3)
(8) |
Let us write in compact form the stacked vector of all control actions (6) with disagreements as in (7) in the navigation frame
(9) |
where is the stacked vectors of all , we define the operator , and is the operator that takes each stacked element (vector or scalar) of and places them in a diagonal block matrix. The matrix will be function of the previously calculated . In particular, we have dropped in the third element of (9) since, as we will see, it can be cancelled out by scaling up or down the disagreements in (8).
We need to introduce different calculations with respect to [14] in order to figure out the matrix since in this work we deal with a different potential function (5) that leads to work with the unit vectors in the control action (9). We first need to define the velocity error .
Proposition II.1
The matrix is calculated from and is given by , where , and defines the desired steady-state rotational motion.
Let us define the stacked vector of desired velocities for the agents. In particular, we have designed it from linear combinations of the unit vectors of the desired relative positions by employing (8). Therefore we can write
(10) |
where we recall that is the stacked vector of desired distances and is the stacked vector of desired relative positions with respect to in Figure 1. Now we calculate the stacked vector with all the desired accelerations
(11) |
Let so that (9) can be written as
(12) |
therefore once , i.e., the formation is at the desired shape and velocity, we have that , and noting that for pure translational motion the accelerations are identically zero because in such a case, then from (11) and (12) we have that
(13) |
The key for showing the exponential stability of and lies on the fact that under the control (9) for these signals are exponentiall stable [14]. Then, the disagreements, i.e. , are treated as parametric disturbances as suggested in [15]. Therefore, if they are small enough (or and big enough), then we do not modify the exponential nature of the convergence of and .
Iii Rotorcraft control
Iii-a Incremental Nonlinear Dynamic Inversion controller
We have taken the concept of Incremental Nonlinear Dynamic Inversion (INDI) [16], and applied it to track the linear accelerations generated by the guidance system for the formation-motion of the rotorcraft. The idea behind INDI is that forces and moments acting on the vehicle are, according to classical mechanics, proportional to the acceleration and angular acceleration of the vehicle. The acceleration can be measured with the accelerometer, and the angular acceleration can be derived from the gyroscope. From a desired increment in linear and angular acceleration, the required increment in inputs can be easily calculated using a control effectiveness matrix. Since the 3D tensions from the rope will be measured by the accelerometer and gyroscope as well, they will be naturally counteracted by the controller, without having to model the load. Though the tension of the ropes may introduce small moments on the rotorcraft, the ropes are attached close to the center of mass and the expected effect on the attitude dynamics is negligible.
To obtain a measurement of the linear acceleration of a rotorcraft, we take the specific force measurement of the accelerometer, and add the gravity vector to that. The acceleration a small time step ahead can now be predicted with
(14) |
where is the acceleration in the navigation frame (the subscript denotes for current value), is the mass of the Bebop, are the three attitude angles roll, pitch and yaw, we group the pitch, roll and thrust in , and we finally define the matrix of partial derivatives of the thrust vector
(15) |
The measured acceleration incorporates disturbances, and the force from the rope tension. Because the acceleration measurement is noisy due to vibrations, we employ a low pass Butterworth filter, denoted with a subscript . In order to synchronize the signals, and will both be filtered such that they will have the same delay. The filtered signals are incorporated in (14) and the equation is inverted to obtain the following incremented controller
(16) |
where is the desired 3D linear acceleration for the rotorcraft in the navigation frame. In particular, the first two components of for each rotorcraft are given by (9) and the third or vertical component is generated by the PD controller
(17) |
where the gains and are chosen according to the stability analysis given in [16].
Remark III.1
To calculate in (16) we need to know
, which is experimentally estimated by a quadratic function
in a static airflow regime. Errors due to the simplifications in the modelling are expected to have a low impact on the performance. This is explained because of the incremental nature of the controller, i.e., if an increment of thrust does not give the desired acceleration, then another increment is applied in a similar way as an integral controller does. However, note that the INDI measures the disturbances and the tensions from the rope, and have a feedforward knowledge of the vehicle and its actuators.Iii-B Effect of the payload on the control
As we have discussed, no explicit knowledge about the payload is incorporated in the control of the rotorcraft since the INDI controller is naturally designed for dealing with non-modelled external forces by measuring them directly. For example, due to the incremental nature of the controller, feedforward control increments will be applied on the actuators as long as the desired acceleration is not reached, which means that steady state offsets will not occur. Consequently, external forces are incorporated in the control either by compensating them, or by taking advantage from them if for example the tension of the load and the desired acceleration have the same direction.
Iv Force and worst case analysis
In this section, as an illustrative example, we conduct an analysis to guarantee the safety of the system while it is close to a steady state, e.g., close to the desired shape with the desired velocity. We analyze the forces involved, in particular the ones in a worst case, and connect them to the physical limits of the vehicles together with the demands from the guidance system in Section II and the controllers in Section III. For the analysis, our vehicles are four rotorcraft Bebop from the company Parrot that have been characterized in our laboratory [16]. Each rotorcraft weighs around grams, an individual motor/propeller can generate Newtons of thrust once it is rotating at Hz in hover flight, and a motor/propeller generates a moment around m/s/rpm.
We describe the forces involved in the system in a steady state configuration as starting point. We set the desired shape as the regular square in Figure 1, where all the ropes have the same length so that the payload can be placed at the center of the square while hanging. This configuration allows an equal distribution of the payload’s weight across all the vehicles. The force diagram on the left of Figure 3 focuses on the vertical plane connecting vehicles and . It shows how the vehicles need to tilt to compensate the horizontal components of the ropes’ tensions. In particular, we define tilt as the angle formed by the thrust force and the horizontal plane parallel to the ground. If the rotorcraft are in equilibrium, is the mass of the payload, and is the fixed length of the rope, then the tension satisfies
(18) |
which increases clearly with . Therefore, we will set the shortest as possible depending on the sizes of the rotorcraft and the expected disturbances that will vary (equal to in the steady state).
The diagram on the right side of Figure 3 is in the plane described by the vehicles and . We describe the situation where we demand the same horizontal acceleration to the vehicles while keeping the altitude constant. In such a case, we see that the force in vehicle will create more tension on its rope since the vehicle is more tilted, and consequently it needs a higher to compensate for gravity. As a result, the vehicle will need to lift more weight from the payload than its neighbor. Because the altitude is constant, and both vehicles equally accelerate, then the payload experiences the same force as well.
We take the above described cases to calculate conservative bounds on the maximum deformation of the formation, i.e., on the norm of the error signal , on the gains and and the disagreements in (9) for a demanded motion once a rotorcraft is close to the equilibrium. We have measured that one actuator of the Bebops can generate around Newtons of force at the of its capacity, so we consider Newtons as a reference thrust force for the following worst case condition. We first estimate the maximum safe tilt for a rotorcraft. During the experiments, the initial positions of the rotorcraft are close to the desired one. Therefore, the payload will always stay close to the centroid as we will see according to the calculations in this section. We consider that a vehicle will share a third of the weight of the payload as a conservative worst case. In particular, we will deal with a payload equal to the mass of a single vehicle. Therefore the maximum vertical force to compensate will be Newtons. A simple trigonometric calculation reveals that the maximum tilt for a rotorcraft is radians. Therefore, we impose a maximum angle of degrees ( rads) for both pitch and roll so that the tilt is below 0.62 rads. Indeed, this relatively small angle helps the INDI controller since for its design we have assumed small increments in the commanded attitude signals. This estimation will leave around Newtons of maximum available force for a rotorcraft in the horizontal plane.
We then estimate what would be a conservative bound for a horizontal force in opposition to the one created by the thrust of the vehicle. Although present, we will not consider any aerodynamic drag since we will set the maximum vehicle’s speed in m/s, and the tension from the rope is substantially bigger than any drag at that speed. Focusing on vehicle , from equation (18) and basic trigonometry we can derive the expression for the bound on the horizontal tension of the rope
(19) |
where we assumed that the horizontal distance between the vehicle and the payload is approximately since the initial positions for the rotorcraft are close to the desired square, and the controllers will keep such a situation if they can cope with the predicted worst cases in this current analysis. We set the length of the rope to approximately meters, and for equation (19) we consider a very conservative meters since in the experiments we will set close to meter. As a result, an upper bound to is , and because the maximum available horizontal force in the worse case for the tilt is Newtons, we can conclude that the maximum norm of the acceleration to be asked by the guidance system is
(20) |
where Kg are the mass of the vehicle and the payload. Consequently, we impose on the vehicles that the maximum for each of the acceleration coordinates in the plane horizontal to ground is , i.e., a vector of magnitude 2.18 with 45 degrees with respect to the X and Y axis. For example, starting from the equation for the vehicle (and the same argument can be extended to the rest of vehicles) we can calculate conservative values for and from the following expression
(21) |
The first term in the equations in (21) is related to the desired speed in one of the horizontal directions. For example, we recall that for a desired translational motion, the desired velocity is designed by the disagreements in Section II. By design, we will not demand a higher speed than m/s, so we can safely assume . The second term refers to the control of inter-vehicle distances. Recall that we start close to the equilibrium with a desired square of side meter, therefore we can assume a very conservative worst case of meter since we consider as worst case for the horizontal tension a distance of meters between vehicles on the side of the squared formation, e.g., meters. Note that looking at Figure 1, almost equally contributes to both components, while for example can be safely omitted in the component. Finally, the disagreement constructs the desired centripetal acceleration towards the centroid, so the formation spins around it. Therefore, in order to satisfy the following worst case condition
(22) |
we assign , and , where sets the limit to the maximum angular velocity of the spinning motion of the formation. With these chosen values, the system remains stable and within its physical limits. Firstly, the forces from the payload in this analysis have been found substantially smaller than the disturbances that can be handled by the Bebops [16] with the INDI controller. Secondly, the incremental accelerations demanded by the guidance system are bounded and within the physical limits to be tracked effectively. Thirdly, the stability analysis of the guidance system [14] guarantees the maximum magnitude of the error signals and . In particular, if the accelerations are tracked correctly and the distances between the vehicles have an initial error of less than meters, then following [14] it can be derived that the maximum for a single error distance cannot be more than a meter if the requested velocity has a speed smaller than a meter per second. All in all, the correct performance of the team carrying a load is guaranteed under the described nominal conditions in this section.
Remark IV.1
For chosen the gains and , we checked in simulation that the desired acceleration signals have a time constant of around second for its exponential decay. The chosen gains for the INDI controller indicates a time constant of around seconds. Therefore, we can guarantee that the guidance system is at least 100 times slower in order to ensure the stability of the two slow-fast interconnected systems.
Remark IV.2
The payload is assumed at rest at the initial conditions, e.g., it is not swinging. Since the payload starts close to the centroid of the formation, and the guidance system guarantees that the centroid is under control, then the forces on the payload are very similar to the ones required to achieve the desired velocities of the formation as a whole. Therefore, as we will see in the experiments, we predict to do not have significant swing motions on the payload. If there are small disturbances on it, the INDI would detect them since changes on the tensions of the ropes are measured by the accelerometers. So, both the INDI and the guidance system will compensate such disturbances.
V Experimental results
The experimental results^{1}^{1}1A high-definition video of this section can be found at https://www.youtube.com/watch?v=HUZH46Oxc5c. are conducted in a controlled area, e.g., no wind or obstacles around. The Bebops are equipped with the Paparazzi autopilot^{2}^{2}2http://wiki.paparazziuav.org/wiki/ and their positions are obtained with an Optitrack camera system. An operator with a gamepad commands the movements in Figure 1 where the sticks set the magnitude of the disagreements of the matrix in Proposition II.1, i.e., the speed of the motions. The operator with the gamepad also controls the scale of the formation and its altitude at once, e.g., to control when the payload is lifted. The guidance system runs in a ground control station. It processes the relative positions of the robots and the input from the gamepad at the fixed frequency of Hz. We show in Figure 4 a caption of the experiments and the trajectories described by the team while it is steered by the operator. We would like to remind that the operator commands the formation as a single super-vehicle, i.e., the operator maneuvers the team as a single solid rigid body. In this experiment, the payload flies at around an altitude of m over the ground. The Figure 5 shows the actual distances between vehicles and their velocities in the navigation frame of coordinates. In particular, we show the distances of the two diagonals and two of the sides of the square as defined in Figure 1. The operator changes the scale of the formation (black-dashed line) over time at the same time he maneuvers the formation. In fact, in order to show the robustness of the system, during some parts of the experiment the operator changes quickly the direction of the desired velocity, e.g., check seconds 253 and 278. One can check that our worst-case analysis holds since the worst error distance we noticed is less than cm around second 296 during a spinning motion of the formation. In the video of the experiments, one can also check the predicted non-swing motion of the payload during its transportation.
Vi Conclusions
This paper has experimentally validated the predicted stability properties of a combined method (motion by disagreements + INDI) for the collaborative transportation of a payload by a team of rotorcraft. In particular, the whole setup allows us to perform an accurate analysis of forces and accelerations in the system. Consequently, this approach enable the possibility of performing a worst-case analysis such that a nominal operation can be guaranteed. Experiments are conducted with a team of four rotorcraft carrying a heavy payload, impossible to be lifted by a single vehicle. The experimental results match with the predicted nominal behaviors.
References
- [1] G.-Z. Yang, J. Bellingham, P. E. Dupont, P. Fischer, L. Floridi, R. Full, N. Jacobstein, V. Kumar, M. McNutt, R. Merrifield et al., “The grand challenges of science robotics,” Science Robotics, vol. 3, no. 14, 2018.
- [2] Z. Wang, Y. Hirata, and K. Kosuge, “Control a rigid caging formation for cooperative object transportation by multiple mobile robots,” in Robotics and Automation, 2004. Proceedings. ICRA’04. 2004 IEEE International Conference on, vol. 2. IEEE, 2004, pp. 1580–1585.
- [3] B. D. O. Anderson, C. Yu, B. Fidan, and J. Hendrickx, “Rigid graph control architectures for autonomous formations,” IEEE Control Systems Magazine, vol. 28, pp. 48–63, 2008.
- [4] H. G. de Marina, B. Jayawardhana, and M. Cao, “Distributed rotational and translational maneuvering of rigid formations and their applications,” IEEE Transactions on Robotics, vol. 32, no. 3, pp. 684–697, June 2016.
- [5] E. J. Smeur, Q. Chu, and G. C. de Croon, “Adaptive incremental nonlinear dynamic inversion for attitude control of micro air vehicles,” Journal of Guidance, Control, and Dynamics, vol. 38, no. 12, pp. 450–461, 2015.
- [6] Q. Jiang and V. Kumar, “The inverse kinematics of cooperative transport with multiple aerial robots,” IEEE Transactions on Robotics, vol. 29, no. 1, pp. 136–145, 2013.
- [7] K. Sreenath and V. Kumar, “Dynamics, control and planning for cooperative manipulation of payloads suspended by cables from multiple quadrotor robots,” rn, vol. 1, no. r2, p. r3, 2013.
- [8] N. Michael, J. Fink, and V. Kumar, “Cooperative manipulation and transportation with aerial robots,” Autonomous Robots, vol. 30, no. 1, pp. 73–86, 2011.
- [9] M. Coppola, K. N. McGuire, K. Y. Scheper, and G. C. de Croon, “On-board communication-based relative localization for collision avoidance in micro air vehicle teams,” Autonomous Robots, pp. 1–19, 2018.
- [10] A. Tagliabue, M. Kamel, S. Verling, R. Siegwart, and J. Nieto, “Collaborative transportation using mavs via passive force control,” in Robotics and Automation (ICRA), 2017 IEEE International Conference on. IEEE, 2017, pp. 5766–5773.
- [11] C. Masone, H. H. Bülthoff, and P. Stegagno, “Cooperative transportation of a payload using quadrotors: A reconfigurable cable-driven parallel robot,” in Intelligent Robots and Systems (IROS), 2016 IEEE/RSJ International Conference on. IEEE, 2016, pp. 1623–1630.
- [12] T. Lee, “Geometric control of quadrotor uavs transporting a cable-suspended rigid body,” IEEE Transactions on Control Systems Technology, vol. 26, no. 1, pp. 255–264, 2018.
- [13] K.-K. Oh, M.-C. Park, and H.-S. Ahn, “A survey of multi-agent formation control,” Automatica, vol. 53, pp. 424–440, 2015.
- [14] H. G. de Marina, B. Jayawardhana, and M. Cao, “Taming mismatches in inter-agent distances for the formation-motion control of second-order agents,” IEEE Transactions on Automatic Control, vol. 63, pp. 449–462, 2018.
- [15] H. G. de Marina, M. Cao, and B. Jayawardhana, “Controlling rigid formations of mobile agents under inconsistent measurements,” Robotics, IEEE Transactions on, vol. 31, no. 1, pp. 31–39, 2015.
- [16] E. J. Smeur, G. de Croon, and Q. Chu, “Cascaded incremental nonlinear dynamic inversion for mav disturbance rejection,” Control Engineering Practice, vol. 73, pp. 79–90, 2018.