目录
一、利用三次多项式曲线求曲率的缺陷
在第4期第一节中，我们利用三次多项式曲线的二次项系数表达了参考线在自车坐标系原点处的曲率，如下图1所示。
尽管三次多项式的系数物理意义明确，具有诸多优点，但仍存在以下不足：
（1）计算曲率的公式忽略了一阶导数，导致得到的曲率存在计算误差，若对曲率计算精度要求较高，显然不符合要求；
（2）前端在处理车道线离散点时，需要先经过曲线拟合，此步骤仍会增加计算成本；
（3）当在车道线不清晰的道路行驶时，无法利用三次函数曲线间接计算曲率，需要利用局部路径规划的离散点进行计算。
鉴于上述情况，我们需要更加一般的曲率计算方法。
二、基于三点求外接圆的曲率计算方法
当参考线是以参数方程给出时，可以借鉴车道线的三次多项式曲线通过求导求曲率，当参考线是以若干密集的离散点给出时，则需要通过计算参考线的曲率半径进而求倒数获得。
如下图2所示，A、B、C分别是参考线的某三个连续的离散点，a，b，c分别是其对边。根据三角形外接圆相关性质，通过作三条边的中垂线的交点可以求得三角形的外接圆圆心O。
在△ABC中，由余弦定理可知：
连接CO并延长交圆周于点D，由于圆O是四边形ABCD的外接圆，根据四边形外接圆对角互补的性质，可得到：
整理上式，曲率可表达为：
因此联立上式，即可求得A、B、C三个连续离散点的曲率。
三、实例介绍
1.MATLAB代码
本实例通过给出一个半径为20的圆形参考路径散点集，然后利用求外接圆的方法求解曲率。
先给出MATLAB代码，如下：
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. 结果展示
结果如下图3所示，可以看出其计算的曲率基本接近0.05。
总结
利用三点求外接圆的方法，思路清晰、数学推导简单，非常适合于在有若干个离散点集的场合使用。