插值与拟合(3):B样条曲线
在路径规划问题中，通常会用到B样条来平滑路径，本文实现并封装了三次准均匀开放B样条曲线，供大学学习使用。作者提供了三套代码方案。可以用于不同平台：方案1：MATLAB；方案2：标准C++；方案3：在ROS-noetic平台中基于C++实现，并基于Rviz实现可视化。
本文的代码有如下优势：1、即拿即用。程序员仅仅需要提供控制点(control_node)、曲线节点数量(也就是很多博客中提到的u的尺寸)，通过两三行代码即可实现B样条；2、可以适用于二维、三维环境。
注意：本文侧重于代码分享，因此对于B样条曲线的原理不做阐述。（网上有很多讲的非常好的博客，作者理论阐述能力有限，故不再班门弄斧）。
提示：关于“三次”“准均匀”“开放(open)”三个前缀，大家可以搜一搜了解即可。大多数场景下我们说的B样条就是“三次”“准均匀”“开放(open)”B样条曲线。（这句话不一定严谨，个人想法）
下面开始代码分享：
1 基于MATLAB实现B样条曲线
如下图所示，程序员需要修改的主要是：第4行control_node以及第12行path_node_num的值。
clc, clear, close allcontrol_node = [0 0; 1 -2; 2 2; 3 -1; 4 1; 5 0]; % 二维坐标
% control_node = [0 0 0;
% 1 -2 -2;
% 2 2 3;
% 3 -1 4;
% 4 1 5;
% 5 0 5];path_node_num = 200;n = size(control_node,1)-1; % n+1个控制点
degree = 3; % 最高次幂3
order = 4; % 阶数4； order = degree + 1;u = linspace(0, 1-0.0005, path_node_num); % ！！！注意：如果u的范围是[0, 1]，下面节点向量的范围也要是0到1
path = zeros(length(u), size(control_node, 2));%% 生成节点向量kontsVector
m = n+1 + degree ;
knotsVector = zeros(n+1+degree+1, 1);
for i= 1:m+1if i <= degree + 1knotsVector(i) = 0;endif i>degree+1 && i<=m-degreeknotsVector(i) = knotsVector(i-1)+1;endif i > m-degreeknotsVector(i) = knotsVector(m-degree)+1;end
end
knotsVector = knotsVector / knotsVector(m+1);%% 获取准均匀B样条曲线
for idx = 1 : length(u)for i = 1 : size(control_node, 1)Bik = BaseFunction(i, order-1 , u(idx), knotsVector);path(idx,:) = path(idx,:) + Bik*control_node(i,:);end
endplot(path(:,1), path(:,2),'g--','LineWidth',5);hold on
scatter(control_node(:,1), control_node(:,2),500, '.');function Bik_u = BaseFunction(i, k , u, NodeVector)if k == 0 % 0次B样条if u >= NodeVector(i) && u < NodeVector(i+1)Bik_u = 1;elseBik_u = 0;end
elseLength1 = NodeVector(i+k) - NodeVector(i);Length2 = NodeVector(i+k+1) - NodeVector(i+1); % 支撑区间的长度if Length1 == 0 % 规定0/0 = 0Length1 = 1;endif Length2 == 0Length2 = 1;endBik_u = (u - NodeVector(i)) / Length1 * BaseFunction(i, k-1, u, NodeVector) ...+ (NodeVector(i+k+1) - u) / Length2 * BaseFunction(i+1, k-1, u, NodeVector);
end
end下面两张图分别是二维和三维环境下的效果：
2 基于标准C++实现B样条曲线
主要包括bspline.h、bspline.cpp以及main.cpp三个代码文件。主要需要在main.cpp中进行三处修改：
第7行：Eigen::MatrixXd control_node(6, 2);其中6表示控制点数量，2表示维度，这两个参数供程序员修改；
第8行：程序员提供控制点
第14行：提供path_node_num，即路径节点数量。
2.1 首先是bspline.h
#ifndef __BSPLINE_H__
#define __BSPLINE_H__#include <iostream>
#include <eigen3/Eigen/Eigen>
#include <vector>class Bspline
{
public:Bspline(){};~Bspline(){};/* 启动B样条 */void Bspline_start();/* 获取控制点、最高次幂、离散点的数量*/Bspline(Eigen::MatrixXd control_node, int degree, int num);/* 生成节点向量 */void get_konts();/* 获取准均匀B样条 */void get_bspline_curve();/* 基函数 */double BaseFunction(int i, int k, double u, std::vector<double> konts_vector);
private:Eigen::MatrixXd m_control_node;Eigen::MatrixXd m_path;// double int m_degree, m_order, m_num, m;std::vector<double> konts_vector;std::vector<double> u;
};#endif2.2 之后是bspline.cpp
#include "bspline.h"Bspline::Bspline(Eigen::MatrixXd control_node, int degree, int num)
{this->m_control_node = control_node; // 获取控制点this->m_degree = degree; // B样条的次数this->m_order = this->m_degree + 1; // B样条的阶数this->m_num = num; // 路径点的数量int n = this->m_control_node.rows() - 1; // n+1个控制点this->m = n + 1 + this->m_degree; // m + 1 = n + 1 + degree + 1// 确定uthis->u.resize(this->m_num);for(int i=0; i<m_num; i++){u[i] = 0 + (1.0-0.005)/(m_num-1) * i;}this->m_path.setZero(this->m_num, this->m_control_node.cols());// 初始化路径
}void Bspline::Bspline_start()
{this->get_konts();this->get_bspline_curve();
}void Bspline::get_konts()
{this->konts_vector.resize(this->m + 1);for(int i = 0; i<=m; i++){if(i <= this->m_degree){this->konts_vector[i] = 0;}if(i > this->m_degree && i< (this->m - this->m_degree)) {this->konts_vector[i] = this->konts_vector[i-1]+1;}if(i >= (this->m - this->m_degree)){this->konts_vector[i] = this->konts_vector[this->m - this->m_degree -1] + 1;}}for(int i=0; i<=m; i++){this->konts_vector[i] = this->konts_vector[i] / this->konts_vector[m];}
}void Bspline::get_bspline_curve()
{double Bik;for(int idx = 0; idx<this->u.size(); idx++){for(int i=0; i<this->m_control_node.rows(); i++){Bik = BaseFunction(i, this->m_degree, this->u[idx], this->konts_vector);for(int j=0; j<this->m_control_node.cols(); j++){this->m_path(idx,j) = this->m_path(idx,j) + Bik*this->m_control_node(i,j);}}}// std::cout << this->m_path ;
}double Bspline::BaseFunction(int i, int k, double u, std::vector<double> konts_vector)
{double Bik_u;if(k==0){if(u>=konts_vector[i] && u<konts_vector[i+1])return Bik_u = 1.0;elsereturn Bik_u = 0.0;}else{double length1 = konts_vector[i+k] - konts_vector[i];double length2 = konts_vector[i+k+1] - konts_vector[i+1];if(length1==0){length1=1;}if(length2==0){length2=1;}return Bik_u = (u - konts_vector[i])/length1 * BaseFunction(i,k-1,u,konts_vector) + (konts_vector[i+k+1]-u) / length2 * BaseFunction(i+1,k-1,u,konts_vector);}
}2.3 最后是main.cpp
#include <iostream>
#include "bspline.h"
#include <eigen3/Eigen/Eigen>int main()
{Eigen::MatrixXd control_node(6, 2);control_node << 0, -2,10, -2,25, -2,25, 0,40, 0,50, 0;int path_node_num = 200;Bspline bspline(control_node, 3, path_node_num);bspline.Bspline_start();return 0;
}3 ROS-noetic+Rviz+C++实现B样条曲线
这一部分主要包括bspline.h、bspline.cpp以及main.cpp三个代码文件。其中，bspline.h、bspline.cpp跟标准C++代码完全一致，读者复制粘贴即可，因此只提供main.cpp文件，内容如下。可供修改的地方主要是三点：
第16行：Eigen::MatrixXd control_node(6, 2);其中6表示控制点数量，2表示维度，这两个参数供程序员修改；注意如果是二维，要把第29行 cloud.points[i].z 设置为0.0。
第17行：程序员提供控制点
第29行：提供path_node_num，即路径节点数量。
注意：这段代码需要读者有基本的ROS和Rviz知识。
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <eigen3/Eigen/Eigen>
#include "bspline.h"
#include <vector>int main(int argc, char *argv[])
{ros::init(argc, argv, "main_node");ros::NodeHandle nh;ros::Publisher pcl_pub = nh.advertise<sensor_msgs::PointCloud2>("path", 10);/* 将路径节点传给cloud */Eigen::MatrixXd control_node(6, 3);control_node << 0, -2, -2,1, -2, 4,2.5, -2, 1,2.5, 0, 0,4.0, 0, 2,5.0, 0, 0; // 3-dim 地图// control_node << 0, 0,// 1, 4,// 2, 2,// 3, 3,// 4.0, 1,// 5.0, 5; // 2-dim 地图int path_node_num = 200; // 散点数量Bspline bspline(control_node, 3, 200);Eigen::MatrixXd path = bspline.Bspline_start();pcl::PointCloud<pcl::PointXYZ> cloud;cloud.points.resize(path.rows());for(size_t i=0; i<cloud.points.size(); i++){cloud.points[i].x = path(i,0);cloud.points[i].y = path(i,1);cloud.points[i].z = path(i,2);}sensor_msgs::PointCloud2 output;pcl::toROSMsg(cloud, output);output.header.frame_id = "map";output.header.stamp = ros::Time::now();while(ros::ok()){ ROS_INFO("path pub...");pcl_pub.publish(output);ros::spinOnce();}return 0;
}既然涉及到ROS，还需要配置一下CMakeLists文件，配置如下：
cmake_minimum_required(VERSION 3.0.2)
project(pcl_pkg)find_package(catkin REQUIRED COMPONENTSpcl_rosroscpprospysensor_msgsstd_msgs
)include_directories(
# include${catkin_INCLUDE_DIRS}${CMAKE_CURRENT_SOURCE_DIR}/include
)add_executable(main_node src/main.cpp src/bspline.cpp)
target_link_libraries(main_node ${catkin_LIBRARIES} )下面两张图分别是Rviz中二维和三维环境下的效果：
