问题描述
对于具有浮动基数的对象，在广义位置的旋转表示为4D四元数。但是，广义速度的旋转仍然表示为3D空间旋转矢量。
是否有建议的方法来限制它们，例如向后欧拉？
prog.AddConstraint(eq(q[t+1],q[t] + h[t] * qd[t+1]))
将角速度转换为四元数形式是否合适，例如here？
或者像约翰·贝茨（John T. Betts）所说的那样，是使用非线性规划的最优控制和估计的实用方法
解决方法
我认为对于您的用例，您想在规划中的相邻航路点之间施加积分约束。在这种情况下，我不建议直接使用Euler集成q[n] = q[n+1] - qdot[n+1] * dt。原因是四元数应始终满足单位长度约束（即q[n]和q[n+1]都位于4D单位球面上）。因此，时间导数qdot沿着该单位球面的切线表面，即q[n+1] - qdot[n+1] * dt在该切线表面上。切圆表面上的任何点（q[n+1]除外）也都不在单位圆的表面上。因此约束q[n+1] = q[n] + qdot[n+1] * dt
只有将dt=0与约束| q [n] |耦合时，才能满足= | q [n + 1] | = 1。以图片形式，您可以在https://www.toppr.com/guides/maths/circles/tangents-to-the-circle/的“与圆相切”部分中使用该图。请注意，q[n+1] - qdot[n+1] * dt在球体的切线上，q[n]在圆上，所以不能有q[n] = q[n+1] - qdot[n+1] * dt。
相反，我建议考虑以下情况
q[n+1] = q[n] ⊗ Δq
其中q[n]是第n个航路点的浮动基数的四元数，q[n+1]是第n+1个航路点的浮动基数的四元数。 ⊗是两个四元数q[n]和Δq之间的Hamiltonian product。因此，这里需要根据角速度计算Δq。
假设第n个和第n + 1个航路点之间的平均角速度为ω∈ℝ³。这意味着浮动基座以a = ω/|ω|的速率围绕|ω|轴旋转，因此
Δq = [cos(|ω|Δt/2),a*sin(|ω|Δt/2)]
有关上述公式的说明，请参见https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Using_quaternion_as_rotations。
如果您使用ω[n+1]作为从第n个航点到第n + 1个航点的平均角速度（类似于在后向Euler中使用v [n + 1]），则您的积分约束为
q[n+1] = q[n] ⊗ [cos(|ω[n+1]|Δt/2),a*sin(|ω[n+1]|Δt/2)]
您还可以将其他量用作平均角速度，例如(ω[n] + ω[n+1])/2。
Betts [1] 在书中给出了另一种看待四元数积分的方法，但这有两个注意事项需要注意。正如书中给出的那样，我们不能直接使用 ODE (6.123a - 6.123d) 进行四元数积分。相反，我们可以使用书中给出的 DAE，方程 6.126a - 6.126c 和 6.126g。这些工作因为我们可以将四元数视为轴角度表示的一种形式：
quat = (cos(phi/2),unit_vec * sin(phi/2))
一旦我们对向量部分进行积分（使用 Euler 或 Range-Kutta 高阶方法），标量部分由旋转四元数的单位四元数约束确定。可以使用 Betts [1] 中给出的角速度的四元数导数公式对矢量部分进行积分。
这个过程有以下两个注意事项：
- 此方法仅适用于非常小的增量 T
- 这种方法有一个独特之处。每次，我们只使用 3 个数字来表示旋转（这里，我们只使用四元数的向量部分进行积分），我们引入了一个奇点。轴角四元数的标量部分确实由单位长度约束确定，除了在 -pi 或 pi 处。在这些点上，此映射不起作用。要深入推导/理解为什么会发生这种情况，请检查 [2] 中等式 8 中的行列式。
当轴角表示中的角度为 pi/-pi 时，就会出现奇点。重要的是要注意，这不一定与在单独的 x/y/z 轴上交叉 pi/-pi 重合。虽然这也可能发生。运行 Betts 示例 6.8 以围绕 x 轴旋转超过 pi 以查看实际效果。
因此，如果您可以确保轨迹期间轴角表示中的角度不会与 pi/-pi 交叉，则可以使用使用 Betts 中给出的 DAE 的积分方法来简化实现。但是一旦你越过 pi/-pi，这将不起作用。然后你将不得不使用适当的四元数积分。戴洪凯在执行中给予了一定的谨慎。您可以查看他关于此主题的其他答案以查找实现细节。还要检查 Drake 中的 UnitQuaternionConstraint。
[1] - Betts,J. T. (2010)。使用非线性规划进行最优控制和估计的实用方法。使用非线性规划进行最佳控制和估计的实用方法。工业与应用数学学会。
[2] - Yang,Y. (2010)。基于四元数的动量偏置天底指向航天器模型。航空航天科学与技术，14(3)，199-202。 https://doi.org/10.1016/j.ast.2009.12.006