基于B样条曲线的路径规划（含matlab代码免费下载）[通俗易懂]

全栈程序员-站长 • 2022年6月18日上午11:36 • 未分类 • 阅读 26

大家好，又见面了，我是你们的朋友全栈君。

本文主要解决的是用B样条曲线实现无人车的路径规划，对于B样条曲线的概念不再赘述，有兴趣的朋友可以自行去了解。

下面介绍如何利用B样条曲线对小车进行路径规划。

1.无人车平面曲线规划

1.1无人车位置控制

无人车位置的闭环控制图
我们一般假设无人车只移动在平地或者类平面场地，跟着我们期望的曲线运动。

原理

设点的坐标为(xd,yd),则可得到θd的表达式为
在这里插入图片描述
那么在实际应用中，我们如何生成一条期望曲线并让小车跟随它运动呢？

1.2小车移库问题

我们在生活中最常见的就是小车移库问题，这也是无人车类比赛中最基础的一类题目。

因为小车不能直线前往另一个库，而是要经过出库、到另一个库、入库三个过程，所以不能简单看作移动问题。

通常面对这类问题，我们需要规划一条由起点出发到达终点的光滑曲线。

那么在出库和入库时，仓库两边的墙就可以看作是我们的障碍点，考过科目二的都知道“倒车入库”过程中是不允许挂线的，而现实中我们的车也有可能因为碰到墙而擦掉漆。

为了避开这些障碍点，给定一组控制点，由这些控制点牵引小车躲避障碍，从而得到一条光滑的曲线，曲线形状大致由这些点予以控制。
*黄色为控制点*
图中黄色的点为控制点，控制点可以分为普通控制点和必过控制点。

给定五个控制点[C1,C2,C3,C4,C5],设样条曲线为P(s),参数s满足0≤s≤1,可以看成是归一化路程。
五个控制点：

再给定四个样条函数：

可得平面轨迹表达式:
在这里插入图片描述
B样条曲线的速度和加速度：

根据上面的公式求得：

B样条曲线P(s)的速度和加速度也是参数s的函数

而任意前后两段B样条曲线的连接点是光滑连续的，如第一段与第二段为
在这里插入图片描述
前一段曲线的终点与后一段曲线的起点，在位置、速度和加速度上都连续。

那么怎样保证P(s)一定过必过控制点？
我们选择给必过控制点加邻近的额外控制点。
在这里插入图片描述
V1表示小车速度方向，L为合适正常数，一般取小车车长的一半

原来的控制点：
在这里插入图片描述
增广后的控制点

以终点为例
在这里插入图片描述

程序流程图

在这里插入图片描述

1.3 matlab程序

五个控制点均为普通控制点，
没有必过控制点。

%设定5个控制点C=[C1 C2 C3 C4 C5]
C=[0  1   2  -2  0
   0 0.3 2.5 2.5 5];
L=0.254/2;      %L为小车长度的一半
RC=[C(:,1),C(:,2),C(:,3),C(:,4),C(:,5)];
N=length(RC);  
s=0:0.01:1;     %归一化路程
%四个样条函数f1(s)、f2(s)、f3(s)、f4(s)
f1s=(1-s).^3/6; 
f2s=(3*s.^3-6*s.^2+4)/6;
f3s=(-3*s.^3+3*s.^2+3*s+1)/6;
f4s=s.^3/6;
figure(1);mycolor='mbc';
plot(RC(1,:),RC(2,:),'r*');
hold on;for i=1:N-3
    P=RC(:,i)*f1s+RC(:,i+1)*f2s+RC(:,i+2)*f3s+RC(:,i+3)*f4s;
    plot(P(1,:),P(2,:),mycolor(mod(i,3)+1));
end
title('无人路径的B样条曲线')
grid on;
hold off

起点、第三个控制点与终点均为必过控制点

%设定5个控制点C=[C1 C2 C3 C4 C5]
C=[0  1   2  -2  0
   0 0.3 2.5 2.5 5];
V1=[1;0];       %起点C1出发速度方向V1
V2=[1;0];       %到终点C5速度方向V2
V3=[-1;1];      %第三个控制点C3的速度方向V3
L=0.254/2;      %L为小车长度的一半
RC=[C(:,1)-V1*L,C(:,1),C(:,1)+V1*L,C(:,2),C(:,3)-V3*L,C(:,3),C(:,3)+V3*L,C(:,4),C(:,5)-V2*L,C(:,5),C(:,5)+V2*L];
N=length(RC);   %控制点扩充后的控制点数目
s=0:0.01:1;     %归一化路程
%四个样条函数f1(s)、f2(s)、f3(s)、f4(s)
f1s=(1-s).^3/6; 
f2s=(3*s.^3-6*s.^2+4)/6;
f3s=(-3*s.^3+3*s.^2+3*s+1)/6;
f4s=s.^3/6;
%绘制过起点、第三个控制点和终点的B样条曲线
figure(1);mycolor='mbc';
plot(RC(1,:),RC(2,:),'r*');
hold on;for i=1:N-3
    P=RC(:,i)*f1s+RC(:,i+1)*f2s+RC(:,i+2)*f3s+RC(:,i+3)*f4s;
    plot(P(1,:),P(2,:),mycolor(mod(i,3)+1));
end
title('过起点、第三个控制点和终点的B样条曲线')
grid on;
hold off

1.4 运行结果

图中星号为控制点。

第一个无避过控制点的曲线
在这里插入图片描述