tello-人臉偵測+無人機跟隨

先到github下載人臉偵測的程式庫

haarcascade_frontalface_default.xml

開一個faceTracking.py的檔和上面程式放在一起

因為PID的部份我還沒弄懂，所以暫時保留不紀錄

等我搞懂後再來補充

概念

無人機跟隨是利用人臉偵測數值的大小來判斷前進或後退

例如:人臉的框離預設的值較遠，這時無人機就必須跟進，

使其保持預設值；反之人臉的框離預設值較近，無人機就

必須向後，向左向右同理。

以下是程式碼

import cv2
import numpy as np
from djitellopy import tello
from time import sleep

#無人機初始化,開始時升高到一定的程度
mytello = tello.Tello()
mytello.connect() #wifi連線
mytello.streamon() #開啓視訊鏡頭
mytello.takeoff() #起飛
print("電池格數剩: {}%".format(mytello.get_battery()))
mytello.send_rc_control(0, 0, 25, 0) #上升至一定的高度
sleep(2)

w, h = 360, 240
fbRange = [6200, 6800]  #前後空間規範
pid = [0.4, 0.4, 0]
pError = 0

#人臉偵測
def findFace(img):
    faceCascade = cv2.CascadeClassifier(r"C:\Users\blair\PycharmProjects\tello/haarcascade_frontalface_default.xml")
    imgGray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    faces = faceCascade.detectMultiScale(imgGray, 1.1, 8)

    myFaceList = []
    myFaceListArea = []

    for (x, y, w, h) in faces:
        cv2.rectangle(img, (x,y), (x+w, y+h), (0, 0, 255), 2)
        cx = x + w // 2
        cy = y + h // 2
        area = w * h
        myFaceList.append([cx, cy])
        myFaceListArea.append(area)
        cv2.circle(img, (cx, cy), 5, (0, 255, 0), cv2.FILLED)
    #找出最大值,預防有多張人臉時不誤判
    if len(myFaceListArea) != 0:
        i = myFaceListArea.index(max(myFaceListArea))
        return img, [myFaceList[i], myFaceListArea[i]]
    else:
        return img, [[0, 0], 0]

#根據pid調節擺動姿態
def trackFace(info, w, pid, pErro):
    fb = 0
    area = info[1]
    x, y = info[0]
    error = x - w // 2
    speed = pid[0] * error + pid[1] * (error - pErro)
    speed = int(np.clip(speed, -100, 100))
    #如果在規範的區域內無人機就靜止不動
    if area > fbRange[0] and area < fbRange[1]:
        fb = 0
    #如果小於規範最大值(臉部距離無人機很近),無人機後退
    elif area > fbRange[1]:
        fb = -20
    # 如果大於規範最大值(臉部距離無機很遠),無人機前近
    elif area < fbRange[0] and area != 0:
        fb = 20

    if x == 0:
        speed = 0
        error = 0
    #print(speed, fb)

    mytello.send_rc_control(0, fb, 0, speed)
    return error

#cap = cv2.VideoCapture(0)

while True:
    #_, img = cap.read()
    img = mytello.get_frame_read().frame
    img = cv2.resize(img, (w, h))
    img, info = findFace(img)
    pError = trackFace(info, w, pid, pError)
    #print("Center: {} //Area: {}".format(info[0], info[1]))
    cv2.imshow("Image", img)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        mytello.land()
        break

實體視角

鏡頭視角