First Experimental Results on Motion Planning for Transportation in Aerial Long-Reach Manipulators with Two Arms

This paper presents the motion planning of a novel aerial robotic system with a long-bar extension and two arms for long-reach manipulation in cluttered environments. The novel aerial long-reach manipulator includes a passive revolute joint between the aerial platform and the dual arm. This feature minimises the torque induced to the aerial system in case of unexpected collisions of the manipulator. The motion planning problem is addressed considering jointly the complete set of configuration variables for the aerial platform and the dual arm. Furthermore, the planner has been built over the fundamentals of RRT* algorithms in order to optimise the performance of the trajectories in terms of energy and time. The proposed planning method has been experimentally validated in a realistic industrial scenario, the transportation of a long bar through a cluttered environment consisting of several pipe structures.


I. INTRODUCTION
Among the numerous applications in which unmanned aerial vehicles (UAVs) can be used, aerial manipulation is arousing much interest. Potential applications in this field include instrument deployment, maintenance operation and contact inspection in industrial sites in which the access is very dangerous or costly. The motivation is to decrease risks and operational costs. Small size rotorcraft can indeed access to hard-to-reach places avoiding unnecessary risks for industrial workers and allowing maintenance operations without neither shut-downs of the facilities nor the use of scaffolding or cranes.
In order to perform the above applications it is necessary that adapted arms and grippers can be seamlessly integrated into the airframe. Furthermore, the existing algorithms for operating autonomously the UAV and the manipulators should be extended for the integrated system. In this respect, one of the most challenging issues is the development of new methods that consider simultaneously the UAV and the manipulator when planning the motion of the complete system. When moving inside a dense industrial installation, this integrated planning will be essential for the generation of accurate and collision-free movements close to obstacles.
Many research works about aerial manipulation have been recently published. [1] presents the design of several lightweight, low-complexity grippers that allow quadrotors to 1  grasp and perch on branches or beams and pick up and transport payloads. In a larger system scale, [2] proposes a system for aerial manipulation, composed of a helicopter and an industrial manipulator. However, very few contributions consider configurations with more than one arm like the articulated ARS-LRM system presented in this paper. The need to employ several arms can be justified in special tasks such as transportation of long pieces (to avoid swinging movements), application of torques or other manipulation tasks that require two hands simultaneously. Thus, [3] proposes a dual arm aerial manipulator to turn a valve that requires a tightly integrated control scheme between aircraft and both manipulators. On the other hand, in [4] a humansize and lightweight dual arm manipulator is integrated in a multirotor platform and tested in outdoor flights.
Despite that a large amount of works have been focused on the development of control techniques for aerial manipulators, not many of them deal with the associated motion planning problem. Furthermore, the existing contributions like [5] usually assume a strong simplification by addressing the planning problem in a decoupled way, i.e. adopting independent planners for the UAV and the manipulators that swap their operation according to the mission phase. In contrast to previous works, the authors of this paper presented in [6] a novel algorithm that considers jointly the aerial platform and the manipulators within the planning operation. This integrated strategy allows the consideration of wider and safer operating conditions. This paper presents the first experimental results corresponding to the approach proposed in [6]. This research line explores new configurations for aerial long-reach manipulation in cluttered environments. To this end, a novel aerial robotic system with two arms for long-reach manipulation (ARS-LRM) has been developed. More precisely, the system consists of a multirotor with a long bar extension that incorporates a lightweight dual arm in the tip (see Fig. 1). This configuration allows aerial manipulation tasks in hardto-reach places increasing considerably the safety distance between rotors and manipulated objects.
Concerning the motion planning problem, the aim is the development of planning strategies to command the ARS-LRM system in a safe and efficient way throughout cluttered environments. The motion planning of multi-body robots like the ARS-LRM system brings new challenges that require methodologies different from those used in traditional approaches. To this end, the motion planning is addressed considering jointly the aerial platform and the dual arm within the planner operation. This integrated strategy allows the consideration of a more complete set of system states that in turn will make it possible to achieve wider operating conditions. Regarding the operation basis of the planner, the fundamentals of the presented strategy have been built over an RRT*-based algorithm. The objective is to guarantee energy and time efficient trajectories for a system with limited manoeuvrability like the ARS-LRM system.
In order to extend the results presented in previous contribution [6], this paper proposes a realistic industrial scenario to carry out the first experimental outdoor tests. The objective is addressing the motion planning required for the transportation of a long bar through a cluttered environment consisting of several pipe structures. In addition to the replication of the industrial facility, this work also considers a new version of the ARS-LRM system. More precisely, an articulated version of the integration between the aerial platform and the extension bar is proposed to make the system safer for these first experiments. Indeed, this modification minimises the torque induced to the aerial platform in case of unexpected collisions of the long-reach manipulator with the pipe structures. The proposal of the new articulated ARS-LRM system also required the derivation of new modelling and control developments in order to carry out the validation simulations before the experimental work. The transportation experiment was performed satisfactorily according to the safety and efficiency objectives, and the resulting trajectories were similar to the simulations launched previously.
Sec. II describes the aerial robotic system for long-reach manipulation. This description firstly covers the mechanical construction of the aerial manipulator and the required mechatronics. Later, it presents the multi-body dynamical model and the derived control approach required to carry out the validation simulations. Then, in Sec. III, the motion planning algorithm is presented. After that, the experimental validation of the proposed approach has been addressed in Sec. IV. The latter includes the description of the industrial scenario, the validation simulations and finally the experimental results when the articulated ARS-LRM system follows the planned trajectory to transport the bar through the scenario. The last section Sec. V is devoted to conclusions.

A. Mechanical Construction and Mechatronics
This section presents the mechanical construction of the proposed aerial robotic system with two arms for long reachmanipulation (ARS-LRM). As can be seen in Fig. 1, the system consists of a multirotor with an articulated long bar extension that incorporates a lightweight dual arm in the tip. The idea of a long-reach, dual arm aerial manipulator was firstly introduced in [6] and [7]. The new prototype is built from the anthropomorphic and lightweight dual arm aerial manipulation system presented in [8], although in this case, the arms were installed over the landing gear. Unlike [7], the flexible long-reach link that supports the arms is not rigidly attached to the base of the multirotor platform, but it is supported by a passive revolute joint. This longreach configuration allows aerial manipulation tasks in hardto-reach places increasing considerably the safety distance between rotors and manipulated objects. At the same time, the passive revolute joint prevents that the manipulator generates high torques over the base of the aerial platform that could not be compensated using the lifting forces generated by the propellers.
Concerning the different subsystems that compose the articulated ARS-LRM system, the aerial platform is a hexarotor whose characteristics are shown in Table I. On the other hand, the long-reach link is a 2.5 × 80 × 0.2 cm anodized aluminium profile which can rotate freely in the pitch angle thanks to a pair of EFOM-08 flange bearings. Finally, the manipulator is the anthropomorphic and lightweight dual arm previously developed in [8]. Each arm provides four degrees of freedom (DOF) in a human-like kinematic configuration to ease its operation for an untrained operator. However, only 2 DOFs (shoulder pitch and elbow pitch) have been considered in this work to simplify the analysis of the obtained results. The complete mechanical specifications of the ARS-LRM system are summarized in Table I.
With respect to the mechatronics required to put into operation a complex system like the ARS-LRM, the resulting architecture is depicted in Fig. 2. An Intel NUC computer onboard the multirotor executes three main software modules: 1) the Planner Dispatcher that reads the trajectory file generated by the motion planner, 2) the UAV Abstraction Layer, a ROS node that interfaces with the low level controller of the aerial platform, and 3) the External Joint Reference  Controller. The low level controller of the hexarotor is a commercial Pixhawk Autopilot, including GPS and IMU sensors. Concerning the manipulator, eight Herkulex servos manage the operation of the joints.

B. Modelling and Control for Validation Simulations
This section presents the new modelling and control developments required for the novel articulated ARS-LRM system presented in this paper. A planar characterization will serve for simulating the result of commanding the planned trajectories before the experiments. This simplified approach eases the modelling and control derivations while maintaining the operation basis of the system. Moreover, this assumption allows to neglect the deflection of the flexible long-reach link as it takes place in the perpendicular direction to the forward movement of the system. The latter has been proved to be valid through experimentation, which also revealed that the air flow produced by the multirotor propellers reduces considerably such deflection.
According to [9], the dynamics of a multirotor is mostly determined by its mechanical model. This paper embraces the same assumption and consequently the behaviour of the ARS-LRM platform will be described by means of an elaborated mechanical model of the complete multi-body system. Kane's method has been used with this purpose since it holds some unique advantages when addressing multi-body systems like the ARS-LRM. Of the latter, the most remarkable are the derivation of a compact model in first order differential equations that are uncoupled in the generalized speed derivatives as well as the easy computerization and the computational efficiency of the resulting equations of motion.
The configuration variables selected as system generalized coordinates are the longitudinal q 1 and vertical q 3 positions of the UAV center of mass A O in the inertial reference frame N , the multirotor pitch angle q 5 , the passive revolute joint angle q 0 and the joint angles both for left L and right R arms q L 7 , q L 8 , q R 7 and q R 8 (see Fig. 3). Generalized speeds u i are defined as: where N v A O is the velocity of the UAV center of mass A O with respect to the inertial reference frame N and i ω j is the angular velocity of the element j with respect to the element i (see Fig. 3 to identify the different elements). Previous equations lead to the following kinematic differential equations:q Regarding forces and torques exerted on the ARS-LRM system (see Fig. 3), the rotors generate a resultant lifting force F 3 a 3 applied at the multirotor center of mass A O as well as a torque T 2 a 2 applied to rigid body A. On the other hand, control actions governing the manipulator are given by the torques applied to the arm joints T R 7 a 2 , T R 8 a 2 , T L 7 a 2 and T L 8 a 2 . Application of Kane's method through MotionGenesis software [10] leads to the following dynamic differential equations for translation and rotation, where g is the gravity acceleration and A, B, C and D are dense matrices depending on the configuration variables q 5 , q 0 , q R 7 , q R 8 , q L 7 , q L 8 and the parameters defining geometry and mass distribution of the ARS-LRM system.
Concerning the control architecture, the same distributed control scheme (see Fig. 4) proposed in [6] has been adopted to provide the simulated ARS-LRM system with the capability of tracking trajectories generated by the motion planner. The resulting control laws make use of nonlinear strategies based on model inversion.

III. THE MOTION PLANNER
Sampling based planners like the family of RRT algorithms [11] have demonstrated high potential in finding fast solutions for high-dimensional robots [12]. Furthermore, some of these methods bring the possibility of generating motion plans that optimize certain cost functions, as for the case of RRT* variations [13]. This makes it possible to find an optimal solution in terms of a specific metric. Taking all these considerations into account, the same RRT*-based algorithm presented in [6] that optimizes energy and time performance has been selected for the articulated ARS-LRM system in this work.
Another determining factor for planner performance is the planning space considered when exploring the different possibilities of motion. In this work, following again [6], the complete set of configuration variables introduced in Sec. II-B for the aerial platform (with the exception of pitch angle q 5 ) and the long-reach manipulator are considered jointly within the planning algorithm (see the variables in green colour in Fig. 3). This integrated strategy allows the consideration of a more complete set of system states. In this way, it is possible to achieve wider and safer operating conditions since equivalent configurations in terms of final effector positions can be differentiated according to the positions of both the multirotor and the intermediate links.
Although the proposed algorithm poses certain structural similarities to the well-known RRT* approach (see Algorithm 1), most of the intermediate functionalities have been customized to deal with the aerial manipulator under study. The most representative developments in this respect are presented below.

A. Computation of the Nearest Node
The N EAREST (T ree, x rand ) function finds the nearest node x nearest to the random state x rand generated in the sampling-based exploration of the planning space. Since nodes include state information both for multirotor and dual arm accordingly with the integrated operation basis of the planner, there will be two different measurements for calculating the nearest node: the difference in position for the multirotor and the difference in angle for the arm joints.
Thus, there appears the need of defining a homogenizing x rand ← SAM P LE() 4: x nearest ← N EAREST (T ree, x rand ) 5: x new ← ST EER(x nearest , x rand ) 6: if ∼ COLLISION (x nearest , x new , map) then 7: x near ← N EAR(T ree, x new ) 8: T ree ← ADD(x nearest , x near , x new ) 9: T ree ← REW IRE(x near , x new ) 10: end if 11: end for 12: trajectory ← T RAJECT ORY (T ree) metric. The reference velocities u ref (for the UAV) and w ref (for the joints) have been defined with this purpose of transforming the heterogeneous measurements into a common metric given by the time magnitude required for each system component to move between the configurations associated with the nodes under analysis. The equations corresponding to this normalization approach are presented below: where ∆q i denotes the increment in variable q i when going from the tree node x to the sampled node x rand , that is,

B. Collision Checking
The COLLISION (x nearest , x new , map) function checks if the branch that would link two nodes produces some collision with the obstacles included in the map. To this end, a representative set of intermediate configurations between the nodes is generated using interpolation. Then, each intermediate configuration is investigated to see if any part of the system collides with the obstacles defined in the scenario.
The above operation deserves special attention since it plays an important role in the advanced functionality of the ARS-LRM planner that allows to differentiate equivalent configurations in terms of final effector positions according to the positions of both the multirotor and the intermediate links. The consideration of the different geometries of the system components, together with joint exploration of the planning space for both system components, are crucial features in this respect. Concerning the former, simplified models that alleviate the computational burden of collision checking but maintaining at the same time their capability to express the heterogeneity existing in the geometry of the different parts, are the desirable option. To this end, the multirotor has been considered rectangularly shaped while the dual arm and the long bar extension are modelled by rectilinear bars with negligible section. Regarding the obstacles, all of them have been considered round or rectangular. In this way, it is possible to approximate complex-shaped obstacles with simple shapes that reduce the complexity of the collision checking algorithm.
Another aspect is the algorithm selected for detecting the collisions. In the case of the multirotor, the approach is straightforward since it only requires checking whether the position of the center of mass is within the limits of the rectangular region that produces collisions with the obstacle (see the left part of Fig. 5).
In contrast, the collision management for the extension bar and the dual arm consists of translating the collision condition to the angular space as shown in the Fig. 5 (right). In this way, the obstacle is characterized in terms of the minimum and maximum link angle that may produce a collision. Then, taking into account also the distance to the obstacle, it is possible to check the collision with a considerably reduction in the computational load with respect to standard procedures.

C. Computation of the Set of Near Nodes
The N EAR(T ree, x new ) function finds the set of tree nodes x near that satisfy simultaneously the following conditions with respect to their distances to the new candidate node x new : the difference in multirotor position is less than threshold γ U AV and the differences in link orientations are all less than threshold γ ARM S . This definition can be expressed mathematically as follows: where ∆q i denotes the increment in variable q i when going from the tree node x to the new candidate node x new , that is,

D. Cost Functions
In order to apply the RRT* optimization sequence within the ADD(x nearest , x near , x new ) and REW IRE(x near , x new ) functions, two different cost indices have been defined: the operation time of the complete system (CF T ), and the linear and angular displacements produced in the multirotor and the dual arm joints respectively (CF E ). These cost indices can be formulated as follows: where t U AV and t ARM S were defined in Eq. (4); ρ U AV was defined in Eq. (5); σ ARM S = ∆q R 7 + ∆q R 8 + ∆q L 7 + ∆q L 8 with ∆q i denoting the increment in variable q i between the nodes in which the cost function is being evaluated (∆q i = q to i − q f rom i ); and p 1,2 are two weighting parameters that allow the prioritization of movements with minimum displacements in the multirotor or the dual arm.

A. Application Scenario: Long Bar Transportation
In order to demonstrate the validity of the motion planning strategy presented in previous section, the algorithm will be tested in a realistic industrial scenario given by the transportation of a long bar. The schematic description of the scenario is shown in Fig. 6 (above), where yellow elements correspond to a existing pipe structure (also presented on the side below) and the dotted surrounding areas denote the safety regions both for pipe structures and operational limits whose violations will be treated as collisions. As depicted in the figure, the ARS-LRM system will be commanded to transport a long bar along the longitudinal direction ( Fig. 1 illustrates the real system with this purpose). Since both the transported bar and the pipe structures exceed the planar characterisation presented in Fig. 6, it is worth mentioning that the potential collisions that could take place between both elements outside the ARS-LRM movement plane are represented with the following criteria in Fig. 6: green colour denotes the region where both the ARS-LRM system and the transported bar do not produce any collision, and red colour the area where ARS-LRM system could navigate but in contrast the presence of the transported bar would provoke a collision. Accordingly with the former, please note that the transported bar could only be passed through the lower (and wider) part of the large pipe structure in the right side.

B. Simulation Results
This section analyses the simulation results corresponding to the application of the motion planning algorithm presented in Sec. III in the scenario previously described. Thus, the motion planner has been firstly executed for the scenario under study and then, the resultant plan has also been provided to the controlled ARS-LRM system. The purpose is to analyse the close-loop behaviour of the system when following the planned trajectories.
The simulation work has been carried out in a Matlab-Simulink framework that provides the graphical evolution of the system variables. In order to better illustrate these results, intuitive snapshot diagrams that summarize the system On the other hand, Fig. 8 shows the evolution of the ARS-LRM system variables (blue line) when tracking the planned trajectory (dashed black line). As can be seen, the motion planner leads the ARS-LRM system to an efficient trajectory in terms of time and energy without producing collisions with the existing pipe structures. More particularly, the figures show how the planner generates an efficient trajectory with a reduced number of ARS-LRM movements thanks to the optimization sequence of the planner as well as the joint consideration of both the multirotor and the longreach manipulator within the planning operation. A complete animation of the simulation results can be found in [14].

C. Flight Tests
Once the presented approach for motion planning of the ARS-LRM system has been validated through simulation, the next step is the experimental validation in the real outdoor scenario (see Fig. 6). For that purpose, the implementation of the ARS-LRM system presented in Sec. II has been commanded to track the same planned trajectory that resulted from the application of the motion planner in previous Sec. IV-B. A complete video of the execution can be found in [15] and a representative set of snapshots are shown in Fig. 9. According to these results, the ARS-LRM system is able to transport the long bar along the cluttered scenario without any collision with the environment and following an efficient trajectory. In order to better analyse the latter, Fig. 10 shows the evolution of the ARS-LRM system variables (red line) in the same manner as in Fig. 8. The similarity existing between the executed trajectories shown in both figures endorses the validity of the proposed approach.

V. CONCLUSIONS
This paper has presented motion planning strategies focused on a novel aerial robotic system with two arms for long-reach manipulation (ARS-LRM). The objective was to move the system, composed by a multirotor and a long bar extension with a dual arm in the tip, through cluttered environments in a safe and efficient way. The motion planning of multi-body robots like the ARS-LRM system brings new challenges that can not be satisfied with traditional approaches. In order to achieve wider operating conditions, the planner considers jointly the aerial platform and the dual arm. On the other hand, the limited manoeuvrability of the system emphasizes the importance of energy and time efficiency in the generated trajectories. Accordingly, a RRT*-based algorithm has been employed to guarantee their optimality.
In order to extend the results presented in previous contributions, this paper proposes a realistic industrial scenario to carry out the first experimental outdoor tests. The objective is addressing the motion planning required for the transportation of a long bar through a cluttered environment consisting of several pipe structures. With this purpose, a new version of the ARS-LRM system is considered. More precisely, an articulated version of the integration between the aerial platform and the extension bar is proposed to make the system safer for these first experiments. Indeed, this modification minimises the torque induced to the aerial platform in case of unexpected collisions of the long-reach manipulator. Furthermore, the proposal of the new articulated ARS-LRM system also required the derivation of new modelling and control developments to carry out the validation simulations before the experimental work. The transportation experiment was performed satisfactorily according to the safety and efficiency objectives, and the resulting trajectories were similar to the simulations launched previously. The latter allows to conclude that the proposed motion planning approach is valid for the new articulated ARS-LRM system.