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

C 언어로 S-Curve 가감속 알고리즘을 구현한 코드입니다. S-Curve는 사다리꼴(Trapezoidal) 가감속 알고리즘의 확장으로, **가속도 변화율(Jerk)**을 조정하여 부드러운 가속과 감속을 제공합니다. 이는 급격한 변화로 인한 진동을 줄이고 시스템의 안정성을 높이는 데 유용합니다.

---

알고리즘 설명

1. 주요 변수

• Jerk ($da/dt$): 가속도 변화율.
• Acceleration ($a$): 가속도.
• Velocity ($v$): 속도.
• Distance ($d$): 총 이동 거리.

2. 구간 나누기

S-Curve는 아래와 같은 7단계로 구성됩니다:

1. 가속 증가 (Jerk > 0).
2. 가속 유지 (Acceleration = 최대 가속도).
3. 가속 감소 (Jerk < 0).
4. 정속 이동 (Velocity = 최대 속도).
5. 감속 증가 (Jerk < 0).
6. 감속 유지 (Acceleration = 최대 감속도).
7. 감속 감소 (Jerk > 0).

---

C 코드

#include <stdio.h>
#include <math.h>

// S-Curve 프로파일 생성 함수
void s_curve_profile(float jerk, float max_acceleration, float max_velocity, float distance, float dt) {
    // 가속 증가 구간 시간과 거리 계산
    float accel_increase_time = max_acceleration / jerk;  // 가속 증가 시간
    float accel_increase_distance = (1.0 / 6.0) * jerk * pow(accel_increase_time, 3);  // 가속 증가 거리

    // 가속 유지 구간 거리 계산
    float accel_steady_distance = 0.5 * max_acceleration * (max_velocity / max_acceleration);  // 최대 가속 유지 거리

    // 정속 구간 거리 계산
    float total_accel_distance = 2 * accel_increase_distance + accel_steady_distance;  // 전체 가속 거리
    float cruise_distance = distance - 2 * total_accel_distance;  // 정속 구간 거리

    if (cruise_distance < 0) {
        printf("S-Curve 패턴 생성 불가: 정속 구간 없음.\n");
        return;
    }

    float cruise_time = cruise_distance / max_velocity;  // 정속 구간 시간
    float total_time = 2 * accel_increase_time + cruise_time;  // 전체 시간 계산

    // 출력 변수 초기화
    float time = 0.0;
    float velocity = 0.0;
    float position = 0.0;
    float acceleration = 0.0;

    printf("Time\tVelocity\tPosition\tAcceleration\n");

    // 가속 증가 구간
    while (time < accel_increase_time) {
        acceleration = jerk * time;
        velocity += acceleration * dt;
        position += velocity * dt;
        printf("%.2f\t%.2f\t\t%.2f\t\t%.2f\n", time, velocity, position, acceleration);
        time += dt;
    }

    // 가속 유지 구간
    while (velocity < max_velocity) {
        acceleration = max_acceleration;
        velocity += acceleration * dt;
        position += velocity * dt;
        printf("%.2f\t%.2f\t\t%.2f\t\t%.2f\n", time, velocity, position, acceleration);
        time += dt;
    }

    // 정속 구간
    while (position < (distance - total_accel_distance)) {
        acceleration = 0.0;
        velocity = max_velocity;
        position += velocity * dt;
        printf("%.2f\t%.2f\t\t%.2f\t\t%.2f\n", time, velocity, position, acceleration);
        time += dt;
    }

    // 감속 증가 구간
    while (velocity > 0.0) {
        acceleration -= jerk * dt;
        velocity += acceleration * dt;
        position += velocity * dt;
        printf("%.2f\t%.2f\t\t%.2f\t\t%.2f\n", time, velocity, position, acceleration);
        time += dt;
    }
}

int main() {
    // S-Curve 프로파일 파라미터
    float jerk = 2.0;              // m/s^3 (가속도 변화율)
    float max_acceleration = 2.0;  // m/s^2
    float max_velocity = 5.0;      // m/s
    float distance = 50.0;         // m
    float dt = 0.1;                // 시간 간격 (s)

    s_curve_profile(jerk, max_acceleration, max_velocity, distance, dt);

    return 0;
}

---

코드 설명

1. S-Curve 프로파일 계산:
   • accel_increase_time과 accel_increase_distance를 통해 가속 증가 구간의 시간을 계산합니다.
   • 가속, 정속, 감속 구간의 거리를 계산하여 전체 패턴을 구분합니다.
2. 구간별 동작:
   • 가속 증가: Jerk를 기반으로 가속도를 증가시키며 속도와 위치를 계산.
   • 가속 유지: 최대 가속도를 유지하며 속도와 위치를 업데이트.
   • 정속 구간: 최대 속도로 일정하게 이동.
   • 감속 증가: Jerk를 기반으로 가속도를 감소시키며 속도와 위치를 계산.
3. 출력 형식:
   • 각 시간 단계에서 시간(Time), 속도(Velocity), 위치(Position), 가속도(Acceleration)를 출력합니다.

---

출력 예시

입력 파라미터:

• Jerk = 2.0 $m/s^3$
• 최대 가속도 = 2.0 $m/s^2$
• 최대 속도 = 5.0 $m/s$
• 총 거리 = 50.0 $m$

출력 결과 (일부 예):

Time	Velocity	Position	Acceleration
0.00	0.00		0.00		0.00
0.10	0.02		0.00		0.20
0.20	0.08		0.01		0.40
0.30	0.18		0.03		0.60
0.40	0.32		0.06		0.80
...
3.00	5.00		40.00		0.00
...
5.40	2.80		48.12		-0.80
5.50	2.40		48.36		-1.00
...

---

코드의 특징

1. 정확성:
   • S-Curve의 모든 구간을 정밀하게 계산하여 부드러운 동작을 제공합니다.
2. 유연성:
   • Jerk, 최대 가속도, 최대 속도, 이동 거리 등 다양한 입력 파라미터에 따라 프로파일을 생성합니다.
3. 확장성:
   • 추가로 실시간 모터 제어, 다축 시스템 제어 등에 통합 가능.

---

응용

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

---

필요에 따라 코드 최적화나 특정 애플리케이션에 맞는 확장 기능을 추가할 수 있습니다.