[로봇공학기초 / 2DOF] #9. 로봇 제어 Part 2 – PD 제어 + 중력 보상

✍️ 서문

이전 글에서는 PID 제어를 통해 기본적인 피드백 제어를 구현해보았습니다.
하지만 실제 로봇팔은 자체 무게와 중력의 영향을 크게 받기 때문에, 단순 PID로는 정확한 제어가 어려운 경우가 많습니다.

그래서 이번 글에서는 PD 제어에 **중력 보상(Gravity Compensation)**을 더해
조금 더 현실적인 제어기를 만들어 보겠습니다.

---

⚖️ 중력 보상이란?

로봇팔은 관절의 각도에 따라 중력 방향으로 떨어지려는 토크가 생깁니다.
이 토크를 미리 계산해서 제어기에 넣어주는 것이 중력 보상입니다.

즉,

τ=τPD+τgravity\tau = \tau_{PD} + \tau_{\text{gravity}}τ=τPD​+τgravity​

또는

τ=G(θ)+Kp(θd−θ)+Kd(θ˙d−θ˙)\tau = G(\theta) + K_p(\theta_d - \theta) + K_d(\dot{\theta}_d - \dot{\theta})τ=G(θ)+Kp​(θd​−θ)+Kd​(θ˙d​−θ˙)

---

🧠 중력항 G(θ)G(\theta)G(θ)

라그랑지안에서 도출한 중력항은 다음과 같습니다:

G(θ)=[(m1+m2)gL1cos⁡θ1+m2gL2cos⁡(θ1+θ2)m2gL2cos⁡(θ1+θ2)]G(\theta) = \begin{bmatrix} (m_1 + m_2)gL_1\cos\theta_1 + m_2gL_2\cos(\theta_1 + \theta_2) \\ m_2gL_2\cos(\theta_1 + \theta_2) \end{bmatrix}G(θ)=[(m1​+m2​)gL1​cosθ1​+m2​gL2​cos(θ1​+θ2​)m2​gL2​cos(θ1​+θ2​)​]

보다 정밀하게 고려하면, 각 링크의 무게중심 위치까지 고려해야 하므로 다음과 같이 표현됩니다:

V=V1+V2V1=m1gr1sin⁡θ1V2=m2g(L1sin⁡θ1+r2sin⁡(θ1+θ2))V=m1gr1sin⁡θ1+m2g(L1sin⁡θ1+r2sin⁡(θ1+θ2))\begin{aligned} V &= V_1 + V_2 \\ V_1 &= m_1gr_1\sin\theta_1 \\ V_2 &= m_2g(L_1\sin\theta_1 + r_2\sin(\theta_1 + \theta_2)) \\ V &= m_1gr_1\sin\theta_1 + m_2g(L_1\sin\theta_1 + r_2\sin(\theta_1 + \theta_2)) \end{aligned}VV1​V2​V​=V1​+V2​=m1​gr1​sinθ1​=m2​g(L1​sinθ1​+r2​sin(θ1​+θ2​))=m1​gr1​sinθ1​+m2​g(L1​sinθ1​+r2​sin(θ1​+θ2​))​

이를 편미분하여 얻은 중력항은:

∂V∂θ1=(m1r1+m2L1)gcos⁡θ1+m2r2gcos⁡(θ1+θ2)∂V∂θ2=m2r2gcos⁡(θ1+θ2)\begin{aligned} \frac{\partial V}{\partial \theta_1} &= (m_1r_1 + m_2L_1)g\cos\theta_1 + m_2r_2g\cos(\theta_1 + \theta_2) \\ \frac{\partial V}{\partial \theta_2} &= m_2r_2g\cos(\theta_1 + \theta_2) \end{aligned}∂θ1​∂V​∂θ2​∂V​​=(m1​r1​+m2​L1​)gcosθ1​+m2​r2​gcos(θ1​+θ2​)=m2​r2​gcos(θ1​+θ2​)​

최종 중력항:

G(θ)=[(m1r1+m2L1)gcos⁡θ1+m2r2gcos⁡(θ1+θ2)m2r2gcos⁡(θ1+θ2)]G(\theta) = \begin{bmatrix} (m_1r_1 + m_2L_1)g\cos\theta_1 + m_2r_2g\cos(\theta_1 + \theta_2) \\ m_2r_2g\cos(\theta_1 + \theta_2) \end{bmatrix}G(θ)=[(m1​r1​+m2​L1​)gcosθ1​+m2​r2​gcos(θ1​+θ2​)m2​r2​gcos(θ1​+θ2​)​]

---

🧮 제어기 구성 (PD + 중력보상)

• 오차 정의:

e=θref−θ,e˙=θ˙ref−θ˙e = \theta_{\text{ref}} - \theta,\quad \dot{e} = \dot{\theta}_{\text{ref}} - \dot{\theta}e=θref​−θ,e˙=θ˙ref​−θ˙

• 제어식:

τ=[Kp100Kp2]e+[Kd100Kd2]e˙+G(θ)\tau = \begin{bmatrix} K_{p1} & 0 \\ 0 & K_{p2} \end{bmatrix} e + \begin{bmatrix} K_{d1} & 0 \\ 0 & K_{d2} \end{bmatrix} \dot{e} + G(\theta)τ=[Kp1​0​0Kp2​​]e+[Kd1​0​0Kd2​​]e˙+G(θ)

---

💻 구현 흐름

1. 목표각 설정:
   θref=[θ1,ref,θ2,ref]\theta_{\text{ref}} = [\theta_{1,\text{ref}}, \theta_{2,\text{ref}}]θref​=[θ1,ref​,θ2,ref​]
2. 현재각 θ\thetaθ 및 속도 θ˙\dot{\theta}θ˙ 측정
3. 오차 계산:
   e=θref−θ,e˙=θ˙ref−θ˙e = \theta_{\text{ref}} - \theta,\quad \dot{e} = \dot{\theta}_{\text{ref}} - \dot{\theta}e=θref​−θ,e˙=θ˙ref​−θ˙
4. PD 항 계산
5. 중력항 G(θ)G(\theta)G(θ) 계산
6. 최종 토크 τ\tauτ 계산

---

🎯 효과 비교

제어 방식결과 요약

P 제어만 사용	중력에 못 이기고 버벅이거나 처짐 발생
PD 제어만 사용	목표는 도달하지만 흔들림, 부정확함 존재
PD + 중력보상 사용	자연스럽고 정확하게 도달, 실용적 제어 가능

---

📌 말단 위치 좌표

x1=L1cos⁡θ1y1=L1sin⁡θ1x2=L1cos⁡θ1+L2cos⁡(θ1+θ2)y2=L1sin⁡θ1+L2sin⁡(θ1+θ2)\begin{aligned} x_1 &= L_1\cos\theta_1 \\ y_1 &= L_1\sin\theta_1 \\ x_2 &= L_1\cos\theta_1 + L_2\cos(\theta_1 + \theta_2) \\ y_2 &= L_1\sin\theta_1 + L_2\sin(\theta_1 + \theta_2) \end{aligned}x1​y1​x2​y2​​=L1​cosθ1​=L1​sinθ1​=L1​cosθ1​+L2​cos(θ1​+θ2​)=L1​sinθ1​+L2​sin(θ1​+θ2​)​

---

🧩 정리 한 마디

중력 보상은 로봇팔 제어에서 필수에 가까운 테크닉입니다.
단순한 PD 제어에 한 줄만 더해도 정확도와 안정성이 급격히 향상됩니다.

---

🔜 다음 편 예고

[2DOF 시리즈] #10. Computed Torque Control – 모델로 예측하고 제어하자!
중력, 관성, 관절 속도까지 한 번에 고려하는 로봇 제어의 꽃 🌸

 

 

🎯 PD 제어 + 중력 보상 (Gravity Compensation)

2자유도 로봇팔에 중력 보정을 더한 PD 제어기를 적용해보자.

---

📌 말단 위치 좌표

x1=L1cos⁡θ1y1=L1sin⁡θ1x2=L1cos⁡θ1+L2cos⁡(θ1+θ2)y2=L1sin⁡θ1+L2sin⁡(θ1+θ2)\begin{aligned} x_1 &= L_1 \cos\theta_1 \\ y_1 &= L_1 \sin\theta_1 \\ x_2 &= L_1 \cos\theta_1 + L_2 \cos(\theta_1 + \theta_2) \\ y_2 &= L_1 \sin\theta_1 + L_2 \sin(\theta_1 + \theta_2) \end{aligned}x1​y1​x2​y2​​=L1​cosθ1​=L1​sinθ1​=L1​cosθ1​+L2​cos(θ1​+θ2​)=L1​sinθ1​+L2​sin(θ1​+θ2​)​

---

🔧 제어기 구조

τPD=Kp(θd−θ)+Kd(θ˙d−θ˙)\tau_{PD} = K_p(\theta_d - \theta) + K_d(\dot{\theta}_d - \dot{\theta})τPD​=Kp​(θd​−θ)+Kd​(θ˙d​−θ˙) τ=G(θ)+Kp(θd−θ)+Kd(θ˙d−θ˙)\tau = G(\theta) + K_p(\theta_d - \theta) + K_d(\dot{\theta}_d - \dot{\theta})τ=G(θ)+Kp​(θd​−θ)+Kd​(θ˙d​−θ˙)

---

⚖️ 중력 포텐셜 에너지

V=mghV = mghV=mgh V1=m1gh1=m1gr1sin⁡θ1V2=m2gh2=m2g(L1sin⁡θ1+r2sin⁡(θ1+θ2))V=V1+V2=m1gr1sin⁡θ1+m2g(L1sin⁡θ1+r2sin⁡(θ1+θ2))\begin{aligned} V_1 &= m_1 g h_1 = m_1 g r_1 \sin\theta_1 \\ V_2 &= m_2 g h_2 = m_2 g \left(L_1 \sin\theta_1 + r_2 \sin(\theta_1 + \theta_2)\right) \\ V &= V_1 + V_2 = m_1 g r_1 \sin\theta_1 + m_2 g \left(L_1 \sin\theta_1 + r_2 \sin(\theta_1 + \theta_2)\right) \end{aligned}V1​V2​V​=m1​gh1​=m1​gr1​sinθ1​=m2​gh2​=m2​g(L1​sinθ1​+r2​sin(θ1​+θ2​))=V1​+V2​=m1​gr1​sinθ1​+m2​g(L1​sinθ1​+r2​sin(θ1​+θ2​))​

---

⚙️ 중력항 G(θ)G(\theta)G(θ)

Gi(θ)=∂V∂θiG_i(\theta) = \frac{\partial V}{\partial \theta_i}Gi​(θ)=∂θi​∂V​ ∂V∂θ1=(m1r1+m2L1)gcos⁡θ1+m2r2gcos⁡(θ1+θ2)∂V∂θ2=m2r2gcos⁡(θ1+θ2)\begin{aligned} \frac{\partial V}{\partial \theta_1} &= (m_1 r_1 + m_2 L_1)g \cos\theta_1 + m_2 r_2 g \cos(\theta_1 + \theta_2) \\ \frac{\partial V}{\partial \theta_2} &= m_2 r_2 g \cos(\theta_1 + \theta_2) \end{aligned}∂θ1​∂V​∂θ2​∂V​​=(m1​r1​+m2​L1​)gcosθ1​+m2​r2​gcos(θ1​+θ2​)=m2​r2​gcos(θ1​+θ2​)​ G(θ)=[G1(θ)G2(θ)]=[(m1r1+m2L1)gcos⁡θ1+m2r2gcos⁡(θ1+θ2)m2r2gcos⁡(θ1+θ2)]G(\theta) = \begin{bmatrix} G_1(\theta) \\ G_2(\theta) \end{bmatrix} = \begin{bmatrix} (m_1 r_1 + m_2 L_1)g \cos\theta_1 + m_2 r_2 g \cos(\theta_1 + \theta_2) \\ m_2 r_2 g \cos(\theta_1 + \theta_2) \end{bmatrix}G(θ)=[G1​(θ)G2​(θ)​]=[(m1​r1​+m2​L1​)gcosθ1​+m2​r2​gcos(θ1​+θ2​)m2​r2​gcos(θ1​+θ2​)​]