衝突のないマルチロボット システムに対するバリア証明書の制約の適用

この例では、Barrier Certificate Enforcement ブロックを使用して衝突のないマルチロボット システムに対するバリア証明書の制約を適用する方法を示します。

概要

この例では、3 台のロボットの目標は互いに衝突せずにターゲット位置に到達することです [1]。2 台のロボットを使用する例については、衝突のないロボットに対するバリア証明書の制約の適用を参照してください。

ロボット ダイナミクスは x-y 平面で double の積分器によってモデル化されます。各ロボットには 4 つの状態 (x 位置、y 位置、x 速度、y 速度) と 2 つの制御変数 (x 加速度と y 加速度) があります。速度は [-2, 2] m/s に制約され、加速度は [-10, 10] $\mathrm{m}/{\mathrm{s}}^2$ に制約されます。PID コントローラーはロボットがターゲット位置に到達するように設計されています。制約を適用する前は、ロボットは互いに接近すると衝突します。

Simulink® モデルを開きます。

mdl = 'barrierCertificate3Robots';
open_system(mdl);

バリア証明書の制約

衝突回避するための制約では、2 台のロボットが互いに接近している場合、任意の 2 台のロボット ($\mathit{i}$、$\mathit{j}$、$\mathit{k}$) 間の距離が指定されたしきい値を超えたまま維持されるようにします [1]。したがって、ロボット $\mathit{i}$ とロボット $\mathit{j}$ のバリア証明書は以下のように求められます。

$$h_{ij}(x)=\sqrt{2{\alpha }_{sum}(\Vert ▵p_{ij}\Vert -D_s)}+\frac{▵p_{ij}^T▵v_{ij}}{\Vert ▵p_{ij}\Vert }$$.

ここでは、変数は次のとおりです。

• 両方のロボットからの最大制動力 — ${\alpha }_{\mathrm{sum}}=20$

• ロボット間の最小距離 — ${\mathit{D}}_{\mathit{s}}=0.7$

• 位置誤差のベクトル — $$▵p_{ij}=p_i-p_j$$

• 速度誤差のベクトル — $$▵v_{ij}=v_i-v_j$$

同様に、ロボット $\mathit{i}$ とロボット $\mathit{k}$ のバリア証明書は ${\mathit{h}}_{\mathrm{ik}}$ によって求められ、ロボット $\mathit{j}$ とロボット $\mathit{k}$ に対しては ${\mathit{h}}_{\mathrm{jk}}$ によって求められます。マルチロボット システムのバリア証明書は $$h(x)=[h_{ij}(x),h_{ik}(x),h_{jk}(x){]}^T$$ によって求められます。

状態 $\mathit{x}={\left[{\mathit{p}}_{\mathit{i}},{\mathit{p}}_{\mathit{j}},{\mathit{v}}_{\mathit{i}},{\mathit{v}}_{\mathit{j}},{\mathit{p}}_{\mathit{k}},{\mathit{v}}_{\mathit{k}}\right]}^{\mathit{T}}$ に対する $\mathit{h}\left(\mathit{x}\right)$ の偏導関数は $\mathit{q}\left(\mathit{x}\right)$ によって表されます。

この例では Symbolic Math Toolbox™ ソフトウェアを使用してバリア証明書とそれぞれの勾配関数を導出します。

この例で用意されている getBarrierCertificateAndJacobian スクリプトは以下のファイルを生成します。

• barrierFcn3Robots.m — バリア証明書の関数

• barrierGradFcn3Robots.m — バリア証明書のヤコビアン

スクリプトを実行します。

getBarrierCertificateAndJacobian;

ans = function_handle with value:
    @barrierFcn3Robots

ans = function_handle with value:
    @barrierGradFcn3Robots

いずれかの関数の詳細については、対応するファイルを開いてください。

Barrier Certificate Enforcement ブロックは形式 $$\underset{}{\overset{\dot{}}{x}}=f(x)+g(x)u$$ のダイナミクスを受け入れます。この例では、次のようになります。

$\mathit{f}\left(\mathit{x}\right)=\left[\begin{array}{cc}0& \mathit{I}\\ 0& 0\end{array}\right]\mathit{x}$ および $\mathit{g}\left(\mathit{x}\right)=\left[\begin{array}{c}0\\ \mathit{I}\end{array}\right]$。

単位行列 $\mathit{I}$ は 6 行 6 列の行列です。

バリア証明書の制約を使用した衝突のないコントローラーのシミュレーション

制約の実装を表示するには、Constraint サブシステムを開きます。

モデルを実行し、シミュレーション結果を確認します。3 台のロボットは、接近しすぎると互いに避けます。

mdl = 'barrierCertificate3Robots';
sim(mdl);

Three robots、visualization、distance サブシステムでスコープを開きます。任意の 2 台のロボット間の距離はしきい値 0.7 m を超えています。

Barrier Certificate Enforcement ブロックによって、3 台のロボットが衝突なしでそれぞれのターゲット位置に到達するように、正常に制御動作が制約されています。

bdclose(mdl)
f = findobj('Name','Three robots');
close(f)

参考文献

[1] Wang, Li, Aaron D. Ames, and Magnus Egerstedt. "Safety Barrier Certificates for Collisions-Free Multirobot Systems." IEEE Transactions on Robotics 33, no. 3 (June 2017): 661–74. https://doi.org/10.1109/TRO.2017.2659727.

参考

Barrier Certificate Enforcement

関連するトピック

• 制御設計へのバリア証明書の適用
• アダプティブ クルーズ コントロールに対するバリア証明書の制約の適用
• PID コントローラーに対するバリア証明書の制約の適用
• 衝突のないロボットに対するバリア証明書の制約の適用