A. P. Mashtakov, Algorithms and Software Solving a Motion Planning Problem for Nonholonomic Five-dimensional Control Systems