曲线生成 | 图解B样条曲线生成原理(附ROS C++/Python/Matlab仿真)

本篇文章分享曲线生成 | 图解B样条曲线生成原理(附ROS C++/Python/Matlab仿真)，对你有帮助的话记得收藏一下，看Python资料网收获更多编程知识

0 专栏介绍

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

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

---

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

1 控制点计算之插值

设B样条曲线为

P ( t ) = ∑ i = 0 n − 1 p i N i , k ( t ) \boldsymbol{P}\left( t \right) =\sum_{i=0}^{n-1}{\boldsymbol{p}_iN_{i,k}\left( t \right)} P(t)=i=0∑n−1​pi​Ni,k​(t)

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

D j = P ( u j ) = ∑ i = 0 n − 1 p i N i , k ( u j ) , j = 0 , 1 , ⋯   , n − 1 \boldsymbol{D}_j=\boldsymbol{P}\left( u_j \right) =\sum_{i=0}^{n-1}{\boldsymbol{p}_iN_{i,k}\left( u_j \right)}, j=0,1,\cdots ,n-1 Dj​=P(uj​)=i=0∑n−1​pi​Ni,k​(uj​),j=0,1,⋯,n−1

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

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

其中

D = [ d 0 T d 1 T ⋮ d n − 1 T ] , P = [ p 0 T p 1 T ⋮ p n − 1 T ] , N = [ N 0 , k ( u 0 ) N 1 , k ( u 0 ) ⋯ N n − 1 , k ( u 0 ) N 0 , k ( u 1 ) N 1 , k ( u 1 ) ⋯ N n − 1 , k ( u 1 ) ⋮ ⋮ ⋱ ⋮ N 0 , k ( u n − 1 ) N 1 , k ( u n − 1 ) ⋯ N n − 1 , k ( u n − 1 ) ] \boldsymbol{D}=\left[ \begin{array}{c} \boldsymbol{d}_{0}^{T}\\ \boldsymbol{d}_{1}^{T}\\ \vdots\\ \boldsymbol{d}_{n-1}^{T}\\\end{array} \right] , \boldsymbol{P}=\left[ \begin{array}{c} \boldsymbol{p}_{0}^{T}\\ \boldsymbol{p}_{1}^{T}\\ \vdots\\ \boldsymbol{p}_{n-1}^{T}\\\end{array} \right] , \boldsymbol{N}=\left[ \begin{matrix} 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)\\ \vdots& \vdots& \ddots& \vdots\\ 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{matrix} \right] D= ​d0T​d1T​⋮dn−1T​​ ​,P= ​p0T​p1T​⋮pn−1T​​ ​,N= ​N0,k​(u0​)N0,k​(u1​)⋮N0,k​(un−1​)​N1,k​(u0​)N1,k​(u1​)⋮N1,k​(un−1​)​⋯⋯⋱⋯​Nn−1,k​(u0​)Nn−1,k​(u1​)⋮Nn−1,k​(un−1​)​ ​

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

2 控制点计算之近似

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

P ( t ) = N 0 , k ( t ) p 0 + ∑ i = 1 n − 2 p i N i , k ( t ) + N n − 1 , k ( t ) p n − 1 \boldsymbol{P}\left( t \right) =N_{0,k}\left( t \right) \boldsymbol{p}_0+\sum_{i=1}^{n-2}{\boldsymbol{p}_iN_{i,k}\left( t \right)}+N_{n-1,k}\left( t \right) \boldsymbol{p}_{n-1} P(t)=N0,k​(t)p0​+i=1∑n−2​pi​Ni,k​(t)+Nn−1,k​(t)pn−1​

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

f ( p 1 , p 2 , ⋯   , p n − 2 ) = ∑ i = 1 n − 2 [ d i − P ( u i ) ] 2 = ∑ i = 1 n − 2 [ ( d i − N 0 , k ( u i ) p 0 − N n − 1 , k ( u i ) p n − 1 ) ⏟ q i − ∑ j = 1 n − 2 p j N j , k ( u i ) ] 2 = ∑ i = 1 n − 2 [ q i T q i − 2 q i ∑ j = 1 n − 2 p j N j , k ( u i ) + ∑ j = 1 n − 2 p j N j , k ( u i ) ∑ j = 1 n − 2 p j N j , k ( u i ) ] \begin{aligned}f\left( \boldsymbol{p}_1,\boldsymbol{p}_2,\cdots ,\boldsymbol{p}_{n-2} \right) &=\sum_{i=1}^{n-2}{\left[ \boldsymbol{d}_i-\boldsymbol{P}\left( u_i \right) \right] ^2}\\&=\sum_{i=1}^{n-2}{\left[ \underset{{ \boldsymbol{q}_i}}{\underbrace{\left( \boldsymbol{d}_i-N_{0,k}\left( u_i \right) \boldsymbol{p}_0-N_{n-1,k}\left( u_i \right) \boldsymbol{p}_{n-1} \right) }}-\sum_{j=1}^{n-2}{\boldsymbol{p}_jN_{j,k}\left( u_i \right)} \right] ^2}\\&=\sum_{i=1}^{n-2}{\left[ \boldsymbol{q}_{i}^{T}\boldsymbol{q}_i-2\boldsymbol{q}_i\sum_{j=1}^{n-2}{\boldsymbol{p}_jN_{j,k}\left( u_i \right)}+\sum_{j=1}^{n-2}{\boldsymbol{p}_jN_{j,k}\left( u_i \right)}\sum_{j=1}^{n-2}{\boldsymbol{p}_jN_{j,k}\left( u_i \right)} \right]}\end{aligned} f(p1​,p2​,⋯,pn−2​)​=i=1∑n−2​[di​−P(ui​)]2=i=1∑n−2​ ​qi​ (di​−N0,k​(ui​)p0​−Nn−1,k​(ui​)pn−1​)​​−j=1∑n−2​pj​Nj,k​(ui​) ​2=i=1∑n−2​[qiT​qi​−2qi​j=1∑n−2​pj​Nj,k​(ui​)+j=1∑n−2​pj​Nj,k​(ui​)j=1∑n−2​pj​Nj,k​(ui​)]​

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

∂ f ( p 1 , p 2 , ⋯   , p n − 2 ) ∂ p g = ∑ i = 1 n − 2 [ − 2 q i N g , k ( u i ) + 2 N g , k ( u i ) ∑ j = 1 n − 2 p j N j , k ( u i ) ] \frac{\partial f\left( \boldsymbol{p}_1,\boldsymbol{p}_2,\cdots ,\boldsymbol{p}_{n-2} \right)}{\partial \boldsymbol{p}_g}=\sum_{i=1}^{n-2}{\left[ -2\boldsymbol{q}_iN_{g,k}\left( u_i \right) +2N_{g,k}\left( u_i \right) \sum_{j=1}^{n-2}{\boldsymbol{p}_jN_{j,k}\left( u_i \right)} \right]} ∂pg​∂f(p1​,p2​,⋯,pn−2​)​=i=1∑n−2​[−2qi​Ng,k​(ui​)+2Ng,k​(ui​)j=1∑n−2​pj​Nj,k​(ui​)]

令 ∂ f ( p 1 , p 2 , ⋯   , p n − 2 ) / ∂ p g = 0 {{\partial f\left( \boldsymbol{p}_1,\boldsymbol{p}_2,\cdots ,\boldsymbol{p}_{n-2} \right)}/{\partial \boldsymbol{p}_g}}=0 ∂f(p1​,p2​,⋯,pn−2​)/∂pg​=0可得

∑ i = 1 n − 2 [ ∑ j = 1 n − 2 N g , k ( u i ) N j , k ( u i ) ] p j = ∑ i = 1 n − 2 q i N g , k ( u i ) \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] \boldsymbol{p}_j}=\sum_{i=1}^{n-2}{\boldsymbol{q}_iN_{g,k}\left( u_i \right)} i=1∑n−2​[j=1∑n−2​Ng,k​(ui​)Nj,k​(ui​)]pj​=i=1∑n−2​qi​Ng,k​(ui​)

改写为矩阵形式

( N T N ) P = Q \left( \boldsymbol{N}^T\boldsymbol{N} \right) \boldsymbol{P}=\boldsymbol{Q} (NTN)P=Q

其中

P = [ p 1 T p 2 T ⋮ p n − 2 T ] , Q = [ ∑ i = 1 n − 2 q i N 1 , k ( u i ) ∑ i = 1 n − 2 q i N 2 , k ( u i ) ⋮ ∑ i = 1 n − 2 q i N n − 2 , k ( u i ) ] , N = [ N 1 , k ( u 1 ) N 2 , k ( u 1 ) ⋯ N n − 2 , k ( u 1 ) N 1 , k ( u 2 ) N 2 , k ( u 2 ) ⋯ N n − 2 , k ( u 2 ) ⋮ ⋮ ⋱ ⋮ N 1 , k ( u n − 2 ) N 2 , k ( u n − 2 ) ⋯ N n − 2 , k ( u n − 2 ) ] \boldsymbol{P}=\left[ \begin{array}{c} \boldsymbol{p}_{1}^{T}\\ \boldsymbol{p}_{2}^{T}\\ \vdots\\ \boldsymbol{p}_{n-2}^{T}\\\end{array} \right] , \boldsymbol{Q}=\left[ \begin{array}{c} \sum_{i=1}^{n-2}{\boldsymbol{q}_iN_{1,k}\left( u_i \right)}\\ \sum_{i=1}^{n-2}{\boldsymbol{q}_iN_{2,k}\left( u_i \right)}\\ \vdots\\ \sum_{i=1}^{n-2}{\boldsymbol{q}_iN_{n-2,k}\left( u_i \right)}\\\end{array} \right] , \boldsymbol{N}=\left[ \begin{matrix} 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)\\ \vdots& \vdots& \ddots& \vdots\\ 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{matrix} \right] P= ​p1T​p2T​⋮pn−2T​​ ​,Q= ​∑i=1n−2​qi​N1,k​(ui​)∑i=1n−2​qi​N2,k​(ui​)⋮∑i=1n−2​qi​Nn−2,k​(ui​)​ ​,N= ​N1,k​(u1​)N1,k​(u2​)⋮N1,k​(un−2​)​N2,k​(u1​)N2,k​(u2​)⋮N2,k​(un−2​)​⋯⋯⋱⋯​Nn−2,k​(u1​)Nn−2,k​(u2​)⋮Nn−2,k​(un−2​)​ ​

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

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深度学习实战》
• 《机器学习强基计划》
• 《运动规划实战精讲》
• …

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

版权声明：本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若内容造成侵权/违法违规/事实不符，请联系邮箱：jacktools123@163.com进行投诉反馈，一经查实，立即删除！