如何生成平滑轨迹？平移矩阵可用B样条，旋转矩阵该如何处理？

旋转矩阵属于特殊正交群SO(3)的元素，直接对矩阵元素做插值或B样条拟合会破坏其正交性（行/列不再是单位正交向量），导致结果不是合法的旋转矩阵。必须先将旋转矩阵转换为旋转空间的连续参数化表示，完成平滑拟合后再转回旋转矩阵，以下是成熟的落地方案：

核心思路 #

将离散的旋转矩阵转换为四元数（最常用、无万向锁问题的表示），对四元数序列做球面空间的平滑样条拟合，最后再将拟合后的四元数转回旋转矩阵，与平移的B样条轨迹同步。

1. 旋转矩阵转四元数 #

每个合法的旋转矩阵都可以唯一对应一个单位四元数（q与-q表示同一旋转），用工具库可以快速完成转换：

import numpy as np
from scipy.spatial.transform import Rotation as Rot

# 示例旋转矩阵转四元数（输出[x, y, z, w]格式）
sample_R = np.array([[-0.97888774, -0.03123735, -0.20199756],
                     [0.08031109, 0.84999346, -0.52063542],
                     [0.18795988, -0.52586628, -0.82953948]])
rot = Rot.from_matrix(sample_R)
quaternion = rot.as_quat()

2. 四元数的平滑样条拟合 #

四元数是三维球面S³上的单位向量，不能直接用普通B样条，需用球面B样条或基于SLERP（球面线性插值）的扩展方法：

• 先统一四元数符号：遍历离散四元数序列，若相邻两个四元数的点积为负，将后一个四元数取反（保证插值走最短旋转路径）
• 构造球面B样条：对四元数序列生成样条曲线，每一段插值基于SLERP扩展，确保中间生成的四元数始终是单位长度，旋转轨迹平滑无突变
• 若用Python生态，可借助pyquaternion库实现四元数样条拟合：

from pyquaternion import Quaternion

# 假设已有离散四元数序列quats = [q0, q1, ..., qn]，对应时间点times = [t0, t1, ..., tn]
spline = Quaternion.spline(times, quats)
# 拟合任意时间点t的四元数
fitted_quat = spline(t)

3. 拟合后的四元数转回旋转矩阵 #

将平滑后的四元数直接转换为旋转矩阵，确保结果是合法的SO(3)元素：

# 四元数转旋转矩阵
fitted_rot = Rot.from_quat(fitted_quat)
fitted_R = fitted_rot.as_matrix()

替代方案：轴角表示 #

若场景特殊，也可以将旋转矩阵转换为轴角表示（一个单位轴向量+旋转角度）：

• 对旋转角度做普通B样条拟合，对轴向量做球面插值
• 最后将轴角转回旋转矩阵
  但四元数的工具链更成熟，且无万向锁问题，优先推荐四元数方案。

关键注意事项 #

• 禁止直接插值旋转矩阵元素：会导致矩阵失去正交性，出现拉伸、镜像等非法变换
• 避免欧拉角插值：欧拉角存在万向锁问题，插值时极易出现旋转跳变，仅在旋转范围完全避开万向锁区域时考虑

内容的提问来源于stack exchange，提问作者Lalalalex