모션제어를 위한 Python으로 구현한 S-Curve 가감속 패턴 생성 알고리즘

S-Curve 가감속 알고리즘은 사다리꼴 가감속에서의 갑작스러운 가속/감속 변화(급격한 jerk)를 줄이고, 모터나 시스템이 더 부드럽게 동작하도록 설계된 방식입니다. S-Curve는 가속도(a)가 선형적으로 변화하여 가속도와 감속의 경계에서 스무스한 움직임을 제공합니다.

---

알고리즘 설명

S-Curve는 가속 구간과 감속 구간에서 가속도의 선형 변화를 포함하며, 정속 구간은 속도가 일정합니다.

1. 주요 변수

• jerk: 가속도의 변화율 ($da/dt$).
• acceleration: 가속도 ($a$).
• velocity: 속도 ($v$).
• distance: 총 이동 거리 ($d$).

2. 구간 나누기

S-Curve는 아래와 같은 7단계로 나뉩니다:

1. 가속 증가 구간 (jerk > 0).
2. 가속 유지 구간 (a = constant).
3. 가속 감소 구간 (jerk < 0).
4. 정속 구간 (v = constant).
5. 감속 증가 구간 (jerk < 0).
6. 감속 유지 구간 (a = constant).
7. 감속 감소 구간 (jerk > 0).

---

Python 코드

아래는 S-Curve 가감속 패턴을 생성하는 Python 코드입니다.

import matplotlib.pyplot as plt
import numpy as np

def s_curve_profile(jerk, max_acceleration, max_velocity, distance, dt=0.01):
    """
    S-Curve 가감속 패턴 생성
    :param jerk: 가속도의 변화율 (m/s^3)
    :param max_acceleration: 최대 가속도 (m/s^2)
    :param max_velocity: 최대 속도 (m/s)
    :param distance: 총 이동 거리 (m)
    :param dt: 시간 간격 (s)
    :return: 시간, 속도, 위치 리스트
    """
    # 가속 구간에서의 거리 및 시간 계산
    accel_time = max_acceleration / jerk  # 가속 증가 시간
    accel_distance = 0.5 * jerk * accel_time**3  # 가속 증가 거리
    cruise_distance = distance - 2 * accel_distance  # 정속 거리
    
    if cruise_distance < 0:
        raise ValueError("거리 값이 너무 작아 정속 구간이 없습니다.")
    
    cruise_time = cruise_distance / max_velocity  # 정속 구간 시간
    total_time = 2 * accel_time + cruise_time  # 총 시간 계산
    
    # 시간, 속도, 위치 초기화
    time = []
    velocity = []
    position = []
    
    current_time = 0
    current_velocity = 0
    current_position = 0
    current_acceleration = 0

    # 가속 증가 구간
    while current_time < accel_time:
        current_acceleration = jerk * current_time
        current_velocity += current_acceleration * dt
        current_position += current_velocity * dt
        time.append(current_time)
        velocity.append(current_velocity)
        position.append(current_position)
        current_time += dt

    # 가속 유지 구간
    while current_velocity < max_velocity:
        current_velocity += max_acceleration * dt
        current_position += current_velocity * dt
        time.append(current_time)
        velocity.append(current_velocity)
        position.append(current_position)
        current_time += dt

    # 정속 구간
    while current_time < accel_time + cruise_time:
        current_position += max_velocity * dt
        time.append(current_time)
        velocity.append(max_velocity)
        position.append(current_position)
        current_time += dt

    # 감속 증가 구간
    while current_velocity > 0:
        current_acceleration -= max_acceleration * dt
        current_velocity += current_acceleration * dt
        current_position += current_velocity * dt
        time.append(current_time)
        velocity.append(current_velocity)
        position.append(current_position)
        current_time += dt

    return time, velocity, position

# 테스트
jerk = 2.0  # m/s^3
max_acceleration = 2.0  # m/s^2
max_velocity = 5.0  # m/s
distance = 50.0  # m

time, velocity, position = s_curve_profile(jerk, max_acceleration, max_velocity, distance)

# 결과 시각화
plt.figure(figsize=(10, 6))

plt.subplot(2, 1, 1)
plt.plot(time, velocity, label="Velocity")
plt.title("S-Curve Velocity Profile")
plt.xlabel("Time (s)")
plt.ylabel("Velocity (m/s)")
plt.grid(True)
plt.legend()

plt.subplot(2, 1, 2)
plt.plot(time, position, label="Position", color="orange")
plt.title("S-Curve Position Profile")
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
plt.grid(True)
plt.legend()

plt.tight_layout()
plt.show()

---

코드 설명

1. accel_time 및 accel_distance:
   • 최대 가속도에 도달하는 데 걸리는 시간과 이동 거리를 계산합니다.
2. cruise_distance:
   • 총 이동 거리에서 가속/감속 구간 거리를 뺀 정속 구간 거리를 계산합니다.
3. 구간별 반복:
   • S-Curve의 7단계를 나눠 각 구간에서 속도와 위치를 계산.
4. 시각화:
   • 속도 프로파일과 위치 프로파일을 그래프로 시각화하여 결과 확인.

---

결과 예시

• 속도 프로파일: S자 형태의 부드러운 곡선을 따름.
• 위치 프로파일: 부드럽게 증가하며 최종 목표 위치에 도달.

---

응용

• 로봇 공학에서 다축 동작의 부드러운 제어.
• 모터 드라이버에서 고정밀 위치 및 속도 제어.
• 물류 시스템에서 컨베이어 벨트 및 리프트 동작 제어.

---

추가적으로, 모터 또는 시스템 특성에 맞춰 jerk, max_acceleration, max_velocity 등을 조정하여 다양한 동작 시뮬레이션이 가능합니다.