1. 功能說明

R037樣機(jī)是一款Delta并聯(lián)機(jī)械臂。本文示例將利用Delta并聯(lián)機(jī)械臂實(shí)現(xiàn)不同點(diǎn)定點(diǎn)搬運(yùn)磁鐵物料的效果。

2. 結(jié)構(gòu)說明

Delta并聯(lián)機(jī)械臂，其驅(qū)動(dòng)系統(tǒng)采用精度較高的42步進(jìn)電機(jī)；傳動(dòng)系統(tǒng)為絲杠和萬(wàn)向球節(jié)；執(zhí)行末端為搭載電磁鐵的圓盤支架。

3. Delta機(jī)械臂運(yùn)動(dòng)學(xué)算法

這里給大家介紹一種Delta并聯(lián)機(jī)械臂的運(yùn)動(dòng)軌跡解法，通過控制電機(jī)的轉(zhuǎn)動(dòng)參數(shù)，最終求解出電磁鐵圓盤支架的運(yùn)動(dòng)軌跡規(guī)律，樣機(jī)采用R037b

該機(jī)械臂由3個(gè)絲杠平臺(tái)構(gòu)成，通過并聯(lián)的方式同時(shí)控制同一個(gè)端點(diǎn)的運(yùn)動(dòng)；三個(gè)絲杠位于一個(gè)正三角形邊線的中心位置，連桿采用球頭萬(wàn)向節(jié)連桿結(jié)構(gòu)。

① 首先我們建立一個(gè)空間直角坐標(biāo)系，該直角坐標(biāo)系以三個(gè)絲杠平臺(tái)在俯視圖方向投影的內(nèi)切圓心為原點(diǎn)，x軸與tower1和tower3之間的連線平行，y軸過tower2，其中z=0的平面設(shè)置在三個(gè)限位開關(guān)所在平面。

② 建立坐標(biāo)系之后，我們可以得出3個(gè)限位開關(guān)Z軸投影的坐標(biāo)為A=（-msin（60°），mcos（60°），0）；B=（0，m，0）；C=（msin（60°），mcos（60°），0）；其中m為在xy投影面上正三角形的內(nèi)切圓心到B點(diǎn)的距離。

③確定各限位開關(guān)的位置（即確定各絲杠平臺(tái)上滑塊的初始位置），絲杠平臺(tái)的運(yùn)動(dòng)可簡(jiǎn)化為如下：【其中N點(diǎn)為滑塊初始位置，Q點(diǎn)為端點(diǎn)初始位置，P為Q點(diǎn)在絲杠平臺(tái)上Z軸的投影；N1，P1，Q1點(diǎn)為絲杠平臺(tái)運(yùn)動(dòng)后的位，T點(diǎn)為某一固定點(diǎn)，假設(shè)為delta機(jī)械臂上端點(diǎn)在Z向可以運(yùn)動(dòng)的最大值在絲杠平臺(tái)Z向的投影點(diǎn)】

④逆運(yùn)動(dòng)學(xué)是根據(jù)Q1點(diǎn)的位置確定NN1的距離。

在圖中有幾個(gè)可以通過測(cè)量得到已知值，分別是連桿長(zhǎng)度NQ/N1Q1、NT的距離、終點(diǎn)Q1點(diǎn)的坐標(biāo)；假設(shè)我們輸入的量是終點(diǎn)Q1的坐標(biāo)（X1,Y1,Z1）;這里需要注意的是Z1坐標(biāo)為負(fù)值，為了方便理解在后面的推導(dǎo)中我們都對(duì)Z1取絕對(duì)值。

我們需要計(jì)算的是NN1的距離：

其中Q1的Z坐標(biāo)與P1的Z坐標(biāo)一致，所以NP1為已知量為Q1的Z坐標(biāo)值Z1，即可以將上面的公式改為：

這里我們只需要計(jì)算出N1P1的值即可：

其中NIQ1為連桿長(zhǎng)度，可通過測(cè)量得知，所以這里面需要我們計(jì)算出Q1P1。

⑤求出Q1P1：【該長(zhǎng)度我們可以通過兩點(diǎn)坐標(biāo)距離公式得出，借助俯視圖投影進(jìn)行計(jì)算】

為方便計(jì)算Q1P1，圖中我們將N,N1,P,P1,T點(diǎn)都投影到Z為0的點(diǎn)，則Q1（X1，Y1，0）。

根據(jù)點(diǎn)坐標(biāo)公式可得：

綜上所述：

注意前面我們對(duì)Z1取了一次絕對(duì)值，實(shí)際Z1為負(fù)值，

所以最終推導(dǎo)公式為：

這樣我們就求出了NN1（絲杠移動(dòng)距離）與Q1（執(zhí)行端運(yùn)動(dòng)的終點(diǎn)）坐標(biāo)的關(guān)系。

4. 功能實(shí)現(xiàn)

4.1 電子硬件

在這個(gè)示例中，我們采用了以下硬件，請(qǐng)大家參考：

4.2 電路連接說明

① 硬件連接-電子元件

各軸步進(jìn)電機(jī)與SH-ST步進(jìn)電機(jī)擴(kuò)展板的接線順序如下（從上至下）：
X：紅藍(lán)黑綠

Y：紅藍(lán)黑綠

Z：黑綠紅藍(lán)

② 硬件連接-限位傳感器

各個(gè)軸的限位傳感器（觸碰）與Bigfish擴(kuò)展板的接線如下：
X：A0

Y：A3

Z：A4

③ 電磁鐵連在Bigfish擴(kuò)展板的D5、D6接口上。

4.3 編寫程序

編程環(huán)境：Arduino 1.8.19

Delta機(jī)械臂有兩種運(yùn)動(dòng)方式：第一種是自動(dòng)運(yùn)行搬運(yùn)；第二種是用電腦發(fā)送指令，然后再根據(jù)指令運(yùn)動(dòng)。

這里僅列出Delta機(jī)械臂自動(dòng)運(yùn)行搬運(yùn)（Delta.ino）的程序：【其它的程序源碼詳見 https://www.robotway.com/h-col-194.html 獲取】

/*------------------------------------------------------------------------------------

  版權(quán)說明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 機(jī)器譜 2023-02-08 https://www.robotway.com/                                 

------------------------------------------------------------------------------------*/

#include "Arduino.h"

#include 

#include 

#include "Configuration.h"


AccelStepper stepper_x(1, 2, 5);      //tower1

AccelStepper stepper_y(1, 3, 6);      //tower2

AccelStepper stepper_z(1, 4, 7);      //tower3

//AccelStepper stepper_a(1, 12, 13);


MultiStepper steppers;


float delta[NUM_STEPPER];                         

float cartesian[NUM_AXIS] = {0.0, 0.0, 0.0};         //當(dāng)前坐標(biāo)

float destination[NUM_AXIS];                         //目標(biāo)坐標(biāo)

boolean dataComplete = false;


float down = -111;

float up = -105;


/*********************************************Main******************************************/

void setup() {

  Serial.begin(9600);

  pinMode(EN, OUTPUT);


  steppers.addStepper(stepper_x);

  steppers.addStepper(stepper_y);

  steppers.addStepper(stepper_z);


  stepperSet(1600, 400.0);

  stepperReset();

  delay(1000);


  Get_command(0, 0, down);

  Process_command();

 

  delay(1000);

}


void loop() {

  float r = 25;

  float x1 = 0.0;

  float y1 = 0.0;


  Get_command(25, 0, down);

  Process_command();

  delay(1000);

 

  for(int i=0;i<2;i++){

    for(float i=0.0;i<=360;i+=10){

      x1 = r * cos(i / 180 * 3.141592);

      y1 = r * sin(i / 180 * 3.141592);

     

      Get_command(x1, y1, down);

      Process_command();                                     

    }

  }

  delay(1000);


  for(int j=0;j<2;j++){

    for(float i=360.0;i>=0;i-=10){

      x1 = r * cos(i / 180 * 3.141592);

      y1 = r * sin(i / 180 * 3.141592);

     

      Get_command(x1, y1, down);

      Process_command();                                     

    }

  }

  delay(1000);




  Get_command(0, 0, down);

  Process_command();  

 

  test();

  delay(1000);

 

  stepperReset();

  delay(1000);

 

}


/***************************************Get_commond*******************************************/

void Get_command(float _dx, float _dy, float _dz){  

  destination[0] = _dx;

  destination[1] = _dy;

  destination[2] = _dz;

 

  if(destination[0] == 0 && destination[1] == 0 && destination[2] == 0){

      stepperReset();  

  }

  else

  {

      dataComplete = true;

  }

 

  if(serial_notes){

   Serial.print("destinationPosition: ");

   Serial.print(destination[0]);

   Serial.print(" ");

   Serial.print(destination[1]);

   Serial.print(" ");

   Serial.println(destination[2]);

  }


}


/***************************************Process_command***************************************/

void Process_command(){

  if(dataComplete){

    digitalWrite(EN, LOW);

   

    if(cartesian[0] == destination[0] && cartesian[1] == destination[1] && cartesian[2] == destination[2]){

      return;  

    }

    else

    {


      Line_DDA(destination[0], destination[1], destination[2]);

    }

   

  }

  dataComplete = false;

  digitalWrite(EN, HIGH);

}


/************************************************** DDA ************************************************/

void Line_DDA(float x1, float y1, float z1)

{

  float x0, y0, z0;         // 當(dāng)前坐標(biāo)點(diǎn)

  float cx, cy;             // x、y 方向上的增量

 

  x0 = cartesian[0];y0 = cartesian[1];z0 = cartesian[2];

   

  int steps = abs(x1 - x0) > abs(y1 - y0) ? abs(x1 - x0) : abs(y1 - y0);

 

  cx = (float)(x1 - x0) / steps;

  cy = (float)(y1 - y0) / steps;

 

  for(int i = 0; i <= steps; i++)

  {

    cartesian[0] = x0 - cartesian[0];

    cartesian[1] = y0 - cartesian[1];

    cartesian[2] = z1 - cartesian[2];


    calculate_delta(cartesian);

   

    stepperSet(1350.0, 50.0);

    stepperMove(delta[0], delta[1], delta[2]);

 

    cartesian[0] = x0;

    cartesian[1] = y0;

    cartesian[2] = z1;

 

    x0 += cx;

    y0 += cy;

   

    if(serial_notes){

      Serial.print("currentPosition: ");

      for(int i=0;i<3;i++){

         Serial.print(cartesian[i]);

         Serial.print(" ");

      }   

      Serial.println();

      Serial.println("-------------------------------------------------");

    }


  }


}


/***************************************calculateDelta****************************************/

void calculate_delta(float cartesian[3])

{

  if(cartesian[0] == 0 && cartesian[1] == 0 && cartesian[2] == 0){

      delta[0] = 0; delta[1] =0; delta[2] = 0;

  }

  else

  {

      delta[TOWER_1] = sqrt(delta_diagonal_rod_2

                       - sq(delta_tower1_x-cartesian[X_AXIS])

                       - sq(delta_tower1_y-cartesian[Y_AXIS])

                       ) + cartesian[Z_AXIS];

      delta[TOWER_2] = sqrt(delta_diagonal_rod_2

                       - sq(delta_tower2_x-cartesian[X_AXIS])

                       - sq(delta_tower2_y-cartesian[Y_AXIS])

                       ) + cartesian[Z_AXIS];

      delta[TOWER_3] = sqrt(delta_diagonal_rod_2

                       - sq(delta_tower3_x-cartesian[X_AXIS])

                       - sq(delta_tower3_y-cartesian[Y_AXIS])

                       ) + cartesian[Z_AXIS];


     for(int i=0;i<3;i++){

        delta[i] = ((delta[i] - 111.96) * stepsPerRevolution / LEAD);

     }

  }


  if(serial_notes){

      Serial.print("cartesian x="); Serial.print(cartesian[X_AXIS]);

      Serial.print(" y="); Serial.print(cartesian[Y_AXIS]);

      Serial.print(" z="); Serial.println(cartesian[Z_AXIS]);

   

      Serial.print("delta tower1="); Serial.print(delta[TOWER_1]);       

      Serial.print(" tower2="); Serial.print(delta[TOWER_2]);

      Serial.print(" tower3="); Serial.println(delta[TOWER_3]);

  }


}


/****************************************stepperMove******************************************/

void stepperMove(long _x, long _y, long _z){             

    long positions[3];

    positions[0] = _x;                        //steps < 0, 向下運(yùn)動(dòng) ; steps > 0, 向上運(yùn)動(dòng)

    positions[1] = _y;

    positions[2] = _z;


    steppers.moveTo(positions);

    steppers.runSpeedToPosition();


    stepper_x.setCurrentPosition(0);

    stepper_y.setCurrentPosition(0);

    stepper_z.setCurrentPosition(0);

}


/****************************************stepperSet******************************************/

void stepperSet(float _v, float _a){

  stepper_x.setMaxSpeed(_v);                  //MaxSpeed: 650

  stepper_x.setAcceleration(_a);  

  stepper_y.setMaxSpeed(_v);

  stepper_y.setAcceleration(_a);

  stepper_z.setMaxSpeed(_v);

  stepper_z.setAcceleration(_a);


}


/****************************************stepperReset******************************************/

void stepperReset(){

  digitalWrite(EN, LOW);

 

  if(cartesian[2] != 0){

    Get_command(0, 0, cartesian[2]);

    Process_command();

    digitalWrite(EN, LOW);

  }

 

  while(digitalRead(SENSOR_TOWER1) && digitalRead(SENSOR_TOWER2) && digitalRead(SENSOR_TOWER3)){

    stepperMove(10, 10, 10);

  }


  stepperSet(1200.0, 100.0);

  stepperMove(-400, 0, 0);

  while(digitalRead(SENSOR_TOWER1)){

    stepperMove(10, 0, 0);

   

  }


  stepperMove(0, -400, 0);

  while(digitalRead(SENSOR_TOWER2)){

    stepperMove(0, 10, 0);

  }


  stepperMove(0, 0, -400);

  while(digitalRead(SENSOR_TOWER3)){

    stepperMove(0, 0, 10);

  }


  for(int i=0;i<3;i++){

     cartesian[i] = 0.0;

  }  


  if(serial_notes) Serial.println("resetComplete!");


  digitalWrite(EN, HIGH);

}


/*************************************************** electromagnet *************************************************************/

void putUp(){

   digitalWrite(9, HIGH);

   digitalWrite(10, LOW);

}


void putDown(){

   digitalWrite(9, LOW);

   digitalWrite(10, LOW);

}


/**************************************************   test    ******************************************************************/

void test(){

    Get_command(0, 0, down);

    Process_command();

    delay(500);

    putUp();

 

    Get_command(0, 0, up);

    Process_command();  

    Get_command(25, 0, up);

    Process_command();


    Get_command(25, 0, down);

    Process_command();  

    putDown();

    delay(500);

    putDown();

    putUp();


    Get_command(25, 0, up);

    Process_command();   

    Get_command(0, 25, up);

    Process_command();   


    Get_command(0, 25, down);

    Process_command();

    putDown();  

    delay(500);

    putDown();

    putUp();


    Get_command(0, 25, up);

    Process_command();   

    Get_command(-25, 0, up);

    Process_command();   


    Get_command(-25, 0, down);

    Process_command();  

    putDown();

    delay(500);

    putUp();


    Get_command(-25, 0, up);

    Process_command();   

    Get_command(0, -25, up);

    Process_command();   


    Get_command(0, -25, down);

    Process_command();  

    putDown();

    delay(500);

    putUp();


    Get_command(0, -25, up);

    Process_command();   

    Get_command(25, 0, up);

    Process_command();   


    Get_command(25, 0, down);

    Process_command();   

    putDown();

    delay(500);

    putUp();


    Get_command(25, 0, up);

    Process_command();   

    Get_command(0, 0, up);

    Process_command();   


    Get_command(0, 0, down);

    Process_command();   

    delay(500);

    putDown();

}

5. 擴(kuò)展

下圖是另一種外觀的Delta機(jī)械臂（R037c），控制原理完全一樣。

審核編輯黃宇