机器人 / 系统课时

检查旋转矩阵是否正交

检查旋转矩阵是否正交

机器人姿态经过连续计算后,矩阵元素可能逐渐偏离合法旋转,导致坐标轴长度、夹角和求逆结果失真。本篇用正交性与行列式两项检查区分正常旋转、数值漂移和镜像反射,并通过可运行的 Python 示例检测与校正。

学习目标

  • 写出三维旋转矩阵的正交性与行列式判据。
  • 计算矩阵的正交误差,识别小幅数值漂移与镜像反射。
  • 用奇异值分解校正接近合法旋转的矩阵,并重新验收。

通过正交性和行列式判定

三维旋转矩阵 RR 必须是 3×33\times3 实矩阵,满足两个条件:

RTR=I3,det⁡(R)=+1R^{\mathsf T}R=I_3,\qquad \det(R)=+1

第一个条件表示矩阵的三列都是单位向量且两两垂直,因此保留长度和夹角,逆矩阵可由转置得到:R−1=RTR^{-1}=R^{\mathsf T}。第二个条件保证坐标轴仍为右手系。仅检查正交性会误放行行列式为 −1-1 的镜像反射;仅检查行列式则无法排除列向量不垂直的矩阵。

浮点数不宜直接比较是否严格相等。定义正交误差 eorth=∥RTR−I3∥Fe_{\mathrm{orth}}=\lVert R^{\mathsf T}R-I_3\rVert_F,其中 ∥⋅∥F\lVert\cdot\rVert_F 是 Frobenius 范数;再计算 edet=∣det⁡(R)−1∣e_{\mathrm{det}}=|\det(R)-1|。下文对 float64 教学样例使用两项误差均小于 10−610^{-6} 的阈值,实际系统须按精度及误差预算选取。

从三根基向量看出两类错误

设 R=[r1 r2 r3]R=[\boldsymbol r_1\ \boldsymbol r_2\ \boldsymbol r_3],合法旋转要求每列长度为 11,任意两列的点积为 00,并保持右手关系。连续积分或重复矩阵相乘时,舍入误差可能使列向量稍微倾斜或伸长;即便行列式接近 11,也不能直接当成合法姿态。

另一个容易混淆的例子是 diag⁡(1,1,−1)\operatorname{diag}(1,1,-1):它满足 RTR=I3R^{\mathsf T}R=I_3,但行列式为 −1-1,表示翻转了一个坐标轴,不属于三维纯旋转。

旋转基向量的正交性、数值漂移与镜像反射对比

用 NumPy 检测并修正漂移

准备 Python 3.10 及以上版本和 NumPy(1.26 或 2.x)。将下面代码保存为 check_rotation.py,执行 python check_rotation.py。示例分别测试绕 Z 轴旋转 30∘30^\circ、人为加入 0.0020.002 元素偏移后的矩阵,以及 Z 轴反射矩阵。最后只对已知来自旋转计算的小幅漂移做 SVD 正交化。

python
import numpy as np

TOL = 1e-6

def inspect(name, matrix):
    # 正交性与行列式必须同时满足,避免将镜像误判为旋转。
    error = np.linalg.norm(matrix.T @ matrix - np.eye(3), ord="fro")
    determinant = np.linalg.det(matrix)
    valid = error < TOL and abs(determinant - 1.0) < TOL
    print(f"{name}: 正交误差={error:.6f}, 行列式={determinant:.6f}, 合法={valid}")

def project_to_rotation(matrix):
    # SVD 投影到 det=+1 的正交矩阵集合,而非简单归一化每一列。
    u, _, vt = np.linalg.svd(matrix)
    return u @ np.diag([1.0, 1.0, np.linalg.det(u @ vt)]) @ vt

angle = np.deg2rad(30.0)
c, s = np.cos(angle), np.sin(angle)
rotation = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])

# 模拟累积计算中某个元素发生小幅漂移。
drifted = rotation.copy()
drifted[0, 0] += 0.002
reflection = np.diag([1.0, 1.0, -1.0])

inspect("正常旋转", rotation)
inspect("数值漂移", drifted)
inspect("镜像反射", reflection)
inspect("漂移修正", project_to_rotation(drifted))

预期输出:

text
正常旋转: 正交误差=0.000000, 行列式=1.000000, 合法=True
数值漂移: 正交误差=0.003745, 行列式=1.001732, 合法=False
镜像反射: 正交误差=0.000000, 行列式=-1.000000, 合法=False
漂移修正: 正交误差=0.000000, 行列式=1.000000, 合法=True

打印的六位小数会将正常浮点残差显示为 0.000000,不代表误差在数学上严格为零。SVD 得到的修正矩阵是按 Frobenius 距离投影到合法旋转集合的结果,修正后仍须重新检查两项指标。

判断该修还是该追查源头

如果只是多次姿态更新后产生小幅误差,且能确认原始量本应为旋转矩阵,可以在控制误差规模后进行 SVD 投影。本例修正前正交误差约 0.0037450.003745,行列式仍接近 11;演示时可设置修复入口条件 eorth<0.01e_{\mathrm{orth}}<0.01 且 edet<0.01e_{\mathrm{det}}<0.01,与最终验收阈值 10−610^{-6} 区分。

若行列式接近 −1-1,优先检查左右手坐标约定、轴取向或坐标转换顺序;这不是普通浮点漂移。若误差突然变大,应追查角度单位是否把度误当弧度、矩阵相乘顺序是否颠倒,以及是否将缩放矩阵混进姿态。不能依靠正交化掩盖这些模型错误。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

检查旋转矩阵是否正交

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通