이번 시간에는 pendulum이 움직일 trajectory를 설정하고 feedback linearization을 통해 이 trajectory를 따라 제어하는 예시(trajectory tracking control)를 구현해보고자 합니다.

PD controller를 학습하기 전, sympy와 numpy를 통해 trajectory generation, optimization을 실습한 바 있었습니다. 이제 우리는 이렇게 만들어진 reference trajectory $q_{ref},\dot{q}{ref},\ddot{q}{ref}$를 따라 로봇을 제어하기 위한 제어기를 개발해볼 것입니다.

• 일전 Feedback linearization 제어기는 모든 state가 0으로 수렴하도록 제어되었지요? 따라서 trajectory generation을 통해 생성된 reference들을 아래와 같이 마이너스 term으로 적용시키면 경로를 따르는 움직임을 구현할 수 있습니다.

• tau를 운동 방정식에 대입한 뒤 양변을 정리해보면 결국 q에 대한 spring-mass-damper system으로 표현할 수 있으며, 이는 reference state와의 차이를 error로 갖는 제어기가 됩니다.

💡 아래 식이 왜 갑자기 등장했는지 의문이 든다면 이전 강의의 Feedback Linearization을 복습하고 돌아오세요!

코드 구현

이번 코드는 두개의 파일로 나눠집니다.

1. trajectory generation code
2. simulation code

• trajectory generation code에선 one_link pendulum이 0 ⇒ pi/2 ⇒ 0으로 움직일 시, transient가 발생하지 않는 reference trajectory를 계산합니다

• onelink_traj.py의 계산 결과를 통해 trajectory 방정식을 구성하는 계수에 대해 모두 알아냈다면, 두번째 코드 파일로 이동하여 timestep을 구성하고 numpy를 통해 상세한 reference state들을 모두 미리 계산해둡니다.

• control signal을 계산하는 get_tau에서 해당 reference state들을 적용하여 tau를 도출하게 되며, 이렇게 계산한 tau를 통해 Feedback Linearization을 구동하는 방식은 이전 강의와 동일합니다.

• 코드를 실행시켜볼까요? animation이 동작한 뒤 state 변화에 대한 그래프를 얻을 수 있습니다.

• one link pendulum에도 noise를 추가하여 시뮬레이션해봅시다.

Double Pendulum

one-link pendulum을 제어해보았으니, 이번에는 double pendulum을 제어해보려 합니다. 제어해야 할 모터만 하나 더 추가된 것일 뿐 절차는 동일합니다.

1. 각 모터에 대한 trajectory 계산 ⇒ sw_link1_traj.py, sw_link2_traj.py
2. system의 EOM을 계산 ⇒ sw_twolink_eom.py
3. feedback linearization을 사용한 제어기를 구성하고 reference로 trajectory를 전달 ⇒ sw_twolink_main.py

• double pendulum에 대한 해석은 복습 차원에서 간단하게 정리해보았습니다.

• 현재 구현된 코드는 link1과 link2를 다음과 같이 움직입니다.

Cartesian Coordinate (task space) Traj

지금까지 우리가 생성한 경로는 모두 joint angle에 대한 것이었습니다. 하지만 실제 manipulator를 다룰 시, 우리는 end-effector의 좌표에 대한 경로가 필요하지요. 따라서, end-effector trajectory를, joint trajectory로 변경하는 작업이 필요합니다.

position의 변환

• 주어진 cartesian position을 joint angle position으로 변환하는 작업은 inverse kinematics와 일맥상통합니다. 코드 구현 시 python의 fsolve를 사용해서 joint angle을 수치 반복 계산으로 도출했던 바 있었습니다

velocity의 변환

• Jabocian의 정의에 의해 Jacobian의 inverse와 reference velocity를 곱하면 joint angle velocity의 trajectory를 얻을 수 있습니다. (Jacobian의 정의에 따른 트릭인데요, Jacobian 자체에 대해 망각했다면 이전 7강 강의를 복습하고 오시기 바랍니다.)

acceleration의 변환

• 앞서 velocity 계산 시 사용했던 방정식을 다시 변형해보겠습니다. 방정식의 양변을 미분하면 acceleration에 대한 관계식을 얻을 수 있습니다.
• 하지만 이번에는 chain rule을 적용해야 함에 유의합니다.

코드 구현

다음과 같은 구현들을 진행해보고자 합니다.

1. end-effector의 trajectory 계산
2. end-effector ⇒ joint trajectory로의 변환
   1. 이 작업을 위해서는 앞선 수식과 같이 Jacobian, Jacobian의 inverse, 미분값 등이 필요합니다.
3. joint trajectory를 얻었으므로 이후 과정은 동일합니다.

• end-effector의 trajectory로는 두가지 방식을 준비하였습니다. (circular / figure eight)

• 원의 방정식과, 숫자 8을 그리는 방정식은 get_circle, get_eight로 구현해 두었습니다.

• 이렇게 얻어진 cartesian trajectory들을 이제 joint angle trajectory로 변경해봅시다.

$$ q_{ref} = f^{-1}(x_{ref}) \\ \dot{q_{ref}} = J^{-1}{\dot{x_{ref}}} \\ \ddot{q_{ref}} = J^{-1}(\ddot{x_{ref}}-\dot{J}\dot{q_{ref}}) $$

💡 위 작업을 위해 Jacobian, Jacobian의 state에 대한 미분이 필요하지요? 이는 sympy를 통해 구해볼 것입니다.

• 참고로 Jacobian의 미분을 계산할 시에도 theta에 대한 chain rule을 적용해야 함에 유의합니다!

$$ \dot{J} = \cfrac{dJ}{d\theta_1} \dot{\theta_1} + \cfrac{dJ}{d\theta_2} \dot{\theta_2} $$

• 첫번째, position에 대한 변환은 fsolve를 통한 inverse_kinematics 계산으로 얻어냅니다.

• 두번째, velocity에 대한 변환은 end-effector의 Jacobian을 사용합니다.

• 세번째, acceleration에 대한 변환은 Jacobian의 미분과 inverse를 사용하지요.

• 이 모든 과정을 거치면 드디어 cartesian coordinate ⇒ joint angle coordiante로의 변환이 마무리됩니다.

앞선 말한 바와 같이, 이후 feedback linearization을 통해 시뮬레이션을 하는 과정은 이전과 완전 동일합니다.

• 이제, 코드를 실행해봅시다. trajectory simulation과, 각 경로들에 대한 plot을 확인할 수 있습니다.