滑らかな加速がしたい(三角関数編)

はじめに

みなさん,台形加速していますか?
僕もしています.


(初めてマウスを作り始めたときの様子)

綺麗な目標速度ですね.
しかし先日「このグラフはありえないよねー」とのツッコミを頂き,滑らかな加減速がしたいなあと思い始めました.
というのも,速度の変化時に微分不可能な部分があるため,追従が悪くなるということだそうです.

今回は,三角関数を用いた加減速についてまとめようとおもいます.

条件

僕の場合,走行するときには次のパラメータを用意します.

  • 初速度 $ v_s $
  • 最高速度 $ v_{max} $
  • 終速度 $ v_e $
  • 最高加速度の絶対値 $ a_{max} $
  • 到達目標距離 $ x_{all} $

今回の加減速では三角関数を使うといいましたが,次のような加速・減速になってほしいなあと思いました.

加速のようす(式 $ f_1 $ )

この式の基本形には $ \cos $ を使っています.

$$
\begin{eqnarray}
f_1 &=& A \left( 1-\cos\left( \alpha t \right) \right) +v_s \\
&=& \dfrac{\Delta v}{2}\left( 1-\cos\dfrac{2a_{max}}{\Delta v}t \right) +v_s
\end{eqnarray}
$$

ただし, $\Delta v = v_{max} - v_s$ としました.

減速のようす(式 $ f_2 $)

こちらも同様に

$$
\begin{eqnarray}
f_1 &=& A \left( 1+\cos \alpha \left(t-t_1-t_2\right) \right) +v_e \\
&=& \dfrac{\Delta v^{\prime}}{2}\left( 1-\cos\dfrac{2a_{max}}{\Delta v^{\prime}}\left(t-t_1 \right) \right) +v_e
\end{eqnarray}
$$

ただし, $\Delta v^{\prime} = v_{max} - v_e$ としました.

これらの加減速区間と等速区間を組み合わせて,次のような速度変化にすることが理想形です.

しかし,目標距離が短くて最高速度に達しない場合を考慮する必要があります.
この場合は次のような速度変化になります.

というわけで,
最高速度に達するかどうかの判定をしてから,目標とする加速距離と加速時間・減速距離と減速時間を導出していきます.

ひたすら計算

加速区間と加速時間

与えられたパラメータから,加速に必要な距離と時間を求めます.
ただし,次のように変数を設定します.

  • 加速区間(加速に必要な距離) $x_1$
  • 加速時間(加速に必要な時間) $t_1$
  • 等速区間(等速運動し続ける距離) $x_2$
  • 等速時間(等速運動し続ける時間) $t_2$
  • 減速区間(減速に必要な距離) $x_3$
  • 減速時間(減速に必要な時間) $t_3$

加速時間

これは簡単で,位相が $\pi$ 動く時間であるので,
$$
\begin{eqnarray}
\dfrac{2 a_{max}}{\Delta v} t_1 &=& \pi \\
t_1 &=& \dfrac{\pi \Delta v}{2 a_{max}}
\end{eqnarray}
$$
として導出できました.

加速区間

では,加速に必要な距離を求めます.
先程導出した $t_1$ までの時間で積分をすれば距離が求まります.

$$
\begin{eqnarray}
x_1 &=& \int^{t_1}_0 f_1 dt \\
&=& \left( \dfrac{\Delta v}{2} + v_s \right) \dfrac{\Delta v}{2 a_{max}} \pi \\
&=& \dfrac{v_{max}^2 - v_s^2}{4 a_{max}} \pi
\end{eqnarray}
$$

減速区間と減速時間

同様に,減速に必要な距離と時間を求めます.

減速時間

$$
\begin{eqnarray}
\dfrac{2 a_{max}}{\Delta v^{\prime}} t_3 &=& \pi \\
t_3 &=& \dfrac{\pi \Delta v^{\prime}}{2 a_{max}}
\end{eqnarray}
$$
として導出できました.

減速区間

では,減速に必要な距離も同様に.
$$
\begin{eqnarray}
x_3 &=& \int^{t_1+t_2+t_3}_{t_1+t_2} f_2 dt \\
&=& \left( \dfrac{\Delta v^{\prime}}{2} + v_e \right) \dfrac{\Delta v^{\prime}}{2 a_{max}} \pi \\
&=& \dfrac{v_{max}^2 - v_e^2}{4 a_{max}} \pi
\end{eqnarray}
$$

最高速度に達するかどうかの判定

ここで,最高速度に達するかどうかの判定を行います.
次の式が満たされる場合は最高速度に達する,そうでない場合は達しないということになります.
$$
\begin{eqnarray}
x_{all} &>& x_{1} + x_{3} \\
&>& \dfrac{\pi}{2 a_{max}} \left( v_{max}^2 - \dfrac{v_s^2 + v_e^2}{2} \right)
\end{eqnarray}
$$

最高速度の設定

先程の式が満たされなかった場合,つまり最高速度の再設定が必要な場合は,次の式から新しい最高速度を求めます.
$$
\begin{eqnarray}
x_{all} &=& x_{1} + x_{3} \\
&=& \dfrac{\pi}{2 a_{max}} \left( v_{max}^2 - \dfrac{v_s^2 + v_e^2}{2} \right) \\
v_{max}^2 &=& 2 a_{max} \dfrac{l}{\pi} + \dfrac{v_s^2 + v_e ^2}{2} \\
v_{max} &=& \sqrt{ 2 a_{max} \dfrac{l}{\pi} + \dfrac{v_s^2 + v_e ^2}{2} }
\end{eqnarray}
$$

おわり

きれいな加速ができるようになりました.