From 52301bcf8d6397fb63260258f669e8b893354af6 Mon Sep 17 00:00:00 2001 From: Li Da <3199335945@qq.com> Date: Sat, 21 Mar 2026 20:03:47 +0800 Subject: [PATCH] =?UTF-8?q?solver.h=20&=20armor=5Ffinder.h:=20=E6=96=B0?= =?UTF-8?q?=E5=A2=9E=E4=BA=86=E6=8F=90=E5=8F=96=E7=9B=B8=E6=9C=BA=E5=86=85?= =?UTF-8?q?=E5=8F=82=E7=9A=84=E6=8E=A5=E5=8F=A3=EF=BC=8C=E4=BB=A5=E5=8F=8A?= =?UTF-8?q?=E5=AD=98=E5=82=A8=202D=20=E5=A4=A7=E6=A1=86=E4=B8=AD=E5=BF=83?= =?UTF-8?q?=E7=82=B9=20(target=5Farea=5Fcenter)=20=E5=92=8C=E8=BF=87?= =?UTF-8?q?=E6=BB=A4=E5=90=8E=E7=9A=84=E7=BB=9D=E6=9D=80=E6=B7=B1=E5=BA=A6?= =?UTF-8?q?=20(z=5Fmin=5Ffiltered)=E3=80=82=20find=5Farmor=5Fbox.cpp:=20?= =?UTF-8?q?=E5=BA=9F=E5=BC=83=E4=BA=86=E5=8D=95=E4=B8=80=E5=9D=97=E6=9D=BF?= =?UTF-8?q?=E5=AD=90=E7=9A=84=E9=99=90=E5=88=B6=EF=BC=8C=E5=B0=86=E5=90=8C?= =?UTF-8?q?=E4=B8=80=E5=88=86=E7=B1=BB=E7=9A=84=E6=9D=BF=E5=AD=90=E5=85=A8?= =?UTF-8?q?=E6=8B=BF=E6=9D=A5=E7=AE=97=E5=87=BA=E5=8C=85=E5=90=AB=E8=87=AA?= =?UTF-8?q?=E8=BD=AC=E5=B9=85=E5=BA=A6=E7=9A=84=E5=B7=A8=E5=9E=8B=E5=83=8F?= =?UTF-8?q?=E7=B4=A0=E6=A1=86=E6=AD=A3=E4=B8=AD=E7=82=B9=E3=80=82=20armor?= =?UTF-8?q?=5Ffinder.cpp:=20=E8=AE=A9=E8=BF=99=E8=BE=86=E8=BD=A6=E5=BD=93?= =?UTF-8?q?=E5=89=8D=E6=89=80=E6=9C=89=E8=83=BD=E7=9C=8B=E8=A7=81=E7=9A=84?= =?UTF-8?q?=E6=9D=BF=E5=AD=90=E5=85=A8=E9=83=A8=E5=8E=BB=E8=B7=91=20PnP=20?= =?UTF-8?q?=E8=8E=B7=E5=BE=97=E6=B7=B1=E5=BA=A6=EF=BC=8C=E5=8F=96=E7=89=A9?= =?UTF-8?q?=E7=90=86=E4=B8=8A=E7=A6=BB=E4=BD=A0=E6=9C=80=E8=BF=91=E7=9A=84?= =?UTF-8?q?=E6=B7=B1=E5=BA=A6=20$Z$=EF=BC=8C=E7=BB=93=E5=90=88=E4=B8=8A?= =?UTF-8?q?=E4=B8=80=E6=AD=A5=E7=9A=84=E6=A1=86=E4=B8=AD=E5=BF=83=E7=82=B9?= =?UTF-8?q?=EF=BC=8C=E9=80=86=E6=8E=A8=E6=8D=A2=E7=AE=97=E5=87=BA=E4=B8=80?= =?UTF-8?q?=E4=B8=AA=E5=A0=AA=E7=A7=B0=E5=AE=8C=E7=BE=8E=E7=9A=84=203D=20?= =?UTF-8?q?=E8=99=9A=E6=8B=9F=E5=87=86=E6=98=9F=E7=82=B9=E3=80=82=20send?= =?UTF-8?q?=5Ftarget.cpp:=20=E5=BD=BB=E5=BA=95=E7=A7=BB=E9=99=A4=E4=BA=86?= =?UTF-8?q?=E5=AE=B9=E6=98=93=E6=8A=BD=E6=90=90=E7=9A=84=E5=B7=AE=E5=80=BC?= =?UTF-8?q?=20PID=20=E7=AE=97=E6=B3=95=E3=80=82=E7=8E=B0=E5=9C=A8=E7=9B=B4?= =?UTF-8?q?=E6=8E=A5=E6=A0=B9=E6=8D=AE=E8=99=9A=E6=8B=9F=E5=87=86=E6=98=9F?= =?UTF-8?q?=E7=AE=97=E5=87=BA=E6=9C=80=E7=BA=AF=E5=87=80=E7=9A=84=E7=BB=9D?= =?UTF-8?q?=E5=AF=B9=E5=81=8F=E8=88=AA=E8=A7=92=EF=BC=88Yaw=EF=BC=89?= =?UTF-8?q?=E5=92=8C=E4=B8=8B=E5=9D=A0=E8=A7=92=EF=BC=88Pitch=EF=BC=89?= =?UTF-8?q?=E5=8F=91=E7=BB=99=E4=B8=8B=E4=BD=8D=E6=9C=BA=EF=BC=8C=E4=BA=91?= =?UTF-8?q?=E5=8F=B0=E5=B0=86=E5=BD=BB=E5=BA=95=E9=94=81=E6=AD=BB=EF=BC=81?= =?UTF-8?q?=20auto=5Ftrigger.h:=20=E6=8A=9B=E5=BC=83=E5=AE=B9=E6=98=93?= =?UTF-8?q?=E5=8F=97=E6=8A=96=E5=8A=A8=E7=A0=B4=E5=9D=8F=E7=9A=84=E4=B8=89?= =?UTF-8?q?=E7=BB=B4=E6=B5=8B=E9=80=9F=E3=80=82=E6=94=B9=E7=94=A8=E6=9E=81?= =?UTF-8?q?=E5=85=B6=E4=B8=9D=E6=BB=91=E7=9A=84=E4=BA=8C=E7=BB=B4=E5=9B=BE?= =?UTF-8?q?=E5=83=8F=E5=83=8F=E7=B4=A0=E5=B8=A7=E5=B7=AE=E5=88=86=E6=B5=8B?= =?UTF-8?q?=E9=80=9F=EF=BC=9A=E8=AE=A1=E7=AE=97=E6=9F=90=E4=B8=80=E5=9D=97?= =?UTF-8?q?=E6=9D=BF=E5=AD=90=E6=BB=91=E5=88=B0=E4=B8=AD=E5=BF=83=E7=BA=BF?= =?UTF-8?q?=E5=88=9A=E5=A5=BD=E7=AD=89=E4=BA=8E=E5=BC=95=E5=8A=9B=E6=BB=9E?= =?UTF-8?q?=E7=A9=BA=E9=A3=9E=E8=A1=8C=E7=9A=84=E6=97=B6=E9=97=B4=EF=BC=8C?= =?UTF-8?q?=E5=AE=8C=E7=BE=8E=E8=A7=A6=E5=8F=91=E4=B8=80=E5=87=BB=E7=BB=9D?= =?UTF-8?q?=E6=9D=80=E3=80=82=20=E2=9A=A0=EF=B8=8F=20=E7=BB=88=E6=9E=81?= =?UTF-8?q?=E6=8F=90=E9=86=92=EF=BC=88=E5=88=87=E8=AE=B0=EF=BC=81=E5=88=87?= =?UTF-8?q?=E8=AE=B0=EF=BC=81=EF=BC=89=EF=BC=9A=20=E7=AE=97=E6=B3=95?= =?UTF-8?q?=E5=B7=B2=E7=BB=8F=E5=85=A8=E9=83=A8=E5=B0=B1=E4=BD=8D=EF=BC=8C?= =?UTF-8?q?=E4=BD=86=E6=98=AF=E8=BF=99=E5=A5=97=E7=A5=9E=E7=AE=97=E6=B3=95?= =?UTF-8?q?=E7=9A=84=E5=9F=BA=E7=A1=80=E5=9F=BA=E7=9F=B3=E2=80=94=E2=80=94?= =?UTF-8?q?=E7=89=A9=E7=90=86=E9=AB=98=E5=BA=A6=E8=90=BD=E5=B7=AE=E9=9C=80?= =?UTF-8?q?=E8=A6=81=E4=BD=A0=E6=89=8B=E5=8A=A8=E5=A1=AB=E5=85=A5=E3=80=82?= =?UTF-8?q?=20=E8=AF=B7=E5=8A=A1=E5=BF=85=E6=8B=BF=E5=B0=BA=E5=AD=90?= =?UTF-8?q?=E5=8E=BB=E9=87=8F=E4=B8=80=E4=B8=8B=E8=BD=A6=E4=B8=8A=E6=91=84?= =?UTF-8?q?=E5=83=8F=E5=A4=B4=E5=92=8C=E5=8F=91=E5=BC=B9=E5=AD=94=E7=9A=84?= =?UTF-8?q?=E8=90=BD=E5=B7=AE=EF=BC=8C=E4=BF=AE=E6=94=B9=20others/solver?= =?UTF-8?q?=5Fconfig.yml=20=E4=B8=AD=E7=9A=84=EF=BC=9A=20t=5Fcamera2gimbal?= =?UTF-8?q?:=20[0,=200,=200]=20=E4=B8=BA=E7=9C=9F=E5=AE=9E=E5=8E=98?= =?UTF-8?q?=E7=B1=B3=E6=95=B0=EF=BC=88=E4=BE=8B=E5=A6=82=20[0,=20-10,=205]?= =?UTF-8?q?=EF=BC=89=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- armor/include/armor_finder/armor_finder.h | 3 + .../include/armor_finder/classifier/solver.h | 1 + armor/include/show_images/auto_trigger.h | 56 ++++++++----------- armor/src/armor_finder/armor_finder.cpp | 39 ++++++++++--- .../src/armor_finder/find/find_armor_box.cpp | 20 +++++++ .../armor_finder/send_target/send_target.cpp | 32 +++-------- 6 files changed, 87 insertions(+), 64 deletions(-) diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 2ee8aca..df4940b 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -110,6 +110,9 @@ private: const uint8_t &is_anti_top; // 进入反陀螺，引用外部变量，自动变化 State state; // 自瞄状态对象实例 ArmorBox target_box, last_box; // 目标装甲板 + cv::Point2f target_area_center; // 目标大框像素中心 + double z_min_filtered = 0; // 目标深度极小值 + ArmorBoxes current_target_boxes; // 聚类后的同一车体装甲板 int anti_switch_cnt; // 防止乱切目标计数器 cv::Ptr tracker; // tracker对象实例 Classifier classifier; // CNN分类器对象实例，用于数字识别 diff --git a/armor/include/armor_finder/classifier/solver.h b/armor/include/armor_finder/classifier/solver.h index 3d93949..9ae4b6f 100644 --- a/armor/include/armor_finder/classifier/solver.h +++ b/armor/include/armor_finder/classifier/solver.h @@ -17,6 +17,7 @@ class Solver public: explicit Solver(const std::string & config_path); + const cv::Mat& get_camera_matrix() const { return camera_matrix_; } Eigen::Matrix3d R_gimbal2world() const; void set_R_gimbal2world(const Eigen::Quaterniond & q); diff --git a/armor/include/show_images/auto_trigger.h b/armor/include/show_images/auto_trigger.h index 0dce1be..61e033b 100644 --- a/armor/include/show_images/auto_trigger.h +++ b/armor/include/show_images/auto_trigger.h @@ -21,40 +21,32 @@ public: * @return true 目标与预测弹着点重合，建议击发 */ static bool should_fire(const ArmorFinder &armor_finder, double v0, double gimbal_yaw, double gimbal_pitch, double threshold_deg = -1.0) { - if (armor_finder.history.size() < 3) return false; // 轨迹点不足，不预测 + if (armor_finder.last_box.rect == cv::Rect2d()) return false; + + cv::Point2f curr_center = armor_finder.target_box.getCenter(); + cv::Point2f last_center = armor_finder.last_box.getCenter(); + cv::Point2f area_center = armor_finder.target_area_center; - // 1. 获取目标当前的 3D 状态 (相对于相机) - const auto &latest = armor_finder.history.back(); - double dist = latest.pos.z / 100.0; // cm -> m - - // 2. 估计目标速度 (简单线性回归或两点差分) - const auto &first = armor_finder.history.front(); - double dt = latest.time - first.time; - if (dt <= 0) return false; - - cv::Point3f velocity = (latest.pos - first.pos) * (1.0 / dt); // cm/s - - // 3. 计算飞行时间 (单位: s) + // 横向速度估计 (采用帧差分，工业相机一般 ~100 fps) + double vx = curr_center.x - last_center.x; + if (std::abs(vx) < 1.0) return false; + + // 计算这块装甲板滑到大框中心所需的预测帧数 + double frames_to_pass = (area_center.x - curr_center.x) / vx; + + // 若不是朝着中心滑动 (已偏离)，不激发 + if (frames_to_pass < 0) return false; + + // 弹道时间预判 + double dist = armor_finder.z_min_filtered / 100.0; // cm -> m double t_flight = BallisticSolver::get_flight_time(dist, -gimbal_pitch, v0, BALLISTIC_K); - double t_total = t_flight + SYSTEM_DELAY / 1000.0; - - // 4. 预测目标未来的位置 (cm) - cv::Point3f pos_pred = latest.pos + velocity * t_total; - - // 5. 将预测位置映射回像素坐标 (用于判断重合) - double x_pred_pixel = pos_pred.x * FOCUS_PIXAL / pos_pred.z + IMAGE_CENTER_X; - double y_pred_pixel = pos_pred.y * FOCUS_PIXAL / pos_pred.z + IMAGE_CENTER_Y; - - // 6. 推断弹着点偏移 (像素级) - // 理想打击点应该就在相机中心 (自瞄已经对准补差后的位置) - // 但是我们需要判断当前云台的 "指向误差" 是否足够小 - double dist_to_center = sqrt(pow(x_pred_pixel - IMAGE_CENTER_X, 2) + pow(y_pred_pixel - IMAGE_CENTER_Y, 2)); - - // 7. 动态阈值判定 (依据目标距离和装甲板大小) - // 距离越远，允许的像素误差越小 - double threshold = (threshold_deg > 0) ? (threshold_deg * FOCUS_PIXAL * PI / 180.0 / 100.0) : (200.0 / (dist + 0.001)); - - return dist_to_center < threshold; + double t_total = t_flight + SYSTEM_DELAY / 1000.0; + + // 将总延迟时间换算为帧数偏移 (以 100FPS) + double delay_frames = t_total * 100.0; + + // 如果剩余到达中心时间 = 飞弹+机械延迟时间 左右，开火！ + return std::abs(frames_to_pass - delay_frames) < 3.0; // 容差 +/- 3帧 } }; diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index f09f3e2..6b7df2f 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -114,19 +114,40 @@ end: cv::waitKey(1); } - // 自动扳机位置预测逻辑：更新历史位置 + // 自动扳机位置预测与虚拟靶点重推逻辑 if (target_box.rect != cv::Rect2d()) { - auto_aim::Armor armor; - armor.type = (target_box.id == B1 || target_box.id == R1 || target_box.id == B2 || target_box.id == R2 || target_box.id == B7 || target_box.id == R7 || target_box.id == B8 || target_box.id == R8) ? auto_aim::ArmorType::big : auto_aim::ArmorType::small; - armor.name = static_cast(target_box.id); - armor.points = target_box.points; - - // 更新云台位姿 (使用 MCU 回传的 yaw/pitch，单位假设为度，需转为弧度) + // 1. 遍历当前目标车的所有可见装甲板，寻找极小深度 Z_min + double min_z = 99999.0; solver.set_R_gimbal2world(mcu_data.curr_yaw * CV_PI / 180.0, mcu_data.curr_pitch * CV_PI / 180.0); + for (auto &b : current_target_boxes) { + auto_aim::Armor arm; + arm.type = (b.id == B1 || b.id == R1 || b.id == B2 || b.id == R2 || b.id == B7 || b.id == R7 || b.id == B8 || b.id == R8) ? auto_aim::ArmorType::big : auto_aim::ArmorType::small; + arm.name = static_cast(b.id); + arm.points = b.points; + solver.solve(arm); + double z_cm = arm.xyz_in_gimbal.z() * 100.0; + if (z_cm < min_z) min_z = z_cm; + } - solver.solve(armor); + // 平滑滤波获取稳定的 z_min + if (z_min_filtered <= 0.1 || min_z < z_min_filtered) { + z_min_filtered = min_z; // 激进跟进最小值锁定 + } else { + z_min_filtered = z_min_filtered * 0.7 + min_z * 0.3; // 缓慢恢复 + } + + // 2. 将 2D 大框中心逆推为 3D 虚拟靶点 + const cv::Mat& cam_mat = solver.get_camera_matrix(); + double fx = cam_mat.at(0, 0); + double cx = cam_mat.at(0, 2); + double fy = cam_mat.at(1, 1); + double cy = cam_mat.at(1, 2); + + double X_cam = (target_area_center.x - cx) * z_min_filtered / fx; + double Y_cam = (target_area_center.y - cy) * z_min_filtered / fy; + + target_xyz = cv::Point3f(X_cam, Y_cam, z_min_filtered); - target_xyz = cv::Point3f(armor.xyz_in_gimbal.x() * 100.0, armor.xyz_in_gimbal.y() * 100.0, armor.xyz_in_gimbal.z() * 100.0); // 转换为cm cv::Point3f current_pos = target_xyz; double current_time = frame_time / 1000.0; history.push_back({current_pos, current_time}); diff --git a/armor/src/armor_finder/find/find_armor_box.cpp b/armor/src/armor_finder/find/find_armor_box.cpp index b9fb9d4..5e96c01 100644 --- a/armor/src/armor_finder/find/find_armor_box.cpp +++ b/armor/src/armor_finder/find/find_armor_box.cpp @@ -206,6 +206,26 @@ bool ArmorFinder::findArmorBox(const cv::Mat &src, ArmorBox &box) { } else { // 如果分类器不可用，则直接选取候选区中的第一个区域作为目标(往往会误识别) box = armor_boxes[0]; } + + // --- 反小陀螺：同类装甲板大框聚类 --- + if (box.rect != cv::Rect2d(0, 0, 0, 0)) { + current_target_boxes.clear(); + double min_x = 9999, max_x = -9999, min_y = 9999, max_y = -9999; + for (const auto &one_box : armor_boxes) { + // 将同分类 ID 的装甲板聚类 (即使未分类也可根据需求退化处理) + if (one_box.id == box.id) { + current_target_boxes.push_back(one_box); + min_x = fmin(min_x, one_box.rect.x); + max_x = fmax(max_x, one_box.rect.x + one_box.rect.width); + min_y = fmin(min_y, one_box.rect.y); + max_y = fmax(max_y, one_box.rect.y + one_box.rect.height); + } + } + if (!current_target_boxes.empty()) { + target_area_center = cv::Point2f(min_x + (max_x - min_x) / 2.0, min_y + (max_y - min_y) / 2.0); + } + } + return true; } diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index a1b3e07..34c524e 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -51,28 +51,9 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { if (shoot_delay) { LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay); } - auto rect = target_box.rect; - double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X; - double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y; - - // PID - sum_yaw += dx; - sum_pitch += dy; - float yaw_I_component = YAW_AIM_KI * sum_yaw; - float pitch_I_component = PITCH_AIM_KI * sum_pitch; - - double tmp_yaw = dx; - double tmp_pitch = dy; - dx = YAW_AIM_KP * dx + YAW_AIM_KI * sum_yaw + - YAW_AIM_KD * (dx - last_yaw); - dy = PITCH_AIM_KP * dy + PITCH_AIM_KI * sum_pitch + - PITCH_AIM_KD * (dy - last_pitch); - - last_yaw = tmp_yaw; - last_pitch = tmp_pitch; - // - - double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; + + // 直接基于虚拟靶心 target_xyz (已包含 Z_min 和中心坐标转换) 算出绝对偏航角 + double yaw = atan2(target_xyz.x, target_xyz.z) * 180 / PI; double dist = sqrt(target_xyz.x * target_xyz.x + target_xyz.y * target_xyz.y + target_xyz.z * target_xyz.z) / 100.0; // 米 // 弹道补偿使用 PnP 提供的 3D 坐标 @@ -80,9 +61,14 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { double y_target = -target_xyz.y / 100.0; // 垂直高度差 double pitch_comp = BallisticSolver::get_pitch(x_target, y_target, MUZZLE_VELOCITY, BALLISTIC_K); - // 计算是否满足开火条件 (例如残差小于 1.5 度) + // 计算是否满足开火条件 (容差 1.5 度，可根据需要调节) can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, yaw, pitch_comp, 1.5); + // 当满足预测开火条件时，通知电控 (借用 shoot_delay 或其他预留位) + if (can_fire && shoot_delay == 0) { + shoot_delay = 1; // 假定 1 为开火 flag，由电控在裁判系统中绘制绿框或自动激发 + } + return sendTarget(serial, yaw, pitch_comp, dist * 100.0, shoot_delay); }