贝塞尔曲线基础

贝塞尔曲线于1962年由法国工程师皮埃尔·贝塞尔（Pierre Bézier）所广泛发表，他运用贝塞尔曲线来为汽车的主体进行设计。贝塞尔曲线最初由Paul de Casteljau于1959年运用de Casteljau演算法开发，以稳定数值的方法求出贝兹曲线。

贝塞尔曲线是如何被绘制出来的？

贝塞尔曲线需要提供几个点的参数，首先是 曲线的起点和终点。然后再提供 任意数量的控制点。如果控制点数量为 0，我们称之为线性贝塞尔；控制点数量为 1，则为二阶贝塞尔曲线；控制点数量为 2，则为三阶贝塞尔曲线，依此类推。

在拥有这些点之后，贝塞尔曲线算法会按照起点、控制点 1、控制点 2、…、终点的顺序，相邻两点依次连接，产生 n 条直线（这也是 n 阶贝塞尔曲线命名的来源）。然后我们会同时从每条直线的起点开始，向终点移动按比例拿到一个点。然后将这些点再连接，产生 n - 1 条直线。就这样，我们继续同样的操作，直到变成一条直线，然后我们按比例取到一个点，这个点就是曲线经过的点。当我们比例一点点变大（从 0 到 1），就拿到了曲线中间的所有点，最终绘制出完整的曲线。
例如：二阶贝塞尔曲线为：

三阶贝塞尔曲线为：

贝塞尔曲线表达式

贝塞尔曲线有一套完整的表达式，n次贝塞尔曲线的数学表达式为：
$$P(t)=\sum _{i=0}^n{B}_{i,n}(t)\cdot P_i$$
其中：
$P(t)$ 是在参数 t 时曲线上的点。
$B_{i,n}(t)$是伯恩斯坦多项式，定义为：$\left(\genfrac{}{}{0ex}{}{n}{i}\right)(1-t{)}^{n-i}{t}^i$
$Pi$​ 是第 i 个控制点。
n 是曲线的度数。
t 是一个在区间 [0, 1] 内的参数。
伯恩斯坦多项式的组合系数$\left(\genfrac{}{}{0ex}{}{n}{i}\right)$是组合数，表示从 n 个不同元素中取 i 个元素的不同组合的数目。

以下列举几种常见的贝塞尔曲线：

一阶贝塞尔曲线

一阶贝塞尔曲线的参数化公式如下：
$$P(t)=(1-t)\cdot P_0+t\cdot P_1$$
也称为线性贝塞尔曲线，是最简单的贝塞尔曲线形式。它实际上是一条直线段，连接两个控制点：起点 P0 和终点 P1。这种曲线的特点是非常简单且直观，因为它仅仅是一条直线。尽管一阶贝塞尔曲线不像更高阶的曲线那样具有复杂的弯曲形状，但它在动画和图形设计中仍然非常有用，尤其是在需要线性插值的情况下。

二阶贝塞尔曲线

二阶贝塞尔曲线的参数化公式如下：
$$P(t)=(1-t{)}^2\cdot P_0+2t(1-t)\cdot P_1+t^2\cdot P_2$$
二阶贝塞尔曲线，也称为二次贝塞尔曲线，是通过两个控制点定义的曲线。这种曲线的形状可以是一条光滑的曲线，根据控制点的不同位置，它可以形成一个抛物线形状。在 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\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{)}^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$$
相比于三阶曲线，四次贝塞尔曲线主要拥有以下特性：
平滑性：四次贝塞尔曲线在整个定义域内都是无限可微的，因此它非常平滑。
局部控制：曲线的一个部分只受其附近控制点的影响，这使得四次贝塞尔曲线在编辑时具有很好的局部控制性。
曲线形状：四次贝塞尔曲线可以创建出复杂的曲线形状，包括平滑的弯曲和转角。

贝塞尔曲线的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)