1. Defects of using cubic polynomial curves to find curvature

In the first section of the fourth issue, we used the quadratic term coefficient of the cubic polynomial curve to express the curvature of the reference line at the origin of the vehicle coordinate system, as shown in Figure 1 below.

Fig. 1 Curve lane line fitting based on cubic polynomial curve

Although the coefficients of cubic polynomials have clear physical meanings and many advantages, there are still the following deficiencies:

(1) The formula for calculating the curvature ignores the first-order derivative, which leads to calculation errors in the obtained curvature. If the calculation accuracy of the curvature is high, it obviously does not meet the requirements;

(2) When the front-end processes the discrete points of the lane line, it needs to go through curve fitting first, and this step will still increase the calculation cost;

(3) When driving on a road with unclear lane lines, the curvature cannot be calculated indirectly by using the cubic function curve, and the discrete points of the local path planning need to be used for calculation.

In view of the above situation, we need a more general curvature calculation method.

2. Curvature calculation method based on three points to find circumscribed circle

When the reference line is given by a parametric equation, the curvature can be obtained by referring to the cubic polynomial curve of the lane line. When the reference line is given by a number of dense discrete points, it is necessary to calculate the radius of curvature of the reference line to obtain Count down to get.

As shown in Figure 2 below, A, B, and C are three continuous discrete points of the reference line, and a, b, and c are their opposite sides. According to the relative properties of the circumscribed circle of a triangle, the center O of the circumscribed circle of a triangle can be obtained by making the intersection of the perpendiculars of the three sides.

Fig.2 Schematic diagram of calculating curvature by three-point circumscribed circle method

In △ABC, it can be known from the law of cosines:

Connect CO and extend the intersecting circle to point D , since the circle O is the circumcircle of the quadrilateral ABCD , according to the property that the opposite corners of the circumcircle of the quadrilateral are complementary, we can get:

After rearranging the above formula, the curvature can be expressed as:

Therefore, by combining the above formulas, the curvatures of three continuous discrete points A, B, and C can be obtained.

3. Example introduction

1. MATLAB code

In this example, a circular reference path scatter set with a radius of 20 is given, and then the curvature is solved by the method of circumscribed circle.

First give the MATLAB code, as follows:

clc
clear
close all
load  refPath.mat
refPath = refPath_circle;

%% 基于三点求外接圆的曲率计算方法
for i = 1:size(refPath,1)-2
    A = refPath(i,:);
    B = refPath(i+1,:);
    C = refPath(i+2,:);
    a = norm(C-B);
    b = norm(C-A);
    c = norm(A-B);
    theta_B = acos((a^2 + c^2 - b^2) / (2*a*c));
    cur(i) = 2*sin(theta_B) / b;
end

%% 计算路径长度
diff_x = diff(refPath(:,1)) ;
diff_y = diff(refPath(:,2)) ;
cumLength = cumsum(sqrt(diff_x.^2 + diff_y.^2));

%% 画图
% 图1：参考曲线
figure
grid on
plot(refPath(:,1), refPath(:,2),'LineWidth', 3,  'Color', 'b');

% 图2：参考曲线的曲率
figure
hold on
grid on
plot(cumLength(1:end-1), cur,'LineWidth', 3,  'Color', 'b');

2. Result display

The results are shown in Figure 3 below, and it can be seen that the calculated curvature is basically close to 0.05.

Fig. 3 Calculating the curvature results of circular scatter points by using the method of finding the circumscribed circle with three points

Summarize

The method of using three points to find the circumcircle has clear ideas and simple mathematical derivation, and is very suitable for use in occasions with several discrete point sets.

[Phase 4 - Interpretation of Terms and Concepts of Intelligent Driving Vehicle Series] Section 2: Curvature Calculation Method Based on Three Points to Find Circumscribed Circle