大家好，又见面了，我是你们的朋友全栈君。
本文主要解决的是用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 运行结果
图中星号为控制点。
第一个无避过控制点的曲线
有必过控制点的曲线
结语
原工程上传到CSDN了，不过最近看了一下下载还需要积分，想要原工程（附带实验报告）可以在评论区留下邮箱，可能有时会忙，三天之内就会发到邮箱内。
2021.8.4 更新一下，CSDN下载可以自己调积分了，我调成了0积分，原工程（附带实验报告）上传到CSDN下载里了，请大家自行下载。
链接附上：https://download.csdn.net/download/Fan_zhaoyang/12463236
觉得写的不错的，对您有帮助的可以点个赞哦~
发布者：全栈程序员-站长，转载请注明出处：https://javaforall.net/150373.html原文链接：https://javaforall.net
