다양한 형태의 로봇이 존재하지만, 특히 바퀴 / 다리를 가진 이동할 수 있는 로봇을 mobile robot이라고 지칭합니다.

image from : omron

이번 시간에는 Mobile Robot Differential Drive 타입의 모바일 로봇에 대한 FK와 IK를 학습해보고, 직접 구현도 진행해보겠습니다.

Differential Drive 모델

DD Type이라고도 불리는 이 로봇은 우리 주변에서 흔히 볼 수 있는 로봇 청소기 형태의 로봇입니다.

양쪽의 바퀴를 통해 전/후/제자리 회전이 가능하지만, 수평 방향으로 이동하는 것은 불가합니다. (이러한 로봇을 non-holonomic 로봇이라고 부릅니다.)

• 2차원 좌표계에서 로봇의 상태는 $(x, y, \theta)$로 표현할 수 있으며, 로봇 관점에서의 선속도와 각속도를 사용하여 표현한 world frame 속도는 아래와 같습니다.
  

• 사실 이 식은 아래의 회전 변환 연산을 통해 얻은 것이지요

⇒ 앞서 이야기한 것과 같이 y축으로는 속도를 가질 수 없기 때문에 $\dot{y}$은 0이 됩니다.

이번 코드에서 고려하지는 않지만, 실제 우리가 로봇에게 내리는 제어값은 2차원 좌표계에서의 속도가 아니라, 바퀴의 속도가 됩니다. 따라서 앞선 값들을 바퀴의 속도 관점에서 새롭게 정리해봅시다.

• 각 바퀴의 각속도와 바퀴 반지름을 곱하면 선속도가 됩니다.

• 양쪽 바퀴 선속도를 회전 중심인 중앙점 관점에서 각각 계산해 더하면 로봇 선속도를 얻을 수 있고, 비슷한 원리로 로봇 각속도를 구할 수 있습니다.

Forward Kinematic

• Euler Integration은 미분의 정리에 기반하여 다음 state를 누적 계산할 수 있습니다. 로봇의 누적 위치를 구하기 위해 Euler Integration을 전개해보면 아래와 같습니다.

⇒ 즉, 매 timestamp마다 주어지는 control signal에 기반하여 해당 계산을 누적하면, 변화하는 로봇의 state를 얻을 수 있게 되며, 이것이 Forward Kinematics가 됩니다.

코드 분석

• 우선, 예제를 실행해볼까요?

cd lec3_dd_mobile_robot
python3 diff_drive_fk.py

코드는 다음과 같이 구성됩니다.

1. control signal 준비
2. Euler Integration을 통한 state 누적 계산
3. Animation 재생

• control signal 준비

로봇 입장에서 양쪽 바퀴의 각속도를 제어값으로 갖지만, 이번 예시에서는 편의를 위해 2차원 평면에서의 선속도와 각속도를 제어값으로 준비하였습니다.

• Euler Integration을 통한 state 누적 계산

control signal과 이전 state를 통해 앞서 그림을 통해 보여드렸던 Euler Integration을 계산합니다. 다음 state인 x, y theta를 얻을 수 있으며, 이를 z라는 리스트에 누적시킵니다.

• 모든 state들은 z에 저장되어 있습니다. 이를 사용하여 애니메이션을 그립니다.

Robot movement in world frame

• Inverse Kinematics를 진행하기 전에, 일전에 배웠던 Homogeneous Matrix를 좀 더 공부하고 넘어가봅시다.

이번에 다루는 모바일 로봇은 계속해서 좌표계가 바뀌게 됩니다. 따라서, 로봇 입장에서의 움직임을 world frame에서의 움직임으로 계속해서 바꿔가면서 누적해야 하지요.

• 예시를 통해 살펴봅시다. $(x=1,y=1,\theta=\pi/2)$상태를 갖는 로봇 입장에서 (1, 0) 전진을 한다면 (1, 2)의 좌표를 가질 것입니다.

• 지금은 직관적으로 알 수 있지만, 이를 homogeneous matrix를 사용하여 나타내면 다음과 같습니다.

우리가 로봇에서 전달하는 제어 신호는 로봇 좌표 기준이지만, 도달하기 원하는 좌표는 world frame상에 있습니다. 따라서 지금 배운 homogeneous matrix가 계속해서 사용됩니다.

Inverse Kinematic

Inverse Kinematic는 코드를 분석하기 전 이론을 먼저 살펴보고자 합니다. 우리가 하고자 하는 바를 명확하게 정의해보면 다음과 같습니다.

• input ⇒ 현재 로봇의 state $(x, y, \theta)$와 로봇이 이동하기 원하는 $(x_{ref}, y_{ref})$
• output ⇒ 해당 ref point로 움직이기 위한 제어 신호 $(u, w)$
• 시뮬레이션을 위해서는 이 v, w를 통해 다시 euler integration을 진행할 것입니다.

⇒ 따라서 우리는 $(x_{ref}, y_{ref})$, $(x, y, \theta)$와 v, w를 연결짓는 상관관계를 이끌어내야 합니다.

그림을 통해 상황에 대해 정의해보겠습니다.

• DD Type 로봇을 직선 운동과 회전 운동만 할 수 있으며 y축 수평 이동은 불가합니다. 따라서, 회전 운동을 통해 목표 지점과 x축을 일직선상에 맞추고 직선 운동을 한다고 설계해보겠습니다.
• 로봇의 원점, 목표지점의 좌표를 $C_0, P_0$ ,회전운동을 한 뒤의 이 두 점의 좌표는 $C_1, P_1$ 라고 합시다.

• $(C_0, P_0)$와 $(C_1, P_1)$는 아래와 같은 관계를 갖습니다. $(P_1, C_1)$은 회전 운동을 거친 뒤 로봇이 직진해야 하는 offset이 되며, 이를 $P_x, P_y$ 라고 설정하겠습니다. (DD type 로봇은 전방 이동만 가능하므로 $P_y$는 0이 되어야 합니다.)

• 앞선 식을 전개하고, 이를 시간에 대해 미분해보겠습니다. 그리고, Forward Kinematics 시 구했던 각 축에 대한 속도를 결합하면 최종적으로 $P_x, P_y$ 와 $v, w$ 를 연결짓는 상관관계를 이끌어낼 수 있게 됩니다.

• v, w를 설정함에 따라 world frame에서 로봇이 어떠한 속도를 갖게 되는지 알게 되었습니다. 하지만, inverse kinematics시 우리가 전달하는 input은 속도가 아닌 위치 입니다. 따라서 원하는 위치로 움직이기 위한 속도를 도축하는 별도의 제어기가 필요합니다.
• 이를 위해 P Gain Controller를 사용해보고자 합니다. (이 제어기가 동작할 수 있는 자세한 원리는 이후 강의에서 살펴보겠습니다.)

• 완성된 행렬식을 통해 $v, w$ 를 얻어낼 수 있게 되었습니다. (2 x 2 행렬이므로 역행렬을 간단히 구할 수 있습니다.)

역행렬의 각 element들을 통해 주의할 점을 알 수 있는데요. $P_x$가 0이면 모든 element들이 무한이 되어버린다는 점입니다. (이러한 이유로 코드 구현 시 $P_x$를 고정값으로 지정하겠습니다.)

지금까지 진행한 내용을 정리해봅시다.

1. 회전 운동과 직선 운동을 갖는 로봇 특성에 따라 이동 후 좌표를 수식화
2. 좌표 전개 후 미분을 통해 control $v, w$와의 관계식 유도
3. P gain controller를 통해 지정된 경로를 따라가도록 제어기 설계

더불어 구현 시 다음과 같은 부분에 유의해야 합니다.

1. $P_x$가 0이되면 안됩니다.
2. P gain $k_{P_x}$는 우리가 설정해야 하는 값으로 여러 시도를 통해 최적화합니다.

코드 구현

• 우선 예제를 실행해봅시다. 시뮬레이션이 종료된 이후 결과 그래프를 볼 수 있습니다.

python3 diff_drive_ik.py

• 첫번째로 구현할 함수는, 로봇 좌표계상의 움직임을 world frame 기준으로 변환하는 helper function입니다.

• 구현해야 할 순서를 먼저 정리해보았습니다.

1. 현재 로봇의 좌표 기준, offset만큼 이동한 뒤의 좌표를 계산합니다. (이번 코드에서는 로봇이 한 step당 이동할 수 있는 거리, offset을 특정 값으로 지정해두었습니다.)
2. 로봇이 따라가야 할 경로와 이 좌표 사이의 error를 구하고 Inverse Kinematics에 따라 새로운 속도값 $v, w$를 계산합니다.
3. 도출한 $v, w$를 통해 로봇을 이동시킵니다. (Euler Integration을 사용합니다.)
4. 이를 통해 움직인 뒤의 새로운 로봇 좌표를 얻게 되면 다시 1번 과정으로 반복합니다.
5. 모든 시뮬레이션이 끝난 뒤 animation, plot을 그립니다.

각 파트별로 코드를 분석해보자면 다음과 같습니다.

• 현재 로봇의 좌표 기준, offset만큼 이동한 뒤의 좌표를 계산합니다.

• 로봇이 따라가야 할 경로와 이 좌표 사이의 error를 구하고 Inverse Kinematics에 따라 새로운 속도값 $v, w$를 계산합니다.

• 도출한 $v, w$를 통해 로봇을 이동시킵니다. (Euler Integration을 사용합니다.)

Animation과 Plot 코드는 생략하겠습니다.

실험해보기

열심히 작성한 코드를 통해 몇가지 실험을 해보겠습니다. $P_x$ 다양한 값으로 설정하여 코드를 실행한 모습들입니다.

• px = 0.01

• px = 0.1

⇒ 우리 코드에서 한 step 당 로봇이 이동할 수 있는 거리를 $P_x$로 고정시켜두었습니다. $P_x$가 커질수록 step당 이동하는 거리는 증가하지만 경로 추적의 정확도가 떨어지는 모습을 볼 수 있습니다.

다음 실험으로, 경로 추정을 위한 K gain을 조정해보겠습니다.

• K gain - 1

• K gain - 3

• K gain - 10

gain 값이 1 ⇒ 3 ⇒ 10으로 커질때마다 정확도가 증가하는 모습을 볼 수 있습니다. 무조건 그런 것은 아니지만 제어 매개변수의 튜닝은 매우 중요합니다. 이후 control 파트에서 좀 더 자세하게 들여다보겠습니다!