This paper proposes an algorithm for finding the optimal trajectory on a set of admissible paths based on the task tree approach. The space solution represents two-dimensional plane. A classical approach is based on Dubin's work that gives sufficient family of trajectories. Those trajectories satisfy the Pontryagin's Maximum Principle which generalize Lagrange's problem of the calculus of variation. Our paper demonstrates how to calculate feasible trajectories for finding the minimum length path. For this purpose a lot of computer resource is needed. It requires a very efficient algorithm for selection of the optimal trajectory based on logic reasoning. The software support for the proposed approach is designed on intuitive and effective way and it can be used for the time critical operations.