grbl控制3轴机械臂 原理 实现 （一） 之2D机械臂模拟及实现

参考文章：https://blog.csdn.net/noahzuo/article/details/53908141

 

 

机械臂简介

机械臂是指高精度，多输入多输出、高度非线性、强耦合的复杂系统。因其独特的操作灵活性, 已在工业装配, 安全防爆等领域得到广泛应用。机械臂是一个复杂系统, 存在着参数摄动、外界干扰及未建模动态等不确定性。因而机械臂的建模模型也存在着不确定性，对于不同的任务, 需要规划机械臂关节空间的运动轨迹，从而级联构成末端位姿

这里我们主要讲解臂杆型机械臂，如图

 运动原理

这里我们先从最简单的讲起，二维的机械臂如下图，这里分为L1大臂和L2小臂

那么这二维的机械臂的末端位置由大臂的角度和小臂的角度，以及这两臂的长所决定。

我们先假设大臂靠墙的坐标是（0,0），有了原点，要算L2末端坐标，这里涉及到运动学。

运动学正解（Forward kinematics）

    通过各个关节的角度和臂长算出L2末端的坐标，这叫正解

运动学逆解（Inverse kinematics）

    通过L2末端的坐标，计算出各个关节的角度，这叫逆解

可以通过几何分析来进行FK和IK的解算（当然解算的方式有很多种，比如DH法等等，这里用简单的几何法讲解）

 

几何分析

运动学逆解（Inverse kinematics）

L2的末端有一个活动范围(reachable workspace)，当解算IK开始的时候，就需要先确保末端关节在这个活动范围内：

L2的末端的坐标位置确定之后，L1和L2臂对应的两个关节偏转角度和就可以通过几何方法确定了。

令L2的末端的最终坐标位置为(X,Y)，连接首关节和尾关节后，可以分析出最终解应该有两个，这两个解关于直线(0,0)到(X,Y)对称。

最终可以使用余弦定理来计算出最终的和，推导过程如下：

因此有：

余弦定理：

余弦定理：

根据余弦定理，将值代入公式得

 

 

 

根据余弦定理：

有：

 

这样得到了关节旋转角度和，注意的是cos函数是偶函数，因此需要考虑到±θ的情况。

 

运动学正解（Forward kinematics）

x1,x2,y1,y2分别是蓝色线段部分

由于x1和x2是平行的，所以L1和x1所成角度，和L1和x2所成角度相同，所以有

根据三角函数:

则

         代入上式得           

 

          代入上式得         

 

软件模拟实现

根据上面的推导的几何公式，写一个模拟的2d机械臂试试看,软件用的processing。

PVector P = new PVector(20,50);
//Version 1.1-fix coordinate

//坐标轴偏移坐标
float coordsetoffx,coordsetoffy;

//IK运算，大小臂夹角，平面投影直线夹角
float [] IKrst  = new float[3];

//大小臂长度
float bigL = 200,smallL = 200;

class arm
{
  //单臂建模为射线
  //位置，方向，长度，坐标偏移
  PVector position;
  PVector vector;
  float L;
  float coordsetoffx,coordsetoffy;
  
  arm(PVector a,float lengths)
  {
    vector = new PVector(random(-1,1),random(-1,1)).normalize();
    position = a;
    L = lengths;
    coordsetoffx = 0;
    coordsetoffy = 0;
  }
  
  arm(PVector a,float lengths,float cx,float cy)
  {
    vector = new PVector(random(-1,1),random(-1,1)).normalize();
    position = a;
    L = lengths;
    coordsetoffx = cx;
    coordsetoffy = cy;
    
  }
  
  //设置方向向量
  void setVector(PVector a)
  {
    vector = a.normalize();
  }
  
  //设置位置
  void setPosition(PVector a)
  {
    position = a;
  }
  
  //获取臂末端位置
  PVector getLPosition()
  { 

    return PVector.add(position,PVector.mult(vector,L));
    
  }
  //绘制臂
  void display()
  {
    PVector temp = PVector.mult(vector,L);
    line(coordsetoffx+position.x,coordsetoffy+position.y,coordsetoffx+position.x+temp.x,coordsetoffy+position.y+temp.y);

  }
  
  

}

//向量坐标变换,将y值翻转
PVector tranPvector(PVector a)
{
  //return new PVector(a.x,-a.y,a.z);
  return new PVector(a.x,-a.y);
}


//IK
float [] IK(float x,float y,float L1,float L2)
{
  //依次是平面直线夹角，大臂夹角，小臂夹角
  float ctaT;
  float cta1;
  float cta2;
  
  float [] ctasuzu  = new float[3];
  
  ctaT = acos(x/sqrt(x*x+y*y));
  //如果坐标0以下翻转角度加减
  if(y < 0)
  {
    ctaT = -ctaT;
  }
  
  cta1 = acos((L1*L1+x*x+y*y-L2*L2)/(2*L1*sqrt(x*x+y*y)))+ctaT;
  cta2 = acos((x*x+y*y-L1*L1-L2*L2)/(2*L1*L2));
  
  ctasuzu[0] = ctaT;
  ctasuzu[1] = cta1;
  ctasuzu[2] = cta2;
  
  return ctasuzu;
  
  
}




//声明arm类的2个变量
arm arm1,arm2,arm3;

void setup()
{
  size(700,700);
  //坐标偏移取窗口中心
  coordsetoffx = width/2;
  coordsetoffy = height/2;
  
  //大臂位置，长度，坐标偏移，
  arm1 = new arm(new PVector(0,0),bigL,coordsetoffx,coordsetoffy);
  //方向
  arm1.setVector(tranPvector(new PVector(0.5,1)));
  
  //将大臂的末端位置给小臂，臂长，坐标偏移
  arm2 = new arm(arm1.getLPosition(),smallL,coordsetoffx,coordsetoffy);

  //knift head
  arm3 = new arm(arm2.getLPosition(),20,coordsetoffx,coordsetoffy);

  background(255);
}

void draw()
{
    background(255);
  
    //映射鼠标到坐标系
    IKrst = IK(map(mouseX,0,width,-width/2,width/2),map(mouseY,height,0,-height/2,height/2),bigL,smallL);
    
    println("position: "+map(mouseX,0,width,-width/2,width/2)+" "+map(mouseY,height,0,-height/2,height/2));
    println("C: "+degrees(IKrst[0])+" "+degrees(IKrst[1])+" "+degrees(IKrst[2])+" ");
    //println("arm2:"+arm2.getLPosition().x,arm2.getLPosition().y);
    //println("arm1:"+arm1.getLPosition().x,arm1.getLPosition().y);
    
    
    float xx = cos(IKrst[1]);
    float yy = sin(IKrst[1]);
    
    float xx2 = cos((IKrst[1]-IKrst[2]));
    float yy2 = sin((IKrst[1]-IKrst[2]));
    
    
    arm1.setVector(tranPvector(new PVector(xx,yy)));
    
    arm2.setPosition(arm1.getLPosition());
    arm2.setVector(tranPvector(new PVector(xx2,yy2)));
  
    arm3.setPosition(arm2.getLPosition());
    arm3.setVector(tranPvector(new PVector(xx2,yy2)));
  
    stroke(0);
    arm1.display();
    arm2.display();
    stroke(255,0,0);
    arm3.display();
    
    noStroke();
    fill(0,128,0);
    ellipse(coordsetoffx+arm2.getLPosition().x,coordsetoffy+arm2.getLPosition().y,10,10);
    
    fill(255,0,0);
    ellipse(coordsetoffx+arm3.getLPosition().x,coordsetoffy+arm3.getLPosition().y,10,10);


  
}

void mousePressed()
{


}

实现效果

 

2维的机械臂运动学原理知道了，接下来讲讲3维的3轴机械臂的运动学正反解。

 

第二篇:grbl控制3轴机械臂 原理 实现 （二） 之3D机械臂模拟及实现