Arduino简单实现两自由度Scara机器人

1.描述

两自由度Scara机器人有两个旋转关节，机器人的控制主要就是电机和末端执行器的控制。在学习marlin固件控制机器人时，由于marlin代码过于繁杂且许多代码都是关于3D打印，与机器人相关的代码占比很少。所以我想自己写一段代码，尝试实现scara的控制。在我写这篇博客时，修改Marlin实现scara控制已完成，功能很强大，后期也会写一写。这里我先讲讲我这个小白写的代码，比较粗浅，慢慢进步吧。
电机控制不是简单地给个脉冲频率让它转动就行，其中需要包含加减速的控制以减少电机因急停、速度快速变化带来的电路冲击，使运动更加平稳。在搜索中我发现了AccelStepper库，大大简化了对电机的控制，很方便，在控制要求不高的场合完全可以使用这个库来实现。用我自己画的四自由度Scara机器人代替下，多了末端z方向自由度和旋转自由度，如下图。

2.数学模型

让机器人运动到目标位置需要给定坐标，我这里设定了工作台坐标系和基坐标系。工作台坐标系也称世界坐标系，是一个通用坐标系，机器人所有的运动都是相对于它来执行的。基坐标系是以主臂转动中心为原点建立的。给机器人发送的是世界坐标系中的坐标，程序通过这两个坐标系的位置关系，求出在基坐标系下的坐标，并由此坐标通过数学模型求出主臂与x轴正方向的夹角，及副臂和主臂的夹角。有了角度，下一步就是驱动电机到达目标角度即可。可以走直线，走圆弧，或者不用差补直接一步到位，这在轨迹规划中设置。
下图是机器人坐标系（左） 和 工作坐标系（右）的关系（这里没有offset-y，如有需要可以加上）。箭头指向为正方向。注意：这里的x轴正方向是有改变的。

输入世界坐标，求出基坐标。

机器人坐标系	世界坐标系
（100,0）	（0,0）
（80 ,60）	（20 ,60）
（0 ,100）	（100 ,100）
（80 ,60）	（20 ,60）
（-80 ,60）	（180 ,60）

接着求出逆解，即电机转角，数学模型如下（这张是引用别人的图片），计算过程在代码中有注释，这里不再赘述。

这样我们就得到电机应该达到目标位置的转角，减去当前位置的转角就是我们即将要驱动电机转动的角度。角度乘以转过每个角度所需的脉冲数即得到开发板所需发出的脉冲数，接着调用AccelStepper库文件，轻松实现梯形加减速到达目标位置。
AccelStepper库详细描述

3.代码实现

#include<AccelStepper.h>

//本程序设计的是scara构型机器手臂，两自由度
//实验耗材：开发板mega2560,扩展板RAMPS1.4，42步进电机两个，A4988驱动器两个，机械限位开关两个，24V开关电源。
//不同电机、驱动器、开发板也可以实现，定义好引脚即可
#define ST1 1 //driver模式，两线电机
#define ST2 1
//定义步进电机引脚
#define X_STEP_PIN         54 //脉冲引脚
#define X_DIR_PIN          55 //方向引脚
#define X_ENABLE_PIN       38 //使能引脚
#define Y_STEP_PIN         60
#define Y_DIR_PIN          61
#define Y_ENABLE_PIN       56
#define KEY_X 3   //主臂限位开关引脚
#define KEY_Y 14  //副臂限位开关引脚
#define LED           13 //LED灯引脚，用来模拟机器人到达目标位置后末端执行器的动作，如气缸、手爪等动作

//定义一些固定参数
#define Linkage_1 50 //主臂长
#define Linkage_2 50 //副臂长
#define L1_2 2500  //主臂长平方
#define L2_2 2500  //副臂长平方
#define SCARA_RAD2DEG  57.29578 //弧度转角度
#define steps_per_unit 8.888888   //电机转过1°角度所需的脉冲数，如果有减速，乘上减速比即可
#define offset_x  -100    //世界坐标系原点 相对 基坐标系原点的 x偏移值，注意正负号（与你建立的世界坐标系位置有关）                     *工作台坐标系
#define offset_y   0     //世界坐标系原点 相对 基坐标系原点的 y偏移值，注意正负号                             

float arr[32]={0};      //存放串口接受的目标位置X Y坐标的数组
char cmd;              //串口接收的字符
int j=0;               //数组中的元素个数

enum AXIS {X_AXIS=0,Y_AXIS=1};       //定义主臂为0，副臂为1
float current_position[2]={0};       //当前位置坐标
float cartesian[2]={0};              //世界坐标系下坐标（自定义的坐标）,坐标型
float SCARA_pos[2]={0};              //基坐标系下的位置,坐标型
float SCARA_C2=0;                    //末端中心相对基坐标系的cos值
float SCARA_S2=0;                    //末端中心相对基坐标系的sin值
float scara[2]={0};                  //原点坐标系坐标下的主臂与x轴夹角,副臂和主臂夹角
int calculate_position[2]={0};       //到达目标位置主臂和副臂所需的脉冲
bool arrival =true;                  //电机到达目标位置标志符
float target_position[2]={0};


//定义两个步进电机对象，这个在AccelStepper库里有详细的介绍，不同电机参照即可
AccelStepper stepper1(ST1,X_STEP_PIN,X_DIR_PIN);
AccelStepper stepper2(ST2,Y_STEP_PIN,Y_DIR_PIN);

 void setup() {
  Serial.begin(250000);

   pinMode(X_ENABLE_PIN ,OUTPUT);
   pinMode(Y_ENABLE_PIN ,OUTPUT);
   pinMode(KEY_X,INPUT_PULLUP);      //限位开关上拉
   pinMode(KEY_Y,INPUT_PULLUP);      //限位开关上拉
   pinMode(LED ,OUTPUT);

//启用以下代码可以实现开机自动归零，若不启用，则需要发'H'使其归零
//  while(digitalRead(KEY_X)==HIGH)
//  {
//   digitalWrite(X_STEP_PIN,HIGH);
//   delay(1);
//   digitalWrite(X_STEP_PIN,LOW);
//   delay(1);
//  }
//   while(digitalRead(KEY_Y)==HIGH)
//   {
//   digitalWrite(Y_STEP_PIN,HIGH);
//   delay(1);
//   digitalWrite(Y_STEP_PIN,LOW);
//   delay(1);
//   }
  
  stepper1.setMaxSpeed(20000.0);     // 1号电机最大速度 
  stepper1.setAcceleration(15000.0);  // 1号电机加速度
 
  stepper2.setMaxSpeed(20000.0);     // 2号电机最大速度
  stepper2.setAcceleration(15000.0);  // 2号电机加速度
}

//串口
//发送给arduino的数据格式为 #a b   a b为世界坐标系下的x y坐标值 
void getSerial()
{
  if(Serial.available()&&j<32)
  {
    cmd=char(Serial.read());  
        if(cmd=='#') //提取坐标（世界坐标系）
        {
         for(int i=0;i<2;i++) //x y坐标是成对发送的
         {
          arr[j]=Serial.parseFloat();//快速提取#后面的两个float型数据
          j++; //数组中的元素个数+1 
         }
        } 
        else if(cmd=='H') //归位（归零），主臂先归位，触碰到限位开关后停止，副臂开始归位，触碰开关后停止
        {
               while(digitalRead(KEY_X)==HIGH)
            {
             digitalWrite(X_STEP_PIN,HIGH);
             delay(1);
             digitalWrite(X_STEP_PIN,LOW);
             delay(1);
            }
             while(digitalRead(KEY_Y)==HIGH)
             {
             digitalWrite(Y_STEP_PIN,HIGH);
             delay(1);
             digitalWrite(Y_STEP_PIN,LOW);
             delay(1);
             }  
             stepper1.setCurrentPosition(stepper1.currentPosition());//将当前位置设置为绝对位置0（这里的绝对位置是归位后主臂和基坐标系x轴正方向的夹角）
             stepper2.setCurrentPosition(stepper2.currentPosition());//如果不归位，上电后的初始坐标就是绝对位置0
          }
         else if(cmd=='P')   //获取当前坐标（工作台坐标系） 
        {
          Serial.print("x:");
          Serial.print(cartesian[X_AXIS]);
          Serial.print("y:");
          Serial.println(cartesian[Y_AXIS]);
          }
          else if(cmd=='S') //获取当前脉冲数    
        {
          Serial.print("x:");
          Serial.print(stepper1.currentPosition());
          Serial.print("y:");
          Serial.println(stepper2.currentPosition());
          }
  }
}

//**********************************************************逆运动学********************************************************
//以下程序需要结合坐标系图
void prepare_move()
{
  
  cartesian[X_AXIS]=arr[X_AXIS];
  cartesian[Y_AXIS]=arr[Y_AXIS];

  //根据数学模型求逆解
  static float SCARA_C2, SCARA_S2, SCARA_K1, SCARA_K2, SCARA_theta, SCARA_psi; 
  SCARA_pos[X_AXIS] = -cartesian[X_AXIS]-offset_x;  //Translate SCARA to standard X Y
  SCARA_pos[Y_AXIS] = cartesian[Y_AXIS]-offset_y ;  // With scaling factor.

  //余弦定理 求副臂和主臂的夹角
  #if (Linkage_1 == Linkage_2)
    SCARA_C2 = ( ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) ) / (2 * (float)L1_2) ) - 1;
  #else
    SCARA_C2 =   ( sq(SCARA_pos[X_AXIS]) + sq(SCARA_pos[Y_AXIS]) - (float)L1_2 - (float)L2_2 ) / 45000; //这里的45000是（2*Linkage_1*Linkage_2）
  #endif
  
  SCARA_S2 = sqrt( 1 - sq(SCARA_C2) );
  SCARA_K1 = Linkage_1 + Linkage_2 * SCARA_C2;//OC的长
  SCARA_K2 = Linkage_2 * SCARA_S2;//OB的长
  SCARA_theta = ( atan2(SCARA_pos[X_AXIS],SCARA_pos[Y_AXIS])-atan2(SCARA_K1, SCARA_K2) ) * -1;
  //，图中角1-角2，即（90-角3）-（90-角OBC），即（角3-角OBC）*-1
  //主臂与x轴正方向的夹角
  SCARA_psi   =   atan2(SCARA_S2,SCARA_C2);       //副臂与主臂的夹角
  scara[X_AXIS] = SCARA_theta * SCARA_RAD2DEG;    //  弧度转角度 
  scara[Y_AXIS] = SCARA_psi * SCARA_RAD2DEG;  
     
  float stepper_x=scara[X_AXIS]*steps_per_unit;  //求达到该角度所需的脉冲数
  float stepper_y=scara[Y_AXIS]*steps_per_unit;
  calculate_position[X_AXIS]=round(stepper_x);   //脉冲数取整
  calculate_position[Y_AXIS]=round(stepper_y);
 
}


void loop() 
{
  getSerial();  //获取串口数据
  if(j>0)   //当数组中有坐标时
  {
    prepare_move(); //准备运动

  if(arrival==true) //当上一次运动到达时，开启下一次运动
   {
    arrival==false;
    target_position[X_AXIS]=calculate_position[X_AXIS];
    target_position[Y_AXIS]=calculate_position[Y_AXIS];
    //执行运动     
    // stepper1.run()该函数不影响后续程序的执行，即电机还没运动到目标位置，下面的程序就已经执行了，所以开启下一次运动前要加判断。
    stepper1.moveTo(target_position[X_AXIS]);              
    stepper2.moveTo(target_position[Y_AXIS]);  
    stepper1.run();   // 1号电机运行
    stepper2.run();   // 2号电机运行
   }
  }
 if(stepper1.currentPosition() == target_position[X_AXIS] && stepper2.currentPosition() == target_position[Y_AXIS]&&(j>0) ) 
 {
    arrival=true;              //当到达目标位置后，LED灯亮，模拟气缸、手爪动作
    digitalWrite(LED,HIGH);
    delay(1000);
    digitalWrite(LED,LOW);
    for(int i=0;i<j-2;i++)
    {
      arr[i]=arr[i+2];//这里采用的是笨方法，后面的坐标覆盖前面的坐标
      }
    j-=2;    //数组中的未执行的坐标个数
 }
}

4.注意点以及扩展

stepper1.run()这个函数是以你设置的加速度和最大速度到达目标位置，是一个梯形加减速的过程。
代码比较粗糙，展示的主要是思路，还有许多细节的地方需要完善。上述代码仅是以一步到位的方式到达目标位置，没有经过差补。如果你想差补，可以在轨迹规划里加入代码，直线插补、关节差补、圆弧差补。我在这里谈一谈直线差补：计算出目标位置与当前位置的距离，x方向和y方向的差值。我设定这段路程分为200小段来走（差补次数可以设定定值也可以是计算出的变量），那么当前位置x+x差值*（第几次差补）/200=下一个目标位置。写一个for循环，走完这200步，把每一步的坐标值带入到逆解里得出角度然后驱动电机。差补的步数越多末端轨迹越像直线，但是差补过多会影响运行速度，且角度太小的话走的会不准确。建议根据路程长短设置差补步数（变量）。再加上一个电机和丝杠，可以实现三自由度，思路都是一样的。
运用Marlin固件也可以控制scara，且更准确，速度上限更大（实测得出）,运行也更稳定。大佬们写的程序确实很厉害，我只是一个小白。在后期会写marlin控制scara所需更改的配置。写这篇文章，只是觉得Arduino的库文件真的很方便。在项目-加载库-管理库里面搜索AccelStepper可以下载这个库文件。