【第4期-智能驾驶汽车系列术语概念解析】第1节：基于车道线方程的曲率计算

小黎的Ally

已于 2023-02-19 14:04:00 修改

阅读量7k

点赞数 22

分类专栏：第4期-智能驾驶汽车系列术语概念解析文章标签：自动驾驶视觉检测

于 2023-01-27 12:08:10 首次发布

本文链接：https://blog.csdn.net/LWH995158080/article/details/128769268

版权

第4期-智能驾驶汽车系列术语概念解析专栏收录该内容

9 篇文章 23 订阅

订阅专栏

前言

车道线检测和识别是自动驾驶汽车前端环境感知的组成部分，检测结果为后端的规划和决策板块提供重要的信息参考，LDW（车道偏离预警）、LKA（车道保持辅助）、ACC（自适应巡航控制）等功能模块都依赖于连续稳定的车道线检测。
如下图1（来源于网络，侵删）所示，智能汽车首先利用摄像头采集到视觉图像，然后通过二值化等数学处理，最后可以提取结构化道路下的清晰车道线。

        车道线检测结果在向后端规控板块进行数据流传递的过程中，若直接将检测到的车道线用若干密集散点进行输出，一方面会增加系统的运算成本，另一方面也会导致检测结果失真。
        因此，可以通过建立车道线函数模型，该模型的待定系数根据本周期的检测结果散点进行函数拟合得到。如此一来，车道线检测结果只需向后端传递较少的几个函数模型的参数，后端接收到参数再通过函数模型还原为车道线。
        三次多项式曲线拟合车道中心线线不仅拟合精度较高，拟合结果的几个待定系数也具有一定的物理属性。

一、利用三次多项式曲线对车道线建模

关于车道线函数模型，首先需要基于本车建立车体坐标系，坐标系原点设为车辆质心。如下图2所示为利用MATLAB的自动驾驶工具箱函数生成的一条模拟路段，蓝车为主车，其余车辆为交通车。

左右侧车道线的数学模型可以利用三次多项式进行描述（MATLAB的自动驾驶工具箱库函数提供了三次方车道边界模型函数，cubicLaneBoudary），如下式所示：

fcsd

式中，ai和bi 分别表示左侧车道线和右侧车道线的待定系数。

为了求解式上式的待定系数，可以基于蓝车所在的中间车道的双侧车道线离散点，利用MATLAB的poliyfit函数进行多项式拟合，拟合结果如下所示：

系数	a0	a1	a2	a3	b0	b1	b2	b3
数值	1.6752	0.1101	-0.016	5.14e-5	-1.964	0.1139	-0.017	6.5e-5

为验证拟合结果的有效性，我们将左右侧车道线的待定系数拟合结果代入上式，绘制三次多项式曲线，结果如下图3的蓝色虚线所示。可以看出，利用三次多项式曲线拟合车道线能达到较高的拟合精度，拟合结果令人满意。

进一步的，考虑到左右侧车道线均能用三次多项式曲线进行拟合，易知车道中心线也可以用三次多项式进行描述。我们设车道中心线的三次函数曲线及一阶导、二阶导如下式所示，并计算x=0处的各阶导数值，得到：

二、三次多项式系数的物理释义

对于零阶导，即原三次函数在起点x=0处的函数值为系数c0，代表截距，由于此时蓝车质心位置为坐标原点，且刚好位于车道中心线附近，故此时截取较小；

对于一阶导，其导函数在起点x=0处的函数值为系数c1，代表在起点处的斜率，根据斜率定义可知当斜率值较小时，斜率值约等于切线与x轴的夹角，即车身相对于车道中心线的航向角：

对于二阶导，其导函数在起点x=0处的函数值为2c2 ，由于一阶导数值远小于1，故车道中心线处的曲率可近似处理为：

对于三阶导，其数值可粗略视为曲率变化率。

综上，利用三次多项式曲线拟合车道线或车道中心线，不仅降低数据传输量，保证数据高保真，而且其系数的物理意义明确，在一定误差范围内可以根据系数直接计算参考线的截距、航向角和曲率，是一种既有理论支撑又便于工程实践应用的车道线模型。值得注意的是，某些资料将三次多项式函数曲线的三阶导粗略视为车道中心线的曲率变化率，此物理量可以用于进行横向轨迹跟踪控制，感兴趣的读者可以参考资料了解。

三、MATLAB代码


clc
clear
close all

%% 构建道路场景
% 场景初始化
scenario = drivingScenario;   % 初始化场景
scenario.SampleTime = 0.1;    % 采样时间

% 道路中心坐标    
roadCenters = [0 0 0; 2 0 0; 4  0 0 ;30 -15 0; 60 -40 0];  

% 建立道路
marking = [laneMarking('Solid') ...
    laneMarking('Dashed') laneMarking('Dashed') laneMarking('Solid')];             % 分界线线型
laneSpecification = lanespec(3, 'Marking', marking);         % 道路规范
road(scenario, roadCenters, 'Lanes', laneSpecification);     % 生成道路

% 获得道路边界,先把边界点位置重新整理次序
rdbdy = roadBoundaries(scenario);
rdbdy = rdbdy{1,1}(1:end-1,:);
rdbdy = [rdbdy(3:end,:); rdbdy(1:2,:)];
rdbdy(1,3) = 0;

%% 构建车辆信息
% 分别三条车道的中心线坐标，以生成车辆航迹点
ptNums = size(rdbdy,1);
for j = 1:ptNums/2
    rightBdyPt = rdbdy(j,:);
    leftBdyPt = rdbdy(ptNums-j+1,:); 
    leftWaypoints(j,:) = leftBdyPt + (rightBdyPt - leftBdyPt)*1/6;
    centerWaypoints(j,:) = leftBdyPt + (rightBdyPt - leftBdyPt)*3/6;
    rightWaypoints(j,:) = leftBdyPt + (rightBdyPt - leftBdyPt)*5/6;
    leftLaneLine(j,:) = leftBdyPt + (rightBdyPt - leftBdyPt)*1/3;
    rightLaneLine(j,:) = leftBdyPt + (rightBdyPt - leftBdyPt)*2/3;
end

% 本车信息
ego.vehicle = [];
ego.waypoints = centerWaypoints;
ego.pos = ego.waypoints(1,:);
ego.speed = 15;

% 1号交通车信息
obs = struct;
obs(1).vehicle = [];
obs(1).waypoints = leftWaypoints(30:end,:);
obs(1).pos = obs(1).waypoints(1,:);
obs(1).speed = 0.001;

% 2号交通车信息
obs(2).vehicle = [];
obs(2).waypoints = centerWaypoints(15:end,:);
obs(2).pos = obs(2).waypoints(1,:);
obs(2).speed = 10;

% 2号交通车信息
obs(3).vehicle = [];
obs(3).waypoints = rightWaypoints(10:end,:);
obs(3).pos = obs(3).waypoints(1,:);
obs(3).speed = 8;

%% 在场景增加车辆
% 根据自车信息增加车辆
ego.vehicle = vehicle(scenario,'Position',ego.pos,'PlotColor','b');
trajectory(ego.vehicle, ego.waypoints,ego.speed);

% 根据交通车信息增加车辆
obs(1).vehicle = vehicle(scenario,'Position',obs(1).pos,'PlotColor','k');
trajectory(obs(1).vehicle, obs(1).waypoints,obs(1).speed);
obs(2).vehicle = vehicle(scenario,'Position',obs(2).pos,'PlotColor','g');
trajectory(obs(2).vehicle, obs(2).waypoints,obs(2).speed);
obs(3).vehicle = vehicle(scenario,'Position',obs(3).pos,'PlotColor','g');
trajectory(obs(3).vehicle, obs(3).waypoints,obs(3).speed);

%% 启动仿真
plot(scenario)
tjty = [];
step = 1;
while advance(scenario)
    pause(0.05)
    tjty(step).time = scenario.SimulationTime;
    tjty(step).pos = scenario.Actors(1, 3).Position(1:2);
    step = step + 1;
end
grid off
axis off
xlim([-5,70])
ylim([-50,10])
hold on

%% 利用三次多项式对车道线进行拟合
% 拟合得到双侧车道线和车道中心线的三次多项式系数
poly_left = polyfit(leftLaneLine(:,1),leftLaneLine(:,2),3);
f_left = polyval(poly_left,leftLaneLine(:,1));
poly_right = polyfit(rightLaneLine(:,1),rightLaneLine(:,2),3);
f_right = polyval(poly_right,rightLaneLine(:,1));
poly_center = polyfit(centerWaypoints(:,1),centerWaypoints(:,2),3);
f_center = polyval(poly_center,centerWaypoints(:,1));

% 画拟合结果图
plot(leftLaneLine(:,1),f_left,'b--')
plot(rightLaneLine(:,1),f_right,'b--')
plot(centerWaypoints(:,1),f_center,'b','linewidth',2)