了解自由浮动体和四元数状态的线性化

Question

我正在尝试线性化具有自由浮动底座和 3 个关节（j1、j2、j3）的自由浮动系统。据我了解，系统状态的位置部分由向量给出（这匹配 MultibodyPlant::num_positions()）：

q (10x1) = [base_quaternion (4x1), base_lin_position (3x1), j1_pos, j2_pos, j3_pos]

由于angular速度只需要3个分量，系统状态的速度部分写为（这匹配MultibodyPlant::num_velocities()）：

q_dot (9x1) = [base_rot_vel (3x1), base_lin_vel (3x1), j1_vel, j2_vel, j3_vel]

使用这个，完整的系统状态给出为（这在使用 MultibodyPlant::SetPositionsAndVelocities 时有效）：

X (19x1) = [q (10x1),q_dot (9x1)]

据此，由其动力学和控制力产生的系统加速度 X_dot = f(X, U) 将写为：

X_dot (18x1)= [q_dot (9x1), q_ddot (9x1)]

由于旋转和angular速度的表示不同，定义X和X_dot所需的项数不同。

在使用 Linearize 对系统进行线性化时，这会带来以下问题：

A和B线性化后的连续时间MultibodyPlant矩阵表示方程X_dot = A*X + B*u。但是，这里涉及的 arrays/matrices 的大小似乎不匹配，因为 X_dot (18x1) 不同于 Linearize 给出的矩阵：A (19x19) 和 B (19x3)。然后我不明白线性系统方程中的矩阵 X_dot 及其大小 19x1?
代表什么加速度
以上问题仅针对连续时间的情况。对于离散时间系统，以下等式在矩阵大小方面没有任何问题：X[n+1] = A_d * X[n] + B_d * u[n]。但是，不清楚在这个线性化正向模拟过程中四元数属性是如何保持的？

Answer 1

我认为符号中存在误解，因为 q_dot ≠ v。相反，q_dot 只是 q 的普通时间导数。

    q (10x1) =     [base_quaternion (4x1), base_lin_position (3x1), j1_pos, j2_pos, j3_pos]
q_dot (10x1) = d/dt[base_quaternion (4x1), base_lin_position (3x1), j1_pos, j2_pos, j3_pos]

Angular 速度只有 3 个分量，因此 v（系统状态的速度部分）及其时间导数 v_dot 是：

    v (9x1) =     [base_rot_vel (3x1), base_lin_vel (3x1), j1_vel, j2_vel, j3_vel]
v_dot (9x1) = d/dt[base_rot_vel (3x1), base_lin_vel (3x1), j1_vel, j2_vel, j3_vel]

完整的系统状态 X 及其时间导数 x_dot 如下所示。

    X (19x1) = [q     (10x1),  v    ( 9x1)]
X_dot (19x1) = [q_dot (10x1),  v_dot (9x1)]

注：X ≠ [q, q_dot]，而是 X = [q, v]。同样，X_dot ≠ [q_dot, q_ddot], 而不是 X = [q_dot, v_dot].

了解自由浮动体和四元数状态的线性化

Understanding Linearization with free-floating bodies and Quaternion States

drake