이번 시간에는 System Dynamics에 기반한 제어 방식인 Pole Placement, LQR (linear quadratic regulator)에 대해 학습해보고자 합니다. 그 전에, 약간의 이론을 설명하면서 강의를 시작해보겠습니다.

💡 이번 강의는 MATLAB TECH TALKS의 영상에서 많은 부분을 차용하였습니다.

• Introduction to State-Space Equations | State Space, Part 1

State-Space Modeling

지금부터 물체의 운동을 State-Space Model이라는 방식으로 새롭게 정의할 것인데요. 시스템의 dynamics를 State-Space Model로 표현하게 되면 아래와 같이 여러 이점들을 갖습니다.

• 복잡한 시스템도 정해진 형태로 표현할 수 있습니다.
• 행렬을 사용하기 때문에 수치적 해를 구할 시 용이하고 시스템에 대한 직관적 이해가 가능합니다.
• 더불어, 현대적인 제어 방식들을 적용하기에도 용이합니다.

State-Space Model은 시스템을 1계 미분 방정식으로 나타내게 됩니다. 일반적인 운동방정식은 위치, 속도, 가속도를 표현하는 2계 미분 시스템인데요. 여기서 차원을 하나 줄여 1계 미분으로 나타낼 수 있는 것이지요. 어떻게 이런 작업이 가능한지 예시를 통해 살펴봅시다.

system equation의 $x,\dot{x},\ddot{x}$ 를 $x_1, x_2, x_3$ 라는 state variable로 정의하고, 마치 다항식을 행렬 형태로 변환하듯 이들 사이의 관계를 행렬 형태로 나타냅니다. 이것이 state equation입니다.

다음으로, output equation이라는 것을 설정하며, 이는 오로지 설계자에게 달린 영역입니다. 센서의 output으로 position을 측정할 수도 있고, velocity를 측정할 수도, position과 velocity를 측정할 수도 있듯이, 사용하는 환경과 사용자의 설계와 관련된 방정식입니다.

• state equation과 output equation을 통해 다시 정리한 현 상황은 아래와 같습니다.

현재 state와 다음 state 사이의 관계를 나타내는 state equation, 현재 state에서 output을 도출하는 output equation에 control signal를 적용해보겠습니다.

• control signal과 state variable 사이의 관계식들을 행렬 형태로 정리하면 $Ax + Bu$, $Cx + Du$의 linear system으로 정리 가능합니다.
• state에 control signal $u$를 적용함에 따라 다음 state가 어떠한 변화를 갖게 되는지 1계 미분 방정식 형태로 표현할 수 있는 것이지요.
  

💡 한걸음 더 나아가자면, 이는 시스템의 에너지가 어떻게 변화하는지를 의미합니다.

연습삼아 spring-mass-damper system을 State-Space Model로 표현해봅시다.

운동에너지와 위치에너지를 구하고 euler-lagrange method를 적용하는 부분은 일전과 동일합니다. 모든 운동방정식을 구한 뒤, 이를 하나의 State-Space Model로 표현하는 과정이 추가된 것이지요.

제어를 하는 입장에서, 우리의 목표는 control signal $u$의 조작을 통해 다음 state $x$를 우리가 원하는 지점으로 수렴시키는 것이 됩니다.

💡 그렇다면 어떤 방향으로 얼마만큼 control signal을 변경해야 할까요?

Pole Placement

State Space Model $\dot{x} = Ax$ 에서 행렬 A에 대한 eigen value decomposition을 적용하면, 시스템의 일반 해를 얻을 수 있습니다. 이는 control이 전혀 없을 때, state model A에 대한 해석이라고 볼 수 있습니다.

• 예시를 통해 살펴봅시다. 0, 1, 2, -1의 행렬로 표현되는 시스템에, eigenvalue decomposition을 적용한 모습입니다.

💡 이 내용이 무슨 소리인지 모르겠다면, 공학 대학에서 배우는 공학 수학 중 “상계수 제차 미분 방정식”을 살펴보세요!

계산한 시스템의 행렬 A의 eigenvalue는 실수부와 허수부로 이루어져 있으며(지금은 실수부만 있지요), 이들이 어떤 값을 갖는지에 따라 시스템의 stability를 판단할 수 있습니다.

• 허수부가 없고, 모든 실수부가 양수 ⇒ 발산
• 허수부가 없고, 모든 실수부가 음수 ⇒ 수렴
• 허수부가 있음 ⇒ 허수부는 cos, sin으로 이루어지는 항이며 기본적으로 진동의 특성을 갖습니다. 실수부가 어떠한 값을 갖는냐에 따라 수렴, 혹은 발산하게 됩니다.

• control u를 통해 시스템에 외력 가해줌으로 행렬 A의 eigenvalue를 조작할 수 있습니다.

아래와 같이 기존 행렬 A가 $(A-Bk)$로 변경되는 것이지요! 이는 곧 시스템의 stability를 변경시키게 됩니다. eigenvalue가 바뀌게 되기 때문이지요. 따라서 pole placement라는 이름이 붙게 된 것입니다.

간단한 예시를 통해 pole placement에 대해 좀 더 직관적인 이해를 가져보겠습니다.

$k_1,k_2$를 통해 행렬 A의 eigenvalue를 조작할 수 있으며, eigenvalue가 허수부가 없이 음수만 존재하도록 $k_1,k_2$를 설정하는 것이지요.

💡 직접 손으로 이 작업을 계산해보면 아마 모두 이해하실 수 있으실 겁니다.

pole placement의 흐름도에서 input에 곱해지는 $k_r$은 원하는 값에 정확하게 도달하게 하기 위한 scaling factor인데요, 원하는 steady state에 도달하기 위한 gain이 됩니다.

$\dot{x} = Ax$를 갖는 우리의 시스템에서 매우 많은 시간이 흐른다면, 특정 값에 수렴하는 정상 상태에 도달할 것입니다. 이때의 수렴하는 값이 바로 아래 그림에서 $y$가 의미하는 바 인데요, 예를 들어, 수렴하기 원하는 값이 1인데, $y=2$의 상황을 갖는다면, scaling factor 0.5를 곱해주어야 할 것입니다.

⇒ 따라서 $k_r$은 steady state 상태에서의 y값의 역수로 설정합니다.

• PID와 Pole Placement의 차이에 대해 다시 한 번 정리해보겠습니다.

Controllability와 Observability

Pole Placement와 같은 제어 로직을 적용하기 전, 반드시 보장되어야 하는 특성 두가지가 존재합니다. 예시를 통해 살펴봅시다.

• 2차원 평면에서 움직이는 자동차를 제어하는 예시입니다. 만약 자동차의 상태를 전혀 알 수 없다면 (C Matrix가 영행렬이라면) 자동차의 제어는 할 수 있지만 현재 상황에 대한 피드백을 받을 수 없으므로 정확한 제어가 힘들 것입니다.

• 반대로 미끄러운 얼음판 위에 존재하는 것 같이 자동차의 제어가 불가능한 상황이라면 (B 행렬이 영행렬이라면) 자동차의 위치는 알 수 있을지라도 정확한 제어 자체가 불가할 것입니다.

이렇게 제어 로직을 적용하기 전, 시스템의 “Controllability”와 “Observability”를 우선 판단하여 의미있는 제어가 가능한지의 여부를 알아내야 합니다.

그럼 시스템이 제어 가능한지, 관측 가능한지의 여부를 어떻게 알 수 있을까요??

state-space model을 구성하는 A, B, C, D 행렬을 통해 Controllability와 Observability를 판단할 수 있습니다. 자세한 수학적 유도는 이 강의에서 진행하지 않겠지만, 우리는 python의 control 모듈을 사용하여 이를 판단할 수 있습니다.

코드 구현

앞서 state-space 학습을 진행하며 spring-mass system을 분석한 바 있습니다. 해당 행렬을 그대로 구현해보고, controllability와 observability 여부를 알아봅시다.

• EigenValue를 구하고, system이 수렴하도록 하는 pole을 계산하는 과정을 직접 구현할 수도 있지만, 이번 실습을 위해 control이라는 별도의 python 패키지를 사용해보고자 합니다. 예시 실행 전 pip install control을 통해 설치해줍니다.

import control

• numpy를 통해 state space를 구성하는 A,B C,D matrix를 구현합니다. (output은 pose를 사용한다고 가정하겠습니다.)

• 이제 이 행렬들과 numpy를 통해 여러 유의미한 정보들을 추출해 보겠습니다. 우선 eigenvalue를 계산하여 pole의 여부를 확인할 수 있습니다.

• control.ctrb와 control.obsv를 사용하여 Controllability / Observability Matrix를 계산할 수 있습니다. 해당 행렬들의 rank가 시스템의 자유도(state vector의 크기)와 같다면 Fully Controllable / Observable하다는 의미가 됩니다.

현재 코드에서 결과는 4*4 matrix인 A에서, controllability와 observability matrix가 dimension 4를 가지므로 Full Rank를 만족합니다. eigenvalue는 실수부와 복소수부를 모두 갖고 있습니다.

• 이번에는 pole placement를 구현해보았습니다. control.place을 통해 A,B matrix와 pole을 전달해주면, 해당 pole로 조정해주는 gain K를 얻을 수 있게 됩니다.

LQR

일전 pole placement를 학습하면서 혹시 이런 생각을 해보지 않으셨나요?

• pole을 적절한 위치로 이동시키는 것이 pole placement인데, 그럼 나의 system에서 최적의 pole 위치는 어디지?

linear quadratic regulator, LQR은 우리에게 이러한 걱정을 덜어주는 optimal control 방식입니다. Cost Function을 세우고, 제어기의 퍼포먼스를 튜닝한 뒤 원하는 상황에 맞는 최적의 Gain을 알려줍니다.

LQR과 Pole Placement의 output은 기본적으로 u = -kx라는 동일한 구조를 갖습니다. 하지만, LQR은 최적의 해를 위해 cost function이라는 것을 사용하는데요. 예시를 통해 살펴봅시다.

집에서 학교까지 이동할 때 가장 적합한 이동수단을 골라보는 상황을 생각해봅시다.

• 시간을 최대로 절약하고자 한다면 비행기를 타는 것이 최적일 것입니다.
• 돈을 최대로 아끼고자 한다면, 자전거를 타는 것이 최적일 것이지요.

⇒ 이렇게 원하는 상황을 수식적으로 표현한 함수를 cost function이라고 부르며, LQR은 cost function을 최소화하는 최적의 값을 찾기 때문에 optimal control이라고 불립니다.

• 시간과 돈 중 어떠한 요소에 더 큰 가중치를 두느냐에 따라 최적의 이동수단은 바뀌게 됩니다. 이처럼 Cost Function이 어떻게 설계되는지에 따라 Control Signal도 다른 양상을 띄게 된답니다.

LQR의 Cost Function은 아래와 같은 형태를 갖습니다.

• Performance를 의미하는 Factor Q와, Effort를 의미하는 Factor R로 구성되어 있습니다. 이를 weighting matrix라고 지칭합니다. (모터의 효율을 Effort, 소요되는 시간을 R 등으로 빗대어 생각할 수 있지요.)
• 행렬 연산에서 $x^TAx$의 형태는 “제곱”의 의미를 갖습니다. 제곱을 하면 음수 항이 사라지지요? 이 연산을 취한 뒤, 적분을 적용하면 함수의 절대적인 면적을 얻을 수 있습니다.
• 시스템이 정상 상태에 도달하기까지의 cost를 모두 더한 것이 cost function이 되기 때문에, 이를 수식적으로 표현하면 시간에 대한 적분이 됩니다. 더불어 프로그램을 통한 수치적 계산 시에는, 연속적인 시간을 일정 단위로 쪼개어 더하게 될 것입니다.

state vector와 control vector는 다차원 벡터이기 때문에 Q와 R은 N * N 형태의 정방 행렬이 됩니다.

• 특정 모터가 매우매우 비싸서 손상을 피해야 하는 경우, 해당 모터의 Effort에 해당하는 $R$ matrix의 element를 크게 설정할 것이며,
• 특정 state의 정확도가 매우 중요한 경우, 해당 state에 해당하는 $Q$ matrix의 element를 크게 설정해야 할 것입니다.

LQR의 Cost Function과 Parameter를 적용하는 절차를 정리해봅시다.

1. Q와 R Matrix를 설정하여 Cost Function J를 지정합니다.
2. 해당 J를 만족시키는 최적의 해를 계산합니다. (Cost Function J를 최소화하는 제어 입력을 계산해보면, 놀랍 항상 $u=-kx$의 형태로 쓸 수 있게 됩니다. 이에 대한 증명은 이번 강의에서 진행하지 않으며 python의 control 모듈에서 제공하는 API를 사용할 것입니다.)
3. 계산한 최적의 제어기를 System에 적용하여 Simulation을 하거나, 실제 모터의 제어를 합니다.

💡그렇다면 Q와 R 행렬은 어떻게 최적화할까요?

⇒ 정답이 있는 것이 아니지만, Q, R의 모든 대각 성분을 1로(Identity Matrix로) 설정한 뒤, 원하는 파트를 조금씩 튜닝하는 방법이 있습니다.

코드 구현

• LQR 또한 control 모듈을 통해 손쉽게 구할 수 있습니다. 다만, control.lqr은 3개의 output을 반환하는데요. 사실 LQR의 해를 구하는 것은 아래의 Ricatti Equation을 푸는 것을 포함합니다. 실질적으론, 디버깅을 위해 두번째 값으로 Ricatti Equation의 해를, 세번째 값으로 조정된 state에서의 eigenvalue를 반환합니다.

• python 코드의 적용 결과는 다음과 같습니다.

⇒ 위 결과에서 보듯이 LQR이 적용된 이후 eigenvalue를 허수부도 포함하고 있습니다. 우리가 설정한 Q,R matrix에 대해 최적의 해를 구해준 것이지 무조건 빠르게 수렴하는 해를 구해주는 것이 아닌 것이지요.

• 일반적으로, Q의 요소들이 R의 요소들보다 크면 좀 더 공격적인 제어가 이루어집니다. ( performance >> effort가 되기 때문입니다.) 코드의 R matrix를 수정한 뒤 다시 실행해봅시다.

⇒ E를 보면 알 수 있듯이 허수부도 없고, pole이 원점에서 매우 멀리 떨어진, 아주 공격적인 제어가 이루어졌습니다.

Simulation 코드 구현

앞서 분석한 spring-mass system이 안정 상태에 도달하도록 직접 제어기를 적용하고, 시뮬레이션 해보겠습니다.

1. Pole Placement / LQR을 통해 Gain K를 계산하고
2. 해당 Gain을 사용하는 제어기를 구현하여 odeint를 통해 integration을 진행합니다.
3. for loop를 통해 지정된 시간 동안 시뮬레이션을 실행하고 결과를 분석합니다.

• 시스템의 dynamcis는 이전 예시와 동일하게 사용할 것입니다.

• odeint의 input이 되는 함수, spring_mass_linear_equ에선 다음 state 전이를 계산합니다. ( 지금까지는 직접 운동방정식을 통해 return array를 만들어 주었지요? 현재는 state-model을 구했기 때문에 이렇게 쉽게 구현할 수 있는 것입니다.)

• 제어기에 해당하는 get_control 함수에서는 gain K를 적용한 control signal을 반환합니다.

• Gain K는 main 함수에서 구해지며 LQR의 경우 Q, R이 필요할텐데 이는 Parameters 클래스에 구현되어 있습니다.

• main문의 주된 역할은 for-loop를 순회하면서 spring_mass_linear_equ의 결과인 state vector들을 차곡차곡 쌓고, 그래프를 그리는 것입니다.

작성된 코드를 실행해봅시다!

python3 sw_spring_mass_sim.py

• position과 velocity가 진동하는 것을 볼 수 있습니다. 이는 아직 제어기를 적용하지 않았기 때문으로 코드의 spring_mass_linear_equ 부분을 수정하여 제어를 적용해봅시다.

• 위 결과는 pole placement를 사용한 것으로 p = [-1,-2,-3,-4]을 적용한 결과입니다. 다음으로 LQR을 적용해보겠습니다. main 함수의 LQR 구현을 주석 해제합니다.

• 현재 적용된 Q, R 값은 아래와 같습니다. 앞서 학습을 모두 이해하셨다면 여러분들은 이제 Q,R 행렬을 조정하여 마음껏 원하는 제어 결과를 얻을 수 있겠지요? exercise로 몇가지 예시를 남겨둘테니 직접 구현해보시기 바랍니다!

Exercise

• ex1) 아래와 같이 2초 안에 시스템을 수렴시키는 aggressive controller를 구현해보세요. (단, 다른 모든 매개변수는 고정시킨 상태로 weighting matrix Q의 수정으로만 구현해봅시다.)

• ex2) Actuator u2로 매우 저가형 모델을 사용하여 max force 0.5의 제한을 갖는다고 합니다. 이를 만족하는 controller를 구현해보세요. (단, 다른 모든 매개변수는 고정시킨 상태로 weighting matrix R의 수정으로만 구현해봅시다.)