视频效果

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

 

 

 

# stm32 # 单片机 # mcu # 物联网 # 嵌入式硬件 # 计算机视觉 # 机器人
Logo
全球具身智能开发者社区

立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。

更多推荐

cover

从“感知缺失”到“力觉觉醒”：FACTR 2如何让低成本机械臂学会“手感”

avatar 全球具身智能开发者社区
cover

多链路聚合设备：从“链路备份”到“确定性传输底座”-1

avatar 全球具身智能开发者社区
cover

如何实现抖店同行数据截流自动化？20核引擎全开，百店并发零报错零中断

avatar 全球具身智能开发者社区