机器人逆运动学进阶：李代数、矩阵指数与旋转流形计算
做机器人逆运动学（IK）的时候，你迟早会遇到矩阵指数和对数这些东西。为什么呢？因为计算三维旋转的误差，不能简单地用欧氏距离那一套，那只对位置有效。旋转得用另一套方法——你需要算两个旋转矩阵之间的差异，这就涉及到矩阵对数了。
这篇文章就是要把这事儿说清楚：从旋转矩阵构成的李群开始，到流形和切空间，再到怎么用叉积算旋转矩阵的导数，如何对旋转矩阵做增量更新，最后是如何计算从一个姿态到另一个姿态需要的角速度。
SO(3)：三维旋转的数学结构
机器人的姿态其实就是一个三维坐标系。这个坐标系由三个互相垂直、长度为1的向量组成，比如最简单的情况：(1,0,0)ᵀ、(0,1,0)ᵀ、(0,0,1)ᵀ。把这三个向量当作列向量排在一起，就得到了一个3×3的旋转矩阵。
虽然这个矩阵在数学上属于ℝ³ˣ³空间，但实际上能表示旋转的矩阵组合要少得多，它们形成一个特殊的群，叫做三维特殊正交群SO(3)。类似的还有二维旋转SO(2)、四维变换矩阵等等，这些都属于李群家族。
流形和切空间：几何概念
实数是一条直线，但SO(3)不是，它在ℝ³ˣ³里形成一个弯曲的流形。这个流形是连续的、可微的，你可以在上面定义距离。想在这个弯曲的表面上移动，就得沿着切线方向走，这就引出了切空间的概念。
就像函数的切线斜率由导数决定一样，我们也可以通过计算流形切线的"斜率"来得到它的导数。
计算旋转矩阵的导数
现在问题来了：一个旋转矩阵R和一个角速度向量ω，怎么算R的变化率Ṙ？
答案是这样的：
这里⎣ω⎦是ω对应的反对称矩阵：
为什么要搞成反对称矩阵？因为实际上是想算ω和R每一列的叉积。有时候你会看到这个操作写成帽子符号，都是一个意思。
所有3×3实反对称矩阵构成的空间叫做𝖘𝖔(3)，这是SO(3)的李代数，也就是SO(3)在单位矩阵处的切空间。虽然它在矩阵乘法下不构成群，但它是个李代数，让我们能在非线性的SO(3)流形上做线性计算。
叉积的几何意义
我们来仔细看看叉积ω×v是什么意思。两个向量的叉积结果是一个同时垂直于这两个向量的新向量，长度正比于两向量夹角的正弦值。
把ω变成反对称矩阵形式，就能用矩阵乘法实现叉积运算：
对矩阵R做这个操作，就等于对R的每一列都做一次叉积。
这里有个细节要注意：这个运算可以左乘也可以右乘，取决于ω是在机体坐标系还是世界坐标系下给出的。一般我们假设ω在机体坐标系，所以⎣ω⎦右乘到R上。
把ω想象成旋转轴，|ω|是转速，这样理解叉积就直观多了。如果v指向某个点，那么ω×v就是这个点绕ω轴旋转时的速度方向。
矩阵指数：精确的旋转更新
有了上面的理解，我们再看这个微分方程：
R的每一列代表一个坐标轴，和ω做叉积得到的矩阵描述了各个轴因为旋转产生的速度。那怎么得到更新后的旋转矩阵呢？
一个朴素的想法是用欧拉积分，就是把R⎣ω⎦乘个小时间步长然后加起来。但这样做有问题：无法保证结果还是正交矩阵，也就是说可能跳出SO(3)。虽然可以每步都强制归一化，但有更精确的方法。
上面那个微分方程其实是个线性常微分方程，解是：
矩阵指数函数通过泰勒展开定义，大部分线性代数库都有实现。
看看用这个方法绕ω=(1.0,1.0,1.0)ᵀ旋转的效果：
矩阵对数：解决逆运动学问题
有时候我们需要反过来算：给定当前姿态R(0)和目标姿态R(t)，需要什么旋转向量才能从一个到另一个？这就像算位置的欧氏距离一样。
对于旋转，我们把R(0)移到等式另一边，算矩阵对数，再除以时间t：
但事情还没完。真正的逆运动学问题需要算误差对关节角的导数。对于n个关节的机器人，我们要算一个3×n的雅可比矩阵：
要对R(t)关于q求导，得先过一遍对数，然后用链式法则。具体怎么做，作者说在另一篇文章里有。
代码实现
下面是生成上面动画的代码。关键是用scipy的expm函数实现矩阵指数，核心更新就是一行：
R = R @ expm(hat(omega) * dt)
。
import numpy as np
from scipy.linalg import expm
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from matplotlib.animation import FuncAnimation def hat(omega): return np.array([ [0, -omega[2], omega[1]], [omega[2], 0, -omega[0]], [-omega[1], omega[0], 0] ]) # Angular velocity vector (rad/s)
omega = np.array([1.0, 1.0, 1.0]) dt = 0.05
T = 5
N = int(T / dt) # Initialize rotation matrix as identity
R = np.eye(3) # Store rotation matrices
Rs = [R.copy()]
for _ in range(N): R = R @ expm(hat(omega) * dt) Rs.append(R.copy()) fig = plt.figure()
ax = fig.add_subplot(111, projection='3d') # Set plot limits and labels
ax.set_xlim([-1.5, 1.5])
ax.set_ylim([-1.5, 1.5])
ax.set_zlim([-1.5, 1.5])
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
ax.set_title('Rotating 3D Coordinate Frame') # Initialize quiver objects for x, y, z axes
quiver_x = ax.quiver(0, 0, 0, 1, 0, 0, color='r', length=1, normalize=True)
quiver_y = ax.quiver(0, 0, 0, 0, 1, 0, color='g', length=1, normalize=True)
quiver_z = ax.quiver(0, 0, 0, 0, 0, 1, color='b', length=1, normalize=True) def update(num): # Remove previous quivers (if any) for artist in ax.collections: artist.remove() R = Rs[num] x_axis = R[:, 0] y_axis = R[:, 1] z_axis = R[:, 2] ax.quiver(0, 0, 0, *x_axis, color='r', length=1, normalize=True) ax.quiver(0, 0, 0, *y_axis, color='g', length=1, normalize=True) ax.quiver(0, 0, 0, *z_axis, color='b', length=1, normalize=True) return [] ani = FuncAnimation(fig, update, frames=len(Rs), interval=50, blit=False) from matplotlib.animation import PillowWriter
ani.save("rotation.gif", writer=PillowWriter(fps=20)) plt.show()
总结
本文从理论到实践全面介绍了矩阵指数在机器人逆运动学中的应用。我们从SO(3)李群和𝖘𝖔(3)李代数的数学结构出发，解释了为什么旋转计算需要特殊的数学工具。通过流形和切空间的几何概念，我们理解了旋转矩阵导数的计算方法，并学会了使用反对称矩阵实现叉积运算。矩阵指数函数提供了精确的旋转更新方法，能够保证旋转矩阵在更新过程中始终保持正交性，避免了传统欧拉积分可能导致的数值不稳定问题。而矩阵对数则解决了逆向问题——如何计算从当前姿态到目标姿态所需的旋转误差和角速度。这套方法的核心优势在于能够在非线性的旋转空间中进行精确计算，为逆运动学算法提供了必要的数学工具，特别是在计算雅可比矩阵时所需的旋转导数。对于需要高精度姿态控制的机器人应用，这些数学工具是必不可少的。
https://avoid.overfit.cn/post/352201d5dfd749e48e2b6ccf8e81abf0
作者:Nikolaus Correll