【机器人学】绕空间任意轴的旋转矩阵推导
注意：文中推导过程基于右手定则用单位向量 n\textbf{n}n 描述旋转轴，用 θ\thetaθ 描述旋转角度。我们希望推导出满足vR(n,θ)=v’\textbf{v}R(n,\theta)=\textbf{v'}vR(n,θ)=v’ 的矩阵 R(n,θ)R(\textbf{n},\theta)R(n,θ)。...
注意：文中推导过程基于右手定则
用单位向量 n \textbf{n} n 描述旋转轴，用 θ \theta θ 描述旋转角度。我们希望推导出满足 v R ( n , θ ) = v’ \textbf{v}R(n,\theta)=\textbf{v'} vR(n,θ)=v’ 的矩阵 R ( n , θ ) R(\textbf{n},\theta) R(n,θ)。
向量 v’ \textbf{v'} v’是向量 v \textbf{v} v绕轴 n \textbf{n} n旋转后的向量。将 v \textbf{v} v分解成两个向量， v ⊥ \textbf{v}_⊥ v⊥和 v ∥ \textbf{v}_∥ v∥，分别垂直于 n \textbf{n} n和平行于 n \textbf{n} n，并有 v = v ∥ + v ⊥ \textbf{v}=\textbf{v}_∥+\textbf{v}_⊥ v=v∥+v⊥。因为 v ∥ \textbf{v}_∥ v∥平行于 n \textbf{n} n，所以绕 n \textbf{n} n旋转不会影响它。故只要计算出 v ⊥ \textbf{v}_⊥ v⊥绕 n \textbf{n} n旋转后的 v’ ⊥ \textbf{v'}_⊥ v’⊥。就能得到 v’ = v ∥ + v’ ⊥ \textbf{v'}=\textbf{v}_∥+\textbf{v'}_⊥ v’=v∥+v’⊥。
视线正对 n \textbf{n} n的正方向，与向量 n \textbf{n} n垂直的平面上的向量如下图所示：
上图两幅图中包含以下向量：
- v ∥ \textbf{v}_∥ v∥是 v \textbf{v} v平行于 n \textbf{n} n的向量，可以表示为 v ∥ = ∣ v ∣ c o s < v , v ∥ > n = ( v ⋅ n ) n \textbf{v}_∥=|\textbf{v}|cos<\textbf{v},\textbf{v}_∥>\textbf{n}=(\textbf{v}\cdot \textbf{n})\textbf{n} v∥=∣v∣cos<v,v∥>n=(v⋅n)n。
- v ⊥ \textbf{v}_⊥ v⊥是 v \textbf{v} v垂直于 n \textbf{n} n的向量。因为 v = v ∥ + v ⊥ \textbf{v}=\textbf{v}_∥+\textbf{v}_⊥ v=v∥+v⊥，所以 v ⊥ = v − v ∥ \textbf{v}_⊥=\textbf{v}-\textbf{v}_∥ v⊥=v−v∥， v ⊥ \textbf{v}_⊥ v⊥是 v \textbf{v} v投影到垂直于 n \textbf{n} n的平面上的结果。
- w \textbf{w} w是同时垂直于 v ∥ \textbf{v}_∥ v∥和 v ⊥ \textbf{v}_⊥ v⊥的向量。它的长度和 v ⊥ \textbf{v}_⊥ v⊥相同。 w \textbf{w} w和 v ⊥ \textbf{v}_⊥ v⊥同在垂直于 n \textbf{n} n的平面中。 w \textbf{w} w是 v ⊥ \textbf{v}_⊥ v⊥绕 n \textbf{n} n旋转90°的结果，由 n × v ⊥ \textbf{n}×\textbf{v}_⊥ n×v⊥可以得到。
-
v’
\textbf{v'}
v’垂直于
n
\textbf{n}
n的分量
v’
⊥
\textbf{v'}_⊥
v’⊥可以表示为：
v’ ⊥ = v ⊥ c o s θ + w s i n θ = ( v − v ∥ ) c o s θ + ( n × v ⊥ ) s i n θ = ( v − v ∥ ) c o s θ + ( n × ( v − v ∥ ) ) s i n θ = ( v − ( v ⋅ n ) n ) c o s θ + ( n × ( v − ( v ⋅ n ) n ) ) s i n θ = ( v − ( v ⋅ n ) n ) c o s θ + ( n × v ) s i n θ \begin{aligned} \textbf{v'}_⊥&=\textbf{v}_⊥cos\theta+\textbf{w}sin\theta\\ &=(\textbf{v}-\textbf{v}_∥)cos\theta+(\textbf{n}×\textbf{v}_⊥)sin\theta\\ &=(\textbf{v}-\textbf{v}_∥)cos\theta+(\textbf{n}×(\textbf{v}-\textbf{v}_∥))sin\theta\\ &=(\textbf{v}-(\textbf{v}\cdot \textbf{n})\textbf{n})cos\theta+(\textbf{n}×(\textbf{v}-(\textbf{v}\cdot \textbf{n})\textbf{n}))sin\theta\\ &=(\textbf{v}-(\textbf{v}\cdot \textbf{n})\textbf{n})cos\theta+(\textbf{n}×\textbf{v})sin\theta\\ \end{aligned} v’⊥=v⊥cosθ+wsinθ=(v−v∥)cosθ+(n×v⊥)sinθ=(v−v∥)cosθ+(n×(v−v∥))sinθ=(v−(v⋅n)n)cosθ+(n×(v−(v⋅n)n))sinθ=(v−(v⋅n)n)cosθ+(n×v)sinθ
对 v’ \textbf{v'} v’ 做如下替换和推导：
v’ = v ∥ + v’ ⊥ = ( v ⋅ n ) n + ( v − ( v ⋅ n ) n ) c o s θ + ( n × v ) s i n θ \begin{aligned} \textbf{v'}&=\textbf{v}_∥+\textbf{v'}_⊥=(\textbf{v}\cdot \textbf{n})\textbf{n}+(\textbf{v}-(\textbf{v}\cdot \textbf{n})\textbf{n})cos\theta+(\textbf{n}×\textbf{v})sin\theta\\ \end{aligned} v’=v∥+v’⊥=(v⋅n)n+(v−(v⋅n)n)cosθ+(n×v)sinθ
① 令 v = [ 1 0 0 ] \textbf{v}=\begin{bmatrix} 1 \\ 0\\0\end{bmatrix} v=⎣⎡100⎦⎤,带入上式：
v’ = ( [ 1 0 0 ] ⋅ [ n x n y n z ] ) [ n x n y n z ] + ( [ 1 0 0 ] − ( [ 1 0 0 ] ⋅ [ n x n y n z ] ) [ n x n y n z ] ) c o s θ + ( [ n x n y n z ] × [ 1 0 0 ] ) s i n θ = [ n x 2 n x n y n x n z ] + [ 1 − n x 2 − n x n y − n x n z ] c o s θ + [ 0 n z − n y ] s i n θ = [ ( 1 − n x 2 ) c o s θ + n x 2 − n x n y c o s θ + n z s i n θ + n x n y − n x n z c o s θ − n y s i n θ + n x n z ] = [ ( 1 − c o s θ ) n x 2 + c o s θ ( 1 − c o s θ ) n x n y + n z s i n θ ( 1 − c o s θ ) n x n z − n y s i n θ ] \begin{aligned} \textbf{v'}&=\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} - \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} × \begin{bmatrix} 1 \\ 0\\0\end{bmatrix} \right)sin\theta\\ &= \begin{bmatrix} n_x^2 \\n_xn_y\\n_xn_z\end{bmatrix} + \begin{bmatrix} 1-n_x^2 \\-n_xn_y\\-n_xn_z\end{bmatrix}cos\theta + \begin{bmatrix}0\\ n_z \\-n_y\end{bmatrix}sin\theta\\ &= \begin{bmatrix} (1-n_x^2)cos\theta+n_x^2\\-n_xn_ycos\theta+n_zsin\theta+n_xn_y\\-n_xn_zcos\theta-n_ysin\theta+n_xn_z\end{bmatrix}\\ &= \begin{bmatrix} (1-cos\theta)n_x^2+cos\theta\\(1-cos\theta)n_xn_y+n_zsin\theta\\(1-cos\theta)n_xn_z-n_ysin\theta\end{bmatrix} \end{aligned} v’=⎝⎛⎣⎡100⎦⎤⋅⎣⎡nxnynz⎦⎤⎠⎞⎣⎡nxnynz⎦⎤+⎝⎛⎣⎡100⎦⎤−⎝⎛⎣⎡100⎦⎤⋅⎣⎡nxnynz⎦⎤⎠⎞⎣⎡nxnynz⎦⎤⎠⎞cosθ+⎝⎛⎣⎡nxnynz⎦⎤×⎣⎡100⎦⎤⎠⎞sinθ=⎣⎡nx2nxnynxnz⎦⎤+⎣⎡1−nx2−nxny−nxnz⎦⎤cosθ+⎣⎡0nz−ny⎦⎤sinθ=⎣⎡(1−nx2)cosθ+nx2−nxnycosθ+nzsinθ+nxny−nxnzcosθ−nysinθ+nxnz⎦⎤=⎣⎡(1−cosθ)nx2+cosθ(1−cosθ)nxny+nzsinθ(1−cosθ)nxnz−nysinθ⎦⎤
② 令
v
=
[
0
1
0
]
\textbf{v}=\begin{bmatrix} 0 \\ 1\\0\end{bmatrix}
v=⎣⎡010⎦⎤,带入上式：
v’
=
(
[
0
1
0
]
⋅
[
n
x
n
y
n
z
]
)
[
n
x
n
y
n
z
]
+
(
[
0
1
0
]
−
(
[
0
1
0
]
⋅
[
n
x
n
y
n
z
]
)
[
n
x
n
y
n
z
]
)
c
o
s
θ
+
(
[
n
x
n
y
n
z
]
×
[
0
1
0
]
)
s
i
n
θ
=
[
n
x
n
y
n
y
2
n
y
n
z
]
+
[
−
n
x
n
y
1
−
n
y
2
−
n
y
n
z
]
c
o
s
θ
+
[
−
n
z
0
n
x
]
s
i
n
θ
=
[
−
n
x
n
y
c
o
s
θ
−
n
z
s
i
n
θ
+
n
x
n
y
(
1
−
n
y
2
)
c
o
s
θ
+
n
y
2
−
n
y
n
z
c
o
s
θ
+
n
x
s
i
n
θ
+
n
y
n
z
]
=
[
(
1
−
c
o
s
θ
)
n
x
n
y
−
n
z
s
i
n
θ
(
1
−
c
o
s
θ
)
n
y
2
+
c
o
s
θ
(
1
−
c
o
s
θ
)
n
y
n
z
+
n
x
s
i
n
θ
]
\begin{aligned} \textbf{v'}&=\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} - \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} × \begin{bmatrix} 0 \\ 1\\0\end{bmatrix} \right)sin\theta\\ &= \begin{bmatrix} n_xn_y \\n_y^2\\n_yn_z\end{bmatrix} + \begin{bmatrix} -n_xn_y \\1-n_y^2\\-n_yn_z\end{bmatrix}cos\theta + \begin{bmatrix} -n_z \\0\\n_x\end{bmatrix}sin\theta\\ &= \begin{bmatrix} -n_xn_ycos\theta-n_zsin\theta+n_xn_y\\(1-n_y^2)cos\theta+n_y^2\\-n_yn_zcos\theta+n_xsin\theta+n_yn_z\\\end{bmatrix}\\ &= \begin{bmatrix} (1-cos\theta)n_xn_y-n_zsin\theta\\(1-cos\theta)n_y^2+cos\theta\\(1-cos\theta)n_yn_z+n_xsin\theta\end{bmatrix} \end{aligned}
v’=⎝⎛⎣⎡010⎦⎤⋅⎣⎡nxnynz⎦⎤⎠⎞⎣⎡nxnynz⎦⎤+⎝⎛⎣⎡010⎦⎤−⎝⎛⎣⎡010⎦⎤⋅⎣⎡nxnynz⎦⎤⎠⎞⎣⎡nxnynz⎦⎤⎠⎞cosθ+⎝⎛⎣⎡nxnynz⎦⎤×⎣⎡010⎦⎤⎠⎞sinθ=⎣⎡nxnyny2nynz⎦⎤+⎣⎡−nxny1−ny2−nynz⎦⎤cosθ+⎣⎡−nz0nx⎦⎤sinθ=⎣⎡−nxnycosθ−nzsinθ+nxny(1−ny2)cosθ+ny2−nynzcosθ+nxsinθ+nynz⎦⎤=⎣⎡(1−cosθ)nxny−nzsinθ(1−cosθ)ny2+cosθ(1−cosθ)nynz+nxsinθ⎦⎤
③ 令
v
=
[
0
0
1
]
\textbf{v}=\begin{bmatrix} 0 \\ 0\\1\end{bmatrix}
v=⎣⎡001⎦⎤,带入上式：
v’
=
(
[
0
0
1
]
⋅
[
n
x
n
y
n
z
]
)
[
n
x
n
y
n
z
]
+
(
[
0
0
1
]
−
(
[
0
0
1
]
⋅
[
n
x
n
y
n
z
]
)
[
n
x
n
y
n
z
]
)
c
o
s
θ
+
(
[
n
x
n
y
n
z
]
×
[
0
0
1
]
)
s
i
n
θ
=
[
n
x
n
z
n
y
n
z
n
z
2
]
+
[
−
n
x
n
z
−
n
y
n
z
1
−
n
z
2
]
c
o
s
θ
+
[
n
y
−
n
x
0
]
s
i
n
θ
=
[
−
n
x
n
z
c
o
s
θ
+
n
y
s
i
n
θ
+
n
x
n
z
−
n
y
n
z
c
o
s
θ
−
n
x
s
i
n
θ
+
n
y
n
z
(
1
−
n
z
2
)
c
o
s
θ
+
n
z
2
]
=
[
(
1
−
c
o
s
θ
)
n
x
n
z
+
n
y
s
i
n
θ
(
1
−
c
o
s
θ
)
n
y
n
z
−
n
x
s
i
n
θ
(
1
−
c
o
s
θ
)
n
z
2
+
c
o
s
θ
]
\begin{aligned} \textbf{v'}&=\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} - \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} × \begin{bmatrix} 0 \\ 0\\1\end{bmatrix} \right)sin\theta\\ &= \begin{bmatrix} n_xn_z \\n_yn_z\\n_z^2\end{bmatrix} + \begin{bmatrix} -n_xn_z \\-n_yn_z\\1-n_z^2\end{bmatrix}cos\theta + \begin{bmatrix} n_y \\-n_x\\0\end{bmatrix}sin\theta\\ &= \begin{bmatrix} -n_xn_zcos\theta+n_ysin\theta+n_xn_z\\-n_yn_zcos\theta-n_xsin\theta+n_yn_z\\(1-n_z^2)cos\theta+n_z^2\end{bmatrix}\\ &= \begin{bmatrix} (1-cos\theta)n_xn_z+n_ysin\theta\\(1-cos\theta)n_yn_z-n_xsin\theta\\(1-cos\theta)n_z^2+cos\theta\end{bmatrix} \end{aligned}
v’=⎝⎛⎣⎡001⎦⎤⋅⎣⎡nxnynz⎦⎤⎠⎞⎣⎡nxnynz⎦⎤+⎝⎛⎣⎡001⎦⎤−⎝⎛⎣⎡001⎦⎤⋅⎣⎡nxnynz⎦⎤⎠⎞⎣⎡nxnynz⎦⎤⎠⎞cosθ+⎝⎛⎣⎡nxnynz⎦⎤×⎣⎡001⎦⎤⎠⎞sinθ=⎣⎡nxnznynznz2⎦⎤+⎣⎡−nxnz−nynz1−nz2⎦⎤cosθ+⎣⎡ny−nx0⎦⎤sinθ=⎣⎡−nxnzcosθ+nysinθ+nxnz−nynzcosθ−nxsinθ+nynz(1−nz2)cosθ+nz2⎦⎤=⎣⎡(1−cosθ)nxnz+nysinθ(1−cosθ)nynz−nxsinθ(1−cosθ)nz2+cosθ⎦⎤
综上可得到绕空间任意轴旋转的转换矩阵为：
R
(
n
,
θ
)
=
[
(
1
−
c
o
s
θ
)
n
x
2
+
c
o
s
θ
(
1
−
c
o
s
θ
)
n
x
n
y
−
n
z
s
i
n
θ
(
1
−
c
o
s
θ
)
n
x
n
z
+
n
y
s
i
n
θ
(
1
−
c
o
s
θ
)
n
x
n
y
+
n
z
s
i
n
θ
(
1
−
c
o
s
θ
)
n
y
2
+
c
o
s
θ
(
1
−
c
o
s
θ
)
n
y
n
z
−
n
x
s
i
n
θ
(
1
−
c
o
s
θ
)
n
x
n
z
−
n
y
s
i
n
θ
(
1
−
c
o
s
θ
)
n
y
n
z
+
n
x
s
i
n
θ
(
1
−
c
o
s
θ
)
n
z
2
+
c
o
s
θ
]
\textbf{R}(\textbf{n},\theta)=\begin{bmatrix} (1-cos\theta)n_x^2+cos\theta & (1-cos\theta)n_xn_y-n_zsin\theta & (1-cos\theta)n_xn_z+n_ysin\theta\\ (1-cos\theta)n_xn_y+n_zsin\theta & (1-cos\theta)n_y^2+cos\theta & (1-cos\theta)n_yn_z-n_xsin\theta\\ (1-cos\theta)n_xn_z-n_ysin\theta & (1-cos\theta)n_yn_z+n_xsin\theta & (1-cos\theta)n_z^2+cos\theta\\ \end{bmatrix}
R(n,θ)=⎣⎡(1−cosθ)nx2+cosθ(1−cosθ)nxny+nzsinθ(1−cosθ)nxnz−nysinθ(1−cosθ)nxny−nzsinθ(1−cosθ)ny2+cosθ(1−cosθ)nynz+nxsinθ(1−cosθ)nxnz+nysinθ(1−cosθ)nynz−nxsinθ(1−cosθ)nz2+cosθ⎦⎤
CSDN联合极客时间，共同打造面向开发者的精品内容学习社区，助力成长！
更多推荐
22
0- 0
已为社区贡献4条内容
所有评论(0)