旋转向量与旋转矩阵之间的关系
1、旋转向量到旋转矩阵的变换
ur机械臂TCP 姿态采用旋转向量表示为: r \bm r r=[Rx,Ry,Rz],其中,旋转向量 r \bm r r可以表示为 r \bm r r= n \bm n n θ \theta θ,表示绕 n \bm n n向量旋转 θ \theta θ角度,可以通过罗德里格斯公式比较容易的求得旋转矩阵 R \bm R R为:
R = c o s ( θ ) I + ( 1 − c o s ( θ ) ) n n T + s i n ( θ ) n ∗ \bm R=cos(\theta)\bm I+(1-cos(\theta))\bm n\bm n^\text{T}+sin(\theta)\bm n^* R=cos(θ)I+(1−cos(θ))