This thesis presents an incremental sampling based optimal motion planning algorithm for systems with nonlinear differential constraints. One of the most famous sampling-based motion planning algorithm is the Rapidly-exploring Random Tree(RRT). RRT algorithm has many advantages. First and foremost, RRT algorithm guarantees probabilistically completeness; the probability of failure decays to zero exponentially with number of samples. Also RRT can be applied to high dimensional planning problem even upto more than 1000 dimensions.
Recently, the study about RRT* algorithm has been growing due to its asymptotically optimality. This thesis extends the studies of RRT* for holonomic systems and systems with linear differential constraints. In order to extend RRT* algorithm for nonlinear systems, a two point boundary value problem(TPBVP) should be solved. However it is difficult and challenging to solve the TPBVP. In this thesis, a TPBVP solver is implemented by Successive Approximation Approach(SAA). By comparison with previous work which uses first-order Taylor approximations, the proposed algorithm(SA-RRT*) produces more realistic and near optimal result. In addition, proposed algorithm was applied to some motion planning problem; Motion planning of a inverted pendulum and two-wheeled mobile robot. SA-RRT* showed more fine results than existing algorithm.