jetbot12 之 人手目标跟踪(hand tracking)

有了目标检测 再结合 云台舵机 PID 算法可以实现简单的 目标跟踪:

注: PID 算法源码是机器人开发板厂家提供， 以下是简单原理介绍：

P - 线性控制量 , I - 积分参数 ， D - 微分参数

自动控制系统的经典算法， 要让舵机到达目标位置Y ，   首要是要给系统一个线性作用力，控制量和移动距离体现为比例系数P。 当然这个作用力的结果会有偏差。

积分参数对于产生的偏差起到滞后的纠正作用。

微分参数通过偏差的变化速度提前预估未来的偏差以起到尽早纠正偏差的作用。能加快系统响应。

PID有严谨的理论推导，和标准的 控制算法， 对于给定的动力系统， P , I , D 参数要通过调较来获得。

 

可以看看百度里的介绍:

https://baike.baidu.com/item/PID%E7%AE%97%E6%B3%95/4660106?fr=aladdin

 

这里的目标跟踪是不严格的， 只是通过简单的检测定位算法 来控制镜头云台转动到目标的中心，起到简单的跟踪效果。 出现多个人手时会产生跟踪的叛变。。

 

jetbot hand detect and tracking

 

目标检测使用了 yolov5s 训练的 人脸 、带口罩人脸、人手  三种分类目标的模型。 并且转了 tensorRT 在 jetson nano 下运行，可以到 640x360@20fps需要的可以私信我。

代码实现:

#!/usr/bin/python3
# -*- coding:utf-8 -*-
import sys 

sys.path.append('/workspace/hugo_py')

import cv2 
import numpy as np
import time
from camera import JetCamera
import traceback
import TRTYolov5 as fd
import PID 

cap_w = 640 
cap_h = 360 
cap_fps = 30

# 云台 PID 控制
target_valuex=2100
target_valuey=2048

xservo_pid = PID.PositionalPID(1.9, 0.3, 0.35)
yservo_pid = PID.PositionalPID(1.5, 0.2, 0.3)

# 云台控制总线
from servoserial import ServoSerial
servo_device = ServoSerial()


def ptz_reset():
    global target_valuex, target_valuey
    target_valuex = 2100
    target_valuey = 2048
    servo_device.Servo_serial_double_control(1, target_valuex, 2,  target_valuey)

    time.sleep(0.1)





def main():
    global target_valuex, target_valuey, xservo_pid, yservo_pid
    ptz_reset()

    cam = JetCamera(cap_w, cap_h, cap_fps)

    engine = fd.create('../build/yolov5s.engine')

   out_win = "foo"
    cv2.namedWindow(out_win, cv2.WINDOW_NORMAL)
    cv2.setWindowProperty(out_win, cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)


    cam.open()

    cnt = 0
    sum_time = 0
    while True:
        try:
            ret, frame = cam.read()
            #print("camera read one frame ")
            if not ret:
                break

            t0 = time.time()
            ret = fd.detect(engine, frame, 0.4)
            res = eval(ret)
            t1 = time.time()

            sum_time += t1 - t0
            cnt += 1

            if cnt % 150 == 0:
                print("frame cnt [%d]   detect delay = %.1fms" % (cnt, (sum_time) * 1000/ cnt ))
                sum_time = 0
                cnt = 0

            print(res)
            if   res['num_det'] > 0:
                for ret in res['objects']:
                    cls_id, x1, y1, x2, y2 = ret
                    color = {0:(0, 0, 255), 1:(255, 0, 0), 2:(0, 255, 0)}[cls_id]

                    cv2.rectangle(frame, (x1, y1), (x2, y2), color, 3)

                    if 2 == cls_id:
                        x_s, y_s, w_s, h_s = x1 * 300 / cap_w, y1 * 300 / cap_h, (x2 - x1) * 300 / cap_w , (y2 - y1 ) * 300 / cap_h


                        xservo_pid.SystemOutput = x_s + w_s / 2
                        xservo_pid.SetStepSignal(150)
                        xservo_pid.SetInertiaTime(0.01, 0.006)
                        target_valuex = int(2100 + xservo_pid.SystemOutput)

                        yservo_pid.SystemOutput = y_s + h_s / 2
                        yservo_pid.SetStepSignal(150)
                        yservo_pid.SetInertiaTime(0.01, 0.006)

                        target_valuey = int(2048 + yservo_pid.SystemOutput)


                        servo_device.Servo_serial_double_control(1, target_valuex, 2 , target_valuey)

            #    r = ret[2]
            #    #print("ret = %s, %s" % (ret, r))
            #    #cv2.rectangle(frame, (int(r[0]), int(r[1])), (int(r[2]), int(r[3])), (255, 255, 0))

            cv2.imshow(out_win, frame)
            cv2.waitKey(1)
        except:
            traceback.print_exc()
            break

    cam.close()


if __name__ == '__main__':
    main()