The navigation subsystem is responsible for maintaining reliable state estimation of the drone at all times. Also, it is responsible for the following actions of the drone. We have two different version of this subsystem. The first figure down below is the diagram for our Gazebo simulation, and the next figure shows the diagram for entire physical system.
Figure 1. Navigation subsystem diagram – simulation
Figure 2. Navigation subsystem diagram – physical system
State Estimation
For simulation we get localization information(i.e. drone odometry), directly through Gazebo interface, and use that for our control. For the Fall semester, due to the coronavirus, we have to rescope our project and we decided to use only GPS as our position sensor, instead of Ultrasonic beacons as we planned. Therefor, we directly use the odometry data provided by DJI flight controller, and use that as our source of localization information. And since the accuracy of DJI odometry data is limited, we have to use Visual Servo algorithm to fly close and above the package to pick it up.
Trajectory Tracking
Based on the waypoints generated by the path planners, the navigation subsystem is responsible for traversing the waypoints as accurately as possible. First, we will talk about our implementation in spring, and then briefly discuss the current implementation in fall semester.
As shown in first figure above, we use a pose controller to track the position setpoint given by the trajectory controller. The pose controller is implemented in PID control and will output velocity command (X, Y, Z velocity and yaw rate) to the Gazebo interface. The interface will directly assign the velocity to our drone model in Gazebo. Therefore, this gives us an assumption we made in the real world system that the velocity command sent to the DJI controller will be tracked perfectly.
For Fall semester, we implemented and tested our trajectory controllers on the physical drone. Algorithm-wise ,the controllers are similar to what we build in simulation. With the benefits of modularity in our software stack, we just need change the parameters for physical drone, and then use the DJI interface instead of Gazebo interface, to switch from navigation in simulation to real world. For the navigation subsystem performance, please visit our media page for our flight test videos.