제7부 실전: ROS2 Humble 프로젝트

7.4 실전 ③ 도립진자 균형 (LQR)

제7부 실전: ROS2 Humble 프로젝트 · 세 번째 프로젝트. 불안정 플랜트의 정수 — 도립진자(cart-pole)를 LQR(4.5)·상태피드백(4.3)으로 세운다. 1.1.9에서 "도립=불안정(우반평면 극)"이라 반드시 피드백이 필요하다 했던 그 시스템을, 드디어 제어한다. 비선형 시뮬레이션에서 균형을 검증.

✅ 검증됨: 카트-폴 비선형 동역학 + LQR을 ros:humble 컨테이너에서 실행해 검증(verify/ce33_cartpole.py) — 개루프 우반평면 극 +4.852(불안정), LQR로 폐루프 안정, θ가 0.2 rad 섭동에서 직립(0.0004 rad)으로 복귀. Gazebo 물리 시뮬레이션은 독자 환경 몫.

이 장을 마치면 불안정 플랜트를 선형화·LQR로 안정화하고, 그것이 비선형 실제에서 균형을 잡음을 확인하며, 도립진자가 밸런싱 로봇과 같은 문제임을 이해해야 한다.

7.4.1 왜 도립진자 — 불안정의 교과서

도립진자(막대를 거꾸로 세움)는 제어의 대표적 벤치마크다. 이유:

• 본질적으로 불안정: 제어 입력이 없으면 넘어진다 — 피드백 없이는 못 세운다(1.1.9.2). 개루프 안정 시스템과 차원이 다른 문제.
• 과소구동(underactuated): 카트를 미는 힘 하나로 카트 위치 + 막대 각도 둘을 동시에 다뤄야 한다.
• 밸런싱 로봇·세그웨이·로켓 착륙과 같은 문제 — 도립진자를 세우면 그것들을 세우는 법을 아는 것.

1.1.8·1.1.9에서 진자의 위 평형이 우반평면 극을 만든다고 했다. 이제 그 극을 LQR로 좌반평면으로 옮긴다.

7.4.2 카트-폴 모델

카트(질량 $M$) 위에 막대(끝 질량 $m$, 길이 $l$)가 핀으로 달려 있고, 카트에 힘 $F$를 준다. 상태 $[x, \dot{x}, \theta, \dot{\theta}]$($\theta$=연직 위로부터 막대 각).

부호 규약(그림): $\theta$는 연직 위 기준으로 잰다, $F$는 카트에 $+x$ 방향으로 가하는 제어 입력, $mg$는 막대 끝 질량에 아래로 작용하는 중력이다. 막대가 조금이라도 기울면($\theta>0$) 중력의 접선 성분이 그 기울기를 더 키우는 방향으로 작용한다 — 이것이 아래 선형화의 (4,3) 원소가 양(+)이 되는 이유이자, 개루프가 불안정한 이유다(1.1.9).

비선형 동역학:

$$\begin{aligned} \ddot{x} &= [F + m l \dot{\theta}^2 \sin\theta - m g \sin\theta\cos\theta] / (M + m \sin^2\theta) \\ \ddot{\theta} &= [-F \cos\theta - m l \dot{\theta}^2 \sin\theta\cos\theta + (M+m) g \sin\theta] / [l (M + m \sin^2\theta)] \end{aligned}$$

직립($\theta=0$) 근처 선형화(1.1.9, $\sin\theta\approx\theta, \cos\theta\approx1, \dot{\theta}^2\approx0$):

$$A = \begin{bmatrix} 0 & 1 & 0 & 0 \\ 0 & 0 & -mg/M & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & (M+m)g/lM & 0 \end{bmatrix}, \quad B = \begin{bmatrix} 0 \\ 1/M \\ 0 \\ -1/lM \end{bmatrix}$$

핵심: $A$의 (4,3) 원소 $(M+m)g/lM > 0$(양의 복원항 반대 = 불안정). $M=1, m=0.2, l=0.5$면 개루프 고유값이 0, 0, +4.852, −4.852 — +4.852가 우반평면! 검증으로 확인된 1.1.9의 그 불안정이다.

7.4.3 LQR 설계

가제어이므로(연습) 상태피드백 F=−Kx로 극을 좌반평면에 옮긴다. 극을 손으로 고르기는 까다로우니(4차·과소구동) LQR(4.5)로:

import control as ct, numpy as np
M,m,l,g = 1.0,0.2,0.5,9.81
A = np.array([[0,1,0,0],[0,0,-m*g/M,0],[0,0,0,1],[0,0,(M+m)*g/(l*M),0]])
B = np.array([[0],[1/M],[0],[-1/(l*M)]])
Q = np.diag([1.0, 1.0, 10.0, 1.0])      # 각(θ)에 큰 가중 → 넘어지지 않게
R = np.array([[0.1]])
K,_,E = ct.lqr(A,B,Q,R)

검증된 결과:

개루프 고유값: [0, 0, +4.852, −4.852]        ← 불안정(우반평면 +4.852)
LQR K = [−3.16, −5.75, −47.58, −10.52]
폐루프 고유값: [−9.00, −3.49, −1.40±0.07j]   ← 모두 좌반평면(안정!)

• $Q$의 $\theta$ 가중(10)이 크면 막대를 우선 세우고, $R$이 작으면 힘을 억제하지 않는다(4.5.4).
• $K$의 부호: 막대가 앞으로 기울면($\theta>0$) 카트를 그쪽으로 밀어($F$) 막대 하단을 지지한다 — 직관과 맞다.

7.4.4 비선형 시뮬레이션 — 실제로 균형을 유지하나 (실측)

선형 LQR 게인을 비선형 카트-폴(7.4.2)에 적용해, 0.2 rad(≈11°) 기울인 초기 상태에서 6초 시뮬레이션:

def deriv(s, F):
    x,xd,th,thd = s; sin,cos = np.sin(th), np.cos(th); den = M + m*sin*sin
    xdd  = (F + m*l*thd*thd*sin - m*g*sin*cos)/den
    thdd = (-F*cos - m*l*thd*thd*sin*cos + (M+m)*g*sin)/(l*den)
    return np.array([xd, xdd, thd, thdd])

s = np.array([0.,0.,0.2,0.]); dt = 0.005
for _ in range(1200):
    F = float((-K @ s)[0])          # 상태피드백
    s = s + dt*deriv(s, F)          # 오일러 적분

실측 결과:

6초 후:  x = 0.002 m   θ = 0.0004 rad   θ̇ = −0.0004     (최대각 0.200, 안 커짐)
→ CE33_BALANCE_PASS

• 막대가 0.2 rad에서 직립(0.0004 rad)으로 복귀, 카트도 원점 부근에 머문다. 불안정 플랜트가 LQR로 안정화됐다.
• 비선형 시뮬레이션에서 균형이 유지된다는 것은 선형화 부호가 옳다는 자기검증이다(틀리면 발산해 넘어진다).
• 1.1.9에서 "도립은 피드백 없이는 못 선다"고 한 그 시스템이, 4.3·4.5의 도구로 실제로 서 있다. 이론에서 균형까지 한 단계를 완결한다.

7.4.5 밸런싱 로봇 = 같은 문제

두 바퀴 밸런싱 로봇(세그웨이류)은 카트-폴을 세운 것이다 — 바퀴 토크가 힘 $F$, 차체 기울기가 $\theta$. 같은 LQR 구조로 선다. 차이는:

• 상태 추정 필요: 차체 기울기 $\theta$·각속도 $\dot{\theta}$를 IMU로 추정(5.2·5.4, 상보/칼만 필터). 직접 못 재는 상태는 관측기·필터로(4.4).
• 모터 동특성: 힘 명령이 바퀴 모터 속도 루프(7.2)를 거친다 — 종속 구조.

그래서 실전 밸런싱 로봇 = LQR(이 장) + IMU 추정(5.4) + 모터 속도 PID(7.2). 이 책의 여러 부가 한 로봇에서 만난다. 심화: 이 종속 구조 자체(느린 바깥·빠른 안쪽의 원리)는 7.7(제어 구조: 피드포워드·종속) 참고.

7.4.6 ROS2로 — Gazebo + ros2_control (독자 환경)

실전 통합:

• Gazebo에 카트-폴/밸런싱 로봇 모델(URDF)을 스폰해 물리 시뮬레이션.
• ros2_control(7.1.5): effort 인터페이스로 힘/토크 명령. 커스텀 컨트롤러 플러그인 또는 노드에서 F=−Kx 계산.
• 상태는 /joint_states·/imu에서, 명령은 effort 인터페이스로.

# (예시 흐름 — GUI·물리 시뮬레이션, 독자 환경)
ros2 launch <cartpole_pkg> gazebo.launch.py
ros2 run ce_examples lqr_balance        # F = -K x 계산해 effort 발행

⚠️ Gazebo 3D 물리는 headless 컨테이너 자동 검증 밖(이 책은 LQR 제어 로직·균형을 비선형 시뮬레이션으로 검증, 3D 물리·시각은 독자 환경).

7.4.7 정직 — 선형화 범위·스윙업·추정

• 선형화는 직립 근처만(1.1.9.3): LQR은 작은 각(≈±15~20°)에서 균형을 유지하지만, 크게 넘어진 상태(누운 막대)는 못 세운다.
• 스윙업(swing-up)은 별개 문제: 누운 막대를 흔들어 올리는 것은 비선형 제어(에너지 기반·6.2)다 — 올린 뒤 LQR로 전환(게인 스케줄링, 6.2.6)한다.
• 상태 추정이 현실의 절반: 실물은 $\theta,\dot{\theta}$를 잡음 속에서 추정해야(5.2·5.4) — 추정 오차·지연이 균형을 교란한다.
• 시뮬레이션→실물: 마찰·지연·바퀴 미끄러짐으로 실물은 더 까다롭다(1.1.11·3.3.9).

7.4.8 정리 — 그리고 다음 장

• 도립진자는 본질적으로 불안정·과소구동한 벤치마크. 개루프 우반평면 극 +4.852(1.1.9 확인)를 LQR/상태피드백으로 좌반평면에 옮겨 안정화(7.4.2·7.4.3).
• 실측: 비선형 시뮬레이션에서 0.2 rad 섭동→직립 복귀(균형 성공). 비선형에서 서는 것이 선형화·부호의 자기검증(7.4.4).
• 밸런싱 로봇은 같은 문제 = LQR + IMU 추정(5.4) + 모터 PID(7.2) — 책의 여러 부가 한 로봇에서 만남(7.4.5).
• 선형화 범위·스윙업·추정은 정직하게(7.4.7).

다음 7.5에서는 추정으로 돌아가 EKF 센서 융합(오도메트리+IMU, robot_localization)을 다룬다 — 5.2·5.3·5.4를 ROS2 실전으로.

7.4.9 연습·생각해 볼 것

1. 불안정 확인. verify/ce33_cartpole.py에서 $K=0$(제어 없음)으로 두면 막대가 어떻게 되나? 개루프 고유값 +4.852와 연결해 설명하라. (7.4.2)
2. Q 튜닝. $Q$의 $\theta$ 가중(10)을 1로 줄이면 균형이 어떻게 변하나? $x$ 가중을 키우면? 직접 바꿔 관찰하라(카트가 원점에 더 붙나, 막대가 더 흔들리나). (7.4.3, 4.5.4)
3. 선형화 한계. 초기 각을 0.2 → 0.5 → 1.0 rad로 키우면 어디서 LQR이 균형을 유지하지 못하나? 왜 그런가? (7.4.7)
4. 밸런싱 로봇. 밸런싱 로봇이 카트-폴과 같은 문제인 이유를, 힘 $F$·각 $\theta$의 대응으로 설명하라. 실물에서 $\theta$를 어떻게 얻나? (7.4.5)
5. 스윙업. 누운 막대를 세우는 스윙업이 왜 LQR로 안 되고 별도 비선형 제어가 필요한가? 올린 뒤에는 어떻게 LQR로 넘기나? (7.4.7, 6.2)
6. (손으로) verify/ce33_cartpole.py로 균형을 재현하고, $Q,R$·초기각·질량을 바꿔 가며 한계를 탐색하라. 가능하면 Gazebo 카트-폴로 물리 시뮬레이션까지. (검증된 스크립트)

막히면 "도립=불안정(우반평면 극), LQR이 극을 좌반평면으로, 비선형에서 서면 부호 맞음, 실전은 +IMU추정+모터PID"로 돌아가라.