Добавьте состояния к концу пути
Создайте объект navPath на основе нескольких точек пути в пространстве Дубинса.
dubinsSpace = stateSpaceDubins([0 25; 0 25; -pi pi])
dubinsSpace =
stateSpaceDubins with properties:
SE2 Properties
Name: 'SE2 Dubins'
StateBounds: [3×2 double]
NumStateVariables: 3
Dubins Vehicle Properties
MinTurningRadius: 1
pathobj = navPath(dubinsSpace)
pathobj =
navPath with properties:
StateSpace: [1×1 stateSpaceDubins]
States: [0×3 double]
NumStates: 0
waypoints = [...
8 10 pi/2;
10 12 pi/4;
12 17 pi/2;
11 10 -pi];
append(pathobj, waypoints);Интерполируйте этот путь так, чтобы он содержал ровно 250 точек.
interpolate(pathobj, 250)
Визуализируйте интерполированный путь и исходные путевые точки.
figure; grid on; axis equal; hold on; plot(pathobj.States(:,1), pathobj.States(:,2), ".b"); plot(waypoints(:,1), waypoints(:,2), "*r", "MarkerSize", 10)
Вычислите длину пути.
len = pathLength(pathobj);
disp("Path length = " + num2str(len))Path length = 19.37