运行效果:
源码:
# 导入必要的模块
from libs.PipeLine import PipeLine, ScopedTiming
from libs.AIBase import AIBase
from libs.AI2D import Ai2d
import os
import ujson
from media.media import *
from time import *
import nncase_runtime as nn
import ulab.numpy as np
import time
import utime
import image
import random
import gc
import sys
import aidemo
from machine import Pin, PWM, FPIOA
# === 配置舵机引脚 ===
# 配置引脚42为PWM0功能
fpioa = FPIOA()
fpioa.set_function(42, FPIOA.PWM0)
# 构建舵机PWM对象，通道0，频率为50Hz（对于标准舵机）
S1 = PWM(0, 50, duty=0, enable=True) # 初始位置设置为中点，即大约0度
# 配置引脚43为PWM1功能
fpioa.set_function(43, FPIOA.PWM1)
# 构建舵机PWM对象，通道1，频率为50Hz（对于标准舵机），负责低头抬头仰头
S2 = PWM(1, 50, duty=0, enable=True) # 初始位置设置为中点，即大约90度
# 初始化X轴和Y轴当前位置
x_position = 0 # X轴初始位置为中间位置
y_position = 0 # Y轴初始位置为中间位置
# 获取当前帧率
clock = time.clock()
# === 设置舵机角度的函数 ===
def set_servo_angle(servo, angle):
"""设置指定舵机的角度。"""
angle = max(-90, min(90, angle))
duty = ((angle + 90) / 180 * 10 + 2.5)
servo.duty(duty)
# === 增量式PID控制器类 ===
class IncrementalPID:
"""PID控制器类，用于增量式PID控制。"""
def __init__(self, kp, ki, kd, imax):
self.kp = kp # 比例系数
self.ki = ki # 积分系数
self.kd = kd # 微分系数
self.imax = imax # 积分最大值
self.last_error = 0
self.integrator = 0
self.last_derivative = 0
self.last_output = 0
def compute(self, error, dt):
"""计算PID输出。"""
p_term = self.kp * error # 比例项
self.integrator += self.ki * error * dt # 积分项
self.integrator = max(-self.imax, min(self.imax, self.integrator))
d_term = (error - self.last_error) / dt # 微分项
self.last_error = error
output = p_term + self.integrator + self.kd * d_term
self.last_output = output
return output
def reset(self):
"""重置PID控制器状态。"""
self.integrator = 0
self.last_error = 0
# === 更新舵机位置（使用PID控制） ===
def update_position_pid(servo_x, servo_y, position_x, position_y, target_x, target_y, pid_controller_x, pid_controller_y):
"""更新舵机位置，使目标位置接近当前位置。"""
global x_position, y_position
error_x = target_x - position_x
error_y = target_y - position_y
dt = 1 / clock.fps() # 动态时间间隔
if dt < 0.02: # 至少20ms
return
output_x = pid_controller_x.compute(error_x, dt)
output_y = pid_controller_y.compute(error_y, dt)
# 限制每次角度调整的最大值
delta_angle_x = min(max(output_x, -20), 20) # 角度限制
delta_angle_y = min(max(output_y, -30), 30) # 角度限制
# 增加死区，仅当输出值大于某个阈值时才进行调整
if abs(delta_angle_x) < 1: # 仅当输出值大于1度时才调整
delta_angle_x = 0
if abs(delta_angle_y) < 1: # 仅当输出值大于1度时才调整
delta_angle_y = 0
# 只有当坐标变化超过阈值时才更新舵机位置
if abs(error_x) > 15 or abs(error_y) > 15:
# 更新舵机位置
position_x += delta_angle_x
position_y += delta_angle_y
position_x = max(-90, min(90, position_x))
position_y = max(-40, min(40, position_y)) # 限制Y轴舵机角度在-40到40度之间
set_servo_angle(servo_x, position_x)
set_servo_angle(servo_y, position_y)
x_position = position_x
y_position = position_y
# 打印调试信息
print(f" X轴误差: {error_x}, X轴输出: {output_x}, 新X轴位置: {position_x + delta_angle_x}")
print(f" Y轴误差: {error_y}, Y轴输出: {output_y}, 新Y轴位置: {position_y + delta_angle_y}")
return False # 表示坐标变化较大
else:
return True # 表示坐标变化较小
# === 自定义人脸检测类 ===
class FaceDetectionApp(AIBase):
"""自定义人脸检测应用类。"""
def __init__(self, kmodel_path, model_input_size, anchors, confidence_threshold=0.5, nms_threshold=0.2, rgb888p_size=[224, 224], display_size=[800, 480], debug_mode=0):
super().__init__(kmodel_path, model_input_size, rgb888p_size, debug_mode)
self.kmodel_path = kmodel_path
self.model_input_size = model_input_size
self.confidence_threshold = confidence_threshold
self.nms_threshold = nms_threshold
self.anchors = anchors
self.rgb888p_size = [ALIGN_UP(rgb888p_size[0], 16), rgb888p_size[1]]
self.display_size = [ALIGN_UP(display_size[0], 16), display_size[1]]
self.debug_mode = debug_mode
self.ai2d = Ai2d(debug_mode)
self.ai2d.set_ai2d_dtype(nn.ai2d_format.NCHW_FMT, nn.ai2d_format.NCHW_FMT, np.uint8, np.uint8)
def config_preprocess(self, input_image_size=None):
"""配置预处理参数。"""
with ScopedTiming("set preprocess config", self.debug_mode > 0):
ai2d_input_size = input_image_size if input_image_size else self.rgb888p_size
top, bottom, left, right = self.get_padding_param()
self.ai2d.pad([0, 0, 0, 0, top, bottom, left, right], 0, [104, 117, 123])
self.ai2d.resize(nn.interp_method.tf_bilinear, nn.interp_mode.half_pixel)
self.ai2d.build([1, 3, ai2d_input_size[1], ai2d_input_size[0]], [1, 3, self.model_input_size[1], self.model_input_size[0]])
def postprocess(self, results):
"""后处理检测结果。"""
with ScopedTiming("postprocess", self.debug_mode > 0):
post_ret = aidemo.face_det_post_process(self.confidence_threshold, self.nms_threshold, self.model_input_size[1], self.anchors, self.rgb888p_size, results)
if len(post_ret) == 0:
return post_ret
else:
return post_ret[0]
def draw_result(self, pl, dets):
"""在屏幕上绘制检测结果。"""
with ScopedTiming("display_draw", self.debug_mode > 0):
if dets:
pl.osd_img.clear()
faces = []
for det in dets:
x, y, w, h = map(lambda x: int(round(x, 0)), det[:4])
x = x * self.display_size[0] // self.rgb888p_size[0]
y = y * self.display_size[1] // self.rgb888p_size[1]
w = w * self.display_size[0] // self.rgb888p_size[0]
h = h * self.display_size[1] // self.rgb888p_size[1]
pl.osd_img.draw_rectangle(x, y, w, h, color=(255, 255, 0, 255), thickness=2)
text_left_top = f"({x}, {y})"
pl.osd_img.draw_string_advanced(x, y - 25, 20, text_left_top, color=(255, 255, 255))
faces.append((x, y, w, h))
return faces
else:
pl.osd_img.clear()
return []
def get_padding_param(self):
"""获取填充参数。"""
dst_w = self.model_input_size[0]
dst_h = self.model_input_size[1]
ratio_w = dst_w / self.rgb888p_size[0]
ratio_h = dst_h / self.rgb888p_size[1]
ratio = min(ratio_w, ratio_h)
new_w = int(ratio * self.rgb888p_size[0])
new_h = int(ratio * self.rgb888p_size[1])
dw = (dst_w - new_w) / 2
dh = (dst_h - new_h) / 2
top = int(round(0))
bottom = int(round(dh * 2 + 0.1))
left = int(round(0))
right = int(round(dw * 2 - 0.1))
return top, bottom, left, right
def main_loop(self):
"""主循环函数。"""
clock = time.clock()
set_servo_angle(S1, 0) # 初始化X轴舵机
set_servo_angle(S2, 0) # 初始化Y轴舵机
try:
while True:
os.exitpoint()
clock.tick()
img = pl.get_frame()
res = self.run(img)
faces = self.draw_result(pl, res)
if faces:
face = faces[0]
face_center_x = face[0] + face[2] / 2
face_center_y = face[1] + face[3] / 2
screen_center_x = 400 # 已知屏幕中心x坐标
screen_center_y = 240 # 已知屏幕中心y坐标
error_x = face_center_x - screen_center_x
error_y = face_center_y - screen_center_y
global target_x, target_y
target_x = x_position - error_x / screen_center_x * 90
target_y = y_position - error_y / screen_center_y * 60
target_x = max(-90, min(90, target_x))
target_y = max(-60, min(60, target_y))
update_position_pid(S1, S2, x_position, y_position, target_x, target_y, pid_controller_x, pid_controller_y)
pl.show_image()
gc.collect()
except Exception as e:
sys.print_exception(e)
# === 初始化PID控制器 ===
# 创建增量式PID控制器实例
# P (比例项): 0.10, 对应误差的比例反馈
# I (积分项): 0.00, 积分作用用于消除静态误差，此处设置为0表示暂时不启用积分作用
# D (微分项): 0.00, 微分作用用于预测系统未来的误差，同样设置为0表示不启用
# imax (积分限幅): 0, 控制积分项的最大累积值，防止积分饱和，这里设置为0表示无限制
pid_controller_x = IncrementalPID(0.10, 0.00, 0.00, 0)
pid_controller_y = IncrementalPID(0.08, 0.00, 0.7, 0)
# === 主程序 ===
if __name__ == "__main__":
display_mode = "lcd"
display_size = [800, 480]
kmodel_path = "/sdcard/app/tests/kmodel/face_detection_320.kmodel"
confidence_threshold = 0.5
nms_threshold = 0.2
anchor_len = 4200
det_dim = 4
anchors_path = "/sdcard/app/tests/utils/prior_data_320.bin"
anchors = np.fromfile(anchors_path, dtype=np.float)
anchors = anchors.reshape((anchor_len, det_dim))
rgb888p_size = [800, 480]
pl = PipeLine(rgb888p_size=rgb888p_size, display_size=display_size, display_mode=display_mode)
pl.create()
face_det = FaceDetectionApp(kmodel_path, model_input_size=[320, 320], anchors=anchors, confidence_threshold=confidence_threshold, nms_threshold=nms_threshold, rgb888p_size=rgb888p_size, display_size=display_size, debug_mode=0)
face_det.config_preprocess()
try:
face_det.main_loop()
except Exception as e:
sys.print_exception(e)
finally:
face_det.deinit() # 反初始化
pl.destroy() # 销毁PipeLine实例
配套使用硬件:
舵机使用了这个店铺得20KG-PWM数字舵机
