lesson 3: 代码实现—整体架构

一、整体说明

Diego 3dof的代码是移植的开源3D打印机项目Marlin的源代码,读者感兴趣可以访问Marlin项目github获取其源代码,Marlin项目由于要考虑适配不同的控制器,所以代码比较复杂,这里对其做了大幅的修改删减,已符合Diego 3dof的控制要求,修改后的文件结构如下图:

修改后一共剩余12个文件,且可以在Arudion UNO上运行,资源占用情况如下所示:

资源占用情况比较理想,如果资源占用情况比较高的情况下Arduino IDE会预警,可能在运行中有不可预知的不稳定现象。

同时由于diego3轴码垛和3D打印机的逆解不同,也做了相应的修改,后面会详细说明。

二、文件功能说明

1. Configuration.h 和Configuration_adv.h

这两个头文件中主要是针对运动学逆解,及马达控制的一些默认配置。

2. pin.h

这个头文件中,定义了Arduino的pin功能的对应关系

3. diego.h

这个头文件中定义了通用的函数及宏定义

4. planner.h 和 planner.cpp

这两个文件中主要是针对速度规划的函数、数据结构及运动学逆解运算。

5. stepper.h 和 stepper.cpp

这两个文件中主要是针对马达的控制算法

6.motion_control.h 和 motion_control.cpp

这两个文件中定义了运动规划的相关函数及数据结构

7. speed_lookupdate.h

这个文件中定义了在算法中用到的速度查询数组

8. diego_3dof.ino

arduino项目的主文件,功能包含,对串口命令的接收、g代码的解析、代码的初始化和主循环。

Marlin的代码在网上有非常多的资源可以参考,在Diego-3dof中主要修改了其配置文件中Pin的定义,及码垛的逆解算法,下一章中将介绍逆解算法的实现。
源代码请见https://github.com/diegorobot/diego-3dof
 

 

Lesson 4: 3轴码垛逆解模型及实现

对于3轴码垛机械臂控制最基本的是对其建立运动学模型,而对于3轴码垛类型机械臂来说运动学模型,其本质就是给定空间3D坐标,求解3个轴的旋转角度。

 

如上图所示,左侧为实物坐标,右侧图为抽象到坐标系的几何表示,逆解过程就是知道末端坐标,而求解各个轴的旋转角度,进而转换为步进电机的步进数。上图中涉及到5个点:

O点:坐标原点,也就是机械臂底盘原点

A点:是两个电机安装位置轴心点,此点的Z轴坐标为已知的固定量

B点:两个活动臂的连接点

C点:臂末端点

D点:末端工具安装点

E点:末端工具的工作点,逆解就是知道这点的坐标,求3个马达的选择角度

下面我们利用立体几何,和解析几何知识来进行你运算分析。

一,假设条件

  • 坐标系采用右手坐标系,如上图所示
  • 机械臂的底座位于右手坐标系的XY平面
  • 底座旋转轴的位置,即为坐标系的原点位置,即上图做所视的O点
  • 我们把AB线所表示臂称之为大臂,BC线所表示的臂称之为小臂
  • 点O,A,B,C,D,E始终处于同一平面,此平面与Y轴的夹角为α
  • OA线与AB延长线之间的夹角为β
  • AB线与CB线延长线之间的夹角为γ
  • 工具安装位置CD始终与XY平面平行
  • 工具安装点与末端工作点的直线DE始终垂直于XY平面
  • 机械臂的初始位置为A,B,C三点的Y轴坐标为O,B,C点收回到距离Z轴最近的位置
  • 初始位置α=0
  • 已知条件:OA,AB,BC,CD,DE长度
  • OA与XY平面之间的夹角θ为固定值

二、数学求解

已知E点的坐标(x,y,z)基, 于以上假设条件,求解α,β,γ。

1.求解α

c点坐标为(cx,cy,cz),与E点的坐标是简单的直角坐标系对应关系

α为整个机械臂投影到XY轴平面的投影线与X轴的角度,可以直接根据E点的坐标使用正切函数计算出α

tan(α)=y/x

α=atan(y/x)

由于在机械臂的运动范围内x坐标可能为0,所以需要对x=0的坐标点做特殊的处理,详见代码

2.求解γ

γ是三角形ABC,角ABC沿AB延长线的补角,所以只要求出角ABC,即可求出γ。角ABC只需要采用三角函数即可以求得,所以求解步骤为:

2.1 求A点的坐标(ax,ay,az)

ax=OA*cos(θ)*sin(α)

ay=OA*cos(θ)*cos(α)

az=OA*sin(θ)

2.2求解三角形AC边长

AC=sqrt((cx-ax)(cx-ax)+(cy-ay)(cy-ay)+(cz-az)(cz-az))

2.3求解γ

γ=180-acos((AB*AB+ BC*BC- AC*AC) / (2 * AB* BC))

3.求解β

三角形ABC的的BAC角是可以根据三角函数求出来,γ在OA的斜率大于AC的斜率时γ等于角BAC减去直线OA和AC的夹角

3.1.求解角ABC

ABC=acos((AB*AB+ AC*AC- BC*BC) / (2 * AB* AC))

3.2.求解OA的斜率

Koa=sin(θ) / cos(θ);

3.3.求解AC的斜率

Kac= (c_y – a_y) / (c_x – a_x);

3.4.求解OA和AC的夹角

OA_AC=atan(|Koa-Kac|/(1-Koa*Kac))

3.5.求解β

如果Koa>Kac ,β=ABC-OA_AC

如果Koa<Kac ,β=ABC+OA_AC

 

4.代码实现

逆解部分的都在planner.cpp文件的plan_buffer_line函数中

 ///////////////diego_3dof kinemitacs///////////////////////
  ///alpha  oa_map_xy to y
  float alpha = 0;
  if(y==0){///////////in the x line
    if(x>0){
      alpha=PI/2;
      c_x=x-DEFAULT_CD_LENGTH_MM;
    }else if(x<0){ alpha=-PI/2; c_x=x+DEFAULT_CD_LENGTH_MM; } }else{ float tan_alpha=x/y; if(tan_alpha==0){///in the y line if(y>0){
        alpha=0;
        c_y = y - DEFAULT_CD_LENGTH_MM;
      }else{
        alpha=PI;
        c_y = y + DEFAULT_CD_LENGTH_MM;
      }
    }else if(tan_alpha>0){
      if(y>0){
        alpha=atan(tan_alpha);
      }else{
        alpha=atan(tan_alpha)-PI;
      }
      c_y=y-DEFAULT_CD_LENGTH_MM*cos(alpha);
      c_x=x-DEFAULT_CD_LENGTH_MM*sin(alpha);
    }else{
      if(y>0){
        alpha=atan(tan_alpha);
      }else{
        alpha=PI/2-atan(tan_alpha);
      }
      c_y=y-DEFAULT_CD_LENGTH_MM*cos(alpha);
      c_x=x-DEFAULT_CD_LENGTH_MM*sin(alpha);
    }
  }

  Serial.print("------------alpha\r\n");
  Serial.print(alpha*180/PI);
  Serial.print("\r\n");
  //gamma 180- ab to bc
  float gamma = 0;
  float default_oa = DEFAULT_OA_LENGTH_MM;
  float default_ab = DEFAULT_AB_LENGTH_MM;
  float default_bc = DEFAULT_BC_LENGTH_MM;
  float default_theta = DEFAULT_THETA_DEGREE;

  float a_x = default_oa * cos(default_theta) * sin(alpha);
  float a_y = default_oa * cos(default_theta) * cos(alpha);
  float a_z = default_oa * sin(default_theta);

  float ac = sqrt(pow(c_x - a_x, 2) + pow(c_y - a_y, 2) + pow(c_z - a_z, 2));

  float cos_gamma = (pow(default_ab, 2) + pow(default_bc, 2) - pow(ac, 2)) / (2 * default_ab * default_bc);
//  Serial.print("-------------cos_gamma");
//  Serial.print(cos_gamma);

  
  //gamma = PI - acos(cos_gamma);
  gamma = acos(cos_gamma);

  Serial.print("-------------gamma\r\n");
  Serial.print(gamma*180/PI);
  Serial.print("\r\n");

  ///beta; map to the axis between oa to ab
  float beta = 0;
  float k_map_oa = sin(default_theta) / cos(default_theta);

  float a_map_x = default_oa * cos(default_theta);  ////62.47
  float a_map_y = default_oa * sin(default_theta);  ////101.7

  float c_map_x = sqrt(pow(c_x, 2) + pow(c_y, 2)); ////130
  float c_map_y = c_z; ////102
  float k_map_ac = (c_map_y - a_map_y) / (c_map_x - a_map_x);
//  Serial.print("-------------c_map_y\r\n");
//  Serial.print(c_map_y);
//  Serial.print("\r\n");
//  Serial.print("-------------a_map_y\r\n");
//  Serial.print(a_map_y);
//  Serial.print("\r\n");
//  Serial.print("-------------c_map_x\r\n");
//  Serial.print(c_map_x);
//  Serial.print("\r\n");
//  Serial.print("-------------a_map_x\r\n");
//  Serial.print(a_map_x);
//  Serial.print("\r\n");
//  Serial.print("-------------k_map_oa\r\n");
//  Serial.print(k_map_oa);
//  Serial.print("\r\n");
//  Serial.print("-------------k_map_ac\r\n");
//  Serial.print(k_map_ac);
//  Serial.print("\r\n");

  float tan_oa_ac = abs(k_map_oa - k_map_ac) / abs(1 - k_map_oa * k_map_ac);
  float cos_ab_ac = (pow(default_ab, 2) + pow(ac, 2) - pow(default_bc, 2)) / (2 * default_ab*ac);
  float ab_ac = acos(cos_ab_ac);
//  Serial.print("-------------ab_ac\r\n");
//  Serial.print(cos_ab_ac);
//  Serial.print("\r\n");
//  Serial.print(ab_ac);
//  Serial.print("\r\n");
  float ac_oa = atan(tan_oa_ac);
//  Serial.print("-------------ac_oa\r\n");
//  Serial.print(ac_oa);
//  Serial.print("\r\n");
  
  if (ac_oa < ab_ac) { if (k_map_ac > k_map_oa) {
      beta = 0 - ab_ac - atan(tan_oa_ac);
    } else {
      beta = 0 - ab_ac + atan(tan_oa_ac);
    }
  } else {
    float oc = sqrt(pow(c_map_x, 2) + pow(c_map_y, 2));
    float cos_oa_ac = (pow(default_oa, 2) + pow(ac, 2) - pow(oc, 2)) / (2 * default_oa * ac);
    float oa_ac = acos(cos_oa_ac);
    beta = PI - oa_ac - ab_ac;
  }

  Serial.print("-------------beta\r\n");
  Serial.print(beta*180/PI);
  Serial.print("\r\n");

  float degree_per_step_axis1 = 2*PI / (DEFAULT_STEPS_PER_CYCLE_AXIS_1 * DEFAULT_REDUCTION_RATE_AXIS_1 * DEFAULT_MICROSTEPS_AXIS_1); //steps per cycle
  float degree_per_step_axis2 = 2*PI / (DEFAULT_STEPS_PER_CYCLE_AXIS_2 * DEFAULT_REDUCTION_RATE_AXIS_2 * DEFAULT_MICROSTEPS_AXIS_2); //steps per cycle
  float degree_per_step_axis3 = 2*PI / (DEFAULT_STEPS_PER_CYCLE_AXIS_3 * DEFAULT_REDUCTION_RATE_AXIS_3 * DEFAULT_MICROSTEPS_AXIS_3); //steps per cycle

  float axis1_degree_steps = alpha / degree_per_step_axis1;
  float axis2_degree_steps = beta / degree_per_step_axis2;
  float axis3_degree_steps = gamma / degree_per_step_axis3;

  target[X_AXIS] = (int32_t)(axis1_degree_steps + 0.5);
  target[Y_AXIS] = (int32_t)(axis2_degree_steps + 0.5);
  target[Z_AXIS] = (int32_t)(axis3_degree_steps + 0.5); 

全部代码请见gitclub

Lesson 2: Hardware diagram

一、硬件连接

Diego 3dof的硬件连接比较简单,Arduino和A4988是通过Arduino CNC sheild v3连接的,Arduino CNC sheild可以直接插在Arduino上,然后再把3个A4988插在Arduino CNC sheild上即可,连接效果如下:

CNC sheild可以支持4路步进电机,由于我们3轴码垛只使用到了3个步进电机,所以只需要插3个A4988即可,步进电机去A4988的对应关系如下图:

 

二、Arduino Pin对应关系

A4988及马达编号Arduino IO对应功能
1D2马达1步进脉冲
1D5马达1方向
2D3马达2步进脉冲
2D6马达2方向
3D4马达2步进脉冲
3D7马达2方向

 

 

三、其他

1.Diego 3轴机械臂的机械部分需要根据图纸进行钣金加工,或者也可以3D打印,钣金加工的精度要高与3D打印的精度

2. 安装过程中需要用到的不同规格的螺丝刀,电钻等工。

Lesson 1: Hardware BOM

diego 3轴机械臂,采用三轴码垛结构,三轴都采用步进电机控制,装配好后如下图所示

如下表格列出了Diego 3轴机械臂所用硬件物料的详细清单,包括每个硬件图片、名称、数量、功能特性和在机械臂中的作用。

PhotoNameCountFeaturediego 3dof 码垛中的作用
Arduino UNO1•处理器 ATmega328
•工作电压 5V
•输入电压(推荐) 7-12V
•输入电压(范围) 6-20V
•数字IO脚 14 (其中6路作为PWM输出)
•模拟输入脚 6
IO脚直流电流 40 mA
•3.3V脚直流电流 50 mA
•Flash Memory 32 KB (ATmega328,其中0.5 KB 用于 bootloader)
•SRAM 2 KB (ATmega328)
•EEPROM 1 KB (ATmega328)
•工作时钟 16 MHz
•主控制板

Arduino cnc shield 1•步进电机驱动扩展板•驱动机械臂步进电机
A49883•步进电机驱动•驱动机械臂步进电机
42步进电机3•42步进电机
• 星形减速,减速比15:1
• 步进角度为1.8°
•机械臂3个轴的驱动马达
12V 18650 battery1•标称电压 :12V
•放电电流 :6.0A
•充电电流 :0.5A
•内阻 :≤60mΩ
•最高充电电压 :12.6V
•最低放电电压 :9.0V
•为6自由度机械臂舵机供电
•为机械爪舵机供电

硬件安装过程中使用的到工具,螺丝刀,老虎钳,电钻等工具未列出来,读者可以根据实际需求自行准备。

Scroll to top