路径处理 | 关键点提取之Douglas–Peucker算法(附ROS C++/Python实现)

目录

• 0 专栏介绍
• 1 路径关键点提取
• 2 道格拉斯-普克算法Douglas–Peucker
• 3 算法实现与可视化

0 专栏介绍

🔥课设、毕设、创新竞赛必备！🔥本专栏涉及更高阶的运动规划算法轨迹优化实战，包括：曲线生成、碰撞检测、安全走廊、优化建模(QP、SQP、NMPC、iLQR等)、轨迹优化(梯度法、曲线法等)，每个算法都包含代码实现加深理解

🚀详情：运动规划实战进阶：轨迹优化篇

---

1 路径关键点提取

路径关键点提取也称为路径降采样，在路径规划中主要用于简化和优化路径表示。一方面，路径降采样可以去除冗余点，从而减少路径中的采样点数量，减小数据存储和传输的成本；另一方面，在路径跟踪和导航时，较少的点可以提高计算效率，减少实时处理的负担。在环境噪声敏感型的算法中，简化路径保留了路径的关键特征和形状，而滤除了噪点，可以增强在执行过程中对微小抖动或误差的鲁棒性

2 道格拉斯-普克算法Douglas–Peucker

道格拉斯-普克算法(Douglas–Peucker)是一种经典的路径关键点提取算法，其基于分治思想，以采样前后路径误差最小化为目标，提取路径关键点。

以下图为例阐述算法原理

1. 初始给定一组有序的路径点列 { p } 0 N − 1 \left\{ \boldsymbol{p} \right\} _{0}^{N-1} {p}0N−1​与误差阈值 δ \delta δ，其中 N N N是路径点数；
   

2. 以路径首末点组成的 p 0 p N − 1 \boldsymbol{p}_0\boldsymbol{p}_{N-1} p0​pN−1​为初始待处理线段，查找 p 0 p N − 1 \boldsymbol{p}_0\boldsymbol{p}_{N-1} p0​pN−1​之间的点列中离 p 0 p N − 1 \boldsymbol{p}_0\boldsymbol{p}_{N-1} p0​pN−1​最远的点，并判断该距离 d d d是否大于阈值 δ \delta δ，若 d > δ d>\delta d>δ则说明该点不能剪枝(否则剪枝前后曲线误差超过预期)；若 d ≤ δ d \le \delta d≤δ则说明该点可以忽略；如图所示需要保留 p 3 \boldsymbol{p}_3 p3​；

1. 对需要保留的节点进行分治，重复步骤(2)直到遍历结束；如图所示，分别以 p 0 p 3 \boldsymbol{p}_0\boldsymbol{p}_3 p0​p3​、 p 3 p N − 1 \boldsymbol{p}_3\boldsymbol{p}_{N-1} p3​pN−1​为待处理线段进行递归

1. 最终得到剪枝后的路径点列如图所示

3 算法实现与可视化

3.1 ROS C++仿真

核心代码如下所示

void RDP::process(const rmp::common::geometry::Points2d& path_in, rmp::common::geometry::Points2d& path_out)
{
  path_out.clear();
  int max_idx = -1;
  double max_dist = -1.0;
  int path_size = static_cast<int>(path_in.size());
  rmp::common::geometry::LineSegment2d line({ path_in[0].x(), path_in[0].y() },
                                            { path_in[path_size - 1].x(), path_in[path_size - 1].y() });
  for (int i = 1; i < path_size - 1; i++)
  {
    double d = line.distanceTo({ path_in[i].x(), path_in[i].y() });
    if (d > max_dist)
    {
      max_dist = d;
      max_idx = i;
    }
  }

  if (max_dist > delta_)
  {
    rmp::common::geometry::Points2d left_pts, right_pts;
    left_pts.reserve(max_idx + 1);
    right_pts.reserve(path_size - max_idx);
    for (int i = 0; i <= max_idx; i++)
    {
      left_pts.emplace_back(path_in[i].x(), path_in[i].y());
    }
    for (int i = max_idx; i < path_size; i++)
    {
      right_pts.emplace_back(path_in[i].x(), path_in[i].y());
    }

    rmp::common::geometry::Points2d left_result, right_result;
    process(left_pts, left_result);
    process(right_pts, right_result);
    path_out.insert(path_out.end(), left_result.begin(), left_result.end() - 1);
    path_out.insert(path_out.end(), right_result.begin(), right_result.end());
  }
  else
  {
    path_out.emplace_back(path_in[0].x(), path_in[0].y());
    path_out.emplace_back(path_in[path_size - 1].x(), path_in[path_size - 1].y());
  }
}

我们用红色的原点表示路径点，绿色曲线段表示路径。下面显示的是未处理的路径点，因为地图栅格分辨率是 0.05 m 0.05m 0.05m，所以看起来非常稠密

经过算法剪枝后的路径如下所示，可以看到既保留了原始路径的几何特征，又大幅度降低了路径冗余度

3.2 Python仿真

核心代码如下所示：

def rdp_rec(M, epsilon, dist=pldist):
    dmax = 0.0
    index = -1

    for i in xrange(1, M.shape[0]):
        d = dist(M[i], M[0], M[-1])

        if d > dmax:
            index = i
            dmax = d

    if dmax > epsilon:
        r1 = rdp_rec(M[:index + 1], epsilon, dist)
        r2 = rdp_rec(M[index:], epsilon, dist)

        return np.vstack((r1[:-1], r2))
    else:
        return np.vstack((M[0], M[-1]))

---

🔥 更多精彩专栏：

• 《ROS从入门到精通》
• 《Pytorch深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …