机器人动力学与控制基础

M(q)·q̈ + C(q, q̇)·q̇ + g(q) + τ_f(q̇) = τ + J^T(q)·F_ext

M(q)   ：n×n 对称正定惯量矩阵
C(q,q̇)：n×n 科氏力与离心力矩阵
g(q)   ：n×1 重力项
τ_f    ：摩擦力矩（非刚体模型项）
τ      ：关节驱动力矩
F_ext  ：末端施加的外力，通过 J^T 映射到关节空间

性质 1：M(q) 对称正定
  保证 q̈ = M^-1(...) 总有解，是数值稳定的基础

性质 2：Ṁ(q) - 2C(q,q̇) 是反对称矩阵
  这意味着 q̇^T (Ṁ - 2C) q̇ = 0
  由此可得能量守恒：无外力时系统总能量不变
  这条性质是无源性（passivity）证明的基础，阻抗控制稳定性靠它

性质 3：方程对参数线性
  M(q)q̈ + C(q,q̇)q̇ + g(q) = Y(q, q̇, q̈) · π
  其中 Y 是回归矩阵（只与运动状态有关），π 是参数向量（质量、惯量等）
  这条性质是参数辨识的理论依据

拉格朗日法（能量视角）：
  L = K - P  （动能 - 势能）
  d/dt(∂L/∂q̇) - ∂L/∂q = τ
  优点：形式统一、便于理论分析、显式给出 M/C/g
  缺点：符号推导随自由度爆炸，6 轴臂手推几乎不可行

牛顿-欧拉法（递推视角）：
  前向递推：从基座到末端，算每个连杆的速度、加速度、惯性力
  后向递推：从末端到基座，算关节力矩
  复杂度 O(n)，是实际计算的唯一选择

ω_i   = R_i^T · ω_{i-1} + z · q̇_i
ω̇_i  = R_i^T · ω̇_{i-1} + R_i^T(ω_{i-1} × z) · q̇_i + z · q̈_i
v̇_i  = R_i^T(v̇_{i-1} + ω̇_{i-1} × r_{i-1} + ω_{i-1} × (ω_{i-1} × r_{i-1}))
a_c,i = v̇_i + ω̇_i × r_c,i + ω_i × (ω_i × r_c,i)
F_i   = m_i · a_c,i
N_i   = I_c,i · ω̇_i + ω_i × (I_c,i · ω_i)

// 递归牛顿-欧拉（RNEA）的核心循环，Pinocchio 的 rnea 即此实现
// 手写版仅用于理解；生产环境直接用库
void rnea(const Model & m, const VectorXd & q,
          const VectorXd & v, const VectorXd & a, VectorXd & tau) {
  // 前向：算速度与加速度
  for (int i = 1; i < m.njoints; ++i) {
    const auto & j = m.joints[i];
    const Matrix3d R = j.placement.rotation().toRotationMatrix();
    const Vector3d r = j.placement.translation();
    omega[i]  = R.transpose() * (omega[i-1] + r.cross(v[i-1])) + j.axis * v[i];
    alpha[i]  = R.transpose() * (alpha[i-1] + r.cross(a[i-1])
                + omega[i-1].cross(omega[i-1].cross(r))) + j.axis * a[i];
    a_c[i]    = alpha[i].cross(c[i]) + omega[i].cross(omega[i].cross(c[i]));
    F[i]      = mass[i] * a_c[i];
    N[i]      = inertia[i] * alpha[i] + omega[i].cross(inertia[i] * omega[i]);
  }
  // 后向：算关节力矩
  for (int i = m.njoints - 1; i >= 1; --i) {
    f[i] = R_next.transpose() * f[i+1] + F[i];
    n[i] = R_next.transpose() * n[i+1] + N[i]
         + c[i].cross(F[i]) + r_next.cross(R_next.transpose() * f[i+1]);
    tau[i-1] = j.axis.dot(n[i]);
  }
}

重力项 g(q)：
  只与位形有关，计算最便宜
  低速重载场景下是主要力矩来源（占 60%~90%）
  所有工业控制器都必须做重力补偿，否则松手就掉落

惯量矩阵 M(q)：
  与位形有关，计算最贵（O(n²) 项）
  决定加速度响应，高速运动时不可忽略
  对角占优通常成立，非对角项代表关节耦合

科氏力 C(q,q̇)q̇：
  与速度平方成正比
  低速时几乎为零，高速时可达重力的 30%
  计算复杂度与 M 相当

步骤：
  1. 设计激励轨迹
     有限傅里叶级数（最常用）：q_i(t) = q0_i + Σ (a_k sin(kωt) + b_k cos(kωt))
     频率选择要覆盖所有关节，且避免共振频率
     典型：基频 0.1 Hz，5 个谐波，单次 20 秒，做 5~10 次不同起始位形

  2. 采集数据
     每个采样点记录 q, q̇, q̈, τ（力矩由电流乘力矩常数得到）
     采样率 1 kHz，滤波（低通 10~20 Hz）后再微分或直接读编码器差分

  3. 构造回归矩阵并求解
     τ = Y(q,q̇,q̈) · π
     最小二乘：π̂ = (Y^T W Y)^-1 Y^T W τ
     加正则项避免病态：π̂ = (Y^T W Y + λI)^-1 Y^T W τ

  4. 验证
     用独立的一组轨迹对比预测力矩与实测力矩
     好的辨识结果：均方根误差 < 5% 峰值力矩

import numpy as np

def identify(Y_list, tau_list, lam=1e-6):
    """Y_list: 每个采样点的回归矩阵 (n x 10*nparams)
       tau_list: 对应的力矩向量"""
    Y = np.vstack(Y_list)
    tau = np.concatenate(tau_list)
    # 加权最小二乘 + 岭正则，抑制病态
    A = Y.T @ Y + lam * np.eye(Y.shape[1])
    b = Y.T @ tau
    pi = np.linalg.solve(A, b)
    # 验证：残差与相对误差
    resid = Y @ pi - tau
    rms = np.sqrt(np.mean(resid**2))
    return pi, rms

τ_i = Kp_i · (q_d,i - q_i) + Ki_i · ∫(q_d,i - q_i)dt + Kd_i · (q̇_d,i - q̇_i)

工程要点：
  - 必须加重力前馈：τ_i += g_i(q)，否则静差与下垂明显
  - 积分项必须限幅（anti-windup），否则大误差后恢复时超调严重
  - 微分项应基于测量速度而非误差微分，避免目标跳变时的微分冲击

struct JointPID {
  double kp, ki, kd;
  double i_term = 0.0, prev_err = 0.0;
  double i_limit = 0.5;      // 积分项限幅（N·m）

  double update(double err, double dt, double gravity_ff) {
    i_term += ki * err * dt;
    i_term = std::clamp(i_term, -i_limit, i_limit);   // anti-windup
    const double d_term = -kd * (err - prev_err) / dt;
    prev_err = err;
    return kp * err + i_term + d_term + gravity_ff;
  }
};

τ = M(q)·(q̈_d + Kd·ė + Kp·e) + C(q,q̇)·q̇ + g(q)

代入动力学方程可得误差动力学：
  ë + Kd·ė + Kp·e = 0

即误差按指定的二阶系统收敛，Kp、Kd 直接对应自然频率与阻尼比：
  Kp = ω_n²,  Kd = 2ζ·ω_n
  典型：ω_n = 20~40 rad/s，ζ = 1.0（临界阻尼）

// 计算力矩控制（1 kHz 循环内）
Eigen::VectorXd q_d, dq_d, ddq_d, q, dq;
Eigen::VectorXd e    = q_d - q;
Eigen::VectorXd de   = dq_d - dq;
Eigen::VectorXd a_d  = ddq_d + Kd.asDiagonal() * de + Kp.asDiagonal() * e;

Eigen::VectorXd tau = pinocchio::rnea(model, data, q, dq, a_d);
// rnea 的第三个参数是期望加速度，直接得到 τ = M·a_d + C·dq + g

阻抗控制（Impedance Control）：
  输入：位置/速度（来自轨迹），输出：力
  F = K·(x_d - x) + D·(ẋ_d - ẋ) + M·(ẍ_d - ẍ)
  τ = J^T · F + g(q)

导纳控制（Admittance Control）：
  输入：力（来自力传感器），输出：位置修正
  M·ẍ_c + D·ẋ_c + K·x_c = F_ext
  x_d' = x_d + x_c，再交给位置控制器

// 笛卡尔阻抗控制（6 维，位置 + 姿态）
Eigen::Matrix<double, 6, 1> x_err;          // 位置误差 + 姿态误差（so(3) 对数）
Eigen::Matrix<double, 6, 1> dx_err;
Eigen::Matrix<double, 6, 1> F =
    K.cwiseProduct(x_err) + D.cwiseProduct(dx_err);

// 映射到关节力矩
Eigen::MatrixXd J(6, n);
pinocchio::getJointJacobian(model, data, ee_id, pinocchio::LOCAL, J);
Eigen::VectorXd tau = J.transpose() * F + gravity_compensation(q);

不稳定的三个来源：
  1. 环境刚度估计错误
     环境比预期硬，闭环增益过高 → 振荡
     对策：在线估计环境刚度，或用自适应阻抗

  2. 延迟
     力传感器采样、滤波、控制周期累加延迟
     闭环带宽上限 ≈ 1/(5~10 × 总延迟)
     若总延迟 10 ms，力控带宽不应超过 10~20 Hz

  3. 接触切换
     从自由空间到接触是模型跳变
     对策：接近阶段用位置控制并降低速度，接触后用渐变的刚度过渡

模型 1（最简）：τ_f = f_v · q̇ + f_c · sign(q̇)
  库仑摩擦 + 粘滞摩擦，两个参数，覆盖 80% 场景

模型 2（Stribeck）：τ_f = f_v·q̇ + f_c·sign(q̇) + (f_s - f_c)·exp(-|q̇/v_s|^δ)·sign(q̇)
  加入静摩擦与 Stribeck 效应，低速更准，参数 5 个

模型 3（LuGre）：内部状态变量 z，dz/dt = q̇ - σ0·|q̇|·z/g(q̇)
  动态摩擦模型，能捕捉预滑移，参数 6 个，计算量最大

class JointController {
 public:
  void updateState(const Eigen::VectorXd & q, const Eigen::VectorXd & dq,
                   double t) {
    q_ = q; dq_ = dq; t_ = t;
  }
  Eigen::VectorXd computeTorque() {
    switch (mode_) {
      case Mode::PID:      return pidTorque();
      case Mode::ComputedTorque: return computedTorque();
      case Mode::Impedance:      return impedanceTorque();
      default:             return Eigen::VectorXd::Zero(n_);   // 安全：零力矩
    }
  }
 private:
  Mode mode_ = Mode::PID;
};

增益整定经验值（1 kHz 控制周期、中等尺寸六轴臂）：
  PID：Kp = 50~200 N·m/rad, Kd = 1~5 N·m·s/rad, Ki = 0~20
  计算力矩：ω_n = 20~40 rad/s, ζ = 0.9~1.1
  阻抗：平移 K = 500~2000 N/m, D = 2ζ√(KM)
  姿态 K = 5~50 N·m/rad, D = 0.5~5 N·m·s/rad