视频效果
https://pan.baidu.com/s/1sMP-tNi0KWiCTKWUEM3WQg?pwd=fehs 提取码:fehs
一、 机械臂本体结构
DH参数表
二、机械臂运动学模型
三、机械臂运动学正反解源码 (Matlab)
正逆解模型
%% 运动学反解模型
%Z=L2*cos(j2)+L3*cos(j2+j3)+L4*cos(j2+j3+j4)+L1;
%len=L2*sin(j2)+L3*sin(j2+j3)+L4*sin(j2+j3+j4);
clc;
close all;
clear all;
syms j1 j2 j3 j4 X Y Z;
n = 0;m = 0;
X=input('输入X的坐标:');
Y=input('输入Y的坐标:');
Z=input('输入Z的坐标:');
% X=57.86;
% Y=189.24;
% Z=30.55;
a1 = 75.7;a2 = 132;a3 = 98.03; a4 = 146;
if (X == 0)
j1 = 90;
else
j1 = abs(atan((Y) / X)*(57.3));
end
for i=0:180
j_all = 3.1415927*i / 180;
len = sqrt((Y)*(Y) + X * X);
high = Z;
L = len - a4 * sin(j_all);
H = high - a4 * cos(j_all) - a1;
Cosj3 = ((L*L) + (H*H) - ((a2)*(a2)) - ((a3)*(a3))) / (2 * (a2)*(a3));
Sinj3 = (sqrt(1 - (Cosj3)*(Cosj3)));
j3 = atan((Sinj3) / (Cosj3))*(57.3);
K2 = a3 * sin(j3 / 57.3);
K1 = a2 + a3 * cos(j3 / 57.3);
Cosj2 = (K2*L + K1 * H) / (K1*K1 + K2 * K2);
Sinj2 = (sqrt(1 - (Cosj2)*(Cosj2)));
j2 = atan((Sinj2) / (Cosj2))*57.3;
j4 = j_all * 57.3 - j2 - j3;
if (j2 >= 0 && j3 >= 0 && j4 >= -90 && j2 <= 180 && j3 <= 180 && j4 <= 90)
n = n + 1;
end
end
for i=0:180
j_all = 3.1415927*i / 180;
len = sqrt((Y)*(Y) + X * X);
high = Z;
L = len - a4 * sin(j_all);
H = high - a4 * cos(j_all) - a1;
Cosj3 = ((L*L) + (H*H) - ((a2)*(a2)) - ((a3)*(a3))) / (2 * (a2)*(a3));
Sinj3 = (sqrt(1 - (Cosj3)*(Cosj3)));
j3 = atan((Sinj3) / (Cosj3))*(57.3);
K2 = a3 * sin(j3 / 57.3);
K1 = a2 + a3 * cos(j3 / 57.3);
Cosj2 = (K2*L + K1 * H) / (K1*K1 + K2 * K2);
Sinj2 = (sqrt(1 - (Cosj2)*(Cosj2)));
j2 = atan((Sinj2) / (Cosj2))*57.3;
j4 = j_all * 57.3 - j2 - j3;
if (j2 >= 0 && j3 >= 0 && j4 >= -90 && j2 <= 180 && j3 <= 180 && j4 <= 90)
m = m + 1;
if (m == n / 2 || m == (n + 1) / 2)
break;
end
end
end
fprintf("\n各关节角度:\n");
fprintf("j1:%f,j2:%f\nj3:%f,j4:%f\n\n", j1, j2, j3, j4);
%% 运动学正解模型
L1 = 75.7;L2 = 132;L3 = 98.03; L4 = 146;
j1_=j1/180*pi;j2_=j2/180*pi;j3_=j3/180*pi;j4_=j4/180*pi;
X0=[1;0;0];
R1=[cos(j2_) -sin(j2_) 0;
sin(j2_) cos(j2_) 0;
0 0 1];
R2=[cos(j3_) -sin(j3_) 0;
sin(j3_) cos(j3_) 0;
0 0 1];
R3=[cos(j4_) -sin(j4_) 0;
sin(j4_) cos(j4_) 0;
0 0 1];
T=L1*X0+L2*R1*X0+L3*R1*R2*X0+L4*R1*R2*R3*X0;
Z=L2*cos(j2_)+L3*cos(j2_+j3_)+L4*cos(j2_+j3_+j4_)+L1;
len=T(2,1);
X_=len/sqrt(1+(tan(j1_))^2);
Y_=sqrt(len^2-len^2/(1+(tan(j1_))^2));
Z_=T(1,1);
fprintf("正解验证:\n");
fprintf("X:%f,Y:%f,Z:%f\n", X_, Y_, Z_);
逆解函数封装
function [j1,j2,j3,j4]=Inverse_kine(X,Y,Z)
n = 0;m = 0;
a1 = 75.7;a2 = 132;a3 = 98.03; a4 = 146;
if (X == 0)
j1 = 90;
else
j1 = abs(atan((Y) / X)*(57.3));
end
for i=0:180
j_all = 3.1415927*i / 180;
len = sqrt((Y)*(Y) + X * X);
high = Z;
L = len - a4 * sin(j_all);
H = high - a4 * cos(j_all) - a1;
Cosj3 = ((L*L) + (H*H) - ((a2)*(a2)) - ((a3)*(a3))) / (2 * (a2)*(a3));
Sinj3 = (sqrt(1 - (Cosj3)*(Cosj3)));
j3 = atan((Sinj3) / (Cosj3))*(57.3);
K2 = a3 * sin(j3 / 57.3);
K1 = a2 + a3 * cos(j3 / 57.3);
Cosj2 = (K2*L + K1 * H) / (K1*K1 + K2 * K2);
Sinj2 = (sqrt(1 - (Cosj2)*(Cosj2)));
j2 = atan((Sinj2) / (Cosj2))*57.3;
j4 = j_all * 57.3 - j2 - j3;
if (j2 >= 0 && j3 >= 0 && j4 >= -90 && j2 <= 180 && j3 <= 180 && j4 <= 90)
n = n + 1;
end
end
for i=0:180
j_all = 3.1415927*i / 180;
len = sqrt((Y)*(Y) + X * X);
high = Z;
L = len - a4 * sin(j_all);
H = high - a4 * cos(j_all) - a1;
Cosj3 = ((L*L) + (H*H) - ((a2)*(a2)) - ((a3)*(a3))) / (2 * (a2)*(a3));
Sinj3 = (sqrt(1 - (Cosj3)*(Cosj3)));
j3 = atan((Sinj3) / (Cosj3))*(57.3);
K2 = a3 * sin(j3 / 57.3);
K1 = a2 + a3 * cos(j3 / 57.3);
Cosj2 = (K2*L + K1 * H) / (K1*K1 + K2 * K2);
Sinj2 = (sqrt(1 - (Cosj2)*(Cosj2)));
j2 = atan((Sinj2) / (Cosj2))*57.3;
j4 = j_all * 57.3 - j2 - j3;
if (j2 >= 0 && j3 >= 0 && j4 >= -90 && j2 <= 180 && j3 <= 180 && j4 <= 90)
m = m + 1;
if (m == n / 2 || m == (n + 1) / 2)
break;
end
end
end
end
C++实现
#include <math.h>
#include <stdio.h>
int main()
{
float a1,a2,a3,a4; //a1为底部圆台高度 剩下三个为三个机械臂长度
float j1,j4,j2,j3; //四个姿态角
float L,H,P; //L =a2*sin(j2) + a3*sin(j2 + j3);H = a2*cos(j2) + a3*cos(j2 + j3); P为底部圆盘半径R
float j_all; //j2,j3,j4之和
float len,high; //总长度,总高度
float Cosj3,Sinj3; //用来存储cosj3,sinj3数值
float Cosj2,Sinj2;
float K1,K2;
float X,Y,Z; //输入 (X,Y,Z)坐标
int i;
float n,m,q;
n = 0;
m = 0;
q = 0;
//目标点坐标(X,Y,Z)
X = 0;
Y = 18;
Z = 0;
P = 14; //底部圆盘半径
a1 = 12; //底部圆盘高度
a2 = 12; //机械臂长度
a3 = 12;
a4 = 12;
if (X == 0)
j1=90;
else
j1 = atan((Y+P)/X)*(57.3);
for(i=0;i<=180;i++)
{
j_all = 3.1415927*i/180;
len = sqrt((Y+P)*(Y+P)+X*X);
high = Z;
L = len - a4*sin(j_all);
H = high - a4*cos(j_all) - a1;
Cosj3 = ((L*L)+(H*H)-((a2)*(a2))-((a3)*(a3)))/(2*(a2)*(a3));
Sinj3 = (sqrt(1-(Cosj3)*(Cosj3)));
j3 = atan((Sinj3)/(Cosj3))*(57.3);
K2 = a3*sin(j3/57.3);
K1 = a2+a3*cos(j3/57.3);
Cosj2 = (K2*L+K1*H)/(K1*K1+K2*K2);
Sinj2 = (sqrt(1-(Cosj2)*(Cosj2)));
j2 = atan((Sinj2)/(Cosj2))*57.3;
j4 = j_all*57.3- j2 - j3;
if(j2>=0&&j3>=0&&j4>=-90&&j2<=180&&j3<=180&&j4<=90)
{
n=n+1;
}
}
for(i=0;i<=180;i++)
{
j_all = 3.1415927*i/180;
len = sqrt((Y+P)*(Y+P)+X*X);
high = Z;
L = len - a4*sin(j_all);
H = high - a4*cos(j_all) - a1;
Cosj3 = ((L*L)+(H*H)-((a2)*(a2))-((a3)*(a3)))/(2*(a2)*(a3));
Sinj3 = (sqrt(1-(Cosj3)*(Cosj3)));
j3 = atan((Sinj3)/(Cosj3))*(57.3);
K2 = a3*sin(j3/57.3);
K1 = a2+a3*cos(j3/57.3);
Cosj2 = (K2*L+K1*H)/(K1*K1+K2*K2);
Sinj2 = (sqrt(1-(Cosj2)*(Cosj2)));
j2 = atan((Sinj2)/(Cosj2))*57.3;
j4 = j_all*57.3- j2 - j3;
if(j2>=0&&j3>=0&&j4>=-90&&j2<=180&&j3<=180&&j4<=90)
{
m=m+1;
if(m==n/2||m==(n+1)/2)
{
break;
}
}
}
printf("j1:%f,j2:%f\nj3:%f,j4:%f\n",j1,j2,j3,j4);
}
四、机械臂运动控制
K210视觉端代码
import ustruct
import sensor,lcd,time
from machine import UART,Timer
from fpioa_manager import fm
fm.register(6, fm.fpioa.UART1_RX, force=True)#映射串口引脚
fm.register(7, fm.fpioa.UART1_TX, force=True)
uart = UART(UART.UART1, 115200, read_buf_len=4096)#初始化串口
sensor.reset()#摄像头初始化
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.set_vflip(1) #后置模式,所见即所得
lcd.init()#lcd初始化
clock=time.clock()
blue_threshold = (13, 78, -21, 28, -94, -19)#蓝色阈值
red_threshold =(20, 55, 75, 21, 69, 9)#红色阈值
green_threshold =(21, 66, -14, -104, 79, -39)#绿色阈值
var_x_stride=10
var_y_stride=10
var_pixels_threshold=80
var_merge=True
var_margin=True
var_uart_feedback=False
def sending_data(color, cx, cy):
global uart
data = ustruct.pack("<bbhhhb",0x2c,0x12,int(color),int(cx),int(cy),0x5b)
uart.write(data)
while True:
# text=uart.read() #读取数据
# if text: #如果读取到了数据
# print("[INFO][USART] ",text.decode('utf-8')) #REPL打印
clock.tick()
img = sensor.snapshot().lens_corr(strength = 1.8, zoom = 1.0)#从感光芯片获得一张图像并且进行畸变校正
blue_blobs = img.find_blobs([blue_threshold],x_stride=var_x_stride, y_stride=var_y_stride, pixels_threshold=var_pixels_threshold,merge=var_merge,margin=var_margin )
red_blobs = img.find_blobs([red_threshold], x_stride=var_x_stride, y_stride=var_y_stride, pixels_threshold=var_pixels_threshold,merge=var_merge,margin=var_margin )
green_blobs = img.find_blobs([green_threshold], x_stride=var_x_stride, y_stride=var_y_stride, pixels_threshold=var_pixels_threshold,merge=var_merge,margin=var_margin )
if blue_blobs:
color_status = ord('B')
for r in blue_blobs:
img.draw_rectangle((r[0],r[1],r[2],r[3]),color=(0,0,255))
img.draw_cross(r[5], r[6],size=2,color=(0,255,0))
img.draw_string(r[0], (r[1]-10), "blue", color=(0,0,255))
sending_data(color_status,r[5],r[6])
if green_blobs:
color_status = ord('G')
for r in green_blobs:
img.draw_rectangle((r[0],r[1],r[2],r[3]),color=(0,255,0))
img.draw_cross(r[5], r[6],size=2,color=(0,255,0))
img.draw_string(r[0], (r[1]-10), "green", color=(0,255,0))
sending_data(color_status,r[5],r[6])
if red_blobs:
color_status = ord('R')
for r in red_blobs:
img.draw_rectangle((r[0],r[1],r[2],r[3]),color=(255,0,0))
img.draw_cross(r[5], r[6],size=2,color=(255,0,0))
img.draw_string(r[0], (r[1]-10), "red", color=(255,0,0))
sending_data(color_status,r[5],r[6])
lcd.display(img) #LCD显示图片
Stm32F407端主要代码(Keil5)
由于代码较多篇幅有限,这里只放核心代码,需要全部工程文件的见文末链接。
#include <ROBOT.h>
unsigned char robot_state = 0;
unsigned int item_amount_blue = 0;
unsigned int item_amount_green = 0;
unsigned int item_amount_red = 0;
//设置工作状态函数,并用LED指示当前工作状态
void setRobotState(int state)
{
if(state)
{
LED1=0;
LED0=1;
robot_state=1;
//NOTE:K210串口关闭直接中断
// USART_ITConfig(USART2, USART_IT_RXNE, ENABLE);//开启中断,继续接收坐标
} else {
LED1=1;
LED0=0;
robot_state=0;
// USART_ITConfig(USART2, USART_IT_RXNE, DISABLE);//关闭中断,防止干扰
}
}
void robot_init()
{
robot_state=0;
change_mode(1,1); //设置锁死模式
change_mode(2,1);
change_mode(3,1);
change_mode(4,1);
change_mode(5,1);
change_mode(6,1);
set_speed(1,40); //设置转速
set_speed(2,40);
set_speed(3,40);
set_speed(4,40);
set_speed(5,40);
set_speed(6,40);
int idlist[20]={1,2,3,4,5,6}; //使用set_angles()时需要初始化的ID数组
//float anglelist[20]={160,185,107,2,0,40};
float anglelist[20]={130,125,107,2,0,100};
set_angles(idlist,anglelist,100,1);
delay_ms(1000);
delay_ms(1000);
delay_ms(1000);
//setRobotState(1);
}
//反馈机器人状态
//当为0时为工作状态-执行搬运任务,1时为空闲状态-等待搬运坐标
int getRobotState()
{
return robot_state;
}
//使末端执行器张开/放置
void openEffector(int delay)
{
set_angle(5,0,100);
if(delay) delay_ms(delay);
}
//使末端执行器闭合/抓起
void closeEffector(int delay)
{
set_angle(5,55,100);
if(delay) delay_ms(delay);
}
/*
* 逆解核心程序
*/
void robot_move(int X,int Y,int Z,int change)
{
if(getRobotState()||(!change))
{
if(change) setRobotState(0);
//计算逆解
double a1, a2, a3, a4; //a1为底部圆台高度 剩下三个为三个机械臂长度
double j1, j4, j2, j3; //四个姿态角
double L, H, P; //L =a2*sin(j2) + a3*sin(j2 + j3);H = a2*cos(j2) + a3*cos(j2 + j3); P为底部圆盘半径R
double j_all; //j2,j3,j4之和
double len, high; //总长度,总高度
double Cosj3, Sinj3; //用来存储cosj3,sinj3数值
double Cosj2, Sinj2;
double K1, K2;
int i;
double n, m;
n = 0;
m = 0;
P = 0;
a1 = 75.7; //底部圆盘高度
a2 = 132; //机械臂长度
a3 = 98.03; //17.83
a4 = 146;
if (X == 0)
j1 = 90;
else
{
j1 = atan((Y + P) / X)*(57.3);
if(j1<0)j1=-j1;
}
for (i = 0; i <= 180; i++)
{
j_all = 3.1415927*i / 180;
len = sqrt((Y + P)*(Y + P) + X * X);
high = Z;
L = len - a4 * sin(j_all);
H = high - a4 * cos(j_all) - a1;
Cosj3 = ((L*L) + (H*H) - ((a2)*(a2)) - ((a3)*(a3))) / (2 * (a2)*(a3));
Sinj3 = (sqrt(1 - (Cosj3)*(Cosj3)));
j3 = atan((Sinj3) / (Cosj3))*(57.3);
K2 = a3 * sin(j3 / 57.3);
K1 = a2 + a3 * cos(j3 / 57.3);
Cosj2 = (K2*L + K1 * H) / (K1*K1 + K2 * K2);
Sinj2 = (sqrt(1 - (Cosj2)*(Cosj2)));
j2 = atan((Sinj2) / (Cosj2))*57.3;
j4 = j_all * 57.3 - j2 - j3;
if (j2 >= 0 && j3 >= 0 && j4 >= -90 && j2 <= 180 && j3 <= 180 && j4 <= 90)
{
n = n + 1;
}
}
for (i = 0; i <= 180; i++)
{
j_all = 3.1415927*i / 180;
len = sqrt((Y + P)*(Y + P) + X * X);
high = Z;
L = len - a4 * sin(j_all);
H = high - a4 * cos(j_all) - a1;
Cosj3 = ((L*L) + (H*H) - ((a2)*(a2)) - ((a3)*(a3))) / (2 * (a2)*(a3));
Sinj3 = (sqrt(1 - (Cosj3)*(Cosj3)));
j3 = atan((Sinj3) / (Cosj3))*(57.3);
K2 = a3 * sin(j3 / 57.3);
K1 = a2 + a3 * cos(j3 / 57.3);
Cosj2 = (K2*L + K1 * H) / (K1*K1 + K2 * K2);
Sinj2 = (sqrt(1 - (Cosj2)*(Cosj2)));
j2 = atan((Sinj2) / (Cosj2))*57.3;
j4 = j_all * 57.3 - j2 - j3;
if (j2 >= 0 && j3 >= 0 && j4 >= -90 && j2 <= 180 && j3 <= 180 && j4 <= 90)
{
m = m + 1;
if (m == n / 2 || m == (n + 1) / 2)
{
break;
}
}
}
if(X>=0 && Y>=0)
{
j1=107-90+j1;
}
else if(X<0 && Y>=0)
{
j1=107+90-j1;
}
else if(X<0 && Y<0)
{
j1=107+90+j1;
}
j2=j2+160;
j3=185-j3;
j4=40+j4;
int idlist[20]={3,1,2,6};
float anglelist[20]={j1,j2,j3,j4};
set_angles(idlist,anglelist,100,1);
delay_ms(2000);
if(change) setRobotState(1);
}
}
/*
* 根据比例系数与参数转换为机器人坐标系坐标并执行相关码垛核心部分
* 已知K210 视觉空间为320x240
* X,Y,color 为串口传入相关数据
*/
void robot_transform(int X,int Y,int color)
{
if(getRobotState()) //防止任务重复
{
setRobotState(0); //设置为工作状态
printf("INPUT X: %d Y: %d Color: %d \n",X,Y,color); //数据反馈
//坐标变换
double scale_x =1.003125;
double scale_y =0.983333;
double distance_x=106,distance_y=93;
int rx = (int)(-distance_x+Y*scale_y); //视觉区域外距离+坐标数值*比例系数
int ry = (int)(distance_y+X*scale_x);
printf("TRANS X: %d Y: %d \n",rx,ry); //数据反馈
if((ry<=140)||(ry>=365))
{
printf("RETURN \n");
setRobotState(1);
return;
}
//误差补偿
if(ry<=200)
{
ry=ry+2;
robot_move(rx,ry,20,0);
} else if(ry<=300)
{
ry=ry+15;
if(rx>=0) rx=rx+15;
else rx=rx-15;
robot_move(rx,ry,15,0);
} else {
ry=ry+15;
if(rx>=0) rx=rx+20;
else rx=rx-15;
robot_move(rx,ry,60,0);
}
printf("MOVE X: %d Y: %d \n",rx,ry); //数据反馈-实际
closeEffector(1000); //抓取物块
int idlist[20]={1,2,3,4,6};
float anglelist[20]={130,125,107,2,100};
set_angles(idlist,anglelist,100,1); //恢复姿态
delay_ms(2000);
int z = 120;
// int zscale = 35;
if(color==71) //绿色
{
// z=z+item_amount_green*zscale;
robot_move(-180,0,z,0);
item_amount_green++;
} else if(color==82) //红色
{
// z=z+item_amount_red*zscale;
robot_move(-180,100,z,0);
item_amount_red++;
} else if(color==66){ //蓝色
// z=z+item_amount_blue*zscale;
robot_move(-180,-100,z,0);
item_amount_blue++;
}
//printf("PUT z:%d",z); //数据反馈
openEffector(1000); //放下物块
set_angle(1,130,100);
delay_ms(1000);
set_angles(idlist,anglelist,100,1); //恢复姿态
delay_ms(1500);
setRobotState(1); //解除工作状态
}
}
五、项目地址
https://pan.baidu.com/s/1RGkw8X3DCYciO0LEpT5UIg?pwd=tpl2 提取码:tpl2