[Issue 4 - Interpretation of Terms and Concepts of Intelligent Driving Vehicle Series] Section 6: Path Planning Algorithm Based on Cubic Spline Interpolation

Table of contents

1. Derivation of piecewise cubic spline interpolation curve

1. The function values ​​of adjacent points are equal

2. The first derivatives of adjacent points are equal

3. The second order derivatives of adjacent points are equal

4. Endpoint Boundary Conditions

2. Case Intensive Lecture and MATLAB Simulation

1. Introduction to the scene

2. MATLAB program

3. Simulation results

---

1. Derivation of piecewise cubic spline interpolation curve

Fig.1 Schematic diagram of piecewise cubic spline curve

         As shown in Figure 1, there are n discrete points, and we hope to use a smooth curve to pass through these n discrete points in turn. As mentioned above, if a high-order polynomial curve is used for interpolation, the Runge phenomenon of endpoint oscillation will appear.

        Therefore, we envisage using several low-order polynomial curves to interpolate two adjacent points, and then connect their first bits to form a smooth complete curve. Going back to Figure 1, n discrete points correspond to n-1 interval segments, that is to say, our goal is to solve n-1 cubic curves, and the expression of each cubic curve is as follows:

Formula 1 

 In the above formula, fi(x) represents the cubic curve function of the i-th segment, and ai,j represents the coefficient of the j-th order term of the i-th segment cubic curve.

        It can be seen from formula 1 that each cubic curve has 4 undetermined coefficients, then n-1 cubic curves have a total of 4(n-1) undetermined coefficients, so it is necessary to construct 4(n-1) independent equations to obtain a unique solution . In other words, we need to set 4(n-1) independent boundary conditions. The process of constructing boundary conditions is actually a process of exploring the equality constraints of multi-segment cubic curves, which can be considered from the following aspects.

1. The function values ​​of adjacent points are equal

        Obviously, we hope that the function value of the first and last two points of each segment of the cubic curve is equal to the discrete point, the expression is as follows:

Formula 2

         Use matrix expression instead:

Formula 3

 

        In the formula, xi and yi represent the abscissa and ordinate of the discrete point, respectively. Therefore, according to the equality constraint that the function values ​​of adjacent points are equal, 2(n-1) boundary conditions can be constructed.

2. The first derivatives of adjacent points are equal

        In order to ensure that the derivatives of the curve at adjacent points are continuous, it is necessary to ensure that the first-order derivatives of adjacent points are equal:

Formula 4

         Use matrix expression instead:

Formula 5

         Therefore, (n-2) boundary conditions can be constructed according to the equality constraint that the first-order derivatives of adjacent points are equal .

3. The second order derivatives of adjacent points are equal

        In order to ensure that the curvature is continuous, the second derivatives of adjacent points also need to be equal:

Formula 6

         Use matrix expression instead:

Formula 7

         (n-2) boundary conditions can be constructed according to the equality constraint that the second order derivatives of adjacent points are equal .

4. Endpoint Boundary Conditions

        The endpoint boundary conditions are divided into natural boundary, fixed boundary and kink boundary. For the natural boundary, the second derivative of the designated endpoint is 0, that is,

Formula 8

         Use matrix expression instead:

Formula 9

         According to the equality constraint of the endpoint condition, two boundary conditions can be constructed .

        To sum up, we can express all the above 4(n-1) equality constraints with matrix expressions, and finally form a linear matrix equation, so we can use MATLAB for matrix calculation and solution to get 4(n-1) undetermined coefficient.

2. Case Intensive Lecture and MATLAB Simulation

1. Introduction to the scene

Figure 2 Schematic diagram of local path planning

       Figure 2 is a schematic diagram of a typical local path planning. The blue car is used as an obstacle car, and the yellow car needs to change lanes to avoid obstacles. We set A(0,-1.75), B(10,-1.75), C(20 ,-1.75) , D(30,1.75 ) , E(40,1.75 ) , F(50,1.75 ) a total of 6 discrete Points can be used for path planning using cubic spline curves.

        MATLAB's piecewise cubic Hermite interpolation polynomial library function pchip , this function can directly calculate piecewise cubic splines, return 4 coefficients of each piece of polynomial , and then call the ppval library function according to the coefficients to generate a series of interpolation points, thus obtaining Cubic splines .

2. MATLAB program

The main program is as follows:

% 基于三次样条曲线的换道路径规划
clc
clear
close all

%% 数据定义
d = 3.5;
k = 4;                                   
P = [0,-d/2; 10,-d/2; 20,-d/2; 30,d/2; 40,d/2; 50,d/2]';

%% 调用pchip函数，生成三次样条曲线
x_seq = P(1,:);
y_seq = P(2,:);
cs = pchip(x_seq,y_seq);
X_seq = linspace(0,50,100);
Y_seq = ppval(cs,X_seq);
path = [X_seq', Y_seq'];

%% 计算长度和曲率
x = path(:,1)';
y = path(:,2)';
diffX = diff(path(:,1));
diffY = diff(path(:,2));
cumLength = cumsum(sqrt(diffX.^2 + diffY.^2));   %长度
heading = atan2(diffY, diffX);
for i = 1:length(x)-2
    cur(i) = getCur(x(i:i+2)',y(i:i+2)');
end
cur(end+1) = cur(end);



%% 画曲率图
figure
hold on
grid on
% 主体图形绘制
plot(cumLength,cur,'LineWidth', 3,  'Color', 'k');
% 坐标轴
set(gca,'LineWidth',2.5,'FontName', 'Times New Roman')
hXLabel = xlabel('路径长度/m');
hYLabel = ylabel('曲率/m^-^1');
% 修改刻度标签字体和字号
set(gca, 'FontSize', 16),...
    set([hXLabel, hYLabel], 'FontName',  'simsun')
set([hXLabel, hYLabel], 'FontSize', 16)


% 画航向角图
figure
hold on
grid on
% 主体图形绘制
plot(cumLength, heading,'LineWidth', 3,  'Color', 'b');
% 坐标轴
set(gca,'LineWidth',2.5,'FontName', 'Times New Roman')
hXLabel = xlabel('路径长度/m');
hYLabel = ylabel('航向角/rad');
% 修改刻度标签字体和字号
set(gca, 'FontSize', 16),...
set([hXLabel, hYLabel], 'FontName',  'simsun')
set([hXLabel, hYLabel], 'FontSize', 16)


%% 画图
d = 3.5;               % 道路标准宽度
W = 1.8;               % 汽车宽度
L = 4.7;               % 车长
figure
len_line = 50;

% 画灰色路面图
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([P(1,1),P(1,1),P(1,1)-L,P(1,1)-L],[-d/2-W/2,-d/2+W/2,-d/2+W/2,-d/2-W/2],'y')
fill([35,35,35-L,35-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(P(1,:),P(2,:),100,'r.')
plot(P(1,:),P(2,:),'r');%路径点
plot(path(:,1),path(:,2), 'y','linewidth',2);%路径点

 The above main program uses the function to solve the curvature of scattered points, and the function body program is as follows

function k=getCur(x,y)
ta=sqrt((x(2)-x(1))^2+(y(2)-y(1))^2);
tb=sqrt((x(3)-x(2))^2+(y(3)-y(2))^2);
M=[1,-ta,ta^2;
    1,0,0;
    1,tb,tb^2];
A=M\x;
B=M\y;
k=(2*(A(2)*B(3)-A(3)*B(2)))/((A(2)^2+B(2)^2)^1.5+1e-10);
end

3. Simulation results

         Execute the above code to draw the scene graph and the planned path together, as shown in Figure 3. 

Figure 3 Simulation scene diagram

Figure 4. The heading angle change diagram of the planned path

Figure 5. Curvature change diagram of the planned path

        It can be seen that the rate of curvature change of the cubic spline curve is relatively large, which has a great relationship with the selection of control points.