基于2-RSS-1U的双足机器人并联踝关节分析与实现
"当你的机器人开始像人类一样思考如何走路时，你会发现，原来最复杂的不是大脑，而是脚踝。"这句话在机器人学界越来越成为共识。论文ASAP中的研究也证实，在sim2real中，偏差最大的正是踝关节控制。
参考文献：On the Comprehensive Kinematics Analysis of a Humanoid Parallel Ankle Mechanism
结构变体：Structural design and motion analysis of parallel ankle joints for humanoid robots
脚踝革命：深入解析人形机器人高性能并联踝关节
传统的单轴踝关节设计，就像给机器人穿了一双"高跟鞋"——虽然能走，但走得很僵硬，很危险。我们需要的是像人类脚踝一样的灵活性：既能前后摆动（pitch），又能左右倾斜（roll）。
技术路线的抉择：串联与并联的深度对比
在机器人双自由度踝关节设计中，串联与并联是两种构型选择，而并联已经逐步成为主流。
1. 串联构型:
这是最直接的实现方式。Pitch（俯仰）和Roll（侧滚）两个自由度通过将两个电机依次堆叠而成。一个电机驱动Pitch轴，另一个安装在其上驱动Roll轴。
- 优势： 控制算法简单直观。每个电机的运动与一个独立的关节自由度直接对应，运动学解算非常直接。
- 劣势：
- 转动惯量大，动态性能差: 致命弱点！处于运动链末端的电机（如Roll电机）及其减速器等组件，成为了上一级关节的额外负载。整个腿部末端的惯量显著增加，这直接导致机器人摆腿速度慢、能耗高。
- 刚度较低，承载能力弱: 负载通过每个关节依次传递，是悬臂梁式的受力结构。每个关节的轴承和齿轮箱都是薄弱点，整体刚性不如并联。
2. 并联构型 (Parallel Kinematics):
并联构型将两个驱动器布置在固定的基座（小腿）上，通过独立的连杆机构共同驱动一个移动平台（脚掌）。
- 初步挑战： 运动学耦合。单个电机的运动会同时影响Pitch和Roll两个自由度，其运动学和动力学模型比串联结构复杂得多。
- 核心优势 (选择并联的根本原因):
- 低转动惯量： 将质量最大的电机部件上移至小腿，极大地降低了脚部和脚踝的转动惯量。这对于实现快速、节能的动态行走和跑跳至关重要。
- 高刚度与高承载力： 闭环的并联结构天然比开环的串联结构具有更高的刚度，能够抵抗更大的外部冲击和负载，为机器人提供更稳定的支撑。
综合考量，从各家一系列先进人形机器人的设计趋势表明，并联踝关节是实现高动态、高效率运动的必然选择。尽管其控制算法更复杂，但其在物理性能上的巨大优势是值得我们投入精力去攻克的。
机构设计：2-RSS-1U 并联方案
在参考了Zhou与Tsagarakis的全面分析后，我们最终选择了 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: 两条并联运动链之间的间距。
论文中的设计参数（l_bar=85mm, l_rod=135mm, l_spacing=43mm），在保证结构紧凑的同时，实现了Roll方向[-20, 20]度和Pitch方向[-58, 42]度的宽阔运动范围。
运动学解算：从电机到足部的精确映射
拥有了先进的机械结构后，下一步是建立其精确的数学模型，以实现有效控制。这包括逆运动学、正运动学和雅可比矩阵的求解。
1. 逆运动学 (IK): 从目标姿态到电机指令
这是控制中最基础的一步：给定脚掌的目标姿态（q_roll, q_pitch），计算出两个驱动电机应达到的角度（θ1, θ2）。
- 求解逻辑： 该问题可以转化为一个清晰的几何约束求解问题。
- 根据目标
q_roll和q_pitch，确定脚掌平台在小腿坐标系下的旋转矩阵。 - 利用该旋转矩阵，计算出连杆与脚掌的连接点
C1和C2的三维坐标。 - 此时，对于每条运动链，构成了由固定点
A_i、计算点C_i和已知连杆长度l_bar、l_rod约束的空间几何问题。 - 通过求解该几何约束，可以得到电机角度
θ_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）迭代法。 这是一种高效的数值求解方法。
- 定义误差函数： 我们定义一个误差函数
e(q) = θ_actual - f_IK(q_guess)。这个函数表示：在我猜测的姿态q_guess下，计算出的理论电机角度与实际读取的电机角度θ_actual之间的差距。我们的目标就是找到一个q，让这个误差e(q)趋近于零。 - 迭代求解： 我们从一个初始猜测值
q_k开始（通常是上一个控制周期的姿态），然后利用雅可比矩阵进行迭代更新：q_{k+1} = q_k + J⁻¹(q_k) * e(q_k)
这个公式的直观意义是：利用雅可比矩阵的逆J⁻¹，将关节空间的误差e(q_k)映射回任务空间，得到一个姿态修正量，从而更新我们的猜测值。 - 收敛： 重复2-3步，直至误差小于预设的阈值。如论文表1所示，该方法收敛速度极快，通常在几次迭代内即可达到非常高的精度，完全满足实时性要求。
- 定义误差函数： 我们定义一个误差函数
个人示例逆解代码
为了方便大家理解和使用，我把这个并联踝关节的逆运动学算法封装成了一份C++示例代码。大家可以随意拿去测试和优化。
在main函数里，我给出了一个完整的使用示例。你只需要：
- 在
ExternalParameters结构体中填入你自己的机构尺寸参数。 - 修改
q_roll_deg和q_pitch_deg为你想要测试的目标角度。
编译并运行，你就能立即得到两个电机需要转动到的精确角度。希望这份代码能帮助大家快速上手并联机构的控制！
//g++ -std=c++17 -O2 -I/usr/include/eigen3 pm_ankle_example.cpp -o pm_ankle_example#include<Eigen/Dense>#include<iostream>#include<cmath>#include<stdexcept>// 并联机构几何参数结构体structExternalParameters{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]};// 单个执行器的几何参数structAnkleActuatorParams{ 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){returnEigen::AngleAxisd(angle_rad, Eigen::Vector3d::UnitX()).toRotationMatrix();}// 绕Y轴旋转的旋转矩阵 Eigen::Matrix3d Ry(double angle_rad){returnEigen::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, cdouble 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};}// 并联机构逆解结果structAnkleIKSolution{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;// 计算执行器1的角度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;// 计算执行器2的角度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;}// 并联机构控制类classPMAnkle{public: ExternalParameters ext_params; AnkleActuatorParams params_1;// 执行器1的参数 AnkleActuatorParams params_2;// 执行器2的参数PMAnkle(){}// 初始化并联机构的几何参数voidPMankleInit(){// 初始化执行器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){returncalculate_ankle_ik_both_actuators(q_roll_rad, q_pitch_rad, params_1, params_2);}};intmain(){ 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;}return0;}一个不错的主流踝关节分析视频
Ankle Deep: What is the Best Design for a Humanoid Ankle? With Scott Walter and Marwa ElDiwiny
传送门
