NGIMU 그냥 해본 것...

Study/외부장치

NGIMU 그냥 해본 것...

BlueBright 2019. 9. 23. 15:43

해당 IMU는 축의 반시계 방향으로 각도가 증가 한다. (보통 기본적인 IMU가 그러한 듯?)

축의 반시계 방향을 정의 하는 방법은

먼저 축의 (+) 방향에 오른손 엄지손가락을 위치시키고, 나머지 손가락으로 축을 감싼다.

축을 감싼 나머지 손가락의 방향이 반시계 방향이되며, 이 방향으로 각도가 (+)가 된다.

~~※ 물론 각도에 대한 값이니까 일부 구간에서 (-)이다.~~

하단의 사이트에서 힌트를 얻어 회전 행렬을 유도 한다.

https://o-tantk.github.io/posts/derive-rotation-matrix/

tantk land

o-tantk.github.io

http://planning.cs.uiuc.edu/node102.html

Yaw, pitch, and roll rotations

A 3D body can be rotated about three orthogonal axes, as shown in Figure 3.8. Borrowing aviation terminology, these rotations will be referred to as yaw, pitch, and roll: A yaw is a counterclockwise rotation of about the -axis. The rotation matrix is given

planning.cs.uiuc.edu

http://www.chrobotics.com/library/understanding-euler-angles

Understanding Euler Angles | CH Robotics

www.chrobotics.com

$R_z(yaw)
= \begin{pmatrix}
\cos(yaw) & - \sin(yaw) & 0\\
\sin(yaw) & cos(yaw) & 0\\
0& 0 & 1
\end{pmatrix}$

$R_y(pitch)
= \begin{pmatrix}
\cos(pitch) & 0 & \sin(pitch) \\
0 & 1 & 0 \\
-\sin(pitch)& 0 & \cos(pitch)
\end{pmatrix}$

$R_x(roll)
= \begin{pmatrix}
1 & 0 & 0 \\
0 & \cos(roll) & -\sin(roll) \\
0 & \sin(roll) & \cos(roll)
\end{pmatrix}$

유도한 회전 행렬을 이용하면, Yaw, pitch, roll에 대한 방향 벡터 행렬 (식)을 구할 수 있다.

※ 행렬은 기준이 되는 축의 특성에 따라서 변화할 수 있다. (예를 들면 시계 방향으로 (+) 라던지...)

방향 벡터의 길이는 단위 벡터 이므로 1이 나온다.

$R(yaw,pitch,roll)
= R_z(yaw)R_y(pitch)R_x(roll)\\
= \begin{pmatrix}
\cos(yaw)\cos(pitch)
& \cos(yaw)\sin(pitch)\sin(roll)-\sin(yaw)\cos(roll)
& \cos(yaw)\sin(pitch)\cos(roll)+\sin(yaw)\sin(roll)\\
\sin(yaw)\cos(pitch)
& \sin(yaw)\sin(pitch)\sin(roll)+\cos(yaw)\cos(roll)
& \sin(yaw)\sin(pitch)\cos(roll)-\cos(yaw)\sin(roll)\\
-\sin(pitch)
& \cos(pitch)\sin(roll)
& \cos(pitch)\cos(roll)
\end{pmatrix}$

x축 기준

기본 위치 벡터

(1,0,0)

$\begin{pmatrix}
x\\
y\\
z
\end{pmatrix} = \begin{pmatrix}
\cos(yaw)\cos(pitch)\\
\sin(yaw)\cos(pitch)\\
-\sin(pitch)
\end{pmatrix}$

y축 기준

기본 위치 벡터

(0,1,0)

$\begin{pmatrix}
x\\
y\\
z
\end{pmatrix}
=
\begin{pmatrix}
\cos(yaw)\sin(pitch)\sin(roll)-\sin(yaw)\cos(roll)\\
\sin(yaw)\sin(pitch)\sin(roll)+\cos(yaw)\cos(roll)\\
\cos(pitch)\sin(roll)
\end{pmatrix}$

z축 기준

기본 위치 벡터

(0,0,1)

$\begin{pmatrix}
x\\
y\\
z
\end{pmatrix}
=
\begin{pmatrix}
\cos(yaw)\sin(pitch)\cos(roll)+\sin(yaw)\sin(roll)\\
\sin(yaw)\sin(pitch)\cos(roll)-\cos(yaw)\sin(roll)\\
\cos(pitch)\cos(roll)
\end{pmatrix}$

기준 : IMU를 북쪽으로 위치시키고 (IMU를 똑바로 두고 X축이 북쪽을 향하도록)

Roll, Pitch, Yaw가 거의 0인 상태에서 시작하면 쉽다.

(이상적인 값은 x축 기준일 때, (1,0,0) y축 기준일 때, (0,1,0)