【机器人学】机器人旋转的数学魔法：Rodrigues公式全解析！

机器人学：Rodrigues 公式

---

---

0 引言

Rodrigues公式：描述三维空间旋转的数学工具，用旋转轴和旋转角（轴角）来计算旋转后的向量或构造旋转矩阵，避免欧拉角的万向锁问题。

在机器人控制中，我们经常需要计算：

• 机械臂关节旋转后，末端执行器的位置（比如机器人拿杯子时手腕怎么转）。
• 无人机/机器人的姿态变化（比如四旋翼从水平变成倾斜飞行）。

Rodrigues 公式的作用：告诉你“某个物体绕一根轴转一定角度后，它的新位置 / 朝向”。

---

1 定义

Rodrigues 公式的两种等价形式：

1. 向量形式

给定：旋转轴为单位向量$\hat{\mathbf{\omega }}$，旋转角为$\theta$，即计算绕单位向量 $\hat{\mathbf{\omega }}$ 旋转 $\theta$ 后的向量 ${\mathbf{r}}^{\prime }$：
$${\mathbf{r}}^{\prime }=\mathbf{r}\cos \theta +(1-\cos \theta )(\hat{\mathbf{\omega }}\cdot \mathbf{r})\hat{\mathbf{\omega }}+\sin \theta (\hat{\mathbf{\omega }}\times \mathbf{r})$$
2. 矩阵形式

通常可用旋转矩阵 $\mathbf{R}$ 表示：${\mathbf{r}}^{\prime }=\mathbf{R}\cdot \mathbf{r}$

其中，$$\mathbf{R}=e^{[\hat{\mathbf{\omega }}]\theta }=\mathbf{I}+[\hat{\mathbf{\omega }}]+(1-cos\theta )[\hat{\mathbf{\omega }}{]}^2$$

（$[\hat{\mathbf{\omega }}]$ 是单位向量 $\hat{\mathbf{\omega }}$ 对应的反对称矩阵）

---

2 推导

2.1 代数方法

需要证明：$$\begin{array}{r}\mathbf{R}=e^{[\hat{\mathbf{\omega }}]\theta }=\mathbf{I}+[\hat{\mathbf{\omega }}]sin\theta +[\hat{\mathbf{\omega }}{]}^2(1-cos\theta )\end{array}$$

前提 1：泰特公式

$f(x)$ 在 $x=0$处的泰特展开：

$$\begin{array}{rl}f(x)& =\sum _{i=0}^{\infty }\frac{f^{(i)}(0)}{i!}{x}^i\\ & =f(0)+\frac{}{1!}\end{array}$$