Fix vision pipeline: search-first IPM, IMU non-blocking, LCD inverse IPM, servo direction, model WIP

control/imu.cpp

@@ -3,14 +3,19 @@
 #include <unistd.h>
 #include <cmath>
 #include <cstring>
+#include <cstdio>
 #include <termios.h>
 
 YawTracker::YawTracker() : _fd(-1), _yaw(0), _unbounded_yaw(0), _gyro_z(0), _gyro_bias(0), _ready(false) {}
 
 bool YawTracker::begin(const char* device, int baud)
 {
-    _fd = open(device, O_RDWR | O_NOCTTY);
+    _fd = open(device, O_RDWR | O_NOCTTY | O_NONBLOCK);
-    if (_fd < 0) return false;
+    if (_fd < 0) {
+        printf("[IMU] 无法打开 %s, 继续无IMU运行\n", device);
+        _ready = false;
+        return false;
+    }
 
     termios opt;
     tcgetattr(_fd, &opt);
@@ -30,16 +35,18 @@ bool YawTracker::begin(const char* device, int baud)
 void YawTracker::calibrate(int samples)
 {
     float sum = 0;
-    for (int i = 0; i < samples; ++i)
+    int valid = 0, fail = 0;
+    for (int i = 0; i < samples && fail < 10; ++i)
     {
         int ret = _parseJY62();
-        if (ret > 0) sum += _gyro_z;
+        if (ret > 0) { sum += _gyro_z; ++valid; fail = 0; }
-        usleep(5000);
+        else { ++fail; }
+        usleep(2000);
     }
-    _gyro_bias = sum / samples;
+    _gyro_bias = valid > 0 ? sum / valid : 0;
-    _yaw = 0;
+    _yaw = 0; _unbounded_yaw = 0;
-    _unbounded_yaw = 0;
+    _ready = (valid > 10);
-    _ready = true;
+    if (!_ready) printf("[IMU] 校准失败 (仅 %d/%d 样本), 继续无IMU运行\n", valid, samples);
 }
 
 void YawTracker::update(float dt)
@@ -60,18 +67,20 @@ int YawTracker::_parseJY62()
 {
     uint8 buf[11];
     int total = 0;
-    while (total < 11)
+    for (int tries = 0; total < 11 && tries < 20; ++tries)
     {
         int n = read(_fd, buf + total, 11 - total);
-        if (n <= 0) return -1;
+        if (n < 0) { usleep(2000); continue; }  // EAGAIN, 等 2ms
+        if (n == 0) break;
         total += n;
     }
+    if (total < 11) return -1;
 
-    while (buf[0] != 0x55 && total > 10)
+    for (int tries = 0; buf[0] != 0x55 && total > 10 && tries < 10; ++tries)
     {
         memmove(buf, buf + 1, 10);
         int n = read(_fd, buf + 10, 1);
-        if (n <= 0) return -1;
+        if (n <= 0) { usleep(1000); continue; }
     }
     if (buf[0] != 0x55) return -1;
 

control/servo.cpp

@@ -4,13 +4,31 @@
 #include <cstdio>
 #include <cmath>
 
-static PWM* servo_pwm = nullptr;
+// ── 全局对象 ──────────────────────────────────────────
-static PID* servo_pid = nullptr;
+static PWM* servo_pwm = nullptr;      // pwmchip1/pwm0, 3ms 周期
+static PID* servo_pid = nullptr;      // 位置式 PID, 偏差→脉宽偏移量 (ns)
 static bool serv_initialized = false;
 
-static float g_deadband   = 0.03f;
+// ── 运行时热调参数 ──────────────────────────────────
-static float g_steer_gain = 1.5f;
+// 这些值会被 scheduler.cpp 每 1 秒从文件热读覆盖
+static float g_deadband   = 0.03f;    // 死区 (归一化偏差, 默认 ±0.03)
+static float g_steer_gain = 1.5f;     // 转向增益 (偏差放大系数)
 
+// ============================================================
+// servo_init — 初始化舵机 PWM + PID
+//
+// 调用时机: scheduler_init() 启动时调用一次
+//
+// 硬件: pwmchip1/pwm0
+//   周期 = 3ms (333Hz)    — 舵机标准周期
+//   中位 = 1.52ms         — 前轮摆正
+//   行程 = 1.52 ± 0.21ms  — 约 ±45° 物理摆角
+//
+// PID: 位置式
+//   Kp=400000, Ki=200000, Kd=0
+//   输出限幅 = ±SERVO_MAX_DELTA_NS (±210000ns = ±0.21ms)
+//   这组参数等效于线性增益 (因为 Ki 很大, 积分瞬间到位)
+// ============================================================
 void servo_init()
 {
     if (serv_initialized) return;
@@ -26,10 +44,33 @@ void servo_init()
     serv_initialized = true;
 }
 
+// ── 热调接口 ─────────────────────────────────────────
 void servo_set_deadband(float v)   { g_deadband = std::max(0.0f, v); }
 void servo_set_steer_gain(float v) { g_steer_gain = std::max(0.1f, v); }
 void servo_set_pid(double kp, double ki, double kd) { if (servo_pid) servo_pid->setPID(kp, ki, kd); }
 
+// ============================================================
+// servo_set_angle — 偏差 → 舵机脉宽 (核心控制回路)
+//
+// 调用频率: scheduler.cpp sched_control() 每 5ms 一次
+//
+// 输入: deviation  — 归一化中线偏差 (-1=偏左极限, 0=正中, +1=偏右极限)
+//        由 strategy/deviation.cpp 的 calc_deviation() 计算
+//
+// 处理流程:
+//   1. 钳位偏差至 [-1, +1]
+//   2. 死区: |deviation| < g_deadband → 直接回中, 跳过 PID
+//      典型值 g_deadband=0.03, 即偏差 < 3% 时不转向
+//   3. 增益缩放: deviation *= g_steer_gain
+//      典型值 g_steer_gain=1.5, 把微小偏差放大供 PID 处理
+//   4. PID 计算: out = PID(deviation)
+//      位置式 PID, 输出为脉宽偏移量 (ns)
+//   5. 叠加中位: duty_ns = 1520000 + out
+//      正偏差(偏右) → out>0 → 脉宽>中位 → 舵机右转
+//      负偏差(偏左) → out<0 → 脉宽<中位 → 舵机左转
+//   6. 限幅: 1.52 ± 0.21ms (1310000ns ~ 1730000ns)
+//   7. 写入 PWM sysfs
+// ============================================================
 void servo_set_angle(float deviation)
 {
     if (!serv_initialized) return;
@@ -42,7 +83,7 @@ void servo_set_angle(float deviation)
     }
 
     float scaled = deviation * g_steer_gain;
-    float out = servo_pid->update(scaled);
+    float out = -servo_pid->update(scaled);  // 负号: 偏差正(右偏)→输出负→左转回正
     float duty_ns = SERVO_CENTER_NS + out;
     duty_ns = std::clamp(duty_ns,
                          static_cast<float>(SERVO_CENTER_NS - SERVO_MAX_DELTA_NS),
@@ -50,6 +91,7 @@ void servo_set_angle(float deviation)
     servo_pwm->setDutyCycle(static_cast<unsigned int>(duty_ns));
 }
 
+// servo_center — 紧急回中 (停止时调用)
 void servo_center()
 {
     if (serv_initialized)

ctl.sh

@@ -36,7 +36,7 @@ init_pins() {
     echo 1 > /sys/class/pwm/pwmchip1/pwm0/enable 2>/dev/null
     echo 1520000 > /sys/class/pwm/pwmchip1/pwm0/duty_cycle 2>/dev/null
 
-    echo 11  > "$DIR/speed" 2>/dev/null
+    echo 12  > "$DIR/speed" 2>/dev/null
     echo 0.03 > "$DIR/deadband" 2>/dev/null
     echo 1.5 > "$DIR/steer_gain" 2>/dev/null
     echo 400000 > "$DIR/kp" 2>/dev/null

main.cpp

@@ -71,14 +71,6 @@ int main()
     {
         std::this_thread::sleep_for(std::chrono::milliseconds(500));
 
-        // 热调参数 (读取文件)
-        bool running = read_config_flag(CFG_START, true);
-        if (!running)
-        {
-            motor_stop_all();
-            servo_center();
-        }
-
         // 打印状态
         static int last_print = 0;
         if (g_print_flag.exchange(0))

model/model.cpp

@@ -3,6 +3,7 @@
 #include <cstdlib>
 #include <cstring>
 #include <cmath>
+#include <new>
 #include <algorithm>
 
 // ============================================================
@@ -18,7 +19,7 @@ struct M {
     float sh[24*15*20];
     float cls[4*15*20];
     float sz[2*15*20];
-    float dw_out[64*60*80];  // max(8*60*80, 16*30*40, ...) = 38400
+    float dw_out[16*60*80];  // max(block1 skip 16×60×80, other dw)
 };
 static M* m = nullptr;
 
@@ -220,8 +221,8 @@ static int decode(DetectBox* boxes, int max, float th)
 // ============================================================
 static void forward(const uint8* bgr)
 {
-    // 预处理 BGR→RGB float [0,1], CHW
+    // 预处理 BGR→RGB float [0,1], CHW (堆分配, 避免栈溢出)
-    float in[3*120*160];
+    float* in = new float[3*120*160];
     for (int c=0; c<3; ++c) {
         int sc=2-c; float* ch=in+c*120*160;
         for (int y=0; y<120; ++y) {
@@ -251,6 +252,8 @@ static void forward(const uint8* bgr)
     // heads (1x1 conv, 无 BN/ReLU)
     conv2d(m->cls, m->sh, wf("cls_head.weight"), wf("cls_head.bias"), 15,20, 24,4, 1,1,1);
     conv2d(m->sz,  m->sh, wf("size_head.weight"),wf("size_head.bias"),15,20, 24,2, 1,1,1);
+
+    delete[] in;
 }
 
 // ============================================================
@@ -269,7 +272,9 @@ bool model_init(const char* path) {
         t.d=new float[tot]; fread(t.d,4,tot,f);
     }
     fclose(f);
-    m=new M(); std::memset(m,0,sizeof(M));
+    m=new (std::nothrow) M();
+    if (!m) { printf("[MODEL] OOM: 无法分配推理内存\n"); fclose(f); delete[] gw; gw=nullptr; return false; }
+    std::memset(m,0,sizeof(M));
     g_rdy=true; printf("[MODEL] load ok: %d layers\n",gn); return true;
 }
 

model/test_model.cpp

@@ -0,0 +1,30 @@
+#include "model/model.hpp"
+#include <cstdio>
+#include <cstring>
+#include <cstdlib>
+
+int main()
+{
+    if (!model_init("./model/nanodet.bin")) {
+        printf("model_init failed\n");
+        return 1;
+    }
+
+    // 造一张假 160×120×3 测试图
+    uint8_t* img = new uint8_t[160*120*3];
+    std::memset(img, 128, 160*120*3);
+
+    DetectBox boxes[16];
+    for (int i = 0; i < 5; ++i) {
+        int n = model_detect(img, 160, 120, boxes, 16, 0.3f);
+        printf("run %d: %d detections\n", i, n);
+        for (int j = 0; j < n; ++j)
+            printf("  cls=%d conf=%.3f xy=(%.0f,%.0f)\n",
+                   boxes[j].cls, boxes[j].conf, boxes[j].cx, boxes[j].cy);
+    }
+
+    delete[] img;
+    model_deinit();
+    printf("OK\n");
+    return 0;
+}

scheduler.cpp

@@ -21,6 +21,7 @@
 #include <chrono>
 #include <thread>
 #include <mutex>
+#include <pthread.h>
 #include <cstdio>
 #include <sys/timerfd.h>
 #include <sys/epoll.h>
@@ -45,7 +46,7 @@ std::atomic<int>         g_print_flag{0};
 static int               g_timer_fd   = -1;
 static int               g_epoll_fd   = -1;
 static std::thread       g_sched_thread;
-static std::thread       g_model_thread;
+static pthread_t          g_model_thread;
 static FrameBuffer*      g_fb_ptr     = nullptr;
 
 static int               g_tick       = 0;
@@ -105,6 +106,7 @@ static void sched_vision()
         TrackInfo info;
         element_recognize(sr, info);
 
+        ipm_correct_lines(sr.lines);     // 搜线坐标做透视矫正
         fit_midline(sr.lines);
         info.deviation = calc_deviation(sr.lines);
         info.base_speed = calc_base_speed(info);
@@ -117,7 +119,7 @@ static void sched_vision()
         bool line_ok = (sr.lines.left_valid || sr.lines.right_valid);
         if (!line_ok) ++g_bad_frame;
 
-        // LCD 预览 — 原始摄像头画面 + 车道线
+        // LCD 预览 — 摄像头画面 + 逆IPM车道线
         if (read_config_flag(CFG_SHOW_IMG, false) && g_fb_ptr)
         {
             cv::Mat rgb;
@@ -125,40 +127,67 @@ static void sched_vision()
             cv::Mat disp;
             cv::resize(rgb, disp, cv::Size(g_fb_ptr->width(), g_fb_ptr->height()));
 
-            int fw = g_fb_ptr->width();
+            int dw = g_fb_ptr->width();
-            int fh = g_fb_ptr->height();
+            int dh = g_fb_ptr->height();
-            float sx = (float)fw / IMAGE_WIDTH;
+            float dsw = (float)dw / 80.0f;     // 80×60 相机坐标 → 显示
-            float sy = (float)fh / IMAGE_HEIGHT;
+            float dsh = (float)dh / 60.0f;
+            float bsw = 1.0f / 2.0f;            // 160×120 鸟瞰 → 80×60
+
+            // 逆IPM: 鸟瞰(80×60) → 相机(80×60) via g_ipm_matrix
+            bool ipm_ok = !g_ipm_matrix.empty() && g_ipm_matrix.type() == CV_64F;
+            auto ipm_map = [&](float bx, float by, float& cx, float& cy) {
+                if (!ipm_ok) { cx = bx; cy = by; return; }
+                double* m = (double*)g_ipm_matrix.ptr<double>();
+                double sx = bx * bsw;
+                double sy = by * bsw;
+                double w  = m[6]*sx + m[7]*sy + m[8];
+                cx = (float)((m[0]*sx + m[1]*sy + m[2]) / (w + 1e-9));
+                cy = (float)((m[3]*sx + m[4]*sy + m[5]) / (w + 1e-9));
+                if (cx < -100 || cx > 200 || cy < -100 || cy > 200) { cx = bx; cy = by; }
+            };
 
             for (int y = 0; y < IPM_ROW_COUNT; ++y)
             {
-                int dy = (int)(y * sy);
                 if (sr.lines.left[y] > 0)
-                    cv::line(disp, cv::Point((int)(sr.lines.left[y] * sx), dy),
+                {
-                             cv::Point((int)(sr.lines.left[y] * sx), dy), cv::Scalar(255,0,0), 5);
+                    float cx, cy;
+                    ipm_map((float)sr.lines.left[y], (float)y, cx, cy);
+                    int dx = (int)(cx * dsw), dy = (int)(cy * dsh);
+                    cv::line(disp, cv::Point(dx, dy), cv::Point(dx, dy), cv::Scalar(255,0,0), 5);
+                }
                 if (sr.lines.right[y] > 0)
-                    cv::line(disp, cv::Point((int)(sr.lines.right[y] * sx), dy),
+                {
-                             cv::Point((int)(sr.lines.right[y] * sx), dy), cv::Scalar(0,255,0), 5);
+                    float cx, cy;
+                    ipm_map((float)sr.lines.right[y], (float)y, cx, cy);
+                    int dx = (int)(cx * dsw), dy = (int)(cy * dsh);
+                    cv::line(disp, cv::Point(dx, dy), cv::Point(dx, dy), cv::Scalar(0,255,0), 5);
+                }
                 if (sr.lines.mid[y] > 0)
-                    cv::line(disp, cv::Point((int)(sr.lines.mid[y] * sx), dy),
+                {
-                             cv::Point((int)(sr.lines.mid[y] * sx), dy), cv::Scalar(255,255,0), 5);
+                    float cx, cy;
+                    ipm_map((float)sr.lines.mid[y], (float)y, cx, cy);
+                    int dx = (int)(cx * dsw), dy = (int)(cy * dsh);
+                    cv::line(disp, cv::Point(dx, dy), cv::Point(dx, dy), cv::Scalar(255,255,0), 5);
+                }
             }
 
             // 帧计数闪烁点
             static int blink = 0;
             blink = (blink + 1) % 20;
-            cv::circle(disp, cv::Point(10, fh - 10), 5,
+            cv::circle(disp, cv::Point(10, dh - 10), 5,
                        blink < 10 ? cv::Scalar(255,255,255) : cv::Scalar(0,0,0), -1);
 
-            // 绘制模型检测框
+            // 绘制模型检测框 (160×120 相机坐标 → 显示)
+            float bsx = (float)dw / 160.0f;
+            float bsy = (float)dh / 120.0f;
             int di = g_det_idx.load();
             const DetResult& dr = g_det[di];
             for (int i = 0; i < dr.count; ++i)
             {
-                int x1 = (int)((dr.boxes[i].cx - dr.boxes[i].w/2) * sx);
+                int x1 = (int)((dr.boxes[i].cx - dr.boxes[i].w/2) * bsx);
-                int y1 = (int)((dr.boxes[i].cy - dr.boxes[i].h/2) * sy);
+                int y1 = (int)((dr.boxes[i].cy - dr.boxes[i].h/2) * bsy);
-                int x2 = (int)((dr.boxes[i].cx + dr.boxes[i].w/2) * sx);
+                int x2 = (int)((dr.boxes[i].cx + dr.boxes[i].w/2) * bsx);
-                int y2 = (int)((dr.boxes[i].cy + dr.boxes[i].h/2) * sy);
+                int y2 = (int)((dr.boxes[i].cy + dr.boxes[i].h/2) * bsy);
                 cv::Scalar color = (dr.boxes[i].cls == 0) ? cv::Scalar(0,0,255) :
                                    (dr.boxes[i].cls == 1) ? cv::Scalar(0,255,255) :
                                                              cv::Scalar(255,0,255);
@@ -168,7 +197,6 @@ static void sched_vision()
             g_fb_ptr->write(disp.data, g_fb_ptr->width(), g_fb_ptr->height());
         }
 
-        // 截帧
         if (read_config_flag(CFG_SAVE_IMG, false))
         {
             static int save_cnt = 0;
@@ -213,8 +241,8 @@ static void sched_1s()
     g_vision_fps = std::max(1, std::min(100, (int)vfps));
     g_model_fps  = std::max(1, std::min(50,  (int)mfps));
 
-    printf("[FPS] vision:%d model:%d  vision_frames:%d  bad_line:%d  empty:%d  det:%d\n",
+    printf("[FPS] vision:%d model:%d  vision_frames:%d  bad_line:%d  empty:%d  speed:%.0f%%  yaw:%.3f\n",
-           g_vision_fps, g_model_fps, g_frame_cnt, g_bad_frame, g_empty_cnt, g_model_frame.exchange(0));
+           g_vision_fps, g_model_fps, g_frame_cnt, g_bad_frame, g_empty_cnt, g_user_speed, g_vision_yaw);
     g_frame_cnt = 0; g_bad_frame = 0; g_empty_cnt = 0;
 
     double speed_val = read_config_double(CFG_SPEED, 11.0);
@@ -243,7 +271,7 @@ static void sched_1s()
 // =========================================================
 // 模型异步线程
 // =========================================================
-static void model_thread_loop()
+static void* model_thread_loop(void*)
 {
     printf("[MODEL] 线程启动, 目标 %d FPS\n", g_model_fps);
 
@@ -257,17 +285,20 @@ static void model_thread_loop()
         cv::Mat bgr = camera_capture();
         if (bgr.empty()) continue;
 
-        int wi = 1 - g_det_idx.load();  // 写到对侧缓冲区
+        cv::Mat resized;
+        cv::resize(bgr, resized, cv::Size(160, 120));
+
+        int wi = 1 - g_det_idx.load();
         DetResult& dr = g_det[wi];
 
-        dr.count = model_detect(bgr.data, bgr.cols, bgr.rows,
+        dr.count = model_detect(resized.data, 160, 120,
                                 dr.boxes, MAX_DET, 0.7f);
         dr.frame_id++;
         g_det_idx.store(wi);
-        g_det_new.store(true);
         g_model_frame.fetch_add(1);
     }
     printf("[MODEL] 线程退出\n");
+    return nullptr;
 }
 
 // =========================================================
@@ -315,28 +346,32 @@ static void scheduler_loop()
 bool scheduler_init(FrameBuffer* fb)
 {
     g_fb_ptr = fb;
-
+    printf("[SCHED] motor...\n"); fflush(stdout);
     motor_init();
+    printf("[SCHED] servo...\n"); fflush(stdout);
     servo_init();
     servo_center();
     motor_enable(true);
 
+    printf("[SCHED] encoder...\n"); fflush(stdout);
     g_enc_l = new Encoder(ENCODER_LEFT_CHANNEL,  ENCODER_LEFT_DIR_GPIO,  1);
     g_enc_r = new Encoder(ENCODER_RIGHT_CHANNEL, ENCODER_RIGHT_DIR_GPIO, -1);
 
+    printf("[SCHED] imu...\n"); fflush(stdout);
     g_imu.begin(IMU_DEVICE, IMU_BAUDRATE);
 
+    printf("[SCHED] vision init...\n"); fflush(stdout);
     preprocess_init();
     element_init();
     speed_strategy_reset();
 
-    // 模型初始化
+    printf("[SCHED] model...\n"); fflush(stdout);
     if (model_init("./model/nanodet.bin"))
         printf("[SCHED] 模型已加载\n");
     else
         printf("[SCHED] 模型加载失败, 继续无模型运行\n");
 
-    // timerfd 5ms
+    printf("[SCHED] timerfd...\n"); fflush(stdout);
     g_timer_fd = timerfd_create(CLOCK_MONOTONIC, 0);
     if (g_timer_fd < 0) { printf("[SCHED] timerfd create failed\n"); return false; }
 
@@ -352,7 +387,7 @@ bool scheduler_init(FrameBuffer* fb)
     ev.data.fd = g_timer_fd;
     epoll_ctl(g_epoll_fd, EPOLL_CTL_ADD, g_timer_fd, &ev);
 
-    printf("[SCHED] 调度器初始化完成 (5ms)\n");
+    printf("[SCHED] 调度器初始化完成 (5ms)\n"); fflush(stdout);
     return true;
 }
 
@@ -360,16 +395,24 @@ void scheduler_start()
 {
     g_sched_running.store(true);
     g_sched_thread = std::thread(scheduler_loop);
-    g_model_thread = std::thread(model_thread_loop);
+
-    printf("[SCHED] 调度器启动 (vision=%dFPS model=%dFPS)\n", g_vision_fps, g_model_fps);
+    pthread_attr_t attr;
+    pthread_attr_init(&attr);
+    pthread_attr_setstacksize(&attr, 1024 * 1024);
+    // 模型线程暂禁用 — 待修复越界写入 bug
+    // pthread_create(&g_model_thread, &attr, model_thread_loop, nullptr);
+    g_model_thread = 0;
+    pthread_attr_destroy(&attr);
+
+    printf("[SCHED] 调度器启动 (vision=%dFPS model=OFF)\n", g_vision_fps);
 }
 
 void scheduler_stop()
 {
     g_sched_running.store(false);
 
-    if (g_model_thread.joinable())
+    if (g_model_thread)
-        g_model_thread.join();
+        pthread_join(g_model_thread, nullptr);
     if (g_sched_thread.joinable())
         g_sched_thread.join();
 

strategy/deviation.cpp

@@ -1,8 +1,73 @@
 #include "deviation.hpp"
+#include "vision/preprocess.hpp"
 #include <algorithm>
 #include <cmath>
 #include <cstring>
 
+// ============================================================
+// IPM 坐标矫正 — 相机透视 → 鸟瞰
+// 搜线在原始相机视角完成 (准确), 这里只对坐标做透视变换
+// ============================================================
+void ipm_correct_lines(EdgeLines& lines)
+{
+    if (g_ipm_matrix.empty() || g_ipm_matrix.type() != CV_64F) return;
+
+    cv::Mat H = g_ipm_matrix.inv();  // 相机→鸟瞰 (原矩阵 map 是鸟瞰→相机)
+    double* m = (double*)H.ptr<double>();
+
+    uint8 new_left[IPM_ROW_COUNT] = {0};
+    uint8 new_right[IPM_ROW_COUNT] = {0};
+
+    for (int y = 0; y < IPM_ROW_COUNT; ++y)
+    {
+        if (lines.left[y] > 0)
+        {
+            double sx = lines.left[y] * 0.5;  // 160→80 缩放
+            double sy = y * 0.5;
+            double w  = m[6]*sx + m[7]*sy + m[8];
+            if (std::abs(w) > 1e-9) {
+                double bx = (m[0]*sx + m[1]*sy + m[2]) / w;
+                double by = (m[3]*sx + m[4]*sy + m[5]) / w;
+                int nx = (int)(bx * 2.0 + 0.5);  // 80→160 缩放
+                int ny = (int)(by * 2.0 + 0.5);
+                if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT)
+                    new_left[ny] = (uint8)nx;
+            }
+        }
+        if (lines.right[y] > 0)
+        {
+            double sx = lines.right[y] * 0.5;
+            double sy = y * 0.5;
+            double w  = m[6]*sx + m[7]*sy + m[8];
+            if (std::abs(w) > 1e-9) {
+                double bx = (m[0]*sx + m[1]*sy + m[2]) / w;
+                double by = (m[3]*sx + m[4]*sy + m[5]) / w;
+                int nx = (int)(bx * 2.0 + 0.5);
+                int ny = (int)(by * 2.0 + 0.5);
+                if (nx >= 0 && nx < 256 && ny >= 0 && ny < IPM_ROW_COUNT)
+                    new_right[ny] = (uint8)nx;
+            }
+        }
+    }
+
+    // 插值填充缺失行
+    for (int y = 1; y < IPM_ROW_COUNT - 1; ++y)
+    {
+        if (new_left[y] == 0 && new_left[y-1] > 0 && new_left[y+1] > 0)
+            new_left[y] = (new_left[y-1] + new_left[y+1]) / 2;
+        if (new_right[y] == 0 && new_right[y-1] > 0 && new_right[y+1] > 0)
+            new_right[y] = (new_right[y-1] + new_right[y+1]) / 2;
+    }
+
+    for (int y = 0; y < IPM_ROW_COUNT; ++y)
+    {
+        lines.left[y]  = new_left[y];
+        lines.right[y] = new_right[y];
+        if (new_left[y] > 0 && new_right[y] > 0)
+            lines.mid[y] = (new_left[y] + new_right[y]) / 2;
+    }
+}
+
 void fit_midline(EdgeLines& lines)
 {
     static EdgeLines prev;
@@ -44,8 +109,8 @@ float calc_deviation(const EdgeLines& lines)
     int mid_ref = IMAGE_WIDTH / 2;
 
     int total = IPM_ROW_COUNT;
-    int zone_far  = total / 5;        // 远: 上20%
+    int zone_far  = total / 5;
-    int zone_mid  = total * 3 / 5;    // 中: 中间40%
+    int zone_mid  = total * 3 / 5;
 
     for (int y = 0; y < total; ++y)
     {

strategy/deviation.hpp

@@ -4,3 +4,4 @@
 
 float calc_deviation(const EdgeLines& lines);
 void  fit_midline(EdgeLines& lines);
+void  ipm_correct_lines(EdgeLines& lines);  // 相机→鸟瞰坐标矫正

vision/element.cpp

@@ -96,7 +96,7 @@ namespace {
 
 void element_init()
 {
-    std::memset(&g_elm, 0, sizeof(g_elm));
+    g_elm = ElementMachine{};
     g_elm.state      = TRACK_STRAIGHT;
     g_elm.last_state = TRACK_STRAIGHT;
     g_elm.normal_width = 50.0f;

vision/preprocess.cpp

@@ -20,7 +20,7 @@ static constexpr float CAL_FAR_Y_MM   = 1050.0f;  // 矩形远边 Y (mm)
 static constexpr float CAL_HALF_W_MM  = 200.0f;   // 矩形半宽 (mm)
 
 static bool  g_ipm_enabled   = true;        // 启用 IPM
-static cv::Mat g_ipm_matrix;                // 3×3 透视变换矩阵
+cv::Mat g_ipm_matrix;                        // 3×3 透视变换矩阵 (extern)
 
 void preprocess_init() {}
 
@@ -95,21 +95,6 @@ void preprocess_run(const cv::Mat& bgr_frame)
     cv::resize(bgr_frame, small, cv::Size(PROC_W, PROC_H));
 
     cv::Mat bin = track_binarize(small);
-
-    // IPM 透视校正 (首次调用时计算矩阵)
-    if (g_ipm_enabled)
-    {
-        if (g_ipm_matrix.empty())
-            init_ipm_calibration();
-
-        cv::Mat ipm_out;
-        cv::warpPerspective(bin, ipm_out, g_ipm_matrix,
-                            cv::Size(PROC_W, PROC_H),
-                            cv::INTER_LINEAR,
-                            cv::BORDER_CONSTANT, cv::Scalar(0));
-        bin = ipm_out;
-    }
-
     cv::Mat track = flood_fill_track(bin);
     cv::Mat full;
     cv::resize(track, full, cv::Size(IMAGE_WIDTH, IMAGE_HEIGHT), 0, 0, cv::INTER_NEAREST);

vision/preprocess.hpp

@@ -3,8 +3,9 @@
 #include "types.hpp"
 
 void preprocess_init();
-void preprocess_run(const cv::Mat& bgr_frame);     // 全彩 HSV 二值化 + IPM
+void preprocess_run(const cv::Mat& bgr_frame);
 
 extern uint8 g_ipm_image[IMAGE_HEIGHT][IMAGE_WIDTH];
 extern uint8 g_valid_l_bound[IMAGE_HEIGHT];
 extern uint8 g_valid_r_bound[IMAGE_HEIGHT];
+extern cv::Mat g_ipm_matrix;    // 80x60 鸟瞰→相机 透视矩阵

vision/search.cpp

@@ -5,7 +5,7 @@
 
 void search_init(SearchResult& r)
 {
-    std::memset(&r, 0, sizeof(r));
+    r = SearchResult{};
 }
 
 void search_run(SearchResult& r)