四元数与旋转矩阵

部分摘自 https://krasjet.github.io/quaternion/quaternion.pdf
里面对四元数和3D旋转的关系以及四元数乘法运算讲得很详细

四元数的介绍视频 https://www.bilibili.com/video/BV1SW411y7W1
https://www.bilibili.com/video/BV1Lt411U7og

四元数乘法python

import numpy as np

def qua_multi(qa,qb,m):
        #qa,qb均为四元数,[x,y,z,w],实轴在最后
    if(m==0):        #qb左乘qa
        qa_m=[[qa[3],-qa[2],qa[1],qa[0]],
        [qa[2],qa[3],-qa[0],qa[1]],
        [-qa[1],qa[0],qa[3],qa[2]],
        [-qa[0],-qa[1],-qa[2],qa[3]]]   

        qc=np.matmul(qa_m,qb)

    if(m==1):      
        #qb右乘qa
        qa_m=[[qa[3],qa[2],-qa[1],qa[0]],
        [-qa[2],qa[3],qa[0],qa[1]],
        [qa[1],-qa[0],qa[3],qa[2]],
        [-qa[0],-qa[1],-qa[2],qa[3]]]
        qc=np.matmul(qa_m,qb)
    return qc

已知末端姿态的四元数表示，求末端z轴的向量表示

#设姿态为q0(x,y,z,w),思路是[0,0,1]左乘q0,右乘q0的逆
q1=[0,0,1,0]
q2=qua_multi(q0,q1,0)     #q1左乘q0
q0_0=[-q0[0],-q0[1],-q0[2],q0[3]]
q3=qua_multi(q0_0,q2,1)    #右乘q0的逆，前提是将q0转化成单位四元数，否则求逆时还要在共轭的基础上除以模长的平方

已知末端姿态，求绕z轴旋转120度的新姿态

末端姿态四元数均是在基座标系下的表示，而z轴是动系的坐标轴

思路1 通过齐次旋转矩阵间接转化

def matrix_from_quaternion(quaternion, pos=None):
        #由四元数返回齐次旋转矩阵
    q = np.array(quaternion, dtype=np.float64, copy=True)
    q = q[[3, 0, 1, 2]]

    if pos is None: pos = np.zeros(3)

    n = np.dot(q, q)
    if n < 1e-20:
        return np.identity(4)
    q *= np.sqrt(2.0 / n)
    q = np.outer(q, q)
    return np.array([
        [1.0 - q[2, 2] - q[3, 3], q[1, 2] - q[3, 0], q[1, 3] + q[2, 0], pos[0]],
        [q[1, 2] + q[3, 0], 1.0 - q[1, 1] - q[3, 3], q[2, 3] - q[1, 0], pos[1]],
        [q[1, 3] - q[2, 0], q[2, 3] + q[1, 0], 1.0 - q[1, 1] - q[2, 2], pos[2]],
        [0.0, 0.0, 0.0, 1.0]])

def quaternion_from_matrix(matrix, isprecise=False):
    #旋转矩阵转四元数
    """
    LAYOUT : [X, Y, Z, W]

    If isprecise is True, the input matrix is assumed to be a precise rotation
    matrix and a faster algorithm is used.
    """
    M = np.array(matrix, dtype=np.float64, copy=False)[:4, :4]
    if isprecise:
        q = np.empty((4,))
        t = np.trace(M)
        if t > M[3, 3]:
            q[0] = t
            q[3] = M[1, 0] - M[0, 1]
            q[2] = M[0, 2] - M[2, 0]
            q[1] = M[2, 1] - M[1, 2]
        else:
            i, j, k = 1, 2, 3
            if M[1, 1] > M[0, 0]:
                i, j, k = 2, 3, 1
            if M[2, 2] > M[i, i]:
                i, j, k = 3, 1, 2
            t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3]
            q[i] = t
            q[j] = M[i, j] + M[j, i]
            q[k] = M[k, i] + M[i, k]
            q[3] = M[k, j] - M[j, k]
        q *= 0.5 / np.sqrt(t * M[3, 3])
    else:
        m00 = M[0, 0]
        m01 = M[0, 1]
        m02 = M[0, 2]
        m10 = M[1, 0]
        m11 = M[1, 1]
        m12 = M[1, 2]
        m20 = M[2, 0]
        m21 = M[2, 1]
        m22 = M[2, 2]
        # symmetric matrix K
        K = np.array([[m00 - m11 - m22, 0.0, 0.0, 0.0],
                    [m01 + m10, m11 - m00 - m22, 0.0, 0.0],
                    [m02 + m20, m12 + m21, m22 - m00 - m11, 0.0],
                    [m21 - m12, m02 - m20, m10 - m01, m00 + m11 + m22]])
        K /= 3.0
        # quaternion is eigenvector of K that corresponds to largest eigenvalue
        w, V = np.linalg.eigh(K)  #特征值和特征向量
        q = V[:, np.argmax(w)]
        if q[0] < 0.0:
            np.negative(q, q)
    return q

def go_to_orien(a): 
    #a为旋转的角度，q0为现在的姿态（四元数[x,y,z,w]）
    q0_now= quaternion.matrix_from_quaternion(q0)    #将现在姿态四元数转换成旋转矩阵
    matri_qd=[[math.cos(a*math.pi/180),-math.sin(a*math.pi/180),0,0],[math.sin(a*math.pi/180),math.cos(a*math.pi/180),0,0],[0,0,1,0],[0,0,0,1]]     #由给定角度计算齐次旋转矩阵
    qd_m= np.matmul(q0_now, matri_qd)     #绕动系坐标轴旋转，右乘
    qd = quaternion.quaternion_from_matrix(qd_m)   #得到目标姿态四元数

思路二 四元数乘法直接计算

def go_to_orien_test(a): 
    #a为旋转的角度，q0为现在的姿态（四元数[x,y,z,w]）

    q1=[0,0,1,0]
    q0=x0[3:]
    q2=qua_multi(q0,q1,0)
    q0_0=[-q0[0],-q0[1],-q0[2],q0[3]]
    q3=qua_multi(q0_0,q2,1)
    #得到现在的z轴在基座标系下的四元数表示[i,j,k,0]

    q1= np.empty((4,))
    q1[0]=math.sin(a*math.pi/180/2)*q3[0]
    q1[1]=math.sin(a*math.pi/180/2)*q3[1]
    q1[2]=math.sin(a*math.pi/180/2)*q3[2]
    q1[3]=math.cos(a*math.pi/180/2)               #绕z轴旋转a度的四元数表示 cos(theta/2)+sin(theta/2)(b*i+c*j+d*k)

    qd=qua_multi(q1,q0,0)       #只用左乘q1即可得到新的姿态四元数，一个点左乘q右乘q逆是得到点新的坐标（一定是纯四元数）

四元数计算公式

四元数乘法

$$
q_{1}=a+b i+c j+d k, \quad q_{2}=e+f i+g j+h k
$$
q2左乘q1
$$
q_{1} q_{2}=\left[\begin{array}{cccc}
a & -b & -c & -d \\
b & a & -d & c \\
c & d & a & -b \\
d & -c & b & a
\end{array}\right]\left[\begin{array}{l}
e \\
f \\
g \\
h
\end{array}\right]
$$
q2右乘q1
$$
q_{2} q_{1}=\left[\begin{array}{cccc}
a & -b & -c & -d \\
b & a & d & -c \\
c & -d & a & b \\
d & c & -b & a
\end{array}\right]\left[\begin{array}{l}
e \\
f \\
g \\
h
\end{array}\right]
$$

四元数距离

为实现姿态的优化和距离度量，需将四元数表达为三维笛卡尔空间的向量。将四元数q在无旋转四元数qI= [1, 0, 0, 0](identity quaternion)的切平面投影定义为四元数的对数

其中acos为修正后的反余弦函数:

一般基于四元数之间的距离设计目标函数，两个四元数q0和q1之间的距离定义为：

计算两个四元数之间的距离,python代码如下:
# 两四元数测地线角度,[0,pi]
def judge_qua_close(q0,q1):
    #input   q0,q1  (x,y,z,w)
    q11 = np.array([-q1[0],-q1[1],-q1[2],q1[3]])   #q1的逆
    mul = qua_multi(q11,q0,0)    #  q0 * q1逆
    logq = qua_log(mul)
    dis_qua = np.linalg.norm(logq) *2.0
    return dis_qua

def qua_multi(qa,qb,m):
    #qa,qb均为四元数,[x,y,z,w],实轴在最后
    if(m==0):        
        #qb左乘qa
        qa_m=[[qa[3],-qa[2],qa[1],qa[0]],
        [qa[2],qa[3],-qa[0],qa[1]],
        [-qa[1],qa[0],qa[3],qa[2]],
        [-qa[0],-qa[1],-qa[2],qa[3]]]   
        qc=np.matmul(qa_m,qb)

    if(m==1):      
        #qb右乘qa
        qa_m=[[qa[3],qa[2],-qa[1],qa[0]],
        [-qa[2],qa[3],qa[0],qa[1]],
        [qa[1],-qa[0],qa[3],qa[2]],
        [-qa[0],-qa[1],-qa[2],qa[3]]]
        qc=np.matmul(qa_m,qb)
    return qc

def qua_log(q0):
    #定义四元数的对数:
        # q在无旋转四元数[0,0,0,1]的切平面的投影定义为四元数的对数
        # input q(4,)  四元数 x y z w
        # output log(q) (3,)
    qv = np.array([q0[0],q0[1],q0[2]])

    logq  =np.zeros((3,))
    if (q0[0]**2+q0[1]**2+q0[2]**2) <1e-5:
        logq = np.zeros((3,))
    else:
        if q0[-1]<0 and q0[-1]>=-1:
            logq = (math.acos(q0[-1])-math.pi)*qv/np.linalg.norm(qv)
        if q0[-1]<=1 and q0[-1]>=0:
            logq = math.acos(q0[-1])*qv/np.linalg.norm(qv)
    return logq