This guidance system for drones encompasses a simulator (APM SITL) or real drone support, path planning, mission creation, drone control, and a web-based ground control app. I started the project for Team Guardian to meet the requirements of the AUVSI competition, where a drone must complete objectives while avoiding stationary and moving objects.
The guidance system is executed on a laptop computer in the field, and continuously sends velocity vectors and other commands over the telemetry radio connection. The drone itself is a multicopter, running stock APM firmware and sending back telemetry at roughly 10 Hz. Running the guidance system on a laptop provides flexibility of developing in a high-level language (Python) and facilitates much easier testing when the simulator is involved. For the task at hand (avoiding large obstacles), the communication latency and potential for dropped packets is not a significant concern.
The DroneKit library provides an application layer that allows high-level commands to be sent to the drone via MAVProxy, and hides the details about the connection. During development, the APM SITL simulator is hooked up identically to a real drone by connecting to its provided telemetry stream. When we tune the real copter PID paramaters, we set them in the SITL drone as well, allowing the simulated drone to have similar flight characteristics to the real drone. This makes it easier to tune paramaters of the guidance system.
Control of drone movement is provided by the velocity vector commands exposed by DroneKit. You need to continuously feed velocity vectors to a drone running APM firmware otherwise it will quickly stop and hover. This turns out to be a really nice safety feature, and apparently wasn’t present in the earliest versions of APM. Yikes!
The guidance system calculates a velocity vector at a user-settable frequency, usually 5-10 Hz. Velocities are specified in NED coordinates - North, East, Down, which makes navigation quite easy using standard equations for navigation on the Earth. To generate a velocity vector, the following things are considered:
- maximum velocity setting
- current and desired location, driven by the path planning algorithm and mission itself
- PID values from the navigation PID controller (internal to the guidance system, separate from the APM PID controllers)
- a few safety checks for altitude limits and overshoot detection
Once the velocity vector is calculated, it is sent up to the drone. It turns out that packet loss is not a significant concern - most of the velocity commands make it up to the copter. The velocity vector needs to be recalculated continuously to account for dynamic effects such as wind, and so the drone won’t stop as mentioned before.
Similar to how missions are planned on standard ground control apps like APM Planner, I introduced the concept of a mission module to the guidance system. Each mission module is a representation of a few things:
- a set of coordinates (multiple coordinates are used to define a search pattern, for example)
- a particular action to perform when the final coordinte is reached
- an altitude
The simplest mission module is a waypoint - it contains a single set of coordinates, an altitude, and tells the drone to hover at the location for a few seconds. An example of an extension to the waypoint module is the drop location module, which sends a few servo commands to release something from the drone as it hovers over a particular location. There are also search pattern modules, where the corners of an area are defined by the user, as well as a swath width, allowing a set of coordinates to be generated in a lawnmower-style zigzag search.
Initially, missions (a collection of mission modules) were defined in a .CSV file. In the future, users will be able to graphically create them using the GCS GUI.
To execute a mission, the guidance system essentially loops through all of the locations in a mission module, one mission module at a time.
The path planning system uses the A* algorithm, which allows a route to be calculated between a particular location and the target location from the current mission module. Along the way, any “obstacles” will be avoided. A* operates on a grid representation of the flight area, and obstacles are simply nodes on the grid that the path is not allowed to use. In practice, I use a circle generation algorithm from computer graphics to find grid nodes around a central coordinate to mark off as the edges of an obstacle.
The real-world resolution of the grid is customizable, and I usually use 1 metre. So, the flight area is broken up into a set of geographical coordinates along a 1m grid. A* operates on the indices of the geographical coordinates to calculate the path. In various places, a KD tree is used to quickly determine the closest grid coordinate to another coordinate (such as a mission module target location). Since the grid resolution is fine compared to the accuracy of the drone’s GPS navigation (3m roughly), this approximation doesn’t result in significant navigation issues in practice.
Upon reaching a target location, A* is executed to find the path to the next location.
Navigation is fairly straightforward, with a bearing to the next location along the A* planned path being repeatedly calculated, and the velocity components being derived from that. These components constitute the setpoint and are fed to a PID loop to determine the final velocity components to send to the drone. Some checking for velocity limits is also implemented, as are some special considerations for reaching targets accurately.
Tuning the PID constants is accomplished in the simulator once a parameter file from the drone is applied to the simulated drone. This provides a reasonably accurate representation of real flight characteristics, and every mission is simulated before execution to ensure safety.
We decided to use a web-based stack for the ground control UI. The main driver for this was wanting a fully featured mapping library that would allow us to place components (icons, paths, etc.) on the map and have them be georeferenced correctly. GUI solutions like QT didn’t have mapping features advanced enough for our use. We also needed to provide our own map tiles as opposed to using Google Maps or something similar, since there is no internet out at the flightline at compeition. OpenLayers was chosen as the mapping library after experimenting with some alternatives.
The ground control system communicates with the guidance layer (providing path planning and navigation). They are maintained as separate projects, and guidance can run standalone from the command line without having a GCS instance connected.