罗德里格斯公式简要介绍

一、罗德里格斯公式（Rodrigues’ rotation formula）是一个用于计算绕任意轴旋转向量的数学公式。它是由欧仁·罗德里格斯（Olinde Rodrigues）在1840年提出的。这个公式在三维空间中描述了一个向量绕着单位向量旋转θ角度后的结果。

设v是一个待旋转的三维向量，k是单位旋转轴（必须是单位向量），θ是旋转角度（通常以弧度为单位），则v绕k旋转θ角度后的向量v’可以通过罗德里格斯公式计算得到：

v’ = v * cos(θ) + (k × v) * sin(θ) + k * (k ⋅ v) * (1 - cos(θ))

其中：

• v’ 是旋转后的向量。

• cos 和 sin 分别表示余弦和正弦函数。

• × 表示向量叉乘。

• ⋅ 表示向量点乘。

罗德里格斯公式是由以下几个部分组成的：

1. v * cos(θ)：v 在旋转轴平行方向的分量，在旋转中不会改变。

2. (k × v) * sin(θ)：v 在垂直于旋转轴的平面上的分量，绕轴旋转后的分量。

3. k * (k ⋅ v) * (1 - cos(θ))：v 在旋转轴方向上的分量，这部分分量在旋转中增强了旋转轴方向的分量。

在图形学、机器人技术、航空航天和其他需要旋转向量的领域中，罗德里格斯公式是一种简单而有效的计算工具。此外，罗德里格斯公式也常被用来构造旋转矩阵或四元数，这在实现三维旋转时非常有用。

二、从轴角变换到旋转矩阵可以使用罗德里格斯公式进行转换

罗德里格斯公式可以用来从轴角表示（axis-angle representation）转换到旋转矩阵。轴角表示是一种描述旋转的方式，它使用一个单位旋转轴 k 和旋转角度 θ 来表示三维空间中的旋转。

给定单位旋转轴 k = (kx, ky, kz).T和旋转角度 θ，可以通过罗德里格斯旋转公式构造对应的旋转矩阵 R：

R = I + sin(θ) * K + (1 - cos(θ)) * K^2；

有的教材也写成：

R = cos(θ) * I + sin(θ) * K + (1 - cos(θ)) * (k * k.T)，其中k = (kx, ky, kz).T为列向量

这两个公式其实是一样的。

这里：

• I 是单位矩阵。

• K 是根据旋转轴 k 定义的反对称矩阵。其形式如下：

• K^2 表示矩阵 K 与自身的矩阵乘积。

将 K 和 K^2 计算出来并代入上述公式，就可以得到完整的旋转矩阵 R。这个矩阵可以用来作用于任意向量 v，即通过矩阵-向量乘法 Rv 来实现向量 v 绕轴 k 旋转 θ 角度的效果。

python案例

R = I + sin(θ) * K + (1 - cos(θ)) * K^2；

import numpy as np
# 这边没有转变成列向量，因为不需要，之后都是大K的计算
k = np.array([1, 0, 0])
# x轴旋转90度
theta = np.pi / 2
K = np.array([[0, -k[2], k[1]],
              [k[2], 0, -k[0]],
              [-k[1], k[0], 0]])
K_squared = np.dot(K, K)
# 通过罗德里格斯公式计算旋转矩阵
R = np.eye(3) + (1 - np.cos(theta)) * K_squared + np.sin(theta) * K
print(np.round(R, 5))

R = cos(θ) * I + sin(θ) * K + (1 - cos(θ)) * (k * k.T)

import numpy as np
# 这边特意把k转变成列向量
k = np.array([1, 0, 0]).reshape((-1, 1))
theta = np.pi / 2
K = np.array([[0, -k[2][0], k[1][0]],
              [k[2][0], 0, -k[0][0]],
              [-k[1][0], k[0][0], 0]])
# K_squared = np.dot(K, K)
print(k, k.T)
K_squared = np.dot(k, k.T)
print(K_squared)
R = np.cos(theta) * np.eye(3) + (1 - np.cos(theta)) * K_squared + np.sin(theta) * K
print(np.round(R, 5))

结果都是一样的，最多就是精度有所差别