01 什么是四元数

02 四元数与旋转矩阵、欧拉角

这里主要参考的是 右手系转左手系、旋转矩阵转四元数、四元数的两种表达（Hamilton/JPL)（这篇文章很棒，需要认真看哦~）

网上关于四元数有左手右手两种表达方式，有些地方标明了是在什么坐标系下的公式，而大多数地方是没有的。

所以如果“四元数转旋转矩阵”和“旋转矩阵转四元数”分别采用了在不同坐标系下的转换公式，将会出错。

在右手、左手坐标系下建立的四元数表示方式分别叫做四元数的两种表达：Hamilton/JPL

这两种表达的区别如下：
在这里插入图片描述
Hamilton对应右手系，JPL对应左手系

定义的差异直接导致计算公式的不同，但实际应用中只要统一标准应该问题不大。

Eigen ，Matlab ，ROS、Google Ceres Solver等都使用Hamilton四元数，而JPL则多用于航空航天领域。

2.1 公式（四元数与旋转矩阵）

Hamilton和JPL这两种表达，基本上就是相差一个符号的正负

2.1.1 四元数转旋转矩阵

Hamilton表达：
在这里插入图片描述

JPL表达：
在这里插入图片描述

2.1.1 旋转矩阵转四元数

Hamilton表达：

r 11 r11 r11等为姿态旋转矩阵中的元素

在这里插入图片描述
JPL表达：
在这里插入图片描述

2.2 公式（四元数与欧拉角）

在这里使用的是Hamilton（右手系）的
在这里插入图片描述

03 编程

3.1 c++

3.1.1 Hamilton表达

参考 基于OpenCV的四元数、旋转矩阵和欧拉角互相转换

  • 四元数转旋转矩阵
void getRotation(double *Quaternion, double *rt_mat)
{
  rt_mat[0] = 1 - 2 * (Quaternion[2] * Quaternion[2]) - 2 * (Quaternion[3] * Quaternion[3]);
  rt_mat[1] = 2 * Quaternion[1] * Quaternion[2] - 2 * Quaternion[0] * Quaternion[3];
  rt_mat[2] = 2 * Quaternion[1] * Quaternion[3] + 2 * Quaternion[0] * Quaternion[2];
  rt_mat[3] = 2 * Quaternion[1] * Quaternion[2] + 2 * Quaternion[0] * Quaternion[3];
  rt_mat[4] = 1 - 2 * (Quaternion[1] * Quaternion[1]) - 2 * (Quaternion[3] * Quaternion[3]);
  rt_mat[5] = 2 * Quaternion[2] * Quaternion[3] - 2 * Quaternion[0] * Quaternion[1];
  rt_mat[6] = 2 * Quaternion[1] * Quaternion[3] - 2 * Quaternion[0] * Quaternion[2];
  rt_mat[7] = 2 * Quaternion[2] * Quaternion[3] + 2 * Quaternion[0] * Quaternion[1];
  rt_mat[8] = 1 - 2 * (Quaternion[1] * Quaternion[1]) - 2 * (Quaternion[2] * Quaternion[2]);
}
  • 旋转矩阵转四元数
void getQuaternion(Mat R, double Q[])
{
    double trace = R.at<double>(0,0) + R.at<double>(1,1) + R.at<double>(2,2);
 
    if (trace > 0.0) 
    {
        double s = sqrt(trace + 1.0);
        Q[3] = (s * 0.5);
        s = 0.5 / s;
        Q[0] = ((R.at<double>(2,1) - R.at<double>(1,2)) * s);
        Q[1] = ((R.at<double>(0,2) - R.at<double>(2,0)) * s);
        Q[2] = ((R.at<double>(1,0) - R.at<double>(0,1)) * s);
    } 
    
    else 
    {
        int i = R.at<double>(0,0) < R.at<double>(1,1) ? (R.at<double>(1,1) < R.at<double>(2,2) ? 2 : 1) : (R.at<double>(0,0) < R.at<double>(2,2) ? 2 : 0); 
        int j = (i + 1) % 3;  
        int k = (i + 2) % 3;

        double s = sqrt(R.at<double>(i, i) - R.at<double>(j,j) - R.at<double>(k,k) + 1.0);
        Q[i] = s * 0.5;
        s = 0.5 / s;

        Q[3] = (R.at<double>(k,j) - R.at<double>(j,k)) * s;
        Q[j] = (R.at<double>(j,i) + R.at<double>(i,j)) * s;
        Q[k] = (R.at<double>(k,i) + R.at<double>(i,k)) * s;
    }
}

3.1.1 JPL表达

参考：从旋转矩阵到四元数

  • 旋转矩阵转四元数
void getQuaternion(cv::Mat R, double Q[])//JPL
{
	double m11=R.at<double>(0,0), m12 = R.at<double>(0, 1), m13 = R.at<double>(0, 2);
	double m21 = R.at<double>(1, 0), m22 = R.at<double>(1, 1), m23 = R.at<double>(1, 2);
	double m31 = R.at<double>(2, 0), m32 = R.at<double>(2, 1), m33 = R.at<double>(2, 2);

	double w, x, y, z;

	//探测四元数中最大的项 
	double fourWSquaredMinusl = m11 + m22 + m33;
	double fourXSquaredMinusl = m11 - m22 - m33;
	double fourYSquaredMinusl = m22 - m11 - m33;
	double fourZSquaredMinusl = m33 - m11 - m22;

	int biggestIndex = 0;
	double fourBiggestSqureMinus1 = fourWSquaredMinusl;
	if (fourXSquaredMinusl > fourBiggestSqureMinus1) {
		fourBiggestSqureMinus1 = fourXSquaredMinusl;
		biggestIndex = 1;
	}
	if (fourYSquaredMinusl > fourBiggestSqureMinus1) {
		fourBiggestSqureMinus1 = fourYSquaredMinusl;
		biggestIndex = 2;
	}
	if (fourZSquaredMinusl > fourBiggestSqureMinus1) {
		fourBiggestSqureMinus1 = fourZSquaredMinusl;
		biggestIndex = 3;
	}

	//计算平方根和除法 
	double biggestVal = sqrt(fourBiggestSqureMinus1 + 1.0f) * 0.5f;
	double mult = 0.25f / biggestVal;

	//计算四元数的值
	switch (biggestIndex) {
	case 0:
		w = biggestVal;
		x = (m23 - m32) * mult;
		y = (m31 - m13) * mult;
		z = (m12 - m21) * mult;
		break;
	case 1:
		x = biggestVal;
		w = (m23 - m32) * mult;
		y = (m12 + m21) * mult;
		z = (m31 + m13) * mult;
		break;
	case 2:
		y = biggestVal;
		w = (m31 - m13) * mult;
		x = (m12 + m21) * mult;
		z = (m23 + m32) * mult;
		break;
	case 3:
		z = biggestVal;
		w = (m12 - m21) * mult;
		x = (m31 + m13) * mult;
		y = (m23 + m32) * mult;
		break;
	}
	Q[0] = x;
	Q[1] = y;
	Q[2] = z;
	Q[3] = w;
}

3.2 matlab

3.2.1 Hamilton表达

  • 四元数转旋转矩阵
function r=q2rot(q)
%   q是从控制器或示教器中读取的向量[q1 q2 q3 q4]；
    w=q(1);
    x=q(2);
    y=q(3);
    z=q(4);

    r=zeros(3,3);
    r(1,1)=1-2*y*y-2*z*z;
    r(1,2)=2*x*y+2*w*z;
    r(1,3)=2*x*z-2*w*y;

    r(2,1)=2*x*y-2*w*z;
    r(2,2)=1-2*x*x-2*z*z;
    r(2,3)=2*z*y+2*w*x;

    r(3,1)=2*x*z+2*w*y;
    r(3,2)=2*y*z-2*w*x;
    r(3,3)=1-2*x*x-2*y*y;
    r=r';
end
  • 欧拉角转旋转矩阵
function H = Eular2Rot( EX,EY,EZ,TransVector )
%   EX,EY,EZ,TransVector是机器手坐标系（或工具坐标系）在基坐标系（世界坐标系或者工件坐标系也可）里面的位置和姿态变换参数，需要从机器人控制器或示教盒中读取
%   EX--机器手坐标系绕基坐标系的X轴旋转欧拉角 
%   EY--绕Y轴旋转欧拉角 
%   EZ--绕Z轴旋转欧拉角
%   TransVector--机器手坐标系原点相对基坐标系的平移向量:3x1
    a=EX/180 *pi;
    b=EY/180 *pi;
    r=EZ/180 *pi;
    Rx=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
    Ry=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
    Rz=[cos(r) -sin(r) 0;sin(r) cos(r) 0;0 0 1];
    R=Rz*Ry*Rx;
    H=[R TransVector;0 0 0 1];
end

3.3 python

3.3.1 Hamilton表达和JPL表达

from math import pi, sqrt
import numpy as np

# 注：Hamilton为右手系
#    JPL     为左手系

def quat_to_pos_matrix_hm(p_x, p_y, p_z, x, y, z, w):
    # 创建位姿矩阵，写入位置
    T = np.matrix([[0, 0, 0, p_x], [0, 0, 0, p_y], [0, 0, 0, p_z], [0, 0, 0, 1]])
    T[0, 0] = 1 - 2 * pow(y, 2) - 2 * pow(z, 2)
    T[0, 1] = 2 * (x * y - w * z)
    T[0, 2] = 2 * (x * z + w * y)

    T[1, 0] = 2 * (x * y + w * z)
    T[1, 1] = 1 - 2 * pow(x, 2) - 2 * pow(z, 2)
    T[1, 2] = 2 * (y * z - w * x)

    T[2, 0] = 2 * (x * z - w * y)
    T[2, 1] = 2 * (y * z + w * x)
    T[2, 2] = 1 - 2 * pow(x, 2) - 2 * pow(y, 2)
    return T
    # return T.tolist()


def quat_to_pos_matrix_JPL(p_x, p_y, p_z, x, y, z, w):
    # 创建位姿矩阵，写入位置
    T = np.matrix([[0, 0, 0, p_x], [0, 0, 0, p_y], [0, 0, 0, p_z], [0, 0, 0, 1]])
    T[0, 0] = 1 - 2 * pow(y, 2) - 2 * pow(z, 2)
    T[0, 1] = 2 * (x * y + w * z)
    T[0, 2] = 2 * (x * z - w * y)

    T[1, 0] = 2 * (x * y - w * z)
    T[1, 1] = 1 - 2 * pow(x, 2) - 2 * pow(z, 2)
    T[1, 2] = 2 * (y * z + w * x)

    T[2, 0] = 2 * (x * z + w * y)
    T[2, 1] = 2 * (y * z - w * x)
    T[2, 2] = 1 - 2 * pow(x, 2) - 2 * pow(y, 2)
    return T
    # return T.tolist()


def pos_matrix_to_quat_hm(T):
    r11 = T[0, 0]
    r12 = T[0, 1]
    r13 = T[0, 2]
    r21 = T[1, 0]
    r22 = T[1, 1]
    r23 = T[1, 2]
    r31 = T[2, 0]
    r32 = T[2, 1]
    r33 = T[2, 2]
    w = (1 / 2) * sqrt(1 + r11 + r22 + r33)
    x = (r32 - r23) / (4 * w)
    y = (r13 - r31) / (4 * w)
    z = (r21 - r12) / (4 * w)
    return x, y, z, w


def pos_matrix_to_quat_JPL(T):
    r11 = T[0, 0]
    r12 = T[0, 1]
    r13 = T[0, 2]
    r21 = T[1, 0]
    r22 = T[1, 1]
    r23 = T[1, 2]
    r31 = T[2, 0]
    r32 = T[2, 1]
    r33 = T[2, 2]
    w = (1 / 2) * sqrt(1 + r11 + r22 + r33)
    x = (r23 - r32) / (4 * w)
    y = (r31 - r13) / (4 * w)
    z = (r12 - r21) / (4 * w)
    return x, y, z, w


if __name__ == "__main__":
    # position:
    p_x = 0.0
    p_y = 0.0881141967773
    p_z = -0.0655107116699

    # orientation:
    x = -0.181168104759
    y = -0.0779586342595
    z = -0.385540513457
    w = 0.901365121161

    T_hm = quat_to_pos_matrix_hm(p_x, p_y, p_z, x, y, z, w)
    print("T_hm")
    print(T_hm)

    T_JPL = quat_to_pos_matrix_JPL(p_x, p_y, p_z, x, y, z, w)
    print("T_JPL")
    print(T_JPL)

    x, y, z, w = pos_matrix_to_quat_hm(T_hm,)
    print("q_hm")
    print(x, y, z, w)

    x, y, z, w = pos_matrix_to_quat_JPL(T_JPL)
    print("q_JPL")
    print(x, y, z, w)

    TT1 = np.matrix([[0.6906, 0.7233, -0.0008, p_x], [-0.6668, 0.6371, 0.3867, p_y], [0.2802, -0.2665, 0.9222, p_z], [0, 0, 0, 1]])
    x, y, z, w = pos_matrix_to_quat_hm(TT1)
    print("q")
    print(x, y, z, w)

3.3.2 python的一些语法

3.3.2.1 ptyhon计算平方

方法一：使用内置模块

>>> import math 
>>> math.pow(12, 2)     # 求平方
144.0

方法二：使用表达式

>>> 12 ** 2             # 求平方
144

方法三：使用内置函数

>>> pow(12, 2)          # 求平方
144
3.3.2.2 ptyhon计算平方根

方法一：

num_sqrt = num ** 0.5

方法二：

import cmath
 
num_sqrt = cmath.sqrt(num)
3.3.2.3 矩阵定义和访问 & 矩阵和列表的转换
import numpy as np
# 定义
T = np.matrix([[1,0,0,0],[0,1,-1,0],[0,1,1,0],[0,0,0,1]])
# 访问
a = T[0, 0]

# 转为list
T2 = T.tolist()
# 转为list的访问
a = T[0][0]

注意:

矩阵的访问是T[0, 0]
把矩阵转为list的访问是T[0][0]

矩阵转换为列表 L=M.tolist()
列表转换为矩阵 M=mat(L)

矩阵、数组、列表的两两转换参考：https://blog.csdn.net/m0_37477175/article/details/72828311

04 有关四元数和旋转矩阵相互转换的一些参考

四元数 q = s + x i + y j + z k q=s+xi+yj+zk q=s+xi+yj+zk
构造纯四元数后（即 p ′ = q p q − 1 p' = q p q^{-1} p′=qpq−1）

在这里插入图片描述
或
在这里插入图片描述
在这里插入图片描述

推荐内容
阅读全文
AI总结
# 矩阵 # matlab # 线性代数
Logo
GitCode 开源社区

旨在为数千万中国开发者提供一个无缝且高效的云端环境，以支持学习、使用和贡献开源项目。

更多推荐

cover

大学生摸鱼暂停！G-Star 校园开发者计划，喊你上车啦?

avatar GitCode 开源社区
cover

G-Star 校园开发者计划·黑科大｜开源第一课之 Git 入门

avatar GitCode 开源社区
cover

小胰宝 PancrePal 强势加入 GitCode！开启开源抗癌新纪元

avatar GitCode 开源社区