Mediapipe智能控制机械臂

【项目背景】

最近入手了一台机械臂（DF 6自由度机械臂），使用威龙24路舵机控制器，配合Veyron_Servo_Driver_24-Channel 软件，实现了一些简单动作后，就一直放在茶几上吃灰。
在做了几个有关Mediapipe相关项目后，今天准备将机械臂与Mediapipe相结合，实现手部运动控制机械臂运动，进行抓取实物、移动位置。此项目将制作、改进、优化整个过程一步一步进行介绍。
【入门知识】
1.机械臂
（1）六自由度机械臂简介
机械臂是机器人技术领域中使用最广泛的自动机械装置。它可以在工业制造，医疗，娱乐服务，军事，半导体制造和太空探索中看到。虽然它们的形状不同，但它们都具有共同的特征，即它们可以接受指令并准确地定位三维（或二维）空间中的点以进行操作。

六自由度机械手臂，顾名思义，由六个关节组成，由伺服电机机械臂驱动。既然它是手臂，那么就有几个关节，可以想象我们的人体手臂，除了肩膀，肘部，手腕三个关节外，加上手指的关节，还有很多关节。我们的机器人手臂也是如此，它使用六个伺服电机来实现简单的手部结构。除了没有人的关节外，还有一些神经组织和神经系统缺失。然而，已经开发出具有“灵巧手”（其可以完成复杂的组装，处理或手动抓取蛋的手）的“人形”机器。

（2）自由度
自由度： 自由度是机器人的一个重要技术指标，它是由机器人的结构决定的，并直接影响到机器人的机动性。
物体上任何一点都与坐标轴的正交集合有关。物体能够对坐标系进行独立运动的数目称为自由度（DOF，degree of freedom)。如下图所示：

2.Mediapipe

MediaPipe是一个用于构建机器学习管道的框架，用于处理视频、音频等时间序列数据。这个跨平台框架适用于桌面/服务器、Android、iOS和嵌入式设备，如Raspberry Pi和Jetson Nano。MediaPipe只需要最少的资源。它是如此微小和高效，甚至嵌入式物联网设备都可以运行它。2019年，MediaPipe公开发布后，为研究人员和开发人员开辟了一个全新的机会世界。
【实现过程】
1.使用Mind+软件中的Python模式直接控制威龙24路舵机控制器

在控制器的产品维护文档中，有“Arduino与Veyron_Servo_Driver_24-Channel串口通讯实例”，经测试，成功!刚开始的想法是Python通过Pinpong库与Arduino通信，然后Arduino再发送控制指令给舵机控制器，一是过程繁琐，二是它们都使用到串口通信，互相干扰，经测试结果失败。再看技术文档，既然可以使用串口通信，那么可以去掉中间的Arduino，直接让Python使用串口与舵机控制器进行通信，经测试，成功！

import serial
import time
serialPort="COM6"
baudRate=9600
ser=serial.Serial(serialPort,baudRate,timeout=0.5)
print(ser.name)#打印设备名称
ser.write("#4 P1500".encode())#通道5到达指定位置
time.sleep(0.005)
ser.write("\r".encode())
time.sleep(3)

ser.write("#5 P2250 T5000".encode())#通道5舵机花5秒转到接近中间位置。
time.sleep(0.005)
ser.write("\r".encode())
time.sleep(5)
ser.write("#5 P750 S1000".encode())#使舵机从2250us移动到750us（大概170度），一共耗时1.5秒。
time.sleep(0.005)
ser.write("\r".encode())
time.sleep(2)

ser.write("#0 P1000 #1 P1000 #2 P1700 #3 P2200 T3000".encode())#同时到达指定位置
time.sleep(0.005)
ser.write("\r".encode())
time.sleep(3)

ser.write("#0 P2100 #1 P2200 #5 P1000 #3 P1500  T3000".encode())#同时到达指定位置
time.sleep(0.005)
ser.write("\r".encode())
time.sleep(3)
ser.close()#关闭端口
复制代码

2.移动物品
使用单舵机移动、多舵机同时移动、时间控制、速度控制多种方法，实现循环移动固定位置物品。

import serial
import time
serialPort="COM6"
baudRate=9600
ser=serial.Serial(serialPort,baudRate,timeout=0.5)
print(ser.name)#打印设备名称
while 1:
    #舵机4回中间位置，机械臂竖直
    ser.write("#4 P1500 T3000".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(3)
    #舵机0开口最大，其它舵机回中间位置
    ser.write("#0 P800 #1 P1500 #2 P1700 #3 P1500 #5 P1500 T5000".encode())#同时到达指定位置
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(5)
   #舵机1抬头
    ser.write("#1 P2400 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
    #舵机4前倾
    ser.write("#4 P300 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
    #舵机3稍前倾
    ser.write("#3 P1800 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
    #舵机2钳口摆正
    ser.write("#2 P1700 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
   #舵机0钳口夹紧
    ser.write("#0 P1800 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
    #除舵机0钳口
    ser.write("#1 P1500 #2 P2000 #3 P1500 #4 P1500 #5 P1500 T5000".encode())#同时到达指定位置
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(5)
#
    ser.write("#1 P2400 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
#
    ser.write("#5 P500 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
#
    ser.write("#2 P1800 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
#
   ser.write("#4 P300 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
#
    ser.write("#3 P1600 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
#
    ser.write("#0 P800 S500".encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(2)
   
ser.close()#关闭端口
复制代码

3.Mediapipe手部运动控制
首先通过Mind+python模式下的库管理安装Mediapipe、cvzone，安装好后，结合上面的程序，编写智能控制机械臂程序：

import cv2
from cvzone import HandTrackingModule as hd
import serial
import time



def numberMap(x, in_min, in_max, out_min, out_max):
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
def main():
    serialPort="COM6"
    baudRate=9600
    ser=serial.Serial(serialPort,baudRate,timeout=0.5)
    length1=0
    P=""
    start_time=time.time()
    cap = cv2.VideoCapture(0)
    detector = hd.HandDetector(detectionCon=0.8, maxHands=1)
    P="#0 P1500 #1 P2400 #2 P1700 #3 P1500 #4 P1500 #5 P1500 T1000"
    ser.write(P.encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(1)
    while True:
        # Get image frame
        success, img = cap.read()
        img=cv2.flip(img,1) # 保存水平翻转图片
        # Find the hand and its landmarks
        hands, img = detector.findHands(img)  # with draw
        # hands = detector.findHands(img, draw=False)  # without draw

        if hands:
            # Hand 1
            hand1 = hands[0]
            lmList1 = hand1["lmList"]  # List of 21 Landmark points
            bbox1 = hand1["bbox"]  # Bounding box info x,y,w,h
            centerPoint1 = hand1['center']  # center of the hand cx,cy
            handType1 = hand1["type"]  # Handtype Left or Right
            length, info, img = detector.findDistance(lmList1[8][0:2], lmList1[4][0:2], img)  # with draw
            fingers1 = detector.fingersUp(hand1)
            length1=250-int(length)
            
            length1=numberMap(length1, 0, 250, 800, 2200)
            cx,cy=detector.midpoint(lmList1[8][0:2], lmList1[4][0:2])
            disX=630-cx
            disX=numberMap(disX, 100, 530, 0, 2200)

            disY=370-cy
            disY=numberMap(disY,20, 350, 400, 2000)
            print(cy)
            if time.time()-start_time>0.5:
               P="#4 P"+str(disY)+" S500"
               ser.write(P.encode())
               time.sleep(0.005)
               ser.write("\r".encode())
               
            
               P="#5 P"+str(disX)+" S500"
               ser.write(P.encode())
               time.sleep(0.005)
               ser.write("\r".encode())
               start_time=time.time()
          
               P="#0 P"+str(length1)+" S500"
               ser.write(P.encode())
               time.sleep(0.005)
               ser.write("\r".encode())
               start_time=time.time()
           
        # Display
        cv2.imshow("Image", img)
        cv2.waitKey(1)


if __name__ == "__main__":
    main()

复制代码

4.进行优化
通过测试，上面的视频也可发现，有时舵机运行过快，出现抖动，这应该是由于舵机运行使用的是速度控制，中间的两个指令的时间间隔不足（上个指令舵机还没有到达指定位置），造成抖动。所以对程序进行修改，将速度控制修改为时间控制（1秒内到达指定位置），并将两指令的发送最小时间间隔也设定为1秒。

import cv2
from cvzone import HandTrackingModule as hd
import serial
import time



def numberMap(x, in_min, in_max, out_min, out_max):
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
def main():
    serialPort="COM6"
    baudRate=9600
    ser=serial.Serial(serialPort,baudRate,timeout=0.5)
    length1=0
    P=""
    start_time=time.time()
    cap = cv2.VideoCapture(0)
    detector = hd.HandDetector(detectionCon=0.8, maxHands=1)
    P="#0 P1500 #1 P2400 #2 P1700 #3 P1500 #4 P1500 #5 P1500 T1000"
    ser.write(P.encode())
    time.sleep(0.005)
    ser.write("\r".encode())
    time.sleep(1)
    length_pre=100
    cy_pre=240
    cx_pre=320
    while True:
        # Get image frame
        success, img = cap.read()
        img=cv2.flip(img,1) # 保存水平翻转图片
        # Find the hand and its landmarks
        hands, img = detector.findHands(img)  # with draw
        # hands = detector.findHands(img, draw=False)  # without draw

        if hands:
            # Hand 1
            hand1 = hands[0]
            lmList1 = hand1["lmList"]  # List of 21 Landmark points
            bbox1 = hand1["bbox"]  # Bounding box info x,y,w,h
            centerPoint1 = hand1['center']  # center of the hand cx,cy
            handType1 = hand1["type"]  # Handtype Left or Right
            length, info, img = detector.findDistance(lmList1[8][0:2], lmList1[4][0:2], img)  # with draw
            fingers1 = detector.fingersUp(hand1)
            length1=250-int(length)
            
            length1=numberMap(length1, 0, 250, 800, 2200)
            cx,cy=detector.midpoint(lmList1[8][0:2], lmList1[4][0:2])
            disX=630-cx
            disX=numberMap(disX, 100, 530, 0, 2200)

            disY=370-cy
            disY=numberMap(disY,20, 350, 400, 2000)
            print(cy)
            if time.time()-start_time>1:
           
             if abs(cy-cy_pre)>5:
               P="#4 P"+str(disY)+" T1000"
               ser.write(P.encode())
               time.sleep(0.005)
               ser.write("\r".encode())
               cy_pre=cy
             if abs(cx-cx_pre)>5:
            
               P="#5 P"+str(disX)+" T1000"
               ser.write(P.encode())
               time.sleep(0.005)
               ser.write("\r".encode())
               start_time=time.time()
               cx_pre=cx
             if abs(length-length_pre)>5:
               P="#0 P"+str(length1)+" T1000"
               ser.write(P.encode())
               time.sleep(0.005)
               ser.write("\r".encode())
               length_pre=length
             start_time=time.time()
           
        # Display
        cv2.imshow("Image", img)
        cv2.waitKey(1)


if __name__ == "__main__":
    main()

复制代码

厉害厉害

不错不错

好像没有宋老师不会玩的东西了！！！！赞！

喜欢，大家一起动手试试

好好 远程控制机械臂

厉害厉害

厉害了！！！

哇塞！ 牛哇牛哇！！！

哇塞！牛哇！！牛哇！！

大牛啊，确实牛！！！

高手在民间，牛

老师，请问威龙24路舵机控制器在哪可以买到，谢谢

谢谢分享，这个机械臂太6了