自分用の備忘録

【Meister振り返り①】任意の速度,距離での台形加速

※この記事は自分向けの備忘録です.台形加速についてなかなか簡単にプログラムがかけたと思っていたので,資料を残そうとMeisterのプログラムを見直したら,台形加速が間違っていたことに気づいてしまいました.記事を信用しないでください.

 

任意の速度,加速度,距離で台形加速できること,任意の角速度,角加速度でスラロームできることは,様々な動作を必要とする斜め走行には必要となってきます.スラロームは角加速度を台形加速してあげればよく,かつはじめと終わりの角速度はともに0でより簡単なので,初期の速度と終端速度が異なる台形加速ができることが難所となると思います.

 

引数

初期速度: v_{start} 

最高速度: v_{max} 

終端速度: v_{end}

加速度: a 

重心の進行距離: x

目標距離: x_{target} 

 

言葉の定義

台形加速の加速終了時点での進んだ距離: x_{accelend}

台形加速の減速開始時点での進んだ距離: x_{decelstart}

三角加速の最高速度: v_{max}'

三角加速の最高速度到達時点の進んだ距離: x_{judge}

 

なお,今回の記事内では初期の距離は0から始まるものとしています.

f:id:staytus:20190329143627p:plain



  • まず台形加速を考える 

 ①加速

初期速度から最高速度に達するまで加速すればよいです.

制御周期ごとに速度を加速度×制御周期だけ加算します.

加速の終わりの時点の進んだ距離x_{accelend}を記録します.

 ②速度維持

減速開始距離まで速度を維持します.

ただ,減速開始距離は計算で求める必要があり,

今回は加減速ともに同じ加速度(減速度)ということに着目して,

 減速開始距離=x_{target}-減速に要する距離=x_{target}-加速に要する距離× \frac{減速に要する距離l_{decel}}{加速に要する距離l_{accel}}

 

すなわち,

 x _ {decelstart}=x_{target}-x_{accelend} \frac{v_{max}^2-v_{end}^2}{v_{max}^2-v_{start}^2}

となります.

 

(∵等加速度直線運動の式:v^2-v_0^2=2alより,

\frac{減速に要する距離l_{decel}}{加速に要する距離l_{accel}}=\frac{v_{max}^2-v_{end}^2}{v_{max}^2-v_{start}^2})

 

v_{max}^2=v_{start}^2では発散するので例外処理します.

 ③減速

減速開始距離から目標距離x_{target}に到達するまで減速します.

 

  • 三角加速について

 ①加速

 三角加速の最高速度到達時点の進んだ距離まで加速します.

これは,計算して求める必要があり,

x_{judge}= \frac{1}{2}x_{target}+ \frac{1}{4a}(v_{end}^2-v_{start}^2)

です.

 (∵

v_{max}'^2-v_{start}^2=2al_{accel}

v_{max}'^2-v_{end}^2=2al_{decel}

l_{accel}+l_{decel}=x_{target}

以上3式より,

x_{decelstart}=x_{accelend}=l_{accel}について解く.)

②減速

減速開始距離から目標距離x_{target}に到達するまで減速します.

 

  • 台形加速と三角加速を一つにまとめる

0⃣初期化

すすんだ距離x,減速開始距離x_{decelstart}を初期化し,判定点x_{judge}を計算する.

1⃣x_{judge}より距離が小さい時,

①最高速v_{max} 以下なら加速する

②最高速v_{max} に達したなら速度を維持する.

 ②になったとき加速終了時点の距離x_{accelend}を記録し,

 上記の計算式より減速開始距離x_{decelstart}を求める.

2⃣x_{judge}より距離が大きい時,

③減速開始距離x_{decelstart}までは速度を維持する.

④減速開始距離x_{decelstart}以降は,減速する.

 

※三角加速のとき減速開始距離x_{decelstart}は計算されない(1⃣の②の条件に入らないため).しっかりと0で初期化しておく必要がある.

※負の加速度,正の減速度には対応していない.加速だけしたいときは,v_{end}=v_{max},減速だけしたいときは[v_{start}=v_{max}]とする.

 

こんな感じで4つの場合分けによって台形加速と三角加速のプログラムが書くことができると思います.台形加速のプログラムで根号を使った計算を避けることを1番に考えているので,拡張性はあまりないと思います.