Trajectory Generation of a Quadrotor Transporting a Bulky Payload in the Cluttered Environments

When an aerial robot carries a bulky and heavy payload, it suffers from a wavering effect because of the large inertia of the heavy mass of a payload, which can cause a serious crash or fall to the ground.We propose the path-planning algorithm for an aerial robot to pass a narrow space while transporting a bulky payload to resolve the issue. Unlike the existing conventional path-planning method that treats the payload as a point-mass, this paper generates the trajectory considering the shape of a payload and passage size of an Unmanned Aerial Vehicle (UAV). First, we design a map-generation algorithm with the Voronoi diagram by exploiting the fact that the UAV has an ellipsoidal shape in transport operation with a bulky object. The modified map data can be exploited for the global path generation such as A* path-planner. For the rotational motion of the UAV, the heading angle was determined by considering the lengths of the major and minor axes of the ellipse. The global path was optimized based on the elastic band optimization algorithm to solve the jerky motion on the trajectory. Through the simulations and experiments, we validate the efficiency of our algorithm in the environment with the narrow space. Experimental results showed that the proposed path-planning algorithm could be used for safe aerial transportation in realistic environments.


I. INTRODUCTION
An increase in the popularity of online shopping has been driving the development of aerial robots for efficient logistics management. Aerial robots are used for aerial delivery [1]- [4] or warehouse management [5]- [7]. However, several issues, such as safety and computational efficiency for path planning must be addressed to transport a bulky payload. Conventional aerial transportation methods [2]- [6] are difficult to use in warehouses or factories with limited spaces because they assume an aerial robot or payload is shaped as a point-mass or circle. Considering a robot or cargo as a circle may not result in a path through a narrow space because of an empty space in the circle.
For the motion planning of a rigid body, rapidly exploring random tree (RRT) or rapidly exploring random tree star (RRT*) is a well-known planning algorithm to generate a path in a narrow space. In [2], the global path was generated using RRT* for multiple aerial robots to carry a bulky payload. In [8], an RRT-based planning algorithm and a model predictive control-based coordination of unmanned aerial vehicles (UAVs) were developed for cooperative aerial robots carrying large, cable-suspended payloads. However, the sampling-based method, such as RRT, has the disadvantage of time consumption in narrow corridors, because of random sampling characteristics. In [9], they resolved this bottleneck issue by calculating each exit as the goal position of RRT in a narrow passage. However, this method needs exact information of narrow space and is slower than conventional RRT. In addition, since the trajectory generated by RRT is not smooth, an aerial robot suffers from disturbances due to the large momentum of bulky payload when robots rotate.
For cooperative aerial robots [10]- [12], an optimization method is used. In [10,11], they presented a trajectory optimization algorithm for cooperative quadrotors with cablesuspended payloads. Despite showing promising experimen-Passage UAV 3D Map FIGURE 1: A Real-time 3D mapping and trajectory, passing the passage using unmanned aerial vehicle tal results, the algorithms in [10,11] can only be applied to aerial robots with cable-suspended payloads in which drones can change shapes. As a result, it cannot account for the effect of high momentum due to the payload. [12] presented a planning method for a fleet of rigidly attached quadrotors transporting heavy objects and trajectory optimization under these output wrench constraints. However, these methods also assumed that the obstacles were modeled as cylinders of infinite height [11] or a fleet of quadrotors forming a circle [12]. In addition, the method in [12] is only verified in simulation and the method in [11] takes a relatively long time to create a path in the narrow-space scenario.
Another method for handling a bulky payload is proposed based on Voronoi graph [13,14]. In [13], they used the overlapped region of generalized Voronoi graph (GVG) circles for sampling free space. Because this method reduces the sampling space, the time consumption of RRT, which has the disadvantage of calculating sampling in all free space, is reduced. They used RRT for sampling of space including a bubble shape with distance information from obstacles. However, it still has a bottleneck problem in confined spaces. In [14], they used k-dimensional tree (KD-tree) to find obstacle distance and made circles based on distance as radius such as GVG. They used the overlapped region for free space and optimized paths in the bounded overlapped regions to avoid obstacles. Although GVG is a useful method according to [13,14], there is still a bottleneck problem in the narrow passage [13]. The method in [14] cannot consider the heading angle of a robot, which can cause a collision of drones with a bulky payload when passing through narrow paths.
To resolve the mentioned issues, we proposed a viable planning framework for a quadrotor transporting a bulky payload (See Fig. 1). Our contributions are summarized as follows: First, a quadrotor with the desired trajectory for a bulky payload was generated in real-time in a narrow passage or complex environment. We created a map-generation algorithm based on a modified Voronoi diagram and an ellipsoidal payload model. The distance between the ellipse's major and minor axes was used to calculate the rotation of an aerial robot. Unlike [11,13,14], our algorithm can generate the desired path of complex-shaped drone in real time. Sec-  ond, we designed a heading-angle optimization algorithm to minimize the angular velocity to pass narrow passage, which minimized the wavering effect of the aerial robot. While our previous work [15] performed trajectory smoothing and speed optimization of a drone without a payload, this paper can handle a drone with a bulky payload in narrow passage. Third, we validated the performance of our proposed framework by conducting simulations and practical outdoor experiments. Moreover, we analyzed the computation time compared to the state-of-art planning algorithm for a rigid body payload. The experimental result showed the feasibility of the proposed algorithm for enabling aerial transportation.

II. METHOD
This section explains generating a global path using a Voronoi diagram with a proposed corner-extension rule. The overall structure is described in Fig. 2. First, to generate original map data in this paper, we used the FIESTA algorithm by HKUST [16]. By using Euclidean signed distance field (ESDF), which means distance from obstacles, FIESTA makes an ESDF map based on point cloud or depth image. This algorithm creates a 3D map as a point cloud (See Fig. 1). FIESTA also provides a 2D ESDF map exploiting if R ≥ r l then 12: extend(Obs) ← r l 13: end if 16: end for distances from obstacles through a 3D map. To implement our algorithm, we modified the data type as a grid map. Next, we modified the map using the Voronoi diagrams and circles. The global waypoint for an aerial robot with a bulky payload was generated using the A* algorithm, and the size of the Voronoi circle corresponding to each waypoint was obtained. This Voronoi circle information was used to determine the desired heading angle. The detailed process for the heading angle is addressed in Sec. III.

A. VORONOI DIAGRAM AND MAP MODIFICATION
Vertices of the Voronoi diagram can provide a safe region for path planning because the vertices are medial axis. This is very useful in determining whether a robot can pass through that space. However, in the conventional Voronoi diagram, we cannot consider the size of a payload. To resolve this issue, first, we assumed that a robot with a bulky payload is an ellipsoidal shape shown in Fig. 3 (a). The length of the major and minor radius of the ellipse was exploited to decide robot rotation and validity in space. To consider the shape of a payload, we proposed the map modification algorithm.
Our proposed method needs vertices of the Voronoi diagram based on a grid map. We generated the Voronoi diagram (i.e., Fig. 2(a)) using [17] because this algorithm provides vertices and distances from obstacles. We drew the Voronoi circles (i.e., Fig. 2(b)) which had the distance to the obstacle as the radius in the map generated by the Voronoi diagram. In this case, since the presence of numerous circles can cause degradation of computational efficiency for the map modification (i.e., Fig. 2(c)), we decreased the number of circles. If the distance between two random circles is below 20 percent of the sum of two radii of circles as follows then one of the two circles is deleted. Here d is the distance between two circles and r 1 and r 2 are the radii of i-th circles. Before generating the path, the corner-extension method was used, as shown in Fig. 3 (b). In conventional A*, the shape of a robot or a payload could not be considered. The method using configuration space obstacles [18] is exploited conventionally. However, this method used only fixed shape of the robot without considering rotation. We modified the map using corner extension with the length of the major radius (r l ) and the minor radius (r s ) to address this issue.
Radius of Voronoi circles was used for operating the corner extension. In Alg.1, V consists of the position (V pos ) and radius(V rad ) of the Voronoi circles. Obs consists of the position of obstacles in the grid map (Obs 1 and Obs 2 in Fig. 3 (b)). We found circles whose radius is the distance from the obstacle to the center point of the circle using distance(point, V pos [j]) function (Line 6-8). For example, as shown in Fig. 3 (b), the nearest circle for Obs 1 is C 1 and C 2 . The circle for Obs 2 is C 3 . Next, we found the nearest circle and saved the radius of that circle among other circles (Line 10). The nearest circle for Obs 1 and Obs 2 is C 2 and C 3 respectively. If the shortest radius is longer than r l , the obstacle size is extended with the radius r l (Line 12). For example, since d 3 in Fig. 3 (b) is larger than r l , the corner for Obs 2 is extended with radius r l . In this area, UAV (Unmanned aerial vehicle) can pass a narrow passage with any heading angle. On the other hand, the obstacle size is extended as r s when the radius is shorter than r l (Line 14). Since the radius of the nearest circle for Obs 1 (i.e., d 2 ) is shorter than r l and larger than r s , the corner for Obs 1 is extended with radius r s . In this area, UAV should rotate to travel a narrow passage, which means that only the thin shape of the UAV can pass. If the radius is shorter than r s , this area is not used for the UAV with any heading angle in this narrow passage. An integrated map to use the former mentioned methods can be used A* algorithm. If the robot turns appropriately, the map makes the UAV pass the narrow corridor.

B. OVERLAPPED REGION
To improve the computational efficiency of the path planning, we reduced the sampling space for A*, known as free-space reduction. For this reason, we used the overlapped region of circles. The overlapped region is a safe area for path planning, corresponding to the blue box in Fig. 3 (c). For example, we selected two circles. Each radius was defined as r 1 and r 2 and distance from centers of circles as d. By the second law of cosine, we can find the blue area using d and β as free space. When this equation is applied to all the circles, the integrated areas depict free spaces.

C. FIND NARROW PASSAGE
The global path consisted of discretized waypoints generated by A*. Before determining the heading angle, we found the nearest Voronoi circles from each waypoint and compared the major and minor radii of the ellipse with the radius of the nearest Voronoi circle. The robot could pass through the corridor when the radius of the nearest Voronoi circle was larger than the minor radius of the ellipse. When the radius of the circle corresponding to the previous waypoint was larger than r l and the radius of the circle corresponding to the current waypoint was shorter than r l , in that case, the UAV could rotate and pass the corridor. It could also rotate in the opposite situation. Therefore, we defined this waypoint for the rotation flag. The flags are used in section III-B.

III. OPTIMIZATION AND HEADING-ANGLE DECISION
In this section, we explain our optimization method for generating smooth trajectory for narrow passage.

A. PATH OPTIMIZATION
To obtain the smooth trajectory, we used the elastic band optimization proposed in [15,19] such as Fig. 5 (a). Elastic band optimization is based on elastic forces and waypoints P, the optimized waypoint Q are defined as follows: where m is the total number of waypoints generated by A*. Each force between waypoint Q k and Q k+1 is defined as F k , and a difference of force between F k and F k−1 in Q k is denoted N k .
To simplify the problem, we assumed that the waypoints were densely distributed over a trajectory and the angle between Q k and Q k+1 (i.e., θ k ) was smaller than 1 radian. This assumption was easily solved by using trajectory interpolation. In this case, we could minimize the sum of the lateral force N k for the whole trajectory, but the lateral force N k constrained as Here, d is the maximum distance between Q k and Q k+1 . R min means the minimum radius of the safe region. If R min is smaller, the angle between waypoints is close to 90 degrees. Finally, waypoint Q k is minimized based on For the safety of a drone, Q k should be regenerated within the region R radius (See Fig. 5 (a)). In this paper, the safety regions vary depending on the radius of the nearest Voronoi circle. For example, if the radius of the nearest Voronoi circle is larger than r s and smaller than r l , then the R radius is set to r s , otherwise, R radius set to r l .

B. HEADING-ANGLE SELECTION AND OPTIMIZATION
The algorithm mentioned in Sec. III-A was used only for the waypoints and not the heading angle. For the decision of the heading angle, we first decided the direction of the optimized path from section III-A and by exploiting the rotation point described in Sec. II-C, a robot was rotated to pass a narrow passage. In this case, the heading angle was selected. The if k == p c then 10: end if 12: end for 13: Optimization(Ψ, Y ) ← eq. (8) drone rotated in a direction that minimized rotation in a positive or negative direction based on the target path. Until passing the narrow space, rotation direction was maintained.
Heading-angle optimization is needed for minimizing the wavering effect. Because this effect is caused by the momentum of the payload, reducing the heading-angle rate of the desired trajectory is helpful. To minimize the headingangle rate, the optimization formula is set for minimizing the heading-angle acceleration. The heading-angle Ψ and the optimized heading-angle Y are defined as follows: The superscript f means the flag for robot rotation. For the optimization process, we define the angular velocity (i.e., For the heading angle optimization, we have to consider the following situation as constraints. First, to synchronize the moving direction of the drone with the direction of the camera, we added constraint that the difference between Ψ f k and Y k is lower than π/2 (11). This constraint kept the drone looking forward. Second, because of the discontinuity of the heading angle near −π and π caused by arc-tangent function, the desired heading angle could be volatile during optimization process (i.e., the left blue box in Fig. 5(b)). To solve this problem, we represented the heading angle as a continuous variable by adding or subtracting 2π to the original value. Third, since the optimization process minimized the variation of the heading angle, the target angle might not change even at the point where the target heading angle should change at the rotation point (See the right red box in Fig. 5(b)). So, an additional constraint was required for both the target heading angle and the optimized heading angles to have the same value when passing the narrow passage (10). The optimization process with the initial and final heading angle (9) can be written as The detailed process for the heading angle optimization is described in Alg. 2. We first found the rotation point (i.e., p s ∈ 1, ..., m) given the optimized waypoints (Line VOLUME 4, 2016 3-5). Then, by using p s and the next rotation flag (i.e., p e ∈ 1, ..., m) obtained by nextRotation function (Line 6), we computed the middle waypoint p c = (p s + p n )/2 between two rotations (See Fig. 6, Line 7). To maintain the rotation while traveling the narrow passage, we added further constraints for the optimization (Line 10). By solving optimization problem, we finally obtain the trajectory (Line 13).

C. VELOCITY OPTIMIZATION
To apply our algorithm for the experiment, we interpolated the trajectory with respect to the user-defined arrival time and optimized the velocity in each waypoint. In a simulation or experiment, the distance is too far to command the UAV to position control, resulting in a very fast velocity. To resolve this issue, we interpolated the trajectory using a polynomial equation and the user-defined maximum velocity information. The detailed process is described in our previous work [15].

IV. SIMULATION
To validate the proposed algorithm, we tested our method through simulations and experiments. Our trajectory generation is implemented in C++ and the optimization tool is developed based on the Gurobi optimizer [20]. Desktop specifications are 2.9 GHz Intel Core TM i7-10700 CPU and 32 GB RAM. System environments are robot operating system (ROS) in Ubuntu 18.04. We used a gazebo-based RotorS simulator manufactured by ETHZ [21]. The payload object has a mass of 300 g and 1.5 m. In the simulation, we used the position controller already implemented on the RotorS simulator without any modification.

A. SIMULATION RESULT
We designed the maze such as Fig. 7 (a). The UAV has the Visual-Inertial (VI) sensor that needs to generate a map by FIESTA [14]. The generated map is shown in Fig. 7 (d). For path planning, this map is converted to a grid map. We set resolution of the grid map as 0.1 m per pixel. The map size is 18.1 m × 21.3 m maze.
The modified map is used for the proposed path planners. When using a drone as illustrated in Fig. 3 (a), the generated trajectory is shown in Fig. 7 (e). The drone with a bulky payload can pass the narrow passage satisfactorily. For two drones with a bulky payload as shown in Fig. 7 (f), each trajectory can be easily obtained from the results as shown in Fig. 7 (e). To do so, the mean value of each trajectory is set to be the same as a single UAV trajectory. This result shows that the proposed method is useful to not only for a single drone but also for multiple drones.

B. COMPARISON WITH OTHER METHODS
To compare our algorithm with the state-of-art planning algorithm for a rigid body payload such as RRT-Connect [22], RRT* [23], Informed-RRT* [24] and Fast Marching Tree star (FMT*) [25], we ran simulations using the Open Motion Planning Library (OMPL) [26]. This library can consider SE2 state-space condition that has the position and rotation information. To consider the robot shape, Flexiblecollision Library (FCL) [27] is exploited. FCL is helpful to decide collision-free rotation at a position. We mixed both the libraries for collision-free path planning. Table 1 shows the calculation time and total path length. In Table 1, RRTC means RRT-Connect algorithm and I-RRT* means Informed-RRT* algorithm. The total heading angle variations(Total yaw in Table 1) are measured based on the following equation Our method shows satisfactory performance at time consumption and rotation. Although our algorithm is a little bit longer than RRT*, I-RRT* and FMT* in point of total length, our algorithm is safer because the center of the payload and the wall are further apart in a narrow path.

C. ANALYSIS
We also compared the stability of a drone while transporting a bulky and heavy payload with or without an the proposed optimization approach. Fig. 9 shows the comparison result. The red line means pitch angle and the blue line means roll angle. If our method is not used when rotating in the corner, the heading-angle angular velocity fluctuates, as shown in Fig. 9 (a). It affects the pitch and roll of the UAV and become unstable (See Fig. 9 (a)). Our proposed method is described in the Fig. 9 (b). Because our method optimized the whole path and minimized the heading angle rotation, the heading angle was changed smoother than the conventional method. It makes the pitch and roll of the UAV more stable than the Fig.  9 (a). This result shows that optimizing the heading-angle in the whole path provides a stable trajectory.

A. UAV STRUCTURE
We used the IFO-S drone as a UAV and installed T265 and global positioning system (GPS) module for odometry. For generating the map by FIESTA [14], Intel Real-sense D455 was used. The altitude sensor was TF-mini LiDAR manufactured by Benewake. We used NVIDIA Jetson Xavier NX as an onboard computer and pixhawk-mini as a flight controller as shown in Fig. 10. The carrying object was a 1 m carbon pipe. An RGB-D sensor generated the map at the onboard computer. The onboard computer sent the map to the laptop with Intel Core TM i7-10750H 2.6 GHz and 16 GB RAM. The laptop modified the map to exploit the A* pathplanner and optimized the path. We used the robust controller to handle an unknown payload developed by [28].

B. OUTDOOR EXPERIMENT
To validate our methods, we designed an obstacle that had a narrow passage (See Fig. 1). The passage width of the obstacle was 0.85 m. The UAV needed to rotate the headingangle to pass the passage. We set the velocity constraint to 0.1 m/s and altitude to 0.6 m. Fig. 11 shows the trajectory and 3D map, that the UAV passes the obstacle. Fig. 12 shows the attitude and position of the UAV with the bulky  object following trajectory. In the 28-30s, the UAV fluctuated because of the obstacle structure. At other times, the UAV followed the trajectory well.

VI. CONCLUSION
In this paper, we proposed the method for trajectory generation for the drone transporting a bulky payload through the narrow corridor. The map was modified to account for the lengths of ellipse's major and minor radii by using the size of the Voronoi circle. In the modified map, our method found a safe free space, generated the desired trajectory, and selected suitable rotation for narrow space. An optimization approach minimized unstable movement at cornering. Through simulations and experiments, we verified that our method guaranteed safety and computational efficiency for path planning.