foreword

In Section 2, we introduced the curvature of the circumscribed circle based on three points. This method is the most accurate calculation method based on mathematical principles. Next, we will establish a quadratic curve for three discrete points for fitting. Approximately solve the curvature in order to expand the reader's mathematical thinking.

1. Curvature Calculation Method Based on Three-point Parametric Equation

Figure 1 below is a schematic diagram of the curvature calculation method based on the three-point parameter equation.

Schematic diagram of three-point parametric equation

We assume that the parametric curve equation passing through three adjacent discrete points A , B and C is as follows :

The formula for calculating the curvature of the parametric curve is as follows :

Obviously, in order to solve the curvature of the parameter curve of the above formula 2 , the six undetermined coefficients of formula ( 1 ) must be calculated first . Note that the lengths of two vectors composed of three discrete points are:

Considering that the three adjacent discrete points A , B , and C are generally very close to each other, and the central angle of the corresponding radius of curvature does not change much, the length of the arc segment AB can be approximately equal to the length of the straight line segment AB . Therefore, for the parameter equation (1) , the change of the independent variable t approximately satisfies the following equation :

Substitute (4) into (3) to get :

Rewrite the above formula into matrix form:

Remember :

Then the undetermined coefficient matrix is obtained as:

Finally, the curvature can be obtained by substituting equation (8) into equation (2) .

2. Example introduction

1. MATLAB code

We also use the circular reference path to calculate the curvature, and then compare the two curvature calculation methods in Section 2 and Section 3 respectively. The MATLAB code is 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));
    cur1(i) = 2*sin(theta_B) / b;
end

%% 基于三点参数方程的曲率计算方法
for i = 1:size(refPath,1)-2
    x = refPath(i:i+2,1);
    y = refPath(i:i+2,2);    
    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;
    cur2(i) = abs(2*(A(2)*B(3)-A(3)*B(2)))/((A(2)^2+B(2)^2)^1.5+1e-10);
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), cur1,'LineWidth', 3,  'Color', 'b');
plot(cumLength(1:end-1), cur2,'r--','LineWidth', 3 );

% 图3：两种曲率差值
figure
hold on
grid on
% 主体图形绘制
plot(cumLength(1:end-1), cur1-cur2,'LineWidth', 3,  'Color', 'b');

2. Result Analysis

The reference curve path is a circle with a radius of 20, which is composed of several dense scattered points, as shown in the figure below.

The comparison of the curvature calculated by the two methods is as follows: the blue solid line represents the method in Section 2, and the red dotted line represents the method in this section.

Comparison of the results of the two calculation methods of curvature in Section 2 and Section 3

The figure below shows the comparison of the difference between the two methods to calculate the curvature. It can be seen that the difference is at the level of 1e-6, which can basically be ignored.

Figure 3 The difference between the two calculation methods of curvature in Section 2 and Section 3

Summarize

Therefore, the two methods in Section 2 and Section 3 can be used universally when there is no higher precision requirement for curvature. But it also needs to be explained that Method 3 only provides readers with a novel idea of curvature calculation, which is used to broaden readers' mathematical thinking. Its calculation process and principle are more complicated than Method 2, and readers are recommended to use Method 2 for curvature calculation.

Some reference materials in this section: https://zhuanlan.zhihu.com/p/72083902h, thanks to the original author of this idea method, if there is any infringement, contact will be deleted.

[Issue 4 - Interpretation of Terms and Concepts of Intelligent Driving Vehicle Series] Section 3: Curvature Calculation Method Based on Three-Point Parametric Equation