三维空间刚体运动表示转换

• 前言
• 1. 旋转矩阵
• • 1.1 旋转矩阵与旋转向量
  • 1.2. 旋转矩阵与欧拉角
• 2. 四元数
• • 2.1. 四元数与旋转向量
  • 2.2. 四元数与旋转矩阵
  • 2.3. 四元数与欧拉角
• 参考链接

前言

\quad 三维空间刚体运动由旋转及平移构成。其中平移普遍利用向量表示，而旋转则有旋转矩阵、旋转向量、欧拉角、四元数等表示方法，这四种方法的优缺点已属老生常谈，本文整理常见表示方法之间的转换关系及其实现方式，便于日常开发应用。

1. 旋转矩阵

1.1 旋转矩阵与旋转向量

\quad 旋转矩阵 R \mathbf{R} R 与旋转向量 θ n \theta\mathbf{n} θn 间通过Rodrigues（罗德里格斯）公式进行转换。旋转向量转换旋转矩阵可表示为：

\quad 上文中式（1）和式（2）两种表达形式等价，具体推导过程可参考如下博文：

• 罗德里格斯公式 理解、推导
• 旋转矩阵罗德里格斯公式（Rodriguez formula）的理解证明

\quad 旋转矩阵转换为旋转向量可表示为：

\quad 上文中，式（3）中 t r ( R ) tr(R) tr(R) 表示矩阵R的迹，即为对角线元素之和。式（6）和式（7）均可用于求解旋转向量，其中式（7）即为求解矩阵 R \mathbf{R} R的特征值1对应的特征向量。
\quad 旋转矩阵与旋转向量间的转换，可借助于opencv的函数实现，或利用实现如下：

//[3] 旋转矩阵转旋转向量
void RotMat2RotVec(cv::Mat& rot, Vec3f &vec)
{
	// 可直接调用opencv Rodrigues函数
	//cv::Rodrigues(rot, vec);
	float theta = acos((trace(rot)[0] - 1.0) / 2);

	Mat N = (rot - rot.t()) / (2 * sin(theta));

	vec[0] = theta * N.at<float>(2, 1);
	vec[1] = theta * N.at<float>(0, 2);
	vec[2] = theta * N.at<float>(1, 0);
}

//[4] 旋转向量转旋转矩阵
void RotVec2RotMat( Vec3f &vec, cv::Mat& rot)
{
	// 可直接调用opencv Rodrigues函数
	//Rodrigues(vec, rot);

	float theta = norm(vec);
	Vec3f norm_vec = vec / theta;

	Mat N = (Mat_<float>(3, 3) <<
		0,				-norm_vec[2],	norm_vec[1],
		norm_vec[2],	0,				-norm_vec[0],
		-norm_vec[1],	norm_vec[0],	0
		);

	rot = Mat::eye(Size(3, 3), CV_32FC1) + sin(theta) *N + (1 - cos(theta))*N*N;
}

1.2. 旋转矩阵与欧拉角

\quad 欧拉角转化为旋转矩阵遵循如下公式：

\quad 基于上式可得出如下表达：
R = [ c o s θ y c o s θ z s i n θ x s i n θ y c o s θ z − c o s θ x s i n θ z c o s θ x s i n θ y c o s θ z + s i n θ x s i n θ z c o s θ y s i n θ z s i n θ x s i n θ y s i n θ z + c o s θ x c o s θ z c o s θ x s i n θ y s i n θ z − s i n θ x c o s θ z − s i n θ y s i n θ x c o s θ y c o s θ x c o s θ y ] \mathbf{R} = \begin{bmatrix} cos\theta_ycos\theta_z & sin\theta_xsin\theta_ycos\theta_z-cos\theta_xsin\theta_z & cos\theta_xsin\theta_ycos\theta_z+sin\theta_xsin\theta_z & \\ cos\theta_ysin\theta_z & sin\theta_xsin\theta_ysin\theta_z+cos\theta_xcos\theta_z & cos\theta_xsin\theta_ysin\theta_z- sin\theta_xcos\theta_z\\ -sin\theta_y & sin\theta_xcos\theta_y & cos\theta_xcos\theta_y\end{bmatrix} R=⎣⎡​cosθy​cosθz​cosθy​sinθz​−sinθy​​sinθx​sinθy​cosθz​−cosθx​sinθz​sinθx​sinθy​sinθz​+cosθx​cosθz​sinθx​cosθy​​cosθx​sinθy​cosθz​+sinθx​sinθz​cosθx​sinθy​sinθz​−sinθx​cosθz​cosθx​cosθy​​⎦⎤​
\quad 由此可得出，旋转向量转换为欧拉角遵循如下关系：
θ x = a r c t r a n ( r 21 / r 22 ) θ y = a r c t r a n ( − r 00 / s q r t ( r 10 2 + r 00 2 ) ) θ z = a r c t r a n ( r 10 / r 00 ) \theta_x = arctran(r_{21}/r_{22}) \\ \theta_y = arctran(-r_{00}/sqrt(r_{10}^2 +r_{00}^2 ))\\ \theta_z = arctran(r_{10}/r_{00}) θx​=arctran(r21​/r22​)θy​=arctran(−r00​/sqrt(r102​+r002​))θz​=arctran(r10​/r00​)
\quad 转换代码实现如下：

//[1] 旋转矩阵转换为欧拉角
void RotMat2EulerAngle(cv::Mat& rot, Vec3f &euler)
{
	// 校验绕y轴的旋转是否为±90°, 若是, 则属于万向锁现象, 等效为两次旋转
	float sy = sqrt(rot.at<float>(0, 0) * rot.at<float>(0, 0) + rot.at<float>(1, 0) * rot.at<float>(1, 0));
	bool singular = sy < 1e-6;

	if (!singular)
	{
		euler[0] = atan2(rot.at<float>(2, 1), rot.at<float>(2, 2));
		euler[1] = atan2(-rot.at<float>(2, 0), sy);
		euler[2] = atan2(rot.at<float>(1, 0), rot.at<float>(0, 0));
	}
	else
	{
		euler[0] = atan2(-rot.at<float>(1, 2), rot.at<float>(1, 1));
		euler[1] = atan2(-rot.at<float>(2, 0), sy);
		euler[2] = 0;
	}
}

//[2] 欧拉角转换为旋转矩阵
void EulerAngle2RotMat(Vec3f &euler, cv::Mat& rot)
{
	Mat Rx = (Mat_<float>(3, 3) <<
		1, 0, 0,
		0, cos(euler[0]), -sin(euler[0]),
		0, sin(euler[0]), cos(euler[0])
		);

	Mat Ry = (Mat_<float>(3, 3) <<
		cos(euler[1]), 0, sin(euler[1]),
		0, 1, 0,
		-sin(euler[1]), 0, cos(euler[1])
		);

	Mat Rz = (Mat_<float>(3, 3) <<
		cos(euler[2]), -sin(euler[2]), 0,
		sin(euler[2]), cos(euler[2]), 0,
		0, 0, 1
		);

	rot = Rz * Ry * Rx;
}

2. 四元数

\quad 为什么可以利用四元数 q \mathbf{q} q描述三维空间的旋转？该问题可以类比“利用复数描述二维平面内的旋转”。对于该理解的详细描述，推荐参考博客：基于四元数的旋转理解。这里引用一下该博客内的一个结论：
\quad 四元数本身是一个四维向量，利用四元数定义的旋转 p ′ = q p \mathbf{p'= qp} p′=qp本身就应该是四维旋转，但这个四维旋转可以拆分为一个左旋转 q L \mathbf{q_L} qL​与一个右旋转 q R \mathbf{q_R} qR​，即 p ′ = q L p q R \mathbf{p'= q_Lpq_R } p′=qL​pqR​。而在使用过程中为什么将 q L \mathbf{q_L} qL​表示为 [ c o s ( θ 2 ) , s i n ( θ 2 ) n x , s i n ( θ 2 ) n y , s i n ( θ 2 ) n z ] \mathbf{[cos(\frac{\theta}{2}), sin(\frac{\theta}{2})n_x, sin(\frac{\theta}{2})n_y , sin(\frac{\theta}{2})n_z]} [cos(2θ​),sin(2θ​)nx​,sin(2θ​)ny​,sin(2θ​)nz​]， q R = q L − 1 \mathbf{q_R = {q_L}^{-1}} qR​=qL​−1，则是由于需要将该旋转限定在3维超平面内，引用博客中解释：

2.1. 四元数与旋转向量

\quad 基于上述理解，四元数（特指单位四元数，下同）转旋转向量表示为如下形式，反之易得旋转向量至四元数的转换关系：

\quad 二者转换代码实现如下：

typedef struct _Quaternionf_
{
	float w;	// q0
	float x;	// q1
	float y;	// q2
	float z;	// q3
}Quaternionf;

//[5] 旋转向量转四元数
void RotVec2Quad(Vec3f &vec, Quaternionf &q)
{
	float theta = norm(vec);
	float qtheta = theta/2;
	q.w = cos(qtheta);
	q.x = sin(qtheta) * vec[0] / theta;
	q.y = sin(qtheta) * vec[1] / theta;
	q.z = sin(qtheta) * vec[2] / theta;
}

//[6] 四元数转旋转向量
void Quad2RotVec(Quaternionf &q, Vec3f &vec)
{
	float theta = 2 * acos(q.w);
	float sint = sin(theta / 2);

	float nx = q.x / sint;
	float ny = q.y / sint;
	float nz = q.z / sint;
	
	vec[0] = theta * nx;
	vec[1] = theta * ny;
	vec[2] = theta * nz;
}

2.2. 四元数与旋转矩阵

\quad 四元数转旋转矩阵公式如下，其推导过程可参考高博《SLAM十四讲》3.4.4节，大致思路是将四元数的旋转表示： p ′ = q L p q R \mathbf{p'= q_Lpq_R } p′=qL​pqR​，写作矩阵形式即可得到下述结果。

\quad 而旋转矩阵转四元数的思路，与转欧拉角的思路类似，都是从旋转矩阵的元素构成出发，构建方程求解四元数的元素：
t r ( R ) = 3 − 4 ( q 1 2 + q 2 2 + q 3 2 ) = 4 q 0 2 − 1 q 0 = t r ( R ) + 1 2 q 1 = r 32 − r 23 4 q 0 q 2 = r 13 − r 31 4 q 0 q 3 = r 21 − r 12 4 q 0 tr(R) = 3-4(q_1^2+q_2^2+q_3^2) = 4q_0^2 -1\\ q_0 = \frac{\sqrt{tr(R)+1}}{2}\\ q_1 = \frac{r_{32}- r_{23}}{4q_0}\\ q_2 = \frac{r_{13}- r_{31}}{4q_0}\\ q_3 = \frac{r_{21}- r_{12}}{4q_0} tr(R)=3−4(q12​+q22​+q32​)=4q02​−1q0​=2tr(R)+1 ​​q1​=4q0​r32​−r23​​q2​=4q0​r13​−r31​​q3​=4q0​r21​−r12​​
\quad 需注意的是，上式不适用于 q 0 q_0 q0​接近0的情况，因此需要构造其他形式的求解方式，在此不做赘述，具体可参考博客：旋转矩阵、欧拉角、四元数理论及其转换关系
\quad 二者转换的代码相对简单，时间关系不在此给出，日后有时间再行补充。

2.3. 四元数与欧拉角

\quad 由欧拉角转四元数的思路，与欧拉角转旋转矩阵类似，其表达形式如下：
q = q z q y q x q x = [ c o s ( θ x 2 ) , s i n ( θ x 2 ) , 0 , 0 ] q y = [ c o s ( θ y 2 ) , 0 , s i n ( θ y 2 ) , 0 ] q z = [ c o s ( θ z 2 ) , 0 , 0 , s i n ( θ z 2 ) ] q = q_z q_yq_x \\ q_x = \begin{bmatrix} cos(\frac{\theta_x}{2}), sin(\frac{\theta_x}{2}), 0, 0 \end{bmatrix} \\ q_y = \begin{bmatrix} cos(\frac{\theta_y}{2}), 0, sin(\frac{\theta_y}{2}), 0 \end{bmatrix} \\ q_z = \begin{bmatrix} cos(\frac{\theta_z}{2}), 0, 0, sin(\frac{\theta_z}{2}) \end{bmatrix} q=qz​qy​qx​qx​=[cos(2θx​​),sin(2θx​​),0,0​]qy​=[cos(2θy​​),0,sin(2θy​​),0​]qz​=[cos(2θz​​),0,0,sin(2θz​​)​]
\quad 由此可得，最终表达式如下：
q = [ c o s ( θ x 2 ) c o s ( θ y 2 ) c o s ( θ z 2 ) + s i n ( θ x 2 ) s i n ( θ y 2 ) s i n ( θ z 2 ) s i n ( θ x 2 ) c o s ( θ y 2 ) c o s ( θ z 2 ) − c o s ( θ x 2 ) s i n ( θ y 2 ) s i n ( θ z 2 ) c o s ( θ x 2 ) s i n ( θ y 2 ) c o s ( θ z 2 ) + s i n ( θ x 2 ) c o s ( θ y 2 ) s i n ( θ z 2 ) c o s ( θ x 2 ) c o s ( θ y 2 ) s i n ( θ z 2 ) − s i n ( θ x 2 ) s i n ( θ y 2 ) c o s ( θ z 2 ) ] \mathbf{q = } \begin{bmatrix} cos(\frac{\theta_x}{2})cos(\frac{\theta_y}{2})cos(\frac{\theta_z}{2}) + sin(\frac{\theta_x}{2})sin(\frac{\theta_y}{2})sin(\frac{\theta_z} {2}) \\ sin(\frac{\theta_x}{2})cos(\frac{\theta_y}{2})cos(\frac{\theta_z}{2}) - cos(\frac{\theta_x}{2})sin(\frac{\theta_y}{2})sin(\frac{\theta_z} {2}) \\ cos(\frac{\theta_x}{2})sin(\frac{\theta_y}{2})cos(\frac{\theta_z}{2}) + sin(\frac{\theta_x}{2})cos(\frac{\theta_y}{2})sin(\frac{\theta_z} {2}) \\ cos(\frac{\theta_x}{2})cos(\frac{\theta_y}{2})sin(\frac{\theta_z}{2}) - sin(\frac{\theta_x}{2})sin(\frac{\theta_y}{2})cos(\frac{\theta_z} {2}) \\ \end{bmatrix} q=⎣⎢⎢⎢⎡​cos(2θx​​)cos(2θy​​)cos(2θz​​)+sin(2θx​​)sin(2θy​​)sin(2θz​​)sin(2θx​​)cos(2θy​​)cos(2θz​​)−cos(2θx​​)sin(2θy​​)sin(2θz​​)cos(2θx​​)sin(2θy​​)cos(2θz​​)+sin(2θx​​)cos(2θy​​)sin(2θz​​)cos(2θx​​)cos(2θy​​)sin(2θz​​)−sin(2θx​​)sin(2θy​​)cos(2θz​​)​⎦⎥⎥⎥⎤​
\quad 从四元数转欧拉角的思路与此前一致，仍然从元素构成出发，但由于其推导复杂度较高，建议以旋转矩阵作为中转。

参考链接

[1] https://www.3dgep.com/understanding-quaternions/#Introduction
[2] https://ntrs.nasa.gov/citations/19770019231
[3] https://zhuanlan.zhihu.com/p/45404840

标签：cos,frac,旋转,四元,三维空间,theta,运动,sin,刚体
来源： https://blog.csdn.net/wanlvby/article/details/122024286