刚体运动学与微分公式速查

约定

本文统一采用：

• 旋转矩阵 $\mathbf{R}\in SO(3)$ 表示从刚体系到世界系的主动旋转；
• 四元数采用 Hamilton convention，标量在前：

$$\mathbf{q}=\left[\begin{array}{c}w\\ \mathbf{v}\end{array}\right]={\left[\begin{array}{cccc}w& x& y& z\end{array}\right]}^{\mathrm{\top }}$$

• 四元数与旋转矩阵满足

$$\mathbf{q}\otimes \mathbf{a}\otimes {\mathbf{q}}^{\ast }⟺\mathbf{R}\mathbf{a}$$

其中向量 $\mathbf{a}\in {\mathbb{R}}^3$ 在四元数运算中视作纯四元数

$${\mathbf{a}}_q=\left[\begin{array}{c}0\\ \mathbf{a}\end{array}\right].$$

• 叉乘矩阵记为

$$[\mathbf{a}{]}_{\times }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$

满足

$$[\mathbf{a}{]}_{\times }\mathbf{b}=\mathbf{a}\times \mathbf{b}.$$

Info

不同教材、软件和论文可能采用不同约定，例如：

• 四元数标量在前或在后；
• Hamilton 或 JPL 四元数；
• $\mathbf{R}$ 表示 body-to-world 或 world-to-body；
• twist 排列为 $[\omega ;v]$ 或 $[v;\omega ]$。

这些约定会改变部分公式的正负号和矩阵排列。使用公式前应首先确认约定。

向量与叉乘矩阵

常用恒等式

$$[\mathbf{a}{]}_{\times }^{\mathrm{\top }}=-[\mathbf{a}{]}_{\times }$$$$[\mathbf{a}{]}_{\times }\mathbf{b}=-[\mathbf{b}{]}_{\times }\mathbf{a}$$$$[\mathbf{a}{]}_{\times }^2=\mathbf{a}{\mathbf{a}}^{\mathrm{\top }}-\parallel \mathbf{a}{\parallel }^2\mathbf{I}$$$$[\mathbf{a}{]}_{\times }[\mathbf{b}{]}_{\times }=\mathbf{b}{\mathbf{a}}^{\mathrm{\top }}-({\mathbf{a}}^{\mathrm{\top }}\mathbf{b})\mathbf{I}$$

对于旋转矩阵：

$$[\mathbf{R}\mathbf{a}{]}_{\times }=\mathbf{R}[\mathbf{a}{]}_{\times }{\mathbf{R}}^{\mathrm{\top }}$$

因此

$$\mathbf{R}[\mathbf{a}{]}_{\times }=[\mathbf{R}\mathbf{a}{]}_{\times }\mathbf{R}.$$

常用微分

对于

$$\mathbf{y}=\mathbf{a}\times \mathbf{b}$$

有

$$\delta \mathbf{y}=-[\mathbf{b}{]}_{\times }\delta \mathbf{a}+[\mathbf{a}{]}_{\times }\delta \mathbf{b}.$$

因此

$$\frac{\partial (\mathbf{a}\times \mathbf{b})}{\partial \mathbf{a}}=-[\mathbf{b}{]}_{\times }$$$$\frac{\partial (\mathbf{a}\times \mathbf{b})}{\partial \mathbf{b}}=[\mathbf{a}{]}_{\times }.$$

旋转矩阵

旋转矩阵满足

$${\mathbf{R}}^{\mathrm{\top }}\mathbf{R}=\mathbf{R}{\mathbf{R}}^{\mathrm{\top }}=\mathbf{I}$$

以及

$$det\mathbf{R}=1.$$

因此

$${\mathbf{R}}^{-1}={\mathbf{R}}^{\mathrm{\top }}.$$

有限旋转通常不可交换：

$${\mathbf{R}}_1{\mathbf{R}}_2\ne {\mathbf{R}}_2{\mathbf{R}}_1.$$

轴角与指数映射

旋转向量定义为

$$\mathit{\varphi }=\theta \mathbf{u}$$

其中

$$\parallel \mathbf{u}\parallel =1.$$

旋转矩阵可以写为

$$\mathbf{R}=\mathrm{Exp}(\mathit{\varphi })=\exp ([\mathit{\varphi }{]}_{\times }).$$

Rodrigues公式：

$$\mathbf{R}=\mathbf{I}+\frac{\sin \theta }{\theta }[\mathit{\varphi }{]}_{\times }+\frac{1-\cos \theta }{{\theta }^2}[\mathit{\varphi }{]}_{\times }^2.$$

若直接使用单位旋转轴 $\mathbf{u}$：

$$\mathbf{R}=\mathbf{I}+\sin \theta [\mathbf{u}{]}_{\times }+(1-\cos \theta )[\mathbf{u}{]}_{\times }^2.$$

旋转矩阵对旋转角的导数

对于固定旋转轴 $\mathbf{u}$：

$$\mathbf{R}(\theta )=\mathrm{Exp}(\theta \mathbf{u})$$

有

$$\boxed{{\displaystyle \frac{\partial \mathbf{R}}{\partial \theta }=\mathbf{R}[\mathbf{u}{]}_{\times }=[\mathbf{u}{]}_{\times }\mathbf{R}}}$$

并且

$$\frac{{\partial }^n\mathbf{R}}{\partial {\theta }^n}=\mathbf{R}[\mathbf{u}{]}_{\times }^n.$$

旋转矩阵的时间导数

角速度在刚体系表示

记刚体系角速度为 ${\mathit{\omega }}_b$：

$$\boxed{{\displaystyle \dot{\mathbf{R}}=\mathbf{R}[{\mathit{\omega }}_b{]}_{\times }}}$$

因此

$$\boxed{{\displaystyle [{\mathit{\omega }}_b{]}_{\times }={\mathbf{R}}^{\mathrm{\top }}\dot{\mathbf{R}}}}$$

角速度在世界系表示

记世界系角速度为 ${\mathit{\omega }}_s$：

$$\boxed{{\displaystyle \dot{\mathbf{R}}=[{\mathit{\omega }}_s{]}_{\times }\mathbf{R}}}$$

因此

$$\boxed{{\displaystyle [{\mathit{\omega }}_s{]}_{\times }=\dot{\mathbf{R}}{\mathbf{R}}^{\mathrm{\top }}}}$$

两者满足

$$\boxed{{\displaystyle {\mathit{\omega }}_s=\mathbf{R}{\mathit{\omega }}_b}}$$

二阶时间导数

刚体系表示：

$$\boxed{{\displaystyle \ddot{\mathbf{R}}=\mathbf{R}([{\mathit{\alpha }}_b{]}_{\times }+[{\mathit{\omega }}_b{]}_{\times }^2)}}$$

世界系表示：

$$\boxed{{\displaystyle \ddot{\mathbf{R}}=([{\mathit{\alpha }}_s{]}_{\times }+[{\mathit{\omega }}_s{]}_{\times }^2)\mathbf{R}}}$$

其中

$$\mathit{\alpha }=\dot{\mathit{\omega }}.$$

旋转矩阵作用于向量

设

$$\mathbf{y}=\mathbf{R}\mathbf{a}.$$

对 $\mathbf{a}$ 求导：

$$\boxed{{\displaystyle \frac{\partial (\mathbf{R}\mathbf{a})}{\partial \mathbf{a}}=\mathbf{R}}}$$

如果 $\mathbf{a}$ 固连于刚体，则

$$\dot{\mathbf{y}}=\dot{\mathbf{R}}\mathbf{a}=\mathbf{R}({\mathit{\omega }}_b\times \mathbf{a})$$

或

$$\boxed{{\displaystyle \dot{\mathbf{y}}={\mathit{\omega }}_s\times \mathbf{y}}}$$

二阶导数：

$$\ddot{\mathbf{y}}={\mathit{\alpha }}_s\times \mathbf{y}+{\mathit{\omega }}_s\times ({\mathit{\omega }}_s\times \mathbf{y}).$$

微小旋转扰动

右扰动：局部/刚体系扰动

定义

$${\mathbf{R}}^{\prime }=\mathbf{R}\mathrm{Exp}(\delta \mathit{\theta }).$$

对于小量：

$$\mathrm{Exp}(\delta \mathit{\theta })\approx \mathbf{I}+[\delta \mathit{\theta }{]}_{\times }.$$

因此

$$\delta (\mathbf{R}\mathbf{a})=-\mathbf{R}[\mathbf{a}{]}_{\times }\delta \mathit{\theta }.$$

所以

$$\boxed{{\displaystyle \frac{\partial (\mathbf{R}\mathbf{a})}{\partial \delta \mathit{\theta }}=-\mathbf{R}[\mathbf{a}{]}_{\times }}}$$

左扰动：世界系扰动

定义

$${\mathbf{R}}^{\prime }=\mathrm{Exp}(\delta \mathit{\theta })\mathbf{R}.$$

则

$$\delta (\mathbf{R}\mathbf{a})=-[\mathbf{R}\mathbf{a}{]}_{\times }\delta \mathit{\theta }.$$

因此

$$\boxed{{\displaystyle \frac{\partial (\mathbf{R}\mathbf{a})}{\partial \delta \mathit{\theta }}=-[\mathbf{R}\mathbf{a}{]}_{\times }}}$$

Info

右扰动的增量 $\delta \mathit{\theta }$ 通常在刚体系表达；

左扰动的增量 $\delta \mathit{\theta }$ 通常在世界系表达。

状态估计、误差状态卡尔曼滤波和李群优化中必须明确使用的是哪一种扰动定义。

SO(3) 左、右 Jacobian

对于

$$\mathit{\varphi }\in {\mathbb{R}}^3,\theta =\parallel \mathit{\varphi }\parallel$$

右 Jacobian 为

$$\boxed{{\displaystyle {\mathbf{J}}_r(\mathit{\varphi })=\mathbf{I}-\frac{1-\cos \theta }{{\theta }^2}[\mathit{\varphi }{]}_{\times }+\frac{\theta -\sin \theta }{{\theta }^3}[\mathit{\varphi }{]}_{\times }^2}}$$

左 Jacobian 为

$$\boxed{{\displaystyle {\mathbf{J}}_l(\mathit{\varphi })=\mathbf{I}+\frac{1-\cos \theta }{{\theta }^2}[\mathit{\varphi }{]}_{\times }+\frac{\theta -\sin \theta }{{\theta }^3}[\mathit{\varphi }{]}_{\times }^2}}$$

二者满足

$${\mathbf{J}}_l(\mathit{\varphi })={\mathbf{J}}_r(-\mathit{\varphi }).$$

对于小角度：

$${\mathbf{J}}_r\approx \mathbf{I}-\frac{1}{2}[\mathit{\varphi }{]}_{\times }+\frac{1}{6}[\mathit{\varphi }{]}_{\times }^2$$$${\mathbf{J}}_l\approx \mathbf{I}+\frac{1}{2}[\mathit{\varphi }{]}_{\times }+\frac{1}{6}[\mathit{\varphi }{]}_{\times }^2.$$

Exp 对旋转向量的微分

如果

$$\mathbf{R}(\mathit{\varphi })=\mathrm{Exp}(\mathit{\varphi })$$

则

$$\mathrm{Exp}(\mathit{\varphi }+\delta \mathit{\varphi })\approx \mathrm{Exp}(\mathit{\varphi })\mathrm{Exp}({\mathbf{J}}_r(\mathit{\varphi })\delta \mathit{\varphi }).$$

因此

$$\boxed{{\displaystyle \delta (\mathbf{R}\mathbf{a})=-\mathbf{R}[\mathbf{a}{]}_{\times }{\mathbf{J}}_r(\mathit{\varphi })\delta \mathit{\varphi }}}$$

即

$$\boxed{{\displaystyle \frac{\partial (\mathbf{R}\mathbf{a})}{\partial \mathit{\varphi }}=-\mathbf{R}[\mathbf{a}{]}_{\times }{\mathbf{J}}_r(\mathit{\varphi })}}$$

这就是状态估计和机器人优化中非常常见的旋转向量 Jacobian。

四元数

单位四元数

$$\mathbf{q}=\left[\begin{array}{c}w\\ \mathbf{v}\end{array}\right]$$

满足

$$w^2+{\mathbf{v}}^{\mathrm{\top }}\mathbf{v}=1.$$

轴角对应关系为

$$w=\cos \frac{\theta }{2}$$$$\mathbf{v}=\mathbf{u}\sin \frac{\theta }{2}.$$

四元数共轭和逆

$${\mathbf{q}}^{\ast }=\left[\begin{array}{c}w\\ -\mathbf{v}\end{array}\right]$$

单位四元数满足

$$\boxed{{\displaystyle {\mathbf{q}}^{-1}={\mathbf{q}}^{\ast }}}$$

以及

$$\mathbf{q}\otimes {\mathbf{q}}^{\ast }=\left[\begin{array}{c}1\\ \mathbf{0}\end{array}\right].$$

四元数乘法

设

$$\mathbf{q}=\left[\begin{array}{c}w\\ \mathbf{v}\end{array}\right],\mathbf{p}=\left[\begin{array}{c}s\\ \mathbf{u}\end{array}\right]$$

则

$$\boxed{{\displaystyle \mathbf{q}\otimes \mathbf{p}=\left[\begin{array}{c}ws-{\mathbf{v}}^{\mathrm{\top }}\mathbf{u}\\ w\mathbf{u}+s\mathbf{v}+\mathbf{v}\times \mathbf{u}\end{array}\right]}}$$

左乘矩阵：

$$\mathbf{q}\otimes \mathbf{p}=\mathbf{L}(\mathbf{q})\mathbf{p}$$

其中

$$\mathbf{L}(\mathbf{q})=\left[\begin{array}{cc}w& -{\mathbf{v}}^{\mathrm{\top }}\\ \mathbf{v}& w\mathbf{I}+[\mathbf{v}{]}_{\times }\end{array}\right].$$

右乘矩阵：

$$\mathbf{q}\otimes \mathbf{p}={\mathbf{R}}_q(\mathbf{p})\mathbf{q}$$

也可以定义

$$\mathbf{p}\otimes \mathbf{q}=\mathbf{R}(\mathbf{q})\mathbf{p}$$

其中常用的四元数右乘矩阵为

$${\mathbf{R}}_q(\mathbf{q})=\left[\begin{array}{cc}w& -{\mathbf{v}}^{\mathrm{\top }}\\ \mathbf{v}& w\mathbf{I}-[\mathbf{v}{]}_{\times }\end{array}\right].$$

Warning

这里的四元数右乘矩阵 ${\mathbf{R}}_q$ 与旋转矩阵 $\mathbf{R}\in SO(3)$ 不应混淆。

四元数与旋转矩阵

由单位四元数得到旋转矩阵：

$$\boxed{{\displaystyle \mathbf{R}(\mathbf{q})=(w^2-{\mathbf{v}}^{\mathrm{\top }}\mathbf{v})\mathbf{I}+2\mathbf{v}{\mathbf{v}}^{\mathrm{\top }}+2w[\mathbf{v}{]}_{\times }}}$$

等价地：

$$\mathbf{R}=\mathbf{I}+2w[\mathbf{v}{]}_{\times }+2[\mathbf{v}{]}_{\times }^2.$$

向量旋转：

$$\boxed{{\displaystyle \mathbf{q}\otimes \left[\begin{array}{c}0\\ \mathbf{a}\end{array}\right]\otimes {\mathbf{q}}^{\ast }=\left[\begin{array}{c}0\\ \mathbf{R}\mathbf{a}\end{array}\right]}}$$

四元数对时间的微分

角速度在刚体系表示

$$\boxed{{\displaystyle \dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathit{\omega }}_b\end{array}\right]}}$$

即

$$\dot{\mathbf{q}}=\frac{1}{2}\left[\begin{array}{c}-{\mathbf{v}}^{\mathrm{\top }}\\ w\mathbf{I}+[\mathbf{v}{]}_{\times }\end{array}\right]{\mathit{\omega }}_b.$$

反过来：

$$\boxed{{\displaystyle {\mathit{\omega }}_b=2\left[\begin{array}{cc}-\mathbf{v}& w\mathbf{I}-[\mathbf{v}{]}_{\times }\end{array}\right]\dot{\mathbf{q}}}}$$

这就是 Shabana 中

$$\mathit{\omega }=\mathbf{G}(\mathbf{q})\dot{\mathbf{q}}$$

形式在当前四元数约定下的具体表达。

角速度在世界系表示

$$\boxed{{\displaystyle \dot{\mathbf{q}}=\frac{1}{2}\left[\begin{array}{c}0\\ {\mathit{\omega }}_s\end{array}\right]\otimes \mathbf{q}}}$$

即

$$\dot{\mathbf{q}}=\frac{1}{2}\left[\begin{array}{c}-{\mathbf{v}}^{\mathrm{\top }}\\ w\mathbf{I}-[\mathbf{v}{]}_{\times }\end{array}\right]{\mathit{\omega }}_s.$$

因此

$$\boxed{{\displaystyle {\mathit{\omega }}_s=2\left[\begin{array}{cc}-\mathbf{v}& w\mathbf{I}+[\mathbf{v}{]}_{\times }\end{array}\right]\dot{\mathbf{q}}}}$$

四元数旋转对变量的 Jacobian

设

$$\mathbf{y}=\mathbf{q}\otimes \mathbf{a}\otimes {\mathbf{q}}^{\ast }=\mathbf{R}(\mathbf{q})\mathbf{a}.$$

对 $\mathbf{a}$ 求导：

$$\boxed{{\displaystyle \frac{\partial \mathbf{y}}{\partial \mathbf{a}}=\mathbf{R}}}$$

对四元数

$$\mathbf{q}=\left[\begin{array}{c}w\\ \mathbf{v}\end{array}\right]$$

求导：

$$\boxed{{\displaystyle \frac{\partial (\mathbf{R}\mathbf{a})}{\partial \mathbf{q}}=2\left[\begin{array}{cc}w\mathbf{a}+\mathbf{v}\times \mathbf{a}& ({\mathbf{v}}^{\mathrm{\top }}\mathbf{a})\mathbf{I}+\mathbf{v}{\mathbf{a}}^{\mathrm{\top }}-\mathbf{a}{\mathbf{v}}^{\mathrm{\top }}-w[\mathbf{a}{]}_{\times }\end{array}\right]}}$$

其维数为

$$3\times 4.$$

Info

四元数只有3个实际旋转自由度，因此直接对4维四元数求导时存在单位模约束。

在优化、ESKF和李群状态估计中，通常更推荐对3维的小旋转扰动 $\delta \mathit{\theta }$ 求导，而不是直接把四元数4个分量当成独立变量。

四元数小扰动

小旋转

$$\delta \mathit{\theta }$$

对应四元数近似为

$$\delta \mathbf{q}\approx \left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathit{\theta }\end{array}\right].$$

右扰动：

$$\boxed{{\displaystyle {\mathbf{q}}^{\prime }=\mathbf{q}\otimes \delta \mathbf{q}}}$$

对应局部/刚体系误差。

左扰动：

$$\boxed{{\displaystyle {\mathbf{q}}^{\prime }=\delta \mathbf{q}\otimes \mathbf{q}}}$$

对应世界系误差。

欧拉角

常见航空 ZYX 形式：

$$\mathbf{R}={\mathbf{R}}_z(\psi ){\mathbf{R}}_y(\theta ){\mathbf{R}}_x(\varphi )$$

其中

• $\varphi$：roll；
• $\theta$：pitch；
• $\psi$：yaw。

角速度一般不能直接写成

$$\mathit{\omega }=\left[\begin{array}{c}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right].$$

而是

$$\boxed{{\displaystyle \mathit{\omega }=\mathbf{G}(\varphi ,\theta ,\psi )\dot{\mathit{\eta }}}}$$

其中

$$\mathit{\eta }=\left[\begin{array}{c}\varphi \\ \theta \\ \psi \end{array}\right].$$

Warning

三参数欧拉角表示必然存在奇异位形。

例如 ZYX 欧拉角在

$$\theta =\pm \frac{\pi }{2}$$

附近发生奇异。

因此动力学、优化和状态估计内部通常优先采用旋转矩阵、四元数或李群表示。

SO(3) 对数映射

对于

$$\mathbf{R}\in SO(3)$$

定义

$$\mathit{\varphi }=\mathrm{Log}(\mathbf{R}{)}^{\vee }.$$

旋转角：

$$\theta ={\cos }^{-1}\left(\frac{\mathrm{tr}(\mathbf{R})-1}{2}\right).$$

在非奇异情况下：

$$\boxed{{\displaystyle \mathit{\varphi }=\frac{\theta }{2\sin \theta }(\mathbf{R}-{\mathbf{R}}^{\mathrm{\top }}{)}^{\vee }}}$$

其中 $\vee$ 为反对称矩阵到向量的映射：

$$[\mathbf{a}{]}_{\times }^{\vee }=\mathbf{a}.$$

SE(3) 齐次变换

刚体位姿：

$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{p}\\ {\mathbf{0}}^{\mathrm{\top }}& 1\end{array}\right]\in SE(3).$$

对点 $\mathbf{a}$：

$$\boxed{{\displaystyle \mathbf{x}=\mathbf{R}\mathbf{a}+\mathbf{p}}}$$

齐次形式：

$$\left[\begin{array}{c}\mathbf{x}\\ 1\end{array}\right]=\mathbf{T}\left[\begin{array}{c}\mathbf{a}\\ 1\end{array}\right].$$

逆变换：

$$\boxed{{\displaystyle {\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathrm{\top }}& -{\mathbf{R}}^{\mathrm{\top }}\mathbf{p}\\ 0& 1\end{array}\right]}}$$

Twist 与 se(3)

本文采用

$$\mathit{\xi }=\left[\begin{array}{c}\mathit{\omega }\\ \mathbf{v}\end{array}\right]\in {\mathbb{R}}^6.$$

hat 运算：

$$\boxed{{\displaystyle {\mathit{\xi }}^{\wedge }=\left[\begin{array}{cc}[\mathit{\omega }{]}_{\times }& \mathbf{v}\\ {\mathbf{0}}^{\mathrm{\top }}& 0\end{array}\right]}}$$

位姿的时间导数

刚体系 twist

如果

$${\mathit{\xi }}_b=\left[\begin{array}{c}{\mathit{\omega }}_b\\ {\mathbf{v}}_b\end{array}\right]$$

则

$$\boxed{{\displaystyle \dot{\mathbf{T}}=\mathbf{T}{\mathit{\xi }}_b^{\wedge }}}$$

因此

$$\boxed{{\displaystyle {\mathit{\xi }}_b^{\wedge }={\mathbf{T}}^{-1}\dot{\mathbf{T}}}}$$

世界系 twist

如果

$${\mathit{\xi }}_s=\left[\begin{array}{c}{\mathit{\omega }}_s\\ {\mathbf{v}}_s\end{array}\right]$$

则

$$\boxed{{\displaystyle \dot{\mathbf{T}}={\mathit{\xi }}_s^{\wedge }\mathbf{T}}}$$

因此

$$\boxed{{\displaystyle {\mathit{\xi }}_s^{\wedge }=\dot{\mathbf{T}}{\mathbf{T}}^{-1}}}$$

伴随矩阵

李群伴随矩阵 Ad

设

$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{p}\\ 0& 1\end{array}\right]$$

对于

$$\mathit{\xi }=\left[\begin{array}{c}\mathit{\omega }\\ \mathbf{v}\end{array}\right]$$

其伴随矩阵为

$$\boxed{{\displaystyle {\mathrm{Ad}}_{\mathbf{T}}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{0}\\ [\mathbf{p}{]}_{\times }\mathbf{R}& \mathbf{R}\end{array}\right]}}$$

用于不同坐标系之间转换 twist：

$$\boxed{{\displaystyle {\mathit{\xi }}_s={\mathrm{Ad}}_{\mathbf{T}}{\mathit{\xi }}_b}}$$

逆关系：

$$\boxed{{\displaystyle {\mathit{\xi }}_b={\mathrm{Ad}}_{{\mathbf{T}}^{-1}}{\mathit{\xi }}_s}}$$

伴随满足

$${\mathrm{Ad}}_{{\mathbf{T}}_1{\mathbf{T}}_2}={\mathrm{Ad}}_{{\mathbf{T}}_1}{\mathrm{Ad}}_{{\mathbf{T}}_2}.$$

李代数伴随矩阵 ad

对于

$$\mathit{\xi }=\left[\begin{array}{c}\mathit{\omega }\\ \mathbf{v}\end{array}\right]$$

定义

$$\boxed{{\displaystyle {\mathrm{ad}}_{\mathit{\xi }}=\left[\begin{array}{cc}[\mathit{\omega }{]}_{\times }& \mathbf{0}\\ [\mathbf{v}{]}_{\times }& [\mathit{\omega }{]}_{\times }\end{array}\right]}}$$

满足

$$[{\mathit{\xi }}_1^{\wedge },{\mathit{\xi }}_2^{\wedge }]={\left({\mathrm{ad}}_{{\mathit{\xi }}_1}{\mathit{\xi }}_2\right)}^{\wedge }.$$

即

$${\mathrm{ad}}_{{\mathit{\xi }}_1}{\mathit{\xi }}_2$$

对应两个 twist 的 Lie bracket。

Wrench 的坐标变换

采用

$$\mathbf{F}=\left[\begin{array}{c}\mathit{\tau }\\ \mathbf{f}\end{array}\right].$$

为了保持功率

$${\mathbf{F}}^{\mathrm{\top }}\mathit{\xi }$$

在坐标变换下不变，wrench 使用 twist 的对偶变换：

$$\boxed{{\displaystyle {\mathbf{F}}_s={\mathrm{Ad}}_{\mathbf{T}}^{-T}{\mathbf{F}}_b}}$$

反之

$$\boxed{{\displaystyle {\mathbf{F}}_b={\mathrm{Ad}}_{\mathbf{T}}^T{\mathbf{F}}_s}}$$

Info

这就是 Featherstone Spatial Vector Algebra 中：

• spatial motion vector；
• spatial force vector；

采用不同变换规律的根本原因。

点速度与刚体 Jacobian

若刚体上的固定点 $\mathbf{a}$ 在世界系位置为

$$\mathbf{x}=\mathbf{p}+\mathbf{R}\mathbf{a}$$

则

$$\dot{\mathbf{x}}=\dot{\mathbf{p}}+{\mathit{\omega }}_s\times (\mathbf{R}\mathbf{a}).$$

因此

$$\boxed{{\displaystyle \dot{\mathbf{x}}={\mathbf{v}}_O-[\mathbf{R}\mathbf{a}{]}_{\times }{\mathit{\omega }}_s}}$$

若 twist 排列为

$$\mathbf{V}=\left[\begin{array}{c}{\mathbf{v}}_O\\ {\mathit{\omega }}_s\end{array}\right]$$

则点速度 Jacobian 为

$$\boxed{{\displaystyle \dot{\mathbf{x}}=\left[\begin{array}{cc}\mathbf{I}& -[\mathbf{R}\mathbf{a}{]}_{\times }\end{array}\right]\mathbf{V}}}$$

刚体点加速度

对于刚体固定点：

$$\boxed{{\displaystyle \ddot{\mathbf{x}}={\mathbf{a}}_O+\mathit{\alpha }\times \mathbf{r}+\mathit{\omega }\times (\mathit{\omega }\times \mathbf{r})}}$$

其中

$$\mathbf{r}=\mathbf{R}\mathbf{a}.$$

三项分别为：

• 参考点平动加速度；
• 切向加速度；
• 向心加速度。

惯性张量的坐标变换

若刚体系惯性张量为

$${\mathbf{I}}_b$$

则世界系表示为

$$\boxed{{\displaystyle {\mathbf{I}}_s=\mathbf{R}{\mathbf{I}}_b{\mathbf{R}}^{\mathrm{\top }}}}$$

时间导数：

$$\boxed{{\displaystyle {\dot{\mathbf{I}}}_s=[{\mathit{\omega }}_s{]}_{\times }{\mathbf{I}}_s-{\mathbf{I}}_s[{\mathit{\omega }}_s{]}_{\times }}}$$

因此角动量

$$\mathbf{H}={\mathbf{I}}_s{\mathit{\omega }}_s$$

满足

$$\mathit{\tau }=\dot{\mathbf{H}}.$$

在刚体系中则得到经典 Euler 方程：

$$\boxed{{\displaystyle {\mathit{\tau }}_b={\mathbf{I}}_b{\dot{\mathit{\omega }}}_b+{\mathit{\omega }}_b\times ({\mathbf{I}}_b{\mathit{\omega }}_b)}}$$

常用矩阵微分

逆矩阵：

$$\boxed{{\displaystyle \frac{d}{dt}{\mathbf{A}}^{-1}=-{\mathbf{A}}^{-1}\dot{\mathbf{A}}{\mathbf{A}}^{-1}}}$$

转置：

$$\frac{d}{dt}{\mathbf{A}}^{\mathrm{\top }}={\dot{\mathbf{A}}}^{\mathrm{\top }}.$$

矩阵乘积：

$$\frac{d}{dt}(\mathbf{A}\mathbf{B})=\dot{\mathbf{A}}\mathbf{B}+\mathbf{A}\dot{\mathbf{B}}.$$

二次型：

$$f={\mathbf{x}}^{\mathrm{\top }}\mathbf{A}\mathbf{x}$$

若 $\mathbf{A}$ 与 $\mathbf{x}$ 无关，则

$$\boxed{{\displaystyle \frac{\partial f}{\partial \mathbf{x}}=(\mathbf{A}+{\mathbf{A}}^{\mathrm{\top }})\mathbf{x}}}$$

若 $\mathbf{A}$ 对称：

$$\frac{\partial f}{\partial \mathbf{x}}=2\mathbf{A}\mathbf{x}.$$

向量归一化的 Jacobian

令

$$\hat{\mathbf{a}}=\frac{\mathbf{a}}{\parallel \mathbf{a}\parallel }$$

则

$$\boxed{{\displaystyle \frac{\partial \hat{\mathbf{a}}}{\partial \mathbf{a}}=\frac{1}{\parallel \mathbf{a}\parallel }(\mathbf{I}-\hat{\mathbf{a}}{\hat{\mathbf{a}}}^{\mathrm{\top }})}}$$

这个公式在：

• 构造单位旋转轴；
• 方向向量约束；
• 几何 Jacobian；
• 传感器模型；

中非常常见。

常用 SO(3) 微分总结

右扰动：

$${\mathbf{R}}^{\prime }=\mathbf{R}\mathrm{Exp}(\delta \mathit{\theta })$$$$\delta \mathbf{R}=\mathbf{R}[\delta \mathit{\theta }{]}_{\times }$$$$\delta (\mathbf{R}\mathbf{a})=-\mathbf{R}[\mathbf{a}{]}_{\times }\delta \mathit{\theta }.$$

左扰动：

$${\mathbf{R}}^{\prime }=\mathrm{Exp}(\delta \mathit{\theta })\mathbf{R}$$$$\delta \mathbf{R}=[\delta \mathit{\theta }{]}_{\times }\mathbf{R}$$$$\delta (\mathbf{R}\mathbf{a})=-[\mathbf{R}\mathbf{a}{]}_{\times }\delta \mathit{\theta }.$$

时间微分：

$$\dot{\mathbf{R}}=\mathbf{R}[{\mathit{\omega }}_b{]}_{\times }=[{\mathit{\omega }}_s{]}_{\times }\mathbf{R}.$$

常用四元数微分总结

向量旋转：

$${\mathbf{a}}_s=\mathbf{q}\otimes {\mathbf{a}}_b\otimes {\mathbf{q}}^{\ast }$$

刚体系角速度：

$$\boxed{{\displaystyle \dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes {\mathit{\omega }}_b}}$$

世界系角速度：

$$\boxed{{\displaystyle \dot{\mathbf{q}}=\frac{1}{2}{\mathit{\omega }}_s\otimes \mathbf{q}}}$$

其中 $\mathit{\omega }$ 在四元数乘法中视为

$$\left[\begin{array}{c}0\\ \mathit{\omega }\end{array}\right].$$

右小扰动：

$${\mathbf{q}}^{\prime }=\mathbf{q}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathit{\theta }\end{array}\right].$$

左小扰动：

$${\mathbf{q}}^{\prime }=\left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathit{\theta }\end{array}\right]\otimes \mathbf{q}.$$

刚体上点的速度

刚体上一点

$$\mathbf{r}=\mathbf{R}+\mathbf{A}\mathbf{u}$$

的速度可以统一写成

$$\dot{\mathbf{r}}=\dot{\mathbf{R}}+\mathit{\omega }\times (\mathbf{A}\mathbf{u}).$$

这些运动学关系是第三章建立质量矩阵、广义力和刚体动力学的基础。

常用的公式链

刚体姿态：

$$\mathbf{q}\leftrightarrow \mathbf{R}\leftrightarrow \mathrm{Exp}(\mathit{\varphi })$$

姿态速度：

$$\dot{\mathbf{q}}\leftrightarrow \mathit{\omega }\leftrightarrow \dot{\mathbf{R}}$$

其中

$$\dot{\mathbf{R}}=\mathbf{R}[{\mathit{\omega }}_b{]}_{\times }$$

刚体位姿：

$$\mathbf{T}\in SE(3)$$

速度：

$$\dot{\mathbf{T}}=\mathbf{T}{\mathit{\xi }}_b^{\wedge }$$

坐标系转换：

$${\mathit{\xi }}_s={\mathrm{Ad}}_{\mathbf{T}}{\mathit{\xi }}_b$$

局部微分：

$$\delta \mathbf{R}=\mathbf{R}[\delta \mathit{\theta }{]}_{\times }$$

因此

$$\delta (\mathbf{R}\mathbf{a})=-\mathbf{R}[\mathbf{a}{]}_{\times }\delta \mathit{\theta }.$$

这几条公式基本贯穿：

• 刚体动力学；
• 多体动力学；
• Featherstone Spatial Vector Algebra；
• ESKF；
• SLAM；
• Lie-group state estimation；
• robot Jacobian；
• NMPC线性化；
• 连续体机器人SE(3)运动学。