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 at https://opensource.org/licenses/MIT by 机器谱 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 ; float cartesian = {0.0, 0.0, 0.0}; //当前坐标 float destination ; //目标坐标 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++){ =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 = _dx; destination = _dy; destination = _dz; if(destination == 0 && destination == 0 && destination == 0){ stepperReset(); } else { dataComplete = true; } if(serial_notes){ Serial.print("destinationPosition: "); Serial.print(destination ); Serial.print(" "); Serial.print(destination ); Serial.print(" "); Serial.println(destination ); } } /***************************************Process_command***************************************/ void Process_command(){ if(dataComplete){ digitalWrite(EN, LOW); if(cartesian == destination && cartesian == destination && cartesian == destination ){ return; } else { Line_DDA(destination , destination , destination ); } } 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 ;y0 = cartesian ;z0 = cartesian ; 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 = x0 - cartesian ; cartesian = y0 - cartesian ; cartesian = z1 - cartesian ; calculate_delta(cartesian); stepperSet(1350.0, 50.0); stepperMove(delta , delta , delta ); cartesian = x0; cartesian = y0; cartesian = z1; x0 += cx; y0 += cy; if(serial_notes){ Serial.print("currentPosition: "); for(int i=0;i<3;i++){ Serial.print(cartesian ); Serial.print(" "); } Serial.println(); Serial.println("-------------------------------------------------"); } } } /***************************************calculateDelta****************************************/ void calculate_delta(float cartesian ) { if(cartesian == 0 && cartesian == 0 && cartesian == 0){ delta = 0; delta =0; delta = 0; } else { delta = sqrt(delta_diagonal_rod_2 - sq(delta_tower1_x-cartesian ) - sq(delta_tower1_y-cartesian ) ) + cartesian ; delta = sqrt(delta_diagonal_rod_2 - sq(delta_tower2_x-cartesian ) - sq(delta_tower2_y-cartesian ) ) + cartesian ; delta = sqrt(delta_diagonal_rod_2 - sq(delta_tower3_x-cartesian ) - sq(delta_tower3_y-cartesian ) ) + cartesian ; for(int i=0;i<3;i++){ delta = ((delta - 111.96) * stepsPerRevolution / LEAD); } } if(serial_notes){ Serial.print("cartesian x="); Serial.print(cartesian ); Serial.print(" y="); Serial.print(cartesian ); Serial.print(" z="); Serial.println(cartesian ); Serial.print("delta tower1="); Serial.print(delta ); Serial.print(" tower2="); Serial.print(delta ); Serial.print(" tower3="); Serial.println(delta ); } } /****************************************stepperMove******************************************/ void stepperMove(long _x, long _y, long _z){ long positions ; positions = _x; //steps 0, 向上运动 positions = _y; positions = _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 != 0){ Get_command(0, 0, cartesian ); 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 = 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),控制原理完全一样。 Delta并联机械臂的程序源代码及样机3D文件详见【R037】Delta并联机械臂-电磁铁搬运https://www.robotway.com/h-col-194.html