2021年MathorCup数学建模A题自动驾驶中的车辆调头问题解题全过程文档及程序

2021年第十一届MathorCup高校数学建模

A题 自动驾驶中的车辆调头问题

原题再现

  自动驾驶是近年人工智能应用的热门研究领域之一，其中调头是自动驾驶中一个非常实际又很有趣的场景。假设无人车为四轮乘用车，采用前轮转向后轮驱动；车身可认为是一个矩形，车长 5 米，车宽 2 米，轴距 2.8米；方向盘最大转角 470°，方向盘与前轮转角的传动比为 16：1（方向盘每转动 16°，前轮转动 1°），方向盘最大转速为 400°/s；最大油门加速度 3m/s`2，极限刹车加速度-5m/s^2。
  无人车的轨迹，指的是一条含有位置和时间等信息的曲线，它由一列轨迹点构成，各个轨迹点中应包含位置坐标、方向角、曲率、曲率变化率、速度、加速度及运动到此点时的时间等信息。这里轨迹指的是车身中某一特定点的轨迹，即控制点。控制点是事先选定的，通常位于无人车车身对称轴（车身可以近似认为是左右轴对称的）上的一点，在行驶时，控制点的位置会与轨迹点相重合，控制点处的速度方向将与轨迹点的方向角一致。由于无人车的车辆动力学性质和安全性要求，轨迹必须满足以下条件：
  （1）任何点的加速度不得高于最大油门加速度，不得低于极限刹车减速度；
  （2）无人车朝向和方向盘转角都不能突变，轨迹线必须是连续、且切线方向连续，当无人车不是静止时，曲率也必须连续（静止时可以原地打方向，非静止时不能）；
  （3）轨迹线的最大曲率应尽量不高于 0.205，禁止高于 0.21；
  （4）按轨迹行驶时，在调头完成之前，无人车车身任何点不得与任何障碍物或者掉头区域边界发生碰撞，且与障碍物至少保留一个最小安全距离，一般不小于 30cm。
  无人车调头轨迹在保证上述条件之外也应具备尽可能舒适的驾乘体感，以及具备尽可能高的通行效率，同时出于规范驾驶的考虑，尽量减少不必要的压车道线行驶。
  请参赛者按照下面问题中简化版的无人车调头场景和附件数据，建立无人车安全快速调头的数学模型，设计无人车调头轨迹的计算方法，在输入外部道路和障碍物信息后，计算出合理可行的无人车轨迹。同时，由于附近障碍物可能由于视野问题被遮挡，导致调头中周围环境信息发生变化，轨迹应当是可以实时更新的。附件数据文件中的边界线均表示为折线点列，按坐标点从上到下的顺序连接形成；所有障碍物均以多边形表示，其中的点均按逆时针顺序排列，路况简图见数据文件。请你们的参赛队完成以下问题：
  问题 1：如图 1 所示场景，无人车（用橙色矩形表示）正在最左侧车道准备调头，存在三个对向车道。附件数据文件中，掉头区域边界以红色边框 A 表示，两条对向车道线以 B、C 表示（均为路面上的虚线），中间的隔离障碍物以 D 表示。无人车起始位置已经在附件数据文件中给出，当无人车身触碰到对向的掉头区域边界（图 1 中绿色部分）时，认为调头完成。请阐述控制点位置，解释其理由；基于此建立无人车调头的数学模型，并给出一个合理可行的算法设计，给出调头轨迹。

  问题 2：当掉头区域狭窄时，判断什么样的场景下无人车能够在不倒车的情况下完成调头，什么情况需要至少一次倒车（如图 2 场景 2，蓝色箭头表示向前，红色箭头表示倒车）才能通过。建立相应的无人车调头的数学模型，给出合理的算法设计，并给出仿真结果。

  问题 3：如图 3 场景，如果道路上还存在其它静止障碍物（附件数据文件中以 F 和 G 表示），当仅存在 F、仅存在 G、或二者都存在时，建立相应的无人车调头的数学模型，给出合理的算法设计，并给出调头轨迹，你的算法应明确如何进行避障？

  问题 4：按交规规定，如果道路上存在人行横道即斑马线，其所占区域在附件数据文件中以 E 表示，则无人车需要尽可能在不压人行横道的情况下通过，如果无法做到，则需要越过（后轮越过）人行横道后再开始调头，禁止骑人行横道调头（如图 4 场景 4 所示）。其中标识“√”的为可行通行方式，标识“×”为骑人行道调头方式。建立相应的无人车调头的数学模型，给出合理的算法设计和相应的调头轨迹；当斑马线和障碍物同时存在时（如图 5 场景 5 所示），你的模型和算法如何同时满足交规并进行避障绕行？

  问题 5：无人车在实际路况行驶中进行调头时，道路中的障碍物通常是处于移动状态，如对向来车等，针对图 3 和图 5 的两种场景，假设图中标识的障碍物位置为无人车处在起始时刻的初始位置，障碍物以一定的速度移动，假设只考虑与道路平行方向移动，请建立更一般的无人车调头轨迹规划模型并给出相应算法设计和无人车调头策略，给出仿真结果。
  问题 6：为保证无人驾驶的安全性，无人车的轨迹规划算法必须拥有尽可能高的求解成功率（应倾向使用更收敛更稳定，失败率更低的计算方法）同时为了能对路况进行快速反应，需要以尽可能高的频率进行计算（计算复杂度尽量低）。你的算法如何在求解成功率和求解耗时这两方面优化？

整体求解过程概述(摘要)

  自动驾驶是近年应用的热门研究领域之一，其中调头是自动驾驶中一个非常实际又很有趣的场景。许多人工智能在导航作业时都需要进行自动调头对准下一作业行的操作，因此，无人车调头的研究成为了车辆自动驾驶的研究重点之一。
  针对问题一，我们根据题目中的方向盘最大转角、方向盘与前轮转角的传动比以及无人车的对称轴来确定其控制点位置，得出控制点与前轴的距离为 1.78m；接着，我们对无人车的调头过程分了三种情况，主要讨论了前两种，经过分析得出控制点的最小转弯半径为 4.93m；检验后，控制点的转弯半径改进为 5.52m，此时无人车成功进入第三车道；然后，建立以调头时间为目标函数的单目标规划模型，求解该模型得出了两种情况调头过程的总时间，分别为 8.16s 、7.69s；最后，运用 Matlab 软件曲线拟合出无人车的调头轨迹（轨迹详见正文）。
  针对问题二，调头区域狭窄时，无人车是否需要倒车完成调头。该题只考虑单排车道的情况，首先，根据无人车从不同位置开始调头，主要讨论了问题一的前两种情况；其次，以调头时间为目标函数，建立目标规划模型，运用 Lingo 软件得出调头过程的总
时间，分别为 8.50s 、8.36s；最后运用 A**算法对该过程进行仿真实验（仿真详见正文）。
  针对问题三，道路上存在静止障碍物。根据附件所给位置信息和题目要求，我们分别讨论了仅存在障碍 F、仅存在障碍 G 和二者都存在的情况。当只讨论障碍 F 时，只有第一车道可以行驶；当只讨论障碍 G 时，三个车道均可以行驶；当二者都存在时，将前两种情况相结合，再确定临界点，建立目标规划模型，求解该模型 得出了调头过程的总时间，分别为 9.11s 、8.69s 、8.97s；运用 Matlab 得出调头轨迹（轨迹详见正文）。
  针对问题四，本题结合交规，在道路上存在人行横道的情况下，无人车调头必须越过人行横道。首先考虑没有障碍物，只有人行横道的情况，无人车从初始位置到后轮越过斑马线时开始调头，并考虑与边界的最小距离，调头之后无人车以最短时间进入车道；其次考虑存在障碍物 F 和 G 的情况，在无人车越过斑马线后，以最小转弯半径调头，为了避开障碍物，需要直行一段距离，再转入第一车道，完成调头。将时间作为目标函数建立模型，求解该模型 得出了调头过程的总时间，分别为 9.00s 、9.64s 。
  针对问题五，假设障碍物 F 和 G 均以恒定速度 8.34m / s移动，且 F，G 与无人车调头之后的运动方向一致，即障碍物 F，G 向下移动，并且障碍物与无人车同时移动。我们分析无人车与障碍物的相对位置，建立无人车调头轨迹规划模型，得出了无人车调头策略，最后运用 A*算法进行仿真实验（仿真详见正文）。
  针对问题六，本题目的是降低轨迹规划算法的复杂度，在保证成功求解的前提下简化算法。因此我们考虑适当减少算法中参数数量，减少无人车转弯的次数来简化模型。

模型假设：

  为了便于考虑问题，我们在不影响模型准确性的前提下，作出以下假设：
  （1）假设加速阶段假设为匀加速运动，减速阶段假设为匀减速运动；
  （2）为保证无人车转弯时安全行驶，假设调头时的速度为v2 =5m / s；
  （3）假设所有转弯过程均为匀速运动且运动轨迹均为圆弧。
  （4）由轮胎与地面的摩擦消耗的时间忽略不计。

问题分析：

  针对问题一
  本题目的在于根据题目以及附件所给信息，阐述控制点位置，并建立无人车调头的数学模型，给出合理算法，以及调头轨迹。首先，我们根据题目中的方向盘最大转角、方向盘与前轮转角的传动比以及无人车的对称轴来确定其控制点位置；然后，根据控制点位置以及附件中的位置信息，计我们算出控制点与障碍物的距离，并重新建立一个新的坐标系；最后，我们对无人车的调头总过程分三种情况分别讨论，建立目标规划模型，并运用 Lingo 软件得出合理可行的算法，再运用 Matlab 软件仿真无人车的调头轨迹。
   针对问题二
  本题是针对调头区域狭窄情况，无人车是否需要掉头完成倒车。在问题一中已经知道三排车道和两排车道的情况都可以不倒车完成调头，因此问题二只需考虑单排车道的情况，根据无人车从不同位置开始调头，分三种情况进行讨论，同样以调头时间为目标函数，将倒车过程融进目标规划模型，运用 Lingo 软件得出合理算法，运用 A**算法进行仿真实验。
   针对问题三
  问题三是在道路上设置静止障碍物，根据附件所给障碍物位置，以及题目要求，分别讨论仅存在障碍 F，仅存在障碍 G 和二者都存在的情况。当只讨论障碍 F 时，发现只有单排车道可以行驶，根据无人车的最小转弯半径以及与障碍物的最小距离确定临界点；当只讨论障碍 G 时，分三种情况讨论；当二者都存在时，将前两种相结合，再确定临界点，建立数学模型，运用 Lingo 得出合理算法，运用 MATLAB 得出调头轨迹。
   针对问题四
  本题结合交规，在道路上存在人行横道的情况，无人车调头必须越过人行横道。首先考虑没有障碍物，只有人行道的情况，无人车从初始位置到后轮越过斑马线开始调头，并考虑与边界的最小距离，调头之后无人车以最短时间进入车道；其次考虑存在障碍物F 和 G 的情况，在无人车越过斑马线后，以最小转弯半径调头，为避开障碍物，需要直行一段距离，在转入第一车道，完成调头。将与障碍物的临界点作为目标函数约束条件建立模型，得出合理算法。
   针对问题五
  在实际路况中，障碍物通常是移动的，本题，假设障碍物 F 和 G 分别是移动的汽车，由于无人车需要掉头，因此 F，G 与无人车调头方向一致，在题目给出的图中看，即障碍 F，G 向下运动，并且障碍物与无人车同时移动，假设障碍物以正常车速均速行驶，通过判断无人车与障碍物的相对位置，建立无人车调头轨迹规划模型，得出无人车调头策略，运用 A*算法进行仿真实验。
   针对问题六
  本题目的是降低轨迹规划算法的复杂度，在保证成功求解的前提下简化算法，为了无人车对路况的快速反应，要降低计算难度，因此就要考虑适当减少算法中参数数量，简化模型，并简化计算过程。

模型的建立与求解整体论文缩略图

全部论文请见下方“ 只会建模 QQ名片” 点击QQ名片即可

程序代码：(代码和文档not free)

The actual procedure is shown in the screenshot

function [allData, scenario, sensor] = ds__4()
%ds__4 - Returns sensor detections
% allData = ds__4 returns sensor detections in a structure
% with time for an internally defined scenario and sensor suite. %
% [allData, scenario, sensors] = ds__4 optionally returns
% the drivingScenario and detection generator objects. % Generated by MATLAB(R) 9.8 (R2020a) and Automated Driving Toolbox 3.1 (R2020a). % Generated on: 18-Apr-2021 12:13:54
% Create the drivingScenario object and ego car
[scenario, egoVehicle] = createDrivingScenario;
% Create all the sensors
sensor = createSensor(scenario);
allData = struct('Time', {}, 'ActorPoses', {}, 'ObjectDetections', {}, 'LaneDetections', {});
running = true;
while runnin

% Generate the target poses of all actors relative to the ego vehicle
poses = targetPoses(egoVehicle);
time = scenario.SimulationTime;
% Generate detections for the sensor
laneDetections = [];
[objectDetections, numObjects, isValidTime] = sensor(poses, time);
objectDetections = objectDetections(1:numObjects);
% Aggregate all detections into a structure for later use
if isValidTime
allData(end + 1) = struct( ... 'Time', scenario.SimulationTime, ... 'ActorPoses', actorPoses(scenario), ... 'ObjectDetections', {objectDetections}, ... 'LaneDetections', {laneDetections}); %#ok<AGROW>
end
% Advance the scenario one time step and exit the loop if the scenario is complete
running = advance(scenario);
end
% Restart the driving scenario to return the actors to their initial positions. restart(scenario);
% Release the sensor object so it can be used again. release(sensor);
%%%%%%%%%%%%%%%%%%%%
% Helper functions %
%%%%%%%%%%%%%%%%%%%%
% Units used in createSensors and createDrivingScenario
% Distance/Position - meters
% Speed - meters/second
% Angles - degrees
% RCS Pattern - dBsm
function sensor = createSensor(scenario)
% createSensors Returns all sensor objects to generate detections
% Assign into each sensor the physical and radar profiles for all actors
profiles = actorProfiles(scenario);
sensor = visionDetectionGenerator('SensorIndex', 1, ...
'SensorLocation', [4 0], ... 'MaxRange', 100, ... 'DetectorOutput', 'Objects only', ... 'Intrinsics', cameraIntrinsics([1814.81018227767 1814.81018227767],[320 240],[480
640]), ... 'ActorProfiles', profiles);
function [scenario, egoVehicle] = createDrivingScenario
% createDrivingScenario Returns the drivingScenario defined in the Designer
% Construct a drivingScenario object. scenario = drivingScenario;
% Add all road segments
roadCenters = [31.476 -5 0;
0 -5 0];
laneSpecification = lanespec(3, 'Width', 3.4872);
road(scenario, roadCenters, 'Lanes', laneSpecification);
roadCenters = [31.476 -12.507 0;
0 -12.507 0];
laneSpecification = lanespec(1, 'Width', 4.5517);
road(scenario, roadCenters, 'Lanes', laneSpecification);
% Add the actors
actor(scenario, ... 'ClassID', 5, ... 'Length', 16.0256, ... 'Width', 1.5434, ... 'Height', 0.8, ... 'Position', [8.12 -11.05 0.01], ... 'PlotColor', [0 114 189] / 255);
actor(scenario, ... 'ClassID', 5, ... 'Length', 4.9, ... 'Width', 4.9998, ... 'Height', 0.8, ... 'Position', [-171.8 46.5 0], ... 'PlotColor', [0 255 0] / 255);
actor(scenario, ... 'ClassID', 5, ... 'Length', 8.2223, ...
'Width', 1.8998, ... 'Height', 0.8, ... 'Position', [-169.7 -11 0], ... 'PlotColor', [0 255 0] / 255);
% Add the ego vehicle
egoVehicle = vehicle(scenario, ... 'ClassID', 1, ... 'Length', 5, ... 'Width', 2, ... 'Position', [5.89 -13.39 0.01], ... 'PlotColor', [0.85 0.325 0.098]);
waypoints = [5.89 -13.39 0.01;
13.62 -13.46 0.01;
16.25 -13.46 0.01;
19.51 -13.11 0.01;
26.74 -10.41 0.01;
27.31 -9.14 0.01;
27.31 -7.72 0.01;
26.67 -6.02 0.01;
25.18 -4.46 0.01;
22.84 -3.32 0.01;
20.36 -3.11 0.01;
17.81 -3.96 0.01;
14.62 -6.16 0.01;
11.99 -7.36 0.01;
7.95 -8.21 0.01;
3.27 -8.21 0.01;
0.15 -8.21 0.01; -3.04 -8.21 0];
speed = 30;
trajectory(egoVehicle, waypoints, speed);
vehicle(scenario, ... 'ClassID', 1, ... 'Length', 4.9, ... 'Width', 4.9998, ... 'Position', [9.51 -2.19 0.01], ... 'PlotColor', [0.929 0.694 0.125])
vehicle(scenario, ... 'ClassID', 1, ... 'Length', 8.2223, ... 'Width', 1.8998, ...
'Position', [17.03 -8.15 0.01], ... 'PlotColor', [0.494 0.184 0.556]);
function [allData, scenario, sensor] = ds__4()
%ds__4 - Returns sensor detections
% allData = ds__4 returns sensor detections in a structure
% with time for an internally defined scenario and sensor suite. %
% [allData, scenario, sensors] = ds__4 optionally returns
% the drivingScenario and detection generator objects. % Generated by MATLAB(R) 9.8 (R2020a) and Automated Driving Toolbox 3.1 (R2020a). % Generated on: 18-Apr-2021 12:16:07
% Create the drivingScenario object and ego car
[scenario, egoVehicle] = createDrivingScenario;
% Create all the sensors
sensor = createSensor(scenario);
allData = struct('Time', {}, 'ActorPoses', {}, 'ObjectDetections', {}, 'LaneDetections', {});
running = true;
while running
% Generate the target poses of all actors relative to the ego vehicle
poses = targetPoses(egoVehicle);
time = scenario.SimulationTime;
% Generate detections for the sensor
laneDetections = [];
[objectDetections, numObjects, isValidTime] = sensor(poses, time);
objectDetections = objectDetections(1:numObjects);
% Aggregate all detections into a structure for later use
if isValidTime
allData(end + 1) = struct( ... 'Time', scenario.SimulationTime, ... 'ActorPoses', actorPoses(scenario), ... 'ObjectDetections', {objectDetections}, ... 'LaneDetections', {laneDetections}); %#ok<AGROW>
end
% Advance the scenario one time step and exit the loop if the scenario is complete
running = advance(scenario);
end

全部论文请见下方“ 只会建模 QQ名片” 点击QQ名片即可