Drone Path Planning, Trajectory Generation, and Control
Abstract
This work investigates path planning, trajectory generation, and feedback control for autonomous quadrotor flight. The initial system was deployed on a Crazyflie 2.1 using Vicon motion capture for state feedback. A subsequent implementation replaced motion-capture measurements with visual-inertial state estimates, thereby introducing drift and sensor noise into the planning problem. The pipeline combined geometric position control, A* search on a three-dimensional occupancy grid, path simplification, and time-parameterized trajectory generation. In three physical maze experiments using Vicon feedback, position errors were generally below 0.1 m and remained below 0.2 m near sharp turns. The visual-inertial extension used a startup hover, Ramer–Douglas–Peucker simplification, waypoint interpolation, and fifth-order minimum-jerk segments to improve robustness to estimator error.
System Architecture
The autonomy stack transformed a known obstacle map into commands for the quadrotor. A planner first generated a collision-free discrete path through a three-dimensional occupancy grid. A trajectory module converted that path into a time-indexed reference, and a feedback controller produced the desired force and attitude. For the physical Crazyflie experiments, a host computer received the vehicle pose from Vicon, evaluated the reference trajectory, and transmitted commands over radio. The Crazyflie’s onboard attitude controller tracked the commanded orientation; the custom moment controller was not used directly on the vehicle.
Geometric Position Control
The position controller combined the desired trajectory acceleration with proportional feedback on position error and derivative feedback on velocity error. The resulting desired force accounted for both tracking correction and nominal trajectory motion. Its direction defined the target body orientation supplied to the onboard attitude controller.
Controller parameters obtained in simulation were too aggressive for the physical platform. The maximum speed was limited to 1 m/s, the simulated gains were initially reduced to approximately 70 percent of their original values, and subsequent tuning used measured step responses. The vertical response exhibited a rise time of approximately 2.4 s, 28 percent overshoot, a settling time of approximately 4 s, and a steady-state error near 0.1 m. These measurements suggested that additional derivative gain could improve damping and that integral feedback could reduce the residual offset. The evaluated controller, however, retained only proportional and derivative position feedback.
Path Planning and Trajectory Generation
Known environments were represented by three-dimensional occupancy grids with inflated obstacles. A* search operated on a 26-connected graph and used Euclidean distance as its heuristic. The resulting path contained closely spaced voxel centers, which increased the size of the trajectory problem and could introduce rapid changes in commanded direction.
For the Vicon-based maze experiments, consecutive path points were divided into groups of five and each group was replaced by its mean. Ramer–Douglas–Peucker simplification was also evaluated, but its tested configuration removed excessive path structure. The reduced waypoint sequence was therefore converted into a constant-speed trajectory at 1 m/s. Although minimum-jerk and minimum-snap alternatives were implemented, the constant-speed reference was selected because it could be validated and tuned during the available hardware sessions.
Visual-Inertial Navigation Extension
The later implementation used a Kalman filter driven by camera and inertial measurements rather than Vicon. Image noise accumulated into pose and accelerometer-bias errors, allowing a nominally collision-free trajectory to drift toward obstacles. The planner and trajectory generator were modified to reduce sensitivity to these errors.
First, the vehicle hovered for one second before initiating the planned motion. This startup interval supplied additional images to the estimator and reduced the observed accelerometer bias. Second, the dense A* path was simplified with Ramer–Douglas–Peucker and then augmented with intermediate points when adjacent simplified waypoints were separated by excessive distance. This interpolation addressed the timing sensitivity of long polynomial segments: overly long segment durations produced curved paths, whereas overly short durations demanded accelerations that caused overshoot.
Segment duration was assigned as a function of the square root of distance rather than under a constant-speed assumption. The final parameter configuration used a 0.15 m occupancy-grid resolution, a 0.5 m obstacle margin, a 0.4 m simplification threshold, a maximum inserted-point spacing of 2 m, and a nominal speed of 2 m/s. Camera alignment with the velocity direction was also evaluated as a means of increasing visible features. The approximation produced yaw changes that were difficult to track at sharp turns, so orientation control was excluded from the submitted system.
Experimental Results
Three Vicon-based maze experiments were performed. Most position-tracking errors remained below 0.1 m and increased to less than 0.2 m near sharp turns, remaining within the 0.25 m obstacle-inflation margin. Velocity estimates contained visibly greater noise, and abrupt changes in direction produced the largest tracking errors. These measurements indicate that geometrically short paths with sharp corners can be difficult to execute even when they remain collision-free in the occupancy grid.
The visual-inertial stage was used to evaluate estimator-aware changes to planning and trajectory generation, but the available report does not provide a comparable aggregate tracking-error result. Its evidence is therefore treated as an implementation study rather than a quantitative comparison with the Vicon experiments.
Author Contributions
For the visual-inertial extension, my primary contribution was the generation and timing of the fifth-order minimum-jerk trajectory segments.
Limitations and Discussion
The Vicon experiments isolated planning and control performance under accurate pose feedback, whereas the visual-inertial experiments exposed coupling between estimation and trajectory design. Direct quantitative comparison between the two stages is limited by their different sensing conditions and trajectory parameters. In addition, the constant-speed trajectory used for the Vicon trials did not explicitly optimize dynamic feasibility, and the visual-inertial implementation omitted active camera-orientation control. Within these experiments, three design considerations were salient: matching gains to the physical platform, preserving dynamically relevant path geometry during simplification, and selecting trajectory timing with estimator behavior in mind. Broader evaluation would be required to determine how these considerations generalize.
Conclusion
The implemented system connected occupancy-grid planning, trajectory generation, and feedback control across simulation, motion-capture flight, and visual-inertial navigation. The physical experiments achieved sub-0.1 m tracking error over most of the tested maze trajectories, with larger errors concentrated at sharp turns. The later estimator-aware design introduced a startup hover, controlled path simplification, waypoint interpolation, and minimum-jerk timing. These observations motivate joint tuning of planning, estimation, and control rather than independent optimization of each subsystem.
Project Materials
Planning and Control on the Physical Crazyflie
Open the physical Crazyflie report in Google Drive