grbl控制3轴机械臂原理实现（三）之如何通过步进电机控制机械臂、插补算法

最新推荐文章于 2023-05-23 11:09:45 发布

ourkix

最新推荐文章于 2023-05-23 11:09:45 发布

阅读量1.1w

点赞数 22

分类专栏：硬件文章标签：图形学机械臂 GRBL 插补

本文链接：https://blog.csdn.net/ourkix/article/details/114148360

版权

硬件专栏收录该内容

13 篇文章 12 订阅

订阅专栏

参考：

图形学入门

https://www.zhihu.com/question/20330720

https://zhuanlan.zhihu.com/p/30553006

https://www.cnblogs.com/soroman/archive/2006/07/27/509602.html

https://blog.csdn.net/tlvc/article/details/4913397

https://blog.csdn.net/u010141928/article/details/79514514

往期回顾：

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

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

步进电机

这里机械臂上所用的步进电机是常用的42步进电机，每个电机控制一个关节，步进角度1.8度，全步模式下，200个脉冲走360度。这里我们在步进电机上面加了减速器，减速比是1:10，即转子真正走一圈是要2000个脉冲，每个脉冲是走0.18度。

前面我们用运动学逆解求出了机械臂的三个角度，那么我们让机械臂对应的电机转动所求得的角度就能得到机械臂的最终姿态了。由于是转动固定角度，要最终姿态保持和预料的一样的话，那么机械臂的初始位置就很重要，要确保机械臂的初始位置已知与准确。比如，大臂的初始位置处于30度，最终姿态是要跑到60度的位置，即步进电机要增加30度，如果初始位置不是处于30度的位置，那么增加30度后最终位置就不会是60度。所以我们在机械臂上电初始化时，就要有个变量保存三个角度的值，以及一个初始位置的校准，确保实际机械臂的角度和变量中的角度值是对应与准确的。

知道上面的知识后，那么要控制机械臂就简单了。说白了就是控制X,Y,Z这3个电机走相应的角度。这里与原版的笛卡尔坐标系控制电机就有点不一样了，在笛卡尔坐标系下，每个电机都对应一个轴，要走到对应的坐标点，分别给对应轴对应的值就行了。比如坐标系的目标点在(0,0,10),那么只要Z轴电机移动10就行了。

而机械臂要走空间坐标系的话就不是这种对应关系，更具逆解的值，要对应的操作3个电机移动不同的角度。所以我们这里要做一些相应的转换。将距离、角度、步数（步进电机要走的步数，即脉冲数）对应起来。

公式：此轴总共要走的脉冲数 = 角度 * 多少脉冲每1度

这里角度我们已经知道了，而多少脉冲每1度（根据电机驱动设置的细分值，和对应电机的步进角度，以及减速器决定）举例：如果电机的细分为全步，就是1细分，步进角度为1.8度，减速器为1:10。

那么一个脉冲所走度数 = 细分*步进角度*减速比

多少脉冲每1度 = (360度 / (细分*步进角度*减速比)) / 360度

代入即得 $\frac{(\frac{360 }{ 1*1.8*\frac{1}{10}})}{360}$ 得出值是5.5555555555....

由此得出每个轴要走的脉冲数，控制电机走对应的脉冲就行了。

关于所走的距离，可以通过运动学正解求出起始点与终点的位置来得到。

如果我们要让机械臂走出直线，即在空间坐标系中任意两点以直线轨迹移动，这里就要涉及到插补了。

插补

由于我们用的是步进电机，运动的时候是一步一步的，并不是平滑的，这里假设在一个平面上面，一个x步进电机，一个y步进电机。那么放大它们所走的轨迹就会是阶梯状的。而它们也只能沿着网格运动。相当于有了一个最小的分辨率，这个分辨率就是一步。如下图所示，如果x，y电机同时走就能走出45度的斜线。

这里所用的算法和图形学有点关系，我们的显示屏都是像素点，那么如何在这些像素网格上画出直线呢？

图形学的画线算法如何运用到，步进电机上呢？我们可以把一像素看成是步进的一步，这样像素每移动一格，对应步进电机就移动一步。

下面一一介绍

直线插补

数值微分DDA(Digital Differential Analyzer)算法

直线方程： $y = kx + b$

递增坐标x来求出y的值，假设知道了 $(x_i,y_i)$

$y_i = k * x_i +b$

那么 $({x_{i+1}} , {y_{i+1}})$ 为：

${y_{i+1}} = k * x_{i+1} +b$

$y_{i+1} = k * (x_i + 1) +b$

$y_{i+1} = k * x_i + b + k$

联合上面的 $y_i = k * x_i +b$ 得

$y_{i+1} = y_i + k$

也就是说每次纵坐标都是在前一个纵坐标的基础上加上斜率k的值得到的，这样运算中少了乘法，运算速度就加快了。

这里用processing编程演示一下

void settings()
{
  //设置显示屏幕窗口大小
  size(150,150);
}

//DDA画线算法
void DDADrawLine(int x0,int y0,int x1,int y1)
{
//算出斜率k值
float k = (y1-y0)*1.0/(x1-x0);
//起始点为y0
float y = y0;

//循环范围x0-x1，每次递增k值
for(int x = x0 ; x <= x1 ; ++ x) {
    //画下一个像素点，y值四舍五入
    setpixelsCT(x,(int)round(y));
    //y值递增k
    y += k;
  }
}


//由于processing的屏幕坐标是左上角为原点，x向右递增，y轴向下递增
//为了适配数学上的坐标轴，做了y轴调转
void setpixelsCT(int x,int y)
{
  loadPixels();
  pixels[((height-1)-y)*xx+x] = color(0);
  updatePixels();
}



void draw()
{
  //背景显示白色
  background(255);

  //鼠标映射，为了适配数学上的坐标轴，做了y轴调转
  DDADrawLine(0,0,mouseX,(int)map(mouseY,height-1,0,0,height-1));
  
  
  
}

程序演示：

Bresenham算法

在效率上，虽然DDA消除了浮点的乘法运算，但是存在浮点加法和取整运算。1965年Bresenham提出了更好的直线生成算法，成为了时至今日图形学领域使用最广泛的直线生成算法。该算法采用增量计算，借助一个误差量的符号确定下一个像素点的位置，该算法中不存在浮点数，只有整数运算，大大提高了运行效率。

Bresenham的思想是将像素中心构造成虚拟网格线，下图中每个红点即是一个像素。计算所画线条与垂直网格线的交点，算出与之相隔最近的像素坐标。

和DDA算法一样，每次x加1的时候纵方向就加了k，保存一个d值，如果d小于等于0.5，那么就画在下面的点，如果d大于0.5就画在上面的点。每次检查d是否在[0,1)范围内，如果不在此范围d减1。d值减一和y的像素坐标会上移一个单位，此时再次从这个新位置计算d值(如上面第二幅图)。

按这思想，无优化代码如下

void BresenhamDrawLine(int x0,int y0,int x1,int y1)
{

  //根据直线方程算出k值
  float k = (y1-y0)*1.0/(x1-x0);

  //初始化y值坐标值
  int y = y0;

  //初始化d值  
  float d = 0;


  //循环绘制像素点 从x0到x1
  for(int x = x0 ; x <= x1 ; ++ x) 
  {
    //如果d值大于0.5像素，表明上面的像素离直线比较近，采用直线上面的点，否则采用下面的点
    if(d > 0.5)
    {
      //绘制一个像素点,采用直线上面的点,将y值加1
      setpixelsCT(x,y+1);
    }
    else
    {
      //绘制一个像素点
      setpixelsCT(x,y);
    }
    
    //如果d值大于1个像素，将标志归零重置，d减一,y值加一像素
    if(d >= 1.0)
    {
      d -= 1.0;
      y++;
    }

    //当x每移动一像素，累加k值到d
    d += k;
    
  }
  
}

接下来优化下，去掉除法和小数运算

消除除法和小数,把d值偏移到0就只需要判断正负号，两边同时乘以2和dx（乘以2是为了去掉0.5小数，乘以dx是为了k值消除除法）

我们已知直线斜率是 $k = \frac{\Delta y}{\Delta x}$

而d的值是是由 $d = d + k$ ，由k值累加得到的，d值最终要与0.5来比较，得出是大于还是小于0.5，由此得出取直线上面的像素还是下面的像素。

所以我们先去掉除法，这里直线的起点 $(x,y)$ ,和终点 $(x_i,y_i)$ 都是整数。

我们直接在k值上乘以一个 $\Delta x$ ,消除k值的除数得到 $\Delta x * k = \Delta x * \frac{\Delta y}{\Delta x}$

又由于要消掉0.5这个小数，所以要乘以2。

那么k值现在为

$2 * \Delta x * k =2 * \Delta x * \frac{\Delta y}{\Delta x}$

$k =2 * \Delta y$

与d值比较的0.5 和 1.0在乘以 $2 * \Delta x$ 后得到 $\Delta x$ 和 $2 * \Delta x$

那么最终代码为：

void LiteBresenhamDrawLine(int x0,int y0,int x1,int y1)
{

  //根据直线方程算出k值
  int dx = abs(x0-x1);
  int dy = abs(y0-y1);

  //初始化y值坐标值
  int y = y0;



  //消除除法和小数,由于只需要判断正负号，两边同时乘以2和dx（乘以2是为了去掉0.5小数，乘以dx是为了k值消除除法）所以得出
  //k = dy/dx;
  //2*dx*k = 2*dx*dy/dx
  int k = 2*dy;

  //初始化d值,由于d值是k的累加，这里也要乘以2dx
  //d = 0
  //d = d - 0.5     
  //这样初始值就是-0.5，一旦d累加增k值大于0，等同于d>0.5,只是整体偏移了-0.5，这样就能看符号判断是否大于或小于0.5了
  //接下来去掉小数，乘以2dx，得到初始值,实际偏移了dx
  int d = -dx;


  //循环绘制像素点 从x0到x1
  for(int x = x0 ; x <= x1 ; ++ x) 
  {
    //如果d值大于0.5像素，表明上面的像素离直线比较近，采用直线上面的点，否则采用下面的点
    //由于都乘以了2dx，所以这里和它比较的0.5也要乘以2dx，成了2*dx*0.5=dx，实际偏移了dx所以为0
    if(d > 0)
    {
      //绘制一个像素点,采用直线上面的点,将y值加1
      setpixelsCT(x,y+1);
    }
    else
    {
      //绘制一个像素点
      setpixelsCT(x,y);
    }
    
    //如果d值大于1个像素，将标志归零重置，d减一,y值加一像素
    //由于都乘以了2dx，所以这里和它比较的1.0也要乘以2dx，成了2*dx*1.0=2dx，实际偏移了dx所以为dx
    if(d >= dx)
    {
      d -= 2*dx;
      y++;
    }

    //当x每移动一像素，累加k值到d
    d += k;
    
  }
  
}

以上示例代码都是基于k值范围在(0,1],要适用所有象限的话，只要在第一象限求出坐标后对应改变x，y值的符号后再绘制即可实现。

圆弧插补

grbl中的圆弧插补是采用直线逼近法，即采用弦高（ED）很小的弦线（直线AB）替代圆弧（圆弧AB）。

$OD=R-ED$ （R半径，ED弦高，已知值）

$AB = 2 * \sqrt{(R^2 - OD^2)}$

认为直线AB的长度等于圆弧AB的弧长，由弧长公式：弧长=弧长对应的圆心角*半径

可以计算出圆心角（OA与OB的夹角），A点坐标为已知，所以可以计算出B点坐标，然后使用直线插补AB线段，使用AB线段代替AB弧。

圆插补

中点画圆法，考虑圆心在原点，半径为R的圆在第一象限内的八分之一圆弧，从点（0, R）到点（R/ , R/ ）顺时针方向确定这段圆弧。假定某点Pi(xi, yi)已经是该圆弧上最接近实际圆弧的点，那么Pi的下一个点只可能是正右方的P1或右下方的P2两者之一

构造判别函数：

F(x, y）= x2 + y2 – R2

当F(x, y）= 0，表示点在圆上，当F(x, y）> 0，表示点在圆外，当F(x, y）< 0，表示点在圆内。如果M是P1和P2的中点，则M的坐标是（xi + 1, yi – 0.5），当F（xi + 1, yi – 0.5）< 0时，M点在圆内，说明P1点离实际圆弧更近，应该取P1作为圆的下一个点。同理分析，当F（xi + 1, yi – 0.5）> 0时，P2离实际圆弧更近，应取P2作为下一个点。当F（xi + 1, yi – 0.5）= 0时，P1和P2都可以作为圆的下一个点，算法约定取P2作为下一个点。

现在将M点坐标（xi + 1, yi – 0.5）带入判别函数F(x, y），得到判别式d：

d = F（xi + 1, yi – 0.5）= (xi + 1)2 + (yi – 0.5)2 – R2

若d < 0，则取P1为下一个点，此时P1的下一个点的判别式为：

d’ = F（xi + 2, yi – 0.5）= (xi + 2)2 + (yi – 0.5)2 – R2

展开后将d带入可得到判别式的递推关系：

d’ = d + 2xi + 3

若d > 0，则取P2为下一个点，此时P2的下一个点的判别式为：

d’ = F（xi + 2, yi – 1.5）= (xi + 2)2 + (yi – 1.5)2 – R2

展开后将d带入可得到判别式的递推关系：

d’ = d + 2(xi - yi) + 5

特别的，在第一个象限的第一个点（0, R）时，可以推倒出判别式d的初始值d0：

d0 = F(1, R – 0.5) = 1 – (R – 0.5)2 – R2 = 1.25 - R

根据上面的分析，可以写出中点画圆法的算法。考虑到圆心不在原点的情况，需要对计算出来的坐标进行了平移，下面就是通用的中点画圆法的processing源代码：

int xx,yy;

void settings()
{
  xx=250;
  yy=250;
  size(xx,yy);
}

void MP_Circle(int xc , int yc , int r)
 {
	int x, y;
	double d;
	x = 0;
	 y = r;
	d = 1.25 - r;
	CirclePlot(xc , yc , x , y);
	while(x < y)
	{
		if(d < 0)
		{
			d = d + 2 * x + 3;
		}
		else
		{
			d = d + 2 * ( x - y ) + 5;
			y--;
		}
		x++;
		CirclePlot(xc , yc , x , y);
	}
}

//coordnate transform 
void setpixelsCT(int x,int y)
{
  loadPixels();
  pixels[((height-1)-y)*xx+x] = color(0);
  updatePixels();
}

//整个圆绘制
void CirclePlot(int X,int Y,int P,int Q)
{
  setpixelsCT(X + P, Y + Q);
  setpixelsCT(X - P, Y + Q);
  setpixelsCT(X + P, Y - Q);
  setpixelsCT(X - P, Y - Q);
  setpixelsCT(X + Q, Y + P);
  setpixelsCT(X - Q, Y + P);
  setpixelsCT(X + Q, Y - P);
  setpixelsCT(X - Q, Y - P);
}


void draw()
{
  background(255);
  
  //画圆
  MP_Circle(125,125,50);
}

改进的中点画圆法－Bresenham算法

中点画圆法中，计算判别式d使用了浮点运算，影响了圆的生成效率。如果能将判别式规约到整数运算，则可以简化计算，提高效率。于是人们针对中点画圆法进行了多种改进，其中一种方式是将d的初始值由1.25 – R改成1 – R，考虑到圆的半径R总是大于2，因此这个修改不会响d的初始值的符号，同时可以避免浮点运算。还有一种方法是将d的计算放大两倍，同时将初始值改成3 – 2R，这样避免了浮点运算，乘二运算也可以用移位快速代替，采用3 – 2R为初始值的改进算法，又称为Bresenham算法：

int xx,yy;

void settings()
{
  xx=250;
  yy=250;
  size(xx,yy);
}

void Bresenham_Circle(int xc , int yc , int r)
{
     int x, y, d;

     x = 0;
     y = r;
     
     d = 3 - 2 * r;
     
     CirclePlot(xc , yc , x , y);
     while(x < y)
     {
         if(d < 0)
         {
             d = d + 4 * x + 6;
         }
        else
       {
           d = d + 4 * ( x - y ) + 10;
            y--;
         }
        x++;
        CirclePlot(xc , yc , x , y);
   }

}

//coordnate transform 
void setpixelsCT(int x,int y)
{
  loadPixels();
  pixels[((height-1)-y)*xx+x] = color(0);
  updatePixels();
}

//整个圆绘制
void CirclePlot(int X,int Y,int P,int Q)
{
  setpixelsCT(X + P, Y + Q);
  setpixelsCT(X - P, Y + Q);
  setpixelsCT(X + P, Y - Q);
  setpixelsCT(X - P, Y - Q);
  setpixelsCT(X + Q, Y + P);
  setpixelsCT(X - Q, Y + P);
  setpixelsCT(X + Q, Y - P);
  setpixelsCT(X - Q, Y - P);
}


void draw()
{
  background(255);
  
  //画圆
  Bresenham_Circle(125,125,50);
}

绘制效果：

通过以上的基础知识了解，终于可以进入正题了，下面讲讲GRBL源码如何修改得能控制机械臂

第四篇:grbl控制3轴机械臂原理实现（四）之GRBL源码修改驱动三轴机械臂

ourkix

关注

22
点赞
踩
181

收藏

觉得还不错? 一键收藏
2
评论
grbl控制3轴机械臂原理实现（三）之如何通过步进电机控制机械臂、插补算法

步进电机这里机械臂上所用的步进电机是常用的42步进电机，每个电机控制一个关节，步进角度1.8度，全步模式下，200个脉冲走360度。这里我们在步进电机上面加了减速器，减速比是1:10，即转子真正走一圈是要2000个脉冲，每个脉冲是走0.18度。前面我们用运动学逆解求出了机械臂的三个角度，那么我们让机械臂对应的电机转动所求得的角度就能得到机械臂的最终姿态了。由于是转动固定角度，要最终姿态保持和预料的一样的话，那么机械臂的初始位置就很重要，要确保机械臂的初始位置已知与准确。比如，大臂的初始位置处于3
复制链接

扫一扫