제7부 실전: ROS2 Humble 프로젝트
7.2 실전 ① 속도·위치 PID
제7부 실전: ROS2 Humble 프로젝트 · 첫 프로젝트. 2.5·3.3의 PID를 ROS2 노드로 구현해 DC모터 속도를 폐루프 제어한다. 토픽으로 연결된 플랜트 노드 + 제어 노드를 실제로 실행해 목표 수렴·정상상태 오차 0·안티와인드업까지 검증한다. turtlesim 위치 제어로 확장.
✅ 검증됨: 이 장의 ROS2 폐루프 노드는
ros:humble컨테이너에서 실제 실행해 검증했다(verify/ce31_pid_node.py). 안티와인드업 비교(7.2.4)는u_max=4시나리오를 재현하는 별도 스크립트(verify/ce31_antiwindup_check.py)로, turtlesim 위치제어(7.2.6)는 노드·로직 검증(verify/ce31_turtle_pid.py)으로 확인했다. 아래 출력은 모두 실측값이다. turtlesim GUI 화면만 독자 환경 몫.
이 장을 마치면 ROS2 토픽으로 폐루프 제어 노드를 구성하고, 정상상태 오차가 적분 작용으로 0이 됨을, 포화 시 안티와인드업이 오버슈트를 줄임을 실행으로 확인해야 한다.
7.2.1 무엇을 만드나 — 토픽으로 닫는 루프
0.1.4의 블록선도를 두 노드로 나눈다:
- PlantNode: DC모터 1차 모델(1.1.7.1) $\tau\dot{\omega}+\omega=K_v$를 적분해 속도 $\omega$를
/motor_speed로 발행, 명령/motor_cmd를 구독. (실물에서는 이 자리에 모터 드라이버·ros2_control하드웨어.) - ControllerNode:
/motor_speed를 구독해 3.3.5 PID로 명령을 계산,/motor_cmd로 발행.
즉 3.3의 제어 루프가 토픽 통신으로 폐루프를 이룬다(7.1.7).
7.2.2 제어 노드 (3.3 PID + rclpy)
3.3.5의 PID 클래스를 그대로 쓰고, rclpy 노드에 통합한다. 참조를 따라가지 않아도 되도록 PID 클래스 본문을 그대로 옮긴다(3.3.5와 동일 — 측정미분+필터·백계산 안티와인드업·설정점가중을 갖춘 완성형):
class PID:
"""디지털 PID: 측정 미분 + 필터 + 백계산 안티와인드업 + 설정점 가중 (3.3.5)."""
def __init__(self, Kp, Ki, Kd, dt, u_min, u_max, N=10, b=1.0):
self.Kp, self.Ki, self.Kd, self.dt = Kp, Ki, Kd, dt
self.u_min, self.u_max = u_min, u_max
self.Tf = 1.0 / N # 미분 필터 시정수 (3.3.5: Tf=1/N)
self.Tt = (Kp / Ki) if Ki else dt # 백계산 추종 시정수≈Ti
self.b = b # 설정점 가중(P)
self.I = 0.0 # 적분기 상태
self.D = 0.0 # 필터된 미분 상태
self.y_prev = None
def update(self, r, y):
dt = self.dt
if self.y_prev is None:
self.y_prev = y
e = r - y
P = self.Kp * (self.b * r - y) # 설정점 가중 P
if self.Tf > 0.0:
self.D = (self.Tf * self.D - self.Kd * (y - self.y_prev)) / (self.Tf + dt)
else:
self.D = 0.0
u_unsat = P + self.I + self.D
u = max(self.u_min, min(self.u_max, u_unsat)) # 포화
self.I += self.Ki * e * dt + (u - u_unsat) * (dt / self.Tt) # 백계산 안티와인드업
self.y_prev = y
return u이제 rclpy 노드에 통합한다:
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
DT = 0.02 # 50 Hz
class ControllerNode(Node):
def __init__(self, setpoint=3.0):
super().__init__('controller')
self.r = setpoint
self.pid = PID(Kp=2.0, Ki=4.0, Kd=0.0, dt=DT, u_min=-10.0, u_max=10.0) # 위 PID
self.pub = self.create_publisher(Float64, 'motor_cmd', 10)
self.create_subscription(Float64, 'motor_speed', self.on_speed, 10)
def on_speed(self, msg):
y = msg.data # ① 측정 ω
u = self.pid.update(self.r, y) # ② PID
m = Float64(); m.data = u; self.pub.publish(m) # ③ 명령 발행(전체 노드·PlantNode(플랜트 모델)는 verify/ce31_pid_node.py 참조 — 위 코드와 동일하며, 컨테이너에서 실행해 검증했다.) 실제로는 Float64 대신 표준 메시지(geometry_msgs/Twist의 angular.z 등, 7.1.3)를 쓴다.
7.2.3 실행 결과 — 목표 수렴, 정상상태 오차 0 (실측)
플랜트·제어 노드를 한 프로세스에서 MultiThreadedExecutor로 5초 실행한 실제 출력:
t= 0.2s r=3.00 ω=1.697 u=4.303
t= 0.4s r=3.00 ω=2.385 u=3.615
t= 0.6s r=3.00 ω=2.733 u=3.267
t= 0.8s r=3.00 ω=2.884 u=3.116
t= 1.0s r=3.00 ω=2.950 u=3.050
t= 1.4s r=3.00 ω=2.990 u=3.010
t= 2.0s r=3.00 ω=2.999 u=3.001
t= 2.2s r=3.00 ω=3.000 u=3.000
⋮
FINAL: setpoint=3.0 measured=3.0000 err=0.0000 → CE31_PID_PASS읽기:
- 속도 $\omega$가 0 → 목표 3.0으로 매끄럽게 수렴, 약 2초에 정착(PI 튜닝).
- 정상상태 오차 = 0.0000: PI의 적분(I) 항이 오차를 정확히 제거했다 — 1.3.6(형 1)·2.5.2.2의 예측이 실제 ROS2 루프에서 확인됨.
- 명령 $u$가 초기 4.3에서 정상상태 3.0(=$r/K$)으로 수렴 — 정상상태에 필요한 입력을 적분기가 스스로 찾았다.
이것이 책 전체의 중간 결산이다: 0.1의 "측정→오차→보정" 직관이, 2.5의 PID 이론이, 3.3의 구현이, 7.1의 ROS2 구조가 한 폐루프에서 실제로 목표를 추종한다.
7.2.4 안티와인드업 — 포화에서의 차이 (실측)
2.5.6.3의 안티와인드업을 과도 구간만 포화하는 시나리오(u_max=4: 정상상태 $u=3$은 도달 가능하나 초기 요구 $u\approx6$은 포화)로 검증한다. verify/ce31_pid_node.py는 이 절의 시나리오와 무관하게 u_max=10(거의 포화 안 됨)·안티와인드업 토글도 없는 별개 스크립트라, 이 비교는 전용 스크립트 verify/ce31_antiwindup_check.py(같은 DC모터 플랜트·PID 게인, u_max=4로 좁히고 백계산 유/무를 토글)로 새로 만들어 확인했다:
anti_windup=False (naive) 오버슈트= 2.67% 피크시각= 1.08s 최종=3.0000
anti_windup=True (백계산) 오버슈트= -0.00% 피크시각= 8.34s 최종=3.0000- 둘 다 적분 작용으로 최종 3.0000에 도달하지만, 백계산 안티와인드업이 오버슈트(2.67%→−0.00%)를 없앤다. 포화로 잘린 양을 적분기가 추종해 누적 폭주를 막은 결과(2.5.6.3).
anti_windup=True쪽은 오버슈트 자체가 없어(단조 수렴) "피크"가 사실상 최종값 근처의 부동소수점 잡음이다. - 이 플랜트·튜닝에서는 차이가 작아 보이지만(완만한 포화), 더 느린 플랜트·긴 포화에서는 효과가 극적으로 커진다 — 직접
u_max·$K_i$를 바꿔 확인하라.
정직: 위 수치는 이 모델·튜닝 기준 실측이다(
ce-verify도커, numpy 2.2.6,verify/ce31_antiwindup_check.py직접 실행). 실물 모터는 마찰·지연·잡음으로 다르게 나오니(1.1.11·3.3.9), 여러분의 시스템에서 재튜닝·재검증하라.
7.2.5 속도에서 위치로 — 적분기가 하나 더
위치 제어는 속도 제어 위에 적분기가 하나 더 붙는다(1.1.7: $\Theta/V = \Omega/V \cdot1/s$). 두 방식:
- 단일 루프: 위치 오차에 PID 직접. 위치는 1형(적분기 내장)이라 P만으로도 계단 위치 오차 0(1.3.6).
- 종속(cascade) 루프: 바깥 위치 루프가 속도 목표를 만들고, 안쪽 속도 루프(7.2.3)가 그것을 추종. 산업 모션 제어의 표준 — 안쪽 루프가 빨라 외란·포화를 먼저 처리한다.
종속 구조는 "느린 바깥(위치) + 빠른 안쪽(속도)"으로 대역폭을 분리한다(2.3·2.6의 정신). 안쪽 루프를 충분히 빠르게 튜닝한 뒤 바깥을 설계한다. 심화: 피드포워드까지 포함한 제어 구조 전반은 7.7(제어 구조: 피드포워드·종속) 참고.
7.2.6 turtlesim으로 — 실제 GUI에서 (독자 환경)
turtlesim(0.3.6)은 위치 제어의 훌륭한 첫 실습이다. 거북이의 /turtle1/pose(turtlesim/msg/Pose: 위치·자세)를 구독해 목표 $(gx,gy)$까지 거리·방위 오차를 P 제어로 /turtle1/cmd_vel(geometry_msgs/Twist)에 반영해 보낸다. 7.2.2의 ControllerNode와 같은 골격(측정 콜백 안에서 오차 계산→제어→발행)이다:
import math
import rclpy
from rclpy.node import Node
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
class TurtleControllerNode(Node):
"""turtlesim 위치제어: /turtle1/pose 구독, 목표(gx,gy)까지 거리·방위 오차를
비례(P) 제어로 /turtle1/cmd_vel(Twist)로 내보낸다."""
def __init__(self, goal_x=8.0, goal_y=8.0):
super().__init__('turtle_pid')
self.gx, self.gy = goal_x, goal_y
self.Kp_lin = 1.0 # 거리 → 선속도 비례 이득
self.Kp_ang = 4.0 # 방위오차 → 각속도 비례 이득
self.max_lin = 2.0 # turtlesim 화면 한계 고려 속도 제한
self.tol = 0.05 # 도달 판정 허용오차 [m]
self.pub = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)
self.create_subscription(Pose, '/turtle1/pose', self.on_pose, 10)
def on_pose(self, msg: Pose):
ex = self.gx - msg.x
ey = self.gy - msg.y
dist = math.hypot(ex, ey) # ① 측정 → 거리오차
heading_err = math.atan2(ey, ex) - msg.theta # 목표 방위 − 현재 자세
heading_err = math.atan2(math.sin(heading_err), math.cos(heading_err)) # [-π,π] 정규화
cmd = Twist()
if dist < self.tol:
self.pub.publish(cmd) # ② 도달 → 정지 명령
self.get_logger().info(f'도달: dist={dist:.3f}')
return
cmd.linear.x = min(self.Kp_lin * dist, self.max_lin) # ② P 제어(선속도)
cmd.angular.z = self.Kp_ang * heading_err # ② P 제어(각속도)
self.pub.publish(cmd) # ③ 구동(명령 발행)
def main():
rclpy.init(); rclpy.spin(TurtleControllerNode(goal_x=8.0, goal_y=8.0)); rclpy.shutdown()방위오차를 각속도로, 거리를 선속도로 P 제어하는 단일 루프 위치 제어다(7.2.5) — 거리가 멀면 빨리, 목표를 등지고 있으면 먼저 실행된다(각속도 이득 Kp_ang이 더 크게 설정되어 방위부터 맞춘다).
# 터미널 1: 거북이 시뮬레이션 (GUI 필요)
ros2 run turtlesim turtlesim_node
# 터미널 2: 내 제어 노드
ros2 run ce_examples turtle_pid노드 생성·구독/발행·제어 로직은 검증했다(ros:humble-ros-base + ros-humble-turtlesim, turtlesim.msg·geometry_msgs.msg import 및 TurtleControllerNode 생성 확인). on_pose 콜백을 가짜 Pose로 직접 호출해 로직도 확인:
pose=(5.5, 5.5, θ=0.00) -> cmd_vel: linear.x=2.0000 angular.z=3.1416 (목표(8,8)까지 대각선, 최대속도+선회)
pose=(0.0, 0.0, θ=1.57) -> cmd_vel: linear.x=2.0000 angular.z=-3.1384 (등진 방향 → 큰 각속도로 방위부터 교정)
pose=(7.98, 7.98, θ=0.00) -> cmd_vel: linear.x=0.0000 angular.z=0.0000 (dist=0.028 < tol → 정지)⚠️ turtlesim_node는 Qt GUI라 headless 컨테이너에서는 화면이 표시되지 않는다(이 책의 자동 검증 밖, 7.1.8과 같은 한계). 위 확인은 노드 기동·통신 설정·제어 로직까지고, 거북이가 실제로 목표까지 매끄럽게 수렴하는 폐루프 거동은 검증 필요 — 데스크톱 ROS2 환경(0.3)에서 직접 실행해 확인하라. 게인(
Kp_lin, Kp_ang)은 예시값이라 진동·오버슈트가 보이면 2.5.4처럼 낮춰 튜닝하라.
7.2.7 정직 — 시뮬레이션에서 실물로
- 시뮬레이션 플랜트 = 모델: PlantNode는 모델(1차 DC모터)이다. 실물은 마찰·포화·지연·잡음·바이어스로 다르다(1.1.11) → 실물 튜닝이 필요.
- 타이밍: 컨테이너 검증은 단일 프로세스·완만한 부하다. 분산·실물에서는 지터·지연이 더해진다(3.3.9·7.1.4) — 빠른 루프는
ros2_control로. - 메시지 타입: 예제는
Float64로 단순화했다. 실전은 표준 메시지(Twist·JointState)와 QoS를 맞춰야(7.1.3·7.1.8).
7.2.8 정리 — 그리고 다음 장
- ROS2 토픽으로 플랜트 노드 + 제어 노드를 폐루프로 연결, 3.3 PID로 속도를 제어했다(7.2.1·7.2.2).
- 실측 검증: 속도가 목표 3.0에 수렴, 정상상태 오차 0(PI 적분 작용), 안티와인드업이 포화 오버슈트를 2.67%→−0.00%로 줄임 — 1.3·2.5의 이론이 실제 ROS2 루프에서 확인됨(7.2.3·7.2.4).
- 위치 제어는 적분기가 하나 더(단일/종속 루프), turtlesim으로 실습(7.2.5·7.2.6). 시뮬레이션→실물 격차는 정직하게(7.2.7).
다음 7.3에서는 이동로봇의 경로 추종(차동구동, Pure Pursuit + PID, Gazebo)으로 — 1차원 속도에서 2차원 주행으로 확장한다.
7.2.9 연습·생각해 볼 것
- 정상상태 오차. 7.2.3에서 $K_i=0$(P만)으로 두면 속도가 목표 3.0에 도달하나? 1.3.6으로 예측하고,
verify/ce31_pid_node.py에서 $K_i=0$으로 바꿔 확인하라. - 튜닝. $K_p$를 키우면 수렴이 빨라지나 무엇이 위험한가? $K_d$를 더하면? 직접 바꿔 응답을 관찰하라. (2.5.4)
- 안티와인드업. 7.2.4에서
u_max를 더 좁히거나(예 3.5) $K_i$를 키우면 안티와인드업 유무 차이가 어떻게 커지나? 왜? (2.5.6.3) - 종속 루프. 위치 제어를 단일 루프 대신 종속(위치→속도→PID)으로 작성하는 이점은? 안쪽 루프는 바깥보다 얼마나 빨라야 하나? (7.2.5)
- 메시지. 이 예제의
Float64를 실제 이동로봇용geometry_msgs/Twist로 바꾸면, 속도 명령은 어느 필드에 담나? (7.1.3) - (손으로)
verify/ce31_pid_node.py를 실행해(컨테이너 또는 0.3 환경) 수렴을 재현하고, 목표setpoint·$K_p,K_i,K_d$·u_max를 바꿔 가며 응답을 관찰하라. 가능하면 turtlesim으로 위치 제어까지. (검증된 스크립트)
막히면 "측정 토픽 구독 + PID + 명령 토픽 발행, 적분이 정상상태 오차를 제거하고, 안티와인드업이 포화를 막는다 — 실제로 확인됨"으로 돌아가라.