离散时间系统输入非零时四元数元素趋于无穷的问题求助

问题核心分析 #

离散时间仿真中四元数趋于无穷，本质是数值稳定性问题，结合你的场景（连续仿真正常、存在足部接触），大概率是以下原因之一：

• 四元数未做单位化：离散积分过程中，数值误差会累积导致四元数模长偏离1，姿态更新的数学逻辑失效，最终引发发散。
• 显式积分器步长过大：足部接触属于刚性约束系统，欧拉法等显式积分对刚性系统的稳定性差，步长不合适时会快速放大误差。
• 接触动力学离散实现错误：接触冲量、约束投影的计算不符合离散时间下的动量守恒，引入异常力矩推动姿态发散。

针对性解决建议 #

• 强制四元数归一化：在每个离散积分步结束后，立即将四元数除以其模长，确保始终保持单位长度。示例代码：

  # 假设q是当前四元数数组
  q_norm = np.linalg.norm(q)
  if q_norm > 1e-6:  # 避免除以零
      q = q / q_norm
  

• 更换稳定积分器：用隐式积分（如向后欧拉法）或高阶显式积分（如RK4）替代基础欧拉法，刚性系统下隐式积分的稳定性优势更明显。
• 检查接触逻辑实现：验证离散时间下接触力/冲量的计算是否正确，比如接触约束的雅克比矩阵投影、冲量更新的动量方程是否符合离散动力学规则，避免异常力矩输入。
• 缩小离散步长：暂时减小仿真步长dt，如果发散现象缓解，说明原步长超出了当前积分器的稳定域，可结合积分器优化调整步长。

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