曲线生成 | 图解贝塞尔曲线生成原理(附ROS C++/Python/Matlab仿真)

0 专栏介绍

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

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

1 贝塞尔曲线的应用

贝塞尔曲线是一种数学曲线，由法国数学家皮埃尔·贝塞尔于1962年引入。它使用一组控制点来定义曲线的形状，这些控制点的位置和数量决定了曲线的特征。贝塞尔曲线的应用非常广泛：

2 图解贝塞尔曲线

设平面上存在 n n n个离散的控制节点，则贝塞尔曲线的阶数为 n − 1 n-1 n−1。这 n n n个节点按某个顺序依次联结形成特征多边形，一个特征多边形将递归地确定一条以比例系数 t ∈ [ 0 , 1 ] t \in [0,1 ] t∈[0,1]为参数的贝塞尔曲线。

曲线生成 | 图解贝塞尔曲线生成原理(附ROS C++/Python/Matlab仿真)-LMLPHP

如图所示为1阶贝塞尔曲线的生成过程，具体地，对于一阶贝塞尔曲线有

P 1 ( t ) = ( 1 − t ) p 0 + t p 1 \boldsymbol{P}_1\left( t \right) =\left( 1-t \right) \boldsymbol{p}_0+t\boldsymbol{p}_1 P1(t)=(1−t)p0+tp1

其中控制节点 p i = [ x i , y i ] T \boldsymbol{p}_i=\left[ x_i,y_i \right] ^T pi=[xi,yi]T。

对于二阶贝塞尔曲线，首先给定比例系数 t ∈ [ 0 , 1 ] t \in [0,1 ] t∈[0,1]，使

∣ p 0 a ∣ ∣ p 0 p 1 ∣ = ∣ p 1 b ∣ ∣ p 1 p 2 ∣ = ∣ a q ∣ ∣ a b ∣ \frac{|\boldsymbol{p}_0\boldsymbol{a}|}{|\boldsymbol{p}_0\boldsymbol{p}_1|}=\frac{|\boldsymbol{p}_1\boldsymbol{b}|}{|\boldsymbol{p}_1\boldsymbol{p}_2|}=\frac{|\boldsymbol{aq}|}{|\boldsymbol{ab}|} ∣p0p1∣∣p0a∣=∣p1p2∣∣p1b∣=∣ab∣∣aq∣

其中 q \boldsymbol{q} q落在由 a \boldsymbol{a} a、 b \boldsymbol{b} b确定的一阶贝塞尔曲线上， a \boldsymbol{a} a与 b \boldsymbol{b} b分别落在由 p 0 \boldsymbol{p}_0 p0、 p 1 \boldsymbol{p}_1 p1与 p 1 \boldsymbol{p}_1 p1、 p 2 \boldsymbol{p}_2 p2确定的一阶贝塞尔曲线上，因此 q \boldsymbol{q} q最终为二阶贝塞尔曲线上的一点，有

P 2 ( t ) = ( 1 − t ) a + t b \boldsymbol{P}_2\left( t \right) =\left( 1-t \right) \boldsymbol{a}+t\boldsymbol{b} P2(t)=(1−t)a+tb

即

P 2 ( t ) = ( 1 − t ) 2 p 0 + 2 t ( 1 − t ) p 1 + t 2 p 2 \boldsymbol{P}_2\left( t \right) =\left( 1-t \right) ^2\boldsymbol{p}_0+2t\left( 1-t \right) \boldsymbol{p}_1+t^2\boldsymbol{p}_2 P2(t)=(1−t)2p0+2t(1−t)p1+t2p2

如图所示

曲线生成 | 图解贝塞尔曲线生成原理(附ROS C++/Python/Matlab仿真)-LMLPHP

递推地，有

P ( t ) = ∑ i = 0 n − 1 p i B i , n − 1 ( t ) , t ∈ [ 0 , 1 ] \boldsymbol{P}\left( t \right) =\sum_{i=0}^{n-1}{\boldsymbol{p}_iB_{i,n-1}\left( t \right)}, t\in \left[ 0,1 \right] P(t)=i=0∑n−1piBi,n−1(t),t∈[0,1]

其中 p i ( i = 0 , ⋯ , n − 1 ) \boldsymbol{p}_i\left( i=0,\cdots ,n-1 \right) pi(i=0,⋯,n−1)为控制节点的有序序列， B i , n ( t ) = C n i t i ( 1 − t ) n − i , t ∈ [ 0 , 1 ] B_{i,n}\left( t \right) =C_{n}^{i}t^i\left( 1-t \right) ^{n-i},t\in \left[ 0,1 \right] Bi,n(t)=Cniti(1−t)n−i,t∈[0,1]称为伯恩斯坦多项式(Bernstein Polynomial)，可视为权重因子，即曲线上某点 P ( t ) \boldsymbol{P}\left( t \right) P(t)是控制节点序列的加权平均

3 贝塞尔曲线的性质

贝塞尔曲线具有非常多优良的性质，主要列举如下

归一性：各项系数和为1
凸包性：贝塞尔曲线始终被所有控制点形成的最小凸多边形所包含
端点性：由于 B 0 , n ( 0 ) = B n , n ( 1 ) = 1 B_{0,n}\left( 0 \right) =B_{n,n}\left( 1 \right) =1 B0,n(0)=Bn,n(1)=1，所以贝塞尔曲线始于 p 0 \boldsymbol{p}_0 p0终于 p n \boldsymbol{p}_n pn，但不经过中间控制节点，即为逼近而非插值
几何不变性：贝塞尔曲线的形状仅与特征多边形各顶点相对位置有关，与坐标系的选择无关
变差伸缩性：若贝塞尔曲线特征多边形是一个平面图形，则平面内任意直线与贝塞尔曲线交点的个数不多于该直线与特征多边形的交点个数
微分： P ′ ( t ) = n ∑ i = 1 n ( p i − p i − 1 ) B i − 1 , n − 1 ( t ) \boldsymbol{P}'\left( t \right) =n\sum\nolimits_{i=1}^n{\left( \boldsymbol{p}_i-\boldsymbol{p}_{i-1} \right) B_{i-1,n-1}\left( t \right)} P′(t)=n∑i=1n(pi−pi−1)Bi−1,n−1(t)，即 n n n阶贝塞尔曲线的导数是 n − 1 n-1 n−1阶贝塞尔曲线，控制节点为 q i = n ( p i + 1 − p i ) , i = 0 , ⋯ , n − 1 \boldsymbol{q}_i=n\left( \boldsymbol{p}_{i+1}-\boldsymbol{p}_i \right) , i=0,\cdots ,n-1 qi=n(pi+1−pi),i=0,⋯,n−1。特别地， P ′ ( 0 ) = n ( p 1 − p 0 ) \boldsymbol{P}'\left( 0 \right) =n\left( \boldsymbol{p}_1-\boldsymbol{p}_0 \right) P′(0)=n(p1−p0)、 P ′ ( 1 ) = n ( p n − p n − 1 ) \boldsymbol{P}'\left( 1 \right) =n\left( \boldsymbol{p}_n-\boldsymbol{p}_{n-1} \right) P′(1)=n(pn−pn−1)，即贝塞尔曲线首末位置切线方向与特征多边形首末边方向一致

4 算法仿真

4.1 ROS C++仿真

核心代码如下

Points2d Bezier::generation(Pose2d start, Pose2d goal)
{
  double sx, sy, syaw;
  double gx, gy, gyaw;
  std::tie(sx, sy, syaw) = start;
  std::tie(gx, gy, gyaw) = goal;

  int n_points = (int)(helper::dist(Point2d(sx, sy), Point2d(gx, gy)) / step_);
  Points2d control_pts = getControlPoints(start, goal);

  Points2d points;
  for (size_t i = 0; i < n_points; i++)
  {
    double t = (double)(i) / (double)(n_points - 1);
    points.push_back(bezier(t, control_pts));
  }

  return points;
}

其中bezier函数实现了伯恩斯坦多项式求和

Point2d Bezier::bezier(double t, Points2d control_pts)
{
  size_t n = control_pts.size() - 1;
  Point2d pt(0, 0);
  for (size_t i = 0; i < n + 1; i++)
  {
    pt.first += _comb(n, i) * std::pow(t, i) * std::pow(1 - t, n - i) * control_pts[i].first;
    pt.second += _comb(n, i) * std::pow(t, i) * std::pow(1 - t, n - i) * control_pts[i].second;
  }
  return pt;
}

4.2 Python仿真

核心代码如下所示

def generation(self, start_pose: tuple, goal_pose: tuple):
	sx, sy, _ = start_pose
	gx, gy, _ = goal_pose
	n_points = int(np.hypot(sx - gx, sy - gy) / self.step)
	control_points = self.getControlPoints(start_pose, goal_pose)

	return [self.bezier(t, control_points) for t in np.linspace(0, 1, n_points)], \
		   control_points

def bezier(self, t: float, control_points: list) ->np.ndarray:
	n = len(control_points) - 1
	control_points = np.array(control_points)
	return np.sum([comb(n, i) * t ** i * (1 - t) ** (n - i) * 
		control_points[i] for i in range(n + 1)], axis=0)

曲线生成 | 图解贝塞尔曲线生成原理(附ROS C++/Python/Matlab仿真)-LMLPHP

4.3 Matlab仿真

核心代码如下所示

function curve = generation(start, goal, param)
    sx = start(1); sy = start(2);
    gx = goal(1); gy = goal(2);
    
    n_points =  hypot(sx - gx, sy - gy) / param.step;
    control_pts = getControlPoints(start, goal, param);
    
    curve = [];
    for t=0:1 / n_points:1
        curve = [curve; bezier(t, control_pts)];
    end
end

function curve_pt = bezier(t, control_pts)
    [m, ~] = size(control_pts);
    n = m - 1;
    pt_x = 0; pt_y = 0;
    for i=0:n
        pt_x = pt_x + nchoosek(n, i) * power(t, i) * power(1 - t, n - i) * control_pts(i + 1, 1);
        pt_y = pt_y + nchoosek(n, i) * power(t, i) * power(1 - t, n - i) * control_pts(i + 1, 2);
    end
    curve_pt = [pt_x, pt_y];
end

曲线生成 | 图解贝塞尔曲线生成原理(附ROS C++/Python/Matlab仿真)-LMLPHP

🔥 更多精彩专栏：

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

Mr.Winter`