当前位置：首页 > news >正文

ego(3)---根据关键点求解B样条控制点

news 2025/9/8 14:43:46

在 B 样条的公式中，比较重要的就是控制点的确定了，因此在 ego 中，在进行初始轨迹生成，然后关键点采样后，会进行 B 样条控制点的计算。

为什么不直接使用多项式的关键点？

多项式在求解过程中，仅依赖起始点与中止点的位置，而在关键点采样后，再进行 B 样条控制点的计算，则 B 样条的曲线会考虑所有关键采样点的信息，会比较全面；

多项式生成轨迹时，未知变量的数量为 6 个，而B样条在关键点采样后再生成，未知变量就与采样点的数量有关，为 k + 2，自由度更高

多项式轨迹牵一发而动全身，一个系数改变会影响整体，而 B 样条只会影响修改点附近的点，比较容易修改轨迹。

ego 中的 B 样条公式构造

3阶B样条

ego 中的 B 样条公式也由 Cox-de Boor 递推公式 而来。

B 样条的核心思想可以归纳为基函数与控制点的线性组合：

其中 P 就是控制点，Bi 就是第 i 个基函数，由于 ego 中采用的是 3 次 B 样条，就可以写作：

ego 之后的求解 B 样条控制点的函数的作用就是求解上式中的 P 。

核心的求解方法也跟初始轨迹生成的求解方法类似，列举 Ax=b 方程，然后使用 QR 分解，来求解未知系数。

ego 中使用 3 次 B 样条，即 k = 3，在 ego 中采用的基函数用于构造 A 矩阵，即：

位置约束系数：（prow = [1,4,1]/6）

速度约束系数：（vrow = [-1,0,1]/(2ts)）

加速度约束系数：（arow = [1,-2,1]/(ts²)）

    // write A  构建约束矩阵 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();    // 起点速度约束(第K行，影响前3个控制点，i=0,1,2)A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();  // 终点速度约束 (第k+1行)，影响最后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();   // 终点加速度约束

而 ego 中的 A 矩阵构造除了使用了所有的采样点外，也使用了起始终止点的速度与加速度信息，即 start_end_derivatives 。

b 矩阵的构造

b 矩阵就直接使用采样点的位置以及起始终止点的速度，加速度即可：

    // write b  构建约束向量 b(采样点与导数的目标值)Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4);// 位置约束目标值：每个采样点的 x,y,z 坐标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);}

也是很同样的使用 QR 分解来求解未知系数。

在 ego 的 B 样条控制点函数后可以加入打印代码，看一下它求解出来的东西：

// 打印控制点（添加此部分代码）
if (ctrl_pts.rows() == 3 && ctrl_pts.cols() > 0) {std::cout << "\n===== B样条控制点（ctrl_pts）=====" << std::endl;std::cout << "控制点数量: " << ctrl_pts.cols() << std::endl;std::cout << "格式: [索引]  x坐标    y坐标    z坐标" << std::endl;std::cout << "-----------------------------------------" << std::endl;// 逐点打印（保留6位小数，确保精度）for (int i = 0; i < ctrl_pts.cols(); ++i) {std::cout << "[" << std::setw(2) << i << "]  "<< std::fixed << std::setprecision(6)<< std::setw(10) << ctrl_pts(0, i) << "  "  // x坐标<< std::setw(10) << ctrl_pts(1, i) << "  "  // y坐标<< std::setw(10) << ctrl_pts(2, i) << std::endl;  // z坐标}std::cout << "=========================================" << std::endl;
} else {std::cout << "\n[警告] 控制点求解失败或为空！" << std::endl;
}