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

目录

• 0 专栏介绍
• 1 控制点计算之插值
• 2 控制点计算之近似
• 3 仿真实现
• • 3.1 ROS C++实现
  • 3.2 Python实现
  • 3.3 Matlab实现

0 专栏介绍

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

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

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

1 控制点计算之插值

设B样条曲线为

P ( t )= ∑ i=0n−1piN 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)

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

D j=P ( uj)= ∑ i=0n−1piN 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= [ d0Td1T⋮d n − 1T],P= [ p0Tp1T⋮p n − 1T],N= [N 0,k( u0) N 1,k( u0)⋯N n−1,k( u0) N 0,k( u1) N 1,k( u1)⋯N n−1,k( u1)⋮⋮⋱ ⋮ 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样条曲线穿过首末控制点，所以令 p0= d0 \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=1n−2piN 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 ( p1, p2, ⋯  , p n − 2) = ∑ i=1n−2[ d i−P ( ui)]2 = ∑ i=1n−2[ ( di− N 0 , k( u i)p0− N n − 1 , k( u i)p n − 1)⏟q i − ∑ j=1n−2pjN j , k( u i) ]2 = ∑ i=1n−2[ qiTqi− 2 qi∑ j = 1 n − 2 p j N j,k( ui)+ ∑ j = 1 n − 2 p j N j,k( ui)∑ j = 1 n − 2 p j N j,k( ui)]\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​)]​

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

∂f ( p1, p2, ⋯  , p n − 2)∂ p g = ∑ i=1n−2[ − 2 qiN g , k( u i)+ 2 N g , k( u i)∑ j = 1 n − 2 p j N j,k( ui)]\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 ) / ∂ pg = 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=1n−2[ ∑ j=1n−2N g , k( u i)N j , k( u i) ]pj = ∑ i=1n−2qiN 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​)

改写为矩阵形式

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

其中

P= [ p1Tp2T⋮p n − 2T],Q= [∑ i=1n−2qiN 1 , k( u i)∑ i=1n−2qiN 2 , k( u i) ⋮ ∑ i=1n−2qiN n − 2 , k( u i) ],N= [N 1,k( u1) N 2,k( u1)⋯N n−2,k( u1) N 1,k( u2) N 2,k( u2)⋯N n−2,k( u2)⋮⋮⋱ ⋮ 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 pointsh = n - 1N = 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 @ qkP = np.linalg.inv(N_.T @ N_) @ QP = 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:nfor j=1:mN(i, j) = baseFunction(j, param.order, t(i), knot);endendN(n, m) = 1.0;points = N * control_pts;end

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

更多精彩专栏：

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

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