이번 시간에는, 3차원 상에서 동작하는 로봇의 예시로 4개의 날개를 가진 Quadcopter를 구현해보고자 합니다. 일전 학습한 3D 각속도에 대한 내용을 충분히 숙지한 뒤, 강의를 따라와주시길 바랍니다.

• Quadcopter는 아래와 같은 형태를 갖습니다. 대각 방향의 모터 날개는 같은 방향으로 회전하며, 다른 축을 갖는 모터는 다른 방향으로 회전합니다.

만약 대각 방향 회전이 반대 방향이 되면 모터 회전에 의한 힘이 상쇄되지 않아 안정된 비행이 불가합니다. 같은 방향으로 회전하게 되면 비록 모멘트가 발생할 수는 있지만, 수평으로 작용하는 힘은 상쇄되는 것이지요.

• 아래 그림에서, w2 w4에 의한 추력은 아래 방향이 되지 않냐는 의문을 가질 수 있는데요. 실제 Quadcopter를 조립할 시 w2, w4와 w1, w3에는 날개가 휘어진 방향이 반대인 것을 사용한답니다

image from : www.lazada.sg

• 모터에 의한 추력은 회전하는 각속도의 제곱에 비례하며, 모든 날개 추력의 합이 Quadcopter를 떠받치는 합력이 됩니다. 이 값이 Quadcopter의 무게와 동일해야 공중에서 정지 상태를 유지할 수 있으며, 이를 Hovering이라고 부릅니다.

• 이번에는 모터의 추력과 각 축에 걸리는 토크 사이의 관계를 계산해봅시다. $\tau = r \times F$라는 관계식을 사용하여 x축과 y축에 대해 걸리는 모멘트를 계산해 보면 아래와 같습니다.

• 각 날개에 의해 발생하는 z축에 작용하는 모멘트를 도출해보면 z축에 대한 최종 모멘트는 아래와 같습니다. 이때, 실제 토크로 변환될 시 곱해지는 상수 b는 drag constant라고 합니다. (일전 모터 토크 계산 시 사용한 상수 k는 힘에 대한 상수이고, 지금의 b는 토크에 대한 상수가 됩니다.)

• 정리해보면, Quadcopter 시스템은 z축과 x y z 축에 대한 토크를 제어할 수 있으며, x y 축으로는 직접 제어가 불가한 “underactuated” 시스템임을 알 수 있습니다.

• 이제 운동방정식을 구해보겠습니다. Quadcopter는 3차원 상의 움직임을 갖기 때문에 6 DOF를 가지며, control variable은 4개가 존재합니다. 특히 각속도 계산 시 body frame에 대한 각속도를 사용해야 한다는 점에 유의합니다.

• EL method를 적용하기 위해 운동에너지와 위치에너지를 구합니다. 운동에너지를 구할 시, body frame 기준 각속도를 사용해야 함에 유의합시다.

• EL equation의 우변에 해당하는 Qj에는 z축으로 작용하는 수평힘과 z x y 축으로 작용하는 torque를 대입해야 합니다.

• Quadcopter 자체는 body frame 기준 +z축으로만 힘을 가할 수 있었습니다. 따라서, 운동방정식을 세울 때에는 이를 world frame으로 변환하는 rotation matrix R을 곱하여 반영해야 합니다.

• 그리고 또 하나의 추가적인 외력이 작용하는데요. 바로 드론의 이동을 방해하는 drag force를 고려해야 합니다. 일전 자유 낙하 운동 구현 시 살펴본 바와 같이 이는 속도에 비례하는 값입니다.

• 각 축에 작용하는 토크들은 앞서 구해둔 수식을 그대로 반영하며, 이렇게 최종 계산된 외력항은 아래와 같습니다.

이제 이 수식들을 사용하여 실제 운동방정식을 계산하고, 시뮬레이션을 구현해봅시다!

코드 구현

sympy와 시뮬레이션 코드가 나뉘어 구현되어 있으며, 각각의 코드에 대해 분석해보겠습니다.

• 운동방정식 계산을 위해 선언하는 sympy 변수들은 다음과 같습니다. 특이사항으로 모터의 추력 계산 시 사용되는 lift constant K, z축 토크를 계산할 시 사용되는 drag constant b가 선언되었으며, 각 축에 대한 drag force constant Ax, Ay, Az도 선언되었습니다.

• 다음으로 회전, 병진 운동에너지, 위치에너지를 구합니다. 회전 운동에너지를 구할 시 사용되는 각속도는 body frame 기준임에 유의합시다.

• 다음으로 우동방정식의 우변이 되는 외력을 모두 구해줍니다. 모터에 의해 각 축에 작용하는 수평힘, 각 회전축에 작용하는 토크를 구하고, 방해하는 힘인 drag force도 적용합니다.

• 마지막으로 이전 강의 3D dynamics에서 이미 구현하였던 운동방정식 계산 코드를 그대로 적용합니다.

• 다음으로 시뮬레이션 코드를 분석해봅시다. 운동방정식을 구현한 quadcopter_dynamics에서 방금 전 sympy 코드의 결과를 복사 & 붙여넣기합니다.

• 매개변수가 많을 뿐이지 실질적인 시뮬레이션 파트는 for-loop에서 모두 이루어지며, 일전 예시들과 일맥상통합니다.

• 이번 예시는 Parameter의 설정이 중요하기에 비중있게 다루고 넘어가겠습니다.

• 이번 예시에서는 별도의 제어기 없이 직접 제어 신호를 수동으로 전달할 것입니다. 이에 따라 각 모터의 회전속도 omega를 설정해야 하는데요. 아래 코드에서 speed는 hovering을 하기 위한 각 모터의 속도가 되며, 이 속도에 offset을 더하거나 뺌으로 제어를 하게 됩니다.

결과 분석

각 모터 속도로 여러 수치를 입력하여 결과의 변화를 분석해봅시다.

• 우선 hovering을 위해 각 모터에 걸려야 하는 각속도를 찾아보겠습니다.

⇒ 추력이 부족하여 Quadcopter가 천천히 하강하고 있습니다. 좀 더 speed를 키워봅시다.

• 미리 계산된 값인 가중치 1.075를 곱해주면 hovering하는 모습을 확인할 수 있습니다. ( 다만, 현재 drag force를 주지 않았기에 실제 상황과는 차이가 있습니다.)

speed = 1.075*omega*np.sqrt(1/self.K)

• 다음으로, omega4를 조금 키워보았습니다. +x 축으로 기우는 quadcopter를 확인할 수 있지요.

• omega1을 키우게 되면, -y 축으로 기우는 모습이 확인됩니다.

• 이번에는 z축 기준 시계 방향으로 돌도록 제어해볼까요? omega1, omega3의 속도는 증가시키고, omega2, omega4의 속도는 감소시켜보겠습니다.

• 수평운동은 어떻게 할 수 있을까요? quadcopter 자체적으로는 z축 제어밖에 할 수 없습니다. 하지만 회전을 갖게 되면 z축 방향 힘을 분산하여 x, y 축으로의 움직임도 가능하지요. initial state의 각도를 바꿈으로 이를 시뮬레이션해봅시다.

수평운동은 어떻게 할 수 있을까요? quadcopter 자체적으로는 z축 제어밖에 할 수 없습니다. 하지만 회전을 갖게 되면 z축 방향 힘을 분산하여 x, y 축으로의 움직임도 가능하지요. initial state의 각도를 바꿈으로 이를 시뮬레이션해봅시다.