完整UR机械臂逆运动学求解过程及c++代码实现

Posted 2023-03-02 神秘孙大哥

有任何问题请在评论区留言，我尽可能的回复大家

一. 逆运动学的求解需要以下数学运算

1. 利用DH参数得到每个关节的变换矩阵；
2. 利用变换矩阵求出机械臂整个链的变换矩阵；
3. 求出末端位姿；
4. 利用已知末端位姿和整个链的变换矩阵，通过逆运动学方程来求解关节角度；
5. 根据需求选解。

二. 代码实现过程

1. 利用DH参数得到每个关节的变换矩阵：

Eigen::Matrix4d DH(double a, double d, double alpha, double theta) 
  Eigen::Matrix4d T;
  T << cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta),
       sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta),
       0, sin(alpha), cos(alpha), d,
       0, 0, 0, 1;
  return T;

2. 利用变换矩阵求出机械臂整个链的变换矩阵：

Eigen::Matrix4d T_0e = Eigen::Matrix4d::Identity(4,4);
for (int i = 0; i < n; i++) 
  T_0e = T_0e * DH(a[i], d[i], alpha[i], theta[i]);

3. 求出末端位姿：

Eigen::Matrix4d T_ee;
T_ee << 0.5938, -0.7381, 0.3254, 0.4494,
        0.8038, 0.5531, 0.2194, -0.1957,
        -0.0332, 0.3868, 0.9214, 0.6733,
        0, 0, 0, 1;

4. 然后利用已知末端位姿和整个链的变换矩阵，通过逆运动学方程来求解关节角度。

三. 逆运动学方程求解关节角度

逆运动学方程求解关节角度是一个非线性方程组，有多种方法求解，如解析解、数值解等。这里以数值解的方法为例，介绍如何用c++代码实现逆运动学方程的求解。

1. 实现齐次变换矩阵的逆变换：

Eigen::Matrix4d invT(const Eigen::Matrix4d& T) 
  Eigen::Matrix4d invT;
  invT.block<3,3>(0,0) = T.block<3,3>(0,0).transpose();
  invT.block<3,1>(0,3) = -invT.block<3,3>(0,0)*T.block<3,1>(0,3);
  invT.block<1,4>(3,0) << 0, 0, 0, 1;
  return invT;

2. 实现逆运动学方程：

Eigen::Matrix<double,6,1> inverseKinematics(const Eigen::Matrix4d& T_ee, 
                                             const Eigen::Matrix4d T_0e, 
                                             const Eigen::Vector3d& p_e,
                                             const Eigen::Vector3d& o_x,
                                             const Eigen::Vector3d& o_y,
                                             const Eigen::Vector3d& o_z) 
  Eigen::Matrix<double,6,1> theta;
  Eigen::Matrix4d T_0e_inv = invT(T_0e);
  Eigen::Matrix4d T_ee_0 = T_ee * T_0e_inv;
  Eigen::Vector3d p_0 = T_ee_0.block<3,1>(0,3);
  Eigen::Vector3d o_z_0 = T_0e_inv.block<3,3>(0,0) * o_z;
  Eigen::Vector3d o_y_0 = T_0e_inv.block<3,3>(0,0) * o_y;
  Eigen::Vector3d o_x_0 = T_0e_inv.block<3,3>(0,0) * o_x;
  // 具体实现逆运动学方程，这里省略
  return theta;

其中逆运动学方程的计算的详细过程如下：
● 求解末端位置p和姿态R的关于机器人的参考坐标系的坐标。
● 根据UR10机械臂的末端位置和姿态，计算关节角度。
实现逆运动学方程的代码，它计算出的结果是一个长度为6的Eigen向量，代表6个关节的角度：

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

Eigen::Matrix<double, 6, 1> inverseKinematics(const Eigen::Matrix4d& T_ee, 
                                             const Eigen::Matrix4d T_0e, 
                                             const Eigen::Vector3d& p_e,
                                             const Eigen::Vector3d& o_x,
                                             const Eigen::Vector3d& o_y,
                                             const Eigen::Vector3d& o_z)

    Eigen::Matrix<double, 6, 1> joint_angles;

    Eigen::Vector3d p_0e = T_0e.block<3,3>(0,0).transpose() * (p_e - T_0e.col(3).head<3>());

    double c5 = T_ee(2,2);
    double s5 = sqrt(1 - c5*c5);
    joint_angles(4) = atan2(s5, c5);
    joint_angles(5) = atan2(-T_ee(0,2), T_ee(1,2));
    joint_angles(3) = atan2(T_ee(2,1)/s5, T_ee(2,0)/s5);
    double s3 = sin(joint_angles(3));
    double c3 = cos(joint_angles(3));
    joint_angles(0) = atan2((p_0e(1)*s3 - p_0e(2)*c3) / s5, p_0e(0) - (p_0e(1)*c3 + p_0e(2)*s3) * c5);
    joint_angles(2) = atan2((p_0e(1)*c3 + p_0e(2)*s3) / c5, p_0e(0) - p_0e(1)*s3 + p_0e(2)*c3);
    joint_angles(1) = atan2(o_y(0), o_x(0));
    return joint_angles;

根据这个计算流程，将步骤 2 中省略的逆运动学方程具体实现的代码补充上：

Eigen::Matrix<double,6,1> inverseKinematics(const Eigen::Matrix4d& T_ee, 
                                             const Eigen::Matrix4d T_0e, 
                                             const Eigen::Vector3d& p_e,
                                             const Eigen::Vector3d& o_x,
                                             const Eigen::Vector3d& o_y,
                                             const Eigen::Vector3d& o_z) 
  Eigen::Matrix<double,6,1> theta;
  Eigen::Matrix4d T_0e_inv = invT(T_0e);
  Eigen::Matrix4d T_ee_0 = T_ee * T_0e_inv;
  Eigen::Vector3d p_0 = T_ee_0.block<3,1>(0,3);
  Eigen::Vector3d o_z_0 = T_0e_inv.block<3,3>(0,0) * o_z;
  Eigen::Vector3d o_y_0 = T_0e_inv.block<3,3>(0,0) * o_y;
  Eigen::Vector3d o_x_0 = T_0e_inv.block<3,3>(0,0) * o_x;
// 逆运动学方程的具体实现
  double q1, q2, q3, q4, q5, q6;
  double d = p_e(2) - p_0(2);
  q1 = atan2(p_0(1), p_0(0));
  double c2 = (pow(p_0(0), 2) + pow(p_0(1), 2) - pow(d, 2) - pow(o_x_0(2), 2)) / (2 * o_x_0(2) * sqrt(pow(p_0(0), 2) + pow(p_0(1), 2) - pow(d, 2)));
  q2 = atan2(sqrt(1-pow(c2, 2)), c2);
  q3 = atan2(o_z_0(2), -o_x_0(0) * sin(q2) + o_x_0(2) * cos(q2));
  double s4 = -o_y_0(2) * cos(q2) - o_y_0(0) * sin(q2) * sin(q3) + o_y_0(1) * sin(q2) * cos(q3);
  double c4 = o_x_0(0) * cos(q3) + o_x_0(1) * sin(q3) + o_x_0(2) * sin(q2);
  q4 = atan2(s4, c4);
  double s5 = o_x_0(0) * cos(q3) * sin(q4) + o_x_0(1) * sin(q3) * sin(q4) + o_x_0(2) * cos(q4);
  double c5 = o_y_0(0) * cos(q3) * cos(q4) + o_y_0(1) * sin(q3) * cos(q4) - o_y_0(2) * sin(q4);
  q5 = atan2(-s5, c5);
  double s6 = -o_x_0(0) * sin(q3) + o_x_0(1) * cos(q3);
  double c6 = o_y_0(0) * cos(q3) * cos(q5) + o_y_0(1) * sin(q3) * cos(q5) - o_y_0(2) * sin(q5);
  q6 = atan2(s6, c6);
  theta << q1, q2, q3, q4, q5, q6;
  return theta;
  

上面代码中的逆运动学方程的返回值 theta 可能会有多组解（UR机械臂通常为8组解），但是通常情况下仅返回一组最合适的解，因为它对应的正运动学方程只能够求出一组解。如果机器人的关节范围限制了某些解的取值范围，则需要在代码中加入关节范围限制的判断，以保证返回的解在关节范围内。
在当前代码中并没有对多组解进行选取的部分，所以该代码中直接返回的是求得的一组解。因为选取某一组解的方式取决于你所实现的逆运动学算法以及实际的应用需求，对于不同的需求，还需要对代码进行进一步的修改以实现选取一组合法的解的功能。
对于选解我在这里举一个例子：机械臂六个关节角度均有最大和最小的限制。
那么选解的代码可以写为：

if (q1 < q1_min) 
    q1 = q1 + 2 * M_PI;
  
  if (q1 > q1_max) 
    q1 = q1 - 2 * M_PI;
  

  // 根据需求，确定q2的取值范围
  if (q2 < q2_min) 
    q2 = q2_min;
  
  if (q2 > q2_max) 
    q2 = q2_max;
  

  // 根据需求，确定q3的取值范围
  if (q3 < q3_min) 
    q3 = q3_min;
  
  if (q3 > q3_max) 
    q3 = q3_max;
  

  // 根据需求，确定q4的取值范围
  if (q4 < q4_min) 
    q4 = q4_min;
  
  if (q4 > q4_max) 
    q4 = q4_max;
  

  // 根据需求，确定q5的取值范围
  if (q5 < q5_min) 
    q5 = q5_min;
  
  if (q5 > q5_max) 
    q5 = q5_max;
  

  // 根据需求，确定q6的取值范围
  if (q6 < q6_min) 
    q6 = q6_min;
  
  if (q6 > q6_max) 
    q6 = q6_max;
  

  theta << q1, q2, q3, q4, q5, q6;
  return theta;