Delta并联机械臂实现电磁铁搬运功能
本帖最后由 机器谱 于 2023-3-17 09:32 编辑1. 功能说明
R037样机是一款Delta并联机械臂。本文示例将利用Delta并联机械臂实现不同点定点搬运磁铁物料的效果。
https://28846868.s21i.faiusr.com/2/ABUIABACGAAgusTRnwYonK-o3gUwsBg4wCA!500x500.jpg.webp
2. 结构说明
Delta并联机械臂，其驱动系统采用精度较高的42步进电机；传动系统为丝杠和万向球节；执行末端为搭载电磁铁的圆盘支架。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgpcXRnwYovMi66wEw6w04xgU!600x600.png.webphttps://28846868.s21i.faiusr.com/4/ABUIABAEGAAgucXRnwYooILFugUw_wU48wQ!600x600.png.webp
3. Delta机械臂运动学算法
这里给大家介绍一种Delta并联机械臂的运动轨迹解法，通过控制电机的转动参数，最终求解出电磁铁圆盘支架的运动轨迹规律，样机采用R037b
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgv7vRnwYooMHWvgcwwj044i0!600x600.png.webp 该机械臂由3个丝杠平台构成，通过并联的方式同时控制同一个端点的运动；三个丝杠位于一个正三角形边线的中心位置，连杆采用球头万向节连杆结构。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgjsfRnwYoyNTWkAEw_gc42gY!450x450.png.webp
① 首先我们建立一个空间直角坐标系，该直角坐标系以三个丝杠平台在俯视图方向投影的内切圆心为原点，x轴与tower1和tower3之间的连线平行，y轴过tower2，其中z=0的平面设置在三个限位开关所在平面。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgh8jRnwYo-I7K0AUwpAU4sAU!400x400.png.webp
② 建立坐标系之后，我们可以得出3个限位开关Z轴投影的坐标为A=（-msin（60°），mcos（60°），0）；B=（0，m，0）；C=（msin（60°），mcos（60°），0）；其中m为在xy投影面上正三角形的内切圆心到B点的距离。
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgz8jRnwYo-r7dxwEwzAc4xQQ!600x600.png.webp
③确定各限位开关的位置（即确定各丝杠平台上滑块的初始位置），丝杠平台的运动可简化为如下：【其中N点为滑块初始位置，Q点为端点初始位置，P为Q点在丝杠平台上Z轴的投影；N1，P1，Q1点为丝杠平台运动后的位，T点为某一固定点，假设为delta机械臂上端点在Z向可以运动的最大值在丝杠平台Z向的投影点】
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgocnRnwYovpuK6gQwrwQ4ngQ!400x400.png.webp
④逆运动学是根据Q1点的位置确定NN1的距离。
在图中有几个可以通过测量得到已知值，分别是连杆长度NQ/N1Q1、NT的距离、终点Q1点的坐标；假设我们输入的量是终点Q1的坐标（X1,Y1,Z1）;这里需要注意的是Z1坐标为负值，为了方便理解在后面的推导中我们都对Z1取绝对值。
我们需要计算的是NN1的距离：https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgoMrRnwYoyY2Y1wUwnQE4Fw.png.webp
其中Q1的Z坐标与P1的Z坐标一致，所以NP1为已知量为Q1的Z坐标值Z1，即可以将上面的公式改为：https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgzMrRnwYojvGRtQYwvQE4Fw.png.webp
这里我们只需要计算出N1P1的值即可：https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgjMvRnwYo_p2M9AIw1gE4Kw.png.webp
其中NIQ1为连杆长度，可通过测量得知，所以这里面需要我们计算出Q1P1。
⑤求出Q1P1：【该长度我们可以通过两点坐标距离公式得出，借助俯视图投影进行计算】
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg2svRnwYo_L7XxwEw0gY4wwQ!450x450.png.webp
为方便计算Q1P1，图中我们将N,N1,P,P1,T点都投影到Z为0的点，则Q1（X1，Y1，0）。
根据点坐标公式可得：https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgr8zRnwYonOa3BTCsAzgr.png.webp
综上所述：https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg58zRnwYo4arKwgMwigQ4Kw.png.webp
注意前面我们对Z1取了一次绝对值，实际Z1为负值，
所以最终推导公式为：https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgpM3RnwYoqJuejgEwigQ4Kw.png.webp
这样我们就求出了NN1（丝杠移动距离）与Q1（执行端运动的终点）坐标的关系。
4. 功能实现
4.1 电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：
主控板Basra主控板（兼容Arduino Uno）
扩展板Bigfish2.1
SH-ST扩展板
传感器触碰传感器
电机步进电机
电池11.1v动力电池
其它
电磁铁、USB线
4.2 电路连接说明
① 硬件连接-电子元件
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAg5tHRnwYov4TLmQcwigY41AU!400x400.png.webp
各轴步进电机与SH-ST步进电机扩展板的接线顺序如下（从上至下）：
X：红蓝黑绿
Y：红蓝黑绿
Z：黑绿红蓝
② 硬件连接-限位传感器
https://28846868.s21i.faiusr.com/4/ABUIABAEGAAgotLRnwYoypvufzDTAziWBA!450x450.png.webp
各个轴的限位传感器（触碰）与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 <AccelStepper.h>
#include <MultiStepper.h>
#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++){
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 = _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;
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 = 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, 向下运动 ; 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），控制原理完全一样。
https://28846868.s21i.faiusr.com/2/ABUIABACGAAg9dTRnwYog83e_gEwlQQ4xAU!450x450.jpg.webp
4. 资料下载
资料内容：
①例程源代码
②样机3D文件
资料下载地址：https://www.robotway.com/h-col-194.html
想了解更多机器人开源项目资料请关注 机器谱网站 https://www.robotway.com
页:
[1]