1. 功能说明

本文示例将利用Delta并联机械臂实现不同点定点搬运磁铁物料的效果。

 2. 结构说明

       Delta并联机械臂，其驱动系统采用精度较高的42步进电机；传动系统为丝杠和万向球节；执行末端为搭载电磁铁的圆盘支架。

3. Delta机械臂运动学算法

       这里给大家介绍一种Delta并联机械臂的运动轨迹解法，通过控制电机的转动参数，最终求解出电磁铁圆盘支架的运动轨迹规律，样机采用R037b

       该机械臂由3个丝杠平台构成，通过并联的方式同时控制同一个端点的运动；三个丝杠位于一个正三角形边线的中心位置，连杆采用球头万向节连杆结构。

 ① 首先我们建立一个空间直角坐标系，该直角坐标系以三个丝杠平台在俯视图方向投影的内切圆心为原点，x轴与tower1和tower3之间的连线平行，y轴过tower2，其中z=0的平面设置在三个限位开关所在平面。

 ② 建立坐标系之后，我们可以得出3个限位开关Z轴投影的坐标为A=（-msin（60°），mcos（60°），0）；B=（0，m，0）；C=（msin（60°），mcos（60°），0）；其中m为在xy投影面上正三角形的内切圆心到B点的距离。

 ③确定各限位开关的位置（即确定各丝杠平台上滑块的初始位置），丝杠平台的运动可简化为如下：【其中N点为滑块初始位置，Q点为端点初始位置，P为Q点在丝杠平台上Z轴的投影；N1，P1，Q1点为丝杠平台运动后的位，T点为某一固定点，假设为delta机械臂上端点在Z向可以运动的最大值在丝杠平台Z向的投影点】

④逆运动学是根据Q1点的位置确定NN1的距离。     

在图中有几个可以通过测量得到已知值，分别是连杆长度NQ/N1Q1、NT的距离、终点Q1点的坐标；假设我们输入的量是终点Q1的坐标（X1,Y1,Z1）;这里需要注意的是Z1坐标为负值，为了方便理解在后面的推导中我们都对Z1取绝对值。     

我们需要计算的是NN1的距离：

 其中Q1的Z坐标与P1的Z坐标一致，所以NP1为已知量为Q1的Z坐标值Z1，即可以将上面的公式改为：

 这里我们只需要计算出N1P1的值即可：

 其中NIQ1为连杆长度，可通过测量得知，所以这里面需要我们计算出Q1P1。     

⑤求出Q1P1：【该长度我们可以通过两点坐标距离公式得出，借助俯视图投影进行计算】

为方便计算Q1P1，图中我们将N,N1,P,P1,T点都投影到Z为0的点，则Q1（X1，Y1，0）。

根据点坐标公式可得：

 综上所述：

注意前面我们对Z1取了一次绝对值，实际Z1为负值，

所以最终推导公式为：

这样我们就求出了NN1（丝杠移动距离）与Q1（执行端运动的终点）坐标的关系。

4. 功能实现

4.1 电子硬件

在这个示例中，我们采用了以下硬件，请大家参考：

4.2 电路连接说明

① 硬件连接-电子元件

各轴步进电机与SH-ST步进电机扩展板的接线顺序如下（从上至下）：
X：红蓝黑绿

Y：红蓝黑绿

Z：黑绿红蓝

② 硬件连接-限位传感器

各个轴的限位传感器（触碰）与Bigfish扩展板的接线如下：
X：A0

Y：A3

Z：A4

③ 电磁铁连在Bigfish扩展板的D5、D6接口上。

4.3 编写程序

编程环境：Arduino 1.8.19

Delta机械臂有两种运动方式：第一种是自动运行搬运；第二种是用电脑发送指令，然后再根据指令运动。     

这里仅列出Delta机械臂自动运行搬运（Delta.ino）的程序：【其它的程序源码可下载文末资料获取】

/*------------------------------------------------------------------------------------版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.Distributed under MIT license.See file LICENSE for detail or copy athttps://opensource.org/licenses/MITby 机器谱 2023-02-08 https://www.robotway.com/                                 ------------------------------------------------------------------------------------*/#include "Arduino.h"#include #include #include "Configuration.h"AccelStepper stepper_x(1, 2, 5);      //tower1AccelStepper stepper_y(1, 3, 6);      //tower2AccelStepper 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};         //当前坐标float destination[NUM_AXIS];                         //目标坐标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;         // 当前坐标点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, 向下运动 ; steps > 0, 向上运动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: 650stepper_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. 扩展

    下图是另一种外观的Delta机械臂（R037c），控制原理完全一样。

例程源代码、样机3D文件资料内容下载详见 Delta并联机械臂