Plan 3D Paths for Drones | Motion Planning Hands-on Using RRT Algorithm, Part 4
From the series: Motion Planning Hands-on Using RRT Algorithm
Are you working with autonomous drone applications such as package delivery or advanced air mobility? Learn how to plan and execute unmanned aerial vehicle (UAV) flights using a guidance model for a fixed-wing aircraft. A fixed-wing UAV is nonholonomic in nature and must obey aerodynamic constraints like maximum roll angle, flight path angle, and airspeed when moving between waypoints.
Watch a demonstration of motion planning of a fixed-wing UAV using the rapidly exploring random tree (RRT) algorithm that is given a start and goal pose on a 3D map. You will learn how to use UAV Toolbox with MATLAB® to generate 3D Dubins motion primitives. You will also learn how to use a customizable path-planning template with Navigation Toolbox™ to define a custom state space and state validator for sampling-based path planning.
Steps include:
- Setting up a 3D map
- Providing the start pose and goal pose
- Planning a path with RRT using 3D Dubins motion primitives
- Smoothing the planned path
- Simulating the UAV flight following the planned path
Published: 12 Jul 2022
Hey, everyone. Welcome back to another Motion Planning Video. In this one, you will learn about planning 3D pads for unmanned aerial vehicles or UAVs using the RRT Algorithm with 3D motion models. You will also learn about the customizable Path Planning Template that you can use to define a sampling-based planning interface. If you haven't watched our previous videos on this topic, please check out the video series on Motion Planning with the RRT Algorithm.
The first video in this series is a prerequisite for this video, and you'll find the link for this in the resources section. In this video, I will walk you through the Motion Planning Workflow using a code example that we provide with the UAV Toolbox in MATLAB. Now, let's look at the workflow. We start with loading the map of a 3D environment. Then we select the desired start and goal locations on this map. Then the Path Planning algorithm works to find an obstacle-free path between the start and the goal location.
And while doing that, visualizing the path helps us to see what optimizations may be required, which brings us to the last step, that is optimizing or making the path smoother. Before jumping to Motion Planning, you may have questions, such as where do the map comes from? Or how will the UAV follow the planned path and avoid unknown obstacles? UAV Toolbox includes ready-to-use examples for you to generate 3D occupancy maps, where you can test the planning algorithms.
You can also create a 3D map by scanning a simulated world using a LiDAR Sensor Model. Once you have the waypoints from the Motion Planning algorithms, you can model the aerodynamics constraints of a UAV and tune waypoint following algorithms to make the UAV follow the planned path. While the Motion Planning algorithms consider the obstacles that were known in the map, you can use obstacle avoidance algorithms to take over and send commands to the UAV when there is an unknown obstacle on the way.
Now, let's get back to Motion Planning, and let's look at the example in MATLAB. Let's start with going into the documentation for UAVs Toolbox. Here we find the planning and control examples, and this is the example, we are going to look at in this video. You can use the RRT Algorithm to plan paths for a quadcopter or a fixed-wing UAV. This example in MATLAB is focused on path planning for fixed wing UAVs.
A fixed-wing UAV is nonholonomic in nature and must obey aerodynamic constraints, like maximum roll angle, flight path angle, airspeed when moving between waypoints. Now let's start implementing the Motion Planning workflow that we saw earlier. The first step is to bring in a 3D occupancy map. We use the load function to bring in the map data stored in the UAV city block mat file.
There is a separate example that you can find a link for in the resources section that tells how to generate these types of mat files. Let's run the section and load the occupancy map. We can see that it gives us a map with some pre-generated obstacles. We see it created an object omap for occupancy map 3D class in the workspace. Let's check what's there inside this object.
The grid resolution is said to be one cell per meter by default for the map. We have set the threshold for free and occupied cells to be the same. So any probability values between 0.65 will be considered as an obstacle free, and above that will be considered as an occupied cell. Now looking at this map, we can select a start and goal location in the free space. Since it's a fixed-wing UAV, we need to provide X, Y, Z, and a heading angle for both the start, as well as the goal State.
Now we plot this and visualize the start and goal on the occupancy map. The next step is to plan a path between the start and the goal location. Now, before going into the implementation of the RRT algorithm with MATLAB, let's take a quick refresher on how it works in theory. So RRT is a tree-based motion planner that builds a search tree incrementally from random samples of a given state space.
So the State-space is a representation of the map in a way that the UAV understands. That means it needs to account for the aerodynamics constraints of the UAV. The tree eventually spans the search in the State-space and connects the start state and the gold states. Now, what's new in this example is that here we use RRT planning for 3D State-space. That means the motion segment or motion primitive connecting the two consecutive states should be defined in a 3D space while satisfying motion constraints of a UAV.
UAV Toolbox provides UAV Dubins connection that computes Dubins path segments between two UAV states. A Dubin path segment connects two poses as a sequence of motions, where these are the included motion options in our documentation. You can define parameters for UAV motion models, such as airspeed, maximum roll angle, and minimum turning radius. Now the UAV Dubins connection object needs to be defined within a State-space.
Before we dive further into the implementation, let's quickly refresh on how we define the State-space and the planner. Here I am referring to the e-book on motion planning with MATLAB, and you can find a link for it in the Resources section. And it shows the four-step motion planning workflow that we use in this UAV example. So I highly recommend you check it out. Now let's get back to MATLAB.
Here we use the helper function, example, helper UAV State-space, that you can find in this folder for this example, which helps to define the State space and provide the UAV constraints. The stage space is defined with the position and the heading angle for the UAV. We now know that the maximum roll angle, airspeed, and flight path angle limit will be used to define the UAV Dubins path segments. We will also set the bounds to specify the position and orientation boundaries of the UAV.
Let's quickly look at how we create the stage space using this helper function. We use nav.StateSpace class from Navigation Toolbox that lets you define a State-space, for some applications, you can use predefined State-space, such as the State-space SE2 and SE3. However, for this example, we define a custom State-space to incorporate the UAV aerodynamic constraints. Now, you don't need to write this from scratch.
You can use an out-of-the-box planning Interface Template to do so. This template provides us with all the required class properties defined in it, where this example helper uses the template to define the custom properties for the UAV. Now, let's scroll down to the distance object. Here we compute the path distance between two states and interpolate to find the intermediate state for a continuous motion.
Let's run this section. And now, we see the State-space object in the workspace. We have all the properties that we can look at here, as well as the properties for Dubins connections is also shown. Now, we use the set workspace gold region property from the State-space class to specify the goalposts and the target goal region around it. This helps to define how large the target workspace goal region around the goalpost is, which is used to bias the sampling in the gold region.
Now, we have our State-space defined. Next we define the State Validator. The purpose of State Validator is to validate the connectivity between the sample states. It determines that a state is valid or invalid based on the information if the state is occupied or free on the map. A motion between two states is valid only if all the intermediate states are valid, which means the UAV does not pass through any occupied cell on the map.
Here we use out-of-the-box State Validator for 3D State-spaces from Navigation Toolbox. So we create a Validator occupancy map 3D object by specifying the State-space object and the inflated map. Then set the validation distance in meters for interpolating between the states. Now, we have the required inputs ready for the path planner to work.
We will now use the planner RRT in the same way we have done in the previous videos of the series. Here we create the planner or the object and specify the State-space and State Validator as inputs. Now, we set the properties for the planners, and the properties we specify here are maximum connection distance, which is the maximum length of a motion allowed in the tree. Another property is the goal bias, which is the probability of choosing the goal state during state sampling.
And the other one is Max iterations, which is maximum number of iterations. We also specify a custom goal function, which determines that a path has reached the goal if the Euclidean distance to the target is below a threshold of 5 meters. Now, based on all the specified properties for the planner, the plan function executes the path planning to perform the RRT-based path planning in the 3D space.
It outputs that object where we can see all the states that the path includes. We can add the Tick Tock functionality here to note the execution time for the planned function. We can further tune the planner output by changing the property values. Now, to visualize the plan path, we interpolate it based on the UAV Dubins connections and plot these interpolated states as a green line, and the simulated states is plotted as a red line.
So this example provides a helper function to simulate the UAV, and you can visualize the UAV following this planned path. It requires these inputs, such as the set of waypoints, airspeed, and time to reach the goal. This helper function uses fixed-wing guidance model to simulate the UAV behavior based on control inputs generated from the waypoints. And it also uses the waypoint follower to simulate the UAV following these waypoints, and both of these are part of the UAV Toolbox.
Now we can see here that the simulated UAV flight deviates slightly from the planned path because of small control tracking errors. Also, the 3D Dubins path assumes instantaneous changes in the UAV roll angle, but the actual dynamics have a slower response to these roll commands. One way to compensate for this lag is to plan the path with more conservative aerodynamic constraints.
Now, we have our visualization and simulation working. Next, we would like to smoothen the path. The original planned path makes some unnecessary turns while navigating towards the goal. To make this path smoother, we simplified the 3D Dubins path by using the path smoothing algorithm provided with this example. There is another helper function that is provided to implement the path smoothing algorithm.
This function removes intermediate 3D Dubins poses based on an iterative strategy. To limit the scope of this video, you can refer to the example link and find more information on the smoothing strategy. The smooth path generated by this process is much shorter and improves tracking characteristics for the fixed wing simulation model. So this brings us to the end of this example, as well as this video.
I highly recommend you to check out all the resources available around path planning for different types of applications that we have shown in previous videos. And I would really recommend you to try running this example yourself. And if you're working with specific 3D Motion Planning or Motion Planning for UAVs, please feel free to reach out to us for any questions on your specific use cases. And thanks a lot for watching this video.