0 专栏介绍

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

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

---

1 贝塞尔曲线的应用

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

• 计算机图形学：贝塞尔曲线可以用于绘制平滑的曲线和曲面。在计算机图形学中，它们被广泛用于绘制二维和三维图形对象，如曲线、路径、字体等。贝塞尔曲线具有良好的平滑性和灵活性，在图形渲染和模型构建方面发挥着重要作用；
• CAD 设计：贝塞尔曲线在计算机辅助设计中起到关键作用。设计师可以使用贝塞尔曲线创建和编辑复杂的曲线形状，如汽车外形、船体曲线、建筑物外观等。贝塞尔曲线的控制点可以通过拖动和调整来改变曲线的形状，使设计过程更加灵活和直观；
• 动画和游戏开发：贝塞尔曲线提供了一种方便的方法来定义和控制动画路径和运动轨迹。动画师可以使用贝塞尔曲线来创建平滑的动画路径，让角色和物体按照指定的路径移动。在游戏开发中，贝塞尔曲线也常用于实现精确的物体运动轨迹和碰撞检测；
• 字体设计：贝塞尔曲线被广泛应用于字体设计中。每个字母、数字或符号都可以由一组贝塞尔曲线组成，通过调整和连接这些曲线，可以创建出各种字体形状和风格。贝塞尔曲线的灵活性使得字体设计者能够轻松地创建出各种自然流畅的字符形状。

2 图解贝塞尔曲线

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

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

$${\mathbf{P}}_1\left(t\right)=\left(1-t\right){\mathbf{p}}_0+t{\mathbf{p}}_1$$

其中控制节点${\mathbf{p}}_i={\left[x_i,y_i\right]}^T$。

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

$$\frac{|{\mathbf{p}}_0\mathbf{a}|}{|{\mathbf{p}}_0{\mathbf{p}}_1|}=\frac{|{\mathbf{p}}_1\mathbf{b}|}{|{\mathbf{p}}_1{\mathbf{p}}_2|}=\frac{|\mathbf{aq}|}{|\mathbf{ab}|}$$

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

$${\mathbf{P}}_2\left(t\right)=\left(1-t\right)\mathbf{a}+t\mathbf{b}$$

即

$${\mathbf{P}}_2\left(t\right)={\left(1-t\right)}^2{\mathbf{p}}_0+2t\left(1-t\right){\mathbf{p}}_1+t^2{\mathbf{p}}_2$$

如图所示

递推地，有

$$\mathbf{P}\left(t\right)=\sum _{i=0}^{n-1}{\mathbf{p}}_i{B}_{i,n-1}\left(t\right),t\in \left[0,1\right]$$

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

3 贝塞尔曲线的性质

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

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

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)

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

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