曲线生成 | 基于多项式插值的轨迹规划(附ROS C++/Python/Matlab仿真)

0 专栏介绍

🔥附C++/Python/Matlab全套代码🔥课程设计、毕业设计、创新竞赛必备！详细介绍全局规划(图搜索、采样法、智能算法等)；局部规划(DWA、APF等)；曲线优化(贝塞尔曲线、B样条曲线等)。

🚀详情：图解自动驾驶中的运动规划(Motion Planning)，附几十种规划算法

---

1 多项式插值

多项式插值(polynomial interpolation)基于一元多项式进行曲线插值，可以保证微分约束的连续性，使轨迹平滑、机械冲击小。多项式插值的应用场景非常广泛，例如

• 信号处理：在数字信号处理中，多项式插值可以用于重建缺失的信号数据，如图像或音频；
• 数据拟合：在数据分析中，多项式插值可以用于拟合数据，从而探究变量之间的关系；
• 数值微积分：在求解微积分问题时，多项式插值可以用于计算积分或导数，因为它可以在给定区间内精确地近似函数；
• 工程设计：多项式插值可以用于工程设计中的机器人轨迹规划、飞行器控制等领域，因为它可以通过已知的起点和终点来生成平滑的路径

必须指出，当使用等距多项式插值时，随着多项式阶数升高，可能造成函数解析区间过小，产生龙格现象(runge phenomenon)：插值两端产生剧烈振荡。因此多项式插值一般采用三次、五次或七次。

2 多项式插值轨迹规划

对路径进行多项式插值时，需要给定机器人在首末位置的位姿以及速度、加速度等微分项作为约束条件。若要求在时间$t_0$内经过两个路径点$\mathbf{\theta }\left(0\right)$、$\mathbf{\theta }\left(t_0\right)$，再考虑首末点的速度约束$\dot{\mathbf{\theta }}\left(0\right)$、$\dot{\mathbf{\theta }}\left(t_0\right)$共四个自由度，由三次多项式可完全确定

$$\mathbf{\theta }\left(t\right)={\mathbf{a}}_0+{\mathbf{a}}_{1}t+{\mathbf{a}}_2{t}^2+{\mathbf{a}}_3{t}^3$$

其中多项式系数由位置约束和速度约束确定

$$\begin{gathered} \left[\begin{array}{c}\mathbf{\theta }\left(0\right)\\ \mathbf{\theta }\left(t_0\right)\\ \dot{\mathbf{\theta }}\left(0\right)\\ \dot{\mathbf{\theta }}\left(t_0\right)\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& t_0& t_0^2& t_0^3\\ 0& 1& 0& 0\\ 0& 1& 2t_0& 3t_0^2\end{array}\right]\left[\begin{array}{c}{\mathbf{a}}_0\\ {\mathbf{a}}_1\\ {\mathbf{a}}_2\\ {\mathbf{a}}_3\end{array}\right] \\ ?\left[\begin{array}{c}{\mathbf{a}}_0\\ {\mathbf{a}}_1\\ {\mathbf{a}}_2\\ {\mathbf{a}}_3\end{array}\right]=\left[\begin{array}{c}\mathbf{\theta }\left(0\right)\\ \dot{\mathbf{\theta }}\left(0\right)\\ \frac{3}{t_0^2}\left[\mathbf{\theta }\left(t_0\right)?\mathbf{\theta }\left(0\right)\right]?\frac{2}{t_0}\dot{\mathbf{\theta }}\left(0\right)?\frac{1}{t_0}\dot{\mathbf{\theta }}\left(t_0\right)\\ ?\frac{2}{t_0^3}\left[\mathbf{\theta }\left(t_0\right)?\mathbf{\theta }\left(0\right)\right]+\frac{1}{t_0^2}\left[\dot{\mathbf{\theta }}\left(0\right)?\dot{\mathbf{\theta }}\left(t_0\right)\right]\end{array}\right] \end{gathered}$$

再考虑首末点的加速度约束$\ddot{\mathbf{\theta }}\left(0\right)$、$\ddot{\mathbf{\theta }}\left(t_0\right)$共六个自由度，由五次多项式曲线可完全确定

$$\mathbf{\theta }\left(t\right)={\mathbf{a}}_0+{\mathbf{a}}_{1}t+{\mathbf{a}}_2{t}^2+{\mathbf{a}}_3{t}^3+{\mathbf{a}}_4{t}^4+{\mathbf{a}}_5{t}^5$$

其中多项式系数由位置约束、速度约束和加速度约束确定

$$\begin{gathered} \left[\begin{array}{c}\mathbf{\theta }\left(0\right)\\ \mathbf{\theta }\left(t_0\right)\\ \dot{\mathbf{\theta }}\left(0\right)\\ \dot{\mathbf{\theta }}\left(t_0\right)\\ \ddot{\mathbf{\theta }}\left(0\right)\\ \ddot{\mathbf{\theta }}\left(t_0\right)\end{array}\right]=\left[\begin{array}{cccccc}l1& 0& 0& 0& 0& 0\\ 1& t_0& t_0^2& t_0^3& t_0^4& t_0^5\\ 0& 1& 0& 0& 0& 0\\ 0& 1& 2t_0& 3t_0^2& 4t_0^3& 5t_0^4\\ 0& 0& 2& 0& 0& 0\\ 0& 0& 2& 6t_0& 12t_0^2& 20t_0^3\end{array}\right]\left[\begin{array}{c}{\mathbf{a}}_0\\ {\mathbf{a}}_1\\ {\mathbf{a}}_2\\ {\mathbf{a}}_3\\ {\mathbf{a}}_4\\ {\mathbf{a}}_5\end{array}\right] \\ ?\left[\begin{array}{c}{\mathbf{a}}_0\\ {\mathbf{a}}_1\\ {\mathbf{a}}_2\\ {\mathbf{a}}_3\\ {\mathbf{a}}_4\\ {\mathbf{a}}_5\end{array}\right]=\left[\begin{array}{c}\mathbf{\theta }\left(0\right)\\ \dot{\mathbf{\theta }}\left(0\right)\\ \frac{1}{2}\ddot{\mathbf{\theta }}\left(0\right)\\ \frac{10}{t_0^3}\left[\mathbf{\theta }\left(t_0\right)?\mathbf{\theta }\left(0\right)\right]?\frac{1}{t_0^2}\left[4\dot{\mathbf{\theta }}\left(t_0\right)+6\dot{\mathbf{\theta }}\left(0\right)\right]+\frac{1}{2t_0}\left[\ddot{\mathbf{\theta }}\left(t_0\right)?3\ddot{\mathbf{\theta }}\left(0\right)\right]\\ \frac{?15}{t_0^4}\left[\mathbf{\theta }\left(t_0\right)?\mathbf{\theta }\left(0\right)\right]+\frac{1}{t_0^3}\left[7\dot{\mathbf{\theta }}\left(t_0\right)+8\dot{\mathbf{\theta }}\left(0\right)\right]?\frac{1}{2t_0^2}\left[2\ddot{\mathbf{\theta }}\left(t_0\right)?3\ddot{\mathbf{\theta }}\left(0\right)\right]\\ \frac{6}{t_0^5}\left[\mathbf{\theta }\left(t_0\right)?\mathbf{\theta }\left(0\right)\right]?\frac{1}{t_0^4}\left[3\dot{\mathbf{\theta }}\left(t_0\right)+3\dot{\mathbf{\theta }}\left(0\right)\right]+\frac{1}{2t_0^3}\left[\ddot{\mathbf{\theta }}\left(t_0\right)?\ddot{\mathbf{\theta }}\left(0\right)\right]\end{array}\right] \end{gathered}$$

若考虑更多时间微分约束，则需要更高次数的多项式曲线，但多项式阶数的升高会影响计算效率，从而降低实时性。

3 算法仿真

3.1 ROS C++仿真

核心代码如下所示

Poly::Poly(std::tuple<double, double, double> state_0, std::tuple<double, double, double> state_1, double t)
{
  double x0, v0, a0;
  double xt, vt, at;
  std::tie(x0, v0, a0) = state_0;
  std::tie(xt, vt, at) = state_1;

  Eigen::Matrix3d A;
  A << std::pow(t, 3), std::pow(t, 4), std::pow(t, 5), 3 * std::pow(t, 2), 4 * std::pow(t, 3), 5 * std::pow(t, 4),
      6 * t, 12 * std::pow(t, 2), 20 * std::pow(t, 3);

  Eigen::Vector3d b(xt - x0 - v0 * t - a0 * t * t / 2, vt - v0 - a0 * t, at - a0);
  Eigen::Vector3d x = A.lu().solve(b);

  // Quintic polynomial coefficient
  p0 = x0;
  p1 = v0;
  p2 = a0 / 2.0;
  p3 = x(0);
  p4 = x(1);
  p5 = x(2);
}

这部分实现的是五次多项式的系数计算

在轨迹规划过程中，只需要遍历路径点，在相邻路径点间进行插值即可

bool Polynomial::run(const Poses2d points, Points2d& path)
{
  if (points.size() < 4)
    return false;
  else
  {
    path.clear();

    // generate velocity and acceleration constraints heuristically
    std::vector<double> v(points.size(), 1.0);
    v[0] = 0.0;

    std::vector<double> a;
    for (size_t i = 0; i < points.size() - 1; i++)
      a.push_back((v[i + 1] - v[i]) / 5);
    a.push_back(0.0);

    for (size_t i = 0; i < points.size() - 1; i++)
    {
      PolyTrajectory traj;
      PolyState start(std::get<0>(points[i]), std::get<1>(points[i]), std::get<2>(points[i]), v[i], a[i]);
      PolyState goal(std::get<0>(points[i + 1]), std::get<1>(points[i + 1]), std::get<2>(points[i + 1]), v[i + 1],
                     a[i + 1]);

      generation(start, goal, traj);

      Points2d path_i = traj.toPath();
      path.insert(path.end(), path_i.begin(), path_i.end());
    }

    return !path.empty();
  }
}

3.2 Python仿真

核心代码如下所示

class Poly:
	'''
	Polynomial interpolation solver
	'''
	def __init__(self, state0: tuple, state1: tuple, t: float) -> None:
	    x0, v0, a0 = state0
	    xt, vt, at = state1
	
	    A = np.array([[t ** 3, t ** 4, t ** 5],
	                  [3 * t ** 2, 4 * t ** 3, 5 * t ** 4],
	                  [6 * t, 12 * t ** 2, 20 * t ** 3]])
	    b = np.array([xt - x0 - v0 * t - a0 * t ** 2 / 2,
	                  vt - v0 - a0 * t,
	                  at - a0])
	    X = np.linalg.solve(A, b)
	
	    # Quintic polynomial coefficient
	    self.p0 = x0
	    self.p1 = v0
	    self.p2 = a0 / 2.0
	    self.p3 = X[0]
	    self.p4 = X[1]
	    self.p5 = X[2]

3.3 Matlab仿真

核心代码如下所示

for T = param.t_min : param.step : param.t_max
	A = [power(T, 3), power(T, 4), power(T, 5);
	        3 * power(T, 2), 4 * power(T, 3), 5 * power(T, 4);
	        6 * T, 12 * power(T, 2), 20 * power(T, 3)];
	b = [gx - sx - sv_x * T - sa_x * T * T / 2;
	        gv_x - sv_x - sa_x * T;
	        ga_x - sa_x];
	X = A \ b;
	px = [sx, sv_x, sa_x / 2, X(1), X(2), X(3)];
	    
	b = [gy - sy - sv_y * T - sa_y * T * T / 2;
	        gv_y - sv_y - sa_y * T;
	        ga_y - sa_y];
	X = A \ b;
	py = [sy, sv_y, sa_y / 2, X(1), X(2), X(3)];
	
	
	for t=0 : param.dt : T + param.dt
	    traj_x = [traj_x, x(px, t)];
	    traj_y = [traj_y, x(py, t)];
	    
	    vx = dx(px, t); vy = dx(py, t);
	    traj_v = [traj_v, hypot(vx, vy)];
	    
	    ax = ddx(px, t); ay = ddx(py, t);
	    a = hypot(ax, ay);
	    if (length(traj_v) >= 2) && (traj_v(end) < traj_v(end - 1))
	        a = -a;
	    end
	    traj_a = [traj_a, a];
	    
	    jx = dddx(px, t); jy = dddx(py, t);
	    j = hypot(jx, jy);
	    if (length(traj_a) >= 2) && (traj_a(end) < traj_a(end - 1))
	        j = -j;
	    end
	    traj_j = [traj_j, j];
	end
	
	if ((max(abs(traj_a)) < param.max_acc) && (max(abs(traj_j)) < param.max_jerk))
	    break;
	else
	    traj_x = []; traj_y = [];
	    traj_v = []; traj_a = []; traj_j = [];
	end
	end

完整工程代码请联系下方博主名片获取

---

🔥 更多精彩专栏：

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

👇源码获取 · 技术交流 · 抱团学习 · 咨询分享 请联系👇