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

问题描述

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

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

由于角速度只需要 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)]

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

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

1. 连续时间 A 线性化后的B 和MultibodyPlant 矩阵表示方程X_dot = A*X + B*u。但是，此处涉及的数组/矩阵的大小似乎不匹配，因为 X_dot (18x1) 与 Linearize 给出的矩阵不同：A (19x19) 和 B (19x3)。然后我不明白线性系统方程中的矩阵 X_dot 用其大小 19x1 表示什么加速度？

2. 以上问题仅针对连续时间的情况。对于离散时间系统，以下等式在矩阵大小方面没有任何问题：X[n+1] = A_d * X[n] + B_d * u[n]。然而，不清楚在这种线性化前向模拟过程中四元数的性质是如何保持的？

解决方法

我认为符号中有误解，因为 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),j3_pos]

角速度只有 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),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]。