Linear Control이 동작하기 위한 조건은 controllability, observability도 있지만 시스템이 Linear 해야 한다는 대전제가 필요합니다. 그럼 sin, cos, 제곱 등의 비선형 함수로 모델링된 nonlinear system에서는 Linear Control을 적용하지 못하는 걸까요? 이러한 non-linear 시스템을 다루는 방법에 대해 알아봅시다.

Nonlinear Control의 쟁점은 non-linear system이 존재할 때, Taylor Series를 통해 이를 선형 근사화시키는 것입니다.

💡그런데… “Taylor Series를 통해 선형 근사화”라는 말이 무엇일까요? sin(x) 함수를 통해 한번 알아보겠습니다.

• sin(x)의 Taylor Series는 아래와 같습니다. (세상 모든 함수는 Taylor Series를 통해 표현할 수 있지요.)
• 이와 같이 비선형 함수인 sin(x)함수이지만, “특정 지점”에 대해선 선형 함수로 표현할 수 있습니다. 이 지점을 operating opin라고 하며, 여기에서 주의해야 할 점으로 선형화한 함수는 “operating point 근처에서만” 원본 비선형 함수와 유사하다는 것입니다.

$$ sin(x) = x - \frac{x^3}{3!} + \frac{x^5}{5!} - \frac{x^7}{7!} + \cdots $$

• 예를 들어, x = 0이라는 operating point에서의 선형화한 함수는 아래와 같이 y = x 입니다. (사실상 세제곱, 다섯제곱 항도 추가해야 하지만 이는 너무 작은 값이기 때문에 계산의 효율을 위해서 제외시킵니다.)

• 하지만, operating point 0을 벗어날수록 이는 sin(x)에서 벗어난 값을 갖습니다.

• 이번에는 operating point $2\pi$ 에서의 선형화를 구해봅시다.

operating point에서의 선형화를 통해 비션형 함수을 단순하게 만들 수 있지만 operating point가 달라지면, 그에 따른 선형화 함수도 달라진다는 점을 기억합시다!

이 개념을 탑재한 상태에서, 이제 시스템의 state와 control signal x,u를 operating point와 미소한 derivative term으로 나누어 생각해보겠습니다.

💡 derivative term이 추가된 까닭은, 앞선 예제와 같이 선형화한 함수가 operating point를 벗어나면 큰 오차를 갖기 때문으로, 매우 작은 범위 내에서 변화를 갖는다는 뜻이 되겠습니다.

• 시스템의 output 또한 operating point와 derivative term으로 나눌 수 있고, linearize를 위해 system equation $F(x,u)$를 Taylor Series로 전개할 수 있습니다. 계산의 효율을 위해 우리는 이 중 1계 미분 항만을 사용해보겠습니다.

• 시스템의 정의에 의해 소거되는 항을 정리한 뒤, 이제 우리는 derivative term du에 대해 LQR, Pole Placement를 적용할 것입니다.

• Differential Drive Car를 예시로 들어 위 작업을 적용해보겠습니다.

• 앞서 control signal u가 operating point + derivative term이라고 언급하고 바로 수식으로 넘어갔는데요. 이에 대해서 추가 설명을 덧붙이고자 합니다.
• operating point u0는 trajectory optimization에 의해 계산된 control signal이며, du는 noise등에 의해 실제 원하는 목표치로부터 차이를 갖는 오차 항이라고 생각할 수 있습니다. non-linear system에서는 linear라고 간주함으로 인해 발생하는 오차 항이라고 할 수 있겠지요.
• 우리의 목표는 오차 항에 대해서 Control Method를 적용함으로 기존 operating point에 도달하는 제어가 되겠습니다.

💡 일전 feedback linearization과 현재의 Linear control은 다른 방식의 제어입니다. 복습 차원에서 비교해보고 넘어가봅시다.

이번에는 non-linear system인 Manipulator system에 linearization을 적용해보겠습니다.

• manipulator의 일반적인 운동방정식을 아래와 같이 정의해보겠습니다. 여기서 $B(q)$란 control signal과 system사이의 상관관계를 표현하는 행렬이 됩니다.

이제 비선형의 State Martix를 vector x에 대해 미분하여 선형화할 것입니다. 복잡한 식의 미분이 이루어지는 만큼 차근차근 살펴봅시다!

• A matrix는 state $q, \dot{q}$에 대해 미분을 진행하면 됩니다. 이들을 각각 f1, f2라고 부르겠습니다. state $\dot{q}$는 운동방정식의 결과를 대입하게 될 것입니다.

• $f_1$에 대한 미분은 손쉽게 구할 수 있습니다. ( $\frac{\partial f_2}{\partial q}, \frac{\partial f_2}{\partial \dot{q}}$ 는 복잡하기 때문에 차근차근 계산해보도록 합시다.)
• $\frac{\partial f_2}{\partial q}$ : operating point에서 $\ddot{q}$는 0에 가깝다고 간주하면 첫번째 항은 0이 되며, 두번째 항에서 $B(q)$는 사실상 상수이기 때문에 소거됩니다.
• $\frac{\partial f_2}{\partial \dot{q}}$ : 마찬가지로 operating point에서 $\dot{q}$는 0에 가깝다고 간주하고, 상수항 B(q)가 소거됩니다.

• 따라서, A matrix는 다음과 같이 정리됩니다.

• B matrix는 $f_1,f_2$를 control vector u에 대해 미분하면 되는데요, 현재 u항을 가진 term이 하나뿐이므로 간단히 계산됩니다.

이러한 계산을 왜 했는지 다잡아보고, 코드 구현을 시작하겠습니다.

1. double pendulum은 Non-linear dynamics를 갖습니다.
2. Non-linear dynamics에 linear control method를 적용하기 위해 선형화를 해주어야 합니다.
3. 비선형 함수의 선형화를 위해 state equation들을 state vector로 1계 미분해주었습니다.
4. 선형화시킨 새로운 A,B를 사용하여 제어하는 것은 control u에 대해 LQR등의 제어 기법을 적용하는 것이 아닌, derivative term $\delta u$에 대해 제어 기법을 적용하는 것이 됩니다.

코드 구현

이번 예시의 코드는 sympy를 통해 대수 계산을 하는 코드와 시뮬레이션 main 코드 두 파트로 나뉘어 있습니다.

1. sw_twolink_eom.py
   1. double pendulum의 운동방정식을 계산하고, 선형화 시 필요한 복잡한 미분 연산 결과들을 미리 구해둡니다.
2. Main
   1. sympy 코드에서 구한 수식에 실제 물성치를 대입하여 선형화된 시스템 관계식을 도출합니다.
   2. 선형화된 matrix A_lin, B_lin에 LQR을 적용하고 gain K를 구합니다.
   3. Gain K와 현재 state x를 통해 control signal, u = -kx를 계산하고 이를 통해 안정적인 제어를 수행합니다.

sympy Part

이번 시간 사용할 모델은 inverted double pendulum으로 pendulum이 완전 곤두선 상태가 state (0, 0)이 됩니다. 따라서 Homogeneous transform에서도 pi/2 만큼의 offset을 갖게 됩니다.

• 선형화한 matrix A는 아래와 같은 형태를 가지므로, $G, C$에 대해 $q, \dot{q}$ 에 대한 미분값이 필요합니다.

• 현재 $G, C$는 2x1 vector이며, $q, \dot{q}$ 또한 2x1 vector입니다. (vector를 vector로 미분하기 위해 Jacobian을 사용하였습니다.)

이제 sympy part에서 계산된 결과를 복사하여 simulation part에 붙여넣고, 실제 물성치를 대입하여 제어 시뮬레이션을 구현해봅시다.

Simulation Part

• linearization, odeint 계산 시 운동 방정식이 반복 사용되기 때문에 별도의 함수를 구현해 두었습니다.

• linearize 함수는 선형화된 A_lin과 B_lin을 구하는 것이 핵심이며, 일전 계산한 미분 결과를 numpy를 통해 구현합니다.

• linearize를 통해 계산한 A_lin, B_lin을 통해 main 함수에서 LQR을 적용하고, Gain K를 구합니다.

• for-loop를 순회하면서 simulation 결과를 수집하고 animation을 그리는 절차는 이전과 동일합니다.

• 코드를 실행하면, double pendulum이 곤두서기 위해 제어되는 모습과 제어 결과를 그래프로 확인할 수 있습니다.

14강부터, 이번 18강 강의까지 “로봇의 제어”라는 측면에서 여러 기법들과 장단점들을 살펴보았습니다. 이 여러가지 방법들이 여러분들의 머릿속을 혼란스럽게 하고 있지 않나 싶어 이 시점에서 지금까지 배운 것들의 정리를 한차례 진행하겠습니다.

• 우리의 로봇을 $\dot{x} = Ax + Bu$의 Linear system을 표현하는 방법을 알아보았고, Pole Placement와 LQR에 대해서도 배워보았습니다.
• Linear system이 아닌 Non-linear의 경우도 오늘 강의에서 다루어 보았지요. 하지만 이 기법은 operating point 주변에서는 잘 동작하지만, 모든 영역의 수렴성을 보장해주지는 않습니다. 이것이 Feedback linearization을 도입하게 되는 계기가 되지요.
• 더불어 PD Controller와 수렴성에 대해서도 살펴보았고, 모터가 추가될수록 튜닝해야 할 매개변수가 많아진다는 단점, 그래서 Feedback linearization을 도입하는 부분까지 살펴보았습니다.

이번 강의에서 배운 제어 기법들은 기초적이지만 실무에서 아직까지 사용되고 있을 정도로 강력합니다. MPC, 강화학습 등 현 시점 트렌디한 기술들이 있지만, 결국 기초를 잘 쌓고 결과에 대해 해석할 수 있는 힘을 길러두는 것이 중요하다는 제 개인적인 의견을 전달드리면서 이번 강의를 마쳐보겠습니다.

Exercises

• 초기 시작점 z0을 바꿔가면서 시뮬레이션을 해보세요.

• Q, R Matrix를 수정하여 LQR 튜닝을 시도해보세요. ex) 2초 내 제어 종료, joint1의 max torque를 20이하로 튜닝 등

• 각종 노이즈를 추가해보세요.

현재 코드에는 control, measurement noise를 추가해 두었습니다. Parameter Class를 수정하여 여러 노이즈를 추가해보고, 어떤 노이즈가 더 치명적인지, 각종 실험을 해보시기 바랍니다.