三维空间绕任意轴旋转矩阵的推导

本文转载自查看原文 2021-12-16 11:27 1441 Robotic基础/ 数学小记

转载至：https://zhuanlan.zhihu.com/p/56587491

推导如下

设 $v$ 是三维空间中任意向量，现求 $v$ 绕 $n$ 顺时针旋转 $\theta$ 所得到的向量 $v^\prime$ ，其中 $n$ 是单位向量，

$n = \begin{bmatrix} n_x \\ n_y \\ n_z \end{bmatrix}$ ， $n_x^2 + n_y^2 + n_z^2 = 1$ 。

首先求 $v$ 在 $n$ 上的投影，记为 $v_\parallel$ ， $v_\parallel = (v^Tn)n$ 。

记 $v_\perp$ 为 $v$ 垂直于 $n$ 的分量， $v_\perp = v - v_\parallel = v - (v^Tn)n$ 。

记 $w$ 为 $n$ 和 $v_\perp$ 的叉积， $w = n \times v_\perp$ 。根据叉乘的定义， $w$ 垂直于 $n$ 和 $v_\perp$ 固定的平面，其方向服从右手法则，大小为 $n$ 和 $v_\perp$ 围成的平行四边形的面积。又因为 $n$ 和 $v_\perp$ 垂直，且 $n$ 为单位向量，所以 $w$ 的大小和 $v_\perp$ 一样。

记 $v_\perp^\prime$ 为 $v_\perp$ 绕 $n$ 旋转 $\theta$ 得到的向量，

$\begin{align} v_\perp^\prime &= v_\perp \cos \theta + w\sin \theta \\ &= \left[ v - \left( v^Tn \right)n \right] \cos \theta + \left( n \times v \right) \sin \theta \end{align}$
下面求得 $v^\prime$ ：

$\begin{align} v^\prime &= v_\perp^\prime + v_\parallel \\ &= \left[ v - \left( v^Tn \right)n \right] \cos \theta + \left( n \times v \right) \sin \theta + (v^Tn)n \end{align}$

令 $v = \begin{bmatrix} 1 \\ 0 \\ 0 \end{bmatrix}$ ，带入上式：

$\begin{align} v^\prime &= \left( \begin{bmatrix} 1 \\ 0 \\ 0 \\ \end{bmatrix} - \left( \begin{bmatrix} 1 \\ 0 \\ 0 \\ \end{bmatrix} \cdot \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \cos \theta + \left( \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \times \begin{bmatrix} 1 \\ 0 \\ 0 \\ \end{bmatrix} \right) \sin \theta + \left( \begin{bmatrix} 1 \\ 0 \\ 0 \\ \end{bmatrix} \cdot \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \\ &=\left( \begin{bmatrix} 1 \\ 0 \\ 0 \\ \end{bmatrix} - n_x \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \cos \theta + \begin{bmatrix} 0 \\ n_z \\ -n_y \\ \end{bmatrix} \sin \theta + n_x \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \\ &= \begin{bmatrix} 1 - n_x^2 \\ -n_x n_y \\ -n_x n_z \\ \end{bmatrix} \cos \theta + \begin{bmatrix} 0 \\ n_z \\ -n_y \\ \end{bmatrix} \sin \theta + \begin{bmatrix} n_x^2 \\ n_x n_y \\ n_x n_z \\ \end{bmatrix} \\ &= \begin{bmatrix} \cos \theta - n_x^2 \cos \theta + n_x^2 \\ -n_xn_y \cos \theta + n_z \sin \theta + n_xn_y \\ -n_xn_z \cos \theta - n_y \sin \theta + n_xn_z \end{bmatrix} \\ &= \begin{bmatrix} n_x^2(1 - \cos \theta) + \cos \theta \\ n_xn_y(1 - \cos \theta) + n_z \sin \theta \\ n_xn_z(1 - \cos \theta) - n_y \sin \theta \end{bmatrix} \end{align}$

同理，令 $v = \begin{bmatrix} 0 \\ 1 \\ 0 \end{bmatrix}$ ，

$\begin{align} v^\prime &= \left( \begin{bmatrix} 0 \\ 1 \\ 0\\ \end{bmatrix} - \left( \begin{bmatrix} 0 \\ 1 \\ 0\\ \end{bmatrix} \cdot \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \cos \theta + \left( \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \times \begin{bmatrix} 0\\ 1\\ 0\\ \end{bmatrix} \right) \sin \theta + \left( \begin{bmatrix} 0 \\ 1 \\ 0\\ \end{bmatrix} \cdot \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \\ &=\left( \begin{bmatrix} 0 \\ 1 \\ 0 \\ \end{bmatrix} - n_y\begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \cos \theta + \begin{bmatrix} -n_z \\ 0 \\ n_x \\ \end{bmatrix} \sin \theta + n_y \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \\ &= \begin{bmatrix} - n_x n_y \\ 1 - n_y^2 \\ -n_y n_z \\ \end{bmatrix} \cos \theta + \begin{bmatrix} -n_z\\ 0 \\ n_x \\ \end{bmatrix} \sin \theta + \begin{bmatrix} n_x n_y \\ n_y^2 \\ n_y n_z \\ \end{bmatrix} \\ &= \begin{bmatrix} -n_x n_y \cos \theta -n_z \sin \theta + n_x n_y \\ \cos \theta -n_y^2 \cos \theta + n_y^2 \\ -n_yn_z \cos \theta + n_x \sin \theta + n_yn_z \end{bmatrix} \\ &= \begin{bmatrix} n_x n_y ( 1 - \cos \theta ) -n_z \sin \theta \\ n_y^2(1 - \cos \theta) + \cos \theta \\ n_yn_z(1 - \cos \theta) + n_x \sin \theta \end{bmatrix} \end{align}$

令 $v = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}$ ，

$\begin{align} v^\prime &= \left( \begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix} - \left( \begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix} \cdot \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \cos \theta + \left( \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \times \begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix} \right) \sin \theta + \left( \begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix} \cdot \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \\ &=\left( \begin{bmatrix} 0 \\ 0 \\ 1 \\ \end{bmatrix} - n_z\begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \right) \cos \theta + \begin{bmatrix} n_y \\ -n_x \\ 0 \\ \end{bmatrix} \sin \theta + n_z \begin{bmatrix} n_x \\ n_y \\ n_z \\ \end{bmatrix} \\ &= \begin{bmatrix} - n_x n_z \\ -n_y n_z \\ 1 - n_z^2 \\ \end{bmatrix} \cos \theta + \begin{bmatrix} n_y \\ -n_x \\ 0 \\ \end{bmatrix} \sin \theta + \begin{bmatrix} n_x n_z \\ n_y n_z \\ n_z^2 \\ \end{bmatrix} \\ &= \begin{bmatrix} -n_x n_z \cos \theta +n_y \sin \theta + n_x n_z \\ -n_yn_z \cos \theta - n_x \sin \theta + n_yn_z \\ (1 - n_z^2) \cos \theta + n_z^2 \end{bmatrix} \\ &= \begin{bmatrix} n_x n_z ( 1 - \cos \theta ) + n_y \sin \theta \\ n_yn_z(1 - \cos \theta) - n_x \sin \theta \\ n_z^2(1 - \cos \theta) + \cos \theta \end{bmatrix} \end{align}$

综上得到变换矩阵：

$T = \begin{bmatrix} n_x^2(1 - \cos \theta) + \cos \theta & n_x n_y ( 1 - \cos \theta ) -n_z \sin \theta & n_x n_z ( 1 - \cos \theta ) + n_y \sin \theta \\ n_xn_y(1 - \cos \theta) + n_z \sin \theta & n_y^2(1 - \cos \theta) + \cos \theta & n_yn_z(1 - \cos \theta) - n_x \sin \theta \\ n_xn_z(1 - \cos \theta) - n_y \sin \theta & n_yn_z(1 - \cos \theta) + n_x \sin \theta & n_z^2(1 - \cos \theta) + \cos \theta\\ \end{bmatrix}$

Reference

三维空间任意一点绕任意轴线旋转
 三维空间绕任意轴旋转矩阵的推导

免责声明！

本站转载的文章为个人学习借鉴使用，本站对版权不负任何法律责任。如果侵犯了您的隐私权益，请联系本站邮箱yoyou2525@163.com删除。

猜您在找 绕空间任意轴旋转矩阵三维空间任意一点绕任意轴线旋转三维空间刚体运动——（1）齐次坐标与旋转矩阵绕任意轴旋转的矩阵推导总结机器人学——1.9-三维空间平移与旋转的组合从零开始一起学习SLAM | 三维空间刚体的旋转 python使用matplotlib绘制三维空间散点图 slam(1)-----三维空间刚体运动绕任意轴旋转【转】【矩阵】三维坐标旋转矩阵算法