机器人路径规划算法实践总结 (Path Planning Practical Guide)

// 可采纳性：h(n) ≤ 实际最短代价 → 保证找到最优解
// 一致性：h(n) ≤ cost(n,n') + h(n') → 保证节点只扩展一次，提升效率

// 对角启发式（8连通网格，满足一致性）
double diagonalHeuristic(int x1, int y1, int x2, int y2, 
                         double cost_straight = 1.0, 
                         double cost_diag = M_SQRT2) {
    int dx = abs(x1 - x2), dy = abs(y1 - y2);
    int min_d = std::min(dx, dy);
    int max_d = std::max(dx, dy);
    // 对角步数×对角代价 + 直线步数×直线代价
    return cost_diag * min_d + cost_straight * (max_d - min_d);
}

// Tie-Breaker：当f值相同时，优先选择h更小的节点，减少不必要的探索
node->f = node->g + node->h * (1.0 + 1e-4);  // 微小扰动打破平局

// 问题1：priority_queue 不支持 decrease-key，重复插入导致内存泄漏
if (!open_set_contains(neighbor, open_set)) {  // O(N) 查找！破坏整体复杂度
    open_set.push(neighbor);
}

// 问题2：g值更新后，queue 中旧节点仍按旧 f 值排序，导致错误扩展
neighbor->g = tentative_g;  // 优先级未同步更新

// 核心数据结构
struct Node {
    int x, y;
    double g = INFINITY, h = 0, f = INFINITY;
    Node* parent = nullptr;
    bool operator>(const Node& other) const { return f > other.f; }
    void reset() { g = h = f = INFINITY; parent = nullptr; }  // 对象复用必备
};

// 坐标编码工具（用于哈希映射）
inline uint64_t encodeCoord(int x, int y, int width) {
    return (static_cast<uint64_t>(x) << 32) | static_cast<uint32_t>(y);
}

// A* 规划器核心类
class AStarPlanner {
private:
    // 坐标 → 节点最新状态的映射（避免重复创建）
    std::unordered_map<uint64_t, Node*> node_registry;
    // 优先队列（存储原始指针，通过 registry 验证有效性）
    std::priority_queue<Node*, std::vector<Node*>, std::greater<Node*>> open_set;
    // 内存池：避免频繁 new/delete
    ObjectPool<Node> node_pool;
    
public:
    std::vector<Point> plan(const GridMap& map, const Point& start, const Point& goal) {
        // 初始化起点
        Node* start_node = acquireNode(start.x, start.y);
        start_node->g = 0;
        start_node->h = diagonalHeuristic(start.x, start.y, goal.x, goal.y);
        start_node->f = start_node->g + start_node->h;
        open_set.push(start_node);
        
        while (!open_set.empty()) {
            Node* current = popValidNode();  // 惰性删除：跳过废弃节点
            if (!current) break;
            
            // 目标检测
            if (current->x == goal.x && current->y == goal.y) {
                auto path = reconstructPath(current);
                cleanup();  // 释放内存池
                return path;
            }
            
            // 扩展邻居（8连通）
            for (const auto& [nx, ny, cost] : getNeighbors8(map, current)) {
                if (map.isOccupied(nx, ny)) continue;
                
                double tentative_g = current->g + cost;
                Node* neighbor = acquireNode(nx, ny);
                
                // 发现更优路径则更新
                if (tentative_g < neighbor->g) {
                    neighbor->parent = current;
                    neighbor->g = tentative_g;
                    neighbor->h = diagonalHeuristic(nx, ny, goal.x, goal.y);
                    neighbor->f = neighbor->g + neighbor->h * 1.001;  // Tie-Breaker
                    open_set.push(neighbor);  // 新状态入队，旧状态惰性丢弃
                }
            }
        }
        cleanup();
        return {};  // 无解
    }
    
private:
    Node* acquireNode(int x, int y) {
        uint64_t key = encodeCoord(x, y, map_width);
        if (auto it = node_registry.find(key); it != node_registry.end()) {
            return it->second;  // 复用已有节点
        }
        Node* node = node_pool.acquire();  // 从内存池获取
        node->x = x; node->y = y;
        node_registry[key] = node;
        return node;
    }
    
    Node* popValidNode() {
        while (!open_set.empty()) {
            Node* top = open_set.top(); open_set.pop();
            uint64_t key = encodeCoord(top->x, top->y, map_width);
            // 惰性删除：只处理与registry中g值一致的节点
            if (node_registry.count(key) && node_registry[key]->g == top->g) {
                return top;
            }
        }
        return nullptr;
    }
    
    void cleanup() {
        for (auto& [key, node] : node_registry) {
            node_pool.release(node);  // 归还内存池
        }
        node_registry.clear();
        while (!open_set.empty()) open_set.pop();
    }
};

// 距离变换 + 阈值膨胀（比逐点卷积更高效）
void inflateObstacles(GridMap& map, double robot_radius, double safety_margin) {
    double inflation_dist = robot_radius + safety_margin;
    int inflation_cells = ceil(inflation_dist / map.resolution);
    
    // 多源BFS计算每个格子到最近障碍物的距离
    std::queue<std::pair<int,int>> q;
    const int dx[] = {1,-1,0,0,1,1,-1,-1};
    const int dy[] = {0,0,1,-1,1,-1,1,-1};
    
    // 初始化
    for (int i = 0; i < map.height; i++) {
        for (int j = 0; j < map.width; j++) {
            if (map.isOccupied(i, j)) {
                map.dist[i][j] = 0;
                q.push({i, j});
            } else {
                map.dist[i][j] = INFINITY;
            }
        }
    }
    
    // BFS传播距离
    while (!q.empty()) {
        auto [x, y] = q.front(); q.pop();
        for (int k = 0; k < 8; k++) {
            int nx = x + dx[k], ny = y + dy[k];
            if (!map.inBounds(nx, ny)) continue;
            double step_cost = (k < 4) ? 1.0 : M_SQRT2;
            double new_dist = map.dist[x][y] + step_cost * map.resolution;
            if (new_dist < map.dist[nx][ny]) {
                map.dist[nx][ny] = new_dist;
                q.push({nx, ny});
            }
        }
    }
    
    // 根据距离设置代价值（指数衰减，越近代价越高）
    for (int i = 0; i < map.height; i++) {
        for (int j = 0; j < map.width; j++) {
            if (map.dist[i][j] <= inflation_dist) {
                // 代价值：0(自由) ~ 254(危险)，255=障碍物
                double ratio = map.dist[i][j] / inflation_dist;
                map.cost[i][j] = static_cast<uint8_t>(254 * (1.0 - ratio));
            }
        }
    }
}

// 状态定义（含航向角，适用于阿克曼/差速底盘）
struct State {
    double x, y, theta;  // 位置 + 航向
    double g = INFINITY; // 累积代价
    State* parent = nullptr;
    void reset() { g = INFINITY; parent = nullptr; }
};

// 运动学约束的 Steer 函数（以阿克曼转向为例）
State steerKinodynamic(const State& from, const Point& target, 
                       double step_size, double min_turn_radius) {
    // 1. 计算期望航向角
    double target_heading = atan2(target.y - from.y, target.x - from.x);
    
    // 2. 考虑最大曲率约束（最小转弯半径）
    double max_turn_angle = step_size / min_turn_radius;  // 单步最大转向角
    double angle_diff = normalizeAngle(target_heading - from.theta);
    double new_heading = from.theta + clamp(angle_diff, -max_turn_angle, max_turn_angle);
    
    // 3. 按新航向生成新状态
    return {
        from.x + step_size * cos(new_heading),
        from.y + step_size * sin(new_heading),
        new_heading,
        INFINITY, nullptr
    };
}

// 连续碰撞检测（避免高速运动漏检）
bool checkCollisionContinuous(const State& s1, const State& s2, 
                              const std::vector<Obstacle>& obs,
                              double robot_radius, double step_ratio = 0.1) {
    double dist = hypot(s2.x - s1.x, s2.y - s1.y);
    int steps = std::max(2, static_cast<int>(dist / (robot_radius * step_ratio)));
    
    for (int i = 1; i < steps; i++) {
        double t = static_cast<double>(i) / steps;
        State mid = lerpState(s1, s2, t);  // 线性插值状态
        if (checkCollisionDiscrete(mid, obs, robot_radius)) return true;
    }
    return false;
}

// RRT* 核心扩展与重连逻辑
class RRTStarPlanner {
private:
    std::vector<State*> tree;
    KDTree* kd_tree;  // 加速最近邻搜索
    ObjectPool<State> state_pool;
    
public:
    std::vector<State> plan(const PlanningContext& ctx, int max_iter = 10000) {
        // 初始化起点
        State* root = state_pool.acquire();
        *root = ctx.start; root->g = 0;
        tree.push_back(root);
        kd_tree->insert(root);
        
        for (int iter = 0; iter < max_iter; iter++) {
            // 1. 采样（可结合目标偏置）
            Point q_rand = (rand01() < 0.05) ? ctx.goal : sampleFreeSpace(ctx.bounds);
            
            // 2. 最近邻搜索（KD-Tree O(log N)）
            State* q_near = kd_tree->nearest(q_rand);
            
            // 3. 运动学感知的扩展
            State q_new = steerKinodynamic(*q_near, q_rand, ctx.step_size, ctx.min_turn_radius);
            if (checkCollisionContinuous(*q_near, q_new, ctx.obstacles, ctx.robot_radius)) {
                continue;  // 碰撞则丢弃
            }
            
            // 4. 创建新节点
            State* q_new_node = state_pool.acquire();
            *q_new_node = q_new;
            q_new_node->parent = q_near;
            q_new_node->g = q_near->g + distance(*q_near, q_new);
            tree.push_back(q_new_node);
            kd_tree->insert(q_new_node);
            
            // 5. RRT* 核心：Rewire（带邻域衰减）
            double radius = std::min(ctx.max_rewire_radius, 
                                   ctx.gamma * pow(log(tree.size()) / tree.size(), 1.0/2.0));
            auto neighbors = kd_tree->radiusSearch(q_new, radius);
            
            for (State* q_near_neighbor : neighbors) {
                if (q_near_neighbor == q_near) continue;
                
                double cost_via_new = q_new_node->g + distance(*q_new_node, *q_near_neighbor);
                if (cost_via_new < q_near_neighbor->g && 
                    !checkCollisionContinuous(*q_new_node, *q_near_neighbor, 
                                            ctx.obstacles, ctx.robot_radius)) {
                    // 仅更新直接邻居，子树代价延迟计算（Lazy Propagation）
                    q_near_neighbor->parent = q_new_node;
                    q_near_neighbor->g = cost_via_new;
                    // 注意：实际工业实现中，g值传播可延迟到查询时计算
                }
            }
            
            // 6. 目标检测（带容差）
            if (distance(q_new, ctx.goal) < ctx.goal_tolerance) {
                return reconstructPath(q_new_node);
            }
        }
        return {};  // 超时未找到
    }
};

// Gaussian 采样：在障碍物边界附近增加采样概率
Point gaussianSample(const PlanningContext& ctx, double sigma) {
    while (true) {
        Point p1 = sampleUniform(ctx.bounds);
        Point p2 = sampleGaussian(p1, sigma);  // 以p1为中心的高斯扰动
        
        // 接受条件：一个点在自由空间，一个点在障碍物内（边界附近）
        bool in1 = isInFreeSpace(p1, ctx.obstacles);
        bool in2 = isInFreeSpace(p2, ctx.obstacles);
        if (in1 != in2) {
            return in1 ? p1 : p2;
        }
    }
}

// Bridge Test：专门针对窄通道的采样
Point bridgeTestSample(const PlanningContext& ctx, double max_len) {
    while (true) {
        Point center = sampleUniform(ctx.bounds);
        Point direction = sampleUnitVector();
        Point p1 = center + direction * max_len/2;
        Point p2 = center - direction * max_len/2;
        
        // 接受条件：两端在障碍物内，中间在自由空间（穿过窄通道）
        if (!isInFreeSpace(p1, ctx.obstacles) && 
            !isInFreeSpace(p2, ctx.obstacles) &&
            isInFreeSpace(center, ctx.obstacles)) {
            return center;
        }
    }
}

// DWA 主循环（20-50Hz）
Twist computeDWACommand(const RobotState& state, 
                        const Costmap& costmap,
                        const Point& local_goal,
                        const DWAParams& params) {
    
    // 1. 动态窗口计算：可达速度空间
    VelocityWindow Vd = computeDynamicWindow(
        state.velocity, 
        params.max_vel, params.min_vel,
        params.max_accel, params.max_omega_accel,
        params.dt  // 仿真步长
    );
    
    // 2. 轨迹预测与采样
    std::vector<Trajectory> trajectories;
    for (double v : sample(Vd.v_range, params.v_resolution)) {
        for (double w : sample(Vd.omega_range, params.omega_resolution)) {
            Trajectory traj = simulateTrajectory(state, v, w, params.dt, params.sim_time);
            if (isValidTrajectory(traj, costmap, params.robot_radius)) {
                trajectories.push_back(traj);
            }
        }
    }
    
    // 3. 代价评估（支持动态权重）
    double obs_weight, goal_weight, vel_weight;
    std::tie(obs_weight, goal_weight, vel_weight) = 
        adaptiveWeights(costmap, state, params);  // 根据环境拥挤度调整
    
    Trajectory* best = nullptr;
    double best_score = -INFINITY;
    
    for (auto& traj : trajectories) {
        double score = 
            obs_weight * obstacleCost(traj, costmap) +
            goal_weight * goalCost(traj, local_goal) +
            vel_weight * velocityCost(traj, params.preferred_vel);
        
        if (score > best_score) {
            best_score = score;
            best = &traj;
        }
    }
    
    // 4. 执行最优 + 安全兜底
    if (best) {
        return {best->v_end, best->omega_end};
    } else {
        // 无可行轨迹：紧急制动
        return {0, 0};
    }
}

// 动态权重调整：根据局部障碍物密度
std::tuple<double, double, double> adaptiveWeights(
    const Costmap& costmap, const RobotState& state, const DWAParams& params) {
    
    double density = computeLocalDensity(costmap, state.pos, params.eval_radius);
    
    if (density > 0.7) {
        // 拥挤环境：优先避障，降低速度追求
        return {params.obs_weight * 2.0, params.goal_weight * 0.3, params.vel_weight * 0.5};
    } else if (density < 0.2) {
        // 开阔环境：优先趋近目标，允许更高速度
        return {params.obs_weight * 0.5, params.goal_weight * 2.0, params.vel_weight * 1.5};
    }
    return {params.obs_weight, params.goal_weight, params.vel_weight};
}

// Frenet 轨迹生成器
class FrenetPlanner {
public:
    // 纵向规划：四次多项式 s(t) = a0 + a1*t + a2*t² + a3*t³ + a4*t⁴
    Polynomial4 planLongitudinal(const LonBoundary& bound, double target_vel) {
        // 边界条件：位置、速度、加速度、加加速度(Jerk)
        // 优化目标：最小化 Jerk + 速度跟踪误差
        return solveQP(bound, target_vel, {JERK_MIN, VEL_TRACK});
    }
    
    // 横向规划：五次多项式 d(s) = b0 + b1*s + b2*s² + b3*s³ + b4*s⁴ + b5*s⁵
    Polynomial5 planLateral(const LatBoundary& bound, double road_width) {
        // 边界条件：横向位置、导数、曲率
        // 约束：道路边界、最大曲率、曲率变化率
        return solveQP(bound, road_width, {CURV_LIMIT, BOUNDARY_PENALTY});
    }
    
    // 轨迹评估与选择
    FrenetTrajectory selectBest(const std::vector<FrenetTrajectory>& candidates) {
        FrenetTrajectory* best = nullptr;
        double min_cost = INFINITY;
        
        for (auto& traj : candidates) {
            if (!isFeasible(traj)) continue;  // 可行性检查
            
            double cost = 
                params.k_jerk * traj.jerk_cost +
                params.k_time * traj.time_cost +
                params.k_lat_dev * traj.lateral_deviation +
                params.k_collision * traj.collision_risk;
            
            if (cost < min_cost) {
                min_cost = cost;
                best = &traj;
            }
        }
        return best ? *best : FrenetTrajectory::emergencyStop();
    }
};

// 预计算查找表 (LUT) 加速多项式求值
class PolynomialLUT {
    std::vector<double> coeff_lut;  // 系数预计算
    std::vector<double> time_grid;  // 时间网格
    
public:
    void build(const Polynomial4& poly, double t_min, double t_max, int resolution) {
        time_grid = linspace(t_min, t_max, resolution);
        coeff_lut.resize(resolution);
        for (int i = 0; i < resolution; i++) {
            coeff_lut[i] = poly.evaluate(time_grid[i]);  // 预计算
        }
    }
    
    double query(double t) {
        // 二分查找 + 线性插值，比直接多项式求值快 3-5 倍
        int idx = binarySearch(time_grid, t);
        return lerp(coeff_lut[idx], coeff_lut[idx+1], 
                   (t - time_grid[idx]) / (time_grid[idx+1] - time_grid[idx]));
    }
};

// 通用对象池：预分配 + 对象复用，避免频繁 new/delete
template<typename T>
class ObjectPool {
private:
    std::vector<T*> free_list;
    std::vector<std::unique_ptr<T>> storage;
    size_t chunk_size = 1024;
    
public:
    T* acquire() {
        if (free_list.empty()) {
            // 批量分配新块，减少系统调用
            for (size_t i = 0; i < chunk_size; i++) {
                storage.emplace_back(std::make_unique<T>());
                free_list.push_back(storage.back().get());
            }
        }
        T* obj = free_list.back();
        free_list.pop_back();
        obj->reset();  // 🔑 关键：重置对象状态，避免脏数据
        return obj;
    }
    
    void release(T* obj) {
        if (!obj) return;
        obj->reset();  // 清理状态
        free_list.push_back(obj);
    }
    
    void clear() {
        free_list.clear();
        storage.clear();  // 如需彻底释放内存
    }
    
    size_t size() const { return storage.size() - free_list.size(); }
};

// 简化版 KD-Tree 接口（实际建议使用 nanoflann / FLANN）
class KDTree {
public:
    void insert(const Point& p, void* data) {
        points_.push_back(p);
        data_.push_back(data);
        dirty_ = true;
    }
    
    void* nearest(const Point& query) {
        rebuildIfNeeded();
        int best_idx = -1;
        double best_dist = INFINITY;
        // 实际实现：递归剪枝搜索，复杂度 O(log N)
        searchRecursive(root_, query, best_idx, best_dist);
        return data_[best_idx];
    }
    
    std::vector<void*> radiusSearch(const Point& query, double radius) {
        rebuildIfNeeded();
        std::vector<void*> results;
        searchRadiusRecursive(root_, query, radius, results);
        return results;
    }
    
private:
    void rebuildIfNeeded() {
        if (dirty_) {
            buildTree();  // O(N log N) 构建
            dirty_ = false;
        }
    }
    bool dirty_ = true;
    std::vector<Point> points_;
    std::vector<void*> data_;
};

// 线段 - 圆形障碍物连续碰撞检测
bool segmentCircleCCD(const Point& p1, const Point& p2, 
                      const Circle& obs, double robot_radius) {
    // 参数化线段: P(t) = p1 + t*(p2-p1), t∈[0,1]
    Vec2 d = p2 - p1;
    Vec2 f = p1 - obs.center;
    
    // 二次方程: ||P(t) - obs.center||² = (robot_radius)²
    double a = d.dot(d);
    double b = 2 * f.dot(d);
    double c = f.dot(f) - obs.radius * obs.radius;
    
    double discriminant = b*b - 4*a*c;
    if (discriminant < 0) return false;  // 无实根，不相交
    
    double t1 = (-b - sqrt(discriminant)) / (2*a);
    double t2 = (-b + sqrt(discriminant)) / (2*a);
    
    // 检查交点是否在线段参数范围内
    return (t1 >= 0 && t1 <= 1) || (t2 >= 0 && t2 <= 1);
}

// 状态插值工具（用于运动学约束的连续检测）
State lerpState(const State& s1, const State& s2, double t) {
    return {
        s1.x + t * (s2.x - s1.x),
        s1.y + t * (s2.y - s1.y),
        normalizeAngle(s1.theta + t * normalizeAngle(s2.theta - s1.theta)),
        INFINITY, nullptr
    };
}