机器视觉运动控制系列3：摄像头控制舵机小记

发表于 阅读次数：

  本篇是该系列历史研究的最后一块组件。把两个部分的内容连起来，并用python实现了控制机械臂舵机转动保持一个人脸始终在摄像头中部的目标。

  前面记了有face_recognition，跑通了Arduino控制舵机，现在把他们连在一起。
  先从python一侧入手，串口通信首先需要安装pyserial。分析下目标问题和解决方案：我想让我的机械臂追随着某个人，现在可以将摄像头固定在机械臂上，不停的在我的摄像头中识别人脸，如果识别到我要找的目标而他不在中间位置的话，发控制使机械臂对应转动调整就OK了。对单一舵机发出控制命令（就选之前转动底盘的12号位舵机吧），下面为python端源码：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
# 控制一个舵机锁定人脸初版，对应ino为ctrl1servo
import face_recognition
import cv2
import time
import threading
import serial

# 这个是摄像头的宽度分辨率
framewidth = 640
# 注意串口号对应                
ser = serial.Serial('COM5', 9600)
# 摄像头张角45度，那么中间的位置就是23度
s = 23
slast = 23


class MyThread(threading.Thread):
    def __init__(self, func, name=''):
        threading.Thread.__init__(self)
        self.name = name
        self.func = func

    def run(self):
        if self.name == "send":
            matter1()
        elif self.name == "change":
            matter2()


def matter1():
    while True:
        global s
        global slast
        # 识别发现位置有变化时发送转动指令
        if (s != slast):
            ser.write(bytes(str(s), encoding="utf8"))
            print(s)
        slast = s
        # 两秒内只发送一次，避免机械臂停不下来的问题
        time.sleep(2)


def matter2():
    global s
    video_capture = cv2.VideoCapture(0)

    obama_image = face_recognition.load_image_file("obama.jpg")
    obama_face_encoding = face_recognition.face_encodings(obama_image)[0]
    lijiawei_image = face_recognition.load_image_file("lijiawei.jpg")
    lijiawei_face_encoding = face_recognition.face_encodings(lijiawei_image)[0]
    known_face_encodings = [
        obama_face_encoding,
        lijiawei_face_encoding]
    known_face_names = [
        "Obama",
        "lijiawei"]

    num = 0
    process_this_frame = True
    while True:
        ret, frame = video_capture.read()

        small_frame = cv2.resize(frame, (0, 0), fx=0.25, fy=0.25)
        rgb_small_frame = small_frame[:, :, ::-1]

        if process_this_frame:
            face_locations = face_recognition.face_locations(rgb_small_frame)
            face_encodings = face_recognition.face_encodings(rgb_small_frame, face_locations)
        process_this_frame = not process_this_frame

        for (top, right, bottom, left), face_encoding in zip(face_locations, face_encodings):
            top *= 4
            right *= 4
            bottom *= 4
            left *= 4

            # s定位是现在人脸的位置
            s = int((left + right - 1) / framewidth * 45 / 2)

            matches = face_recognition.compare_faces(known_face_encodings, face_encoding)
            name = "Unknown"
            for i in range(len(known_face_names)):
                if matches[i]:
                    name = known_face_names[i]
                else:
                    pass

            cv2.putText(frame, "cam", (50, 100), cv2.FONT_HERSHEY_DUPLEX, 1, (0, 0, 255), 1)
            num = num + 1
            filename = "output/frames_%s.jpg" % num
            cv2.imwrite(filename, frame)

            cv2.rectangle(frame, (left, top), (right, bottom), (0, 0, 255), 2)
            cv2.rectangle(frame, (left, bottom - 35), (right, bottom), (0, 0, 255), cv2.FILLED)
            font = cv2.FONT_HERSHEY_DUPLEX
            cv2.putText(frame, name, (left + 6, bottom - 6), font, 1.0, (255, 255, 255), 1)

        cv2.imshow('Video', frame)

        if cv2.waitKey(1) & 0xFF == ord('q'):
            break

    video_capture.release()
    cv2.destroyAllWindows()


if __name__ == '__main__':
    # 一个线程发送控制信号，一个线程专门做识别
    thing1 = MyThread(matter1, "send")
    thing2 = MyThread(matter2, "change")
    thing1.start()
    thing2.start()
    thing1.join()
    thing2.join()
    # 断开连接
    ser.close()

  这样python会把当前目标在摄像头中的角度位置传入arduino，这边对应的控制处理方法ctrl1servo.ino为：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
#include <Servo.h> 
Servo myservo;  
int pos = 90;
int poslast = 90;
//串口接收角度数据，转int
int nextmove = 0;
char line[100] = "";
void setup() 
{
  Serial.begin(9600); 
  myservo.attach(12);
  myservo.write(pos);
}

void loop() 
{
  poslast = pos;
  if (Serial.available() > 0) 
  {
    nextmove = Serial.readBytesUntil('\n', line, 500);
    nextmove = atoi(line);
    for(int i = 0;i < 5;i++)
    {
      line[i]='\0';
    }
    Serial.println(nextmove);   
    
   //如果人脸中心在15到30度之间的话可以接受，就不用转动了
   if(nextmove > 30)
   {
    for(int i = 0;i < nextmove - 22;i += 1) 
    {
     pos -= 1;
     pos=constrain(pos,0,180);
     if(!(pos == poslast))
     {
      myservo.write(pos);
      delay(20);
     }
    }
   }
   if(nextmove < 15)
   {
    for(int i = 0;i < 23 - nextmove;i += 1) 
    {
     pos += 1;
     pos=constrain(pos,0,180);
     if(!(pos == poslast))
     {
      myservo.write(pos);
      delay(20);
     }
    }
   }
  } 
}

  之前做的这个项目想法还是很有意思的，基本功能跑通算是实现了，但效果上看仍有很大的改进空间，结果boss突然出了些事情，出师未捷吧哈哈瞬间没了动力，哎。。。有时间了有新的想法的话我再把他捡起来继续完善和优化吧。