梯度下降路径平滑算法详解（C++）

算法概述：

        梯度下降平滑算法是一种常用的路径后处理方法，用于优化通过路径规划算法（如混合A*）得到的初始路径。其基本思想是将路径看作一系列点，然后通过迭代调整这些点的位置，使得路径同时满足平滑性（如曲率小）和安全（远离障碍物）的要求。

---

算法实现关键步骤：

1. 目标函数：定义一个包含三个部分的目标函数：

    平滑项：相邻三个点形成的两个向量的夹角变化小（即路径曲率小）

    紧致项：调整后的点不要偏离原始点太远

    障碍物项：路径点要远离障碍物

2. 迭代更新：对路径上的每个点（起点和终点固定除外）计算目标函数的梯度，然后沿着梯度的反方向（即下降方向）调整点的位置。

3. 数学表达：

  设路径点为：$P_0, P_1, ..., P_{n-1}$，其中$P_0$和$P_{n-1}$固定。

  目标函数：$O = \alpha \cdot O_{smooth} + \beta \cdot O_{compact} + \gamma \cdot O_{obstacle}$

  平滑项：$O_{smooth} = \sum_{i=1}^{n-2} \| (P_{i+1} - P_i) - (P_i - P_{i-1}) \|^2$ （即二阶差分）

  紧致项：$O_{compact} = \sum_{i=1}^{n-2} \| P_i - P_i^{original} \|^2$ （与原始点距离）

  障碍物项：$O_{obstacle} = \sum_{i=1}^{n-2} \exp(-d_i / \sigma)$ （$d_i$为到最近障碍物的距离，$\sigma$为尺度参数）

4. 梯度计算：

  平滑项梯度：对点$P_i$，其梯度为 $2[ (P_i - P_{i-1}) - (P_{i+1} - P_i) ] + 2[ (P_i - P_{i+1}) - (P_{i-1} - P_i) ]$，化简得：$4(P_i - P_{i-1}/2 - P_{i+1}/2)$

  紧致项梯度：$2(P_i - P_i^{original})$

  障碍物项梯度：$-\frac{1}{\sigma} \exp(-d_i/\sigma) \cdot \nabla d_i$，其中$\nabla d_i$是距离场梯度（指向障碍物反方向）

5. 更新规则：

$P_i = P_i - \eta \nabla O_i$，其中$\eta$为学习率。

---

算法原理：

算法原理

梯度下降路径平滑算法是一种基于优化的路径后处理方法，用于改进混合A*等路径规划算法生成的初始路径。其核心思想是通过迭代调整路径点的位置，使路径在满足安全性的前提下更加平滑。

目标函数

算法通过最小化一个包含三个关键项的目标函数来实现路径平滑：

1. 平滑项 (Smoothness Term):

   • 惩罚路径点的曲率变化

   • 计算公式: (P_{i+1} - P_i) - (P_i - P_{i-1})

   • 使路径点趋向于与前后的中点重合，从而拉直路径

2. 紧致项 (Compactness Term):

   • 惩罚路径点偏离原始路径

   • 计算公式: P_i - P_i^{original}

   • 确保平滑后的路径不会过度偏离原始安全通道

3. 障碍物项 (Obstacle Term):

   • 惩罚路径点靠近障碍物

   • 计算公式: exp(-d_i / σ)

   • 其中d_i是到最近障碍物的距离，σ控制影响范围

优化过程

目标函数：O = α·O_smooth + β·O_compact + γ·O_obstacle

1. 初始化平滑路径为原始路径

2. 对于每次迭代：
   a. 对每个路径点（起点和终点除外）计算目标函数的梯度
   b. 沿梯度反方向更新路径点位置

3. 重复直到收敛或达到最大迭代次数

---

算法实现：

#include 
#include 
#include 
#include 

// 二维点结构
struct Point {
    double x, y;
    Point(double x = 0, double y = 0) : x(x), y(y) {}
    
    // 向量加法
    Point operator+(const Point& other) const {
        return Point(x + other.x, y + other.y);
    }
    
    // 向量减法
    Point operator-(const Point& other) const {
        return Point(x - other.x, y - other.y);
    }
    
    // 标量乘法
    Point operator*(double scalar) const {
        return Point(x * scalar, y * scalar);
    }
};

// 计算两点间欧氏距离
double distance(const Point& a, const Point& b) {
    double dx = a.x - b.x;
    double dy = a.y - b.y;
    return std::sqrt(dx*dx + dy*dy);
}

// 梯度下降路径平滑类
class GradientDescentSmoother {
public:
    // 构造函数：设置权重参数
    GradientDescentSmoother(double alpha, double beta, double gamma, 
                          double sigma, double eta, int iterations)
        : alpha(alpha), beta(beta), gamma(gamma), 
          sigma(sigma), eta(eta), iterations(iterations) {}

    // 平滑路径主函数
    std::vector smoothPath(const std::vector& original_path, 
                                 const std::vector>& grid) {
        // 创建路径的副本用于调整（起点和终点固定）
        std::vector path = original_path;
        int n = path.size();
        if (n <= 2) return path; // 路径太短则直接返回

        // 迭代优化
        for (int iter = 0; iter < iterations; ++iter) {
            // 复制当前路径用于同时更新
            std::vector new_path = path;

            // 遍历路径中间点（跳过起点和终点）
            for (int i = 1; i < n - 1; ++i) {
                // 1. 计算平滑项梯度
                Point grad_smooth = computeSmoothnessGradient(path, i);
                
                // 2. 计算紧致项梯度
                Point grad_compact = computeCompactnessGradient(path[i], original_path[i]);
                
                // 3. 计算障碍物项梯度
                Point grad_obstacle = computeObstacleGradient(path[i], grid);
                
                // 总梯度 = 各项加权和
                Point total_grad = grad_smooth * alpha + grad_compact * beta + grad_obstacle * gamma;
                
                // 梯度下降更新：新位置 = 原位置 - 学习率 * 梯度
                new_path[i] = new_path[i] - total_grad * eta;
                
                // 确保点不越界
                new_path[i].x = std::max(0.0, std::min(new_path[i].x, static_cast(grid[0].size() - 1)));
                new_path[i].y = std::max(0.0, std::min(new_path[i].y, static_cast(grid.size() - 1)));
            }

            // 更新路径
            path = new_path;
        }
        return path;
    }

private:
    // 平滑项权重
    double alpha;
    // 紧致项权重
    double beta;
    // 障碍物项权重
    double gamma;
    // 障碍物项尺度参数
    double sigma;
    // 学习率
    double eta;
    // 迭代次数
    int iterations;

    // 计算平滑项梯度
    Point computeSmoothnessGradient(const std::vector& path, int i) {
        /*
        平滑项公式：O_smooth = ||(P_{i+1}-P_i) - (P_i - P_{i-1})||^2
        对P_i求导：4*(P_i - (P_{i-1}+P_{i+1})/2)
        物理意义：使P_i趋向于P_{i-1}和P_{i+1}的中点
        */
        Point prev = path[i-1];
        Point next = path[i+1];
        Point midpoint((prev.x + next.x) / 2.0, (prev.y + next.y) / 2.0);
        return (path[i] - midpoint) * 4.0;
    }

    // 计算紧致项梯度
    Point computeCompactnessGradient(const Point& current, const Point& original) {
        /*
        紧致项公式：O_compact = ||P_i - P_i^{original}||^2
        对P_i求导：2*(P_i - P_i^{original})
        物理意义：使P_i趋向于原始位置
        */
        return (current - original) * 2.0;
    }

    // 计算障碍物项梯度
    Point computeObstacleGradient(const Point& p, const std::vector>& grid) {
        /*
        障碍物项公式：O_obstacle = exp(-d_i / σ)
        对P_i求导：-(1/σ) * exp(-d_i / σ) * ∇d_i
        其中∇d_i是距离场的梯度（指向远离障碍物的方向）
        */
        
        // 找到最近障碍物
        Point closest_obstacle;
        double min_dist = findMinDistanceToObstacle(p, grid, closest_obstacle);
        
        // 如果没有找到障碍物，返回零向量
        if (min_dist > 10.0 * sigma) {
            return Point(0, 0);
        }
        
        // 计算距离梯度向量（指向远离障碍物的方向）
        Point dist_grad(p.x - closest_obstacle.x, p.y - closest_obstacle.y);
        double norm = distance(Point(0, 0), dist_grad);
        
        // 归一化梯度向量
        if (norm > 1e-5) {
            dist_grad.x /= norm;
            dist_grad.y /= norm;
        }
        
        // 计算障碍物项权重
        double obstacle_weight = -(1.0 / sigma) * std::exp(-min_dist / sigma);
        
        return dist_grad * obstacle_weight;
    }

    // 找到给定点到最近障碍物的距离和位置
    double findMinDistanceToObstacle(const Point& p, 
                                    const std::vector>& grid,
                                    Point& closest_obstacle) {
        double min_dist = std::numeric_limits::max();
        int grid_width = grid[0].size();
        int grid_height = grid.size();
        
        // 搜索范围：为避免计算整个地图，只搜索附近区域
        int search_radius = 5; // 搜索半径（可根据实际情况调整）
        int x_min = std::max(0, static_cast(p.x) - search_radius);
        int x_max = std::min(grid_width - 1, static_cast(p.x) + search_radius);
        int y_min = std::max(0, static_cast(p.y) - search_radius);
        int y_max = std::min(grid_height - 1, static_cast(p.y) + search_radius);
        
        // 遍历搜索区域内的所有网格单元
        for (int y = y_min; y <= y_max; ++y) {
            for (int x = x_min; x <= x_max; ++x) {
                if (grid[y][x]) { // 如果是障碍物
                    // 计算到障碍物中心的距离
                    Point obstacle_center(x + 0.5, y + 0.5);
                    double dist = distance(p, obstacle_center);
                    
                    // 更新最小距离和最近障碍物
                    if (dist < min_dist) {
                        min_dist = dist;
                        closest_obstacle = obstacle_center;
                    }
                }
            }
        }
        
        // 如果没有找到障碍物，返回一个大数
        if (min_dist == std::numeric_limits::max()) {
            return 10.0 * sigma; // 足够大的距离
        }
        
        return min_dist;
    }
};

// 可视化函数（简单文本显示）
void visualizePath(const std::vector& path, const std::vector>& grid) {
    int height = grid.size();
    int width = grid[0].size();
    
    // 创建显示网格
    std::vector> display(height, std::vector(width, '.'));
    
    // 标记障碍物
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            if (grid[y][x]) {
                display[y][x] = '#';
            }
        }
    }
    
    // 标记路径
    for (const auto& p : path) {
        int x = static_cast(p.x);
        int y = static_cast(p.y);
        if (x >= 0 && x < width && y >= 0 && y < height) {
            // 起点和终点特殊标记
            if (&p == &path.front()) {
                display[y][x] = 'S';
            } else if (&p == &path.back()) {
                display[y][x] = 'G';
            } else {
                display[y][x] = '*';
            }
        }
    }
    
    // 打印网格
    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; ++x) {
            std::cout << display[y][x] << ' ';
        }
        std::cout << '\n';
    }
}

int main() {
    // 创建10x10的地图（0=可通行，1=障碍物）
    const int width = 10, height = 10;
    std::vector> grid(height, std::vector(width, false));
    
    // 设置障碍物（形成一条通道）
    for (int y = 3; y <= 6; ++y) {
        grid[y][3] = true;
        grid[y][7] = true;
    }
    grid[4][4] = true;
    grid[5][5] = true;
    
    // 创建一条初始路径（故意设计得曲折）
    std::vector original_path = {
        Point(1.0, 1.0),  // 起点
        Point(2.0, 2.0),
        Point(3.0, 3.0),
        Point(4.0, 4.5),  // 避开障碍物
        Point(5.0, 4.0),
        Point(6.0, 4.0),
        Point(7.0, 4.0),
        Point(8.0, 5.0),
        Point(8.5, 8.0),  // 终点
    };

    // 创建平滑器（参数需要根据实际情况调整）
    GradientDescentSmoother smoother(
        0.5,   // alpha: 平滑项权重
        0.2,   // beta: 紧致项权重
        0.3,   // gamma: 障碍物项权重
        0.5,   // sigma: 障碍物项尺度参数
        0.1,   // eta: 学习率
        100    // iterations: 迭代次数
    );

    // 平滑路径
    std::vector smoothed_path = smoother.smoothPath(original_path, grid);

    // 打印结果
    std::cout << "原始路径:\n";
    visualizePath(original_path, grid);
    
    std::cout << "\n平滑后路径:\n";
    visualizePath(smoothed_path, grid);

    return 0;
}

---

算法实现详解

1. 平滑项梯度计算

Point computeSmoothnessGradient(const std::vector& path, int i) {
    Point prev = path[i-1];
    Point next = path[i+1];
    Point midpoint((prev.x + next.x) / 2.0, (prev.y + next.y) / 2.0);
    return (path[i] - midpoint) * 4.0;
}

• 数学原理：平滑项 O_smooth = ||(P_{i+1}-P_i) - (P_i - P_{i-1})||^2

• 物理意义：使当前点P_i趋向于前一点P_{i-1}和后一点P_{i+1}的中点

• 梯度计算：对P_i求导得到 4*(P_i - (P_{i-1} + P_{i+1})/2)

2. 紧致项梯度计算

Point computeCompactnessGradient(const Point& current, const Point& original) {
    return (current - original) * 2.0;
}

• 数学原理：紧致项 O_compact = ||P_i - P_i^{original}||^2

• 物理意义：防止平滑后的路径过度偏离原始路径，确保仍在安全通道内

• 梯度计算：对P_i求导得到 2*(P_i - P_i^{original})

3. 障碍物项梯度计算

Point computeObstacleGradient(const Point& p, const std::vector>& grid) {
    // 找到最近障碍物
    Point closest_obstacle;
    double min_dist = findMinDistanceToObstacle(p, grid, closest_obstacle);
    
    // 计算距离梯度向量（指向远离障碍物的方向）
    Point dist_grad(p.x - closest_obstacle.x, p.y - closest_obstacle.y);
    double norm = distance(Point(0, 0), dist_grad);
    
    // 归一化梯度向量
    if (norm > 1e-5) {
        dist_grad.x /= norm;
        dist_grad.y /= norm;
    }
    
    // 计算障碍物项权重
    double obstacle_weight = -(1.0 / sigma) * std::exp(-min_dist / sigma);
    
    return dist_grad * obstacle_weight;
}

• 数学原理：障碍物项 O_obstacle = exp(-d_i / σ)

• 物理意义：当路径点靠近障碍物时产生排斥力，排斥力随距离指数衰减

• 梯度计算：-(1/σ) * exp(-d_i / σ) * ∇d_i，其中∇d_i是距离场的梯度

4. 参数调整建议

1. α (平滑项权重):

   • 控制路径的平滑程度

   • 值越大，路径越直

   • 典型值范围: 0.3-1.0

2. β (紧致项权重):

   • 控制路径与原始路径的贴合程度

   • 值越大，平滑路径越接近原始路径

   • 典型值范围: 0.1-0.5

3. γ (障碍物项权重):

   • 控制路径远离障碍物的程度

   • 值越大，路径越远离障碍物

   • 典型值范围: 0.2-0.8

4. σ (障碍物尺度参数):

   • 控制障碍物排斥力的作用范围

   • 值越大，障碍物影响范围越广

   • 典型值: 网格大小的0.5-2倍

5. η (学习率):

   • 控制每次更新的步长

   • 值太大会导致震荡，值太小收敛慢

   • 典型值范围: 0.05-0.2

6. 迭代次数:

   • 取决于路径长度和期望平滑度

   • 典型值: 50-200次

---

算法特点与优化方向

优势：

1. 计算效率高：仅需O(N·M)计算量（N路径点，M障碍物）

2. 易于实现：核心算法仅需几十行代码

3. 可扩展性强：可添加更多约束项（如最大曲率、方向约束等）

局限性：

1. 局部最优：梯度下降可能陷入局部最优解

2. 参数敏感：效果依赖于参数调优

3. 障碍物处理：简单搜索最近障碍物可能不够高效

优化方向：

1. 距离场加速：预计算整个地图的距离场，避免实时搜索障碍物

2. 曲率约束：添加最大曲率限制，确保路径符合车辆运动学

3. 自适应学习率：根据收敛情况动态调整学习率

4. 多分辨率优化：先粗后细的多层次平滑策略

5. 物理约束：结合车辆动力学模型进行平滑

梯度下降路径平滑算法是一种高效实用的路径优化方法，能够显著改善混合A*等算法生成的初始路径。通过合理调整各项权重参数，可以在路径平滑度、安全性和计算效率之间取得良好平衡。在实际应用中，建议结合具体场景需求进行参数调优，并考虑添加额外约束以满足特定运动规划需求。