B-spline 控制点生成

一、概述

本知识文档详细介绍了如何将离散的轨迹点转换为 B-spline 的控制点的方法，包括其背后的数学原理和相应的代码实现。B-spline 曲线在多个领域，如计算机图形学、机器人路径规划、动画制作等，有着广泛的应用，它能够生成平滑且灵活的曲线，而控制点的生成过程对于曲线的最终形状和性质起着决定性的作用。

二、B-Spline 曲线的数学表示

B-spline 曲线可以用以下公式表示：
$\mathbf{p}(s(t)) = s(t)^T M_p \mathbf{q}_m$
其中：

$(\mathbf{p}(s(t)))$ ：表示 B-spline 曲线在参数 $(t)$ 处的点，它是曲线在不同参数值下的位置描述。
$(s (t))$ ：B-spline 基函数向量，该向量是关于参数 $(t)$ 的函数，其值取决于参数 $(t)$ 的取值，决定了基函数在不同位置的贡献大小。
$M_p)$ ：B-spline 的基矩阵，它定义了基函数的组合方式，是一个固定的矩阵，描述了基函数之间的内在联系。
$(\mathbf{q}_m)$ ：B-spline 的控制点向量，它是我们要生成的核心元素，决定了曲线的形状，通过调整控制点的位置，可以改变 B-spline 曲线的形状和走势。

目标：
给定一组离散的轨迹点（包含位置信息）以及边界条件（速度和加速度），通过反演过程求解出控制点 $\mathbf{q}_m )$ ，使得 B-spline 曲线尽可能拟合这些轨迹点，并满足边界条件。

三、代码实现细节

（一）构造矩阵 $(A)$

矩阵 $(A)$ 是线性方程组 $\mathbf{q}_m = \mathbf{b} )$ 中的系数矩阵，用于建立控制点与轨迹点之间的线性关系，包含位置约束、速度约束和加速度约束。

1. 位置约束

for (int i = 0; i < K; ++i) 
  A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();

作用：
- 确保 B-spline 曲线在每个采样点 $(i)$ 处经过给定的轨迹点。
解释：
- 每个轨迹点 $\mathbf{p}_i )$ 被表示为三个连续控制点的线性组合，权重来自 B-spline 的位置基函数，具体公式为：
  $\mathbf{p}_i = \frac{1}{6} \cdot \left( \mathbf{q}_{i-1} + 4 \cdot \mathbf{q}_i + \mathbf{q}_{i+1} \right)$
- 这意味着每个轨迹点是三个控制点的加权平均，权重分别为 $\frac{1}{6} )$ 、 $\frac{4}{6} )$ 和 $\frac{1}{6} )$ 。
- 在代码中，A.block(i, i, 1, 3) 表示在矩阵 $(A)$ 的第 $(i)$ 行，从第 $(i)$ 列开始的 1 行 3 列的子矩阵，赋值为 $\frac{1}{6} )$ 乘以基函数向量的转置，以反映位置约束在矩阵中的体现。

2. 速度约束

A.block(K, 0, 1, 3)         = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();

作用：
- 确保 B-spline 曲线在起点和终点处的速度与给定的边界条件相匹配。
解释：
- 起点速度约束：
  $\mathbf{v}_{\text{start}} = \frac{1}{2 \cdot ts} \cdot \left( \mathbf{q}_1 - \mathbf{q}_{-1} \right)$
  由于控制点索引从 0 开始，可能会涉及虚拟控制点或进行适当的边界处理。
- 终点速度约束：
  $\mathbf{v}_{\text{end}} = \frac{1}{2 \cdot ts} \cdot \left( \mathbf{q}_{K+1} - \mathbf{q}_{K-1} \right)$
- 代码中，vrow 是速度基函数向量，表示控制点对速度的影响。通过调整矩阵 $(A)$ 的相应位置，使控制点的差分与给定的速度相匹配，将速度约束融入到矩阵 $(A)$ 中。

3. 加速度约束

A.block(K + 2, 0, 1, 3)     = (1 / ts / ts) * arow.transpose();
A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();

作用：
- 确保 B-spline 曲线在起点和终点处的加速度与给定的边界条件相匹配。
解释：
- 起点加速度约束：
  $\mathbf{a}_{\text{start}} = \frac{1}{ts^2} \cdot \left( \mathbf{q}_{-1} - 2 \cdot \mathbf{q}_0 + \mathbf{q}_1 \right)$
- 终点加速度约束：
  $\mathbf{a}_{\text{end}} = \frac{1}{ts^2} \cdot \left( \mathbf{q}_{K-1} - 2 \cdot \mathbf{q}_K + \mathbf{q}_{K+1} \right)$
- 代码中，arow 是加速度基函数向量，表示控制点对加速度的影响。通过调整矩阵 $(A)$ 的相应位置，使控制点的二阶差分与给定的加速度相匹配，将加速度约束融入矩阵 $(A)$ 中。

（二）构造右侧向量 $\mathbf{b} )$

向量 $\mathbf{b} )$ 包含了所有的约束条件，包括轨迹点的位置以及边界条件（速度和加速度）。

1. 轨迹点约束

for (int i = 0; i < K; ++i) {
  bx(i) = point_set[i].x;
  by(i) = point_set[i].y;
  bz(i) = point_set[i].z;
}

作用：
- 将每个离散的轨迹点的位置信息填入向量 $\mathbf{b} )$ 的前 $(K)$ 个元素。
解释：
- $\mathbf{b} )$ 的前 $(K)$ 行对应于位置约束，每一行分别对应轨迹点在 $(x)$ 、 $(y)$ 、 $(z)$ 方向上的坐标，这些轨迹点是 B-spline 曲线需要通过的点。

2. 边界条件约束（速度和加速度）

for (int i = 0; i < 4; ++i) {
  bx(K + i) = start_end_derivative[i].x;
  by(K + i) = start_end_derivative[i].y;
  bz(K + i) = start_end_derivative[i].z;
}

作用：
- 将起点和终点的速度、加速度约束填入向量 $\mathbf{b} )$ 的后 4 个元素。
解释：
- $\mathbf{b} )$ 的后 4 行分别对应起点和终点的速度与加速度约束，start_end_derivative 包含了这些边界条件的具体数值，确保 B-spline 曲线在起点和终点处的导数（速度和加速度）与预期一致。

（三）求解线性方程 $\mathbf{q} = \mathbf{b} )$

Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);

作用：
- 分别求解 $(x)$ 、 $(y)$ 、 $(z)$ 方向上的控制点坐标。
解释：
- 由于每个维度（ $(x)$ 、 $(y)$ 、 $(z)$ ）是独立的，可以分别构造和求解线性方程组。
- 使用 Eigen 库中的 colPivHouseholderQr 分解方法进行求解，该方法适用于一般的线性方程组，具有较好的数值稳定性。从数学角度看，求解过程可表示为：
  $\mathbf{q}_m = A^{-1} \mathbf{b}$
  但实际计算中通过分解方法避免了直接求逆，提高了计算效率和精度。

（四）组合控制点矩阵

ctrl_pts.resize(K + 2, 3);
ctrl_pts.col(0) = px;
ctrl_pts.col(1) = py;
ctrl_pts.col(2) = pz;

作用：
- 将求解得到的 $(x)$ 、 $(y)$ 、 $(z)$ 方向上的控制点组合成一个 $\times 3 )$ 的矩阵，表示 B-spline 的控制点。
解释：
- ctrl_pts 是一个矩阵，每一行代表一个控制点的三维坐标。
- resize(K + 2, 3)：假设 $(K)$ 是轨迹点的数量，通常控制点的数量比轨迹点多 2 个，以满足边界条件的需求。将分别求解得到的 $(x)$ 、 $(y)$ 、 $(z)$ 坐标赋值给 ctrl_pts 的对应列，形成最终的控制点矩阵。

四、总结公式关系

（一）矩阵 $(A)$

数学上：
- $(A)$ 反映了 B-spline 基函数对控制点的权重，建立了控制点与轨迹点、边界条件之间的线性关系。
代码中：
- 通过位置、速度、加速度约束，构造了包含所有约束的矩阵 $(A)$ 。

（二）向量 $\mathbf{b} )$

数学上：
- $\mathbf{b} )$ 包含了所有的约束条件，包括轨迹点的位置以及边界条件（速度和加速度）。
代码中：
- 将轨迹点和边界条件的数值填入向量 $\mathbf{b} )$ 。

（三）线性求解

数学上：
- 通过求解线性方程组 $\mathbf{q}_m = \mathbf{b} )$ ，得到控制点 $\mathbf{q}_m )$ 。
代码中：
- 使用 Eigen 库的 QR 分解方法分别求解 $(x)$ 、 $(y)$ 、 $(z)$ 方向上的控制点坐标。

（四）结果

数学上：
- 通过控制点 $\mathbf{q}_m )$ 定义了 B-spline 曲线的形状，使其满足给定的轨迹点和边界条件。
代码中：
- 将求解得到的控制点组合成矩阵 ctrl_pts，用于后续的 B-spline 曲线生成与应用。

五、附加说明

（一）B-spline 基函数

B-spline 基函数是一组分段多项式函数，具有局部支撑性和良好的数值稳定性。这意味着它们仅在局部区间内有非零值，使得曲线在局部区域内的修改不会影响远处的曲线形状，从而提供了灵活且易于控制的曲线生成方式。

（二）控制点的作用

控制点不一定位于曲线上，但它们决定了曲线的形状和方向。通过调整控制点的位置，可以精确控制 B-spline 曲线的走势，为用户提供了强大的曲线形状控制能力。

（三）边界条件的重要性

在实际应用中，如路径规划、动画制作等，除了希望曲线通过特定的点外，还常常需要控制曲线在起点和终点的速度与加速度，以确保运动的平滑性和可控性，边界条件的引入能够让生成的 B-spline 曲线更好地满足实际需求。

（四）线性方程组的求解

求解控制点的过程实际上是在优化问题中寻找一组控制点，使得 B-spline 曲线尽可能满足所有的约束条件。选择合适的求解方法（如 QR 分解）可以提高计算效率和结果的稳定性，避免直接求逆带来的数值不稳定性。

六、结论

通过上述步骤，从离散的轨迹点和边界条件出发，通过构造矩阵 $(A)$ 和向量 $\mathbf{b} )$ ，求解线性方程组，最终得到 B-spline 曲线的控制点。这一过程不仅保证了曲线经过所有指定的轨迹点，还满足了起点和终点的速度与加速度要求，生成了一条平滑且符合预期运动特性的 B-spline 曲线，为多个领域提供了一种高效且灵活的曲线生成与控制手段。

以下是实现上述功能的 C++ 代码：

/*----------------------------------转化成为 B 样条的控制点---------------------------------*/
#include <iostream>
#include <vector>
#include <Eigen/Dense>

class NonUniformBspline {
public:
    void parameterizeToBspline(const double& ts, const std::vector<Eigen::Vector3d>& point_set,
                           const std::vector<Eigen::Vector3d>& start_end_derivative,
                           Eigen::MatrixXd& ctrl_pts) {
        if (ts <= 0) {
            std::cout << "[B-spline]:time step error." << std::endl;
            return;
        }

        if (point_set.size() < 2) {
            std::cout << "[B-spline]:point set have only " << point_set.size() << " points." << std::endl;
            return;
        }

        if (start_end_derivative.size()!= 4) {
            std::cout << "[B-spline]:derivatives error." << std::endl;
        }

        int K = point_set.size();

        // write A
        Eigen::Vector3d prow(3), vrow(3), arow(3);
        prow << 1, 4, 1;
        vrow << -1, 0, 1;
        arow << 1, -2, 1;

        Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2);

        for (int i = 0; i < K; ++i) 
            A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();

        A.block(K, 0, 1, 3)         = (1 / 2.0 / ts) * vrow.transpose();
        A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();

        A.block(K + 2, 0, 1, 3)     = (1 / ts / ts) * arow.transpose();
        A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();
        std::cout << "A:\n" << A << std::endl;

        // A.block(0, 0, K, K + 2) = (1 / 6.0) * A.block(0, 0, K, K + 2);
        // A.block(K, 0, 2, K + 2) = (1 / 2.0 / ts) * A.block(K, 0, 2, K + 2);
        // A.row(K + 4) = (1 / ts / ts) * A.row(K + 4);
        // A.row(K + 5) = (1 / ts / ts) * A.row(K + 5);

        // write b
        Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4);
        for (int i = 0; i < K; ++i) {
            bx(i) = point_set[i](0);
            by(i) = point_set[i](1);
            bz(i) = point_set[i](2);
        }

        for (int i = 0; i < 4; ++i) {
            bx(K + i) = start_end_derivative[i](0);
            by(K + i) = start_end_derivative[i](1);
            bz(K + i) = start_end_derivative[i](2);
        }

        // solve Ax = b
        Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
        Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
        Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);

        // convert to control pts
        ctrl_pts.resize(K + 2, 3);
        ctrl_pts.col(0) = px;
        ctrl_pts.col(1) = py;
        ctrl_pts.col(2) = pz;

        std::cout << "[B-spline]: parameterization ok." << std::endl;//B 样条控制点
    }
};

代码解释：

该代码定义了一个名为 NonUniformBspline 的类，其中包含一个成员函数 parameterizeToBspline。
首先，代码会检查输入参数 ts 的合法性，确保其大于 0，以及 point_set 中至少有 2 个点，start_end_derivative 的大小为 4。
然后，定义了用于位置、速度和加速度约束的向量 prow、vrow 和 arow，并初始化矩阵 A 为零矩阵。
通过循环和矩阵块操作将不同约束信息存储在矩阵 A 中，将轨迹点和边界条件存储在向量 bx、by 和 bz 中。
使用 colPivHouseholderQr 分解方法求解线性方程组得到控制点在 $(x)$ 、 $(y)$ 、 $(z)$ 方向上的坐标，并存储在 px、py 和 pz 中。
最后将这些控制点存储在 ctrl_pts 矩阵中，完成 B-spline 控制点的生成。