在计算机视觉、机器人学、分子动力学等领域，经常需要求解两个点集之间的最优旋转矩阵------即找到一个旋转矩阵R\mathbf{R}R，使得源点集经过旋转后与目标点集的误差最小（通常为均方误差）。Kabsch算法是解决这一问题的经典方法，由Wolfgang Kabsch于1976年提出，其核心通过奇异值分解（SVD，Singular Value Decomposition）实现旋转矩阵的高效、最优求解。
一、"最优旋转矩阵"定义
Kabsch算法的目标可表述为：
给定两组对应点集 （源点集{Pi}i=1n\{\mathbf{P}i\}{i=1}^n{Pi}i=1n和目标点集{Qi}i=1n\{\mathbf{Q}i\}{i=1}^n{Qi}i=1n，n≥3n \geq 3n≥3且点不共线），求解旋转矩阵R\mathbf{R}R（3×3正交矩阵，det(R)=1\det(\mathbf{R})=1det(R)=1），使得旋转后的源点集{RPi}\{\mathbf{R}\mathbf{P}i\}{RPi}与目标点集{Qi}\{\mathbf{Q}i\}{Qi}的均方误差（MSE）最小 ，即：
minR∑i=1n∥Qi−RPi∥2 \min{\mathbf{R}} \sum{i=1}^n \|\mathbf{Q}_i - \mathbf{R}\mathbf{P}_i\|^2 Rmini=1∑n∥Qi−RPi∥2
需注意：若点集间存在平移（即Qi=RPi+t\mathbf{Q}_i = \mathbf{R}\mathbf{P}_i + \mathbf{t}Qi=RPi+t，t\mathbf{t}t为平移向量），Kabsch算法需先消除平移影响（"中心化"），再求解旋转矩阵。因此，算法的完整目标是同时处理旋转和平移，找到R\mathbf{R}R和t\mathbf{t}t使误差最小，但核心是旋转矩阵的求解。
二、预处理：消除平移影响（中心化）
旋转矩阵仅描述绕原点的旋转，而实际点集可能存在平移。因此，第一步需通过"中心化"去除平移，将问题转化为"绕原点的旋转"。
步骤1：计算质心
- 源点集的质心（几何中心）：
CP=1n∑i=1nPi\mathbf{C}P = \frac{1}{n} \sum{i=1}^n \mathbf{P}_iCP=n1i=1∑nPi - 目标点集的质心：
CQ=1n∑i=1nQi\mathbf{C}Q = \frac{1}{n} \sum{i=1}^n \mathbf{Q}_iCQ=n1i=1∑nQi
步骤2：中心化点集
将所有点减去各自的质心，得到"去平移"后的点集：
pi=Pi−CP,qi=Qi−CQ \mathbf{p}_i = \mathbf{P}_i - \mathbf{C}_P, \quad \mathbf{q}_i = \mathbf{Q}_i - \mathbf{C}_Q pi=Pi−CP,qi=Qi−CQ
此时，点集间的变换关系简化为qi≈Rpi\mathbf{q}i \approx \mathbf{R}\mathbf{p}iqi≈Rpi（无平移），原目标函数可改写为：
minR∑i=1n∥qi−Rpi∥2 \min{\mathbf{R}} \sum{i=1}^n \|\mathbf{q}_i - \mathbf{R}\mathbf{p}_i\|^2 Rmini=1∑n∥qi−Rpi∥2
（注：平移向量t\mathbf{t}t可由t=CQ−RCP\mathbf{t} = \mathbf{C}_Q - \mathbf{R}\mathbf{C}_Pt=CQ−RCP直接计算，因此核心是求解R\mathbf{R}R。）
三、核心步骤：构造协方差矩阵与SVD分解
步骤1：构造协方差矩阵H\mathbf{H}H
中心化后的点集{pi}\{\mathbf{p}_i\}{pi}和{qi}\{\mathbf{q}i\}{qi}的协方差矩阵H\mathbf{H}H定义为：
H=∑i=1nqipiT \mathbf{H} = \sum{i=1}^n \mathbf{q}_i \mathbf{p}_i^T H=i=1∑nqipiT
其中，pi\mathbf{p}_ipi是3×1列向量，piT\mathbf{p}_i^TpiT是其转置（1×3行向量），因此H\mathbf{H}H是3×3矩阵。H\mathbf{H}H的物理意义是描述两个点集的"相关性"------行列元素越大，说明对应维度的点坐标变化越一致，是后续求解旋转矩阵的关键。
步骤2：奇异值分解（SVD）
对协方差矩阵H\mathbf{H}H进行奇异值分解（SVD）：
H=UΣVT \mathbf{H} = \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^T H=UΣVT
其中：
- U\mathbf{U}U和V\mathbf{V}V是3×3正交矩阵 （满足UTU=I\mathbf{U}^T\mathbf{U} = \mathbf{I}UTU=I，VTV=I\mathbf{V}^T\mathbf{V} = \mathbf{I}VTV=I）；
- Σ\boldsymbol{\Sigma}Σ是3×3对角矩阵，对角元素为奇异值σ1≥σ2≥σ3≥0\sigma_1 \geq \sigma_2 \geq \sigma_3 \geq 0σ1≥σ2≥σ3≥0，描述H\mathbf{H}H在对应正交方向上的"强度"。
步骤3：构造旋转矩阵R\mathbf{R}R
Kabsch算法的核心结论是：最优旋转矩阵可由SVD的结果直接构造 。初步旋转矩阵为：
R=UVT \mathbf{R} = \mathbf{U} \mathbf{V}^T R=UVT
但需满足旋转矩阵的基本性质：det(R)=1\det(\mathbf{R}) = 1det(R)=1（右手系旋转，无镜像）。若det(R)=−1\det(\mathbf{R}) = -1det(R)=−1（此时R\mathbf{R}R包含镜像变换，非纯旋转），需进行修正：
R=USVT \mathbf{R} = \mathbf{U} \mathbf{S} \mathbf{V}^T R=USVT
其中S\mathbf{S}S是修正矩阵：
S=[10001000det(UVT)] \mathbf{S} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(\mathbf{U}\mathbf{V}^T) \end{bmatrix} S= 10001000det(UVT)
当det(UVT)=−1\det(\mathbf{U}\mathbf{V}^T) = -1det(UVT)=−1时，S\mathbf{S}S的最后一个对角元为-1，修正后det(R)=1\det(\mathbf{R}) = 1det(R)=1，保证是纯旋转。
四、数学原理：为什么SVD能得到最优解？
Kabsch算法的最优性需通过目标函数的推导证明。将目标函数展开：
∑i=1n∥qi−Rpi∥2=∑i=1n(qiTqi−2qiTRpi+piTRTRpi) \sum_{i=1}^n \|\mathbf{q}_i - \mathbf{R}\mathbf{p}i\|^2 = \sum{i=1}^n (\mathbf{q}_i^T\mathbf{q}_i - 2\mathbf{q}_i^T\mathbf{R}\mathbf{p}_i + \mathbf{p}_i^T\mathbf{R}^T\mathbf{R}\mathbf{p}_i) i=1∑n∥qi−Rpi∥2=i=1∑n(qiTqi−2qiTRpi+piTRTRpi)
由于R\mathbf{R}R是正交矩阵（RTR=I\mathbf{R}^T\mathbf{R} = \mathbf{I}RTR=I），且∑qiTqi\sum \mathbf{q}_i^T\mathbf{q}_i∑qiTqi和∑piTpi\sum \mathbf{p}_i^T\mathbf{p}i∑piTpi是与R\mathbf{R}R无关的常数（旋转保距），目标函数的最小化等价于最大化 ：
∑i=1nqiTRpi=tr(R∑i=1npiqiT)=tr(RHT) \sum{i=1}^n \mathbf{q}_i^T\mathbf{R}\mathbf{p}i = \text{tr}(\mathbf{R} \sum{i=1}^n \mathbf{p}_i \mathbf{q}_i^T) = \text{tr}(\mathbf{R} \mathbf{H}^T) i=1∑nqiTRpi=tr(Ri=1∑npiqiT)=tr(RHT)
（tr(⋅)\text{tr}(\cdot)tr(⋅)表示矩阵的迹，即对角线元素之和，利用性质∑aiTbi=tr(∑biaiT)\sum \mathbf{a}_i^T\mathbf{b}_i = \text{tr}(\sum \mathbf{b}_i \mathbf{a}_i^T)∑aiTbi=tr(∑biaiT)）。
结合SVD结果H=UΣVT\mathbf{H} = \mathbf{U}\boldsymbol{\Sigma}\mathbf{V}^TH=UΣVT，则HT=VΣUT\mathbf{H}^T = \mathbf{V}\boldsymbol{\Sigma}\mathbf{U}^THT=VΣUT，代入上式得：
tr(RHT)=tr(RVΣUT)=tr(ΣUTRV) \text{tr}(\mathbf{R} \mathbf{H}^T) = \text{tr}(\mathbf{R} \mathbf{V}\boldsymbol{\Sigma}\mathbf{U}^T) = \text{tr}(\boldsymbol{\Sigma} \mathbf{U}^T \mathbf{R} \mathbf{V}) tr(RHT)=tr(RVΣUT)=tr(ΣUTRV)
令M=UTRV\mathbf{M} = \mathbf{U}^T \mathbf{R} \mathbf{V}M=UTRV（M\mathbf{M}M也是正交矩阵，因正交矩阵乘积仍正交），则上式简化为tr(ΣM)\text{tr}(\boldsymbol{\Sigma} \mathbf{M})tr(ΣM)。由于Σ\boldsymbol{\Sigma}Σ是对角矩阵，tr(ΣM)=∑σimii\text{tr}(\boldsymbol{\Sigma} \mathbf{M}) = \sum \sigma_i m_{ii}tr(ΣM)=∑σimii（miim_{ii}mii是M\mathbf{M}M的对角元）。
根据正交矩阵性质，∣mii∣≤1|m_{ii}| \leq 1∣mii∣≤1，因此最大值当且仅当mii=1m_{ii} = 1mii=1（即M=I\mathbf{M} = \mathbf{I}M=I）时取得，此时R=UVT\mathbf{R} = \mathbf{U} \mathbf{V}^TR=UVT。若det(UVT)=−1\det(\mathbf{U}\mathbf{V}^T) = -1det(UVT)=−1，则需修正以保证R\mathbf{R}R是旋转矩阵（而非镜像），此时最优解仍通过S\mathbf{S}S修正实现。
五、适用条件
1. 点集的要求
- 数量与分布 ：至少需要3个不共线 的点（三维），否则协方差矩阵H\mathbf{H}H的秩不足（< 3），无法唯一确定旋转矩阵。实际应用中通常使用更多点（如5-10个）以降低噪声影响。
- 对应关系 ：必须已知点的一一对应关系（Pi\mathbf{P}_iPi对应Qi\mathbf{Q}_iQi），若对应关系未知，需先通过ICP（迭代最近点）等算法匹配点集。
2. 噪声与鲁棒性
- 测量点集不可避免存在噪声（如相机像素误差、传感器噪声），Kabsch算法通过SVD对噪声有天然抑制作用------奇异值分解会"突出"主要变换方向，弱化噪声的影响。
- 若存在 outliers（错误点），可结合RANSAC算法：随机采样子集点求解R\mathbf{R}R，通过误差筛选最优子集，提高鲁棒性。
3. 二维情况的扩展
Kabsch算法可直接推广到二维：此时点集为2D，协方差矩阵H\mathbf{H}H是2×2矩阵，SVD分解后U\mathbf{U}U和V\mathbf{V}V为2×2正交矩阵，旋转矩阵R=UVT\mathbf{R} = \mathbf{U}\mathbf{V}^TR=UVT，并通过det(R)=1\det(\mathbf{R}) = 1det(R)=1验证（二维旋转矩阵行列式必为1）。
六、机器人领域应用场景
Kabsch算法因高效性和最优性，在多个领域有核心应用：
-
相机标定 ：求解相机外参（旋转矩阵）。通过棋盘格标定板的世界坐标点{Pi}\{\mathbf{P}_i\}{Pi}和图像坐标点{Qi}\{\mathbf{Q}_i\}{Qi}（经透视投影转换为3D），用Kabsch算法计算相机相对于世界坐标系的旋转。
-
机器人手眼标定 ：确定相机与机械臂末端的相对位姿。已知机械臂末端在基坐标系的位姿{Pi}\{\mathbf{P}_i\}{Pi}和相机拍摄的目标点{Qi}\{\mathbf{Q}_i\}{Qi}，通过Kabsch算法求解二者的旋转关系。
-
点云配准：在三维重建中，将不同视角的点云对齐。对两组点云先通过ICP匹配对应点，再用Kabsch算法求解旋转矩阵，实现点云融合。
手眼标定中，物理上只有 1 个标定板圆心 / 棋盘格角点，标定流程将一个点转换为点云：控制机械臂移动到 N 个不同位姿（N≥3，通常取 9~16 组）
每一组记录两个数据：末端在基坐标系下的位姿：BTFlangeB_T^{Flange}BTFlange（位置 + 旋转）
相机拍到的目标点在相机坐标系下的坐标：PcamP_{cam}Pcam
利用刚体变换不变性，把单一点扩展成两组匹配点云！
七、扩展：加权Kabsch算法
当点集中不同点的测量精度不同（如部分点噪声更大），可引入权重改进算法：
- 定义权重wi≥0w_i \geq 0wi≥0（精度高的点权重更大），构造加权协方差矩阵：
H=∑i=1nwiqipiT\mathbf{H} = \sum_{i=1}^n w_i \mathbf{q}_i \mathbf{p}_i^TH=i=1∑nwiqipiT - 后续SVD分解和旋转矩阵构造步骤与标准Kabsch算法一致。
加权Kabsch算法能更合理地利用点集信息，进一步提高旋转矩阵的求解精度。
Kabsch算法是求解最优旋转矩阵的标杆方法，其核心逻辑是：通过中心化去除平移影响，构造协方差矩阵描述点集相关性，利用SVD分解直接生成最优旋转矩阵，并通过行列式修正保证旋转的物理有效性。算法的数学严谨性（基于最小二乘最优解）和计算高效性（SVD可通过数值库快速实现）使其在科研和工程中被广泛采用。