Motivated by autonomous aerial vehicles, this thesis provides a methodology for optimal trajectory planning of affine systems in non-convex environments. The resulting approximation of the optimal trajectory can then be provided to a flight controller as a reference trajectory, which compares the actual state of the system with the reference trajectory and performs the necessary control input corrections. More specifically, a modified trajectory planner inspired by Kinodynamic RRT* is presented to solve optimal control problems for input constrained affine systems with non-convex state spaces. As a result, if a solution is obtained then the solution is guaranteed to verify the state and control input constraints of the problem. Additionally, a randomized sampler function is proposed for Kinodynamic RRT* using a Gaussian distribution across the system’s state space. When the distribution is adequately sized lower cost approximate solutions of the optimal trajectory problem is obtained in less computation time when compared with other methods in the literature. The results are successfully applied to optimal control problems for an affine double integrator with drift that is subject to a maximum control input magnitude in non-convex environments.