机械臂：物体跟踪

---

总体结构

提示：这里可以添加本文要记录的大概内容：
例如：随着人工智能的不断发展，机器学习这门技术也越来越重要，很多人都开启了学习机器学习，本文就介绍了机器学习的基础内容。

---

提示：以下是本篇文章正文内容，下面案例可供参考

一、机器人舵机

1.硬件连接

通过官方Debugger调试器、USB转串口模块

2.驱动程序

通讯协议：UART
语言：python
指令异步发送

引入库

import serial
import serial.tools.list_ports
import time

通讯类

class Communication():
    # 初始化
    def __init__(self, com, bps, timeout):
        self.port = com
        self.bps = bps
        self.timeout = timeout
        global Ret
        try:
            # 打开串口，并得到串口对象
            self.main_engine = serial.Serial(self.port, self.bps, timeout=self.timeout)
            # 判断是否打开成功
            if (self.main_engine.is_open):
                print("---打开成功---")
                Ret = True
        except Exception as e:
            print("---异常---：", e)

    # 打印设备基本信息
    def Print_Name(self):
        print(self.main_engine.name)  # 设备名字
        print(self.main_engine.port)  # 读或者写端口
        print(self.main_engine.baudrate)  # 波特率
        print(self.main_engine.bytesize)  # 字节大小
        print(self.main_engine.parity)  # 校验位
        print(self.main_engine.stopbits)  # 停止位
        print(self.main_engine.timeout)  # 读超时设置
        print(self.main_engine.writeTimeout)  # 写超时
        print(self.main_engine.xonxoff)  # 软件流控
        print(self.main_engine.rtscts)  # 软件流控
        print(self.main_engine.dsrdtr)  # 硬件流控
        print(self.main_engine.interCharTimeout)  # 字符间隔超时

    # 打开串口
    def Open_Engine(self):
        self.main_engine.open()

    # 关闭串口
    def Close_Engine(self):
        self.main_engine.close()
        print(self.main_engine.is_open)  # 检验串口是否打开

    # 打印可用串口列表
    @staticmethod
    def Print_Used_Com():
        port_list = list(serial.tools.list_ports.comports())
        print(port_list)

    # 接收指定大小的数据
    # 从串口读size个字节。如果指定超时，则可能在超时后返回较少的字节；如果没有指定超时，则会一直等到收完指定的字节数。
    def Read_Size(self, size):
        return self.main_engine.read(size=size)

    # 接收一行数据
    # 使用readline()时应该注意：打开串口时应该指定超时，否则如果串口没有收到新行，则会一直等待。
    # 如果没有超时，readline会报异常。
    def Read_Line(self):
        return self.main_engine.readline()

    # 发数据
    def Send_data(self, data):
        #self.main_engine.write(chr(data).encode("utf-8"))
        self.main_engine.write(data)
    # 更多示例
    #self.main_engine.write(chr(0x06).encode("utf-8")) # 十六制发送一个数据
    # print(self.main_engine.read().hex()) # # 十六进制的读取读一个字节
    # print(self.main_engine.read())#读一个字节
    # print(self.main_engine.read(10).decode("gbk"))#读十个字节
    # print(self.main_engine.readline().decode("gbk"))#读一行
    # print(self.main_engine.readlines())#读取多行，返回列表，必须匹配超时（timeout)使用
    # print(self.main_engine.in_waiting)#获取输入缓冲区的剩余字节数
    # print(self.main_engine.out_waiting)#获取输出缓冲区的字节数
    # print(self.main_engine.readall())#读取全部字符。

    # 接收数据
    # 一个整型数据占两个字节
    # 一个字符占一个字节

    def Recive_data(self, way):
        # 循环接收数据，此为死循环，可用线程实现
        flag = 1
        print("开始接收数据：")
        while flag:
            try:
                # 一个字节一个字节的接收
                if self.main_engine.in_waiting:
                    if (way == 0):
                        for i in range(self.main_engine.in_waiting):
                            print("接收ascii数据：" + str(self.Read_Size(1)))
                            data1 = self.Read_Size(1).hex()  # 转为十六进制
                            #data2 = int(data1, 16)  # 转为十进制
                            if (data2 == "exit"):  # 退出标志
                                break
                            else:
                                print("收到数据十六进制：" + data1 + " 收到数据十进制：" + str(data2))
                    if (way == 1):
                        # 整体接收
                        #data = self.main_engine.read(self.main_engine.in_waiting).decode("utf-8")#方式一
                        #data = self.main_engine.read_all()  # 方式二
                        data = self.Read_Size(1).hex()
                        if (data == "exit"):  # 退出标志
                            flag = 0
                            break
                        else:
                            print("接收HEX数据：", data)
            except Exception as e:
                print("异常报错：", e)

指令类：

class Instructions():
    def __init__(self,ID):
        self.ID = ID
        self.head = [0XFF,0XFF]
    def Command(self,Instruction,Parameter):
        Data_Length = [len(Parameter)+2]
        Data_Check_Sum = [abs(255-(sum(self.ID) + sum(Data_Length) + sum(Instruction) + sum(Parameter)))]
        data = self.head+self.ID+Data_Length+Instruction+Parameter+Data_Check_Sum
        return data

舵机控制函数

def Servo_move(ID,angle):
    Servo = Instructions([ID])
    angle_hex_h = int(angle/75)
    angle_hex_l = angle%75
    par = [0X1E, angle_hex_l, angle_hex_h]
    data = bytes(Servo.Command([0X04],par))
    print("send:", data)
    for i in range(3):
        Engine1.Send_data(data)
    #Engine1.Recive_data(1)

异步命令激活函数

def Active():
    data = bytes([0XFF, 0XFF, 0XFE, 0X02, 0X05, 0XFA])
    print("执行")
    for i in range(3):
        Engine1.Send_data(data)
    #time.sleep(3)

例子：

Communication.Print_Used_Com()
Ret = False  # 是否创建成功标志
#建立连接
Engine1 = Communication("com3", 1000000, 0.5)
#初始化舵机
Servo_0 = Instructions([0X00])
if (Ret):
    angle = 0
    while True:
        angle = angle+200
        if angle>200:
            angle = 0
        Servo_move(0,angle)#控制0号舵机，转动angle的角度
        Active()#告诉所有舵机执行指令

二、人脸检测

1.引入库

# coding=utf-8
# import the necessary packages
import time
import cv2 as cv
import numpy as np

2.加载模型

提前将模型文件保存到和代码同一文件夹中

# 摄像头
cap = cv.VideoCapture(0)
# 加载 model
face_cascade=cv.CascadeClassifier('haarcascade_frontalface_default.xml')

3.检测并框出人脸

# 画框
def draw_rectangle(img, rect):
    for x,y,w,h in rect:
        cv.rectangle(img, (x, y), (x+w, y+h), (128, 128, 0), 2)

def detect_face(image):
    results=face_cascade.detectMultiScale(gray,1.1,5)
    if results != ():
        for x,y,w,h in results:
            return results,gray[y:y+h, x:x+w]
    else:
        return [[0, 0, 0, 0]], None
while(cap.isOpened()):
    # USB摄像头工作时,读取一帧图像
    ret, image = cap.read()
    gray = cv.cvtColor(image, cv.COLOR_BGR2GRAY)#获取灰度图
    rect,face_img = detect_face(gray)
    # 输出人脸框 及相关的文字信息
    draw_rectangle(image, rect)
    cv.imshow("Frame", image)
    if key == ord("q"):
        break
# 释放资源和关闭窗口
cap.release()
cv.destroyAllWindows()

三、小球（圆形物体）检测

1.思路

小球（圆形物体）检测4种途径：
1.霍夫圆检测
2.利用opencv检测器，训练建立小球检测器
3.使用深度学习的预训练模型（paddlepaddle、tensorflow、caffe）， opencv可以直接推理caffe模型
4.调用百度通用物体检测API

2.引入库

import cv2
import cv2 as cv

2.检测并框出小球（霍夫圆检测）

def circel_detect(img):
    # 转换为灰度图
    img_gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # medianBlur 平滑（模糊）处理
    img_gray = cv2.medianBlur(img_gray, 7)
    # 圆检测
    circles = cv2.HoughCircles(img_gray, cv2.HOUGH_GRADIENT, 1, 200, param1=65, param2=60, minRadius=50, maxRadius=200)
    return circles

霍夫圆检测算法原理介绍.

while (True):
    rect, img = cap.read()
    (h, w) = img.shape[:2]
    center = [w / 2, h / 2]
    circles = circel_detect(img)
    if circles is None:
        pass
    else:
        circles = np.uint16(np.around(circles))
        for i in circles[0, :]:
            # 勾画正方形，origin图像、i[2]*2是边长
            cv2.rectangle(img, (i[0] - i[2], i[1] - i[2]), (i[0] + i[2], i[1] + i[2]), (255, 0, 0), 2)
            cv2.circle(img, (i[0],i[1]), 2, (0, 0, 255), 4)
            cv2.line(img, (i[0],i[1]), (int(center[0]),int(center[1])), (0, 255, 0), 2)
            cv2.circle(img, (int(center[0]),int(center[1])), 5, (0, 0, 255), 4)
	cv.imshow('img', img)
    k = cv.waitKey(30) & 0xff
    if k == 27:
        break
    elif k == 32:
        print('s')
# 释放资源和关闭窗口
cap.release()
cv.destroyAllWindows()

效果图：

四、控制机械臂

1.人脸跟随代码（不包含指令类和通讯类）

# 打印接口信息
Communication.Print_Used_Com()
Ret = True  # 是否创建成功标志
# cds = CDS(dev="/dev/ttyUSB0")#Linux系统
cds = CDS(dev="com4")  # windows系统

cds.set_moter_mode(1)  # 设置1号舵机为电机模式
cds.set_moter_mode(2)  # 设置2号舵机为电机模式

# dnn模型
model = cv.dnn.readNetFromCaffe("model/deploy.prototxt", "model/res10_300x300_ssd_iter_140000.caffemodel")
# cascade人脸检测器
# face_cascade = cv.CascadeClassifier('haarcascade_frontalface_default.xml')



cap = cv.VideoCapture(0, cv.CAP_DSHOW)  # 笔记摄像头
last_face_pos = np.array([0, 0])
while (cap.isOpened()):
    _, img = cap.read()
    # img_h, img_w, _ = image.shape
    (h, w) = img.shape[:2]
    center = [w / 2, h / 2]

    # gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    image_blob = cv.dnn.blobFromImage(
        cv.resize(img, (300, 300)), 1.0, (300, 300),
        (104.0, 177.0, 123.0), swapRB=False, crop=False
    )

    model.setInput(image_blob)
    detections = model.forward()
    no_face = True
    face_idx = 0
    min_len = w * h
    for i in range(0, detections.shape[2]):
        confidence = detections[0, 0, i, 2]
        # filter out weak detections with less than 50% confidence
        box = detections[0, 0, i, 3:7] * np.array([w, h, w, h])
        (startX, startY, endX, endY) = box.astype("int")
        face_center = np.array([(startX + endX) / 2, (startY + endY) / 2])
        if confidence > 0.8 and endX < w and endY < h:
            no_face = False
            cur_face_len = np.sqrt(np.sum(np.square(face_center - last_face_pos)))
            if cur_face_len < min_len:
                face_idx = i
                min_len = cur_face_len
                cv.rectangle(img, (startX, startY), (endX, endY), (0, 255, 255))

    # faces = face_cascade.detectMultiScale(gray, 1.3, 5)
    if not no_face:
        box = detections[0, 0, face_idx, 3:7] * np.array([w, h, w, h])
        (startX, startY, endX, endY) = box.astype("int")
        face_pos = [int((startX + endX) / 2), int((startY + endY) / 2)]
        cv2.circle(img, (face_pos[0], face_pos[1]), 2, (0, 0, 255), 4)
        cv2.line(img, (face_pos[0], face_pos[1]), (int(center[0]), int(center[1])), (0, 255, 0), 2)
        cv2.circle(img, (int(center[0]), int(center[1])), 5, (0, 0, 255), 4)

        det_x = face_pos[0] - center[0]
        det_y = face_pos[1] - center[1]
        speed_x = int(abs(det_x) * 1.1)
        speed_y = int(abs(det_y) * 1.0)
        speed_x = min([speed_x, 200])
        speed_y = min([speed_y, 200])
        print("speed_x:",speed_x,"speed_y:",speed_y)
        cds.set_speed(1, speed_x, det_x < 0)
        cds.set_speed(2, speed_y, det_y > 0)
        #cds.Active()

        last_face_pos = np.array(face_pos)
    else:

        cds.set_speed(1, 0)
        cds.set_speed(2, 0)


    img = cv.flip(img, 1)
    cv.imshow('img', img)
    k = cv.waitKey(30) & 0xff
    if k == 27:
        break
# 释放资源和关闭窗口
cap.release()
cv.destroyAllWindows()

效果图：

2.小球（圆形物体）跟随代码（不包含指令类和通讯类）

# 打印接口信息
Communication.Print_Used_Com()
Ret = True  # 是否创建成功标志
# cds = CDS(dev="/dev/ttyUSB0")#Linux系统
cds = CDS(dev="com4")  # windows系统

cds.set_moter_mode(1)  # 设置1号舵机为电机模式
cds.set_moter_mode(2)  # 设置2号舵机为电机模式

cap = cv.VideoCapture(0,cv.CAP_DSHOW)#笔记摄像头

def circel_detect(origin):
    # 转换为灰度图
    img_gray = cv2.cvtColor(origin, cv2.COLOR_BGR2GRAY)
    # medianBlur 平滑（模糊）处理
    img_gray = cv2.medianBlur(img_gray, 7)
    # 圆检测
    circles = cv2.HoughCircles(img_gray, cv2.HOUGH_GRADIENT, 1, 200, param1=65, param2=60, minRadius=50, maxRadius=200)
    return circles

max_speed = 300
while (True):
    rect, img = cap.read()
    (h, w) = img.shape[:2]
    center = [w / 2, h / 2]
    param_x_speed = 300 / center[0]
    param_y_speed = 300 / center[1]
    circles = circel_detect(img)
    if circles is None:
        cds.set_speed(1, 0)
        cds.set_speed(2, 0)
        #print("speed_x:", 0, "speed_y:", 0)
        pass
    else:
        circles = np.uint16(np.around(circles))
        for i in circles[0, :]:
            # 勾画正方形，origin图像、i[2]*2是边长
            cv2.rectangle(img, (i[0] - i[2], i[1] - i[2]), (i[0] + i[2], i[1] + i[2]), (255, 0, 0), 2)
            cv2.circle(img, (i[0],i[1]), 2, (0, 0, 255), 4)
            cv2.line(img, (i[0],i[1]), (int(center[0]),int(center[1])), (0, 255, 0), 2)
            cv2.circle(img, (int(center[0]),int(center[1])), 5, (0, 0, 255), 4)

            det_x = i[0] - center[0]
            det_y = i[1] - center[1]

            speed_x = int(abs(det_x) * 1.1)
            speed_y = int(abs(det_y) * 1.0)
            speed_x = min([speed_x, 200])
            speed_y = min([speed_y, 200])
        print("speed_x:", speed_x, "speed_y:", speed_y)
        cds.set_speed(1, speed_x, det_x < 0)
        cds.set_speed(2, speed_y, det_y > 0)

    cv.imshow('img', img)
    k = cv.waitKey(30) & 0xff
    if k == 27:
        break
    elif k == 32:
        print('s')
# 释放资源和关闭窗口
cap.release()
cv.destroyAllWindows()