Planning Approach – Combining Vector Fields
The vector field planning approach computes a desired vehicle velocity based on a global plan, detected objects, and other sensor data. As such, it can function as a reactive planner when avoiding obstacles. The current implementation has two vector states: go-to-goal and avoid. If the vehicle is not near an obstacle, the desired velocity will point directly to the current goal point. As the vehicle approaches an object, the planner switches to an avoidance mode. In this avoidance mode, the vehicle decides which direction best avoids the obstacle, then orbits the obstacle in that direction at a safe distance. The two images below show the orbit field (left) and the go-to-goal field (right).
Hardware in the Loop Testing
We conducted HITL simulation testing on the planning subsystem. This setup used the DJI low level controllers and Assistant software to simulate the physical state of the drone. Performing this simulation also validated the commands to the drone from the vector field planner. The results are shown below.
Future Path Visualization
One of the major requirements of the system is to report flight advisories to a remote operator. After considering several design choices, we implemented a future path visualization. This method enables to operator to view the vehicle, obstacles, and recommended path simultaneously. When approaching an obstacle, the operator can use their own judgement to follow the planned path or make modifications as desired. After creating the raw path, a sliding window filter smooths the path.
The planner computes the path by simulating the drone’s motion through the vector field. At each step in the simulation the planner updates the obstacles’ positions using the information from the trajectory predictor, and computes the drone’s desired velocity using the updated vector field. A PD controller simulates the drone executing these velocities for a planning horizon of 8 seconds.
Intelligent Gap Behavior
During testing, we noticed a problem when avoiding multiple closely located obstacles. The planner forces the drone to orbit around obstacles based on if the obstacle is to the right or the left of the drone. As a consequence, if obstacles are place on either side of the drone, it will attempt to fly through them regardless of their spacing. To fix this, the planner now takes into account a minimum gap distance between obstacles when setting the orbit. If the gap distance is above the threshold, the drone will fly through the gap. If the gap distance is too small, the drone will fly around both obstacles. After testing in real life, we tuned the minimum gap distance to 4 meters. The left image shows the failure case before these improvements. The middle image shows the same case with proper behavior. The right image shows the obstacles properly space with the planner passing through them.
Dynamic Obstacle Avoidance
The AACAS planner can handle both dynamic and static obstacles. To accomplish this, we take into account the velocity of nearby obstacles. Using this information, the planner can make informed decisions about object trajectory and compute potential collision points. For the given use case, the obstacle motion was assumed to be linear. Given the relative position and velocity of a moving object, the planner predicts the time until the drone path and the obstacle path cross. Then, the planner estimates the position of the obstacle at that time. The assigned orbit follows the convention described for static obstacles. Early results showed a safety buffer was needed to encourage the drone to fly behind moving obstacles.
Simulation Development
The simulation work for the AACAS project uses two different environments: MATLAB and RVIZ. The MATLAB environment is used to prototype planning approaches and roughly tune parameters. After successful implementation in MATLAB, the design is implemented in ROS and tested in RVIZ The basic RVIZ simulator uses simple PD kinematic control to simulate motion, but we also implement a Hardware in the Loop (HITL) simulator for higher fidelity testing. Successful testing in all simulation environments is required before flying any code on the drone.
MATLAB
The MATLAB simulator is a kinematic simulator that controls the vehicle motion using state space control. The framework for the simulator was inspired by Dr. Greg Droge at Utah State University but has been heavily modified for the AACAS project. Currently the simulator only visualizes and plans in a 2-Dimensional plane, but holds the capability to expand into 3-Dimensional simulation. This image shows a screen shot of the MATLAB simulator executing the vector field planning approach developed for PR4, click for a video of the simulation. The blue hexagon shows the vehicle. The red ball shows the obstacle. The black line shows a simulated sensor detection of the obstacle. The purple arrows show the vector field computation throughout the simulation space.
RVIZ and ROS
Significant progress was made to enhance the ROS simulation. There are three ways we can simulate the AACAS system in ROS. Hardware in the Loop (HITL), Kinematic Software in the Loop (KSITL), and Dynamic Software in the Loop (DSITL). The simulator in PR 4 was a DSITL which relied heavily on third party libraries and did not interface directly with the DJI OSDK. As a result, we implemented our own KSITL simulation environment in ROS that also interfaces with the DJI HITL simulation environment. The following images show the software architecture, and ROS node graph for the custom AACAS simulator.