麦轮底盘后腿VMC建模
1. 坐标系定义策略 (Coordinate System Strategy)
为了应对未来可能的坐标系变动，将 “物理安装角度” 与 “数学计算角度” 彻底分离。
1.1 全局数学坐标系 (Global Math Frame)
为了保证三角函数导数（\(\sin' = \cos\), \(\cos' = -\sin\)）的符号正确性，核心解算层必须遵守标准定义：
原点 \(A\)：关节转轴中心 \((0,0)\)。
X 轴：水平向右（或其他你定义的绝对参考方向）。
Y 轴：垂直向上（符合右手定则）。
角度 \(\theta\)：以 X 轴正方向为 \(0\)，逆时针 (CCW) 为正方向。
1.2 映射函数 (Mapping Function)
需要一个输入适配器，将电机的原始读数转换为上述数学角度。
\[\theta_{global} = \text{Dir} \cdot (\alpha_{raw} - \alpha_{offset}) + \theta_{bias}\]
raw: 编码器原始值。offset: 机械零位（例如腿完全水平向后的编码器值）。Dir: 旋转方向修正 (\(1\) 或 \(-1\))。bias: 机械零位在数学坐标系中的角度（例如水平向后对应 \(\pi\)）。
2. 几何参数定义
| 符号 | 说明 | 备注 |
|---|---|---|
| \(l_2\) | 曲柄 AD 长度 | 主动臂 |
| \(l_{23}\) | 连杆 DC 长度 | |
| \(l_3\) | 摇杆 BC 长度 | |
| \(l_4\) | 机架 AB 长度 | 定点距离 |
| \(l_1\) | 延长段 DE 长度 | |
| \(\alpha_{offset}\) | 结构偏置角 | D-E 连线相对于 C-D 连线的夹角 |
| \(B\) | 定点 B 坐标 | 固定值 \((x_B, y_B)\) |
3. 正运动学解算 (Forward Kinematics)
输入：数学角度 \(\theta\) (对应 \(A\) 点主动臂角度)。
输出：末端坐标 \(E(x_E, y_E)\) 及中间点状态。
3.1 关键点坐标
主动点 D (标准极坐标公式):
\[x_D = l_2 \cos\theta\]
\[y_D = l_2 \sin\theta\]
从动点 C (双圆交点法):
计算 \(D \to B\) 的向量及距离：
\[d^2 = (x_B - x_D)^2 + (y_B - y_D)^2\]
\[d = \sqrt{d^2}\]
计算投影几何量：
\[a = \frac{l_{23}^2 - l_3^2 + d^2}{2d}\]
\[h = \sqrt{l_{23}^2 - a^2}\]
(代码提示：计算 \(h\) 时需做 sqrt(max(0, …)) 保护)
计算 C 点（向量旋转）：
\[x_C = x_D + a \frac{x_B - x_D}{d} \pm h \frac{y_B - y_D}{d}\]
\[y_C = y_D + a \frac{y_B - y_D}{d} \mp h \frac{x_B - x_D}{d}\]
(符号选取 \(\pm\) 取决于机构的肘部朝向，需根据实际装配选定一组)
末端点 E (刚体偏置外推):
计算 C 点到 D 点的向量绝对偏角 \(\phi_{CD}\)：
\[\phi_{CD} = \text{atan2}(y_D - y_C, x_D - x_C)\]
叠加刚体偏置角得到 DE 向量的绝对偏角 \(\phi_{DE}\)：
\[\phi_{DE} = \phi_{CD} + \alpha_{offset}\]
利用极坐标基于 D 点推算 E 点坐标：
\[x_E = x_D + l_1 \cos(\phi_{DE})\]
\[y_E = y_D + l_1 \sin(\phi_{DE})\]
4. 雅可比矩阵推导 (Jacobian Derivation)
由于是单自由度系统，\(J\) 为 \(2 \times 1\) 向量。采用速度投影法与刚体运动学结合计算。
4.1 D 点 Jacobian (\(J_D\))
\[J_{Dx} = \frac{\partial x_D}{\partial \theta} = -l_2 \sin\theta\]
\[J_{Dy} = \frac{\partial y_D}{\partial \theta} = l_2 \cos\theta\]
4.2 C 点 Jacobian (\(J_C\))
构建速度约束方程组：
\[\begin{cases} (\vec{C} - \vec{B}) \cdot \vec{v}_C = 0 \\ (\vec{C} - \vec{D}) \cdot (\vec{v}_C - \vec{v}_D) = 0 \end{cases}\]
利用 Cramer 法则求解线性方程组 \(A \cdot J_C = b\)：
系数行列式：
\[det = (x_C - x_B)(y_C - y_D) - (y_C - y_B)(x_C - x_D)\]
右侧常数项：
\[R = (x_C - x_D)J_{Dx} + (y_C - y_D)J_{Dy}\]
求解结果：
\[J_{Cx} = \frac{-(y_C - y_B) \cdot R}{det}\]
\[J_{Cy} = \frac{(x_C - x_B) \cdot R}{det}\]
4.3 E 点 Jacobian (\(J_E\))
由于 C-D-E 是一个刚体，我们首先需要求出连杆 CD 的角速度标量 \(\omega_{CD}\)，然后利用刚体速度公式 \(\vec{v}_E = \vec{v}_D + \vec{\omega}_{CD} \times \vec{r}_{DE}\) 进行求解。
计算连杆 CD 的角速度：
令 \(\vec{r}_{DC} = \begin{bmatrix} x_C - x_D \\ y_C - y_D \end{bmatrix}\)，相对速度 \(\vec{v}_{DC} = \begin{bmatrix} J_{Cx} - J_{Dx} \\ J_{Cy} - J_{Dy} \end{bmatrix}\)。
\[\omega_{CD} = \frac{(x_C - x_D)(J_{Cy} - J_{Dy}) - (y_C - y_D)(J_{Cx} - J_{Dx})}{(x_C - x_D)^2 + (y_C - y_D)^2}\]
计算 E 点速度 (雅可比)：
令 \(\vec{r}_{DE} = \begin{bmatrix} x_E - x_D \\ y_E - y_D \end{bmatrix}\)。
\[J_{Ex} = J_{Dx} - \omega_{CD} \cdot (y_E - y_D)\]
\[J_{Ey} = J_{Dy} + \omega_{CD} \cdot (x_E - x_D)\]
5. VMC 力控模型 (Force Control)
在极坐标系下定义虚拟力，以便直观调整参数。
5.1 定义虚拟力
腿长方向 (\(L\))：模拟弹簧阻尼 (Spring-Damper)。
\[F_{radial} = K_p (L_{des} - L_{curr}) + K_d (\dot{L}_{des} - \dot{L}_{curr})\]
- 注：\(L_{curr} = \sqrt{x_E^2 + y_E^2}\)
摆动方向 (\(\phi\))：模拟扭簧或 PD 控制。
\[T_{swing} = K_{p\phi} (\phi_{des} - \phi_{curr}) + K_{d\phi} (\dot{\phi}_{des} - \dot{\phi}_{curr})\]
5.2 映射到笛卡尔空间 (\(F_{cartesian}\))
构建变换矩阵 \(M\)，将 \([F_{radial}, T_{swing}]^T\) 映射为 \([F_x, F_y]^T\)。
\[\begin{bmatrix} F_x \\ F_y \end{bmatrix} = \begin{bmatrix} \frac{x_E}{L} & -\frac{y_E}{L^2} \\ \frac{y_E}{L} & \frac{x_E}{L^2} \end{bmatrix} \begin{bmatrix} F_{radial} \\ T_{swing} \end{bmatrix}\]
代数展开形式 (代码用)：
\[F_x = F_{radial} \cdot \frac{x_E}{L} - T_{swing} \cdot \frac{y_E}{L^2}\]
\[F_y = F_{radial} \cdot \frac{y_E}{L} + T_{swing} \cdot \frac{x_E}{L^2}\]
注意：此处 \(T_{swing}\) 的正方向定义为逆时针。如果 \(T_{swing}\) 定义相反，需调整第二项符号。
6. 最终力矩计算 (Torque Calculation)
利用虚功原理将笛卡尔力映射回电机力矩：
\[\tau_{motor} = J_{E}^T \cdot F_{cartesian}\]
\[\tau_{motor} = J_{Ex} \cdot F_x + J_{Ey} \cdot F_y\]