이번 시간에는 로봇 예시를 통해 지금까지 학습한 내용들을 다시 한 번 복기하는 시간을 가져보고자 합니다. 2D space에서 동작하는 Bicopter를 살펴보겠습니다!

Bicopter는 몸체인 base link의 양 끝으로 추력을 가진 모터가 장착되어 있으며, 2D space에서 수평 운동과 회전 운동을 가질 수 있습니다. base link의 중심을 floating point x, y라고 정의한 뒤, 운동방정식을 세워봅시다.

• 운동에너지와 위치에너지를 계산하여 euler-lagrange equation을 세웁니다. ( 회전 운동 에너지를 고려해야 함에 유의합니다.)

• 모터 추력에 의해 발생하는 외력은 끝점에서의 Jacobian과 추력의 곱을 통해 계산할 수 있습니다.

• base frame 기준 각 날개 끝점의 좌표를 계산해봅시다.

• state (x, y, theta)에 대해 Jacobian을 계산하면 아래와 같은 2x3 행렬을 구할 수 있습니다.

• Bicopter의 양 날개에서 발생하는 추력을 몸체 기준으로 변환합시다. 회전 행렬을 곱해주면 되겠지요?

앞서 구한 Jacobian과 몸체 기준의 추력을 사용하여 Bicopter에 작용하는 최종 외력을 구합니다. (지금 이 과정은 날개 입장에서의 힘을 로봇 몸체에서의 힘으로 변환하기 위한 작업이라는 것 기억하시죠?)

• x, y 축으로 작용하는 수평 힘과 moment에 대한 식을 얻을 수 있습니다.

• 외력을 고려한 운동방정식을 최종적으로 정리해 봅시다. 그리고 편의를 위해 실제 제어 시 변경하는 값은 양쪽 날개 속도의 합 $v_s (=v_1+v_2)$와 차이 $v_d(=v_1-v_2)$라고 정리해 보았습니다.

이 시점에서 운동방정식 결과에 대해 간단히 분석을 해봅시다.

• 현재 드론이 동작하는 2차원 평면은 3 dof를 갖습니다. 하지만 우리가 제어할 수 있는 값은 왼쪽과 오른쪽 추력뿐이지요. 이렇게 dof에 비해 제어 가능한 자유도가 적은 경우를 “underactuated”라고 부릅니다.
• 만약 $\phi$가 0이라면, $\ddot{x}=0$이 되고, 이 상태에서 $\ddot{y}=0$이 되기 위해선 $v_s=mg$ 의 값을 가져야 합니다. 이는 곧 bicopter의 중력을 보상하는 만큼의 제어가 이루어져 가만히 정지한다는 뜻이며, 이 상태를 “hovering”이라고 부릅니다.
• 만약 $\phi$가 90도라면 $\ddot{y}$가 상수가 되며, 전혀 제어할 수 없는 상황이 됩니다. 이렇게 되지 않도록 제어 시 주의해야겠지요!

코드 구현

Bicopter의 시뮬레이션 구현을 위해 두가지 파일을 사용하도록 하겠습니다.

1. 운동방정식을 계산하는 sympy 코드 derive_bicopter
2. 시뮬레이션을 작성하는 numpy 코드 main_bipcopter

• 지금까지 강의를 잘 따라오셨다면, 운동 방정식을 코드 구현하는 것은 크게 어려울 것이 없을것이라 생각합니다. 이번 구현에서 주의해야 할 외력에 대해서만 다잡고 가겠습니다.

• endpoint P, R에 대해 Jacobian을 계산합니다.

• 모터의 추력 u1, u2를 로봇 중심 좌표 기준으로 변환합니다.

• 계산한 결과 Q는 운동방정식의 우변이 됩니다.

시뮬레이션을 구현한 main copter code는 다음과 같은 구조를 갖습니다. (사실 Bicopter라는 시스템이 사용되는 것일 뿐 구현 절차는 이전과 완전 동일합니다.)

1. odeint의 input이 되는 함수 bicopter_dynamics와 제어기 controller 함수를 구현합니다.
2. timestep을 정의하고 for-loop를 통해 데이터를 시뮬레이션합니다.

• 다음은 1번에 해당하는 구현들입니다. 앞서 derive_bicopter의 결과를 대입하기만 하면 됩니다.

• timestep과 initial state를 지정하고, for-loop를 순회하는 main 코드는 다음과 같습니다.

• 코드 실행 결과는 다음과 같습니다. 3차원 상의 드론을 측면에서 보고 있는 상황이라고 생각하면 됩니다. 중력이 작용하고 있기 때문에 아무런 추력이 없다면 그대로 자유낙하해버리고 말 것입니다.

Exercise

• controller의 us와 ud, initial state를 조정하여 Bicopter의 움직임에 어떠한 변화가 생기는지 확인해보세요.
• 전후좌우 평행 이동을 하기 위해 필요한 control이 어떻게 되는지 실험을 통해 확인해보세요.

Copter Trajectory

Bicopter가 특정 경로를 따라 이동하도록 1) 경로를 생성하고, 2) 해당 경로를 따라 움직이게 하는 제어기를 구현해봅시다.

• bicopter의 운동방정식은 sin, cos이 포함된 비선형 형태를 갖습니다. 따라서 linear control 시간에 학습한 것과 같이 operating point와 derivative term으로 제어를 나누고, 각각을 구해서 더해보려 합니다.

• bicopter가 어떠한 움직임도 갖지 않는 상황, Nominal State를 만족하기 위한 us와 ud를 구해봅시다.

• derivative term을 구하기 위해 ud와 us를 운동방정식에 대입하고, $\phi$ 가 매우 작다는 가정을 사용하여 각 운동방정식을 다시 정리합니다.
• 최종 정리된 결과를 보면, $\ddot{y}$에 us가, $\ddot{\phi}$에 ud가 포함되어 있으며, $\ddot{x}$는 오로지 $\phi$ 에 대한 함수가 됨이 확인됩니다. (underactuated system이기 때문에 이러한 상황이 발생한 것입니다.

• 우리가 원하는 것은 bicopter가 Cartesian coordinate에서 정해서 경로를 따라 움직이게 하는 것이지요? 일전 $\phi$가 90도가 되어버리면 전체 시스템이 제어 불가능해졌던 경험도 고려해보면, theta에 대해서는 최대한 움직임이 없게 하는 것이 맞아보입니다. 이를 고려하여 제어기 작성 시 약간의 트릭을 사용해봅시다.
• $\phi$를 x에 대한 식으로 변형한 뒤, x의 reference를 사용하여 PD 제어를 적용합니다. 이렇게 계산된 $\phi_{ref}$를 $v_d$의 제어 시 사용하는 것이지요.

현 상황을 다시 정리해보자면 아래와 같습니다.

1. $\ddot{x}$와 $\phi$사이의 관계를 이용해 $\phi_{ref}$를 세웁니다.
2. $v_s$를 통한 $\ddot{y}$ 제어는 일반적인 PD controller를 사용합니다.
3. $v_d$를 통한 $\phi$ 제어 시, 1에서 구한 $\phi_{ref}$를 사용합니다.

현재의 제어 방식은 운동방정식에 기반하였고, PD Gain을 사용한 Feedback Linearization이라고 볼 수 있습니다.

코드 구현

일전 main_bipcopter 코드에서 1) 경로 생성과 2) 제어기 부분이 수정되었습니다.

• 경로 생성을 위한 함수들은 공통적으로 로봇의 초기 위치, timestep을 input으로 받고, $x, y, \dot{x}, \dot{y}, \ddot{x}, \ddot{y}$ 에 대한 reference를 반환합니다. 원 경로와 숫자 8을 그리는 경로가 구현되어 있습니다.

• 앞선 수식을 그대로 controller 함수 내부에 구현하였으며, 다시 한 번 말하지만, 이는 reference points들과 현재 state를 feedback으로 사용하는 제어기입니다.

• 숫자 8을 그리는 경로를 사용하여 시뮬레이션한 모습입니다. 그래프를 보면 알 수 있듯 거의 완벽에 가까운 제어가 되었네요.

Exercise

• $x, y, \phi$에 대한 gain값을 수정하여 예제를 다시 실행해보고, 결과에 어떠한 차이가 있는지 확인해보세요.

• 현재의 시뮬레이션 결과는 아주 완벽한 상황에서 계산되었습니다. control, measurement noise를 추가하여 예제를 다시 실행해보고 어떠한 noise가 가장 큰 영향을 미치는지도 확인해보세요.