A low-cost autonomous navigation system for a quadrotor in complex outdoor environments

In this article, we design a low-cost navigation system for a quadrotor working in unknown outdoor environments. To reduce the computing burden of the quadrotor, we build a separated system and transfer the computing resources from onboard side to ground station side. Both sides’ communication is guaranteed by 5G wireless networks. We utilize a stereo camera to acquire point clouds and build Octomap for the quadrotor’s navigation. Then, the trajectory is generated in two stages. In the first stage, a modified RRT*-CONNECT algorithm is adopted to generate a set of collision-free waypoints. In the second stage, a curve fitting algorithm is utilized to get a smooth piecewise Bezier trajectory. The advantage of the proposed method is to optimize the path into a safe, smooth, and dynamically feasible trajectory in real time. The modules of the state estimation, dense mapping, and motion planning are integrated into a DJI Matrice 100 quadrotor. Finally, simulation and experiments are both conducted to show the validity and practicality of our method.


Introduction
Micro aerial vehicles (MAVs) are becoming more and more popular in many fields. It is convenient to utilize such platforms to execute flight missions in complex scenes for their wide coverage, high mobility, and agility. As the applications in the fields of bridge inspection, power line inspection, forest fire prevention, search, and rescue, the demand for MAVs' autonomous flight in unknown scenes has become extremely larger than before.
The purpose of this article is to build an inexpensive platform for a quadrotor to navigate autonomously in complex environments, such as woods and ruins. As shown in Figure 1, our plan is to make a quadrotor able to fly in unknown woods autonomously. Nowadays, the huge breakthroughs in localization, mapping, navigation, and planning have made autonomous onboard flight possible in complex places. [1][2][3] These works need the quadrotors to carry on an advanced onboard computer to process localization, mapping, planning, and flight control simultaneously. However, in cluttered outdoor scenes such as forests and ruined places, these application scenarios are unfriendly and accidents may happen sometimes, which is a deadly threat for the quadrotors to carry out missions in these environments. Once the quadrotors are missing or broken badly, it will cause significant economic losses to users. So we will design a low-cost and separated navigation system to solve this problem. The system is combined with a lightweight quadrotor and a ground station like other state-of-the-art ones. 4,5 The quadrotor carries a low-cost Intel NUC (Intel I5 5300U, DDR3L), by which it is easy to: (1) control the autopilot by serial bus; (2) get the video stream from onboard camera; and (3) transmit the video stream data to ground station.
We use 5G wireless communication to build a high bandwidth data transmission between the quadrotor and the ground station. The quadrotor's autonomous flight in unknown environments needs sufficient perception of surroundings to localize itself and create a dense globally consistent map of the environment. We transfer this work from the onboard computer to the ground station since the low-cost computer has weaker computing performance than the one in our previous work. 6 In mobile robotics, various vision-based 7-9 and laser-based 10 algorithms have been proposed and used for simultaneous localization and mapping (SLAM). In our previous work, a 3D-LiDAR is used to locate itself and plan trajectories on Octomap, 11 which is generated from raw point clouds. But we use an inexpensive and lightweight stereo camera (Intel Realsense D435) for perception in this article. We deploy the VINS-MONO 12 for localization and Dense Surfel Mapping 13 to get local dense point cloud map. We also use Octomap to perform three-dimensional (3-D) map building for its efficient memory usage. The Octomap is transformed from raw point clouds obtained by stereo camera in real-time navigation. We use the flexible collision library 14 to detect whether the quadrotor and the obstacles are overlapping. A set of collision-free waypoints are generated by a modified RRT*-CONNECTED algorithm on Octomap. Then, a smooth trajectory consisting of multiple Bezier curves is generated, which is proposed by Gao et al. 15 The safety and dynamic feasibility of the trajectory are guaranteed by introducing a set of constraints. All operations can be demonstrated on the Rviz, which is a 3-D visualization tool of the robot operating system (ROS). 16 We sum up the main contributions of this article as follows. Firstly, we design a low-cost separated navigation system, which is combined with a lightweight quadrotor and a ground station based on 5G communication. Then, a modified RRT*-CONNECT algorithm is proposed for the quadrotor to generate a set of optimized collision-free waypoints on Octomap. Moreover, our system integrates the functions of real-time state estimation, mapping, planning, and flight control for the quadrotor working in unknown outdoor complex environment.
The outline of this article is as follows. We present the related literature in the "Related work" section and introduce the software and hardware architecture of the system in the "System overview" section. A feasible path planning method is detailed in the "Path planning" section . The results of both the simulation and outdoor experiments are presented in the "Experimental results" section. Finally, the article is concluded in the "Conclusions" section.

Related work
Basically, recent works 1,3,15 on practical quadrotor path planning have separated the trajectory generation problem in two steps. The first step is path finding. There are a lot of works of MAV's path finding in recent years, and the algorithms are diverse, ranging from the searching-based A* 17 to sampling-based RRT. 18 They all have been applied effectively in different scenes. The searching-based algorithms can be used in a dispersed space which is modeled from a real-world scene. The sampling-based motion planning is an algorithm proposed in last decade and has attracted great attention. In 2000, Kuffner et al. proposed RRT-CONNECT, 19 which is a bidirectional search algorithm. The search speed and efficiency have been significantly improved over the original RRT algorithm. MIT's Karaman et al. proposed RRT*. 20 Compared with RRT, this algorithm can not only find a fast and feasible solution but also ensure asymptotic optimality. CMU's Gammell et al. proposed a modified informed RRT*, 21 which can reduce the search range by constantly finding a better path by minimizing the sampling space. Based on these methods, we will combine the RRT-CONNECT with RRT* and add some evaluation conditions in the generation of random points.
The second step is path optimizing. After finding a set of collision-free waypoints, we cannot execute flight mission on them directly. We have to generate another smooth, safe, and dynamically feasible trajectory based on these waypoints we have found. Many methods 22,23 have been adopted to optimize the trajectory. Mellinger et al. 24 were the first people to parameterize the path into a minimum snap trajectory. The trajectory is represented by piecewise polynomial functions, and the trajectory optimization process is modeled as a quadratic programming problem. Richter et al. proposed an analytic solution for minimum snap trajectory generation. 25 Gao et al. come up with a way to create a corridor which is composed of spheres on each control point and formulate the problem as quadratically constrained quadratic programming. 26 Gao et al. proposed a method to model the trajectory with multiple Bezier segments by deploying Bernstein polynomial basis. 15 Usually, MAV's state estimation relies on GPS in outdoor environments. When the MAV flies in cluttered outdoor or indoor scenes, it relies on ultrasonic or optic flow algorithm to localize itself. In the study by Mellinger et al., 24 the position and orientation of a quadrotor were captured by a Vicon system. But as the application of the MAV become more and more wide, we need the quadrotor to execute missions in some complex outdoor environments, such as woods and mountains, where the GPS signal is poor and lots of obstacles exist. The quadrotor must equip with sufficient perceptual ability to estimate its state and map its surroundings. Various laser-based and vision-based algorithms have been proposed with the rapid development in SLAM. A laser scanner 25 was used to re-localize against a previously recorded map and a global plan was calculated before flying. The laser-based strategy was also used in our previous work 6 for its long-range perceptual ability and immunity to illuminated conditions. But the heavyweight and the high cost of Velodyne VLP-16 are obvious in the practical applications of the quadrotor San Jose, California, United States. In these years, the vision-based strategy is very popular for its lightweight and low cost. These strategies use monocular camera, 27 stereo camera, 28 monocular camera with IMU, 12 or stereo camera with IMU. 13 In this article, we will deploy the VINS-MONO for localization and Dense Surfel Mapping to get local dense point cloud map.
There are three forms to represent the map. The first form is to utilize the raw point cloud as the map and the trajectory is generated on it directly. The distance between one waypoint of the trajectory and its nearest point cloud is calculated by utilizing KD-tree 26 to store the information of all point clouds. But this way needs to store all point cloud information, and it will become slow when we send the target position directly on Rviz. The second form is voxel grid, which reduces the number of point clouds using the center point to represent neighboring point clouds in one cube unit. It is efficient for representing small scenes, but it cannot store historical point cloud information. Another way is Octomap, which is memory efficient. It just needs the sensor to publish local point clouds and can record all history points. In this research, we will deploy Octomap to represent the global map.

Hardware architecture
The hardware equipment is shown in Figure 2, and the overall hardware architecture pipeline is shown in Figure 3.
Flight platform. We use a commercial DJI M100 as the flight platform. It is controlled by an onboard autopilot and can be accessed through DJI's Onboard SDK, which contains a set of functions that allow the user to send control commands and receive telemetry data from the drone. The quadrotor carries an inexpensive Intel NUC (Intel i5-5300U) and an Intel Realsense D435 as perception sensor. A Netcore NW392 gigabit wireless network card (802.11ac) is used for the quadrotor to communicate with the ground station. The quadrotor does not rely on the GPS in autonomous mode. But when there is emergency happening, we have to switch autonomous state to remote control state. The DJI M100 has to rely on the GPS to localize itself in remote control state, so we do not remove the GPS. Ground station. We use a laptop (MSI GP62M with Intel i7 7700HQ quad-core processor) as a ground station, which communicates with flight platform by HUAWEI Honor X2 router. The laptop and the router are connected by a Gigabit cable.
The maximum speed of the onboard computer side is up to 863 m/s, and the ground station side's maximum speed is up to 1000 m/s. According to our initial thoughts, we treat the flight platform as an entire low-cost sensor to explore the unknown scene and transfer the computing resource from the quadrotor to the ground station. The huge amount of data transmission between two sides is guaranteed by the high bandwidth of 5G communication.

Software architecture
The entire interactive software architecture is shown in Figure 4. We use ROS as our development environment. Each dashed box represents a ROS node running at different rate. The visualized platform is based on Rviz, which is a 3-D visualization interface of ROS. Firstly, the monocular color stream, dual IR stream, and depth stream are obtained from stereo camera D435 by running Intel Realsense SDK. The inertial measurement unit (IMU) data are obtained by running Onboard SDK. The video stream and the IMU data are transmitted to perception module by building SSH connection between both sides. The VINS-MONO is used for localization by merging the monocular color stream and IMU data. The Surfel Mapping module is used to build a local dense point cloud map by fusing the pose data from VINS-MONO and the depth data from onboard side. Then, these point clouds are transformed to Octomap for getting a globally consistent map. A collision-free and dynamically feasible Bezier trajectory is generated by running path planning module. A set of kinodynamic commands are generated by calculating the desired states of the quadrotor and transmitted to Onboard SDK online by wireless communication.

Path planning
Generating a set of collision-free waypoints RRT* is a fast random sampling algorithm which guarantees asymptotic optimality by selecting the local nearest node. However, RRT* does not consider the target's position as guidance. Therefore, we proposed a Heuristic RRT* algorithm in our previous work and employed this method in the flight mission. To improve the efficiency to find a set of optimal collision-free waypoints, we combine the RRT-CONNECT with RRT* and add some evaluation conditions in the generation of random points. In Figure 5, the red point is the start position and the green point is the goal position. The start point generates a red tree toward the goal point and the goal point generates a blue tree toward the start point. A greedy heuristic strategy is used in trees' generation. When the closest distance between two trees is less than a threshold, we believe two trees are connected and connect the closest points of both trees with a green line in the yellow circle.
The expansion from both sides and the greedy choice are the core of our improved algorithm, which are demonstrated in Algorithm 1. Let G be the map in the configuration space which consisted of vertices V and edges E. Denote the straight line connected by point q1 and point q2 as LINE (q1, q2). The random exploring map G 1 is expanded from the initial node qinit, which is also the start position of the quadrotor. Another random exploring map G 2 is also initialized with the goal node qgoal, which is located at the goal position. The generation of the qrand is chosen by Sample(qinit, qgoal, p, G 1 ). Firstly, heuristic information is given to generate the random tree in the direction of qgoal point. Then, we check whether q n which consisted of LINE(qparent, qnearst) and LINE(qnearst, qrand) is suitable for flight. From Figure 6, we can get that if q n is small, the quadrotor needs to fly a sharp turn to follow the path, and this kind of small angle is not good for generating Bezier curve in the next section. So we only take the qrand with qn > 90 to make sure the quadrotor avoids emergent brake. Since we only concern the translational movement of the quadrotor, the yaw angle's planning is left as our future work. The map G 1 is locally trimmed by Reconnect(qnew, qnearest, G 1 ) in Algorithm 2 which uses the asymptotic optimality strategy of RRT* algorithm. Then, we get the qnew point and the trimmed map G 1 . The following steps are the same as RRT-CONNECT algorithm basically, which is processed by lines 5-30 in Algorithm 1.

A feasible path optimization method
Mellinger et al. 24 have suggested a way to represent the MAVs' full-state space by 4-D variables consisting of 3-D coordinates fx, y, zg and yaw angle . A piecewise parameterized trajectory can be used for MAVs' path following on the basis of differential flatness. Since the trajectory optimization process can be formulated as a Quadratic Programming, we utilize the minimum jerk of the trajectory as objective function to generate smooth trajectory. The trajectory is represented by Bernstein polynomial basis which is written as where ½c 0 j ; c 1 j ; . . . ; c n j is a set of control points of the jth piece of Bezier curve and n is the degree of Bernstein basis. The trajectory is divided into multiple segments by time, each segment is represented by a piece of Bezier curve as f ðtÞ ¼ where m is the number of segments of the trajectory and p 1 ; p 2 ; . . . ; p m are the parameter vectors on every segment of the Bezier curve to map the time period from ½0; 1 to ½T iÀ1 ; T i in each curve. i represents the number of the control point. j represents the number of the segments. m is any of x, y, and z dimensions. T 1 ; T 2 ; . . . ; T m are end times of each segment. The cost function is written as follows where k represents the kth-order derivative of the objective function. We set k ¼ 3 as the original article. 15 It is in a quadratic form p T Q 0 p. p represents the vector consisting of all control points ½c 0 j ; c 1 j ; . . . ; c n j . Q 0 is the Hessian matrix of the objective function and can be converted into minimum snap trajectory form. Then, a set of constrains are enforced during the path generation to ensure the dynamical feasibility of the trajectory.
Waypoints constraints. Bezier curve has the property that the head point and tail point are connected by the curve, and other points are not in the curve. We add position, velocity, acceleration, and their derivatives as equality constraints on both head point and tail point. A hth (h < n)-order derivative of head point of the m dimension can be represented as Continuity constrains. For each two adjacent curve segments of the trajectory, they must be continuous. So the continuous equality constrains are added to any two adjacent segments. For the jth and ( j þ1)th piece curves, we have Safety constrains. In the study by Gao et al., 15 a collisionfree sphere area is generated by considering the nearest obstacles. And the area is enlarged to a cube by gauging near grids along the direction of the 3-D axis. This method can ensure the trajectory constrained in the middle of the corridors, but it takes much time in finding the corridor, so we just follow the same way in our previous work, 6 where v m is the maximum velocity and a m is the maximum acceleration of the MAV. The trajectory optimization process is modeled as a convex quadratic programming with constraints, and convex solvers are used to find the solution within polynomial time.
min c T Qc; s:t:

Experimental results
In this section, we present the time and spatial cost comparison result between our proposed algorithm and Heuristic RRT*. We also demonstrate the simulation and the real experimental results of autonomous flight in woods. From Figure 8, it can be seen that the simulation of the path finding process of RRT*-CONNECT and Heuristic RRT* are both executed on a known map. Figure 9 shows a simulation test on a known map. Figure 10 shows the real trajectory of autonomous flight in woods. The flight is fully autonomous in the unknown woods. Each path planning of the quadrotor is global. When the collision occurs, a new trajectory would be generated from its current position to the goal position. Figure 8 shows one of our comparative tests. We test our proposed algorithm and Heuristic RRT* in Figure 8(a) and (b). Firstly, we generate five different maps and each of them has six random cubic obstacles in a 30 Â 30 Â 30 m 3 cubic space. Then, we performed 100 comparative tests on each map. Table 1 shows five sets of experimental data. Each set represents the mean value of 100 comparative experiments on time and space. The result shows RRT*-CONNECT is more efficient in time cost. We test the path finding algorithms on our ground station (Intel i7 7700HQ).  Simulation results Figure 9 shows one of autonomous flight tests in simulation. An Octomap is transformed from a dense point cloud data file, which is sampled by Velodyne VLP-16. A white smooth Bezier trajectory is generated using Bernstein polynomial basis. Then, a set of high-frequent kinodynamic commands are calculated and transmitted to the quadrotor for executing the flight.

Outdoor experimental results
Our real flight experiment is operated in woods besides our laboratory building. This environment contains many woods with cluttered branches, which is shown in Figure 1. The whole process of path planning is demonstrated in Figure 10. In Figure 10(a), the ground station generates a trajectory which is collision free with current obstacles and the quadrotor follows the path. When a new perceived obstacle collides with the original path, the ground station will regenerate a new trajectory. This flight process replans twice, which is shown in Figure 10(b) and (c

Conclusion
This article is focused on how to design and implement a low-cost separated system for quadrotors to carry out   autonomous path planning in cluttered outdoor scenes. To accomplish online local map building, 3-D point cloud acquired by the onboard stereo camera is transformed to Octomap, which is a memory efficient way to represent the unstructured outdoor scenes precisely. Furthermore, an improved 3-D path finding algorithm named as RRT*-CONNECT is proposed to generate a set of collision-free waypoints and then a smooth and dynamically feasible trajectory with a set of constrains is generated using curve fitting algorithm. Experimental results show that our proposed low-cost autonomous system is a feasible solution for a quadrotor to conduct path planning in unknown cluttered woods environment and can reduce the economic losses to users when the quadrotors are missing or broken.

Declaration of conflicting interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work was supported in part by the National Key Research and Development Project under grant 2018YFB1307503 and in part by the National Natural Science Foundation of China under grant 61973049.