PD Controller와 Feedback Linearization을 사용하여 2D Manipulator의 제어를 시뮬레이션해봅시다.

• one link manipulator의 운동방정식을 연습삼아 다시 구해보겠습니다.

one link manipulator가 원하는 point(지금의 경우 joint angle theta가 될 것입니다.)에 도달하도록 제어기를 구현하고 시뮬레이션 해보는 것이 이번 강의의 목표입니다.

PD Control

로봇의 현재 상태 $q, \dot{q}$ 을 통해 목표 지점 $q_{des}$에 도달하도록 하는 PD 제어기는 다음과 같이 표현할 수 있습니다.

• 파이썬 코드로 이를 표현하면 다음과 같아지지요.

• get_tau의 리턴값은 운동 방정식의 우변에 해당하고, 따라서 odeint의 input이 되는 함수, one_link_manipulator는 아래와 같이 정의됩니다.

• 원하는 timestep을 지정하고, 초기값을 설정한 뒤, odeint를 통해 시뮬레이션하는 로직은 일전 예시들과 모두 동일합니다. 다만, 매 step마다 control signal을 별도로 저장하기 위해 for-loop를 사용하였습니다.

• 복습 차원에서 Parameter 클래스를 살펴보고자 합니다. Kp, Kd 값들과 desire point, beam의 질량과 관성 모멘트 등을 설정하고 있습니다.

• 마지막 main문입니다. 원하는 목표 지점 $q_{des}$에 도달하기 위해 현재 state를 기준으로 가해줘야 하는 토크를 계산하고 이를 매 step마다 계속해서 적용하게 됩니다.

• 코드를 실행해볼까요? 시뮬레이션 animation과 함께 state 변화에 대한 그래프를 확인할 수 있습니다.

cd ./3b_pendulum_control_partitioning
python3 onelink_main.py

💡 지금 max torque가 200을 넘어가고 있지요? 실제 모터는 이러한 강력한 힘을 순식간에 낼 수 없습니다. 따라서 일전 trajectory optimization을 적용했던 바 있었지요 (복습 차 말해보았습니다.)

Feedback Linearization

이번에는 PD 제어가 아니라 시스템의 운동방정식에 기반하여 tau를 계산하는 Feedback Linearization을 구현해봅시다. 참고로, 로보틱스에서 Feedback Linearization은 Computed Torque Method라고도 불린답니다. 😁👍

• 코드상의 구조는 유지한 상태로 제어기에 해당하는 get_tau 함수만 수정하면 되겠지요.

• 코드를 완성한 뒤, 일반 PD controller와 실행 결과를 비교해봅시다.

cd ./3b_pendulum_control_partitioning
python3 onelink_main.py

⇒ 왼쪽이 PD Controller, 오른쪽이 Feedback linearization의 결과가 됩니다. 사실상 지금은 큰 차이를 느낄 수 없습니다.

Add Noise

이 시점에서 시스템에 noise를 적용해보겠습니다! 실제 상황을 고려하여 총 2가지의 노이즈를 적용해보려 합니다.

1. tau noise - 실제 우리가 적용한 토크만큼 힘이 가해지지 않을 수 있습니다.
2. state noise - 관측한 theta, omega에 노이즈가 있을 수 있습니다.

• tau noise는 one_link_manipulator 함수에서 적용되며 제어기를 통해 얻은 control signal에서 tau_disturb를 빼주게 됩니다.

• state noise는 main 함수의 for-loop에서 적용되며, 다음 z0를 갱신할 시 noise를 더해주게 됩니다.

• 적용하는 noise의 평균과 표준편차는 Parameters 클래스 내부에 정의합니다.

• noise를 적용하여 일전 코드를 다시 실행해봅시다!

• 이번에는 theta_noise_std를 0.05 ⇒ 0.1로 증가시킨 뒤 코드를 재실행 해보았습니다.

💡 theta가 radian 값을 갖는다는 것을 고려해봤을 때, radian 0.1은 약 5도 이며, 이정도 오차에도 위 그림과 같이 요동이 심하다는 것을 확인할 수 있습니다. 실제 로봇을 개발할 때에도 이를 고려하여 정확한 피드백을 전달하는 모터를 사용해야 할 것입니다.