solver.h & armor_finder.h: 新增了提取相机内参的接口,以及存储 2D 大框中心点 (target_area_center) 和过滤后的绝杀深度 (z_min_filtered)。
find_armor_box.cpp: 废弃了单一块板子的限制,将同一分类的板子全拿来算出包含自转幅度的巨型像素框正中点。
armor_finder.cpp: 让这辆车当前所有能看见的板子全部去跑 PnP 获得深度,取物理上离你最近的深度 $Z$,结合上一步的框中心点,逆推换算出一个堪称完美的 3D 虚拟准星点。
send_target.cpp: 彻底移除了容易抽搐的差值 PID 算法。现在直接根据虚拟准星算出最纯净的绝对偏航角(Yaw)和下坠角(Pitch)发给下位机,云台将彻底锁死!
auto_trigger.h: 抛弃容易受抖动破坏的三维测速。改用极其丝滑的二维图像像素帧差分测速:计算某一块板子滑到中心线刚好等于引力滞空飞行的时间,完美触发一击绝杀。
⚠️ 终极提醒(切记!切记!): 算法已经全部就位,但是这套神算法的基础基石——物理高度落差需要你手动填入。 请务必拿尺子去量一下车上摄像头和发弹孔的落差,修改 others/solver_config.yml 中的: t_camera2gimbal: [0, 0, 0] 为真实厘米数(例如 [0, -10, 5])。
This commit is contained in:
@@ -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帧
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
Reference in New Issue
Block a user