navPath

計画されたパス

説明

navPath オブジェクトには、通常、幾何学的パス プランナーで作成されたパスが格納されます。パス上の点は、関連付けられた状態空間に状態として格納されます。

作成

構文

path = navPath

path = navPath(space)

path = navPath(space,states)

path = navPath(space,states,maxNumStates)

説明

path = navPath は、SE(2) 状態空間を使ってパス オブジェクト path を既定の設定で作成します。

path = navPath(space) は、space で指定された状態空間を使ってパス オブジェクトを作成します。space 入力は、StateSpace プロパティの値も設定します。

path = navPath(space,states) では、states で指定された状態サンプルでパスを初期化できます。states は状態サンプルの行列として指定します。状態 space オブジェクトの StateBounds 外にある状態はその範囲まで減らされます。states 入力は、States プロパティの値も設定します。

path = navPath(space,states,maxNumStates) は、パスで許可される指定した最大状態数 maxNumStates を使用してパス オブジェクトを作成します。maxNumStates 入力は、MaxNumStates プロパティの値も設定します。

プロパティ

StateSpace — パスの状態空間

stateSpaceSE2 (既定値) | 状態空間オブジェクト

パスの状態空間。状態空間オブジェクトとして指定します。パスの各状態は、指定された状態空間からのサンプルです。stateSpaceSE2、stateSpaceDubins、stateSpaceReedsShepp、stateSpaceSE3 などのオブジェクトを状態空間オブジェクトとして使用できます。nav.StateSpace オブジェクトを使用して状態空間オブジェクトをカスタマイズすることもできます。

States — パスの状態

zeros(0,StateSpace.NumStateVariables) (既定値) | 実数値の M 行 N 列の行列

パスの状態。実数値の M 行 N 列の行列。M はパス内の状態数で、N は各状態の次元です。このプロパティは、オブジェクトの作成中または関数 append を使用する場合にのみ設定できます。

例: [0 0 0; 1 1 0; 2 2 0]

例: [0 0 0 1 0 0 0; 1 1 1 1 0 0 0; 2 2 1 1 0 0 0]

データ型: double

NumStates — パス内の状態サンプルの数

読み取り専用: 0 (既定値) | 非負の整数

この プロパティ は読み取り専用です。

パス内の状態サンプルの数。非負の整数として指定します。この数は、States プロパティで指定された状態行列の行数と同じです。

データ型: double

MaxNumStates — パスで許可される最大状態数

inf (既定値) | 正のスカラー整数

パスで許可される最大状態数。正のスカラー整数として指定します。

inf として指定すると、パスは明示的にサイズ変更可能になります。コード生成では、DynamicMemoryAllocation を 'On' に設定する必要があります。

正のスカラー整数として指定すると、オブジェクト内の状態の最大数が指定した値に制限されます。コード生成時に DynamicMemoryAllocation を有効にすることができない場合に、これを使用してサイズ変更可能なパスを作成します。

データ型: double

オブジェクト関数

append	Add states to end of path
copy	Create copy of path object
interpolate	パスに沿って点を内挿
pathLength	パスの長さ

例

複数のウェイポイントに基づいた navPath の作成

Dubins 空間で、複数のウェイポイントに基づいて 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
    MaxNumStates: Inf

waypoints = [8 10 pi/2;
             7 14 pi/4;
             10 17 pi/2;
             10 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.4722

SE(3) 状態空間での複数のウェイポイントに基づいた navPath の作成

都市ブロックの 3 次元占有マップをワークスペースに読み込みます。セルを障害物なしとして見なすしきい値を指定します。

mapData = load("dMapCityBlock.mat");
omap = mapData.omap;
omap.FreeThreshold = 0.5;

占有マップをインフレートして、障害物の周囲の安全な動作のためにバッファー ゾーンを追加します。

inflate(omap,1)

状態変数の範囲を指定して SE(3) 状態空間オブジェクトを作成します。

ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;inf inf;inf inf]);

SE(3) 状態空間で、複数のウェイポイントに基づいて navPath オブジェクトを作成します。

path = navPath(ss);
waypoints = [40 180 15 0.7 0.2 0 0.1;
             55 120 20 0.6 0.2 0 0.1;
             100 100 25 0.5 0.2 0 0.1;
             130 90 30 0.4 0 0.1 0.6;
             150 33 35 0.3 0 0.1 0.6];
append(path,waypoints)

ちょうど 250 個の点が含まれるよう、そのパスに内挿を行います。

interpolate(path,250)

内挿されたパスと元のウェイポイントを可視化します。

show(omap)
axis equal
view([-10 55])
hold on
% Start state
scatter3(waypoints(1,1),waypoints(1,2),waypoints(1,3),"g","filled")
% Goal state
scatter3(waypoints(end,1),waypoints(end,2),waypoints(end,3),"r","filled")
% Intermediate waypoints
scatter3(waypoints(2:end-1,1),waypoints(2:end-1,2), ...
         waypoints(2:end-1,3),"y","filled")
% Path
plot3(path.States(:,1),path.States(:,2),path.States(:,3), ...
      "r-",LineWidth=2)

パスの長さを計算します。

len = pathLength(path);
disp("Path length = " + num2str(len))

Path length = 204.1797

拡張機能

C/C++ コード生成
MATLAB® Coder™ を使用して C および C++ コードを生成します。

バージョン履歴

R2019b で導入

参考

オブジェクト

• stateSpaceSE2 | stateSpaceReedsShepp | stateSpaceDubins | stateSpaceSE3 | pathmetrics

クラス

• nav.StateSpace

関数

• append | copy | interpolate | pathLength | shortenpath