贝塞尔曲线于1962年由法国工程师皮埃尔·贝塞尔（Pierre Bézier）所广泛发表，他运用贝塞尔曲线来为汽车的主体进行设计。贝塞尔曲线最初由Paul de Casteljau于1959年运用de Casteljau演算法开发，以稳定数值的方法求出贝兹曲线。
贝塞尔曲线是如何被绘制出来的？
贝塞尔曲线需要提供几个点的参数，首先是 曲线的起点和终点。然后再提供 任意数量的控制点。如果控制点数量为 0，我们称之为线性贝塞尔；控制点数量为 1，则为二阶贝塞尔曲线；控制点数量为 2，则为三阶贝塞尔曲线，依此类推。
在拥有这些点之后，贝塞尔曲线算法会按照起点、控制点 1、控制点 2、…、终点的顺序，相邻两点依次连接，产生 n 条直线（这也是 n 阶贝塞尔曲线命名的来源）。然后我们会同时从每条直线的起点开始，向终点移动按比例拿到一个点。然后将这些点再连接，产生 n - 1 条直线。就这样，我们继续同样的操作，直到变成一条直线，然后我们按比例取到一个点，这个点就是曲线经过的点。当我们比例一点点变大（从 0 到 1），就拿到了曲线中间的所有点，最终绘制出完整的曲线。
例如：二阶贝塞尔曲线为：
三阶贝塞尔曲线为：
贝塞尔曲线表达式
贝塞尔曲线有一套完整的表达式，n次贝塞尔曲线的数学表达式为：
P
(
t
)
=
∑
i
=
0
n
B
i
,
n
(
t
)
⋅
P
i
P(t) = \sum_{i=0}^{n} B_{i,n}(t) \cdot P_i
P(t)=i=0∑nBi,n(t)⋅Pi
其中：
P
(
t
)
P(t)
P(t) 是在参数 t 时曲线上的点。
B
i
,
n
(
t
)
B_{i,n}(t)
Bi,n(t)是伯恩斯坦多项式，定义为：
(
n
i
)
(
1
−
t
)
n
−
i
t
i
\binom{n}{i} (1 - t)^{n-i} t^i
(in)(1−t)n−iti
P
i
Pi
Pi 是第 i 个控制点。
n 是曲线的度数。
t 是一个在区间 [0, 1] 内的参数。
伯恩斯坦多项式的组合系数
(
n
i
)
\binom{n}{i}
(in)是组合数，表示从 n 个不同元素中取 i 个元素的不同组合的数目。
以下列举几种常见的贝塞尔曲线：
一阶贝塞尔曲线
一阶贝塞尔曲线的参数化公式如下：
P
(
t
)
=
(
1
−
t
)
⋅
P
0
+
t
⋅
P
1
P(t) = (1 - t) \cdot P_0 + t \cdot P_1
P(t)=(1−t)⋅P0+t⋅P1
也称为线性贝塞尔曲线，是最简单的贝塞尔曲线形式。它实际上是一条直线段，连接两个控制点：起点 P0 和终点 P1。这种曲线的特点是非常简单且直观，因为它仅仅是一条直线。尽管一阶贝塞尔曲线不像更高阶的曲线那样具有复杂的弯曲形状，但它在动画和图形设计中仍然非常有用，尤其是在需要线性插值的情况下。
二阶贝塞尔曲线
二阶贝塞尔曲线的参数化公式如下：
P
(
t
)
=
(
1
−
t
)
2
⋅
P
0
+
2
t
(
1
−
t
)
⋅
P
1
+
t
2
⋅
P
2
P(t) = (1 - t)^2 \cdot P_0 + 2t(1 - t) \cdot P_1 + t^2 \cdot P_2
P(t)=(1−t)2⋅P0+2t(1−t)⋅P1+t2⋅P2
二阶贝塞尔曲线，也称为二次贝塞尔曲线，是通过两个控制点定义的曲线。这种曲线的形状可以是一条光滑的曲线，根据控制点的不同位置，它可以形成一个抛物线形状。在 t 的不同值下，曲线的表现如下：
● 当 t=0 时，P(t) 等于 P0，曲线开始于点 P0。
● 当 t=1 时，P(t) 等于 P2，曲线结束于点 P2。
对于 t 的其他值，P(t) 将位于由 P0、P1 和 P2 定义的曲线上的某一点。随着 t 从 0 增加到 1，曲线将从 P0 开始，根据 P1 的位置向上或向下弯曲，然后平滑地过渡到 P2。
三阶贝塞尔曲线
三阶贝塞尔曲线的参数化公式如下：
P
(
t
)
=
(
1
−
t
)
3
⋅
P
0
+
3
(
1
−
t
)
2
t
⋅
P
1
+
3
(
1
−
t
)
t
2
⋅
P
2
+
t
3
⋅
P
3
P(t) = (1 - t)^3 \cdot P_0 + 3(1 - t)^2 t \cdot P_1 + 3(1 - t) t^2 \cdot P_2 + t^3 \cdot P_3
P(t)=(1−t)3⋅P0+3(1−t)2t⋅P1+3(1−t)t2⋅P2+t3⋅P3
三阶贝塞尔曲线是一种通过四个控制点定义的曲线。这种曲线可以形成更加复杂的弯曲形状，比二阶贝塞尔曲线具有更高的灵活性。三次贝塞尔曲线在计算机图形学、动画、字体设计等领域中非常流行，因为它们能够创建出平滑且富有表现力的曲线形状。通过调整控制点的位置，设计师可以精确控制曲线的弯曲程度和方向。
四阶贝塞尔曲线
四阶贝塞尔曲线的参数化公式如下：
P
(
t
)
=
(
1
−
t
)
4
⋅
P
0
+
4
(
1
−
t
)
3
t
⋅
P
1
+
6
(
1
−
t
)
2
t
2
⋅
P
2
+
4
(
1
−
t
)
t
3
⋅
P
3
+
t
4
⋅
P
4
P(t) = (1 - t)^4 \cdot P_0 + 4(1 - t)^3 t \cdot P_1 + 6(1 - t)^2 t^2 \cdot P_2 + 4(1 - t) t^3 \cdot P_3 + t^4 \cdot P_4
P(t)=(1−t)4⋅P0+4(1−t)3t⋅P1+6(1−t)2t2⋅P2+4(1−t)t3⋅P3+t4⋅P4
相比于三阶曲线，四次贝塞尔曲线主要拥有以下特性：
平滑性：四次贝塞尔曲线在整个定义域内都是无限可微的，因此它非常平滑。
局部控制：曲线的一个部分只受其附近控制点的影响，这使得四次贝塞尔曲线在编辑时具有很好的局部控制性。
曲线形状：四次贝塞尔曲线可以创建出复杂的曲线形状，包括平滑的弯曲和转角。
贝塞尔曲线的C++简单实现
限于篇幅这里只展示三阶曲线绘制，其他曲线可以根据输入变化以及公式套用即可：
#include "ros/ros.h"
#include <stdio.h>
#include <math.h>
#include <vector>
#include <string.h>
#include <iostream>
#include <iomanip>
#include <nav_msgs/Path.h>
class CubicBezier
{
public:
ros::Publisher path_pub;
CubicBezier(float x1, float y1, float x2, float y2, float x3, float y3, float x4, float y4)
{
const int precision = 100;
points[0] = x1;
points[1] = y1;
points[2] = x2;
points[3] = y2;
points[4] = x3;
points[5] = y3;
points[6] = x4;
points[7] = y4;
coords = getCoordsArray(precision);
ros::NodeHandle n;
path_pub = n.advertise<nav_msgs::Path>("bezier_path",1,true);
}
~CubicBezier()
{
std::cout << "CubicBezier的析构函数" << std::endl;
}
float points[8] = {0,0,0.3,0.1,0.3,0.1,1,1};
std::vector<std::pair<float, float>> coords;
//定义三阶贝塞尔公式 当t变化时计算曲线上对应的点坐标
std::pair<float, float> getCoord(float t)
{
if (t < 0 || t>1)
{
return std::pair<float, float>(0, 0);
}
const float t1 = 1 - t;
const float coefficient1 = pow(t1, 3);
const float coefficient2 = 3 * t * pow(t1, 2);
const float coefficient3 = 3 * t1 * pow(t, 2);
const float coefficient4 = pow(t, 3);
const float px = coefficient1 * points[0] + coefficient2 * points[2] + coefficient3 * points[4] + coefficient4 * points[6];
const float py = coefficient1 * points[1] + coefficient2 * points[3] + coefficient3 * points[5] + coefficient4 * points[7];
std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(3);
//std::cout << "point：(" << px<<"," << py<<")" <<std::endl;
std::pair<float, float> point(px, py);
//std::cout << "point：(" << point.first << "," << point.second << ")" << std::endl;
return point;
}
//设置一定的步长 将每次生成的点记录到vector里
std::vector<std::pair<float, float>> getCoordsArray(int precision)
{
const float step = 1.0 / (precision + 1);
std::vector<std::pair<float, float>> result;
for (int t = 0; t <= precision +1; t++) {
float a = t * step;
result.push_back(std::make_pair<float, float>((getCoord(a)).first, (getCoord(a)).second));
//std::cout << "1:" << (getCoord(a)).first << "," << (getCoord(a)).second << std::endl;
std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(3);
//std::cout << "2 (" << result[t].first<<","<< result[t].second<<")" << std::endl;
}
coords = result;
return result;
}
//通过近似处点的直线方程利用x来确定y的取值
float getY(float x)
{
if (x >= 1) return 1;
if (x <= 0) return 0;
int startX = 0;
//std::cout << "点数：" << coords.size() << std::endl;
for (int i = 0; i < coords.size(); i++)
{
if (coords[i].first >= x)
{
startX = i;
break;
}
}
const std::pair<float, float> axis1 = coords[startX];
const std::pair<float, float> axis2 = coords[startX - 1];
const float k = (axis2.second - axis1.second) / (axis2.first - axis1.first);
const float b = axis1.second - k * axis1.first;
return k * x + b;
}
void getpath()
{
nav_msgs::Path path;
path.header.frame_id = "map";
for(float i=0;i<1;i+=0.01)
{
std::pair<float, float> point1 = getCoord(i);
std::cout << "point: (" << point1.first<<","<< point1.second<<")"<< std::endl;
geometry_msgs::PoseStamped point;
point.header.frame_id = "map";
point.pose.position.x = point1.first;
point.pose.position.y = point1.second;
path.poses.push_back(point);
}
path_pub.publish(path);
}
};
void test()
{
CubicBezier Bezier(0.2,0,0.2,0.3, 0, 0.5,0,1);
std::cout << "Bezier: ";
for (int i = 0; i < 8; i++)
{
std::cout << Bezier.points[i] << " ";
}
std::cout << std::endl;
while(true)
{
ROS_INFO("debug");
Bezier.getpath();
ros::Duration(0.5).sleep();
}
std::pair<float, float> point1 = Bezier.getCoord(0.3);
std::cout << "point: (" << point1.first<<","<< point1.second<<")"<< std::endl;
std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(3);
std::cout << "Y: " << Bezier.getY(0.7) << std::endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "bezier_node");
ROS_INFO("bezier_node start");
test();
ros::spin();
return 0;
}
在ROS下运行上面的程序，可以在RVIZ中看到对应曲线：
其中，曲线的点来自于：
CubicBezier Bezier(0.2,0,0.2,0.3, 0, 0.5,0,1);
这里确定了输入的起点坐标（0.2,0）以及终点坐标（0，1）且对应的两个控制点分别为(0.2,0.3)与(0, 0.5)