使用Drake符号法获取浮基机器人运动方程回归矩阵时出错

问题背景 #

使用Drake进行8连杆、7驱动自由度浮基机器人的系统辨识，遵循《Underactuated Robotics》第18章Notebook流程，机器人参数为nq=14，nv=13，n_actuator=7。运行代码时DecomposeLumpedParameters函数报错，残差中出现q的高次幂，不符合预期的线性参数形式。

实现代码 #

plant=MultibodyPlant(0.0)
parser = Parser(plant)
parser.package_map().Add("rsc", "/home/ros/workspaces/rsc/")
wlr4p_url = (
    "package://rsc/wlr4p/urdf/"
    "wlr4p.urdf"
)
(wlr4p,)=parser.AddModels(url=wlr4p_url)
plant.Finalize()

sym_plant = plant.ToSymbolic()

sym_context = sym_plant.CreateDefaultContext()

links_name = ("base_link", "body", "thigh_left", "calf_left", "wheel_left", "thigh_right", "calf_right", "wheel_right")

m = []
l = []
L = []
for i, link_name in enumerate(links_name):
    m += [Variable("m"+str(i))]
    l += [Variable("l" + tmp + str(i)) for tmp in ["x", "y", "z"]]
    L += [Variable("L" + tmp + str(i)) for tmp in ["xx", "xy", "xz", "yy", "yz", "zz"]]

q = MakeVectorVariable(sym_plant.num_positions(), "q")
v = MakeVectorVariable(sym_plant.num_velocities(), "v")
vd = MakeVectorVariable(sym_plant.num_velocities(), "\dot{v}")
tau = MakeVectorVariable(sym_plant.num_actuated_dofs(), "u")

sym_plant.get_actuation_input_port().FixValue(sym_context, tau)
sym_plant.SetPositions(sym_context, q)
sym_plant.SetVelocities(sym_context, v)

for i, link in enumerate(links_name):
    rbt_body = sym_plant.GetBodyByName(link)
    body_inertia = SpatialInertia_[Expression](
        m[i], 
        [l[3*i], l[3*i+1], l[3*i+2]],
        UnitInertia_[Expression](L[6*i], L[6*i+3], L[6*i+5], L[6*i+1], L[6*i+2], L[6*i+4]) 
    )
    rbt_body.SetSpatialInertiaInBodyFrame(sym_context, body_inertia)

derivatives = sym_context.Clone().get_mutable_continuous_state()

derivatives.SetFromVector(np.hstack((0*q, vd)))
residual = sym_plant.CalcImplicitTimeDerivativesResidual(sym_context, derivatives)

W, alpha , w0 = DecomposeLumpedParameters(residual[sym_plant.num_positions():], m+l+L)

报错信息 #

RuntimeError                              Traceback (most recent call last)
/tmp/ipykernel_18187/1339963687.py in <module>
----> 1 W, alpha , w0 = DecomposeLumpedParameters(residual[sym_plant.num_positions():], m+l+L)

RuntimeError: ((-2048 * m4 * Lxz4 * q(0) * q(1) * pow(q(2), 2) * pow(q(3), 4) * v(7) * v(9) * sin(q(8)) * sin(q(9)) * sin(q(10)) * cos(q(8)) * cos(q(9)) * cos(q(10))) / (24 * (pow(q(0), 2) * pow(q(1), 2) * pow(q(2), 2) * pow(q(3), 2)) + 12 * (pow(q(0), 2) * pow(q(1), 2) * pow(q(2), 4)) + 12 * (pow(q(0), 2) * pow(q(1), 2) * pow(q(3), 4)) + 12 * (pow(q(0), 2) * pow(q(1), 4) * pow(q(2), 2)) + 12 * (pow(q(0), 2) * pow(q(1), 4) * pow(q(3), 2)) + 4 * (pow(q(0), 2) * pow(q(1), 6)) + 12 * (pow(q(0), 2) * pow(q(2), 2) * pow(q(3), 4)) + 12 * (pow(q(0), 2) * pow(q(2), 4) * pow(q(3), 2)) + 4 * (pow(q(0), 2) * pow(q(2), 6)) + 4 * (pow(q(0), 2) * pow(q(3), 6)) + 12 * (pow(q(0), 4) * pow(q(1), 2) * pow(q(2), 2)) + 12 * (pow(q(0), 4) * pow(q(1), 2) * pow(q(3), 2)) + 6 * (pow(q(0), 4) * pow(q(1), 4)) + 12 * (pow(q(0), 4) * pow(q(2), 2) * pow(q(3), 2)) + 6 * (pow(q(0), 4) * pow(q(2), 4)) + 6 * (pow(q(0), 4) * pow(q(3), 4)) + 4 * (pow(q(0), 6) * pow(q(1), 2)) + 4 * (pow(q(0), 6) * pow(q(2), 2)) + 4 * (pow(q(0), 6) * pow(q(3), 2)) + 12 * (pow(q(1), 2) * pow(q(2), 2) * pow(q(3), 4)) + 12 * (pow(q(1), 2) * pow(q(2), 4) * pow(q(3), 2)) + 4 * (pow(q(1), 2) * pow(q(2), 6)) + 4 * (pow(q(1), 2) * pow(q(3), 6)) + 12 * (pow(q(1), 4) * pow(q(2), 2) * pow(q(3), 2)) + 6 * (pow(q(1), 4) * pow(q(2), 4)) + 6 * (pow(q(1), 4) * pow(q(3), 4)) + 4 * (pow(q(1), 6) * pow(q(2), 2)) + 4 * (pow(q(1), 6) * pow(q(3), 2)) + 4 * (pow(q(2), 2) * pow(q(3), 6)) + 6 * (pow(q(2), 4) * pow(q(3), 4)) + 4 * (pow(q(2), 6) * pow(q(3), 2)) + pow(q(0), 8) + pow(q(1), 8) + pow(q(2), 8) + pow(q(3), 8))) 
cannot be factored into lumped parameters, since it depends on both parameters and non-parameter variables.

问题原因与解决方案 #

核心原因 #

错误里的高次幂q项来自浮基姿态的四元数表示。Drake默认的浮基关节用单位四元数描述姿态，而四元数的单位约束（q₀²+q₁²+q₂²+q₃²=1）会导致动力学计算中出现四元数分量的高次幂分母，破坏了惯性参数的线性结构，使得DecomposeLumpedParameters无法将残差分解为Y(q,v,vd)φ的形式。

代码修正方案 #

将浮基姿态表示改为**滚动-俯仰-偏航角（RPY）**或欧拉角，消除四元数的非线性约束，确保运动方程对惯性参数保持线性：

1. 修改MultibodyPlant的浮基关节类型
   如果从URDF加载时默认生成了四元数浮基关节，需要手动替换为RPY浮基关节：

   from pydrake.multibody.tree import RollPitchYawFloatingJoint
   
   plant = MultibodyPlant(0.0)
   parser = Parser(plant)
   parser.package_map().Add("rsc", "/home/ros/workspaces/rsc/")
   wlr4p_url = "package://rsc/wlr4p/urdf/wlr4p.urdf"
   (wlr4p,) = parser.AddModels(url=wlr4p_url)
   
   # 移除默认的四元数浮基关节
   base_link = plant.GetBodyByName("base_link")
   default_floating_joint = plant.GetJointBetween(plant.world_body(), base_link)
   plant.RemoveJoint(default_floating_joint)
   
   # 添加RPY浮基关节
   plant.AddJoint(RollPitchYawFloatingJoint("base_joint", plant.world_body(), base_link))
   
   plant.Finalize()
   

2. 或修改URDF定义
   在URDF中直接定义RPY类型的浮基关节，避免依赖Drake的默认设置。

概念澄清 #

浮基机器人的动力学方程确实是关于惯性参数（质量、质心位置、惯量）的线性形式：M(q)\dot{v} + C(q,v)v = Y(q,v,\dot{v})φ + τ，其中Y是回归矩阵，φ是惯性参数向量。但四元数的单位化约束会引入非线性项，打破这种线性性，因此必须使用无约束的姿态表示（如RPY角）才能进行参数分解。

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