0 专栏介绍

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

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

---

在曲线生成 | 图解B样条曲线生成原理(基本概念与节点生成算法)中，我们介绍了B样条曲线的基本概念，例如基函数的递推、曲线支撑性原理、节点生成公式等。本文进一步计算控制点计算和曲线生成原理。

1 控制点计算之插值

设B样条曲线为

$$\mathbf{P}\left(t\right)=\sum _{i=0}^{n-1}{\mathbf{p}}_i{N}_{i,k}\left(t\right)$$

其中${\mathbf{p}}_i$是待求的控制节点。令参数向量满足其映射为数据点

$${\mathbf{D}}_j=\mathbf{P}\left(u_j\right)=\sum _{i=0}^{n-1}{\mathbf{p}}_i{N}_{i,k}\left(u_j\right),j=0,1,\cdots ,n-1$$

这里$N$个数据点可求解出$N$个控制点，所以$n=N$，可以矩阵化为

$$\mathbf{D}=\mathbf{NP}$$

其中

$$\mathbf{D}=\left[\begin{array}{c}{\mathbf{d}}_0^T\\ {\mathbf{d}}_1^T\\ ⋮\\ {\mathbf{d}}_{n-1}^T\end{array}\right],\mathbf{P}=\left[\begin{array}{c}{\mathbf{p}}_0^T\\ {\mathbf{p}}_1^T\\ ⋮\\ {\mathbf{p}}_{n-1}^T\end{array}\right],\mathbf{N}=\left[\begin{array}{cccc}{N}_{0,k}\left(u_0\right)& N_{1,k}\left(u_0\right)& \cdots & N_{n-1,k}\left(u_0\right)\\ N_{0,k}\left(u_1\right)& N_{1,k}\left(u_1\right)& \cdots & N_{n-1,k}\left(u_1\right)\\ ⋮& ⋮& \ddots & ⋮\\ N_{0,k}\left(u_{n-1}\right)& N_{1,k}\left(u_{n-1}\right)& \cdots & N_{n-1,k}\left(u_{n-1}\right)\end{array}\right]$$

求解该方程即可得到控制点。

2 控制点计算之近似

在插值问题中，插值曲线可能会在数据点间波动，而非紧密遵循数据多边形。为克服这个问题，可以放宽曲线必须穿过所有数据点的硬约束。除了第一个和最后一个数据点，曲线不必包含任何其他点，通过约束最小二乘误差来实现最优近似。考虑到节点向量中首末节点重复度为$k+1$时，B样条曲线穿过首末控制点，所以令${\mathbf{p}}_0={\mathbf{d}}_0$，${\mathbf{p}}_{n-1}={\mathbf{d}}_{n-1}$，则

$$\mathbf{P}\left(t\right)=N_{0,k}\left(t\right){\mathbf{p}}_0+\sum _{i=1}^{n-2}{\mathbf{p}}_i{N}_{i,k}\left(t\right)+N_{n-1,k}\left(t\right){\mathbf{p}}_{n-1}$$

从而可以计算最小二乘误差

$$\begin{array}{rl}f\left({\mathbf{p}}_1,{\mathbf{p}}_2,\cdots ,{\mathbf{p}}_{n-2}\right)& =\sum _{i=1}^{n-2}{\left[{\mathbf{d}}_i-\mathbf{P}\left(u_i\right)\right]}^2\\ & =\sum _{i=1}^{n-2}{\left[\underset{{\mathbf{q}}_i}{\underbrace{\left({\mathbf{d}}_i-N_{0,k}\left(u_i\right){\mathbf{p}}_0-N_{n-1,k}\left(u_i\right){\mathbf{p}}_{n-1}\right)}}-\sum _{j=1}^{n-2}{\mathbf{p}}_j{N}_{j,k}\left(u_i\right)\right]}^2\\ & =\sum _{i=1}^{n-2}\left[{\mathbf{q}}_i^T{\mathbf{q}}_i-2{\mathbf{q}}_i\sum _{j=1}^{n-2}{\mathbf{p}}_j{N}_{j,k}\left(u_i\right)+\sum _{j=1}^{n-2}{\mathbf{p}}_j{N}_{j,k}\left(u_i\right)\sum _{j=1}^{n-2}{\mathbf{p}}_j{N}_{j,k}\left(u_i\right)\right]\end{array}$$

将误差函数对${\mathbf{p}}_g\left(g=1,2,\cdots ,n-2\right)$求偏导，可得

$$\frac{\partial f\left({\mathbf{p}}_1,{\mathbf{p}}_2,\cdots ,{\mathbf{p}}_{n-2}\right)}{\partial {\mathbf{p}}_g}=\sum _{i=1}^{n-2}\left[-2{\mathbf{q}}_i{N}_{g,k}\left(u_i\right)+2N_{g,k}\left(u_i\right)\sum _{j=1}^{n-2}{\mathbf{p}}_j{N}_{j,k}\left(u_i\right)\right]$$

令$\partial f\left({\mathbf{p}}_1,{\mathbf{p}}_2,\cdots ,{\mathbf{p}}_{n-2}\right)/\partial {\mathbf{p}}_g=0$可得

$$\sum _{i=1}^{n-2}\left[\sum _{j=1}^{n-2}{N}_{g,k}\left(u_i\right)N_{j,k}\left(u_i\right)\right]{\mathbf{p}}_j=\sum _{i=1}^{n-2}{\mathbf{q}}_i{N}_{g,k}\left(u_i\right)$$

改写为矩阵形式

$$\left({\mathbf{N}}^T\mathbf{N}\right)\mathbf{P}=\mathbf{Q}$$

其中

$$\mathbf{P}=\left[\begin{array}{c}{\mathbf{p}}_1^T\\ {\mathbf{p}}_2^T\\ ⋮\\ {\mathbf{p}}_{n-2}^T\end{array}\right],\mathbf{Q}=\left[\begin{array}{c}{\sum }_{i=1}^{n-2}{\mathbf{q}}_i{N}_{1,k}\left(u_i\right)\\ {\sum }_{i=1}^{n-2}{\mathbf{q}}_i{N}_{2,k}\left(u_i\right)\\ ⋮\\ {\sum }_{i=1}^{n-2}{\mathbf{q}}_i{N}_{n-2,k}\left(u_i\right)\end{array}\right],\mathbf{N}=\left[\begin{array}{cccc}{N}_{1,k}\left(u_1\right)& N_{2,k}\left(u_1\right)& \cdots & N_{n-2,k}\left(u_1\right)\\ N_{1,k}\left(u_2\right)& N_{2,k}\left(u_2\right)& \cdots & N_{n-2,k}\left(u_2\right)\\ ⋮& ⋮& \ddots & ⋮\\ N_{1,k}\left(u_{n-2}\right)& N_{2,k}\left(u_{n-2}\right)& \cdots & N_{n-2,k}\left(u_{n-2}\right)\end{array}\right]$$

求解该方程即可得到控制点。

3 仿真实现

3.1 ROS C++实现

核心代码如下所示：

Points2d BSpline::interpolation(const Points2d points, const std::vector<double> param, const std::vector<double> knot)
{
  size_t n = points.size();
  Eigen::MatrixXd N = Eigen::MatrixXd::Zero(n, n);
  Eigen::MatrixXd D(n, 2);

  for (size_t i = 0; i < n; i++)
    for (size_t j = 0; j < n; j++)
      N(i, j) = baseFunction(j, order_, param[i], knot);
  N(n - 1, n - 1) = 1;

  for (size_t i = 0; i < n; i++)
  {
    D(i, 0) = points[i].first;
    D(i, 1) = points[i].second;
  }

  Eigen::MatrixXd C = N.inverse() * D;

  std::vector<std::pair<double, double>> control_points(n);

  for (size_t i = 0; i < n; i++)
    control_points[i] = { C(i, 0), C(i, 1) };

  return control_points;
}

3.2 Python实现

核心代码如下所示：

def approximation(self, points: list, param: list, knot: list):
	n = len(points)
	D = np.array(points)
	
	# heuristically setting the number of control points
	h = n - 1
	
	N = np.zeros((n, h))
	for i in range(n):
	    for j in range(h):
	        N[i][j] = self.baseFunction(j, self.k, param[i], knot)
	N_ = N[1 : n - 1, 1 : h - 1]
	
	qk = np.zeros((n - 2, 2))
	for i in range(1, n - 1):
	    qk[i - 1] = D[i, :] - N[i][0] * D[0, :] - N[i][h - 1] * D[-1, :]
	Q = N_.T @ qk
	
	P = np.linalg.inv(N_.T @ N_) @ Q
	P = np.insert(P, 0, D[0, :], axis=0)
	P = np.insert(P, len(P), D[-1, :], axis=0)
	
	return P

3.3 Matlab实现

核心代码如下所示：

function points = generation(knot, control_pts, param)
  n = ceil(1.0 / param.step);
  t = (0 : n - 1) / (n - 1);

  [m, ~] = size(control_pts);
  N = zeros(n, m);

  for i=1:n
    for j=1:m
      N(i, j) = baseFunction(j, param.order, t(i), knot);
    end
  end
  
  N(n, m) = 1.0;

  points = N * control_pts;
end

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

---

🔥 更多精彩专栏：

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

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