旋转的表示转换与实现

旋转的表示

• 轴角 Angle/Axis
• 四元数 Quaternion
• 旋转矩阵
• 欧拉角

轴角 Angle/Axis

$\theta$单位是rad

$$Axis=(x,y,z),Angle=\theta$$

四元数 Quaternion

$$q=(q_w,q_x,q_y,q_z)$$

在实际使用中，不同库有不同的顺序
在Eigen 中

// Eigen::quaternion 有两种初始化方式，
// 方式1
Eigen::Vector4d q_(x,y,z,w)；
Eigen::quaterniond q(q_) ；
// 方式2
Eigen::quaterniond q(w,x,y,z);

在Scipy库中 顺序是 (x,y,z,w),以单位四元数为例

r = R.from_quat([[0, 0, 0, 1]])

在isaacsim 中 顺序是 (w,x,y,z)

旋转矩阵

$$R=\left[\begin{array}{ccc}{r}_{xx}& r_{xy}& r_{xz}\\ r_{yx}& r_{yy}& r_{yz}\\ r_{zx}& r_{zy}& r_{zz}\end{array}\right]$$

欧拉角

欧拉角有12组旋转顺序，内旋 外旋各六组，我们常用外旋的Z-Y-X 顺序
形成的旋转矩阵表示为

$$R=X(\varphi )Y(\theta )Z(\psi )$$

https://en.wikipedia.org/wiki/Euler_angles

转换与实现

Axis/Angle -> quaternion

对于以下Axis/Angle

$$Axis=(x,y,z)，Angle=\theta$$

到四元数的转换为

$$\begin{array}{rl}\mathbf{q}& =\left[\begin{array}{c}cos(\frac{\theta }{2})\\ x\ast sin(\frac{\theta }{2})\\ y\ast sin(\frac{\theta }{2})\\ z\ast sin(\frac{\theta }{2})\end{array}\right]\end{array}$$

rotate_matrix -> quaternion

$$\begin{array}{rl}{q}_w& =\frac{\sqrt{r_{xx}+r_{yy}+r_{zz}}}{2}\\ q_x& =\frac{r_{zy}-r_{yz}}{4q_w}\\ q_y& =\frac{r_{xz}-r_{zx}}{4q_w}\\ q_y& =\frac{r_{yx}-r_{xy}}{4q_w}\end{array}$$

# 将旋转矩阵转化为四元数

qw = np.sqrt(1.0+R[0, 0]+R[1, 1]+R[2, 2])/2.0

if(return_order == "wxyz"):
	q = np.array([qw,
	(R[2, 1]-R[1, 2])/(4*qw),
	(R[0, 2]-R[2, 0])/(4*qw),
	(R[1, 0]-R[0, 1])/(4*qw)])
else: # xyzw
	q = np.array([(R[2, 1]-R[1, 2])/(4*qw),
	(R[0, 2]-R[2, 0])/(4*qw),
	(R[1, 0]-R[0, 1])/(4*qw),
	qw])

return q

rotate_matrix -> euler

euler -> rotate_matrix

对于欧拉角(顺序 rpy/xyz)

$$(roll,pitch,yaw)$$

其旋转矩阵为

$$\begin{array}{rl}{\mathbf{R}}_x& =\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos (roll)& -\sin (roll)\\ 0& \sin (roll)& \cos (roll)\end{array}\right]\\ {\mathbf{R}}_x& =\left[\begin{array}{ccc}\cos (pitch)& 0& \sin (pitch)\\ 0& 1& 0\\ -\sin (pitch)& 0& \cos (pitch)\end{array}\right]\\ {\mathbf{R}}_x& =\left[\begin{array}{ccc}\cos (yaw)& -\sin (yaw)& 0\\ \sin (yaw)& \cos (yaw)& 0\\ 0& 0& 1\end{array}\right]\\ \mathbf{R}& ={\mathbf{R}}_x{\mathbf{R}}_y{\mathbf{R}}_z\end{array}$$

R_x = lambda roll : np.array([ [1 ,0 , 0 ],
[0 ,np.cos(roll) ,-np.sin(roll)],
[0 ,np.sin(roll) , np.cos(roll)]])

R_y = lambda pitch : np.array([[np.cos(pitch) , 0, np.sin(pitch)],
[0,1,0],
[-np.sin(pitch),0,np.cos(pitch)]])

R_z = lambda yaw : np.array([[np.cos(yaw) ,-np.sin(yaw), 0],
[np.sin(yaw),np.cos(yaw),0],
[0,0,1]])
if(order == "xyz" or order == "rpy"):
	R = np.dot(np.dot(R_x,R_y),R_z)

R = np.dot(np.dot(R_x,R_y),R_z) 表明了顺序

euler -> rotate_matrix

quaternion -> rotation_matrix

对于 四元数

$$q=(w,x,y,z)$$

旋转矩阵为

$$R=\left[\begin{array}{ccc}1-2\ast (y\ast y+z\ast z)& 2\ast (x\ast y-z\ast w)& 2\ast (x\ast z+y\ast w)\\ 2\ast (x\ast y+z\ast w)& 1-2\ast (x\ast x+z\ast z)& 2\ast (y\ast z-x\ast w)\\ 2\ast (x\ast z-y\ast w)& 2\ast (y\ast z+x\ast w)& 1-2\ast (x\ast x+y\ast y)\end{array}\right]$$

r00 = 1 - 2 * (y * y + z * z)
r01 = 2 * (x * y - z * w)
r02 = 2 * (x * z + y * w)
  
r10 = 2 * (x * y + z * w)
r11 = 1 - 2 * (x * x + z * z)
r12 = 2 * (y * z - x * w)

r20 = 2 * (x * z - y * w)
r21 = 2 * (y * z + x * w)
r22 = 1 - 2 * (x * x + y * y)

# 3x3 rotation matrix
rot_matrix = np.array([[r00, r01, r02],
[r10, r11, r12],
[r20, r21, r22]])

quaternion -> euler

对于 四元数

$$q_0=q_w,q_1=q_x,q_2=q_y,q_3=q_z$$

欧拉角为

$$\begin{array}{rl}\text{ roll }& ={\tan }^{-1}\left(\frac{2(q_0{q}_1+q_2{q}_3)}{q_0^2-q_1^2-q_2^2+q_3^2}\right)=\mathrm{atan}2[2(q_0{q}_1+q_2{q}_3),q_0^2-q_1^2-q_2^2+q_3^2]\\ \text{ pitch }& ={\sin }^{-1}\left(2(q_0{q}_2-q_1{q}_3)\right)=\mathrm{asin}\left[2(q_0{q}_2-q_1{q}_3)\right]\\ \text{ yaw }& ={\tan }^{-1}\left(\frac{2(q_0{q}_3+q_1{q}_2)}{q_0^2+q_1^2-q_2^2-q_3^2}\right)=\mathrm{atan}2[2(q_0{q}_3+q_1{q}_2),q_0^2+q_1^2-q_2^2-q_3^2]\end{array}$$

运算

四元数乘法

先执行 ${\mathbf{q}}_0$ ，在执行 ${\mathbf{q}}_1$ ，可用乘法表示，实现如下

def quaternion_multiply(quaternion0, quaternion1,quat_order="xyzw"):
	if(quat_order == "wxyz"):
		w0, x0, y0, z0 = quaternion0
		w1, x1, y1, z1 = quaternion1
	else: # xyzw
		x0, y0, z0,w0 = quaternion0
		x1, y1, z1,w1 = quaternion1
	return np.array([-x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
	x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
	-x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
	x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=np.float64)# wxyz

四元数等价与旋转矩阵，像矩阵乘法一样，四元数乘法满足结合律$((a\ast b)\ast c=a\ast (b\ast c))$ 但是不满足交换律 $a\ast b!=b\ast a$

四元数求逆

$${\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }=(q_0,-q_1,-q2,-q{3}_{)}$$

旋转矩阵求逆

旋转矩阵是一个 正交矩阵(orthogonal matrix),因此

$$R^{\mathsf{T}}=R^{-1}$$

参考

• https://zhuanlan.zhihu.com/p/45404840
• https://en.wikipedia.org/wiki/Rotation_matrix
• https://paroj.github.io/gltut/Positioning/Tut08 Quaternions.html#:~:text=If the two quaternions being,b*a ).
• https://danceswithcode.net/engineeringnotes/quaternions/quaternions.html#:~:text=Inverting or conjugating a rotation,it to its original location.