Quaternion Base

-

쿼터니언 (4원수) 회전 정리

1. 3차원에서 Quaternion rotation을 할 때, 특정 벡터를 쿼터니언으로 표기를 하면 아래와 같다. 4차원 세상의 쿼터니언과 3차원의 벡터가 연결되는 다리를 만드는 과정이라고 생각하면 된다.

P = (Pv, Pw)
P = (p, 0)
실수부가 0이고, 허수부가 p인 pure Quaternion이 된다.

 [그림 1]


2. rotation axis u, rotation angle theta 를 통해 Quaternion rotation q를 정의해보자.

이 때, rotation Quaternion q의 크기는 1 (norm(q) == 1) 인 unit Quaternion이 되어야 한다. 이 unit Quaternion은 극형식으로 나타낸다.

 e^(u_q * theta) = u_q * sin(theta) + cos(theta)
-> Quaternion version of Euler formula

3. Ken Shoemaker라는 분의 Quaternion 관련된 논문에 의하면,  Quaternion P를 축 u_q 중심으로 2theta 만큼 회전하는 것은 아래와 같이 표현이 된다. (Quaternion P is rotate 2*theta around u_q)


 q * p * q^-1

 q는 unit Quaternion이므로 다시 정리하면,

 q * p * q^*

 이 공식을 이용하여서 2 * theta 만큼 회전된 결과인 쿼터니언의 허수부를 구할 수 있다.

4. 연속으로 q에 대해서, 그리고 r에 대해서 회전을 할 경우 아래와 같다.

e^(u_q * theta) = u_q * sin(theta) + cos(theta)
e^(u_r * theta) = u_r * sin(theta) + cos(theta)

 r * (q * p * q^*) * r^* =   (r * q) * p * (q^* * r^*)

5. 쿼터니언을 회전 행렬로 변환하는 방법을 알아보자.


 [그림 2]

위의 그림을 참고해서 보자면, qpq*를 설명하기 위한 예제로 2개의 Quaternion의 곱셈 pq를 행렬로 위의 그림과 같이 표현 할 수 있다.

 그리고 M_q * N_q * p 에 대해서 변환하여 최종 매트릭스를 구해낸다. (그림 2에서 2번에서 4번 과정을 참고)

 이제 Quaternion rotation을 행렬의 형태로 표현하는 것이 가능하다.

6. 그럼 Quaternion rotation matrix로 부터 회전축 u_q 혹은 q를 구할 수 있을까? 가능하다.

그림 2의 5번 과정을 참고하자.
결국 q_w(회전축의 실수부)을 알면, q_v인 (q_x, q_y, q_z)를 구할 수 있다.

 그럼 q_w은 어떻게 구할 수 있을까?
그림 2의 6번 과정을 참고하자.

7. 최종적으로 Quaternion rotation matrix M_q는 4번을, 그리고 그의 역과정은 7번을 참고하면 된다.

8. 마지막으로 예전에 만들어놓은 Quaternion 관련 파이썬 코드들 덧붙인다.

- Euler to Quaternion
- Quaternion to Euler
- Quaternion Rotation Matrix

def eul2quat(eulAngleList, seq):
    mod_eulAng = np.dot((1/2), eulAngleList)
    #Euler Angle = (phi, theta, psi) as X, Y, Z
    if seq == 'ZYX': #Z_psi, 0 -> Y_theta, 1 -> X_phi, 2 / unit is RAD!!!!!        
                qList = [np.cos(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.cos(mod_eulAng[2]) + \
                 np.sin(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.sin(mod_eulAng[2]), \
                 np.cos(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.sin(mod_eulAng[2]) - \
                 np.sin(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.cos(mod_eulAng[2]), \
                 np.cos(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.cos(mod_eulAng[2]) + \
                 np.sin(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.sin(mod_eulAng[2]), \
                 np.sin(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.cos(mod_eulAng[2]) - \
                 np.cos(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.sin(mod_eulAng[2])
                 ]

    elif seq == 'XYZ': #X_phi 0 -> Y_theta, 1 -> Z_psi, 2 / unit is RAD!!!!!        
                 qList = [np.cos(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.cos(mod_eulAng[2]) - \
                 np.sin(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.sin(mod_eulAng[2]), \
                 np.sin(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.cos(mod_eulAng[2]) + \
                 np.cos(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.sin(mod_eulAng[2]), \
                 np.cos(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.cos(mod_eulAng[2]) - \
                 np.sin(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.sin(mod_eulAng[2]), \
                 np.cos(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.sin(mod_eulAng[2]) + \
                 np.sin(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.cos(mod_eulAng[2])
                 ]

    elif seq == 'ZYZ': #Z_phi 0 -> Y_theta, 1 -> Z_psi, 2 / unit is RAD!!!!!        
                 qList = [np.cos(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.cos(mod_eulAng[2]) - \
                 np.sin(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.sin(mod_eulAng[2]), \
                 np.cos(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.sin(mod_eulAng[2]) - \
                 np.sin(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.cos(mod_eulAng[2]), \
                 np.cos(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.cos(mod_eulAng[2]) + \
                 np.sin(mod_eulAng[0]) * np.sin(mod_eulAng[1]) * np.sin(mod_eulAng[2]), \
                 np.sin(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.cos(mod_eulAng[2]) + \
                 np.cos(mod_eulAng[0]) * np.cos(mod_eulAng[1]) * np.sin(mod_eulAng[2])
                 ]
    else:
        print("there is an error on arguments")
        return [None, None, None, None]


    return qList


def eul2quat2(eulAngleList, seq):
    # X_Roll, Y_Pitch, Z_Yaw: Rotation as ZYX, Yaw --> Pitch --> Roll    
"""R(y) = [[cos_y, -sin_y, 0.], [sin_y, cos_y, 0.], [0., 0., 1.]]         
R(p) = [[cos_p, 0., sin_p], [0., 1., 0.], [-sin_p, 0., cos_p]]         
R(r) = [[1., 0., 0.], [0, cos_r, -sin_r], [0., sin_r, cos_r]]         
dcm_bn = R(y)*R(p)*R(r)   """    
    if seq == 'ZYX':
        yaw = eulAngleList[0]
        pitch = eulAngleList[1]
        roll = eulAngleList[2]
    elif seq == 'XYZ':
        yaw = eulAngleList[2]
        pitch = eulAngleList[1]
        roll = eulAngleList[0]
    elif seq == 'ZYZ':
        yaw = eulAngleList[0]
        pitch = eulAngleList[1]
        roll = eulAngleList[2]
    elif seq == 'XYX':
        roll = eulAngleList[0]
        pitch = eulAngleList[1]
        yaw = eulAngleList[2]


    cos_y, sin_y = np.cos(yaw), np.sin(yaw)
    cos_p, sin_p = np.cos(pitch), np.sin(pitch)
    cos_r, sin_r = np.cos(roll), np.sin(roll)
    rot_y = np.array([[cos_y, -sin_y, 0.], [sin_y, cos_y, 0.], [0., 0., 1.]])
    rot_p = np.array([[cos_p, 0., sin_p], [0., 1., 0.], [-sin_p, 0., cos_p]])
    rot_r = np.array([[1., 0., 0.], [0, cos_r, -sin_r], [0., sin_r, cos_r]])
    rot_yy = np.array([[cos_r, -sin_r, 0.], [sin_r, cos_r, 0.], [0., 0., 1.]])
    rot_rr = np.array([[1., 0., 0.], [0, cos_y, -sin_y], [0., sin_y, cos_y]])

    if seq == 'ZYX':
        dcm_bn = rot_y @ rot_p @ rot_r
    elif seq == 'XYZ':
        dcm_bn = rot_r @ rot_p @ rot_y
    elif seq == 'ZYZ':
        dcm_bn = rot_y @ rot_p @ rot_yy
    elif seq == 'XYX':
        dcm_bn = rot_r @ rot_p @ rot_rr


    c11, c12, c13 = dcm_bn[0, 0], dcm_bn[0, 1], dcm_bn[0, 2]
    c21, c22, c23 = dcm_bn[1, 0], dcm_bn[1, 1], dcm_bn[1, 2]
    c31, c32, c33 = dcm_bn[2, 0], dcm_bn[2, 1], dcm_bn[2, 2]
    tr = c11 + c22 + c33
    q1 = 0.5 * np.sqrt(1 + tr)
    k2 = np.sqrt(0.25 * (2 * c11 + 1 - tr))
    k3 = np.sqrt(0.25 * (2 * c22 + 1 - tr))
    k4 = np.sqrt(0.25 * (2 * c33 + 1 - tr))

    if k2 >= k3 and k2 >= k4:
        q2 = k2 * np.sign(c32 - c23)
        q3 = k3 * np.sign(q2 * (c21 + c12))
        q4 = k4 * np.sign(q2 * (c31 + c13))
    elif k3 >= k2 and k3 >= k4:
        q3 = k3 * np.sign(c13 - c31)
        q2 = k2 * np.sign(q3 * (c12 + c21))
        q4 = k4 * np.sign(q3 * (c32 + c23))
    else:
        q4 = k4 * np.sign(c21 - c12)
        q2 = k2 * np.sign(q4 * (c13 + c31))
        q3 = k3 * np.sign(q4 * (c23 + c32))

    quat = np.array([q1, q2, q3, q4])

    return quat, dcm_bn


def quat2eul(qList, seq):
    #Euler Angle = (phi, theta, psi) as X, Y, Z    
    qw = qList[0]
    qx = qList[1]
    qy = qList[2]
    qz = qList[3]

    if seq == 'ZYX': #Z_psi, 0 -> Y_theta, 1 -> X_phi, 2 / unit is RAD!!!!!        
        aSinInput = -2 * (qx * qz - qw * qy)
        if aSinInput > 1:
            aSinInput = 1.        elif aSinInput < -1:
            aSinInput = -1.
        theta = np.arcsin(aSinInput)

        if (theta <= np.pi/2) & (theta >= -np.pi/2):
            eulList = np.array([np.arctan2(2 * (qx * qy + qw * qz), qw**2 + qx**2 - qy**2 - qz**2),\
                   np.arcsin(aSinInput), \
                   np.arctan2(2 * (qy * qz + qw * qx), qw**2 - qx**2 - qy**2 + qz**2)])

        elif (theta >= np.pi/2) & (theta <= 3*np.pi/2):
            eulList = np.array([np.arctan2(-2 * (qx * qy + qw * qz), -( qw**2 + qx**2 - qy**2 - qz**2 )),\
                   np.arcsin(aSinInput), \
                   np.arctan2(-2 * (qy * qz + qw * qx), -(qw**2 - qx**2 - qy**2 + qz**2))])

    elif seq == 'XYZ': #X_phi 0 -> Y_theta, 1 -> Z_psi, 2 / unit is RAD!!!!!        aSinInput = 2 * (qx * qz + qy * qw)

        if aSinInput > 1:
            aSinInput = 1.        elif aSinInput < -1:
            aSinInput = -1.
        eulList = np.array([np.arctan2(-2 * (qy * qz - qx * qw), qw**2 - qx**2 - qy**2 + qz**2),\
               np.arcsin(aSinInput),\
               np.arctan2(-2 * (qx * qy - qz * qw), qw**2 + qx**2 - qy**2 - qz**2)])


    #elif seq == 'ZYZ': #Z_phi 0 -> Y_theta, 1 -> Z_psi, 2 / unit is RAD!!!!!
    else:
        print("there is an error on arguments")
        return [None, None, None]

    eulList = np.real(eulList)

    return eulList

def RotMatQuat(quatList): #3 by 3    
#w: quatList[0], x: quatList[1], y: quatList[2], z: quatList[3]    
    w = quatList[0]
    x = quatList[1]
    y = quatList[2]
    z = quatList[3]

    RMat = [[w**2 + x**2 - y**2 - z**2, 2 * (x*y - w*z), 2*(x*z + w*y)],\
            [2*(x*y + w*z), w**2 - x**2 + y**2 - z**2, 2*(y*z -w*x)],\
            [2*(x*z - w*y), 2*(y*z +w*x), w**2 - x**2 - y**2 + z**2]]

    return RMat


def RotMatQuat2(q):
    dcm = [[1 - 2 * (q[2] ** 2 + q[3] ** 2), 2 * (q[1] * q[2] - q[0] * q[3]),            
    2 * (q[1] * q[3] + q[0] * q[2])],           
   [2 * (q[1] * q[2] + q[0] * q[3]), 1 - 2 * (q[1] ** 2 + q[3] ** 2),            
    2 * (q[2] * q[3] - q[0] * q[1])],           
   [2 * (q[1] * q[3] - q[0] * q[2]), 2 * (q[2] * q[3] + q[0] * q[1]),            
   1 - 2 * (q[1] ** 2 + q[2] ** 2)]]
    return np.array(dcm)

참고:
- https://blog.naver.com/jidon333/220889264929
- https://blog.naver.com/jidon333/220889335389

Comments

Post a Comment

Popular posts from this blog

AVR for Arduino/Atmel MCU

-
Image
AVR 코딩에 대한 이해가 필요해서 아래와 같이 정리를 했다. 1. AVR이란 Atmel에서 개발한 MCU의 등록 상표인데, 아두이노 우노 보드에 쓰여서 많이 쓰이고 있다. 2. 아두이노는 Atmel의 MCU를 메인 프로세서로 사용하고, 언어 특성은 C/C++ 이다. 하지만 AVR 코딩 특성을 일부 반영하여서 아두이노 보드를 고속으로 동작시킬 수 있다. 3. 즉 아두이노에서 사용되는 명령어 세트들은 아두이노 보드를 보다 고속으로 작동시킬 수 있도록 AVR 수준 내지는 어셈블리어 형태에 가까운 특별한 명령어들이 사용된다. 그래서 외부로부터의 인터럽트 처리과정 (ISR)과 각 핀들의 입출력 모드 설정 (pinMode) 및 입출력에 해당하는 명령어를 혼합 사용하여 아두이노 보드의 연산 속도를 끌어올릴 수 있다. 4. 핀 모드 설정 방식 - pinMode(pin#, OUTPUT); pinMode(pin#, INPUT); - PORT  아두이노 우노 보드의 PIN PORT는 아래와 같이 정의된다.     - PORTD: Digital pin 0~7 - PD0 ~ PD7     - PORTB: Digital pin 8~13 - PB0 ~ PB5     - 이렇게 PORTD, PORTB로 정의된 것은 C/C++에서 정의되는 것이 아니라, AVR에서 정의한 것이다. 5. 아두이노의 핀맵을 살펴보면서 설정을 어떻게 하는 지 보자. Arduino UNO Pin Map 위의 아두이노 핀맵에서 Digital Pin으로 설정된 0 ~ 13을 사용하려면, 4번의 방법으로 pinMode(pin, args)으로 미리 설정해주어야 한다. 참고로 이 pinMode는 "Arduino.h"에서 정의되어 이용이 가능한 것이다.  혹은 PORTD, PORTB를 이용해서 설정할 수도 있다. 참고로 이 PORTD, PORTB, DDRD, DDRB는 AVR에서 정의되는 ...
Read more

UART Communication

-
Image
- 로보틱스에서 액츄에이터(모터)와 통신을 할 때, 가장 많이 이용되는 serial communication이 UART 통신이다. MCU에서 UART (TTL Level) pin이 항상 붙어 있어서 쉽게 이용가능함. - MCU 단에서 나오는 UART RX, TX pin에 추가로 RS-232 혹은 RS-485의 moduel or IC를 붙여야 한다. 그냥 MCU 단에서 나오는 데이터는 Raw UART(?)이다. 보통 혼용해서 부르는 데, 확실히 485나 232로 구현하기 위해서는 driver를 붙여야함. - UART == TTL 로 생각하면 될 것 같다. 그리고 RS는 Recommended Standard이다. - UART로 daisy chain 통신을 구현할 수 있다. 하지만 1:N 통신은 RS-485로 구현하자. - UART 패킷 구조 - 9600 baud rate의 경우, start bit - 8 bits of data - no parity bit - 1 stop bit으로 구성된다. - 485로 Bulk read, bulk write을 구현가능 - 직렬 통신 방식이란? 데이터 비트를 1개의 비트 단위에 따라서 외부로 송신 혹은 수신하는 방식이다. 구현하기 쉽다. 멀리갈 수 있다. 기존의 통신선로를 쉽게 활용할 수 있다. 대표적으로 LAN, RS-232 등이 있다. - 많은 사람들이 비동기식 직렬 통신 방식을 RS-232라고 알고 있는 데, 실제로는 RS-232라는 것은 비동기식 통신 컨트롤러에서 나오는 디지털 신호를 외부와 연결시키는 전기적인 신호 방식의 하나일 뿐이다. 또한 UART는 RS-232와 voltage 만 제외하고 모두 같다. 아래와 같이 UART(TTL Level)보다 높은 전압으로 좀 더 먼거리의 통신이 가능하게 한다. UART: Logic 0 -> 0V / Logic 1 -> 3.3V or 5V RS-232: Logic 0 -> +12V (+3 to +25V) / Logic 1 -...
Read more

RS-485 Communication 구현

-
Image
RS-485 Communication Line 구현 1. 현재는 UART로 통신하는 기계를 Noise Robustness와 N:N Communication을 위해서 hardware와 software를 구현하는 과정을 적는다. 2. 메인 통신 모듈은 Teensy 3.5 (pin 버젼)이며, RS-485 Transceiver는 SP3485의 Breakout 모듈을 사용한다. 3. Digikey Order 내역 (1~2주 정도 걸림) - SP3485 Transceiver, RS-485 Interface Evaluation Board: 1568-1589-ND Datasheet: https://www.sparkfun.com/datasheets/Components/General/sp3485CN-LTR.pdf Schematics of the Module: https://cdn.sparkfun.com/datasheets/BreakoutBoards/RS485_Breakout_v10.pdf RS-485 모듈 SP3485 Schematics - Teensy 3.5: 1568-1464-ND Datasheet1: Find MK64FX512VMD12 at NXP Datasheet2: https://media.digikey.com/pdf/Data%20Sheets/Sparkfun%20PDFs/DEV-14056_Web.pdf Tutorials: https://www.pjrc.com/teensy/tutorial.html https://www.pjrc.com/teensy/pins.html https://www.pjrc.com/teensy/usb_debug_only.html Teensy 3.5 with Hearders Teensy 3.5 Pin Map (Frontside) Teensy 3.5 Pin Map (Backside) 4. pySerial + Arduino ...
Read more