局部路径规划算法 - B样条曲线（B-spline Curves）

最新推荐文章于 2024-09-21 09:03:33 发布

Big David

最新推荐文章于 2024-09-21 09:03:33 发布

阅读量2.6k

点赞数 15

分类专栏：决策规划控制 Planning模块文章标签：算法自动驾驶 B样条 c++ matlab

本文链接：https://blog.csdn.net/BigDavid123/article/details/136834903

版权

决策规划控制同时被 2 个专栏收录

42 篇文章 60 订阅

订阅专栏

Planning模块

32 篇文章 39 订阅

订阅专栏

参考资料：
（1）B样条曲线（B-spline Curves）
（2）B样条曲线法

1 算法概述

1.1 算法简介

样条是一根富有弹性的细木条或塑料条，在应用CAD/CAM技术以前，航空、船舶和汽车制造业普遍采用手工绘制自由曲线。绘制员用压铁压住样条，使其通过所有给定的型值点，再适当地调整压铁，改变样条形态，直到符合设计要求。
B样条曲线是B-样条基函数（给定区间上的所有样条函数组成一个线性空间）的线性组合。

1.2 算法思想

贝塞尔曲线的缺陷：
① 确定了多边形的顶点数（n+1个），也就决定了所定义的Bezier曲线的阶次（n次），这样很不灵活。
② 当顶点数（ n+1 ）较大时，曲线的次数较高，曲线的导数次数也会较高，因此曲线会出现较多的峰谷值。
③ 贝塞尔曲线无法进行局部修改。

引出了B样条曲线，除了保持Bezier曲线所具有的优点外，还弥补了上述所有的缺陷。
B样条曲线：
（1）可以指定阶次；
（2）移动控制点仅仅改变曲线的部分形状，而不是整体
B样条曲线是贝塞尔曲线的一般化，贝塞尔曲线可以认为是B样条曲线的特例
在这里插入图片描述

2 算法原理

2.1 B样条曲线方程

设 $n + 1$ 个控制点： $P_0，P_1，P_2，...，P_n$ ，这些控制点用于定义样条曲线的走向、界限范围，则具有 $n + 1$ 个控制点的 $k$ 阶 $B$ 样条曲线的定义：
$p(u)=\left[\begin{array}{llll} P_{0} & P_{1} & \cdots & P_{n} \end{array}\right]\left[\begin{array}{c} B_{0, k}(u) \\ B_{1, k}(u) \\ \vdots \\ B_{n, k}(u) \end{array}\right]=\sum_{i=0}^{n} P_{i} B_{i, k}(u)$
式中， $B_{i,k}(u)$ 是第 $i$ 个 $k$ 阶 $B$ 样条基函数，与控制点 $P_i$ 相对应， $k \geq 1$ ； $u$ 是自变量。

基函数具有如下德布尔-考克斯递推式：
$B_{i, k}(u)=\left\{\begin{array}{ll} \left\{\begin{array}{ll} 1, & u_{i} \leq u<u_{i+1} \\ 0, \text { 其他 } \end{array}\right. & k=1 \\ \frac{u-u_{i}}{u_{i+k-1}-u_{i}} B_{i, k-1}(u)+\frac{u_{i+k}-u}{u_{i+k}-u_{i+1}} B_{i+1, k-1}(u), & k \geq 2 \end{array}\right.$
如果遇到分母为 0的情况：0/0=0；如果此时分子不为0，则约定分母为1 。

式中， $u_i$ 是一组被称为节点矢量的非递减序列的连续变化值，首末值一般定义为 0 和 1 ，该序列如下：
$\left[u_{0}, u_{1}, \cdots, u_{k}, u_{k+1}, \cdots, u_{n}, u_{n+1}, \cdots, u_{n+k}\right]$

K阶B样条是关于u的 $k - 1$ 次曲线（即基函数的次数为 $k - 1$ ）
段数=控制点个数-次数= $(n + 1) - (k - 1) = n - k + 2$

段数：一个B样条曲线含由几段贝塞尔曲线

$B_{i,k}(u)$ 涉及的节点为 $u_i,u_{i+1},...,u_{i+k}$ 一共 $k + 1$ 个节点， $k$ 个区间，因此从 $B_{0,k}(u)$ 到 $B_{n,k}(u)$ 共涉及 $n + k + 1$ 个节点

2.2 B样条计算

根据递推式，当阶数k=1时，不同基函数的非零域如下图：
在这里插入图片描述
所有节点区间列在左边（第一）列，所有零次基函数在第二列，使用如下三角计算格式。

2.3 B样条分类

均匀B样条曲线：当节点沿参数轴均匀等距分布，为均匀B样均匀B样条曲线：当节点沿参数轴均匀等距分布，为均匀B样条曲线，如 $U =$ { $0, 1, 2, 3, 4, 5, 6$ }。当n和k一定时，均匀B样条的基函数呈周期性，所有基函数有相同形状，每个后续基函数仅仅是前面基函数在新位置上的重复。
准均匀B样条曲线：两端节点具有重复度k，中间节点非递减的序列，如 $U =$ { $0, 0, 0, 1, 2, 3, 4, 5, 5, 5$ }。准均匀B样条曲线保留了贝塞尔曲线在两个端点处的性质：样条曲线在端点处的切线即为倒数两个端点的连线。准均匀B样条曲线用途最为广泛。
（1）一般来说，次数越高，则曲线的导数次数也会较高，那么将会有很多零点存在，较多的导数零点就导致原曲线存在较多的极值，使曲线出现较多的峰谷值；次数越低，样条曲线逼近控制点效果越好。
（2）另一方面，三次B样条曲线能够实现二阶导数连续，故最终选择准均匀三次B样条曲线作为轨迹规划的曲线比较合适
$k=\left|\frac{f^{\prime \prime}}{\left(1+f^{\prime 2}\right)^{\frac{3}{2}}}\right|$
分段B样条曲线：其节点矢量中两端节点的重复度与准均匀B样条曲线相同，为k。不同的是内节点（即除去两端节点后的剩余中间节点）重复度为k-1。该类型有限制条件，控制顶点数减1必须等于次数的正整数倍，即 $\frac{n}{k-1}$ =正整数。
一般非均匀B样条曲线：对任意分布的节点矢量 $U=[u_0,u_1,...,u_{n+k}]$

2.4 总结

在这里插入图片描述
许多论文中的分类是open 、clamped、closed
（1）如果节点向量没有任何特别的结构，那么产生的曲线不会与控制曲线的第一边和最后一边接触，曲线也不会分别与第一个控制点和最后一个控制点的第一边和最后一边相切。如下面图a所示。这种类型的B-样条曲线称为开（open ）B-样条曲线。对于开（open）B-样条曲线，u的定义域是 $u_{k-1},u_{n+2}]$
（2）clamped B-样条曲线即准均匀B样条曲线，如下图b。
（3）通过重复某些节点和控制点，产生的曲线会是闭（closed）曲线。这种情况，产生的曲线的开始和结尾连接在一起形成了一个闭环如下边图c所示。
在这里插入图片描述

3 代码实现

MATLAB（Ally）
BaseFunction.m

function Bik_u = BaseFunction(i, k , u, NodeVector)

if k == 0       % 0次B样条
    if u >= NodeVector(i+1) && u < NodeVector(i+2)
        Bik_u = 1;
    else
        Bik_u = 0;
    end
else
    Length1 = NodeVector(i+k+1) - NodeVector(i+1);
    Length2 = NodeVector(i+k+2) - NodeVector(i+2);      % 支撑区间的长度
    if Length1 == 0       % 规定0/0 = 0
        Length1 = 1;
    end
    if Length2 == 0
        Length2 = 1;
    end
    Bik_u = (u - NodeVector(i+1)) / Length1 * BaseFunction(i, k-1, u, NodeVector) ...
        + (NodeVector(i+k+2) - u) / Length2 * BaseFunction(i+1, k-1, u, NodeVector);
end

U_quasi_uniform.m

function NodeVector = U_quasi_uniform(n, k)
% 准均匀B样条的节点向量计算，共n+1个控制顶点，k次B样条，k+1阶
NodeVector = zeros(1, n+k+2);
piecewise = n - k + 1;       % 曲线的段数
if piecewise == 1            % 只有一段曲线时，n = k
    for i = k+2 : n+k+2
        NodeVector(1, i) = 1;
    end
else
    flag = 1;           % 不止一段曲线时
    while flag ~= piecewise
        NodeVector(1, k+flag+1) = NodeVector(1, k + flag) + 1/piecewise;
        flag = flag + 1;
    end
    NodeVector(1, n+2 : n+k+2) = 1;   % 节点向量前面和后面有（k+1）个重复值（阶数）
end

main.m

clc
clear
close all

%% 数据定义
k = 4;                                    % k阶、k-1次B样条
flag = 2;                                  %1,2分别绘制均匀B样条曲线、准均匀B样条曲线
d = 3.5;
P=[0, 10, 25, 25, 40, 50;
    -d/2,-d/2,-d/2+0.5,d/2-0.5,d/2,d/2 ];   %n=5, 6个控制点，可以满足曲率连续
n = size(P,2)-1;                          % n是控制点个数，从0开始计数

%% 生成B样条曲线

path=[];
Bik = zeros(n+1, 1);

if flag == 1     % 均匀B样条
    NodeVector = linspace(0, 1, n+k+1); %节点矢量
    for u = (k-1)/(n+k+1) : 0.001 : (n+2)/(n+k+1)
        for i = 0 : 1 : n
            Bik(i+1, 1) = BaseFunction(i, k-1 , u, NodeVector);
        end
        p_u = P * Bik;
        path = [path; [p_u(1,1),p_u(2,1)]];
    end
    
elseif flag == 2  % 准均匀B样条
    NodeVector = U_quasi_uniform(n, k-1); % 准均匀B样条的节点矢量
    for u = 0 : 0.005 : 1-0.005
        for i = 0 : 1 : n
            Bik(i+1, 1) = BaseFunction(i, k-1 , u, NodeVector);
        end
        p_u = P * Bik;
        path=[path; [p_u(1),p_u(2)]];
    end
else
    fprintf('error!\n');
end

%% 画图
d = 3.5;               % 道路标准宽度
W = 1.8;               % 汽车宽度
L = 4.7;               % 车长
figure
len_line = 50;
P0 = [0, -d/2];

% 画灰色路面图
GreyZone = [-5,-d-0.5; -5,d+0.5; len_line,d+0.5; len_line,-d-0.5];
fill(GreyZone(:,1),GreyZone(:,2),[0.5 0.5 0.5]);
hold on
fill([P0(1),P0(1),P0(1)-L,P0(1)-L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],'b')  

% 画分界线
plot([-5, len_line],[0, 0], 'w--', 'linewidth',2);  %分界线
plot([-5,len_line],[d,d],'w','linewidth',2);     %左边界线
plot([-5,len_line],[-d,-d],'w','linewidth',2);  %左边界线

% 设置坐标轴显示范围
axis equal
set(gca, 'XLim',[-5 len_line]); 
set(gca, 'YLim',[-4 4]); 

% 绘制路径
scatter(path(:,1),path(:,2),100, '.b');%路径点
scatter(P(1,:),P(2,:),'g')
plot(P(1,:),P(2,:),'r');%路径点

运行结果：
在这里插入图片描述
C++：
BSpline.h

#pragma once 
#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);

BSpline.cpp

#include "BSpline.h"

// 基函数定义
// 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) {
    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 < 1e-15) denominator_1 = 1;
        if(denominator_2 < 1e-15) 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
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样条
// 分段Bezier曲线的节点向量计算，共n+1个控制顶点，k阶B样条，k-1次曲线
// 分段Bezier端节点重复度为k，内间节点重复度为k-1,且满足n/(k-1)为正整数
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的数重复
        int piecewise = n / (k-1);  //设定内节点的值
        if (piecewise > 1) {
            //内节点重复k-1次
            for (int i = 1; i < piecewise; i++) {
                for(int j = 0; j < k - 1; j++) node_vector[(k - 1) * i + j + 1] = i / piecewise;
            }
            
        }
    } else {
        cout<<"error!需要满足n是k-1的整数倍且k-1为正整数"<<endl;
    }
    return node_vector;
}

main.cpp

#include "BSpline.h"
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;

int main(int argc, char** argv) {
    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";
    plt::save(filename);
    plt::show();
    return 0;
}