Posted on by byeongkyu

Kinematic Model for 3 Wheel Omni Drive Robot

3 Wheel Omni Drive 로봇은 메카넘휠(90º) 3개를 120º 간격으로 배치하여 구성하며, 바퀴 속도의 조합에 따라 전방향으로 이동 가능하다.

식은 다음과 같이 나타낼 수 있다.

\dot{\phi} = J_{2}^{-1} J_{1f} R(\theta) \dot{\xi_{I}} \dot{\xi_{I}} = R(\theta)^{-1}J_{1f}^{-1}J_{2}\dot{\phi}

여기서,

\dot{\phi} = \begin{bmatrix} \dot{\phi}_{1} \\ \dot{\phi}_{2} \\ \dot{\phi}_{3} \end{bmatrix} \dot{\xi_{I}} = \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} J_{2} = \begin{bmatrix} 2r & 0 & 0 \\ 0 & 2r & 0 \\ 0 & 0 & 2r \end{bmatrix} J_{1f} = \begin{bmatrix} sin(\frac{\pi}{3}) & -cos(\frac{\pi}{3}) & -l \\ 0 & -cos(\pi) & -l \\ sin(-\frac{\pi}{3}) & -cos(-\frac{\pi}{3}) & -l \end{bmatrix} R(\theta) = \begin{bmatrix} cos \theta & sin\theta & 0 \\ -sin \theta & cos \theta & 0 \\ 0 & 0 & 1\end{bmatrix}

r는 바퀴의 반지름, l은 로봇 중심으로부터 바퀴까지의 거리이다.

이제 바퀴의 속도 변경에 따른, 로봇의 움직임을 구하려면,

\begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} cos \theta & sin\theta & 0 \\ -sin \theta & cos \theta & 0 \\ 0 & 0 & 1\end{bmatrix}^{-1} \begin{bmatrix} sin(\frac{\pi}{3}) & -cos(\frac{\pi}{3}) & -l \\ 0 & -cos(\pi) & -l \\ sin(-\frac{\pi}{3}) & -cos(-\frac{\pi}{3}) & -l \end{bmatrix}^{-1} \begin{bmatrix} 2r & 0 & 0 \\ 0 & 2r & 0 \\ 0 & 0 & 2r \end{bmatrix} \begin{bmatrix} \dot{\phi}_{1} \\ \dot{\phi}_{2} \\ \dot{\phi}_{3} \end{bmatrix}

여기서 나온 결과, 즉 로봇의 속도 (\dot{x}, \dot{y}, \dot{\theta})를 누적하면 Odometry 정보를 얻을 수 있다.

이제 로봇을 제어하기 위해서, 원하는 로봇의 선속도 (\dot{x}, \dot{y} ), 각속도 (\dot{\theta})가 입력이 되면,

\begin{bmatrix} \dot{\phi}_{1} \\ \dot{\phi}_{2} \\ \dot{\phi}_{3} \end{bmatrix} = \begin{bmatrix} 2r & 0 & 0 \\ 0 & 2r & 0 \\ 0 & 0 & 2r \end{bmatrix}^{-1} \begin{bmatrix} sin(\frac{\pi}{3}) & -cos(\frac{\pi}{3}) & -l \\ 0 & -cos(\pi) & -l \\ sin(-\frac{\pi}{3}) & -cos(-\frac{\pi}{3}) & -l \end{bmatrix} \begin{bmatrix} cos(0) & sin(0) & 0 \\ -sin(0) & cos(0) & 0 \\ 0 & 0 & 1\end{bmatrix} \begin{bmatrix} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{bmatrix}

와 같이 각 바퀴의 속도를 구할 수 있다. 이제 이 속도를 바퀴 모터에 적용하면 로봇이 원하는 방향으로 움직인다.

참고:

Leave a Reply

This site uses Akismet to reduce spam. Learn how your comment data is processed.