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

已于 2022-11-17 18:44:03 修改
阅读量1.9k 收藏 27
点赞数 4
分类专栏： 基于Qt与OpenGL的UR3机械臂仿真与控制 文章标签： c++ 算法
于 2022-11-15 16:04:06 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/weixin_43220219/article/details/127867646
版权

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 <Eigen/Dense>
#include <QWidget>
#include <iostream>


#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 <QDebug>

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<<"正运动学解:"<<T_<<std::endl;
}

//逆运动学
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:"<<angle11<<std::endl;
//    std::cout<<"angle12:"<<angle12<<std::endl;

    //求解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:"<<angle51<<std::endl;
//    std::cout<<"angle52:"<<angle52<<std::endl;
//    std::cout<<"angle53:"<<angle53<<std::endl;
//    std::cout<<"angle54:"<<angle54<<std::endl;

    //求解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:"<<fenzi1<<std::endl;
//    std::cout<<"fenmu1:"<<fenmu1<<std::endl;
    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:"<<fenzi2<<std::endl;
//    std::cout<<"fenmu2:"<<fenmu2<<std::endl;
    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:"<<angle61<<std::endl;
//    std::cout<<"angle62:"<<angle62<<std::endl;
//    std::cout<<"angle63:"<<angle63<<std::endl;
//    std::cout<<"angle64:"<<angle64<<std::endl;

    //求解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:"<<angle31<<std::endl;
//    std::cout<<"angle32:"<<angle32<<std::endl;
//    std::cout<<"angle33:"<<angle33<<std::endl;
//    std::cout<<"angle34:"<<angle34<<std::endl;
//    std::cout<<"angle35:"<<angle35<<std::endl;
//    std::cout<<"angle36:"<<angle36<<std::endl;
//    std::cout<<"angle37:"<<angle37<<std::endl;
//    std::cout<<"angle38:"<<angle38<<std::endl;

    //求解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:"<<angle21<<std::endl;
//    std::cout<<"angle22:"<<angle22<<std::endl;
//    std::cout<<"angle23:"<<angle23<<std::endl;
//    std::cout<<"angle24:"<<angle24<<std::endl;
//    std::cout<<"angle25:"<<angle25<<std::endl;
//    std::cout<<"angle26:"<<angle26<<std::endl;
//    std::cout<<"angle27:"<<angle27<<std::endl;
//    std::cout<<"angle28:"<<angle28<<std::endl;

    //求解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:"<<angle41<<std::endl;
//    std::cout<<"angle42:"<<angle42<<std::endl;
//    std::cout<<"angle43:"<<angle43<<std::endl;
//    std::cout<<"angle44:"<<angle44<<std::endl;
//    std::cout<<"angle45:"<<angle45<<std::endl;
//    std::cout<<"angle46:"<<angle46<<std::endl;
//    std::cout<<"angle47:"<<angle47<<std::endl;
//    std::cout<<"angle48:"<<angle48<<std::endl;
    //压入结果容器
    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:"<<fenzi2/fenmu2<<std::endl;

    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<<"第"<<i<<"组解正确"<<std::endl;
                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;
    }
}

完整代码

运动学代码

mai0026
关注
  • 4
    点赞
  • 踩
  • 27
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 2
    评论
专栏目录
UR机械臂运动学正解逆解完整代码C++版本
12-24
推导过程见我的文章 https://blog.csdn.net/sinat_32804425/article/details/122125911?spm=1001.2014.3001.5502，有详细公式说明，以及结果验证。可自己建立一个工程，下载配置好EIGEN库，就可以运行。
ur机械臂客户端反馈信息解析
04-12
ur机械臂客户端反馈信息解析，控制器始终通过控制器中的几个服务器插座提供表示机器人状态的数据，例如位置，温度等。从每个服务器套接字传输的数据可以彼此不同。可以编写自定义程序来读取这些流。完整说明（excel表）
ROS noetic与UR3真实机械臂通信
luojunqi2017的博客
04-27 3000
ROS noetic与UR3机械臂硬件进行通信ROS设置下载安装功能包功能快捷键合理的创建标题，有助于目录的生成如何改变文本的样式插入链接与图片如何插入一段漂亮的代码片生成一个适合你的列表创建一个表格设定内容居中、居左、居右SmartyPants创建一个自定义列表如何创建一个注脚注释也是必不可少的KaTeX数学公式新的甘特图功能，丰富你的文章UML 图表FLowchart流程图导出与导入导出导入 Ubuntu版本：20.04 ROS版本：noetic 机械臂：优傲机器人ur3，控制器cb3，软件版本3.14
c++运动学正反解 ros_ROS系统MoveIt玩转双臂机器人系列（六）–D-H逆运动学求解程序（C++）...
weixin_39586335的博客
12-19 454
注：本篇博文全部源码下载地址为：Git Repo。一、转换矩阵经过上一篇博客介绍，我们已经获得了Rob一个手臂的D-H参数表，如上表所示，我们要把这些参数转换成相邻坐标系的转换矩阵，D和H两位老前辈已经推导出通用公式了，通用公式如图1，其中cθi = cos(θi) ，sθi = sin(θi)。这是一个4×4的矩阵，它表征了相邻两个坐标系的位置和姿态两个维度的转换关系，具体说明见上一篇博文。...
UR3机器人运动学分析之逆运动学分析
G_tiro的博客
07-17 1284
UR3机器人逆运动学求解，解析解完整求解步骤
ur3机械臂，零件图和装配图
12-04
ur3机械臂，零件图和装配图。对需要机械臂模型的有很大的帮助。
UR机械臂逆运动学解析解
weixin_44986426的博客
04-27 4965
UR机械臂逆运动学解析解 ur10机械臂的DH（修正）参数为 d1=0.1273;a2=-0.612;a3=-0.5723;d4=0.163941;d5=0.1157;d6=0.6922; 首先，推导机械臂的正向运动学方程，根据DH参数可以计算机械臂的正向运动学方程表示为： 0T6=0T11T22T33T44T55T6=[nxoxaxpxnyoyaypynzozazpz0001]{}^0T_6={}^0T_1 {}^1T_2 {}^2T_3 {}^3T_4 {}^4T_5 {}^5T_6=\left[ \
二叉树深度的递归与非递归求法
weixin_45719829的博客
10-13 218
二叉树深度的递归与非递归求法递归求法非递归求法 递归求法 // 注意递归边界 int depth(node* root) { if(root == NULL) return 0; if(root->left == NULL && root->right == NULL) return 1; int ldepth = depth(root->left)+1; int rdepth = depth(root->ri
【机器人学】冗余七自由度机械臂的解析解逆解算法
热门推荐
vrijheid的博客
12-02 1万+
An Analytical Solution for a Redundant Manipulator with Seven Degrees of Freedom作者IntroductionManipulator ModelSelf-motion 作者 Takashi Nammoto and Kazuhiro Kosuge Introduction offset用来增加任务空间；冗余自由度用来缓解关节限制和奇异性。 本文的目的在于求出七自由度解析逆解 with an offset in the upper
UR机械臂正逆运动学求解
weixin_43956732的博客
06-17 7197
最近有个任务：求解UR机械臂正逆运动学，在网上参考了一下大家的求解办法，众说纷纭，其中有些朋友求解过程非常常规，但是最后求解的8组解，只有4组可用。在这里我介绍一个可以求解8组解析解的方法，供大家参考。        以UR5机械臂结构和尺寸参数为例进行正逆运动学求解，下图分别是UR5结构图和标准DH系参数：   1. 正运动学求解 正运动学是已知关节六个角度求变换矩阵T 其中： 整理得： 带入DH参数，求解：     &nbs
UR3机械臂图纸.zip，solidworks文件，2D+3D
03-27
UR3机械臂图纸.zip，solidworks文件，2D+3D
ur3-control:ur3机械手控制
03-30
ur3控制 来Qt中的minimizzare il codice（展开所有）：工具->选项->环境->键盘Nel campo过滤器：cerca“ UnFoldAll” Nel campo快捷方式设置为快捷方式。 “ Ctrl +ì”申请 快来使用utilizzare游戏杆所有内部代码：1）Controllare che il nodo游戏杆sia presente nel cmakelist del pacchetto。 2）roslaunch ur3_moveit_config demo.launch rosrun代码ERC rosrun代码操纵杆3）All'interno di ERC selezionaremodalità操纵杆。 奎尔·蓬托·达尔·加托操纵杆
ROS与UR3通信，实现moveit控制机械臂
09-20
ROS与UR3通信，实现moveit控制机械臂详细教程，包括如何安装功能包，然后配置连接，算是初步入门
6自由度机器人运动学正反解C++程序
07-26
6自由度机器人运动学正、反解C++程序，可直接运行，简单易懂！
UR3机器人编程教程
10-15
机器人编程资料参考 使用说明等！
UR3-6自由度机械臂正逆运动学Python实现代码
03-07
Python实现代码UR3-6自由度机械臂正逆运动学，函数封装成类，结构简单，可直接运行 正运动学：标准DH参数法 逆运动学：解析法
C++实现UR5机械臂正运动学代码
12-24
UR5机械臂正运动学完整c++代码，说明详见文章https://blog.csdn.net/sinat_32804425/article/details/122125911?spm=1001.2014.3001.5502
UR.rar_UR 机器人_UR 运动学_five axis_运动学逆解_逆解
07-13
该文件包括UR五轴机器人运动学控制正逆解的Matlab程序
UR3六自由度机械臂.STEP
06-23
UR3UR3六自由度机械臂
ur5机械臂运动学逆解matlab
最新发布
01-10
根据提供的引用内容，可以得知有关UR5机械臂运动学逆解的matlab仿真的相关信息。具体来说，可以使用基于POE公式的UR5机械臂逆运动学建模求解与matlab仿真方法来实现UR5机械臂的运动学逆解。 以下是基于POE公式的UR5机械臂逆运动学建模求解与matlab仿真的步骤： 1. 理论基础： - 一阶运动学：用于描述机械臂的位置和姿态。 - 牛顿-拉夫森数值迭代算法：用于求解非线性方程组，可以用于求解机械臂的逆运动学问题。 2. 基于POE公式的UR5机械臂逆运动学求解： - 使用POE公式建立机械臂的运动学模型。 - 根据机械臂的末端位置和姿态，利用牛顿-拉夫森数值迭代算法求解机械臂的关节角度。 3. Matlab仿真验证： - 在Matlab环境中编写程序，实现基于POE公式的UR5机械臂逆运动学求解。 - 输入机械臂的末端位置和姿态，输出机械臂的关节角度。 - 运行程序进行仿真验证，检查逆运动学求解的准确性和稳定性。 通过以上步骤，可以实现UR5机械臂的运动学逆解，并在Matlab环境中进行仿真验证。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

mai0026

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值