旋转的表示转换与实现

旋转的表示

  • 轴角 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 = \begin{bmatrix} r_{xx} & r_{xy} & r_{xz} \\ r_{yx} & r_{yy} & r_{yz} \\ r_{zx} & r_{zy} & r_{zz} \end{bmatrix} \]

欧拉角

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

\[R=X(\phi)Y(\theta)Z(\psi) \]

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

转换与实现

Axis/Angle -> quaternion

对于以下Axis/Angle

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

到四元数的转换为

\[\begin{aligned} \mathbf{q}&=\begin{bmatrix} cos(\frac{\theta}{2})\\ x*sin(\frac{\theta}{2})\\ y*sin(\frac{\theta}{2})\\ z*sin(\frac{\theta}{2})\\ \end{bmatrix} \end{aligned} \]

rotate_matrix -> quaternion

\[\begin{aligned} 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{aligned} \]

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

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{aligned} \mathbf{R}_x &= \begin{bmatrix} 1 & 0 &0 \\ 0 & \cos{(roll)} & -\sin{(roll)} \\ 0 & \sin{(roll)} & \cos{(roll)} \end{bmatrix}\\ \mathbf{R}_x &= \begin{bmatrix} \cos{(pitch)} & 0 &\sin{(pitch)} \\ 0 & 1 &0 \\ -\sin{(pitch)} & 0 & \cos{(pitch)} \end{bmatrix}\\ \mathbf{R}_x &= \begin{bmatrix} \cos{(yaw)} & -\sin{(yaw)} & 0 \\ \sin{(yaw)} & \cos{(yaw)} & 0 \\ 0 &0 & 1 \\ \end{bmatrix}\\ \mathbf{R} &= \mathbf{R}_x\mathbf{R}_y\mathbf{R}_z \end{aligned} \]

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 = \begin{bmatrix} 1 - 2 * (y * y + z * z) & 2 * (x * y - z * w) & 2 * (x * z + y * w) \\ 2 * (x * y + z * w) & 1 - 2 * (x * x + z * z) & 2 * (y * z - x * w) \\ 2 * (x * z - y * w) & 2 * (y * z + x * w) & 1 - 2 * (x * x + y * y) \end{bmatrix} \]

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{aligned} \text { roll }&=\tan ^{-1}\left(\frac{2\left(q_{0} q_{1}+q_{2} q_{3}\right)}{q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2}}\right)=\operatorname{atan} 2\left[2\left(q_{0} q_{1}+q_{2} q_{3}\right), q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2}\right] \\ \text { pitch }&=\sin ^{-1}\left(2\left(q_{0} q_{2}-q_{1} q_{3}\right)\right)=\operatorname{asin}\left[2\left(q_{0} q_{2}-q_{1} q_{3}\right)\right] \\ \text { yaw }&=\tan ^{-1}\left(\frac{2\left(q_{0} q_{3}+q_{1} q_{2}\right)}{q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2}}\right)=\operatorname{atan} 2\left[2\left(q_{0} q_{3}+q_{1} q_{2}\right), q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2}\right] \end{aligned} \]

运算

四元数乘法

先执行 \(\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*b) * c = a * (b*c) )\) 但是不满足交换律 \(  a*b != b*a \)

四元数求逆

\[\mathbf{q}^{−1} = \mathbf{q}^* = ( q_0, -q_1, −q2, −q3_ ) \]

旋转矩阵求逆

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

\[R^{\mathsf {T}}=R^{-1} \]

参考

posted @ 2024-01-02 20:49  Hyaline-w  阅读(179)  评论(0)    收藏  举报