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

Leave a Reply

Scroll to top
%d bloggers like this: