基于yolov5的六轴机械臂实现

一、机械臂逆解代码

#!/usr/bin/env python3
#coding=utf-8
import time
import math
from Arm_Lib import Arm_Device

# 创建机械臂对象
Arm = Arm_Device()
time.sleep(0.1)

##P控制，保证舵机可以到对应的角度
def Ser_P(ser_j1, ser_j2, ser_j3, ser_j4, ser_j5, ser_j6):
    Arm.Arm_serial_servo_write6(ser_j1, ser_j2, ser_j3, ser_j4, ser_j5, ser_j6, 2000)
    time.sleep(2)
    Ser = [0]
    for i in range(6):
        ser = Arm.Arm_serial_servo_read(i + 1)
        Ser += [ser]
    err_j1 = Ser[1] - ser_j1
    err_j2 = Ser[2] - ser_j2
    err_j3 = Ser[3] - ser_j3
    err_j4 = Ser[4] - ser_j4
    err_j5 = Ser[5] - ser_j5
    err_j6 = Ser[6] - ser_j6
    j1_P = ser_j1 - err_j1
    j2_P = ser_j2 - err_j2
    j3_P = ser_j3 - err_j3
    j4_P = ser_j4 - err_j4
    j5_P = ser_j5 - err_j5
    j6_P = ser_j6 - err_j6
    Arm.Arm_serial_servo_write6(j1_P, j2_P, j3_P, j4_P, j5_P, j6_P, 2000)
    time.sleep(2)

##单位：mm
def Servo_Calculate(x_pixel,y_pixel):
    K = 1  ##像素点到实际距离的比例系数
    R = 5  ##开始时摄像头到机械臂原点的水平距离
    Y_s = y_pixel  ##目标物体在摄像头中的坐标
    X_s = x_pixel
    ##舵臂的尺寸
    a1 = 120.00
    a2 = 82.85
    a3 = 82.85
    a4 = 180.00
    ##用于标记中间的三角函数值
    sinj2 = 0
    cosj2 = 0
    sinj3 = 0
    cosj3 = 0
    x = K * Y_s * math.sin(math.pi / 4)
    y = K * X_s
    len = math.sqrt((x + R) * (x + R) + y * y)
    high = 0
    ##存放一系列求解的角度值
    arr_j2 = [0]
    arr_j3 = [0]
    arr_j4 = [0]
    num = 0  ##用于记录可解的个数

    ##求解舵机1的角度
    if x == 0:
        j1 = 90
    else:
        j1 = int(math.atan((R + x) / y) * 57.3)
    ##遍历求解j2、j3、j4的值
    for i in range(0, 180):
        ##j_all = j2 + j3 + j4
        j_all = math.pi * i / 180

        L = len - a4 * math.sin(j_all)
        H = high - a4 * math.cos(j_all) - a1
        cosj3 = ((L * L) + (H * H) - (a2 * a2) - (a3 * a3)) / (2 * a2 * a3)
        sinj3 = math.sqrt(math.fabs(1 - cosj3 * cosj3))

        j3 = math.atan(sinj3 / cosj3) * 57.3

        K2 = a3 * math.sin(j3 / 57.3)
        K1 = a2 + a3 * math.cos(j3 / 57.3)

        cosj2 = (K2 * L + K1 * H) / (K1 * K1 + K2 * K2)
        sinj2 = math.sqrt(math.fabs(1 - cosj2 * cosj2))

        j2 = math.atan(sinj2 / cosj2) * 57.3
        j4 = j_all * 57.3 - j2 - j3

        ##选择满足条件的角度值
        if j2 >= 0 and j2 <= 90 and j3 >= 0 and j3 <= 90 and j4 >= 0 and j4 <= 90:
            arr_j2 += [j2]
            arr_j3 += [j3]
            arr_j4 += [j4]
            num = num + 1
    ##取可解值的中间值
    n = int(num / 2)
    print("num:", num)
    print("n:", n)
    j2 = 90 + int(arr_j2[n])
    j3 = 90 + int(arr_j3[n])
    j4 = 90 + int(arr_j4[n])
    Ser_P(j1,j2,j3,j4,90,90)
    Ser_t = [0]
    for i in range(6):
        ser_t = Arm.Arm_serial_servo_read(i+1)
        Ser_t += [ser_t]
    print("j1:",j1,"j2:",j2,"j3:",j3,"j4:",j4,"j5:",90,"j6:",90)
    print("Ser_t:",Ser_t)

if __name__ == '__main__':
    try:
        while True:
            ##初始化机械臂的位置
            Ser_P(90, 135, 0, 0, 90, 90)
            # Servo_Calculate(20,5)
    except keyboardInterrupt:
        pass

视频及资料链接：基于YoloV5的六轴机械臂抓取实现_哔哩哔哩_bilibili