再利用可能なコンポーネントを使用したロボットの軌跡の計画

この例では次を使用します。

この例では、entry 端子と exit 端子を使用して、リンクされた Atomic サブチャートに出入りする複数の接続を作成する方法を説明します。entry 端子と exit 端子を使用すると、Atomic サブチャートに出入りするためのロジックを分離し、チャートが Stateflow® 階層の境界を越えて遷移できるようになります。entry 端子と exit 端子の詳細については、ステートの境界を越える入口および出口接続の作成を参照してください。

この例では、Simulink® モデルはドッキングステーションを探して障害物コースを動くロボットをシミュレートします。このモデルには 3 つの Stateflow チャートがあります。

Route Control は、ロボットがドックを探知し障害物コースを移動するために使用する手法を定義します。
Robot は、ドックやその周囲の障害物を基準としたロボットの物理特性 (位置や動きの方向など) を定義します。
Plot Trajectory は、ロボットが障害物を避けてドックを探知する際に取る経路の視覚的表現を作成します。

探知手法の定義

Route Control チャートは、ロボットがドッキングステーションを探知するために使用する手法を定義します。このチャートは、Simulink ライブラリモデル sfRobotExampleLib.slx からリンクされている Atomic サブチャートを組み合わせて構成されています。リンクされた Atomic サブチャートはマクロとして機能し、ロボットに前への移動、左または右への回転、ドックとの無線連絡を指示します。これらのサブチャートを 1 つ以上組み合わせて、独自の探知手法をプログラムできます。

この例では、ロボットは直線上を動き、一定間隔でドックと無線連絡を行います。ロボットがドックから離れていることを検知した場合、ロボットは右に 45 度回転し、引き続き探知を行います。ロボットが障害物に接触した場合は、ロボットは左に 45 度回転し、引き続き探知を行います。初期状態では、ロボットは秒速 0.5 メートルで移動します。ロボットがドックから 2 メートル以内の範囲に入ると、速度を秒速 0.1 メートルまで下げます。ロボットがドッキングステーション付近に到達すると、シミュレーションは停止します。到達していない場合でも、tMax 秒が経過すると、シミュレーションは停止して警告が発生します。

サブチャートに入る接続を複数作成

Route Control チャートでは、リンクされた Atomic サブチャート Rotate には Left および Right というラベルがある 2 つの entry 端子があります。シミュレーション中、チャートがこれらの entry 端子のいずれかに遷移すると、Rotate サブチャートがアクティブになります。

サブチャート内では、各 entry 端子には一致する entry ジャンクションがあります。これらの entry ジャンクションからステート Rotate に接続する遷移は、ローカルデータオブジェクト direction の値を 1 または -1 に設定します。これらの値は、ロボットに時計回りに回転するか反時計回りに回転するかを指示します。その後、ステート entry アクションは、Robot チャートで関数 rotate を呼び出します。この関数は、ロボットの移動方向を direction*angle の角度だけ変更します。パラメーター angle は、サブチャートのプロパティダイアログボックスの [マッピング] タブで pi/4 に指定されます。詳細については、Atomic サブチャートおよびボックスでの変数のマッピングを参照してください。

サブチャートから出る接続を複数作成

リンクされた Atomic サブチャート Move には、Done および HIT というラベルがある 2 つの exit 端子があります。サブチャート内では、各 exit 端子には一致する exit ジャンクションがあります。シミュレーション中、サブステート Forward のステートアクションは、Robot チャートで関数 move を呼び出します。この関数は、ロボットの位置をパラメーター step で指定した距離だけ動かします。この値は、サブチャートのプロパティダイアログボックスの [マッピング] タブで 0.1 に指定されています。関数 obstacle により、ロボットが障害物に衝突したことが示されると、ロボットは最後の安全な場所まで戻り、チャートは exit 端子 HIT を使用してサブチャートの外に遷移します。それ以外の場合、ロボットは 1 秒間前に移動し、チャートは exit 端子 Done を使用してサブチャートの外に遷移します。

同様に、リンクされた Atomic サブチャート ContactDock には、CloserToDock および FartherFromDock というラベルがある 2 つの exit 端子があります。サブチャート内では、各 exit 端子には一致する exit ジャンクションがあります。シミュレーション中、サブステート ContactDock のステートアクションは、Robot チャートで関数 distanceToDock を呼び出します。この関数は、ロボットからドッキングステーションまでの距離を判別します。この距離が最後のタイムステップで減少した場合、チャートは exit 端子 CloserToDock を使用してサブチャートの外に遷移します。それ以外の場合、チャートは exit 端子 FartherFromDock を使用してサブチャートの外に遷移します。

物理環境と障害物コースのモデル化

Robot チャートは、ロボットの位置と方向を維持します。また、このチャートはロボットの移動と回転、ロボットからドックまでの距離の判別、障害物の検出を行うための複数の関数をエクスポートします。これらの関数を呼び出すことにより、Route Control チャートがロボットの位置と方向を直接操作することはなくなります。

マスクパラメーター [Starting Position (x,y)] と [Starting direction (o'clock)] は、startPos と startDir の値を指定します。これらの値を変更するには、Robot チャートをダブルクリックして [ブロックパラメーター] ダイアログボックスを開きます。詳細については、マスクを作成して Simulink とパラメーターを共有を参照してください。

ワークスペース変数 dock、dockWidth、circles、boxes は、ドッキングステーションとその周囲の障害物の位置とサイズを指定します。

dock は 2 要素のベクトルで、ドックの水平座標と垂直座標を指定します。
dockWidth はスカラーで、ドックの幅を指定します。
circles は行 3 列の行列で、各円形障害物の水平座標、垂直座標、半径を指定します。ここでは円形障害物の数です。
boxes は行 4 列の行列で、各矩形障害物の水平座標、垂直座標、幅、高さを指定します。ここでは矩形障害物の数です。

障害物コースには任意の数の障害物を含めることができますが、円形障害物と矩形障害物が少なくとも 1 つずつ必要です。既定では、この例のモデルの PreLoadFcn コールバックは、3 つの円形障害物と 3 つの矩形障害物がある障害物コースを定義します。

ライブラリ内の再利用可能なコンポーネントのパッケージ化

Robot チャートは、Simulink ライブラリモデル sfRobotExampleLib.slx からリンクされているライブラリチャートです。このライブラリには一体型のツールキットがあり、独自のロボットシミュレーションをプログラミングするためにこのチャートおよびリンクされた Atomic サブチャートを定義できます。詳細については、カスタムライブラリ (Simulink)を参照してください。

ロボットの軌跡のプロット

チャート Plot Trajectory はワークスペース変数 dock、dockWidth、circles、boxes を読み取り、また出力信号 x と y を Robot チャートから、done を Route Control チャートからそれぞれ読み取って、障害物コースと、ロボットが障害物を避けてドックを探知する際に取る経路の視覚的表現を作成します。

シミュレーションを開始すると、チャートは補助関数 sfRobotScene を呼び出して、MATLAB® Figure を作成します。ドッキングステーションは緑、障害物は赤で表示されます。この関数のコードはこの例の最後で紹介します。その後、シミュレーションの各ステップで、チャートは青色の円を使用してロボットの位置をプロットします。入力信号 done がシミュレーションの終了を示すと、チャートはロボットの最終的な位置をプロットし、出力信号 stop の値を true に設定します。これにより、Stop ブロックはシミュレーションを終了します。

sfRobotScene、plot、holdを外部関数として呼び出すため、チャートは関数coder.extrinsic (Simulink)を使用します。詳細については、Stateflow チャートでの外部 MATLAB 関数の呼び出しを参照してください。

障害物コースの要素の表示

補助関数 sfRobotScene は MATLAB Figure を作成します。ドッキングステーションは緑、障害物は赤で表示されます。

function sfRobotScene(dock,width,boxes,circles)

plot(nsidedpoly(8,Center=dock,Radius=2*width),FaceColor="green");
daspect([1 1 1])
hold on

for i = 1:height(circles)
    center = circles(i,1:2);
    radius = circles(i,3);
    plot(nsidedpoly(20,Center=center,Radius=radius),FaceColor="red");
end

for i = 1:height(boxes)
    box = boxes(i,:);
    X = [box(1) box(1)+box(3) box(1)+box(3) box(1)];
    Y = [box(2) box(2) box(2)-box(4) box(2)-box(4)];
    plot(polyshape(X,Y),FaceColor="red");
end

fig = gcf;
fig.Name = "Robot Obstacle Course";
fig.NumberTitle = 'off';
figure(fig)

end

参考

plot | hold | coder.extrinsic (Simulink)