轨迹优化 | 基于Savitzky-Golay滤波的无约束路径平滑(附ROS C++/Python仿真)

0 专栏介绍

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

1 什么是Savitzky-Golay滤波？

Savitzky--Golay滤波器是一种数字滤波器，能够应用于一组数字数据点，用于平滑数据，即在不扭曲信号趋势的情况下提高数据的精度，如下图所示。Savitzky--Golay滤波器使用低阶多项式通过最小二乘法拟合相邻数据点的连续子集。当数据点是等间距时，可以找到最小二乘方程的解析解，形式为一组卷积系数，这些系数可以应用于所有数据子集，用于估算平滑信号在每个子集中心点的值。该方法可以扩展到2维和3维数据的处理。

2 Savitzky-Golay滤波推导

形式化地，针对由 K K K个点组成的离散路径 { x k } \left\{ x_k \right\} {xk}，构造以 x k x_k xk为中心，其相邻的共 2 M + 1 2M+1 2M+1个数据点组成的连续子集窗口 W = { x ^ i } i = − M M W=\left\{ \hat{x}_i \right\} _{i=-M}^{M} W={x^i}i=−MM。用 N N N次多项式

p ( i ) = ∑ n = 0 N a n i n p\left( i \right) =\sum_{n=0}^N{a_ni^n} p(i)=n=0∑Nanin

拟合 W W W内的数据，拟合残差为

ε = ∑ i = − M M ( p ( i ) − x ^ i ) 2 = ∑ i = − M M ( ∑ j = 0 N a j i j − x ^ i ) 2 \varepsilon =\sum_{i=-M}^M{\left( p\left( i \right) -\hat{x}i \right) ^2}=\sum{i=-M}^M{\left( \sum_{j=0}^N{a_ji^j}-\hat{x}_i \right) ^2} ε=i=−M∑M(p(i)−x^i)2=i=−M∑M(j=0∑Najij−x^i)2

为了使残差最小，令

∂ ε ∂ a = [ 2 ∑ i = − M M i 0 ( ∑ j = 0 N a j i j − x ^ i ) ⋯ 2 ∑ i = − M M i N ( ∑ j = 0 N a j i j − x ^ i ) ] ( N + 1 ) × 1 T = 0 \frac{\partial \varepsilon}{\partial \boldsymbol{a}}=\left[ \begin{matrix} 2\sum_{i=-M}^M{i^0\left( \sum_{j=0}^N{a_ji^j}-\hat{x}i \right)}& \cdots& 2\sum{i=-M}^M{i^N\left( \sum_{j=0}^N{a_ji^j}-\hat{x}_i \right)}\\\end{matrix} \right] {{\left( N+1 \right) \times 1}}^{T}=\mathbf{0} ∂a∂ε=[2∑i=−MMi0(∑j=0Najij−x^i)⋯2∑i=−MMiN(∑j=0Najij−x^i)](N+1)×1T=0

其中 a = [ a 0 a 1 ⋯ a N ] T \boldsymbol{a}=\left[ \begin{matrix} a_0& a_1& \cdots& a_N\\\end{matrix} \right] ^T a=[a0a1⋯aN]T为多项式系数向量

设只关联于预设的窗口大小 M M M和多项式拟合次数 N N N的参数矩阵

A = [ ( − M ) 0 ( − M ) 1 ⋯ ( − M ) N ⋮ ⋮ ⋱ ⋮ ( M ) 0 ( M ) 1 ⋯ ( M ) N ] ( 2 M + 1 ) × ( N + 1 ) \boldsymbol{A}=\left[ \begin{matrix} \left( -M \right) ^0& \left( -M \right) ^1& \cdots& \left( -M \right) ^N\\ \vdots& \vdots& \ddots& \vdots\\ \left( M \right) ^0& \left( M \right) ^1& \cdots& \left( M \right) ^N\\\end{matrix} \right] _{\left( 2M+1 \right) \times \left( N+1 \right)} A=⎣⎢⎡(−M)0⋮(M)0(−M)1⋮(M)1⋯⋱⋯(−M)N⋮(M)N⎦⎥⎤(2M+1)×(N+1)

则最小二乘关系可表示为

A T A a = A T x ^ ⇒ a = H x ^ \boldsymbol{A}^T\boldsymbol{Aa}=\boldsymbol{A}^T\boldsymbol{\hat{x}}\Rightarrow \boldsymbol{a}=\boldsymbol{H\hat{x}} ATAa=ATx^⇒a=Hx^

其中 H = ( A T A ) − 1 A T \boldsymbol{H}=\left( \boldsymbol{A}^T\boldsymbol{A} \right) ^{-1}\boldsymbol{A}^T H=(ATA)−1AT

由于每次平滑只输出中心的平滑信号，因此只需考虑 H \boldsymbol{H} H的第一行元素，称为Savitzky-Golay滤波器的卷积核

3 算法仿真与验证

3.1 ROS C++仿真

核心算法如下所示

cpp 复制代码

bool SavitzkyGolayPathProcessor::_applyFilterOverPath(const Points3d& path_in, Points3d& path_out)
{
  int path_size = static_cast<int>(path_in.size());
  if (path_size < kernel.size())
  {
    return true;
  }

  path_out.clear();
  auto pt_m3 = path_in[0];
  auto pt_m2 = path_in[0];
  auto pt_m1 = path_in[0];
  auto pt = path_in[1];
  auto pt_p1 = path_in[2];
  auto pt_p2 = path_in[3];
  auto pt_p3 = path_in[4];

  // First ang last point is fixed
  path_out.emplace_back(path_in[0].x(), path_in[0].y());
  for (unsigned int idx = 1; idx < path_size; idx++)
  {
    Point3d smooth_pt;
    if (!_applyFilter({ pt_m3, pt_m2, pt_m1, pt, pt_p1, pt_p2, pt_p3 }, smooth_pt))
    {
      return false;
    }
    pt_m3 = pt_m2;
    pt_m2 = pt_m1;
    pt_m1 = pt;
    pt = pt_p1;
    pt_p1 = pt_p2;
    pt_p2 = pt_p3;
    pt_p3 = idx + 4 < path_size - 1 ? path_in[idx + 4] : path_in.back();
    path_out.emplace_back(smooth_pt.x(), smooth_pt.y());
  }
  return true;
}

算法效果如下所示

3.2 Python仿真

核心算法如下所示

python 复制代码

def applyFilterOverAxes(self, waypoints: list) -> list:
    M = int((len(self.kernel) - 1) / 2)
    filtered_waypoints = [waypoints[0]]
    window = [waypoints[0] for _ in range(M)]
    window.append(waypoints[1])
    for i in range(M):
        window.append(waypoints[2 + i])

    # First point is fixed
    path_len = len(waypoints)
    for i in range(path_len):
        smooth_pt = self.applyFilter(window)
        filtered_waypoints.append((smooth_pt.x, smooth_pt.y))
        for j in range(2 * M):
            window[j] = window[j + 1]

        if i + M + 1 < path_len - 1:
            window[-1] = waypoints[i + M + 1]
        else:
            # Return the last point
            window[-1] = waypoints[-1]
    return filtered_waypoints