仿人机器人学习笔记（一）坐标变换

最近在看梶田秀司的《仿人机器人》自学仿人机器人的相关知识，虽然是本很老的书，但是作为入门教材我觉得还是挺好的，所以想做一个系列的学习笔记，毕竟感觉很多数学推导或者分析思路还是很难都记住，希望之后回顾的时候能有用。

第一部分是坐标变换，涉及到世界坐标系和局部坐标系以及坐标系的其次变换等内容

坐标系

世界坐标系的原点一般选择在机器人处于初始位置时其腰部坐标系原点的铅垂线与地面的交点，坐标系用右手法则，世界坐标系中的位置、速度、姿态分别称为绝对位置、绝对速度和绝对姿态

绝对位置的三维矢量
Ph=[PhxPhyPhz]\ P_h = \left[ \begin{matrix} P_{hx} \\ P_{hy} \\ P_{hz} \end{matrix} \right] Ph=⎣⎡PhxPhyPhz⎦⎤

局部坐标系与世界坐标系相比只是更加灵活，性质相似

齐次变换

PhP_hPh为目标点在世界坐标系中的绝对位置，PaP_aPa为局部坐标系a在世界坐标系中的位置，Γ\GammaΓ为目标点在局部坐标系a中的坐标左乘旋转矩阵（局部坐标系相对世界坐标系的旋转)
Ph=Pa+Γ=Pa+Ra∗Pah\ P_h = P_a + \Gamma = P_a + R_a* P_a^h Ph=Pa+Γ=Pa+Ra∗Pah
上式又可写作
[Ph1]=[RaPa0001]∗[Pah1]\left[ \begin{matrix} P_h \\ 1 \end{matrix} \right] = \left[ \begin{matrix} \ \ \ R_a \ \ \ \ \ P_a \\ 0 \ 0\ 0\ \ \ \ 1 \end{matrix} \right] * \left[ \begin{matrix} P_a^h \\ 1 \end{matrix} \right] [Ph1]=[ Ra Pa0 0 0 1]∗[Pah1]
并将左乘的矩阵记为TaT_aTa，称为齐次变换矩阵，其中既有局部坐标系的原点相对于世界坐标系的位置，其相对世界坐标系的旋转状态，并由此唯一确定局部坐标系，并且其满足链式相乘法则（通过局部坐标系之间的其次变换矩阵不断左乘得到局部坐标系相对世界坐标系的位置）

转动特性

相对X轴旋转称为滚动（Roll），并用ϕ\ \ \phi ϕ 表示，其对应的旋转矩阵为[1000cosϕ−sinϕ0sinϕcosϕ]\left[\begin{matrix} 1 \ \ \ \ \ \ \ \ 0 \ \ \ \ \ \ \ \ \ \ 0 \\ 0 \ \ cos\phi\ -sin\phi\\ 0 \ \ \ sin\phi\ \ \ cos\phi \end{matrix}\right]⎣⎡1 0 00 cosϕ −sinϕ0 sinϕ cosϕ⎦⎤
相对Y轴旋转称为俯仰（Pitch），并用θ\ \ \theta θ 表示，其对应的旋转矩阵为[cosϕ0−sinϕ010sinϕ0cosϕ]\left[\begin{matrix} cos\phi \ \ \ \ 0 \ \ \ \ -sin\phi \\ 0 \ \ \ \ \ \ \ 1 \ \ \ \ \ \ \ \ 0 \\ sin\phi \ \ \ \ \ \ 0\ \ \ cos\phi \end{matrix}\right]⎣⎡cosϕ 0 −sinϕ0 1 0sinϕ 0 cosϕ⎦⎤
相对Z轴旋转称为偏摆（Yaw），并用Ψ\ \ \Psi Ψ 表示，其对应的旋转矩阵为[cosϕ−sinϕ0sinϕcosϕ0001]\left[\begin{matrix}cos\phi\ -sin\phi\ \ \ 0 \\ sin\phi\ \ \ \ \ cos\phi \ \ \ \ 0 \\ 0 \ \ \ \ \ \ \ \ \ \ 0 \ \ \ \ \ \ \ \ \ \ \ 1 \ \ \end{matrix}\right]⎣⎡cosϕ −sinϕ 0sinϕ cosϕ 00 0 1 ⎦⎤
旋转矩阵可以解释为对矢量转动的操作算子，也可以视为局部坐标系的姿态
旋转矩阵为正交矩阵（即其逆矩阵为其转置）

角速度矢量与线速度

角速度矢量可以表示为一个单位矢量和一个标量的乘积，单位矢量的方向与转动轴方向一致，标量大小与转动速率一致，即w=a⃗∗q\ \ w = \vec{a} * q w=a∗q 。例如圆柱体以1 rad/s 绕 Z 轴转动，此时其w=[001]\ \ w = \left[\begin{matrix} 0 \\0\\1 \end{matrix}\right] w=⎣⎡001⎦⎤
令矢量p⃗\ \vec{p} p 为转动轴上任意点道物体上某点的矢量，那么改点的线速度可以表示为 p˙=w⃗×p⃗=[wxwywz]×[pxpypz]=[wypx−wzpywzpx−wxpzwxpy−wypx]\dot p=\ \vec{w}\times\vec{p} = \left[\begin{matrix} w_x \\w_y\\w_z \end{matrix}\right] \times \left[\begin{matrix} p_x \\p_y\\p_z \end{matrix}\right] = \left[\begin{matrix} w_yp_x -w_zp_y \\w_zp_x-w_xp_z\\w_xp_y-w_yp_x \end{matrix}\right]p˙= w×p=⎣⎡wxwywz⎦⎤×⎣⎡pxpypz⎦⎤=⎣⎡wypx−wzpywzpx−wxpzwxpy−wypx⎦⎤
对角速度矢量左乘一个旋转矩阵R\ \R R（不是齐次变换矩阵）可将转动轴方向进行旋转得到新的角速度矢量
考虑角速度矢量、位置矢量和速度矢量在旋转矩阵R\ \R R作用下的转动：
w,=R⋅wp,=R⋅pv,=R⋅v由此可得R(w⃗×p⃗)=(R⋅w⃗)×(R⋅p⃗)w^, = \R\cdot w \\p^,=\R\cdot p\\v^, = \R\cdot v\\由此可得\ \R(\vec{w}\times\vec{p})=(\R\cdot\vec{w})\times(\R\ \cdot\vec{p})w,=R⋅wp,=R⋅pv,=R⋅v由此可得 R(w×p)=(R⋅w)×(R ⋅p)
考虑旋转矩阵的微分与角速度矢量之间的关系
Ph=Pah+Γ=Pah+Ra⋅Pa\ P_h = P_a^h + \Gamma = P_a^h + R_a\cdot P_a Ph=Pah+Γ=Pah+Ra⋅Pa

假定Pa与Pah\ \ P_a\ \ 与\ \ P_a^h Pa 与 Pah 不变（即物体相对局部坐标系的位置不变，局部坐标系原点在世界坐标系中的坐标不变，只有局部坐标系的旋转运动）
对上式进行微分，可得 P˙h=R˙a⋅Pa\dot P_h = \dot R_a\cdot P_aP˙h=R˙a⋅Pa
而由于 Ra−1⋅Ph=RaT⋅Ph=PaR_a^{-1}\cdot P_h = R_a^{T}\cdot P_h = P_aRa−1⋅Ph=RaT⋅Ph=Pa
可推得P˙h=R˙⋅RT⋅Ph\dot P_h = \dot R\cdot R^T\cdot P_hP˙h=R˙⋅RT⋅Ph
再由之前利用旋转矩阵得到速度的叉乘公式：P˙h=w⃗×Ph\dot P_h=\ \vec{w}\times P_hP˙h= w×Ph可得:
w×p=R˙RTp=[wypx−wzpywzpx−wxpzwxpy−wypx]=[0−wzwywz0−wx−wywx0]=S⋅pw\times p=\dot RR^Tp = \left[\begin{matrix} w_yp_x -w_zp_y \\w_zp_x-w_xp_z\\w_xp_y-w_yp_x \end{matrix}\right] = \left[\begin{matrix} 0\ \ \ \ -w_z \ \ \ \ w_y \\w_z\ \ \ \ 0 \ \ \ \ -w_x \ \\-w_y \ \ \ \ \ w_x\ \ \ \ \ 0 \end{matrix}\right] = S\cdot pw×p=R˙RTp=⎣⎡wypx−wzpywzpx−wxpzwxpy−wypx⎦⎤=⎣⎡0 −wz wywz 0 −wx −wy wx 0⎦⎤=S⋅p

将最右侧的式子定义为矩阵 S，则其为斜对称矩阵（满足 ST=−SS^T = -SST=−S）
由于可以从矩阵 S中获得角速度的三维矢量，所以定义运算
[0−wzwywz0−wx−wywx0]V=[wxwywz][wxwywz]Λ=[0−wzwywz0−wx−wywx0]\left[\begin{matrix} 0\ \ \ \ -w_z \ \ \ \ w_y \\w_z\ \ \ \ 0 \ \ \ \ -w_x \ \\-w_y \ \ \ \ \ w_x\ \ \ \ \ 0 \end{matrix}\right]^V = \left[\begin{matrix} w_x\\w_y\\w_z \end{matrix}\right] \\ \ \\ \left[\begin{matrix} w_x\\w_y\\w_z \end{matrix}\right]^{\Lambda} = \left[\begin{matrix} 0\ \ \ \ -w_z \ \ \ \ w_y \\w_z\ \ \ \ 0 \ \ \ \ -w_x \ \\-w_y \ \ \ \ \ w_x\ \ \ \ \ 0 \end{matrix}\right]⎣⎡0 −wz wywz 0 −wx −wy wx 0⎦⎤V=⎣⎡wxwywz⎦⎤ ⎣⎡wxwywz⎦⎤Λ=⎣⎡0 −wz wywz 0 −wx −wy wx 0⎦⎤
可得角速度矢量与旋转矩阵的微分的关系
p˙=w×p=w^⋅pw^=R˙⋅RTw=(R˙⋅RT)Λ\dot p =w\times p =\hat{w}\cdot p \\ \hat{w} = \dot R\cdot R^T\\ \ w =(\dot R\cdot R^T)^{\Lambda}p˙=w×p=w^⋅pw^=R˙⋅RT w=(R˙⋅RT)Λ
而在第二个等式的两边同时右乘 R 可得R˙=w^⋅R\dot R = \hat{w}\cdot RR˙=w^⋅R
称为转动的基本方程式，通过求解R的微分方程可以得到旋转矩阵R，在初始条件R(0) = 单位矩阵I以及角速度w\ \ w w 保持恒定的情况下可得旋转矩阵的R(t)=I+w^t+(w^t)2/2!+(w^t)3/3!....R(t)=I+\hat{w}t+(\hat{w}t)^2/2! + (\hat{w}t)^3/3! \ ....R(t)=I+w^t+(w^t)2/2!+(w^t)3/3! ....符合实数中指数函数ete^tet的泰勒级数展开，因此将其标记为ew^te^{\hat{w}t}ew^t利用角速度矢量可以视为单位矢量乘上一个标量的理解方式（w=a⃗⋅ww=\vec{a}\cdot ww=a⋅w)，可以将上式简化
w=a⃗⋅w，∣∣a∣∣=1w^=a^∗wa^3=−a^（可验证)结合sint与cost的泰勒级数展开可得R(t)=ew^t=I+a^sin(wt)+a2^(1−cos(wt))将wt记为θ，有R(t)=ew^t=I+a^sin(θ)+a2^(1−cos(θ))w=\vec{a}\cdot w\ ，||a||=1 \\ \hat{w} = \hat{a}*w\\ \hat{a}^3 = -\hat{a}（可验证)\\ 结合\ sint\ 与\ cost\ 的泰勒级数展开\\ 可得 \ R(t) = e^{\hat{w}t} = I + \hat{a}sin(wt)+\hat{a^2}(1-cos(wt))\\ 将wt记为\theta，\ 有R(t) = e^{\hat{w}t} = I + \hat{a}sin(\theta)+\hat{a^2}(1-cos(\theta))w=a⋅w ，∣∣a∣∣=1w^=a^∗wa^3=−a^（可验证)结合 sint 与 cost 的泰勒级数展开可得 R(t)=ew^t=I+a^sin(wt)+a2^(1−cos(wt))将wt记为θ，有R(t)=ew^t=I+a^sin(θ)+a2^(1−cos(θ))

矩阵对数

上述推导利用泰勒级数展开定义了矩阵指数，因此也定义矩阵对数
w^=ln(R)=ln(ew^t)\hat{w} = ln(R) = ln(e^{\hat{w}t}) w^=ln(R)=ln(ew^t)常用这一式子求给定旋转矩阵后对应的转动速度矢量（一秒钟内物体的转动量）
w=(ln(R))V(ln(R))V={[000]TR=Eθ2sinθ[r32−r23r13−r31r21−r12]R≠ER=[r11r12r13r21r22r23r31r32r33]，θ=arccos(r11+r22+r33−12)w = (ln(R))^{V}\\ \ \\ (ln(R))^{V} = \begin{cases} \left[\begin{matrix}\ 0\ 0\ 0\end{matrix}\right]^T & R = E \\ \frac{\theta}{2sin\theta}\left[\begin{matrix}\ r_{32}-r_{23}\\ r_{13} - r_{31} \\ r_{21}-r_{12}\end{matrix}\right] & R \not=E \end{cases}\\ \ \\ R = \left[\begin{matrix}\ r_{11}\ r_{12} \ r_{13}\\ r_{21} \ r_{22} \ r_{23} \\ r_{31} \ r_{32} \ r_{33}\end{matrix}\right] ，\theta=arccos(\frac{r_{11}+r_{22}+r_{33}-1}{2}) w=(ln(R))V (ln(R))V=⎩⎪⎪⎨⎪⎪⎧[ 0 0 0]T2sinθθ⎣⎡ r32−r23r13−r31r21−r12⎦⎤R=ER=E R=⎣⎡ r11 r12 r13r21 r22 r23r31 r32 r33⎦⎤，θ=arccos(2r11+r22+r33−1)

补充

求两个局部坐标系之间的旋转矩阵 R=R1TR2R = R_1^TR_2R=R1TR2
求旋转矩阵对应的角速度矢量 w=(ln(R))Vw = (ln(R))^Vw=(ln(R))V
世界坐标系中的角速度矢量 w=R1w1w = R_1w_1w=R1w1
姿态的插补 R(t)=R1ew^tR(t) = R_1e^{\hat{w}t}R(t)=R1ew^t