SO(3) 中刚性 Body 旋转轨迹的直接 Transcription/Collocation

Question

我正在尝试使用 DirectTranscription 和 DirectCollocation 在 SO(3) 中为 free-floating 框找到最佳轨迹，即将卫星从一个姿势旋转到另一个姿势。首先，我使用 示例 6.12：刚体 Body 的重新定向来自 Betts, J. T. (2010)。使用非线性规划进行最优控制和估计的实用方法。

在这里，body 惯量为 Ixx = 5621，Iyy = 4547，Izz = 2364。我做了一个简单的 box.urdf有了这些惯性。

初始旋转四元数为[1,0,0,0]最终旋转四元数为[cos(phi/2),sin(phi/2),0,0](phi为 150 度），初始和最终旋转速度为零。对于初始和最终状态，位置及其导数设置为零。

因为这基本上是 SO(3) 中多轴的双积分器问题，最佳轨迹是 bang-bang 控制器。

我正在使用 MultibodyPlant 的 get_applied_generalized_force_input_port() 进行驱动 (u)，因此该端口索引用于初始化轨迹优化。

在 DirectTransciption 和 DirectCollocation 中，优化都失败了，但原因不同：

DirectTransciption ：由于不可行的约束，这失败了。但是相同的约束似乎对 DirectCollocation 有效，因为这会产生另一个错误。使用的代码片段：

NDirTrans = 300

prog = DirectTranscription(plant_d, trajOptContext, NDirTrans, 
input_port_index=plant_d.get_applied_generalized_force_input_port().get_index())

prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
prog.AddBoundingBoxConstraint(xf, xf, prog.final_state())

for u_index in range(prog.input().size):
    prog.AddConstraintToAllKnotPoints(prog.input()[u_index] <= 50)
    prog.AddConstraintToAllKnotPoints(prog.input()[u_index] >= 50)

result = Solve(prog)

DirectCollocation：由于以下错误而失败：

RuntimeError: Encountered singular articulated body hinge inertia for body node index 1. 
Please ensure that this body has non-zero inertia along all axes of motion.

我确认节点索引 1 处的 body 沿所有轴确实具有惯性参数，因此我被困在这里。

可以看到 google 重现错误的 colab 脚本 here。不需要运行程序来查看错误，因为单元格输出已保存并且无需运行程序即可查看。这包括查看节点索引为 1 的 Body 的空间惯性矩阵。

Answer 1

我认为直接转录不适用于您的表述。直接转录使用显式欧拉方法，即 x[n+1] = x[n] + xdot[n] * dt。由于你的状态的一部分是四元数，你有

quat[n+1] = quat[n] + quat_dot[n] * dt

另一方面，四元数必须在SO(3)流形上（这里指的是四元数必须在单位圆上），并且四元数的时间导数垂直于该四元数，如结果quat[n] + quat_dot[n] * dt在单位圆的切面上，通过quat[n]。该切面与单位圆的唯一交点是quat[n]本身，因此满足显式欧拉积分的唯一解是quat[n+1] = quat[n]，这意味着所有四元数沿轨迹相同，因此不可行你收到的消息。

当您使用直接搭配时，问题得到缓解，但仍然不是在 SO(3) 上执行 integration/interpolation 的正确方法（直接搭配并不总是满足 SO(3)约束）。您可以检查 paper 传统方式在 SO(3) 上的集成。

SO(3) 中刚性 Body 旋转轨迹的直接 Transcription/Collocation

Direct Transcription/Collocation for Rigid Body Rotation Trajectory in SO(3)

drake