约束浮动基础四元数位置和 angular 速度的推荐方法是什么？
What is the recommended way of constraining floating base quaternion position and angular velocity?
对于具有浮动底座的对象，广义位置的旋转表示为 4D 四元数。然而，广义速度中的旋转仍然表示为3D空间旋转矢量。
是否有推荐的方法来约束它们，例如后向欧拉？
prog.AddConstraint(eq(q[t+1], q[t] + h[t] * qd[t+1]))
将angular速度转换为四元数形式是否合适，例如here?
或者正如 John T. Betts 在使用非线性规划进行最优控制和估计的实用方法中所说的那样
我认为对于您的用例，您希望在规划中对相邻 waypoints 之间施加集成约束。在这种情况下，我不建议直接使用欧拉积分 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。所以在这里你需要根据 angular 速度计算 Δq。
假设第n个和第n+1个waypoints之间的平均angular速度为ω∈ℝ³。这意味着浮动底座以 |ω| 的速率绕轴 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 个航路点的平均 angular 速度（类似于在后向欧拉中使用 v[n+1]），那么你的积分约束是
q[n+1] = q[n] ⊗ [cos(|ω[n+1]|Δt/2), a*sin(|ω[n+1]|Δt/2)]
您也可以使用其他数量作为平均 angular 速度，例如 (ω[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].
中给出的 angular 速度的四元数导数公式进行积分
此过程有以下两个注意事项：
- 此方法仅适用于非常小的 delta Ts
- 这个方法有一个奇点。每次，我们只使用 3 个数字来表示旋转（这里，我们使用四元数的向量部分只是为了积分），我们引入了一个奇点。轴角四元数的标量部分确实由单位长度约束确定，除了 -pi 或 pi。在这些点上，此映射不起作用。要深入 derivation/understanding 为什么会发生这种情况，请检查 [2].
中等式 8 中的行列式
当轴角表示中的角度为 pi/-pi 时出现奇点。重要的是要注意，这不一定与在单个 x/y/z 轴上交叉 pi/-pi 一致。虽然这也可能发生。 运行 Betts 示例 6.8 绕 x 轴旋转超过 pi 以查看实际效果。
因此，如果您可以确保轨迹期间轴角表示中的角度不会交叉 pi/-pi，则可以使用 Betts 中给出的使用 DAE 的积分方法来简化实现。但是一跨pi/-pi，这就不行了。然后你将不得不使用适当的四元数积分。正如 Hongkai Dai 在实施过程中给予的一定关注。您可以查看他关于此主题的其他答案以查找实施细节。还要检查 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
对于具有浮动底座的对象，广义位置的旋转表示为 4D 四元数。然而，广义速度中的旋转仍然表示为3D空间旋转矢量。
是否有推荐的方法来约束它们，例如后向欧拉？
prog.AddConstraint(eq(q[t+1], q[t] + h[t] * qd[t+1]))
将angular速度转换为四元数形式是否合适，例如here?
或者正如 John T. Betts 在使用非线性规划进行最优控制和估计的实用方法中所说的那样
我认为对于您的用例，您希望在规划中对相邻 waypoints 之间施加集成约束。在这种情况下，我不建议直接使用欧拉积分 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。所以在这里你需要根据 angular 速度计算 Δq。
假设第n个和第n+1个waypoints之间的平均angular速度为ω∈ℝ³。这意味着浮动底座以 |ω| 的速率绕轴 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 个航路点的平均 angular 速度（类似于在后向欧拉中使用 v[n+1]），那么你的积分约束是
q[n+1] = q[n] ⊗ [cos(|ω[n+1]|Δt/2), a*sin(|ω[n+1]|Δt/2)]
您也可以使用其他数量作为平均 angular 速度，例如 (ω[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].
中给出的 angular 速度的四元数导数公式进行积分此过程有以下两个注意事项：
- 此方法仅适用于非常小的 delta Ts
- 这个方法有一个奇点。每次，我们只使用 3 个数字来表示旋转（这里，我们使用四元数的向量部分只是为了积分），我们引入了一个奇点。轴角四元数的标量部分确实由单位长度约束确定，除了 -pi 或 pi。在这些点上，此映射不起作用。要深入 derivation/understanding 为什么会发生这种情况，请检查 [2]. 中等式 8 中的行列式
当轴角表示中的角度为 pi/-pi 时出现奇点。重要的是要注意，这不一定与在单个 x/y/z 轴上交叉 pi/-pi 一致。虽然这也可能发生。 运行 Betts 示例 6.8 绕 x 轴旋转超过 pi 以查看实际效果。
因此，如果您可以确保轨迹期间轴角表示中的角度不会交叉 pi/-pi，则可以使用 Betts 中给出的使用 DAE 的积分方法来简化实现。但是一跨pi/-pi，这就不行了。然后你将不得不使用适当的四元数积分。正如 Hongkai Dai 在实施过程中给予的一定关注。您可以查看他关于此主题的其他答案以查找实施细节。还要检查 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