无人机航线平滑处理（b样插值）

一、b样条插值

简单粗暴：B-样条曲线入门 - 知乎 (zhihu.com)

【轨迹规划】3：准均匀B样条曲线C++实现_向蓝的博客-CSDN博客

 上图红色为折线；黄色为贝塞尔曲线，根据所有控制点确定最终曲线；绿色是b样条曲线，它会根据控制点去弯折。

二、代码

//b样条插值
class BSplineCurve {
public:
	vector control_points; // 控制点,数量未定
	vector knots; // 结点向量
	int degree; // 曲线次数
	//Constructor
	BSplineCurve(vector points, int k) {
		control_points = points;
		degree = k;
		// 初始化结点向量, m = n + 1+ k   ,m+1节点数量, n+1控制点数量 ,k 次数
		int num_knots = control_points.size() + degree + 1;
		double delta = 1.0 / (double)(num_knots - 2 * degree - 1);
		for (int i = 0; i < num_knots; i++) {
			if (i < degree + 1) {
				knots.push_back(0.0);
			}
			else if (i >= num_knots - degree) {
				knots.push_back(1.0);
			}
			else {
				knots.push_back(knots.back() + delta);
			}
			//cout << "knot" << i << "=" << knots[i] << endl;
		}
	}

	// 计算基函数值
	float BasisFunction(int i, int k, double u) {
		if (k == 0) {
			if (u >= knots[i] && u < knots[i + 1]) {
				return 1;
			}
			else {
				return 0;
			}
		}
		float a = 0.0, b = 0.0;
		if (knots[i + k] - knots[i] != 0.0) {
			a = (u - knots[i]) / (knots[i + k] - knots[i]);
		}
		if (knots[i + k + 1] - knots[i + 1] != 0.0) {
			b = (knots[i + k + 1] - u) / (knots[i + k + 1] - knots[i + 1]);
		}
		return a * BasisFunction(i, k - 1, u) + b * BasisFunction(i + 1, k - 1, u);
	}
	// 计算均匀 B 样条曲线上的点
	Point CalculatePoint(float u) {
		Point res;
		for (int i = 0; i < control_points.size(); ++i) {
			float basis = BasisFunction(i, degree, u);
			res.x += control_points[i].x * basis;
			res.y += control_points[i].y * basis;
			res.z += control_points[i].z * basis;
		}
		return res;
	}
};

用法

//绘制b样条插值曲线
int degree = 2; // 曲线次数
BSplineCurve curve(pts, degree);
step = 0.005;
for (float u = 0; u <= 1; u += step) {
	Point p = curve.CalculatePoint(u);
}