1. 坐标系定义策略 (Coordinate System Strategy)

为了应对未来可能的坐标系变动，将 “物理安装角度” 与 “数学计算角度” 彻底分离。

1.1 全局数学坐标系 (Global Math Frame)

为了保证三角函数导数（sin⁡′=cos⁡\sin' = \cos, cos⁡′=−sin⁡\cos' = -\sin）的符号正确性，核心解算层必须遵守标准定义：

  • 原点 AA：关节转轴中心 (0,0)(0,0)。

  • X 轴：水平向右（或其他你定义的绝对参考方向）。

  • Y 轴：垂直向上（符合右手定则）。

  • 角度 θ\theta：以 X 轴正方向为 00，逆时针 (CCW) 为正方向。

1.2 映射函数 (Mapping Function)

需要一个输入适配器，将电机的原始读数转换为上述数学角度。

θglobal=Dir⋅(αraw−αoffset)+θbias\theta_{global} = \text{Dir} \cdot (\alpha_{raw} - \alpha_{offset}) + \theta_{bias}

  • raw: 编码器原始值。

  • offset: 机械零位（例如腿完全水平向后的编码器值）。

  • Dir: 旋转方向修正 (11 或 −1-1)。

  • bias: 机械零位在数学坐标系中的角度（例如水平向后对应 π\pi）。

2. 几何参数定义

符号说明备注
l2l_2曲柄 AD 长度主动臂
l23l_{23}连杆 DC 长度
l3l_3摇杆 BC 长度
l4l_4机架 AB 长度定点距离
l1l_1延长段 DE 长度
αoffset\alpha_{offset}结构偏置角D-E 连线相对于 C-D 连线的夹角
BB定点 B 坐标固定值 (xB,yB)(x_B, y_B)

3. 正运动学解算 (Forward Kinematics)

输入：数学角度 θ\theta (对应 AA 点主动臂角度)。

输出：末端坐标 E(xE,yE)E(x_E, y_E) 及中间点状态。

3.1 关键点坐标

  1. 主动点 D (标准极坐标公式):

    xD=l2cos⁡θx_D = l_2 \cos\theta

    yD=l2sin⁡θy_D = l_2 \sin\theta

  2. 从动点 C (双圆交点法):

    • 计算 D→BD \to B 的向量及距离：

      d2=(xB−xD)2+(yB−yD)2d^2 = (x_B - x_D)^2 + (y_B - y_D)^2

      d=d2d = \sqrt{d^2}

    • 计算投影几何量：

      a=l232−l32+d22da = \frac{l_{23}^2 - l_3^2 + d^2}{2d}

      h=l232−a2h = \sqrt{l_{23}^2 - a^2}

      (代码提示：计算 hh 时需做 sqrt(max(0, …)) 保护)

    • 计算 C 点（向量旋转）：

      xC=xD+axB−xDd±hyB−yDdx_C = x_D + a \frac{x_B - x_D}{d} \pm h \frac{y_B - y_D}{d}

      yC=yD+ayB−yDd∓hxB−xDdy_C = y_D + a \frac{y_B - y_D}{d} \mp h \frac{x_B - x_D}{d}

      (符号选取 ±\pm 取决于机构的肘部朝向，需根据实际装配选定一组)

  3. 末端点 E (刚体偏置外推):

    • 计算 C 点到 D 点的向量绝对偏角 ϕCD\phi_{CD}：

      ϕCD=atan2(yD−yC,xD−xC)\phi_{CD} = \text{atan2}(y_D - y_C, x_D - x_C)

    • 叠加刚体偏置角得到 DE 向量的绝对偏角 ϕDE\phi_{DE}：

      ϕDE=ϕCD+αoffset\phi_{DE} = \phi_{CD} + \alpha_{offset}

    • 利用极坐标基于 D 点推算 E 点坐标：

      xE=xD+l1cos⁡(ϕDE)x_E = x_D + l_1 \cos(\phi_{DE})

      yE=yD+l1sin⁡(ϕDE)y_E = y_D + l_1 \sin(\phi_{DE})

4. 雅可比矩阵推导 (Jacobian Derivation)

由于是单自由度系统，JJ 为 2×12 \times 1 向量。采用速度投影法与刚体运动学结合计算。

4.1 D 点 Jacobian (JDJ_D)

JDx=∂xD∂θ=−l2sin⁡θJ_{Dx} = \frac{\partial x_D}{\partial \theta} = -l_2 \sin\theta

JDy=∂yD∂θ=l2cos⁡θJ_{Dy} = \frac{\partial y_D}{\partial \theta} = l_2 \cos\theta

4.2 C 点 Jacobian (JCJ_C)

构建速度约束方程组：

{(C⃗−B⃗)⋅v⃗C=0(C⃗−D⃗)⋅(v⃗C−v⃗D)=0\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⋅JC=bA \cdot J_C = b：

  • 系数行列式：

    det=(xC−xB)(yC−yD)−(yC−yB)(xC−xD)det = (x_C - x_B)(y_C - y_D) - (y_C - y_B)(x_C - x_D)

  • 右侧常数项：

    R=(xC−xD)JDx+(yC−yD)JDyR = (x_C - x_D)J_{Dx} + (y_C - y_D)J_{Dy}

  • 求解结果：

    JCx=−(yC−yB)⋅RdetJ_{Cx} = \frac{-(y_C - y_B) \cdot R}{det}

    JCy=(xC−xB)⋅RdetJ_{Cy} = \frac{(x_C - x_B) \cdot R}{det}

4.3 E 点 Jacobian (JEJ_E)

由于 C-D-E 是一个刚体，我们首先需要求出连杆 CD 的角速度标量 ωCD\omega_{CD}，然后利用刚体速度公式 v⃗E=v⃗D+ω⃗CD×r⃗DE\vec{v}_E = \vec{v}_D + \vec{\omega}_{CD} \times \vec{r}_{DE} 进行求解。

  • 计算连杆 CD 的角速度：

    令 r⃗DC=[xC−xDyC−yD]\vec{r}_{DC} = \begin{bmatrix} x_C - x_D \\ y_C - y_D \end{bmatrix}，相对速度 v⃗DC=[JCx−JDxJCy−JDy]\vec{v}_{DC} = \begin{bmatrix} J_{Cx} - J_{Dx} \\ J_{Cy} - J_{Dy} \end{bmatrix}。

    ωCD=(xC−xD)(JCy−JDy)−(yC−yD)(JCx−JDx)(xC−xD)2+(yC−yD)2\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 点速度 (雅可比)：

    令 r⃗DE=[xE−xDyE−yD]\vec{r}_{DE} = \begin{bmatrix} x_E - x_D \\ y_E - y_D \end{bmatrix}。

    JEx=JDx−ωCD⋅(yE−yD)J_{Ex} = J_{Dx} - \omega_{CD} \cdot (y_E - y_D)

    JEy=JDy+ωCD⋅(xE−xD)J_{Ey} = J_{Dy} + \omega_{CD} \cdot (x_E - x_D)

5. VMC 力控模型 (Force Control)

在极坐标系下定义虚拟力，以便直观调整参数。

5.1 定义虚拟力

  1. 腿长方向 (LL)：模拟弹簧阻尼 (Spring-Damper)。

    Fradial=Kp(Ldes−Lcurr)+Kd(L˙des−L˙curr)F_{radial} = K_p (L_{des} - L_{curr}) + K_d (\dot{L}_{des} - \dot{L}_{curr})

    • 注：Lcurr=xE2+yE2L_{curr} = \sqrt{x_E^2 + y_E^2}

  2. 摆动方向 (ϕ\phi)：模拟扭簧或 PD 控制。

    Tswing=Kpϕ(ϕdes−ϕcurr)+Kdϕ(ϕ˙des−ϕ˙curr)T_{swing} = K_{p\phi} (\phi_{des} - \phi_{curr}) + K_{d\phi} (\dot{\phi}_{des} - \dot{\phi}_{curr})

5.2 映射到笛卡尔空间 (FcartesianF_{cartesian})

构建变换矩阵 MM，将 [Fradial,Tswing]T[F_{radial}, T_{swing}]^T 映射为 [Fx,Fy]T[F_x, F_y]^T。

[FxFy]=[xEL−yEL2yELxEL2][FradialTswing]\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}

代数展开形式 (代码用)：

Fx=Fradial⋅xEL−Tswing⋅yEL2F_x = F_{radial} \cdot \frac{x_E}{L} - T_{swing} \cdot \frac{y_E}{L^2}

Fy=Fradial⋅yEL+Tswing⋅xEL2F_y = F_{radial} \cdot \frac{y_E}{L} + T_{swing} \cdot \frac{x_E}{L^2}

注意：此处 TswingT_{swing} 的正方向定义为逆时针。如果 TswingT_{swing} 定义相反，需调整第二项符号。

6. 最终力矩计算 (Torque Calculation)

利用虚功原理将笛卡尔力映射回电机力矩：

τmotor=JET⋅Fcartesian\tau_{motor} = J_{E}^T \cdot F_{cartesian}

τmotor=JEx⋅Fx+JEy⋅Fy\tau_{motor} = J_{Ex} \cdot F_x + J_{Ey} \cdot F_y