なめらかなスラロームの設計② 設計方法

なめらかなスラロームの設計② 設計方法

説明記事
マイクロマウス, 制御

前回の記事の続きです。

今回の記事では、なめらかなスラローム軌道の具体的な設計方法を紹介します。

なめらかなスラロームの設計 目次

手順の概要

今回のスラローム設計は、事前の軌道設計と走行時の適用に分けて考えることができます。

  1. 事前にコンピュータで、 スラローム軌道の形状 を設計する
  2. 走行時にマイコンで、そのときの並進速度をもとに 軌道をスケーリング する

まずはじめに、それぞれの概要を説明します。

1. 事前の形状設計の概要

スラローム走行をするときには、ターンの角度や移動距離などさまざまな拘束条件があります。

事前の形状設計では、それらを満たすような軌道の 形状 を設計します。

このときに、スラローム 前後に追加する直線 の長さも決定します。

この計算はスラロームの形状や角速度をプロットしながらいい感じに設計するので、PC上で処理します。

2. 走行時のスケーリングの概要

事前設計したスラローム形状は、設計した並進速度と角速度を同時に守って走行したときに実現する形状です。

そのため、設計時とは異なる並進速度でターンを走行したい場合は、その並進速度にあった角速度を再設計しなければなりません。

今回紹介する方法では角速度の関数形に注目して、適切にスケーリングを行うことで角速度を導出します。

この方法を用いることで、任意の並進速度でターンに差し掛かったとしても、その並進速度を維持した状態でターンをすることができます。

この計算は走行時に行う必要があるので、マイコン上で行うことになります。

事前の概形設計

それでは、具体的な設計方法を説明します。

例えば、マイクロマウスの探索90度ターンの軌道を設計する場合を考えます。

スラローム形状

ターンの前後には、5 mm の直線を追加したので、カーブ部分の $(x,y)$ 変位は (40 mm, 40 mm) となります。

座標のとり方

今回の記事では、$x$-$y$ 座標系を考え、初期状態の機体進行方向を $x$ 軸にとります。

姿勢角 $\theta$ は、$x$ 軸となす角を表します。

つまり、前進すると $x$ 座標が正になり、左折をすると $y$ 座標と角度 $\theta$ が正になり、右折ならば負になるという形です。

座標系

事前設計時に与える情報

ここで、今回スラローム形状を設計する上で必要な拘束条件を列挙します。

項目変数探索90度ターンの例備考
移動位置$(x_\mathrm{end},~y_\mathrm{end})$(45, 45) [mm]前後の追加直線を含めた変位
移動角度$\theta_\mathrm{end}$$\pi/2$ [rad]ターンの角度
曲線の$y$成分$y_\mathrm{curve}$40 [mm]前後の追加直線の調整に使用
最大角躍度$\dddot{\theta}_{\max}$1200 $\pi$ [rad/s/s/s]応答を見ながら調整
最大角加速度$\ddot{\theta}_{\max}$36 $\pi$ [rad/s/s]応答を見ながら調整
最大角速度$\dot{\theta}_{\max}$3 $\pi$ [rad/s]応答を見ながら調整

与える情報

事前設計時に得られる情報

次に、事前設計によって得られる情報です。

項目変数探索90度ターンの例備考
曲線変位$(x_\mathrm{curve},~y_\mathrm{curve})$(40, 40) [mm]カーブ部分の変位
旋回 前直線 長$l_1$5 [mm]カーブ前の直線の長さ
旋回 後直線 長$l_2$5 [mm]カーブ後の直線の長さ
基準旋回時間$t_\mathrm{ref}$0.28 [s]カーブ部分にかかる時間
基準並進速度$v_\mathrm{ref}$241.59 [mm/s]後の計算で用いる定数
基準 角躍度 関数$\dddot{\theta}_\mathrm{ref}(t)$下図にプロット$\ddot{\theta}_\mathrm{ref}(t)$ の時間微分
基準 角加速度 関数$\ddot{\theta}_\mathrm{ref}(t)$下図にプロット$\omega_\mathrm{ref}(t)$ の時間微分
基準 角速度 関数$\omega_\mathrm{ref}(t)$下図にプロット積分結果が移動角度 $\theta_\mathrm{end}$ と一致
基準 角度 関数$\theta_\mathrm{ref}(t)$下図にプロット$\theta_\mathrm{ref}(0)=0$, $\theta_\mathrm{ref}(t_\mathrm{ref}) = \theta_\mathrm{end}$

関数の具体的な形は参考記事をご覧ください。

角速度分布

導出手順

  1. 積分結果が移動角度 $\theta_\mathrm{end}$ となるなめらかな 角速度分布 関数 $\omega_\mathrm{ref}(t)$ を求める
  2. 位置 $(x,y)$ の数値積分によって カーブの終点位置 を求める
  3. カーブの終点位置が目標位置 $(x_\mathrm{curve},y_\mathrm{curve})$ になるような基準並進速度 $v_\mathrm{ref}$ を求める
  4. 前後の直線を含めた移動距離が $(x_\mathrm{end},y_\mathrm{end})$ と一致するようにカーブ前後の直線の長さを求める

それぞれの手順について、以下で説明していきます。

なめらかな角速度の設計

過去に紹介したなめらかな加速の設計方法を利用して、なめらかな角速度の関数 $\omega(t)$ を求めます。

具体的には、参考記事の変数を以下のように割り当てて、各関数を入手します。

項目参考記事における表記本記事で設定する変数
拘束条件
最大角躍度$j_\max$$\dddot{\theta}_{\max}$
最大角加速度$a_\max$$\ddot{\theta}_{\max}$
最大角速度$v_\max$$\dot{\theta}_{\max}$
始点速度$v_\mathrm{start}$0
目標速度$v_\mathrm{target}$0
移動距離$d$$\theta_\mathrm{end}$
導出結果
基準角躍度$j(t)$$\dddot{\theta}_\mathrm{ref}(t)$
基準角加速度$a(t)$$\ddot{\theta}_\mathrm{ref}(t)$
基準角速度$v(t)$$\dot{\theta}_\mathrm{ref}(t) =\omega_\mathrm{ref}(t)$
基準角度$x(t)$$\theta_\mathrm{ref}(t)$

参考記事では並進速度の曲線加速について述べられているので、それを回転の角速度に変換して考えています。

この表のように設定することでなめらかな角速度軌道が得られます。

数値積分

上記の方法で角度の関数 $\theta_\mathrm{ref}(t),~0\leq t \leq t_\mathrm{ref}$ が得られました。

この軌道を並進速度 $v$ で走行した場合、各時刻 $t$ における位置 $(x, y)$ は以下のように表されます。

$$ \begin{align} x(t) &= \int_0^t v \cos(\theta(t)) dt \\ y(t) &= \int_0^t v \sin(\theta(t)) dt \end{align} $$

この関数を解析的に積分できればいいのですが、一般にはできないといわれています。

そこで並進速度 $v_0$ (600 [mm/s] など現実的な速度) を適当に与えて数値積分によって終点位置を求めます。

$$ \begin{align} x_0 &= \int_0^{t_\mathrm{end}} v_0 \cos(\theta_\mathrm{ref}(t)) dt \\ y_0 &= \int_0^{t_\mathrm{end}} v_0 \sin(\theta_\mathrm{ref}(t)) dt \end{align} $$

このとき、関数 $\theta_\mathrm{ref}(t)$ は連続な関数として求められているので、単にオイラー積分するのではなく、 ルンゲ=クッタ法 によって4次関数近似をして積分することで、かなりの高精度で積分することが可能です。

詳しくは上記リンク先のWikipediaまたは次回の記事のC++による実装をご覧ください。

基準並進速度を求める

上記の方法によって適当な並進速度 $v_0$ で走行したときの終点位置 $(x_0,y_0)$ が求まりました。

もちろん、並進速度 $v_0$ は適当に与えたものなので、$(x_0,y_0)$ はまだ $(x_\mathrm{curve},y_\mathrm{curve})$ とはなりません。

そこで基準並進速度 $v_\mathrm{ref}$ を次のように定義します。

$$ \begin{align} v_\mathrm{ref} = \frac{y_\mathrm{curve}}{y_0} v_0 \end{align} $$

こうすると、並進速度 $v_\mathrm{ref}$ および角速度 $\omega_\mathrm{ref}(t)$ で走行したとき、走行位置はちょうど $(x_\mathrm{curve},y_\mathrm{curve})$ となります。

前後の直線の長さを求める

ここまでの計算で必要な情報が揃ったので、前後の直線の長さは次式によって求めることができます。

$$ \begin{align} l_1 &= x_\mathrm{end} - x_\mathrm{curve} - \frac{y_\mathrm{end} - y_\mathrm{curve}}{\tan\theta_\mathrm{end}} \\ l_2 &= \frac{y_\mathrm{end} - y_\mathrm{curve}}{\sin\theta_\mathrm{end}} \end{align} $$

ただし、 $\sin\theta_\mathrm{end} = 0$ となる180度ターンなどの場合はこの式では計算できないので、例外的に対応します。

よくよく考えてみると、180度ターンの場合は前後の直線はターンを平行移動させるだけなので、自由に設定できることがわかります。

180度ターン

この場合、ターンの内側が柱にぶつからない程度に直線長を設定しましょう。

事前設計のまとめ

以上までの事前設計により、基準並進速度 $v_\mathrm{ref}$ および 基準角速度 $\omega_\mathrm{ref}(t)$ と前後の直線長 $l_1,l_2$ が求まりました。

これらの速度で走行すると $(x,y,\theta)$ の積分結果が $(x_\mathrm{curve},y_\mathrm{curve},\theta_\mathrm{end})$ となり、前後の直線をあわせると総移動位置は $(x_\mathrm{end},y_\mathrm{end})$ に一致します。

以上の手順でスラロームの形状を設計することができました。

走行時のスケーリング方法

次に、設計したスラローム形状をそのときの並進速度で走行する方法を紹介します。

  1. 走行する並進速度 $v$ を用いて、積分結果が事前設計の概形になるような角速度 $\omega(t)$ を生成する。
  2. 並進速度 $v$ と求めた角速度 $\omega(t)$ で走行する。
  3. 各時刻で数値積分を行うことで、位置 $(x, y)$ が得られる。

並進速度を用いて角速度分布を求める

さて、事前設計では基準速度 $v_\mathrm{ref}$ で走行したときに所望の形状になるスラローム形状を設計しました。

それをそのまま走行時の並進速度 $v$ で走行すると、積分結果の位置 $(x,y)$ がズレてしまいます。

そこで、以下のようなスケーリング係数を定義します。

$$ \begin{align} k &:= \frac{v}{v_\mathrm{ref}} \end{align} $$

ここで、 角躍度が0次関数、角加速度が1次関数、角速度が2次関数となっていることを利用すると、

最大角躍度を $k^3 \dddot{\theta}_\max$, 最大角加速度を $k^2 \ddot{\theta}_\max$, 最大角速度を $k \dot{\theta}_\max$, 移動角度を $\theta_\mathrm{end}$ として、事前設計と同様に角速度分布 $v(t)$ を求めると、並進速度 $v$ で走行したときの位置の積分結果が $(x,y) = (x_\mathrm{curve},y_\mathrm{curve})$ となり、求めていた軌道が得られれます。

なお、この処理はターンに差し掛かったときに1度計算するだけで大丈夫です。

数値積分によって各時刻の位置を求める

上記の方法で並進速度と角速度が得られたので、スラロームの走行自体は可能なのですが、速度だけでなく位置のフィードバック制御を行うためには、各時刻の位置 $x(t),~y(t)$ が必要となります。

そこで、事前設計のときと同様に、ルンゲ=クッタ法を用いて数値積分することによって、各時刻の位置を算出します。

この処理は、走行をしながら各時刻に計算するといいでしょう。

走行時の処理のまとめ

以上の方法で、走行時にそのときの並進速度 $v$ を用いて、走行すべき角速度や位置を求めることができました。

これが走行する目標軌道となるので、あとは任意のフィードバック制御則で軌道追従を行うことになります。

その方法については、こちらの記事が参考になります。

入出力情報のまとめ

上記までがなめらかなスラローム軌道の設計方法です。

ここで、本設計で与える情報と得られる情報をまとめると、次のようになります。

設計時に与える情報

  • 最大角躍度 $\dddot{\theta}_{\max}$
  • 最大角加速度 $\ddot{\theta}_{\max}$
  • 最大角速度 $\dot{\theta}_{\max}$
  • 旋回角度 $\theta_{\rm end}$
  • 移動位置 $x_{\rm end},~ y_{\rm end}$
  • 曲線部分の$y$成分 $y_\mathrm{curve}$

設計時に得られる情報

  • 基準並進速度 $v_{\rm ref}$
  • 曲線部分 $x_\mathrm{curve}, y_\mathrm{curve}$
  • 旋回前 直線長 $l_1$
  • 旋回後 直線長 $l_2$

走行時に与える情報

  • 並進速度 $v$

走行時に得られる情報

  • 時刻 $t$ における以下の値
    • 角躍度 $\dddot{\theta}(t)$
    • 角加速度 $\ddot{\theta}(t)$
    • 角速度 $\dot{\theta}(t)$
    • 角度 $\theta(t)$
    • 位置 $x(t)$, $y(t)$

まとめ

今回の記事では、なめらかなスラローム軌道の具体的な設計方法について述べました。

次回の記事では、C++による実装例を紹介します。