제어공학.jpg

가. 비례제어

import matplotlib.pyplot as plt
import numpy as np

# 파라미터
Kp = 2.0
desired_distance = 1.0
max_speed = 5.0
dt = 0.1
total_time = 20

# 초기 위치 (작업자와 로봇)
worker_pos = 0.0   # 작업자 위치 (m)
robot_pos = -1.0   # 로봇 초기 위치 (m) → 작업자보다 1m 뒤에서 시작

worker_positions = [worker_pos]
robot_positions = [robot_pos]
distances = [worker_pos - robot_pos]
times = [0]

# 시뮬레이션
for t in np.arange(dt, total_time+dt, dt):
    # 작업자 움직임 정의 (예: t<10s → 0.2m/s로 이동, 이후 멈춤)
    if t < 10:
        worker_pos += 0.2 * dt
    else:
        worker_pos += 0.0
    
    # 현재 거리
    distance = worker_pos - robot_pos
    
    # 제어 입력 (속도)
    error = desired_distance - distance
    speed = Kp * error
    
    # 속도 제한
    speed = max(min(speed, max_speed), -max_speed)
    
    # 로봇 위치 갱신
    robot_pos += speed * dt
    
    # 기록
    worker_positions.append(worker_pos)
    robot_positions.append(robot_pos)
    distances.append(distance)
    times.append(t)

# 그래프 출력
plt.figure(figsize=(10,6))
plt.plot(times, worker_positions, label="Worker Position", linewidth=2)
plt.plot(times, robot_positions, label="Robot Position", linewidth=2)
plt.axhline(y=0, color='k', linestyle=':')
plt.title("Worker Following Simulation (P-Control)")
plt.xlabel("Time (s)")
plt.ylabel("Position (m)")
plt.legend()
plt.grid(True)
plt.show()

plt.figure(figsize=(10,4))
plt.plot(times, distances, label="Distance to Worker", linewidth=2)
plt.axhline(y=desired_distance, color='r', linestyle='--', label="Desired Distance (1m)")
plt.title("Distance Between Worker and Robot")
plt.xlabel("Time (s)")
plt.ylabel("Distance (m)")
plt.legend()
plt.grid(True)
plt.show()

나. PID 제어

import time

class RobotController:
    def __init__(self):
        # PID 제어기 파라미터 (Kp, Ki, Kd 값은 환경에 맞게 조정 필요)
        self.Kp = 2.0   # 비례 게인
        self.Ki = 0.5   # 적분 게인
        self.Kd = 0.1   # 미분 게인

        # PID 변수
        self.prev_error = 0.0   # 이전 오차
        self.integral = 0.0     # 누적 오차

        # 목표 거리 (1m 유지)
        self.setpoint = 1.0

        # 최고 속도 제한 (m/s)
        self.max_speed = 1.6

    # -------------------------
    # 1. 로봇 플랫폼 목표 각도 계산
    # -------------------------
    def calculate_target_angle(self, bboxes):
        x_center = (bboxes[0] + bboxes[2]) / 2
        y_center = (bboxes[1] + bboxes[3]) / 2

        x_deviation_pixels = IMAGE_WIDTH / 2 - x_center
        y_deviation_pixels = IMAGE_HEIGHT / 2 - y_center

        steer_target_angle = x_deviation_pixels * HORIZONTAL_ANGLE_PER_PIXEL
        vt_angle = y_deviation_pixels * VERTICAL_ANGLE_PER_PIXEL

        if -2 <= steer_target_angle <= 2:
            target_angle = 0
            Vertical_angle = round((vt_angle * -1), 1)
        else:
            target_angle = round(steer_target_angle, 1)
            Vertical_angle = round((vt_angle * -1), 1)

        return target_angle, Vertical_angle

    # -------------------------
    # 2. PID 제어 출력 = 속도 명령
    # -------------------------
    def calculate_speed_pid(self, distance, dt=0.1):
        # 오차 = 목표거리 - 실제거리
        error = self.setpoint - distance

        # 적분항
        self.integral += error * dt

        # 미분항
        derivative = (error - self.prev_error) / dt

        # PID 제어 출력 (output은 RPM) 
        output = (self.Kp * error) + (self.Ki * self.integral) + (self.Kd * derivative)

        # 속도 제한 (-1.6 ~ 1.6 m/s)
        output = max(min(output, self.max_speed), -self.max_speed)

        # 이전 오차 갱신
        self.prev_error = error

        # PID 출력값을 그대로 로봇 속도 명령으로 반환
        return round(output, 2)

다. PID 제어

image.png

image.png

<aside>

Weak: Kp=1, Ki=0.1, Kd=0.05

Baseline: Kp=2, Ki=0.5, Kd=0.1

Aggressive: Kp=4, Ki=1.0, Kd=0.2

라. 비례제어 vs PID 제어

image.png

위치 추적 그래프