実際に使える制御工学：応用編（２）「モーション（動作指令）生成。物理法則は守りましょう。」

＜この記事で伝えたい事＞ 

• モーションを作るときは、物理的な制約を考える必要があります
  モータのトルクや速度には限界があります。
  メカトロニクス機器は、その範囲でだけ動けます。
  
  

• 滑らかな動きは、実用性が高いです
  基本的に、波形が滑らかだと振動なく動けます。
  ただ、滑らかすぎると、動きが遅くなるトレードオフがあります。

1. はじめに

自己紹介はこちらです。

この記事は、主に若手～中堅エンジニアに向けた技術紹介・解説記事シリーズの第２弾です。

今回は、具体的な動かし方を決定するモーション生成方法の一例を紹介・解説します。

2. モーション生成時に考えるべき物理的制約

冒頭述べたように、モータなどのアクチュエータが出せるトルクや速度には限界があります。
 
この限界を最も無視したモーションが、ステップ位置指令です。

上図では、時間0.2[s]で位置が0→1[rad]に立ち上がっています。
一見、この位置指令波形には何の問題もないように見えるかもしれません。

しかし、この位置指令波形の速度がどうなっているか考えてみましょう。
時間0.2[s]での速度が∞になっています。

この位置指令波形を言語化すると
「0.2[s]のタイミングで0[rad]から1[rad]の位置に瞬間移動しろ！」
と言っていることになります。

いかに物理法則を無視したムチャな命令か分かってもらえると思います。
 
 
現実的には、出せる速度には限界があります。
加えて、加減速に使える推力/トルクにも限界があります。
 
少なくとも、この２つは考慮してモーションを作らないと、そのとおりの動きはできません。

我々は、物理的に実行不可能なムチャな命令を出さないよう、最低限の物理的制約は押さえてモーションを生成する必要があります。

3. 速度台形指令

モータが実現可能な最速のモーションが速度台形指令です。

これは、その名のとおり速度波形が台形のモーションです。
下図は、速度台形指令における位置・速度・加速度波形です。

• 移動距離$${\bm{X^{ref}}}$$

• 最大速度$${\bm{V_{max}}}$$

• 最大加速度$${\bm{A_{max}}}$$

• 最大減速度$${\bm{D_{max}}}$$
  は設計者が決められます。

• 加速時間$${\bm{t_a}}$$

• 一定速時間$${\bm{t_v}}$$

• 減速時間$${\bm{t_d}}$$
  は下式で算出します。

$$
\begin{array}{l}
t_a=\cfrac{V_{max}}{A_{max}}\\
t_d=\cfrac{V_{max}}{D_{max}}\\
t_v=\cfrac{X^{ref}}{V_{max}}-\cfrac{t_a+t_d}{2}
\end{array}
$$

モータ慣性モーメントを$${J}$$、
モータ最大トルクを$${\tau_{max}}$$として

$${A_{max}=\cfrac{\tau_{max}}{J}}$$
$${D_{max}=\cfrac{\tau_{max}}{J}}$$

と最大加減速度を決定し、かつ、$${V_{max}}$$をそのままモータ最大速度とすると

という動きになります。
これが実現可能な最速動作です。

ただ、摩擦などによるトルク損失を考えると、最大加減速度計算時のトルクには、モータ最大トルクよりやや小さめの数値を用いるべきです。
（そうしないと、ちょっとしたパラメータ誤差でトルク超過します）

なお、パラメータの数値次第で$${t_v}$$はマイナスになります。
$${t_v}$$がマイナスのとき、定速区間は存在しません。
そのパラメータ条件では、常に加速か減速をする動きしかできません。
 
このため、$${t_v}$$がマイナスの場合、以下の例外処理が必要です。

この例外処理は、$${V_{max},A_{max},D_{max}}$$の全てを小さくする方向にオーバーライドをかけます。
よって、安全面の問題はありません。

4. S字加減速指令

前述のとおり、速度台形指令は実現可能な最速モーションです。

ただし、速度台形指令では、加速度波形の急峻な立ち上がり・立ち下がりが発生します。
これは、ハンマーなどでインパクトを与えるのと似たようなもので、機器の振動源となりやすいです。

機器を振動しにくくするには、加速度波形を滑らかにするのが有効です。

S字加減速指令は滑らかなモーション生成方法の代表例の１つです。
加速時の速度波形が（ななめに傾けた）S字のようなモーションです。

下図は、S字加減速指令における位置・速度・加速度波形です。

• 移動距離$${\bm{X^{ref}}}$$

• 最大速度$${\bm{V_{max}}}$$

• 最大加速度$${\bm{A_{max}}}$$

• 最大減速度$${\bm{D_{max}}}$$
  は設計者が決められます。

• 加速時間$${\bm{t_a}}$$

• 一定速時間$${\bm{t_v}}$$

• 減速時間$${\bm{t_d}}$$
  は下式で算出します。

$$
\begin{array}{l}
t_a=\cfrac{2\cdot V_{max}}{A_{max}}\\
t_d=\cfrac{2\cdot V_{max}}{D_{max}}\\
t_v=\cfrac{X^{ref}}{V_{max}}-\cfrac{t_a+t_d}{2}
\end{array}
$$

$${A_{max},D_{max}}$$は、使ってよいトルクと機器の質量（or 慣性モーメント）から決めるのが妥当です。

速度台形指令もそうですが、加速時トルクと減速時トルクは別の数値にして問題ありません。
 
S字加減速指令でも、パラメータ次第で$${t_v}$$はマイナスになります。
$${t_v}$$がマイナスの場合、以下の例外処理が必要です。

速度台形指令と同様、$${V_{max},A_{max},D_{max}}$$を小さくする方向にオーバーライドをかけるので、安全面の問題はありません。

5. ユニバーサルカム曲線

速度台形指令では振動する、
かといってS字加減速指令では遅すぎる、
そんなときは、ユニバーサルカム曲線の出番です。

これは、元々は汎用的なカム曲線の数式です。
ここでは、それを応用したモーションをユニバーサルカム曲線と呼びます。

下図は、ユニバーサルカム曲線における加速度波形です。

加速度の時間関数は、以下になります。
（$${A_{max}}$$は最大加速度、$${D_{max}}$$は最大減速度）

$$
\begin{array}{ll}
A=A_{max}\cdot sin(\frac{\pi}{2}\cdot\frac{t-t_0}{t_1-t_0})&：(区間１，t_0≤t<t_1)\\
A=A_{max}&：(区間２，t_1≤t<t_2)\\
A=A_{max}\cdot cos(\frac{\pi}{2}\cdot\frac{t-t_2}{t_3-t_2})&：(区間３，t_2≤t<t_3)\\
A=0&：(区間４，t_3≤t<t_4)\\
A=-D_{max}\cdot sin(\frac{\pi}{2}\cdot\frac{t-t_4}{t_5-t_4})&：(区間５，t_4≤t<t_5)\\
A=-D_{max}&：(区間６，t_5≤t<t_6)\\
A=-D_{max}\cdot cos(\frac{\pi}{2}\cdot\frac{t-t_6}{t_7-t_6})&：(区間７，t_6≤t<t_7)
\end{array}
$$

数式どおりですが、ユニバーサルカム曲線は加速度をいきなりMAX/MINにしません。
SIN状の傾きをもって、なだらかにMAX/MINに近づけます。
このため、速度台形指令よりも滑らかな動きを実現できます。
（例外として、区間１,３,５,７が全て０だと速度台形指令と完全一致）

また、このモーションは最大加速度・減速度を出し続ける区間を持ちます。
このため、S字加減速指令より速い動きになります。
（例外として、区間２,６が全て０だとS字加減速指令と完全一致）

汎用的な数式だけあり、ユニバーサルカム曲線は設計自由度が高いです。
しかし、自由度が高すぎると、それを持て余すのはよくあることです。

そんなときは、敢えて拘束条件（=制限）をかけて自由度を下げます。
そうしてやると、扱いやすくなります。
 
ここでは、以下の拘束条件を設定します。

下図は、拘束条件をかけたユニバーサルカム曲線指令における位置・速度・加速度波形です。

• 移動距離$${\bm{X^{ref}}}$$

• 最大速度$${\bm{V_{max}}}$$

• 最大加速度$${\bm{A_{max}}}$$

• 最大減速度$${\bm{D_{max}}}$$

• 比率$${\bm{\alpha, \beta}}$$
  は設計者が決められます。
  （ただし、$${\alpha, \beta}$$の取れる範囲は0～1）

• 加速時間$${\bm{t_a}}$$

• 一定速時間$${\bm{t_v}}$$

• 減速時間$${\bm{t_d}}$$
  は下式で算出します。

$$
\begin{array}{l}
t_a=\cfrac{2\cdot V_{max}}{A_{max}\cdot (2\cdot\frac{\alpha}{\pi}+1-\alpha)}\\
t_d=\cfrac{2\cdot V_{max}}{D_{max}\cdot (2\cdot\frac{\beta}{\pi}+1-\beta)}\\
t_v=\cfrac{X^{ref}}{V_{max}}-\cfrac{t_a+t_d}{2}
\end{array}
$$

これまでのモーションと同様、$${A_{max},D_{max}}$$は使ってよいトルクと機器の質量（or 慣性モーメント）から決めるのが妥当です。
加速時トルクと減速時トルクは別の数値にして問題ありません。

この指令でも、パラメータ次第で$${t_v}$$はマイナスになります。
$${t_v}$$がマイナスの場合、以下の例外処理が必要です。

これまでのモーションと同様、$${V_{max},A_{max},D_{max}}$$を小さくする方向にオーバーライドをかけるので、安全面の問題はありません。

6. おわりに

この記事では、モーション生成方法を紹介・解説しました。
 
モーション生成には物理的制約を考慮する必要がありますが、制御の安定性は気にせず生成できます。

手を加えやすいためか、各メーカーの独自色が出やすい技術と思います。
例えば、同じ加速度波形を時間差で２発重ねることで特定周波数成分を打ち消す方法もあります。
（インプットシェーピングと呼ばれる技術です）
 
色々なメーカーの製品を触る機会があれば、比較してみるのも面白いかもしれません。

前回：応用編（１）「３重ループ制御」はこちら
次回：応用編（３）「ノッチフィルタによる振動抑制」はこちら

付録1. 速度台形指令の時間関数

下式が、速度台形指令の位置$${X}$$・速度$${V}$$・加速度$${A}$$の時間関数です。

■$${\bm{0}}$$≤$${\bm{t}}$$<$${\bm{t_a}}$$のとき

$$
\begin{array}{ll}
X&=\cfrac{A_{max}}{2}\cdot t^2\\
V&=A_{max}\cdot t\\
A&=A_{max}
\end{array}
$$

■$${\bm{t_a}}$$≤$${\bm{t}}$$<$${\bm{(t_a+t_v)}}$$のとき

$$
\begin{array}{ll}
X&=\cfrac{A_{max}}{2}\cdot t_a^2+V_{max}\cdot (t-t_a)\\
V&=V_{max}\\
A&=0
\end{array}
$$

■$${\bm{(t_a+t_v)}}$$≤$${\bm{t}}$$<$${\bm{(t_a+t_v+t_d)}}$$のとき

$$
\begin{array}{ll}
X&=\cfrac{A_{max}}{2}\cdot t_a^2+V_{max}\cdot t_v\\
　&　+V_{max}\cdot (t-(t_a+t_v))\\
　&　　-\cfrac{D_{max}}{2}\cdot(t-(t_a+t_v))^2\\
V&=V_{max}-D_{max}\cdot (t-(t_a+t_v))\\
A&=-D_{max}
\end{array}
$$

■$${\bm{(t_a+t_v+t_d)}}$$≤$${\bm{t}}$$のとき

$$
\begin{array}{ll}
X&=X^{ref}\\
V&=0\\
A&=0
\end{array}
$$

付録2. S字加減速指令の時間関数

下式が、S字加減速指令の位置$${X}$$・速度$${V}$$・加速度$${A}$$の時間関数です。

■$${\bm{0}}$$≤$${\bm{t}}$$<$${\bm{t_a}}$$のとき

$$
\begin{array}{ll}
X&=\cfrac{A_{max}}{2}\cdot\left(\frac{t^2}{2}+(\frac{t_a}{2\cdot\pi})^2\cdot(cos(\frac{2\cdot\pi}{t_a}\cdot t)-1)\right)\\
V&=\cfrac{A_{max}}{2}\cdot\left(t-\frac{t_a}{2\cdot\pi}\cdot sin(\frac{2\cdot \pi}{t_a}\cdot t)\right)\\
A&=\cfrac{A_{max}}{2}\cdot\left(1-cos(\frac{2\cdot\pi}{t_a}\cdot t)\right)
\end{array}
$$

■$${\bm{t_a}}$$≤$${\bm{t}}$$<$${\bm{(t_a+t_v)}}$$のとき

$$
\begin{array}{ll}
X&=V_{max}\cdot(t-t_a)+\cfrac{A_{max}\cdot t_a^2}{4}\\
V&=V_{max}\\
A&=0
\end{array}
$$

■$${\bm{(t_a+t_v)}}$$≤$${\bm{t}}$$<$${\bm{(t_a+t_v+t_d)}}$$のとき

$$
\begin{array}{ll}
X&=V_{max}\cdot t_v+\cfrac{A_{max}\cdot t_a^2}{4}+V_{max}\cdot(t-t_a-t_v)\\
&　-\cfrac{D_{max}}{2}\cdot\left(\frac{(t-t_a-t_v)^2}{2}+(\frac{t_d}{2\cdot\pi})^2\cdot(cos(\frac{2\cdot\pi}{t_d}\cdot(t-t_a-t_v))-1)\right)\\
V&=V_{max}-\cfrac{D_{max}}{2}\cdot\left(t-(t_a+t_v)-\frac{t_d}{2\cdot \pi}\cdot sin(\frac{2\cdot \pi}{t_d}\cdot (t-(t_a+t_v)))\right)\\
A&=-\cfrac{D_{max}}{2}\cdot \left(1-cos(\frac{2\cdot \pi}{t_d}\cdot t)\right)
\end{array}
$$

■$${\bm{(t_a+t_v+t_d)}}$$≤$${\bm{t}}$$のとき

$$
\begin{array}{ll}
X&=X^{ref}\\
V&=0\\
A&=0
\end{array}
$$

付録3. ユニバーサルカム曲線の時間関数

下式は、ユニバーサルカム曲線の各区間の終了時位置・速度です。
（あらかじめ算出しておくと、後の計算が楽です）

■区間１終了時位置・速度

$$
\begin{array}{ll}
x_1&= A_{max}\cdot\frac{(\alpha\cdot t_a)^2}{2\cdot\pi} - A_{max}\cdot\left(\frac{\alpha\cdot t_a}{\pi}\right)^2\\
v_1&=A_{max}\cdot\frac{\alpha\cdot t_a}{\pi}
\end{array}
$$

■区間２終了時位置・速度

$$
\begin{array}{ll}
x_2&=\frac{A_{max}}{2}\cdot\left((1-\alpha)\cdot t_a\right)^2 + v_1\cdot(1-\alpha)\cdot t_a + x_1\\
v_2&=A_{max}\cdot(1-\alpha)\cdot t_a + v_1
\end{array}
$$

■区間３終了時位置・速度

$$
\begin{array}{ll}
x_3&=A_{max}\cdot\left(\frac{\alpha\cdot t_a}{\pi}\right)^2 + v_2\cdot\frac{\alpha\cdot t_a}{2} + x_2\\
v_3&=A_{max}\cdot\frac{\alpha\cdot t_a}{\pi} + v_2
\end{array}
$$

■区間４終了時位置・速度

$$
\begin{array}{ll}
x_4&=v_3*t_v + x_3\\
v_4&=v_3
\end{array}
$$

■区間５終了時位置・速度

$$
\begin{array}{ll}
x_5&=-D_{max}\cdot\frac{(\beta\cdot t_d)^2}{2\cdot\pi} + D_{max}\cdot\left(\frac{\beta\cdot t_d}{\pi}\right)^2 + v_4\cdot\frac{\beta\cdot t_d}{2} + x_4\\
v_5&=-D_{max}\cdot\frac{\beta\cdot t_d}{\pi} + v_4
\end{array}
$$

■区間６終了時位置・速度

$$
\begin{array}{ll}
x_6&=-\frac{D_{max}}{2}\cdot\left((1-\beta)\cdot t_d\right)^2 + v_5\cdot (1-\beta)\cdot t_d + x_5\\
v_6&=-D_{max}\cdot(1-\beta)\cdot t_d + v_5
\end{array}
$$

■区間７終了時位置・速度

$$
\begin{array}{ll}
x_7&=-D_{max}\cdot \left(\frac{\beta\cdot t_d}{\pi} \right)^2 + v_6\cdot\frac{\beta\cdot t_d}{2} + x_6\\\
&(=X^{ref})\\
v_7&=-D_{max}\cdot\frac{\beta\cdot t_d}{\pi} + v_6\\
&(=0)
\end{array}
$$

また、下式が、ユニバーサルカム曲線の位置$${X}$$・速度$${V}$$・加速度$${A}$$の時間関数です。

■区間１（$${\bm{0}}$$≤$${\bm{t}}$$<$${\bm{0.5\cdot\alpha\cdot t_a}}$$）のとき

$$
\begin{array}{ll}
T&=t\\
X&=A_{max}\cdot \frac{\alpha\cdot t_a}{\pi}\cdot T - A_{max}\cdot \left(\frac{\alpha\cdot t_a}{\pi}\right)^2\cdot sin(\frac{\pi}{\alpha\cdot t_a}\cdot T)\\
V&=A_{max}\cdot \frac{\alpha\cdot t_a}{\pi}\cdot \left(1-cos(\frac{\pi}{\alpha\cdot t_a}\cdot T)\right)\\
A&=A_{max}\cdot sin(\frac{\pi}{\alpha\cdot t_a}\cdot T)
\end{array}
$$

■区間２（$${\bm{0.5\cdot\alpha\cdot t_a}}$$≤$${\bm{t}}$$<$${\bm{(1-0.5\cdot\alpha)\cdot t_a}}$$）のとき

$$
\begin{array}{ll}
T&=t-0.5\cdot\alpha\cdot t_a\\
X&=0.5*A_{max}\cdot T^2 + v_1\cdot T + x_1\\
V&=A_{max}\cdot T + v_1\\
A&=A_{max}
\end{array}
$$

■区間３（$${\bm{(1-0.5\cdot\alpha)\cdot t_a}}$$≤$${\bm{t}}$$<$${\bm{t_a}}$$）のとき

$$
\begin{array}{ll}
T&=t-(1-0.5\cdot\alpha)\cdot t_a\\
X&=A_{max}\cdot \left(\frac{\alpha*t_a}{\pi}\right)^2\cdot \left(1 - cos(\frac{\pi}{\alpha\cdot t_a}\cdot T)\right) + v_2\cdot T + x_2\\
V&=A_{max}\cdot \frac{\alpha\cdot t_a}{\pi}\cdot sin(\frac{\pi}{\alpha\cdot t_a}\cdot T) + v_2\\
A&=A_{max}\cdot cos(\frac{\pi}{\alpha\cdot t_a}\cdot T)
\end{array}
$$

■区間４（$${\bm{t_a}}$$≤$${\bm{t}}$$<$${\bm{t_a+t_v}}$$）のとき

$$
\begin{array}{ll}
T&=t-t_a\\
X&=v_3\cdot T + x_3\\
V&=v_3\\
A&=0
\end{array}
$$

■区間５（$${\bm{t_a+t_v}}$$≤$${\bm{t}}$$<$${\bm{t_a+t_v+0.5\cdot\beta\cdot t_d}}$$）のとき

$$
\begin{array}{ll}
T&=t-(t_a+t_v)\\
X&=-D_{max}\cdot \frac{\beta\cdot t_d}{\pi}\cdot T + D_{max}\cdot \left(\frac{\beta\cdot t_d}{\pi}\right)^2\cdot sin(\frac{\pi}{\beta\cdot t_d}\cdot T) + v_4\cdot T + x_4\\
V&=-D_{max}\cdot \frac{\beta\cdot t_d}{\pi}\cdot (1-cos(\frac{\pi}{\beta\cdot t_d}\cdot T)) + v_4\\
A&=-D_{max}\cdot sin(\frac{\pi}{\beta\cdot t_d}\cdot T)
\end{array}
$$

■区間６（$${\bm{t_a+t_v+0.5\cdot \beta\cdot t_d}}$$≤$${\bm{t}}$$<$${\bm{t_a+t_v+(1-0.5\cdot \beta)\cdot t_d}}$$）のとき

$$
\begin{array}{ll}
T&=t-(t_a+t_v+0.5\cdot \beta\cdot t_d)\\
X&=-0.5\cdot D_{max}\cdot T^2 + v_5\cdot T + x_5\\
V&=-D_{max}\cdot T + v_5\\
A&=-D_{max}
\end{array}
$$

■区間７（$${\bm{t_a+t_v+(1-0.5\cdot \beta)\cdot t_d}}$$≤$${\bm{t}}$$<$${\bm{t_a+t_v+t_d}}$$）のとき

$$
\begin{array}{ll}
T&=t-\left(t_a+t_v+(1-0.5\cdot \beta)\cdot t_d\right)\\
X&=-D_{max}\cdot \left(\frac{\beta\cdot t_d}{\pi}\right)^2\cdot \left(1 - cos(\frac{\pi}{\beta\cdot t_d}\cdot T)\right) + v_6\cdot T + x_6\\
V&=-D_{max}\cdot \frac{\beta\cdot t_d}{\pi}\cdot sin(\frac{\pi}{\beta\cdot t_d}\cdot T) + v_6\\
A&=-D_{max}\cdot cos(\frac{\pi}{\beta\cdot t_d}\cdot T)
\end{array}
$$

■停止区間（$${\bm{t_a+t_v+t_d}}$$≤$${\bm{t}}$$）のとき

$$
\begin{array}{ll}
T&=t-(t_a+t_v+t_d)\\
X&=X^{ref}\\
V&=0\\
A&=0
\end{array}
$$

前回：応用編（１）「３重ループ制御」はこちら
次回：応用編（３）「ノッチフィルタによる振動抑制」はこちら