基于 2-RSS-1U 的双足机器人并联踝关节分析与实现

论文中的设计参数（l_bar=85mm, l_rod=135mm, l_spacing=43mm），在保证结构紧凑的同时，实现了 Roll 方向[-20, 20]度和 Pitch 方向[-58, 42]度的宽阔运动范围。

运动学解算：从电机到足部的精确映射

拥有了先进的机械结构后，下一步是建立其精确的数学模型，以实现有效控制。这包括逆运动学、正运动学和雅可比矩阵的求解。

1. 逆运动学 (IK): 从目标姿态到电机指令

这是控制中最基础的一步：给定脚掌的目标姿态（q_roll, q_pitch），计算出两个驱动电机应达到的角度（θ1, θ2）。

• 求解逻辑： 该问题可以转化为一个清晰的几何约束求解问题。
  1. 根据目标 q_roll 和 q_pitch，确定脚掌平台在小腿坐标系下的旋转矩阵。
  2. 利用该旋转矩阵，计算出连杆与脚掌的连接点 C1 和 C2 的三维坐标。
  3. 此时，对于每条运动链，构成了由固定点 A_i、计算点 C_i 和已知连杆长度 l_bar、l_rod 约束的空间几何问题。
  4. 通过求解该几何约束，可以得到电机角度 θ_i 的解析解。
• 实现： 如论文中的公式 (10) 所示，该问题存在封闭形式的解析解。这意味着 IK 计算速度极快，且没有奇异性或多解问题，为实时位置控制提供了坚实的基础。

2. 雅可比矩阵 (Jacobian): 速度与力矩的传递桥梁

雅可比矩阵（Jacobian）建立了机器人关节空间速度与任务空间（末端执行器）速度之间的线性关系。对于我们的并联踝关节：

• 关节空间 是由两个主动驱动电机的角度 θ = [θ₁, θ₂]ᵀ 构成的。其速度为 θ̇ = [θ̇₁, θ̇₂]ᵀ。
• 任务空间 是由脚掌的姿态角 q = [q_roll, q_pitch]ᵀ 构成的。其角速度为 q̇ = [q̇_roll, q̇_pitch]ᵀ。

逆运动学函数可以表示为 θ = f(q)。对这个函数关于时间求导，根据链式法则，我们得到：

θ̇ = (∂f/∂q) * q̇

这个偏导数矩阵 ∂f/∂q 就是我们所说的雅可比矩阵 J。因此，速度关系可以精确地写为：

θ̇ = J * q̇

展开这个公式，我们可以看到雅可比矩阵 J 的具体形式：

[ θ̇₁ ]   [ ∂θ₁/∂q_roll  ∂θ₁/∂q_pitch ]   [ q̇_roll ]
[      ] = [                                  ] * [       ]
[ θ̇₂ ]   [ ∂θ₂/∂q_roll  ∂θ₂/∂q_pitch ]   [ q̇_pitch ]

其中：

• ∂θ₁/∂q_roll 表示当 q_pitch 保持不变时，q_roll 的微小变化会引起电机 1 角度 θ₁ 多大的变化。
• 其他三项的物理意义依此类推。
• 核心作用： 雅可比矩阵是并联机构控制的精髓所在。
  • 正运动学求解： 它是牛顿 - 拉夫逊法迭代步骤中的关键部分。
  • 速度控制： 实现了从期望的脚掌角速度到所需电机角速度的直接计算。
  • 力矩控制： 通过其转置矩阵 J^T，可以建立末端力矩与电机驱动力矩之间的关系。这使得我们可以精确控制脚掌与地面的接触力，实现柔顺控制、力矩伺服等高级功能，这是提升机器人与环境交互能力的核心技术。

3. 正运动学 (FK): 从编码器读数到实际姿态

这是 IK 的逆问题：根据从电机编码器读取的实际角度（θ1, θ2），反算出脚掌的当前姿态（q_roll, q_pitch）。这对于状态估计、闭环控制和安全监控至关重要。

• 挑战： 这个问题无法直接求得解析解，其数学形式对应一个复杂的高阶多项式方程组，直接求解非常困难。
• 解决方案：牛顿 - 拉夫逊（Newton-Raphson）迭代法。 这是一种高效的数值求解方法。
  1. 定义误差函数： 我们定义一个误差函数 e(q) = θ_actual - f_IK(q_guess)。这个函数表示：在我猜测的姿态 q_guess 下，计算出的理论电机角度与实际读取的电机角度 θ_actual 之间的差距。我们的目标就是找到一个 q，让这个误差 e(q) 趋近于零。
  2. 迭代求解： 我们从一个初始猜测值 q_k 开始（通常是上一个控制周期的姿态），然后利用雅可比矩阵进行迭代更新：
     **q_{k+1} = q_k + J⁻¹(q_k) * e(q_k)**
     这个公式的直观意义是：利用雅可比矩阵的逆 J⁻¹，将关节空间的误差 e(q_k) 映射回任务空间，得到一个姿态修正量，从而更新我们的猜测值。
  3. 收敛： 重复 2-3 步，直至误差小于预设的阈值。如论文表 1 所示，该方法收敛速度极快，通常在几次迭代内即可达到非常高的精度，完全满足实时性要求。

逆运动学 C++ 实现示例

以下提供一份基于 Eigen 库的 C++ 逆运动学算法示例代码，涵盖参数初始化、姿态映射及错误处理逻辑。

#include <Eigen/Dense>
#include <iostream>
#include <cmath>
#include <stdexcept>

// 并联机构几何参数结构体
struct ExternalParameters {
    double pm_ankle_a_1[3] = {0.0, 53, 180}; // 执行器 1 的 A 点坐标 [mm]
    double pm_ankle_c_1[3] = {-70.0, 53, 0.0}; // 执行器 1 的 C 点初始坐标 [mm]
    double pm_ankle_bar_rod_1[2] = {70, 180}; // 执行器 1 连杆长度 [法兰杆，连杆] [mm]
    double pm_ankle_a_2[3] = {0.0, -53, 100.0}; // 执行器 2 的 A 点坐标 [mm]
    double pm_ankle_c_2[3] = {-70.0, -53, 0.0}; // 执行器 2 的 C 点初始坐标 [mm]
    double pm_ankle_bar_rod_2[2] = {70, 100}; // 执行器 2 连杆长度 [法兰杆，连杆] [mm]
};

// 单个执行器的几何参数
struct AnkleActuatorParams {
    Eigen::Vector3d r_Ai_0; // A 点在基坐标系下的位置向量 [mm]
    Eigen::Vector3d r_Ci_0_initial; // C 点在初始姿态下的位置向量 [mm]
    double l_bar; // 法兰杆长度 [mm]
    double l_rod; // 连杆长度 [mm]
    double alpha_offset_rad; // 电机零位偏移 [rad]
    double l_bar_sq; // l_bar 的平方，用于加速计算 [mm^2]
    double l_rod_sq; // l_rod 的平方，用于加速计算 [mm^2]

    AnkleActuatorParams() : l_bar(0), l_rod(0), alpha_offset_rad(0), l_bar_sq(0), l_rod_sq(0) {}

    AnkleActuatorParams(const Eigen::Vector3d& r_ai_0, const Eigen::Vector3d& r_ci_0_initial, double lbar, double lrod, double alpha_rad = 0.0)
        : r_Ai_0(r_ai_0), r_Ci_0_initial(r_ci_0_initial), l_bar(lbar), l_rod(lrod), alpha_offset_rad(alpha_rad) {
        if (l_bar <= 1e-9) {
            throw std::runtime_error("l_bar 必须大于零");
        }
        l_bar_sq = l_bar * l_bar;
        l_rod_sq = l_rod * l_rod;
    }
};

// 绕 X 轴旋转的旋转矩阵
Eigen::Matrix3d Rx(double angle_rad) {
    return Eigen::AngleAxisd(angle_rad, Eigen::Vector3d::UnitX()).toRotationMatrix();
}

// 绕 Y 轴旋转的旋转矩阵
Eigen::Matrix3d Ry(double angle_rad) {
    return Eigen::AngleAxisd(angle_rad, Eigen::Vector3d::UnitY()).toRotationMatrix();
}

// 计算单个执行器的逆运动学
// 输入：roll 和 pitch 角度，执行器几何参数
// 输出：pair<电机角度 [rad], 计算是否成功>
std::pair<double, bool> calculate_single_theta_i_internal(double q_roll_rad, double q_pitch_rad, const AnkleActuatorParams& params) {
    const Eigen::Vector3d& r_Ai = params.r_Ai_0;
    // 计算旋转后的 C 点位置
    Eigen::Matrix3d X_rot = Ry(q_pitch_rad) * Rx(q_roll_rad);
    Eigen::Vector3d r_Ci = X_rot * params.r_Ci_0_initial;
    Eigen::Vector3d r_Ci_minus_r_Ai = r_Ci - r_Ai;

    // 计算中间变量 a, b, c
    double a_val = r_Ci_minus_r_Ai.x();
    double b_val = (r_Ai - r_Ci).z();
    double norm_sq_r_Ci_minus_r_Ai = r_Ci_minus_r_Ai.squaredNorm();

    if (params.l_bar < 1e-9) {
        std::cerr << "错误：l_bar 接近零" << std::endl;
        return {0.0, false};
    }
    double c_val = (params.l_rod_sq - params.l_bar_sq - norm_sq_r_Ci_minus_r_Ai) / (2.0 * params.l_bar);

    // 检查 arcsin 参数的有效性
    double den_asin = a_val * a_val + b_val * b_val;
    double inner_sqrt_term = a_val * a_val + b_val * b_val - c_val * c_val;

    if (inner_sqrt_term < -1e-9) {
        std::cerr << "错误：配置不可达，平方根项为负 (" << inner_sqrt_term << ")" << std::endl;
        return {0.0, false};
    }
    if (inner_sqrt_term < 0) inner_sqrt_term = 0;
    double num_asin = b_val * c_val + std::abs(a_val) * std::sqrt(inner_sqrt_term);

    if (std::abs(den_asin) < 1e-12) {
        std::cerr << "错误：arcsin 分母接近零" << std::endl;
        if (std::abs(num_asin) < 1e-9) {
            std::cout << "警告：奇异构型，theta 未定义" << std::endl;
        }
        return {0.0, false};
    }
    double arg_asin = num_asin / den_asin;
    if (std::abs(arg_asin) > 1.0 + 1e-9) {
        std::cerr << "错误：arcsin 参数超出定义域 [-1, 1]: " << arg_asin << std::endl;
        return {0.0, false};
    }
    arg_asin = std::max(-1.0, std::min(1.0, arg_asin));

    // 计算电机角度
    double theta_i_rad = std::asin(arg_asin);
    theta_i_rad += params.alpha_offset_rad;
    return {theta_i_rad, true};
}

// 并联机构逆解结果
struct AnkleIKSolution {
    double theta_1_rad; // 执行器 1 的电机角度 [rad]
    bool success_1;     // 执行器 1 计算是否成功
    double theta_2_rad; // 执行器 2 的电机角度 [rad]
    bool success_2;     // 执行器 2 计算是否成功

    AnkleIKSolution() : theta_1_rad(0), success_1(false), theta_2_rad(0), success_2(false) {}
};

// 计算并联机构的逆运动学（两个执行器）
// 输入：踝关节的目标 roll 和 pitch 角度，两个执行器的几何参数
// 输出：两个执行器的电机角度及计算状态
AnkleIKSolution calculate_ankle_ik_both_actuators(double q_roll_rad, double q_pitch_rad, const AnkleActuatorParams& params_1, const AnkleActuatorParams& params_2) {
    AnkleIKSolution solution;
    auto result_1 = calculate_single_theta_i_internal(q_roll_rad, q_pitch_rad, params_1);
    solution.theta_1_rad = result_1.first;
    solution.success_1 = result_1.second;

    auto result_2 = calculate_single_theta_i_internal(q_roll_rad, q_pitch_rad, params_2);
    solution.theta_2_rad = result_2.first;
    solution.success_2 = result_2.second;

    return solution;
}

// 并联机构控制类
class PMAnkle {
public:
    ExternalParameters ext_params;
    AnkleActuatorParams params_1; // 执行器 1 的参数
    AnkleActuatorParams params_2; // 执行器 2 的参数

    PMAnkle() {}

    // 初始化并联机构的几何参数
    void PMankleInit() {
        // 初始化执行器 1
        Eigen::Vector3d r_A_1_vec(ext_params.pm_ankle_a_1[0], ext_params.pm_ankle_a_1[1], ext_params.pm_ankle_a_1[2]);
        Eigen::Vector3d r_C_1_vec(ext_params.pm_ankle_c_1[0], ext_params.pm_ankle_c_1[1], ext_params.pm_ankle_c_1[2]);
        double L_bar_1 = ext_params.pm_ankle_bar_rod_1[0];
        double L_rod_1 = ext_params.pm_ankle_bar_rod_1[1];
        double alpha_1_rad = 0.0;
        params_1 = AnkleActuatorParams(r_A_1_vec, r_C_1_vec, L_bar_1, L_rod_1, alpha_1_rad);

        // 初始化执行器 2
        Eigen::Vector3d r_A_2_vec(ext_params.pm_ankle_a_2[0], ext_params.pm_ankle_a_2[1], ext_params.pm_ankle_a_2[2]);
        Eigen::Vector3d r_C_2_vec(ext_params.pm_ankle_c_2[0], ext_params.pm_ankle_c_2[1], ext_params.pm_ankle_c_2[2]);
        double L_bar_2 = ext_params.pm_ankle_bar_rod_2[0];
        double L_rod_2 = ext_params.pm_ankle_bar_rod_2[1];
        double alpha_2_rad = 0.0;
        params_2 = AnkleActuatorParams(r_A_2_vec, r_C_2_vec, L_bar_2, L_rod_2, alpha_2_rad);

        std::cout << "并联机构初始化完成" << std::endl;
        std::cout << "  执行器 1 - L_bar: " << params_1.l_bar << " mm, L_rod: " << params_1.l_rod << " mm" << std::endl;
        std::cout << "  执行器 2 - L_bar: " << params_2.l_bar << " mm, L_rod: " << params_2.l_rod << " mm" << std::endl;
    }

    // 计算给定姿态角度下的电机角度
    AnkleIKSolution getAnkleMotorAngles(double q_roll_rad, double q_pitch_rad) {
        return calculate_ankle_ik_both_actuators(q_roll_rad, q_pitch_rad, params_1, params_2);
    }
};

int main() {
    PMAnkle ankle_ankle;
    std::cout << "=== 踝关节并联机构逆运动学求解 ===" << std::endl;
    ankle_ankle.PMankleInit();

    // 设置目标姿态角
    double q_roll_deg = 10.0;
    double q_pitch_deg = 10.0;
    double q_roll_rad = q_roll_deg * M_PI / 180.0;
    double q_pitch_rad = q_pitch_deg * M_PI / 180.0;

    std::cout << "\n目标姿态角:" << std::endl;
    std::cout << " Roll: " << q_roll_deg << " 度 (" << q_roll_rad << " rad)" << std::endl;
    std::cout << " Pitch: " << q_pitch_deg << " 度 (" << q_pitch_rad << " rad)" << std::endl;

    // 求解逆运动学
    AnkleIKSolution solution = ankle_ankle.getAnkleMotorAngles(q_roll_rad, q_pitch_rad);

    std::cout << "\n电机角度:" << std::endl;
    if (solution.success_1) {
        std::cout << "  电机 1: " << solution.theta_1_rad * 180.0 / M_PI << " 度 (" << solution.theta_1_rad << " rad)" << std::endl;
    } else {
        std::cout << "  电机 1: 计算失败" << std::endl;
    }
    if (solution.success_2) {
        std::cout << "  电机 2: " << solution.theta_2_rad * 180.0 / M_PI << " 度 (" << solution.theta_2_rad << " rad)" << std::endl;
    } else {
        std::cout << "  电机 2: 计算失败" << std::endl;
    }
    return 0;
}