From 31da324c52f596a65a5c3a5bb6200ccbcc4bd5a3 Mon Sep 17 00:00:00 2001 From: yimiao Date: Thu, 28 May 2026 11:03:15 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B0=8F=E5=B0=8F=E7=9A=84=E4=BF=AE=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- control.py | 209 ++++++++++++++++++++++++----------------- infer_camera_direct.py | 18 ++-- 2 files changed, 132 insertions(+), 95 deletions(-) diff --git a/control.py b/control.py index e7f07c6..08c3250 100644 --- a/control.py +++ b/control.py @@ -1,103 +1,140 @@ import time from pymavlink import mavutil +from pymavlink.mavextra import integral + + +# 自定义 PID 部分 +class PID: + """ 将 PID 部分分离出来为一个类""" + def __init__(self, kp, ki, kd, max_out, min_out): + self.kp = kp + self.ki = ki + self.kd = kd + self.max_out = max_out + self.min_out = min_out + + self.last_err = 0.0 + self.integral = 0.0 + self.last_time = time.time() + + def update(self, error): + current_time = time.time() + dt = current_time - self.last_time + if dt <=0: dt = 0.033 # 30Hz + + p_out = self.kp * error + + self.integral = error * dt + i_out = self.ki * self.integral + i_out = max(min(i_out, self.max_out * 0.2), self.min_out * 0.2) + + d_out = self.kd * (self.last_err - error) / dt + + output = p_out + i_out + d_out + output = max(min(output, self.max_out), self.min_out) + + self.last_err = error + self.last_time = current_time + + return output + + def reset(self): + self.last_err = 0.0 + self.integral = 0.0 + + class UAVController: def __init__(self, port='/dev/ttyUSB0', baud=115200): - """初始化与飞控的 MAVLink 串口连接""" + """ + 初始化与飞控的 MAVLink 串口连接 + port:串口地址 + baud:波特率 + """ try: + print(f"正在尝试通过 MAVLink 连接飞控:{port}...") self.master = mavutil.mavlink_connection(port, baud=baud) print(f" [飞控通信] 已连接串口{port}, 请等待心跳包...") self.master.wait_heartbeat() print(" [飞控通信] 收到飞控心跳包, 链接正常") except Exception as e: print(f" [飞控通信] 连接失败:{e}, 请检查") + self.master = None - # =================== PID 参数 =================== - # 此时参数设定，err_x: 无人机的左右平移, err_y: 无人机的上下平移 (后续还要修改) - # =============================================== - self.kp = 1.2 # 响应强度 - self.ki = 0.02 # 消除风偏 - self.kd = 0.3 # 刹车减震 + # 初始化Y轴 (左右方向) 和Z轴 (上下方向) 的 PID 调节器 + self.vy = PID(kp=1.0, ki=0.02, kd=0.3, max_out=0.8, min_out=-0.8) + self.vz = PID(kp=1.0, ki=0.02, kd=0.3, max_out=0.4, min_out=-0.4) - # 内部缓存 - self.last_err_x = 0.0 - self.last_err_y = 0.0 - self.integral_x = 0.0 - self.integral_y = 0.0 - self.last_time = time.time() + # 视觉丢失保护 + self.lost_counter = 0 + self.LOST_THRESHOLD = 30 # 假设摄像头是30帧就对应1秒 - def update_and_send_command(self, err_x, err_y): - """通过输入图像误差, 计算 PID 并通过 MAVLink 发送指令""" - if self.master is None: - return - - current_time = time.time() - dt = current_time - self.last_time - if dt <=0: dt = 0.02 #防止除以0 - - # ------------ PID 计算 ------------- - # 1. 比例项 (p) - p_out_x = self.kp * err_x - p_out_y = self.ki * err_y - - # 2. 积分项 (I) - self.integral_x = max(-0.3, min(self.integral_x + err_x * dt, 0.3)) - self.integral_y = max(-0.3, min(self.integral_y + err_y * dt, 0.3)) - i_out_x = self.ki * self.integral_x - i_out_y = self.kd * self.integral_y - - # 3. 微分项 (D) - d_out_x = self.kp * (err_x - self.last_err_x) / dt - d_out_y = self.kp * (err_y - self.last_err_y) / dt - - # 4. 总输出 (映射为物理速度: m/s) - vel_x_cmd = p_out_x + i_out_x + d_out_x - vel_y_cmd = p_out_y + i_out_y + d_out_y - - # ------------- 安全限速 -------------- - # 限制下速度，怕测试时飞太快 - MAX_VEL = 1.0 - vel_x_cmd = max(min(MAX_VEL, vel_x_cmd), -MAX_VEL) - vel_y_cmd = max(min(MAX_VEL, vel_y_cmd), -MAX_VEL) - - # 更新缓存 - self.last_err_x = err_x - self.last_err_y = err_y - self.last_time = current_time - - # ------------- 坐标系映射 ------------- - # 图像 X 轴正方向对应无人机机体坐标系的右侧平移速度 (vy) - # 图像 Y 轴正方向对应无人机(此时对应前视相机)下降速度 (vz) - vy = vel_x_cmd # 图像右侧，往右飞 - vz = vel_y_cmd * 1.0# 图像下方，往下飞, (乘1.0表示向下为正) - vx = 0.5 # 无人机以 0.5 m/s 持续前进 - - # ---------- 发送 MAVLink 指令 -------- - # 飞控处于 OFFBOARD (PX4) 模式才能响应此指令 - self.master.mav.set_position_target_local_ned_send( - 0, # 系统启动时间戳（填 0 即可） - self.master.target_system, # 目标系统 ID（自动获取） - self.master.target_component, # 目标组件 ID（自动获取） - mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED, # 选择“机体坐标系”（以无人机当前朝向为基准） - 0b0000111111000111, # 位掩码控制：二进制代表“忽略位置和加速度，只控制速度” - 0, 0, 0, # 位置 X, Y, Z (被掩码忽略) - vx, vy, vz, # 实际控制物理速度 (单位: 米/秒): 前进, 右移, 下降 - 0, 0, 0, # 加速度 (被掩码忽略) - 0, 0 # 偏航角, 偏航角速度 (被掩码忽略) - ) - print(f" MAVLink -> 前进:{vx:.2f} m/s, 右移:{vy:.2f} m/s, 下降:{vz:.2f} m/s") - - def send_hover_cmd(self): - """ 目标丢失保护: 发送指令, 让无人机原地悬停""" + def send_body_velocity_cmd(self, vx, vy, vz): + """ + 使用 MAVLink 标准消息向飞控发送【机体坐标系下】的三个轴向移动速度。 + 👉 坐标系规定 (MAV_FRAME_BODY_OFFSET_NED): + vx (+) 前飞 / (-) 后退 + vy (+) 右移 / (-) 左移 + vz (+) 下降 / (-) 上升 (注意：NED 坐标系下 Z 轴向下为正) + """ if self.master is None: return - self.master.mav.set_position_target_local_ned_send( - 0, - self.master.target_system, - self.master.target_component, - 0b0000111111000111, - 0, 0, 0, - 0.0, 0.0, 0.0, - 0, 0, 0, - 0, 0 + + # 核心：定义掩码（Type Mask） + # 16 位整型，每一位用来“忽略”某些参数。 + # 我们只想要控制 vx, vy, vz 速度，因此需要将其余的 位置(Position)、加速度(Acceleration) 和 偏航(Yaw) 标志位掩蔽屏蔽掉。 + # 二进制：0b0000111111000111 -> 十进制: 4039 + type_mask = ( + mavutil.mavlink.POSITION_TARGET_TYPEMASK_X_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_Y_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_Z_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AX_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AY_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_AZ_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_IGNORE | + mavutil.mavlink.POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE ) - print("[安全兜底] 目标跟丢或未准许控制！已向飞控发送原地刹车悬停指令。") + + self.master.mav.set_position_target_local_ned_send( + 0, # 系统时间戳（填0即可） + self.master.target_system, # 目标系统 ID + self.master.target_component, # 目标组件 ID + mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED, # 🌟 关键：指定控制基于“机体本地坐标系” + type_mask, # 掩码配置 + 0, 0, 0, # x, y, z 位置 (被忽略) + vx, vy, vz, # 🌟 实际生效的机体期望物理速度 (m/s) + 0, 0, 0, # ax, ay, az 加速度 (被忽略) + 0, 0 # yaw, yaw_rate (被忽略) + ) + + + def update_control(self, has_target, err_x = 0.0, err_y = 0.0): + """ + 接收检测结果并更新控制输出 + """ + + if has_target: + self.lost_counter = 0 + + # 1. 计算机体坐标系的各速度分量 + # 气球偏右 -> err_x > 0 -> 无人机右移对准 -> vy 需要正数 + vy_cmd = self.vy.update(err_x) + # 若气球偏下 -> err_y > 0 -> 画面中视野偏低表示无人机相对较高，需向下对齐气球。 + # 根据 MAVLink 机体 NED 坐标系：Z轴向下为正，故向上爬升（UP）vz 必须是【负数】。 + raw_vz = self.vz.update(err_y) + vz_cmd = raw_vz * 1.0 + # 初始追踪先原地锁定悬停对准，前飞速度 vx_cmd 设为 0。调试稳定后再缓慢给 0.1~0.2 + vx_cmd = 0.0 + + print( + f"🎯 视觉锁定中 -> Err_X: {err_x:+.2f} Err_Y: {err_y:+.2f} | MAVLink 输出 -> vx: {vx_cmd:.2f}, vy(右): {vy_cmd:+.2f}, vz(下): {vz_cmd:+.2f} m/s") + self.send_body_velocity_cmd(vx_cmd, vy_cmd, vz_cmd) + else: + self.lost_counter += 1 + if self.lost_counter >= self.LOST_THRESHOLD: + # 🚨 安全锁：长时间丢失目标，清除 PID 内部积分，强制发送 0 速度原地刹车悬停 + self.vy.reset() + self.vz.reset() + print(f"🚨 警告: 目标丢失超过 {self.LOST_THRESHOLD} 帧！MAVLink 发送安全原地锁死悬停指令 🔒") + self.send_body_velocity_cmd(0.0, 0.0, 0.0) + + diff --git a/infer_camera_direct.py b/infer_camera_direct.py index fb2d575..6cf9fd0 100644 --- a/infer_camera_direct.py +++ b/infer_camera_direct.py @@ -107,14 +107,9 @@ def draw_boxes(img, detections): def main(): detector = YOLO26UAVInfer(MODEL_PATH) cap = cv2.VideoCapture(INPUT_VIDEO) - from control import UAVController - # 初始化飞控通信类 (后续需根据实际串口修改, 这里临时选用 /dev/ttyUSB0代替) - uav_controller = UAVController(port='/dev/ttyUSB0', baud=115200) - - # 控制总开关: 只有当人在地面发送允许指令才会开启 AI 追踪 - AI_CONTROL_ENABLE = False + uav = UAVController() if not cap.isOpened(): print("❌ 无法打开视频源") @@ -163,17 +158,22 @@ def main(): # 画中心对准线 cv2.line(frame, (int(img_center_x), int(img_center_y)), (int(target_x), int(target_y)), (255, 0, 0), 2) - cv2.putText(frame, f"MAVLink Out -> Err_X: {err_x:+.3f} Err_Y: {err_y:+.3f}", (20, 70), - cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2) + cv2.putText(frame, f"MAVLink Out -> Err_X: {err_x:+.3f} Err_Y: {err_y:+.3f}", (20, 70),cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2) + # ================ MAVLink 动作调用 ================= + uav.update_control(has_target=True, err_x=err_x, err_y=err_y) + # ================================================= else: cv2.putText(frame, "MAVLink Out -> Target LOST (HOVER)", (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2) + # ================ MAVLink 丢失保护 ================= + uav.update_control(has_target=False) + # ================================================= # 5. 在原始分辨率大图上，渲染精准目标框 frame = draw_boxes(frame, detections) cv2.putText(frame, f"NPU Pure FPS: {fps_pure:.1f}", (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2) - # 🚀【核心优化点】只有在进入发送队列的前一秒，才将画好完美框的图 resize 为轻量化分辨率 + # 只有在进入发送队列的前一秒，才将画好完美框的图 resize 为轻量化分辨率 send_frame = cv2.resize(frame, (640, 480)) # 6. 非阻塞式尝试推入图传队列