commit b样条

CMakeLists.txt

@@ -39,3 +39,5 @@ target_link_libraries(main PRIVATE chhRobotics_CPP)
 
 add_subdirectory(PathTracking bin)
 
+add_subdirectory(PathPlanning bin_planning)
+

PathPlanning/B-spline/BSpline.cpp

@@ -0,0 +1,85 @@
+//
+// Created by chh3213 on 2022/11/25.
+//
+
+#include "BSpline.h"
+
+/**
+ * 基函数定义
+ * @param i
+ * @param k B样条阶数k
+ * @param u 自变量
+ * @param node_vector 节点向量 array([u0,u1,u2,...,u_n+k],shape=[1,n+k+1].
+ */
+double baseFunction(int i, int k, double u, vector<double> node_vector) {
+    //0次B样条（1阶B样条）
+    double Bik_u;
+    if(k==1){
+        if(u>=node_vector[i]&&u<node_vector[i+1]){
+            Bik_u=1;
+        }else{
+            Bik_u=0;
+        }
+
+    }else{
+        //公式中的两个分母
+        double denominator_1  =node_vector[i+k-1]-node_vector[i];
+        double denominator_2  =node_vector[i+k]-node_vector[i+1];
+        //# 如果遇到分母为 0的情况：
+        //# 1. 如果此时分子也为0，约定这一项整体为0；
+        //# 2. 如果此时分子不为0，则约定分母为1 。
+        if(denominator_1==0)denominator_1=1;
+        if(denominator_2==0)denominator_2=1;
+        Bik_u = (u-node_vector[i])/denominator_1* baseFunction(i,k-1,u,node_vector)+(node_vector[i+k]-u)/denominator_2*
+                                                                                                         baseFunction(i+1,k-1,u,node_vector);
+    }
+    return Bik_u;
+}
+
+/**
+ * 准均匀B样条的节点向量计算
+ * 首末值定义为 0 和 1
+ * @param n 控制点个数-1，控制点共n+1个
+ * @param k B样条阶数k， k阶B样条，k-1次曲线.
+ * @return
+ */
+vector<double> u_quasi_uniform(int n, int k) {
+    vector<double> node_vector(n+k+1); //准均匀B样条的节点向量计算，共n+1个控制顶点，k-1次B样条，k阶
+    double  piecewise = n - k + 2; //B样条曲线的段数:控制点个数-次数
+    if(piecewise==1){//只有一段曲线时，n = k-1
+        for(int i=n+1;i<n+k+1;i++)node_vector[i]=1;
+    }else{
+        //中间段内节点均匀分布：两端共2k个节点，中间还剩(n+k+1-2k=n-k+1）个节点
+        for(int i=0;i<n-k+1;i++){
+            node_vector[k+i]=node_vector[k+i-1]+1/piecewise;
+        }
+        for(int i=n+1;i<n+k+1;i++)node_vector[i]=1;//末尾重复度k
+    }
+    return node_vector;
+}
+
+/**
+ * 分段B样条
+ * 首末值定义为 0 和 1
+ * 分段Bezier曲线的节点向量计算，共n+1个控制顶点，k阶B样条，k-1次曲线
+ * 分段Bezier端节点重复度为k，内间节点重复度为k-1,且满足n/(k-1)为正整数
+ * @param n 控制点个数-1，控制点共n+1个
+ * @param k B样条阶数k， k阶B样条，k-1次曲线
+ * @return
+ */
+vector<double> u_piecewise_B_Spline(int n, int k) {
+    vector<double> node_vector(n+k+1);
+    if(n%(k-1)==0&&(k-1)>0){//满足n是k-1的整数倍且k-1为正整数
+        for(int i=n+1;i<n+k+1;i++)node_vector[i]=1;//末尾n+1到n+k+1的数重复
+        double piecewise = n / (k-1);  //设定内节点的值
+        if(piecewise>1){
+            //内节点重复k-1次
+            for(int i=k;i<n+1;i++)node_vector[i]=1/piecewise;
+        }
+    }else{
+        cout<<"error!需要满足n是k-1的整数倍且k-1为正整数"<<endl;
+    }
+    return node_vector;
+}
+
+

PathPlanning/B-spline/BSpline.h

@@ -0,0 +1,24 @@
+//
+// Created by chh3213 on 2022/11/25.
+//
+
+#ifndef CHHROBOTICS_CPP_BSPLINE_H
+#define CHHROBOTICS_CPP_BSPLINE_H
+#include <iostream>
+#include <Eigen/Dense>
+#include<vector>
+#include<cmath>
+#include<algorithm>
+using namespace std;
+using namespace Eigen;
+
+double baseFunction(int i, int k, double u, vector<double>node_vector);
+
+vector<double> u_quasi_uniform(int n,int k);
+
+
+vector<double> u_piecewise_B_Spline(int n,int k);
+
+
+
+#endif //CHHROBOTICS_CPP_BSPLINE_H

PathPlanning/B-spline/main.cpp

@@ -0,0 +1,89 @@
+//
+// Created by chh3213 on 2022/11/25.
+//
+
+#include "BSpline.h"
+#include "../../matplotlibcpp.h"
+namespace plt = matplotlibcpp;
+
+int main(){
+    vector<Vector2d>Ps{Vector2d (0,0),Vector2d(1,1),Vector2d(2,1),Vector2d(3,0),Vector2d(4,2)};
+    //vector<Vector2d>Ps{Vector2d (9.036145, 51.779661),Vector2d(21.084337, 70.084746),Vector2d(37.607573, 50.254237),Vector2d(51.893287, 69.745763),Vector2d(61.187608,  49.576271)};
+
+    vector<double>x_ref,y_ref;
+    for(int i=0;i<Ps.size();i++){
+        x_ref.push_back(Ps[i][0]);
+        y_ref.push_back(Ps[i][1]);
+    }
+    vector<double>x_,y_;
+
+    int n =Ps.size()-1; //控制点个数-1
+    int k = 3; //k阶、k-1次B样条
+    Vector2d p_u(0,0);
+    vector<double>bik_u(n+1);
+
+    int flag;
+    cout<<"请选择：1. 均匀B样条 2.准均匀B样条  3.分段B样条 0. 退出 "<<endl;
+    cin>>flag;
+    vector<double>node_vector;
+    switch (flag) {
+        case 1://均匀B样条
+            for(int i=0;i<n+k+1;i++){
+                node_vector.push_back((double)i/(n+k+1));
+            }
+
+            break;
+        case 2:
+            node_vector= u_quasi_uniform(n,k);
+
+            break;
+        case 3:
+            node_vector = u_piecewise_B_Spline(n,k);
+
+            break;
+        default:
+            return 0;
+    }
+    if(flag==1){
+        for(double u = (double)(k-1)/(n+k+1);u<(double)(n + 2) / (n + k+1 );u+=0.005){
+            for(int i=0;i<n+1;i++){
+                bik_u[i]= baseFunction(i,k,u,node_vector);
+                //cout<<bik_u[i]<<endl;
+            }
+            for(int i=0;i< Ps.size();i++){
+                p_u = p_u + Ps[i]*bik_u[i];
+            }
+            //cout<<p_u<<endl;
+            x_.push_back(p_u[0]);
+            y_.push_back(p_u[1]);
+            p_u=Vector2d (0,0);
+
+        }
+    }else{
+        for(double u = 0;u<1;u+=0.005){
+            for(int i=0;i<n+1;i++){
+                bik_u[i]= baseFunction(i,k,u,node_vector);
+            }
+            for(int i=0;i< Ps.size();i++){
+                p_u = p_u + Ps[i]*bik_u[i];
+                //cout<<p_u<<","<<endl;
+            }
+            x_.push_back(p_u[0]);
+            y_.push_back(p_u[1]);
+            p_u=Vector2d (0,0);
+
+        }
+    }
+
+    //画图
+    //plt::xlim(0,1);
+    plt::plot(x_, y_,"r");
+    plt::plot(x_ref,y_ref);
+    plt::pause(0.01);
+    // save figure
+    const char* filename = "./b_spline_demo.png";
+    cout << "Saving result to " << filename << std::endl;
+    plt::save(filename);
+    plt::show();
+    return 0;
+}

PathPlanning/CMakeLists.txt

@@ -0,0 +1,17 @@
+cmake_minimum_required(VERSION 3.21)
+project(chhRobotics_CPP)
+
+set(CMAKE_CXX_STANDARD 14)
+
+
+find_package(Eigen3 REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+
+# bezier
+add_executable(bezier_demo Bezier/main.cpp Bezier/BezierCurve.cpp)
+target_link_libraries(bezier_demo PRIVATE chhRobotics_CPP)
+
+# B-spline
+add_executable(b_spline_demo B-spline/main.cpp B-spline/BSpline.cpp)
+target_link_libraries(b_spline_demo PRIVATE chhRobotics_CPP)