14강 PD Controller, Feedback Linearization
Spring - Mass - Damper System
시스템의 Euler Lagrange 방정식을 푼 결과였던 운동방정식(EOM)은 다음과 같은 형태로 간소화할 수 있었습니다.
- 보통 시스템의 모션을 얻기 위해 $\ddot{q}$ term이 필요하게 됩니다. 이는 운동방정식을 $Ax=b$ 꼴로 변형하여 구해낼 수 있게 되며, 이때의 $A, M(q)$ 행렬은 $\ddot{q}$ 에 대한 자코비안을 계산함으로 손쉽게 얻을 수 있습니다.
- 외력이 없을 때, 이 운동 방정식은 상계수 미분방정식이 되어 아래와 같은 고유 진동수를 갖는 해를 도출할 수 있습니다. 오랜 시간이 흘러 ( $t\rightarrow \infty$ ) 시스템이 정상 상태(steady state)에 도달했을 때 Spring - Mass - Damper 시스템은 고유 진동수를 가진 움직임을 가질 것입니다.
- 더불어, 정상상태를 기준으로 시스템의 수렴, 진동, 발산 여부도 확인할 수 있습니다. 자세한 내용은 기계진동학과 관련이 있으며 이번 강의에서는 결과에 대해서만 확인하고 넘어가겠습니다.
각 상황에 따라 time-state의 변화를 그래프로 그려보면 다음과 같습니다.
- under-damped - red
- overdamped - green
- critically damped - blue
⇒ 우리의 목적은, (controller의 목적은) 적절한 외력, control force를 가해 $\xi$ =1을 만족시키는 것이지요!
PD controller
시스템에 가해주는 힘(혹은 토크), tau를 아래와 같이 선정해보겠습니다.
💡 참고로 $q$와 $\dot{q}$은 모터에 장착된 센서로 알아낼 수 있다고 가정합니다.
- 이렇게 모델링한 힘을 운동방정식에 넣어, critically damped 조건을 적용하면 아래와 같아집니다.
- critically damped 조건을 위한 Kp, Kd에 대해 관계식을 얻어낼 수 있는 것이지요.
💡 sqrt에 따라 +, - term이 등장했는데 둘 중 어떤 것을 골라야 할까요? ⇒ -c term을 보상하는 + term을 선택해야 합니다.
⇒ Kp가 정해지면, 그에 따른 Kd는 위 수식에 따라 정해지게 되지만, Kp를 설정하는 것은 오로지 사용자에게 달리게 됩니다. 따라서 실험을 통해 적절한 Kp를 구하는 기준이 필요합니다.
코드 구현
앞서 살펴보았던 spring-mass-damper system을 시뮬레이션 해보고, PD controller도 적용해봅시다.
시간에 대한 질점의 위치를 확인할 수 있습니다. ( 앞선 그림을 상기해보면 under-damped 상태라는 것을 확인할 수 있지요!)
💡 적절한 제어를 통해 critical damp point에 도달한 모습을 볼 수 있습니다.
지금의 예시를 다시 한 번 정의하고 코드 분석을 해봅시다.
우리의 목표는 0.5에 위치한 질점에 적절한 제어 입력을 가해 위치 0으로 빠르게 도달하도록 하는 것입니다. 이를 위해 PD controller를 사용하고자 하며, Kp, Kd 매개변수를 설정해야 합니다.
실험해보기
Parameters의 Kp를 100으로 키운 뒤 다시 코드를 실행해봅시다. position 0로 훨씬 빠르게 수렴하는 모습을 확인할 수 있습니다.
⇒ 이처럼 Kp가 커지면 더욱 빠르게 수렴하지만, 이것이 마냥 좋지 많은 않습니다. Kp가 커질수록 시스템의 노이즈에 더욱 취약해진다는 단점이 있답니다.
2D case
이번에는 차원을 확장하여 2개의 모터를 제어하는 시스템을 생각해보겠습니다. 각 모터에 대한 운동 방정식 2개가 주어짐에 따라 Kp, Kd가 2-by-2 행렬로 확장이 됩니다.
이렇게 되면, 미지수는 8개가 된 반면, 주어진 조건은 여전히 2개가 되어버리면서 올바른 우리가 임의로 설정해야 하는 값들이 너무 많아진다는 문제가 발생합니다.
⇒ 따라서 이를 해결하는 새로운 방식으로 Feedback linearization, 혹은 Control partition 이라는 방식을 배워보겠습니다.
Feedback linearization
Feedback linearization의 기본 개념은 다음과 같습니다.
우리는 현재 시스템의 운동 방정식을 알고 있기 때문에, $q, \dot{q}$ 을 측정할 수 있다면 로봇이 어떠한 상태에 처해 있는지 추측할 수 있습니다.
⇒ 따라서 8개의 Kp, Kd 변수들을 직접 최적화하지 않아도 현재 상태를 기반으로(Feedback) 제어기를 훨씬 간소화할 수 있게 됩니다.
- 위 그림을 간단히 설명하면, 원하는 reference point가 존재할 시, 운동 방정식에 기반하여 이를 위한 reference torque $\tau_{ref}$를 계산하는 과정입니다.
- 단, 이 모든 것이 동작하기 위한 전제조건은, 운동 방정식을 구성하는 $M, C, G$와 $q, \dot{q}$를 정확하게 알고 있어야 한다는 것인데요. 실제 로봇 시스템에서는 각종 외란과 센서 노이즈, 제어 노이즈로 인해 정확한 모델링이 사실상 불가합니다. 따라서 이러한 불확실성(uncertainty)을 고려한 항을 $\hat{M}, \hat{C}, \hat{G}$ 이라고 지칭하겠습니다.
구현 측면에서 Feedback linearization은 다음과 같습니다.
- uncertainty를 고려한 $\hat{M}, \hat{C}, \hat{G}$ 을 통해 tau를 선정합니다.
- 이렇게 되면, 양변의 C, G term이 상쇄되면서 M term만 남게 되지요. 결국, M C G와 같은 물성치에 대한 항 없이, 순수 state만의 미분 방정식이 남게 됩니다.
- M term에 대해 정리한 수식에 이전 정상 상태의 조건을 적용하면, kp와 kd 사이의 조건식을 다시 얻어낼 수 있게 됩니다.
- Feedback Linearization의 가장 큰 전제 조건이자 특징은 시스템의 M, C, G와 매우 근사한 $\hat{M}, \hat{C}, \hat{G}$ 를 사용하여 제어입력을 선정한다는 것입니다. 이 부분에서 너무 큰 노이즈가 포함되거나, 모종의 이유로 근사하지 못한 수치를 사용하면 Feedback Linearization은 정상적으로 동작하지 못합니다.
⇒ Feedback Linearization을 통해 q1: 0.5 / q2: 1.0 위치에 빠르게 도달하는 모습을 확인할 수 있습니다.
- 이번에는 코드의 Parameter 부분에서
self.Kp를 좀 더 작은 값으로 변경해보겠습니다.
# self.Kp = 100 * np.identity(2) self.Kp = 10 * np.identity(2) # self.Kp = 1 * np.identity(2) self.Kd = 2 * np.sqrt(self.Kp)
⇒ 확실히 이전 보다 좀 더 느리게 목표 지점에 도달하는 것을 확인할 수 있지요.
- 이번에는 hat 행렬들에 적용되는 Random Noise를 조절해보겠습니다.
self.uncertainty = 5.0 self.M_hat = self.M + self.uncertainty * np.random.rand(2, 2) self.C_hat = self.C + self.uncertainty * np.random.rand(2, 2) self.G_hat = self.G + self.uncertainty * np.random.rand(2, 2)
⇒ 실제 모터 제어를 시도하면 아마 위와 같이 over/under damping이 발생할 것입니다.