UR3机械臂运动学反解之解析解

UR3机械臂运动学反解之解析解

基于Qt+OpenGL的UR3机械臂仿真与控制系统（四）

反解演示

上面三个图片分别是UR3机械臂的逆运动学演示图片

先给定UR3机器人末端固定位置（x=250，y=0，z=450） 与固定姿态（先沿y轴转90°再沿z轴转90°），求解机械臂反解(多解)，再将机械臂设置为其中三个解的位置的情况。由此验证反解的正确性。

解析解推导

建立的DH参数如上图

C++解析解代码

ur3_controller.h

#ifndef UR3_CONTROLLER_H
#define UR3_CONTROLLER_H

#include 
#include 
#include 


#define PI acos(-1)


class UR3_CONTROLLER{
public:
  std::vector<double> thetax_;
  std::vector<double> theta_record_;  //用了记录上一组角度，做动画插值
  std::vector<double> dx_;
  std::vector<double> ax_;
  std::vector<double> alphax_;
  Eigen::Matrix4d T_;
  std::vector<std::vector<double>> result_;
  double nx;double ox;double ax;double px;
  double ny;double oy;double ay;double py;
  double nz;double oz;double az;double pz;
  void initDH();
  void initDH(std::vector<float> a,
              std::vector<float> d,
              std::vector<float> alpha);
  void printfDH();
  void setTheta(double theta1,double theta2,
                double theta3,double theta4,
                double theta5,double theta6);
  void setTheta(std::vector<double> thetas);
  void setAngle(std::vector<float> angles);
  void setT(Eigen::Matrix4d T);
  Eigen::Matrix4d getTx(double a,double alpha,double d,double theta);
  void positiveKinematics();  //正运动学
  Eigen::Matrix4d positiveKinematicsMat(std::vector<double> angle);
  std::vector<double> getTheta3(double theta1,double theta5,double theta6);
  double getTheta2(double theta1,double theta3,double theta6);
  double getTheta4(double theta1,double theta2,double theta3,double theta6);
  void negitiveKinematics();  //逆运动学
  void filterSlove(bool positive);  //挑选正确的解
};
#endif // UR3_CONTROLLER_H

ur3_controller.cpp

#include "./ur3_controller.h"
#include 

void UR3_CONTROLLER::initDH(){
    //初始化DH参数列表
    dx_.push_back(151.9);
    dx_.push_back(0);
    dx_.push_back(0);
    dx_.push_back(110.4);
    dx_.push_back(83.4);
    dx_.push_back(81.4);
    ax_.push_back(0);
    ax_.push_back(0);
    ax_.push_back(243.65);
    ax_.push_back(213);
    ax_.push_back(0);
    ax_.push_back(0);
    alphax_.push_back(0);
    alphax_.push_back(PI/2);
    alphax_.push_back(0);
    alphax_.push_back(0);
    alphax_.push_back(PI/2);
    alphax_.push_back(-PI/2);
    printfDH();
}

void UR3_CONTROLLER::initDH(std::vector<float> a,
                            std::vector<float> d,
                            std::vector<float> alpha){
    //初始化DH参数列表
    for (int i=0;i<a.size();i++){
        ax_.push_back(a[i]*1000);
        dx_.push_back(d[i]*1000);
        alphax_.push_back(alpha[i]*PI/180);
    }
    printfDH();
}



void UR3_CONTROLLER::printfDH(){
    std::cout<<std::endl;
    std::cout<<"-------DH参数列表加载成功-------"<<std::endl;
    for(int i=0;i<ax_.size();i++){
        std::cout<<"d"<<i<<":"<<dx_[i]<<"  a"<<i<<":"<<ax_[i]
                 <<"  alpha"<<i<<":"<<alphax_[i];
        if (i != ax_.size()-1)
            std::cout<<std::endl;
    }
    std::cout<<std::endl;
    std::cout<<"------------------------------"<<std::endl;
    std::cout<<std::endl;
}


void UR3_CONTROLLER::setTheta(double theta1,double theta2,
                              double theta3,double theta4,
                              double theta5,double theta6){
    if (thetax_.size()!=6){
        thetax_.clear();
        thetax_.push_back(theta1);
        thetax_.push_back(theta2);
        thetax_.push_back(theta3);
        thetax_.push_back(theta4);
        thetax_.push_back(theta5);
        thetax_.push_back(theta6);
    }
    else{
        thetax_[0] = theta1;
        thetax_[1] = theta2;
        thetax_[2] = theta3;
        thetax_[3] = theta4;
        thetax_[4] = theta5;
        thetax_[5] = theta6;
    }
}


void UR3_CONTROLLER::setTheta(std::vector<double> thetas){
    thetax_.clear();
    for (int i=0;i<thetas.size();i++)
        thetax_.push_back(thetas[i]);
}


void UR3_CONTROLLER::setAngle(std::vector<float> angles){
    thetax_.clear();
    for (int i=0;i<angles.size();i++)
        thetax_.push_back(angles[i]*PI/180);
}


Eigen::Matrix4d UR3_CONTROLLER::getTx(double a,double alpha,double d,double theta){
    Eigen::Matrix4d Tx;
    double T11 = cos(theta);
    double T12 = -1*sin(theta);
    double T14 = a;
    double T21 = sin(theta)*cos(alpha);
    double T22 = cos(theta)*cos(alpha);
    double T23 = -1*sin(alpha);
    double T24 = -1*d*sin(alpha);
    double T31 = sin(theta)*sin(alpha);
    double T32 = cos(theta)*sin(alpha);
    double T33 = cos(alpha);
    double T34 = d*cos(alpha);
    Tx << T11  ,T12  ,0    ,T14  ,
          T21  ,T22  ,T23  ,T24  ,
          T31  ,T32  ,T33  ,T34  ,
            0  ,  0  ,  0  ,  1;
    Eigen::Matrix4d mat1;
    mat1<<1,0,0,a,
          0,cos(alpha),-1*sin(alpha),0,
          0,sin(alpha),cos(alpha),0,
          0,0,0,1;
    Eigen::Matrix4d mat2;
    mat2<<cos(theta),-1*sin(theta),0,0,
              sin(theta),cos(theta),0,0,
              0,0,1,d,
              0,0,0,1;
    return mat1*mat2;
}

void UR3_CONTROLLER::setT(Eigen::Matrix4d T){
    T_ = T;
    nx = T_(0,0);ox = T_(0,1);ax = T_(0,2);px = T_(0,3);
    ny = T_(1,0);oy = T_(1,1);ay = T_(1,2);py = T_(1,3);
    nz = T_(2,0);oz = T_(2,1);az = T_(2,2);pz = T_(2,3);
}

//正运动学
void UR3_CONTROLLER::positiveKinematics(){
    Eigen::Matrix4d T10 = getTx(ax_[0],alphax_[0],dx_[0],thetax_[0]);
    Eigen::Matrix4d T21 = getTx(ax_[1],alphax_[1],dx_[1],thetax_[1]);
    Eigen::Matrix4d T32 = getTx(ax_[2],alphax_[2],dx_[2],thetax_[2]);
    Eigen::Matrix4d T43 = getTx(ax_[3],alphax_[3],dx_[3],thetax_[3]);
    Eigen::Matrix4d T54 = getTx(ax_[4],alphax_[4],dx_[4],thetax_[4]);
    Eigen::Matrix4d T65 = getTx(ax_[5],alphax_[5],dx_[5],thetax_[5]);
    T_ = T10*T21*T32*T43*T54*T65;
    nx = T_(0,0);ox = T_(0,1);ax = T_(0,2);px = T_(0,3);
    ny = T_(1,0);oy = T_(1,1);ay = T_(1,2);py = T_(1,3);
    nz = T_(2,0);oz = T_(2,1);az = T_(2,2);pz = T_(2,3);
    //std::cout<<"正运动学解:"<
}

//逆运动学
void UR3_CONTROLLER::negitiveKinematics(){
    result_.clear();
    //求解theta1
    double m = py-dx_[5]*ay;
    double n = dx_[5]*ax-px;
    double phi = atan2(m,n);
    double theta11 = std::atan2(-1*dx_[3],sqrt(m*m+n*n-dx_[3]*dx_[3]))-phi;
    double theta12 = std::atan2(-1*dx_[3],-1*sqrt(m*m+n*n-dx_[3]*dx_[3]))-phi;
    if (theta11<-1*PI){
        theta11 = theta11+2*PI;
    }
    else if(theta11>PI){
        theta11 = theta11-2*PI;
    }
    if (theta12<-1*PI){
        theta12 = theta12+2*PI;
    }
    else if (theta12>PI){
        theta12 = theta12-2*PI;
    }
    double angle11 = theta11*180/PI;
    double angle12 = theta12*180/PI;
//    std::cout<<"angle11:"<
//    std::cout<<"angle12:"<

    //求解theta5,一个theta1对应两个theta5
    double theta51 = acos(sin(theta11)*ax-cos(theta11)*ay);
    double theta52 = -1*acos(sin(theta11)*ax-cos(theta11)*ay);
    double theta53 = acos(sin(theta12)*ax-cos(theta12)*ay);
    double theta54 = -1*acos(sin(theta12)*ax-cos(theta12)*ay);
    double angle51 = theta51*180/PI;
    double angle52 = theta52*180/PI;
    double angle53 = theta53*180/PI;
    double angle54 = theta54*180/PI;
//    std::cout<<"angle51:"<
//    std::cout<<"angle52:"<
//    std::cout<<"angle53:"<
//    std::cout<<"angle54:"<

    //求解theta6,一个theta1对应一个theta6,由atan()计算所得的角都应该注意值域的问题
    double theta61,theta62,theta63,theta64;

    double fenzi1 = (-1*sin(theta11)*ox+cos(theta11)*oy)/sin(theta51);
    double fenmu1 = (sin(theta11)*nx-cos(theta11)*ny)/(sin(theta51));
//    std::cout<<"fenzi1:"<
//    std::cout<<"fenmu1:"<
    theta61 = std::atan2(fenzi1,fenmu1);

    double fenzi2 = (-1*sin(theta11)*ox+cos(theta11)*oy)/sin(theta52);
    double fenmu2 = (sin(theta11)*nx-cos(theta11)*ny)/(sin(theta52));
//    std::cout<<"fenzi2:"<
//    std::cout<<"fenmu2:"<
    theta62 = std::atan2(fenzi2,fenmu2);

    double fenzi3 = (-1*sin(theta12)*ox+cos(theta12)*oy)/sin(theta53);
    double fenmu3 = (sin(theta12)*nx-cos(theta12)*ny)/(sin(theta53));
    theta63 = std::atan2(fenzi3,fenmu3);

    double fenzi4 = (-1*sin(theta12)*ox+cos(theta12)*oy)/sin(theta54);
    double fenmu4 = (sin(theta12)*nx-cos(theta12)*ny)/(sin(theta54));
    theta64 = std::atan2(fenzi4,fenmu4);

    double angle61 = theta61*180/PI;
    double angle62 = theta62*180/PI;
    double angle63 = theta63*180/PI;
    double angle64 = theta64*180/PI;
//    std::cout<<"angle61:"<
//    std::cout<<"angle62:"<
//    std::cout<<"angle63:"<
//    std::cout<<"angle64:"<

    //求解theta3
    std::vector<double> theta3;  //中间变量暂存返回结果
    theta3 = getTheta3(theta11,theta51,theta61);
    double theta31 = theta3[0];
    double theta32 = theta3[1];
    double angle31 = theta31*180/PI;
    double angle32 = theta32*180/PI;
    theta3 = getTheta3(theta11,theta52,theta62);
    double theta33 = theta3[0];
    double theta34 = theta3[1];
    double angle33 = theta33*180/PI;
    double angle34 = theta34*180/PI;
    theta3 = getTheta3(theta12,theta53,theta63);
    double theta35 = theta3[0];
    double theta36 = theta3[1];
    double angle35 = theta35*180/PI;
    double angle36 = theta36*180/PI;
    theta3 = getTheta3(theta12,theta54,theta64);
    double theta37 = theta3[0];
    double theta38 = theta3[1];
    double angle37 = theta37*180/PI;
    double angle38 = theta38*180/PI;
//    std::cout<<"angle31:"<
//    std::cout<<"angle32:"<
//    std::cout<<"angle33:"<
//    std::cout<<"angle34:"<
//    std::cout<<"angle35:"<
//    std::cout<<"angle36:"<
//    std::cout<<"angle37:"<
//    std::cout<<"angle38:"<

    //求解theta2
    double theta21 = getTheta2(theta11,theta31,theta61);
    double theta22 = getTheta2(theta11,theta32,theta61);
    double theta23 = getTheta2(theta11,theta33,theta62);
    double theta24 = getTheta2(theta11,theta34,theta62);
    double theta25 = getTheta2(theta12,theta35,theta63);
    double theta26 = getTheta2(theta12,theta36,theta63);
    double theta27 = getTheta2(theta12,theta37,theta64);
    double theta28 = getTheta2(theta12,theta38,theta64);
    double angle21 = theta21*180/PI;
    double angle22 = theta22*180/PI;
    double angle23 = theta23*180/PI;
    double angle24 = theta24*180/PI;
    double angle25 = theta25*180/PI;
    double angle26 = theta26*180/PI;
    double angle27 = theta27*180/PI;
    double angle28 = theta28*180/PI;
//    std::cout<<"angle21:"<
//    std::cout<<"angle22:"<
//    std::cout<<"angle23:"<
//    std::cout<<"angle24:"<
//    std::cout<<"angle25:"<
//    std::cout<<"angle26:"<
//    std::cout<<"angle27:"<
//    std::cout<<"angle28:"<

    //求解theta4
    double theta41 = getTheta4(theta11,theta21,theta31,theta61);
    double theta42 = getTheta4(theta11,theta22,theta32,theta61);
    double theta43 = getTheta4(theta11,theta23,theta33,theta62);
    double theta44 = getTheta4(theta11,theta24,theta34,theta62);
    double theta45 = getTheta4(theta12,theta25,theta35,theta63);
    double theta46 = getTheta4(theta12,theta26,theta36,theta63);
    double theta47 = getTheta4(theta12,theta27,theta37,theta64);
    double theta48 = getTheta4(theta12,theta28,theta38,theta64);
    double angle41 = theta41*180/PI;
    double angle42 = theta42*180/PI;
    double angle43 = theta43*180/PI;
    double angle44 = theta44*180/PI;
    double angle45 = theta45*180/PI;
    double angle46 = theta46*180/PI;
    double angle47 = theta47*180/PI;
    double angle48 = theta48*180/PI;
//    std::cout<<"angle41:"<
//    std::cout<<"angle42:"<
//    std::cout<<"angle43:"<
//    std::cout<<"angle44:"<
//    std::cout<<"angle45:"<
//    std::cout<<"angle46:"<
//    std::cout<<"angle47:"<
//    std::cout<<"angle48:"<
    //压入结果容器
    result_.clear();
    std::vector<double> middle_result;
    middle_result.push_back(angle11);middle_result.push_back(angle21);middle_result.push_back(angle31);
    middle_result.push_back(angle41);middle_result.push_back(angle51);middle_result.push_back(angle61);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle11);middle_result.push_back(angle22);middle_result.push_back(angle32);
    middle_result.push_back(angle42);middle_result.push_back(angle51);middle_result.push_back(angle61);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle11);middle_result.push_back(angle23);middle_result.push_back(angle33);
    middle_result.push_back(angle43);middle_result.push_back(angle52);middle_result.push_back(angle62);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle11);middle_result.push_back(angle24);middle_result.push_back(angle34);
    middle_result.push_back(angle44);middle_result.push_back(angle52);middle_result.push_back(angle62);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle12);middle_result.push_back(angle25);middle_result.push_back(angle35);
    middle_result.push_back(angle45);middle_result.push_back(angle53);middle_result.push_back(angle63);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle12);middle_result.push_back(angle26);middle_result.push_back(angle36);
    middle_result.push_back(angle46);middle_result.push_back(angle53);middle_result.push_back(angle63);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle12);middle_result.push_back(angle27);middle_result.push_back(angle37);
    middle_result.push_back(angle47);middle_result.push_back(angle54);middle_result.push_back(angle64);
    result_.push_back(middle_result);
    middle_result.clear();

    middle_result.push_back(angle12);middle_result.push_back(angle28);middle_result.push_back(angle38);
    middle_result.push_back(angle48);middle_result.push_back(angle54);middle_result.push_back(angle64);
    result_.push_back(middle_result);
    middle_result.clear();

    //输出结果(可能含有不正确的解)
    for (int i=0;i<result_.size();i++){
        std::cout<<"第"<<i<<"组解:";
        for (int j=0;j<result_[i].size();j++){
            std::cout<<"   "<<result_[i][j];
        }
        std::cout<<std::endl;
    }
    std::cout<<std::endl;
}


//求解theta3
std::vector<double> UR3_CONTROLLER::getTheta3(double theta1,double theta5,double theta6){
    std::vector<double> theta3;  // 存放运算结果
    double a = dx_[4]*(sin(theta6)*(cos(theta1)*nx+sin(theta1)*ny)+cos(theta6)*(cos(theta1)*ox+sin(theta1)*oy))-dx_[5]*(cos(theta1)*ax+sin(theta1)*ay)+cos(theta1)*px+sin(theta1)*py;
    double b = dx_[4]*(sin(theta6)*nz+cos(theta6)*oz)-dx_[5]*az+pz-dx_[0];

    double fenzi = (a*a+b*b-ax_[3]*ax_[3]-ax_[2]*ax_[2]);
    double fenmu = (2*ax_[2]*ax_[3]);

    //std::cout<<"fenzi2:"<

    double theta31 = acos(fenzi/fenmu);
    double theta32 = -1*acos(fenzi/fenmu);
    theta3.clear();
    theta3.push_back(theta31);
    theta3.push_back(theta32);
    return theta3;
}


//求解theta2
double UR3_CONTROLLER::getTheta2(double theta1,double theta3,double theta6){
    double a = dx_[4]*(sin(theta6)*(cos(theta1)*nx+sin(theta1)*ny)+cos(theta6)*(cos(theta1)*ox+sin(theta1)*oy))-dx_[5]*(cos(theta1)*ax+sin(theta1)*ay)+cos(theta1)*px+sin(theta1)*py;
    double b = dx_[4]*(sin(theta6)*nz+cos(theta6)*oz)-dx_[5]*az+pz-dx_[0];
    double fenzi = (b*(ax_[3]*cos(theta3)+ax_[2])-ax_[3]*sin(theta3)*a)/(ax_[2]*ax_[2]+ax_[3]*ax_[3]+2*ax_[2]*ax_[3]*cos(theta3));
    double fenmu = (a*(ax_[3]*cos(theta3)+ax_[2])+ax_[3]*sin(theta3)*b)/(ax_[2]*ax_[2]+ax_[3]*ax_[3]+2*ax_[2]*ax_[3]*cos(theta3));
    double theta2 = std::atan2(fenzi,fenmu);
    return theta2;
}


//求解theta4
double UR3_CONTROLLER::getTheta4(double theta1,double theta2,double theta3,double theta6){
    double nx = T_(0,0);double ny = T_(1,0);double nz = T_(2,0);
    double ox = T_(0,1);double oy = T_(1,1);double oz = T_(2,1);
    double fenzi = -1*sin(theta6)*(cos(theta1)*nx+sin(theta1)*ny)-cos(theta6)*(cos(theta1)*ox+sin(theta1)*oy);
    double fenmu = nz*sin(theta6)+oz*cos(theta6);
    //因为atan(x)所求的结果落在-PI/2到PI/2上,theta2+theta3+theta4显然不一定落在此区域上
    double theta234;
    if ((fenzi>0 && fenmu<0)){
        theta234 = PI+atan(fenzi/fenmu);
    }
    else if (fenzi<0 && fenmu<0){
        theta234 = atan(fenzi/fenmu)-PI;
    }
    else{
        theta234 = atan(fenzi/fenmu);
    }
    //因为theta2+theta3+theta4的取值范围可以到[-540,540] (该范围可以在后期传入范围参数确定)
    double theta4;
    //尝试覆盖[-540,540]所有theta2+theta3+theta4的可能性
    if (2*PI+theta234-theta2-theta3<PI && 2*PI+theta234>-1*PI){
        theta4 = theta234+2*PI-theta2-theta3;
    }
    else if (-2*PI+theta234-theta2-theta3<PI && -2*PI+theta234>-1*PI){
        theta4 = theta234-2*PI-theta2-theta3;
    }
    else
        theta4 = theta234-theta2-theta3;
    return theta4;
}

Eigen::Matrix4d UR3_CONTROLLER::positiveKinematicsMat(std::vector<double> angle){
    double theta1 = angle[0]*PI/180;
    double theta2 = angle[1]*PI/180;
    double theta3 = angle[2]*PI/180;
    double theta4 = angle[3]*PI/180;
    double theta5 = angle[4]*PI/180;
    double theta6 = angle[5]*PI/180;
    Eigen::Matrix4d T10 = getTx(ax_[0],alphax_[0],dx_[0],theta1);
    Eigen::Matrix4d T21 = getTx(ax_[1],alphax_[1],dx_[1],theta2);
    Eigen::Matrix4d T32 = getTx(ax_[2],alphax_[2],dx_[2],theta3);
    Eigen::Matrix4d T43 = getTx(ax_[3],alphax_[3],dx_[3],theta4);
    Eigen::Matrix4d T54 = getTx(ax_[4],alphax_[4],dx_[4],theta5);
    Eigen::Matrix4d T65 = getTx(ax_[5],alphax_[5],dx_[5],theta6);
    return T10*T21*T32*T43*T54*T65;
}


void UR3_CONTROLLER::filterSlove(bool positive){
    if (positive == false){//如果是逆运动学
        std::vector<std::vector<double>> right_result;
        for (int i=0;i<result_.size();i++){
            bool status = true;
            for (int j=0;j<result_[i].size();j++){  //检查每个元素的取值
                if (status == false)
                    break;
                if (result_[i][j]<180&&result_[i][j]>-1*180){
                }
                else{
                    status = false;
                }
            }
            if (status == true){
                right_result.push_back(result_[i]);
            }
        }
        result_.clear();
        result_ = right_result;
    }
    else{  //如果是正运动学
        std::vector<std::vector<double>> right_result;
        std::cout<<"输出矩阵查看问题:"<<T_<<std::endl;
        for (int i=0;i<result_.size();i++){
            Eigen::Matrix4d T = positiveKinematicsMat(result_[i]);
            std::cout<<"对比矩阵:"<<T<<std::endl;
            //计算两矩阵偏差
            double dv = pow(T(0,0)-T_(0,0),2)+pow(T(0,1)-T_(0,1),2)+pow(T(0,2)-T_(0,2),2)+pow(T(0,3)-T_(0,3),2)
                       +pow(T(1,0)-T_(1,0),2)+pow(T(1,1)-T_(1,1),2)+pow(T(1,2)-T_(1,2),2)+pow(T(1,3)-T_(1,3),2)
                       +pow(T(2,0)-T_(2,0),2)+pow(T(2,1)-T_(2,1),2)+pow(T(2,2)-T_(2,2),2)+pow(T(2,3)-T_(2,3),2);
            if (dv<0.1){
//                std::cout<<"第"<
                right_result.push_back(result_[i]);
            }
        }
        result_.clear();
        result_ = right_result;
    }
    //输出正确结果
    for (int i=0;i<result_.size();i++){
        std::cout<<"第"<<i<<"组解:";
        for (int j=0;j<result_[i].size();j++){
            std::cout<<"   "<<result_[i][j];
        }
        std::cout<<std::endl;
    }
}

完整代码

运动学代码