位置決め機構を「最高速度まで加速し、一定速で進み、停止位置へ向けて減速する」と考えると、基本となるのが台形速度プロファイルです。速度を時間に対して描くと台形になるため、この名前で呼ばれます。
ただし、移動距離が短い場合は最高速度へ到達する前に減速を始めなければなりません。このとき速度線図は台形ではなく三角形になります。最高速度と加速度を入力するだけでは不十分で、移動距離からプロファイル形状を判定する必要があります。
この記事では、停止状態から移動して再び停止する1軸運動を対象に、加速時間、等速時間、ピーク速度、総移動時間を計算し、位置・速度・加速度の時系列をPythonで生成します。
台形速度プロファイルの前提
今回扱うモデルでは、開始速度と終了速度を0とし、加速側と減速側で同じ加速度の絶対値を使います。
| 記号 | 内容 | 単位 |
|---|---|---|
| L | 移動距離 | m |
| v_max | 許容最高速度 | m/s |
| a_max | 許容最大加速度 | m/s² |
| t_a | 加速時間 | s |
| t_c | 等速時間 | s |
| T | 総移動時間 | s |
WPILibのTrapezoidProfileも、機構に許容する最高速度と最大加速度を制約として与えます。MathWorksのtrapveltrajは、加速・等速・減速の3区間を持つ台形速度軌道を生成します。
この計算で得るのは指令軌道です。モーター、減速機、負荷、制御器が実際に追従できるかは、トルク、回転速度、制御帯域、剛性などを別に確認します。
最高速度へ到達できる距離を求める
停止状態から一定加速度 $a_{\max}$ で最高速度 $v_{\max}$ まで加速する時間は次式です。
$$ t_a=\frac{v_{\max}}{a_{\max}} $$
この間に進む距離は次のとおりです。
$$ L_a=\frac{1}{2}a_{\max}t_a^2=\frac{v_{\max}^2}{2a_{\max}} $$
同じ距離を減速にも必要とするため、最高速度へ到達してから停止できる最小移動距離は次式になります。
$$ L_{\mathrm{limit}}=2L_a=\frac{v_{\max}^2}{a_{\max}} $$
移動距離 $L$ と $L_{\mathrm{limit}}$ の比較で形状を判定できます。
- $L\geq L_{\mathrm{limit}}$：最高速度へ到達する台形プロファイル
- $L
この判定を省くと、短い移動で等速時間が負になり、実現できない指令値を生成します。
台形プロファイルの時間を計算する
台形プロファイルでは、加速と減速に使う合計距離が $v_{\max}^2/a_{\max}$ です。残りが等速区間の距離になります。
$$ L_c=L-\frac{v_{\max}^2}{a_{\max}} $$
等速時間は次式です。
$$ t_c=\frac{L_c}{v_{\max}} $$
総移動時間は、加速、等速、減速の合計です。
$$ T=2t_a+t_c=\frac{L}{v_{\max}}+\frac{v_{\max}}{a_{\max}} $$
数値例：1.2 mを移動する
次の条件を考えます。
| 項目 | 値 |
|---|---|
| 移動距離 | 1.2 m |
| 最高速度 | 0.8 m/s |
| 最大加速度 | 1.5 m/s² |
最高速度へ到達するために必要な最小距離は次の値です。
$$ L_{\mathrm{limit}}=\frac{0.8^2}{1.5}\approx0.427\ \mathrm{m} $$
移動距離1.2 mの方が長いため、台形プロファイルになります。
$$ t_a=\frac{0.8}{1.5}\approx0.533\ \mathrm{s} $$
$$ t_c=\frac{1.2-0.8^2/1.5}{0.8}\approx0.967\ \mathrm{s} $$
したがって、総移動時間は約2.033 sです。
短距離では三角形プロファイルになる
三角形プロファイルには等速区間がありません。加速距離と減速距離がそれぞれ $L/2$ になるため、到達可能なピーク速度は次式です。
$$ v_{\mathrm{peak}}=\sqrt{a_{\max}L} $$
加速時間と総移動時間は次のとおりです。
$$ t_a=\sqrt{\frac{L}{a_{\max}}} $$
$$ T=2t_a=2\sqrt{\frac{L}{a_{\max}}} $$
同じ最高速度0.8 m/s、最大加速度1.5 m/s²で移動距離を0.2 mにすると、ピーク速度は約0.548 m/s、総移動時間は約0.730 sです。指令上限は0.8 m/sでも、短距離のため実際にはそこまで加速できません。
各時刻の位置・速度・加速度
台形プロファイルの各区間は、一定加速度運動の式で表せます。加速区間では次のとおりです。
$$ x(t)=\frac{1}{2}a_{\max}t^2,\qquad v(t)=a_{\max}t $$
等速区間では、加速終了位置を $L_a$ として次式になります。
$$ x(t)=L_a+v_{\max}(t-t_a),\qquad v(t)=v_{\max} $$
減速区間は、終了時刻からの残り時間を $\tau=T-t$ とすると簡潔に書けます。
$$ x(t)=L-\frac{1}{2}a_{\max}\tau^2,\qquad v(t)=a_{\max}\tau $$
三角形の場合も同じ式を使えますが、$v_{\max}$の代わりに計算した$v_{\mathrm{peak}}$を使い、等速時間を0にします。
Pythonでプロファイルを生成する
次のコードは、移動距離、最高速度、最大加速度から形状を自動判定し、位置・速度・加速度の時系列を生成します。
from dataclasses import dataclass
from math import sqrt
import numpy as np
@dataclass(frozen=True)
class MotionProfile:
profile_type: str
peak_velocity_m_s: float
acceleration_time_s: float
cruise_time_s: float
total_time_s: float
def calculate_profile(distance_m: float, max_velocity_m_s: float,
max_acceleration_m_s2: float) -> MotionProfile:
if distance_m <= 0 or max_velocity_m_s <= 0 or max_acceleration_m_s2 <= 0:
raise ValueError("all inputs must be positive")
limit_distance = max_velocity_m_s**2 / max_acceleration_m_s2
if distance_m >= limit_distance:
peak_velocity = max_velocity_m_s
acceleration_time = peak_velocity / max_acceleration_m_s2
cruise_time = (distance_m - limit_distance) / peak_velocity
profile_type = "trapezoidal"
else:
peak_velocity = sqrt(max_acceleration_m_s2 * distance_m)
acceleration_time = peak_velocity / max_acceleration_m_s2
cruise_time = 0.0
profile_type = "triangular"
return MotionProfile(
profile_type=profile_type,
peak_velocity_m_s=peak_velocity,
acceleration_time_s=acceleration_time,
cruise_time_s=cruise_time,
total_time_s=2 * acceleration_time + cruise_time,
)
def sample_profile(distance_m: float, max_acceleration_m_s2: float,
profile: MotionProfile, samples: int = 501):
t_a = profile.acceleration_time_s
t_c = profile.cruise_time_s
total = profile.total_time_s
peak = profile.peak_velocity_m_s
x_a = 0.5 * max_acceleration_m_s2 * t_a**2
time = np.linspace(0.0, total, samples)
position = np.zeros_like(time)
velocity = np.zeros_like(time)
acceleration = np.zeros_like(time)
for i, t in enumerate(time):
if t >= total:
position[i] = distance_m
elif t < t_a:
acceleration[i] = max_acceleration_m_s2
velocity[i] = max_acceleration_m_s2 * t
position[i] = 0.5 * max_acceleration_m_s2 * t**2
elif t < t_a + t_c:
velocity[i] = peak
position[i] = x_a + peak * (t - t_a)
else:
remaining = total - t
acceleration[i] = -max_acceleration_m_s2
velocity[i] = max_acceleration_m_s2 * remaining
position[i] = distance_m - 0.5 * max_acceleration_m_s2 * remaining**2
return time, position, velocity, acceleration
profile = calculate_profile(1.2, 0.8, 1.5)
print(profile)
time, position, velocity, acceleration = sample_profile(1.2, 1.5, profile)
np.savetxt(
"trapezoidal-motion-profile.csv",
np.column_stack([time, position, velocity, acceleration]),
delimiter=",",
header="time_s,position_m,velocity_m_s,acceleration_m_s2",
comments="",
)数値例ではprofile_typeがtrapezoidal、ピーク速度が0.8 m/s、総移動時間が約2.033 sになります。距離を0.2 mへ変更するとtriangularへ切り替わります。
機械設計へつなげる
台形速度プロファイルから最大加速度が決まれば、並進質量 $m$ に必要な慣性力は次式で見積もれます。
$$ F_a=ma_{\max} $$
この力をボールねじ、ラック＆ピニオン、ベルト、減速機などの伝達条件でモータートルクへ換算します。等速区間では加速力が0になりますが、摩擦、傾斜、加工反力などの定常負荷は残ります。
また、理想的な台形プロファイルは区間境界で加速度が瞬時に変化します。加速度の時間変化率であるjerkは理論上無限大になり、機械の振動や追従誤差を励起することがあります。高速位置決めや低剛性機構では、jerkを制限するS字プロファイルも比較します。
実務上の注意
- 開始速度と終了速度が0でない場合は、加速距離と減速距離を別々に計算する
- 正逆方向を扱うプログラムでは、距離の絶対値で時間を計算し、符号を出力へ戻す
- モーターの最高回転数だけでなく、加速中のピークトルクを確認する
- 指令周期に対して移動時間が短すぎないか確認する
- 機械の共振やバックラッシが大きい場合はS字加減速を検討する
- 複数軸では、各軸の終了時刻をそろえる同期計画が必要になる
- 計算上の位置・速度・加速度と、実機のエンコーダ値を区別して保存する
まとめ
台形速度プロファイルは、移動距離、最高速度、最大加速度の3値から作れます。ただし、最初に $L_{\mathrm{limit}}=v_{\max}^2/a_{\max}$ を計算し、最高速度へ到達できるか判定することが重要です。
長距離では加速・等速・減速の台形、短距離では加速・減速だけの三角形になります。位置、速度、加速度を同じ入力から時系列化しておけば、モータートルク計算、制御周期の検討、シミュレーション、Web計算ツールへ展開できます。
