1. UR机械臂运动学基础机械臂运动学是机器人控制的核心内容它研究的是机械臂各关节运动与末端执行器位置姿态之间的关系。URUniversal Robots作为协作机器人领域的领导者其六轴机械臂在工业自动化中应用广泛。理解UR机械臂的正逆运动学是进行轨迹规划、力控制和避障等高级功能的基础。运动学分为正运动学和逆运动学两个部分。正运动学解决的是已知关节角度求末端位姿的问题而逆运动学则是已知末端位姿求关节角度。在实际项目中我们通常需要同时使用这两种方法先用逆运动学计算目标位姿对应的关节角度再用正运动学验证计算结果的正确性。2. DH参数建模2.1 改进DH参数法UR机械臂采用改进的Denavit-HartenbergDH参数法建立运动学模型。与标准DH参数法相比改进DH参数法在坐标系定义和参数含义上有所不同更适合描述UR这类串联机械臂。改进DH参数法的四个关键参数定义如下aᵢ沿Xᵢ轴从Zᵢ移动到Zᵢ₊₁的距离连杆长度αᵢ绕Xᵢ轴从Zᵢ旋转到Zᵢ₊₁的角度连杆扭转角dᵢ沿Zᵢ轴从Xᵢ₋₁移动到Xᵢ的距离连杆偏移θᵢ绕Zᵢ轴从Xᵢ₋₁旋转到Xᵢ的角度关节角度UR5机械臂的DH参数表如下关节α (rad)a (mm)d (mm)θ (rad)1π/2089.2θ₁20-4250θ₂30-3920θ₃4π/20109.3θ₄5-π/2094.75θ₅60082.5θ₆2.2 坐标系建立技巧在实际建立坐标系时有几个关键点需要注意Z轴始终沿关节旋转轴方向对于旋转关节X轴方向沿相邻Z轴的公垂线原点位于相邻Z轴公垂线与当前Z轴的交点最后一个坐标系工具坐标系通常与末端法兰中心重合初学者常犯的错误是混淆了标准DH和改进DH的坐标系定义。建议在纸上先画出所有关节的Z轴再根据右手定则确定X轴方向最后标出各个坐标系的原点位置。3. 正运动学推导与实现3.1 变换矩阵推导基于改进DH参数相邻坐标系i-1到i的变换矩阵可以表示为^{i-1}T_i Rot(Z,θ_i) \cdot Trans(Z,d_i) \cdot Trans(X,a_i) \cdot Rot(X,α_i)展开后得到4×4齐次变换矩阵^{i-1}T_i \begin{bmatrix} cosθ_i -sinθ_i 0 a_i \\ sinθ_i cosα_i cosθ_i cosα_i -sinα_i -sinα_i d_i \\ sinθ_i sinα_i cosθ_i sinα_i cosα_i cosα_i d_i \\ 0 0 0 1 \end{bmatrix}正运动学的最终目标是计算从基座标系到末端坐标系的变换矩阵⁰T₆可以通过连续相乘得到^0T_6 ^0T_1 \cdot ^1T_2 \cdot ^2T_3 \cdot ^3T_4 \cdot ^4T_5 \cdot ^5T_63.2 C代码实现以下是使用Eigen库实现正运动学的关键代码#include Eigen/Dense using namespace Eigen; Matrix4d computeTransformMatrix(double a, double alpha, double d, double theta) { Matrix4d T; double ct cos(theta), st sin(theta); double ca cos(alpha), sa sin(alpha); T ct, -st, 0, a, st*ca, ct*ca, -sa, -sa*d, st*sa, ct*sa, ca, ca*d, 0, 0, 0, 1; return T; } Matrix4d forwardKinematics(const Vector6d joint_angles) { // UR5 DH parameters const double a[6] {0, -0.425, -0.392, 0, 0, 0}; const double alpha[6] {M_PI/2, 0, 0, M_PI/2, -M_PI/2, 0}; const double d[6] {0.0892, 0, 0, 0.1093, 0.09475, 0.0825}; Matrix4d T Matrix4d::Identity(); for(int i0; i6; i) { T T * computeTransformMatrix(a[i], alpha[i], d[i], joint_angles[i]); } return T; }这段代码先定义了计算单个变换矩阵的函数然后通过循环连续相乘得到最终变换矩阵。注意角度单位是弧度长度单位是米。4. 逆运动学解析4.1 逆解基本思路UR机械臂的逆运动学求解采用解析法通过几何关系和代数运算直接求出关节角度。由于六自由度机械臂的逆解通常有多组解UR机械臂有8组解我们需要找到最符合实际应用的那一组。逆运动学求解的关键步骤根据末端位姿计算变换矩阵⁰T₆利用变换矩阵相等关系建立方程逐步求解各关节角度通常按θ₁→θ₅→θ₆→θ₃→θ₂→θ₄的顺序处理多解情况选择最优解4.2 关节角度求解详解关节1求解通过比较变换矩阵两边第三行第四列元素可以得到θ₁ atan2(p_y, p_x) ± atan2(√(p_x² p_y² - d₄²), d₄)这里需要注意atan2函数的使用它能够返回完整的360°范围内的角度避免了普通反正切函数的象限问题。关节5求解通过比较第三行第三列元素θ₅ ±acos(a_z sinθ₁ - n_z cosθ₁)由于cos函数的对称性这里会产生两组解。关节6求解利用旋转矩阵元素关系θ₆ atan2((-a_y sinθ₁ a_x cosθ₁)/sinθ₅, (o_y sinθ₁ - o_x cosθ₁)/sinθ₅)需要注意当θ₅接近0时会出现奇异点需要特殊处理。关节3求解通过几何关系建立方程θ₃ ±acos((A² B² - a₂² - a₃²)/(2a₂a₃))其中A和B是与末端位置相关的中间变量。关节2和关节4求解在已知其他关节角度后可以通过矩阵相等关系直接推导出θ₂和θ₄的表达式同样需要使用atan2函数来保证角度范围的正确性。4.3 逆运动学C实现以下是逆运动学的部分实现代码Vector6d inverseKinematics(const Matrix4d T) { Vector6d solutions[8]; // 存储8组解 Vector6d best_solution; // 提取旋转矩阵和平移向量 Matrix3d R T.block3,3(0,0); Vector3d p T.block3,1(0,3); // 求解θ1 double theta1[2]; double temp p.x()*p.x() p.y()*p.y() - d4*d4; theta1[0] atan2(p.y(), p.x()) atan2(sqrt(temp), d4); theta1[1] atan2(p.y(), p.x()) atan2(-sqrt(temp), d4); // 求解θ5 for(int i0; i2; i) { double s1 sin(theta1[i]), c1 cos(theta1[i]); double theta5[2]; theta5[0] acos(R(2,2)*s1 - R(0,2)*c1); theta5[1] -theta5[0]; // 继续求解其他关节角度... } // 选择最优解如最小位移解 return best_solution; }在实际实现中需要考虑数值稳定性、奇异点处理等问题。特别是在θ₅接近0时需要使用不同的求解策略。5. 验证与调试5.1 URSim仿真验证UR官方提供的URSim仿真器是验证运动学算法的理想工具。验证步骤如下在URSim中手动设置一组关节角度记录末端位姿将相同关节角度输入自研的正运动学程序比较结果将URSim中的末端位姿输入逆运动学程序验证是否能还原原始关节角度典型验证数据示例输入关节角度(度): [30, -45, 60, -30, 45, 0] URSim末端位姿: [0.2, 0.3, 0.4, 0.1, 0.2, 0.3] 自研正解结果: [0.199, 0.301, 0.399, 0.099, 0.201, 0.299]5.2 常见问题排查在实际项目中我遇到过几个典型问题姿态表示混淆UR控制器支持RPY角和旋转向量两种姿态表示必须明确使用哪一种。建议统一使用RPY角并注意旋转顺序通常为ZYX。单位不一致DH参数表中的长度单位可能是毫米而算法实现中使用米需要统一。奇异点处理当机械臂完全伸直或折叠时逆解可能不存在或有无穷多解需要特殊处理。多解选择八组解中如何选择最优解通常考虑关节限位、运动平滑性等因素。6. 完整代码框架以下是整合正逆运动学的完整类定义class URKinematics { public: URKinematics() { // UR5 DH parameters a_ 0, -0.425, -0.392, 0, 0, 0; alpha_ M_PI/2, 0, 0, M_PI/2, -M_PI/2, 0; d_ 0.0892, 0, 0, 0.1093, 0.09475, 0.0825; } Matrix4d forward(const Vector6d q) { Matrix4d T Matrix4d::Identity(); for(int i0; i6; i) { T * computeDHMatrix(a_[i], alpha_[i], d_[i], q[i]); } return T; } std::vectorVector6d inverse(const Matrix4d T) { std::vectorVector6d solutions; // 实现逆运动学求解返回所有有效解 return solutions; } Vector6d bestSolution(const Matrix4d T) { auto all_solutions inverse(T); // 选择最优解策略 return all_solutions[0]; } private: Vector6d a_, alpha_, d_; Matrix4d computeDHMatrix(double a, double alpha, double d, double theta) { // 同前... } };这个框架可以根据实际需求扩展比如添加奇异点处理、关节限位检查等功能。完整实现需要考虑更多工程细节如数值稳定性、计算效率等。