双足机器人 2-RSS-1U 并联踝关节设计与运动学分析

双足机器人 2-RSS-1U 并联踝关节设计与运动学分析

在双足机器人的行走控制中，脚踝往往是被低估的关键环节。大量仿真到实物的迁移（Sim2Real）研究表明，踝关节控制的偏差是导致步态不稳的主要原因之一。为了实现高动态、高效率的运动，选择合适的踝关节构型至关重要。

串联与并联：技术路线的抉择

传统的单轴或串联踝关节设计虽然控制简单，但存在明显的物理局限。想象一下给机器人穿了一双'高跟鞋'，虽然能动，但动作僵硬且风险较高。我们需要的是像人类脚踝一样的灵活性：既能前后摆动（Pitch），又能左右倾斜（Roll）。

串联构型

这是最直接的实现方式，两个电机依次堆叠驱动 Pitch 和 Roll 轴。

• 优势：控制算法直观，每个电机对应一个自由度，运动学解算简单。
• 劣势：
  • 转动惯量大：末端电机及其减速器成为上一级关节的负载，导致摆腿速度慢、能耗高。
  • 刚度较低：悬臂梁式的受力结构使得整体刚性不如并联，承载能力弱。

并联构型 (Parallel Kinematics)

将驱动器布置在基座（小腿）上，通过连杆机构共同驱动脚掌平台。

• 挑战：运动学耦合复杂，单个电机同时影响多个自由度。
• 核心优势：
  • 低转动惯量：质量最大的电机上移至小腿，极大降低了脚部惯量，利于快速动态行走。
  • 高刚度与承载力：闭环结构天然比开环更刚硬，能抵抗外部冲击。

综合来看，尽管并联控制算法更复杂，但其物理性能优势使其成为先进人形机器人的必然选择。

机构设计：2-RSS-1U 方案

参考相关文献后，我们选择了 2-RSS-1U 这一并联构型。

[图：2-RSS-1U 机构示意图]

该名称精确描述了机械结构：

• 2-RSS：两条相同的 R-S-S 运动链。
  • R (Revolute)：位于小腿上的主动旋转关节，由电机驱动。
  • S (Spherical)：两个球形关节，分别位于连杆中段和连接脚掌处，提供空间自由度。
• 1-U：中心的一个被动万向节 (Universal Joint)，定义了脚掌的 Pitch 和 Roll 运动。

相比于 UPU 或 UPS 等构型，RSS 结构在力矩传递上表现更佳，力矩传递比在运动空间内变化更小，有利于复杂地形下的精确力控制。

在设计阶段，我们重点关注了关键几何参数：

• l_bar：电机驱动臂长度。
• l_rod：中间连杆长度。
• l_spacing：两条运动链间距。

合理的参数配置能在保证紧凑的同时，实现 Roll 方向 [-20, 20] 度和 Pitch 方向 [-58, 42] 度的宽阔运动范围。

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

有了先进的机械结构，下一步是建立精确的数学模型。

逆运动学 (IK)

给定脚掌目标姿态 (q_roll, q_pitch)，计算电机角度 (θ1, θ2)。

求解逻辑转化为几何约束问题：根据目标姿态确定旋转矩阵，计算连杆连接点坐标，利用已知连杆长度约束求解电机角度。该问题存在封闭形式的解析解，计算速度快且无奇异性，为实时位置控制奠定基础。

雅可比矩阵 (Jacobian)

雅可比矩阵建立了关节空间速度与任务空间速度之间的线性关系。

对于并联踝关节：

• 关节空间：电机角度 θ = [θ₁, θ₂]ᵀ
• 任务空间：脚掌姿态角 q = [q_roll, q_pitch]ᵀ

速度关系可表示为：θ̇ = J * q̇

其中 J 是偏导数矩阵。它是并联机构控制的精髓：

• 正运动学求解：牛顿 - 拉夫逊法迭代的关键部分。
• 速度控制：直接计算期望脚掌角速度对应的电机角速度。
• 力矩控制：通过转置矩阵 J^T 建立末端力矩与电机电驱力的关系，支持柔顺控制和力矩伺服。

正运动学 (FK)

根据编码器读数反算脚掌实际姿态。这是一个高阶多项式方程组，无法直接求得解析解。

解决方案：牛顿 - 拉夫逊迭代法。

1. 定义误差函数：e(q) = θ_actual - f_IK(q_guess)。
2. 迭代更新：q_{k+1} = q_k + J⁻¹(q_k) * e(q_k)。
3. 收敛：重复直至误差小于阈值。该方法收敛极快，通常几次迭代即可满足实时性要求。

参考实现代码

下面给出一个基于 C++ 和 Eigen 库的逆运动学实现示例。你可以将其作为基础框架，填入实际的机构尺寸参数进行测试。

#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 的平方
    double l_rod_sq;             // l_rod 的平方

    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();
}

// 计算单个执行器的逆运动学
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;
    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;

    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);

    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;
    bool success_1;
    double theta_2_rad;
    bool success_2;

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

// 计算并联机构的逆运动学
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;
    AnkleActuatorParams params_2;

    PMAnkle() {}

    void PMankleInit() {
        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];
        params_1 = AnkleActuatorParams(r_A_1_vec, r_C_1_vec, L_bar_1, L_rod_1, 0.0);

        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];
        params_2 = AnkleActuatorParams(r_A_2_vec, r_C_2_vec, L_bar_2, L_rod_2, 0.0);

        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;
}

以上代码展示了如何封装逆运动学逻辑。编译时请确保链接 Eigen 库（例如 g++ -std=c++17 -O2 -I/usr/include/eigen3 pm_ankle_example.cpp -o pm_ankle_example）。在实际项目中，建议将参数配置化，并增加异常处理机制以适应不同的硬件环境。