在路徑規劃問題中，通常會用到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;
};#endif

2.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中二維和三維環境下的效果：