机器人 / 系统课时
检查旋转矩阵是否正交
检查旋转矩阵是否正交
机器人姿态经过连续计算后,矩阵元素可能逐渐偏离合法旋转,导致坐标轴长度、夹角和求逆结果失真。本篇用正交性与行列式两项检查区分正常旋转、数值漂移和镜像反射,并通过可运行的 Python 示例检测与校正。
学习目标
- 写出三维旋转矩阵的正交性与行列式判据。
- 计算矩阵的正交误差,识别小幅数值漂移与镜像反射。
- 用奇异值分解校正接近合法旋转的矩阵,并重新验收。
通过正交性和行列式判定
三维旋转矩阵 必须是 实矩阵,满足两个条件:
第一个条件表示矩阵的三列都是单位向量且两两垂直,因此保留长度和夹角,逆矩阵可由转置得到:。第二个条件保证坐标轴仍为右手系。仅检查正交性会误放行行列式为 的镜像反射;仅检查行列式则无法排除列向量不垂直的矩阵。
浮点数不宜直接比较是否严格相等。定义正交误差 ,其中 是 Frobenius 范数;再计算 。下文对 float64 教学样例使用两项误差均小于 的阈值,实际系统须按精度及误差预算选取。
从三根基向量看出两类错误
设 ,合法旋转要求每列长度为 ,任意两列的点积为 ,并保持右手关系。连续积分或重复矩阵相乘时,舍入误差可能使列向量稍微倾斜或伸长;即便行列式接近 ,也不能直接当成合法姿态。
另一个容易混淆的例子是 :它满足 ,但行列式为 ,表示翻转了一个坐标轴,不属于三维纯旋转。

用 NumPy 检测并修正漂移
准备 Python 3.10 及以上版本和 NumPy(1.26 或 2.x)。将下面代码保存为 check_rotation.py,执行 python check_rotation.py。示例分别测试绕 Z 轴旋转 、人为加入 元素偏移后的矩阵,以及 Z 轴反射矩阵。最后只对已知来自旋转计算的小幅漂移做 SVD 正交化。
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))预期输出:
正常旋转: 正交误差=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 投影。本例修正前正交误差约 ,行列式仍接近 ;演示时可设置修复入口条件 且 ,与最终验收阈值 区分。
若行列式接近 ,优先检查左右手坐标约定、轴取向或坐标转换顺序;这不是普通浮点漂移。若误差突然变大,应追查角度单位是否把度误当弧度、矩阵相乘顺序是否颠倒,以及是否将缩放矩阵混进姿态。不能依靠正交化掩盖这些模型错误。