Delta并联机械臂实现电磁铁搬运功能
1. 功能说明
r037样机是一款delta并联机械臂。本文示例将利用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 电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：
主控板 basra主控板（兼容arduino uno）
扩展板 bigfish2.1
sh-st扩展板
传感器 触碰传感器
电机 步进电机
电池 11.1v动力电池
其它 电磁铁、usb线
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）的程序：【其它的程序源码详见 https://www.robotway.com/h-col-194.html 获取】
/*------------------------------------------------------------------------------------ 版权说明：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.haccelstepper 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 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, 向上运动 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. 扩展
下图是另一种外观的delta机械臂（r037c），控制原理完全一样。
章丘南外环隧道项目电力监控系统系统简介
产品推荐 | 红外光源OPT-IXX-IR系列
波音公司正在考虑研发767宽体飞机用于货运市场
国内首家公测一加推出AndroidPBeta版
区块链+金融，落地如此难，这家公司为何这么有把握
Delta并联机械臂实现电磁铁搬运功能
用CD4017B制作多路脉冲分配器
单细胞+转录组测序揭示GABA信号调控神经发生
测试治具和工装夹具的区别是什么
无人驾驶的未来，究竟长什么样？
Vitis调试技巧-从Vitis中导出数据到本地
诺德股份6μm锂电高端铜箔创新高
干货：蓝牙网关的内部系统架构和外部结构介绍，值得收藏！
未来的技术趋势——机器人思维
保护物联网知识产权免受智能制造的影响
医药箱设计将更注重产品的防潮设计和使用寿命
索尼正在开发一款新的c智能手机该机将配备6颗后置摄像头
手机来电提醒器,Phone call indicator
Q4净利大涨66.4％！意法半导体将扩增碳化硅产能
FatFs特点和应用接口以及其移植方法
r037样机是一款delta并联机械臂。本文示例将利用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 电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：
主控板 basra主控板（兼容arduino uno）
扩展板 bigfish2.1
sh-st扩展板
传感器 触碰传感器
电机 步进电机
电池 11.1v动力电池
其它 电磁铁、usb线
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）的程序：【其它的程序源码详见 https://www.robotway.com/h-col-194.html 获取】
/*------------------------------------------------------------------------------------ 版权说明：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.haccelstepper 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 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, 向上运动 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. 扩展
下图是另一种外观的delta机械臂（r037c），控制原理完全一样。
章丘南外环隧道项目电力监控系统系统简介
产品推荐 | 红外光源OPT-IXX-IR系列
波音公司正在考虑研发767宽体飞机用于货运市场
国内首家公测一加推出AndroidPBeta版
区块链+金融，落地如此难，这家公司为何这么有把握
Delta并联机械臂实现电磁铁搬运功能
用CD4017B制作多路脉冲分配器
单细胞+转录组测序揭示GABA信号调控神经发生
测试治具和工装夹具的区别是什么
无人驾驶的未来，究竟长什么样？
Vitis调试技巧-从Vitis中导出数据到本地
诺德股份6μm锂电高端铜箔创新高
干货：蓝牙网关的内部系统架构和外部结构介绍，值得收藏！
未来的技术趋势——机器人思维
保护物联网知识产权免受智能制造的影响
医药箱设计将更注重产品的防潮设计和使用寿命
索尼正在开发一款新的c智能手机该机将配备6颗后置摄像头
手机来电提醒器,Phone call indicator
Q4净利大涨66.4％！意法半导体将扩增碳化硅产能
FatFs特点和应用接口以及其移植方法