滑らかな加速の設計③ 走行距離を考慮した速度設計

設計2・走行距離を考慮した速度設計

前回の記事の続きです.

この記事では,設計1の曲線加速を組み合わせて,走行距離を考慮した速度設計について考えます.

滑らかな加速の設計 目次

設計内容

設計1によって,始点速度と終点速度を与えれば,それを繋ぐ曲線加速を生成できるようになりました.

今度はそれを組み合わせて,始点速度→加速→最大速度→減速→終点速度,というような速度軌道を設計します.

加速曲線

加速曲線

ここで,図中の赤線である最大速度とその継続時間は,走行距離の拘束条件を満たすように算出します.

拘束条件

与える情報は以下の通りです.

  • 最大加速度 $a_{m}$
  • 始点速度 $v_{s}$
  • 飽和速度 $v_{a}$
  • 目標速度 $v_{t}$
  • 走行距離 $d$
  • 加速度が曲線である時間 $t_c$

設計1との違い

設計1と比べて,飽和速度$v_a$と走行距離$d$が新たに加わりました.

また,終点速度$v_e$という名前だった速度が目標速度$v_t$という名前に変わりました.

というのも,走行距離$d$と最大加速度$a_m$の拘束条件により,与えられた目標速度$v_t$に達することができない場合があるからです.

結果的に算出される終点速度$v_e$は,目標速度と$v_t$と異なる場合があるので注意します.

この場合,当初の設計よりも速い速度でターンに突入することがありあますが,無理に減速して姿勢を崩した状態で突入するよりはマシでしょう.

求めた式

  • 走行距離などの拘束条件から終点速度$v_e$を求める式
  • 走行距離などの拘束条件から最大速度$v_m$を求める式
  • 任意の時刻 $t$ における,加速度 $a(t)$,速度 $v(t)$ および位置 $x(t)$ を返す式

走行距離の拘束から終点速度$v_e$を求める式

与えられた拘束条件から終点速度を求める式です.

$$ \begin{align} v_e(t_c, a_m, v_s, v_t, d) &:= \left\{\begin{array}{ll} v_{e1} & (d \ge d_m) \newline v_{e2} & (d < d_m) \end{array}\right. \end{align} $$ ただし, $$ \begin{align} d_m &:= 2 v_s t_c + a_m t_c^2 \newline v_{e1} &:= \frac{-a_m t_c + \sqrt{a_m^2 t_c^2-4(a_m t_c v_s - v_s^2 - 2a_m d)}}{2} \newline v_{e2} &:= \frac{1}{3}\left(c +\frac{4a^2}{c} -a \right) \newline a &:= v_s \newline b &:= \frac{a_md^2}{t_c} \newline c &:= \sqrt[3]{\frac{\sqrt{27b(32a^3+27b)} + 16a^3+27b}{2}} \end{align} $$

少し複雑になってしまいましたが,一応これで求めることができます.

注意点として,$v_{e2}$の計算する際,減速の場合,2乗根の中身が負になる場合があるので,この部分の計算は複素数で行う必要があります.

答えは必ず実数になるので,どうにか実数で計算できないか考えましたが,今のところ解決策は見つかっていません.

走行距離などから最大速度$v_m$を求める式

与えられた拘束条件から最大速度を求める式です.

$$ \begin{align} v_m(t_c, a_m, v_s, v_a, v_e, d) &:= \min\{v_a,~v_{m2}\} \end{align} $$ ただし, $$ \begin{align} v_{m2} &:= \max\{v_s, v_e, v_{m1}\} \newline v_{m1} &:= \frac{-a_mt_c + \sqrt{a_m^2t_c^2-(v_s+v_e)a_mt_c+4a_md+2(v_s^2+v_e^2)}}{2} \end{align} $$

任意の時刻 $t$ における,加速度 $a(t)$,速度 $v(t)$ および位置 $x(t)$

ここままで与えられた,もしくは算出した下記パラメータを用いて設計1の曲線加速を組み合わせます.

  • 最大加速度 $a_{m}$
  • 始点速度 $v_{s}$
  • 最大速度 $v_{m}$
  • 終点速度 $v_{e}$
  • 走行距離 $d$
  • 加速度が曲線である時間 $t_c$

設計1の曲線を2つ組み合わせる

設計1の曲線を2つ組み合わせる

前回の記事で設計した設計1の曲線加速に対して,

  • 最大加速度を$a_m$
  • 始点速度を$v_s$
  • 終点速度を$\color{red}{v_m}$
  • 加速度が曲線である時間を$t_c$

としたものを加速曲線 $ac$,

  • 最大加速度を$a_m$
  • 始点速度を$\color{red}{v_m}$
  • 終点速度を$v_e$
  • 加速度が曲線である時間を$t_c$

としたものを減速曲線 $dc$として定義します.

このとき,減速曲線$dc$の方は,上図の通り$t_2$だけ時間をシフトした形で使用します.

各境界点時刻の定義は,以下の通りです.

$$ \begin{align} t_0 &:= 0 \newline t_1 &:= ac.t_{end} \newline t_2 &:= t_1 + \frac{d - ac.x_{end} - dc.x_{end}}{v_m} \newline t_3 &:= t_2 + dc.t_{end} \end{align} $$

$ac.t_{end}$などの表記は,察してください.

導出過程

走行距離などから終点速度を求める式

走行距離$d$がある程度小さいとき,与えられた拘束条件では目標速度$v_t$にたどり着けない場合があります.

ここでは,そのような場合にギリギリたどり着ける終点速度$v_e$を求めます.

加速曲線

加速曲線

例によって,上図のように,走行距離$d$の大きさによって等加速度の時間$t_m$の有無が変わるので場合分けが必要です.

そこで,$t_m=0$となる走行距離$d_m$を求めます.

$t_m=0$となる場合

$t_m=0$となる場合

図の曲線の数式を積分して面積を求めます.

$$ \begin{align} d_m & = \int_{t_0}^{t_1}v(t) dt + \int_{t_1}^{t_2}v(t) dt \newline &= \int_{t_0}^{t_1}\left( v_0+\frac{1}{2} j_m(t-t_0)^2 \right) dt \newline &\quad+ \int_{t_1}^{t_2}\left( v_0+\frac{1}{2} j_m(t_1-t_0)^2 +j_m(t_1-t_0)(t-t_1) - \frac{1}{2}j_m(t-t_1)^2 \right) dt \newline &= 2v_0t_c+a_mt_c^2 \end{align} $$

ただし,最大躍度$j_m:=a_m/t_c$を用いました.

このあとは,走行距離$d$が$d_m$より大きいか小さいかで場合分けをして,終端速度$v_e$を求めます.

$d\ge d_m$のとき

$d\ge d_m$のとき,$t_m \ge 0$となります.

求める終点速度を$v_{e1}$とおくと, 曲線の対称性により,台形の面積の公式を用いて,

$$ \begin{align} d &= \frac{1}{2}(v_s+v_{e1})(t_3-t_0) \newline &= \frac{1}{2}(v_s+v_{e1})(t_c+t_m+t_c) \newline &= \frac{1}{2}(v_s+v_{e1})\left(t_c+\frac{v_{e1}-v_s}{a_m}\right) \newline \Leftrightarrow v_{e1} &= \frac{1}{2}\left(\sqrt{4v_s^2-4v_sa_mt_c+a_m(a_mt_c^2+8d)}-a_mt_c\right) \end{align} $$

$d < d_m$のとき

$d < d_m$のとき,$t_m < 0$となります.

求める終点速度を$v_{e2}$とおくと, 曲線の対称性により,台形の面積の公式を用いて,

$$ \begin{align} d &= \frac{1}{2}(v_s+v_{e2})(t_3-t_0) \newline &= \frac{1}{2}(v_s+v_{e2})2\sqrt{\frac{t_c}{a_m}(v_{e2}-v_s)} \newline \Leftrightarrow v_{e2}^3 &+ v_s v_{e2}^2-v_s^2v_{e2}-v_s^3-\frac{a_md^2}{t_c} = 0 \newline \Leftrightarrow v_{e2} &= \frac{1}{3}\left(c +\frac{4a^2}{c} -a \right) \newline a &:= v_s \newline b &:= \frac{a_md^2}{t_c} \newline c &:= \sqrt[3]{\frac{\sqrt{27b(32a^3+27b)} + 16a^3+27b}{2}} \end{align} $$

以上をまとめると,たどり着き得る終点速度$v_{e1}$は,

$$ \begin{align} v_{e} &:= \left\{\begin{array}{ll} v_{e1} & (d \ge d_m) \newline v_{e2} & (d < d_m) \end{array}\right. \end{align} $$

これで,走行距離から逆算した終点速度$v_e$が求まりました.

走行距離などから最大速度を求める式

次に,下図のように走行するために,最大速度$v_m$を求めます.

加速曲線

加速曲線

まずはじめに,飽和速度$v_a$は考えずに,始点速度$v_s$から最大速度$v_{m1}$に達して終点速度$v_e$に達する下図の場合を考えます.

加速曲線

加速曲線

その間の走行距離が$d$となることから$v_{m1}$求めると以下が得られます.

$$ \begin{align} d &= \frac{1}{2}(v_s+v_{m1})(t_1-t_0)+ \frac{1}{2}(v_{m1}+v_e)(t_3-t_2) \newline &= \frac{1}{2}(v_s+v_{m1})\left(t_c+\frac{v_{m1}-v_s}{a_m}\right)+ \frac{1}{2}(v_{m1}+v_e)\left(t_c+\frac{v_e-v_{m1}}{a_m}\right) \newline \Leftrightarrow v_{m1} &:= \frac{-a_mt_c + \sqrt{a_m^2t_c^2-(v_s+v_e)a_mt_c+4a_md+2(v_s^2+v_e^2)}}{2} \end{align} $$

ここで,無駄な減速を避けるため,以下の式で飽和します.

$$ \begin{align} v_{m2} &:= \max\{v_s,~v_e,~v_{m1}\} \end{align} $$

最後に,最大速度$v_m$は,飽和速度$v_a$を超えてはならないので,それで飽和させます.

$$ \begin{align} v_m = \min\{v_a,~v_{m2}\} \end{align} $$

これで,下図のように走行する最大速度$v_m$が求まりました.

加速曲線

加速曲線

まとめ

本記事では,走行距離の拘束条件を満たすような曲線加速を設計しました.

次回の記事では,そのためのC++実装例を紹介します.

追記:2019.03.19

一部の場合分けのミスを修正しました.