关键词：
总结一下自己这一年的项目经验，由于本人技术水平有限，写作内容难免有错，如果发现还请不吝赐教~
在项目中本人负责的是自动驾驶的决策规划控制算法部分，在研发过程中负责对接上层感知定位模块和下层线控系统，项目实现了在园区场景下的自动驾驶。实现的场景包括单障碍物换道避障，多障碍物换道避障，跟车行驶，停车等待行人等。
接下来准备把项目中用到的横向控制、纵向控制、换道轨迹规划和决策模型分别记录，并附上核心代码，本文是第一篇。先放几个测试场景的视频作为预告。
园区自动驾驶（一）——基于Pure Pursuit的横向控制
由于本文是针对园区的自动驾驶，车速较低，且循迹路径是一条固定的路线，测试路径如下图所示，图中：绿色路径是录制的离线路径，录制方法是手动开车时记录下组合惯导系统的实时定位，并保存；紫色的路径是实际行驶路径；蓝色长条不是车辆模型，长度也不是车身长度，蓝条后端是车辆当前位置（后轴），蓝条前端是纯追踪算法当前的目标点。
在讲纯追踪算法之前，先陈述下使用纯追踪算法对车辆运动学和动力学模型的简化。
车辆运动学和动力学模型简化
二自由度车辆运动学模型和动力学模型这位大佬讲的很清楚，总结一下结论，简化包括：
-
不考虑车辆在Z轴方向的运动，只考虑XY水平面的运动，如下图所示；
-
左右侧车轮转角一致，这样可将左右侧轮胎合并为一个轮胎，以便于搭建单车模型，如图2所示；
-
车辆行驶速度变化缓慢，忽略前后轴载荷的转移；
-
车身及悬架系统是刚性的；
-
简化结论有 ：
1、一般条件下：
，其中 航向角为yaw，横摆角为
；
但低速条件下，认为车不会发生侧向滑动（漂移），因此质心侧偏角 β ≈ 0，可得： 航向角yaw=横摆角 。
2、一般条件下，轮胎由于是胶体，会在压力下发生形变，导致轮胎侧偏；
但低速条件下，将轮胎视为完全刚体，不会发生变形，不考虑轮胎侧偏，且后轮一般不转向，因此后轮转角 
。
bicycle模型
在以上简化假设都成立的情况下，才能将四轮akerman转向模型简化为二轮bicycle模型。
如上图所示，bicycle模型中，L是车的轴距，R是当前转向角下，车辆遵循的转弯半径，可以得到前轮转角δ 为：
纯追踪算法推导
接下来就是老生常谈的推导了，图懒得用visio画了，手画快些。
将四轮akerman模型简化成bicycle模型后，从录制的离线路径中选取下一个要追踪的目标点G(gx,gy)，控制目标是让车辆的后轴中点S（组合惯导系统位于车辆后轴，因此定位得到的车辆当前位置其实是车辆后轴的位置）经过该目标点G。
目标点G，车辆位置S和转向圆圆心三个点构成了一个等腰三角形，根据正弦定理，可以推出以下三个公式：
其中R为转向圆半径； 
是希望控制车辆到达的下一个位置，即 look-ahead distance（前视距离）：人类驾驶员开车时，眼睛看向道路前方的位置就是想开到的地方；纯追踪算法也一样，变成了在离线轨迹上寻找下一个期望到达的位置；夹角 α 为车身坐标系的x轴与车身坐标系原点与目标点的夹角，夹角 α 如下图所示。
1/R也就是这段圆弧的曲率，为：
将1/R带入到bicycle模型的前轮转角公式中，可得：
将转角 δ 视作时间 t 的函数，可得：
上式就是纯跟踪算法的输出表达式，输出量为车辆前轮转角 δ 。
为了更好的理解影响纯跟踪算法精度的因素，定义车辆当前位姿 S 和目标点 G 的横向误差 
，可得夹角 α 正弦：
转角 δ 可以重写为：
由于轴距 L 是常数，横向误差 是自变量，因此因变量 δ 的控制效果主要取决于前视距离 。
通常来说， 被认为是车速的函数，需要根据车速传感器反馈的当前车速来实时更新当前的前视距离。
除了车速外，道路曲率也是影响循迹精度的关键。在弯道下，前视距离如果太大，可能导致循迹精度下降。
因此，本文将前视距离视作是车速和道路曲率的一次函数，即 
，那么前轮的转角公式就变为：
其中，
为最小前视距离，本文的最小前视距离大小根据道路曲率设置：如果道路曲率太大，则减小 ，使追踪更加精确；如果道路曲率较小，则增大 ，使轨迹追踪更平滑。
基于C++的纯追踪算法实车实现核心代码
本实现方法在ros环境下实现，纯追踪算法只用来控制转向角度，速度控制将在下一篇纵向控制中介绍。
首先根据实车型号定义各项参数。
#define K (1.5) // 前视距离系数
#define L_VEHICLE (2.9) // 车的轴距
int L0 = 2; //最小前视距离，根据道路曲率变化 本文将初始最小前视距离设为2 m，前视距离系数设为1.5 ，AionLX 车的轴距为2.9m。
定义车辆状态类，在bicycle模型中，位姿只考虑车辆当前世界坐标系下坐标（x，y）,车辆偏航角yaw，以及车辆的速度speed：
struct State
double x = 0; // m
double y = 0; // m
double yaw = 0; // degree
double speed = 0; // m/s
;
State vehicleState;订阅话题，接受定位和传感器节点发布的消息，更新车辆状态：
vehicle_location = node.subscribe("/geometry_pose", 2, &pure_pursuit::location_update, (pure_pursuit *) this);
veh_speed = node.subscribe("/vehicle_response", 2, &pure_pursuit::vehReportCallback, (pure_pursuit *) this);接收定位传来的当前位姿，包括世界坐标系坐标和四元数，用ros的tf::Quaternion库将四元数转为 roll，pitch 和 yaw，得到航向角yaw：
//用PoseStamped更新位姿(x,y,yaw)
void pure_pursuit::location_update(const geometry_msgs::PoseStamped::ConstPtr locat)
vehicleState.x = locat->pose.position.x;
vehicleState.y = locat->pose.position.y;
//根据当前位置更新最小前视距离L0，其中boundary是直道和弯道的分界点
if(vehicleState.x>boundary.x&&vehicleState.y>boundary.y)
L0=1; //最小前视距离默认为2，弯道上变为1
//用vehLocUpdatRdy表示位姿是否更新，默认为false
vehLocUpdatRdy = true;
tf::Quaternion quat(locat->pose.orientation.x, locat->pose.orientation.y, locat->pose.orientation.z, locat->pose.orientation.w);
// 用tf::Quaternion库将四元数转为 roll pitch 和 yaw
double roll, pitch, yaw;
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);
vehicleState.yaw = yaw;
接受传感器传来的当前车速，更新当前车速：
//用传感器回调函数更新当前车速
void pure_pursuit::vehReportCallback(const common_msgs::veh_response::ConstPtr &veh_report_msg)
vehicleState.speed = static_cast<double>(veh_report_msg->veh_spd);
纯追踪横向控制核心代码：
//根据目标进行转角控制
double pure_pursuit::pure_pursuit_control(const State &s, const vector <Point> &path, int *lastIndex)
int index = get_goal_index(s, path); // 搜索目标点，返回目标点的标签
// 用上一个循环的目标点判断是否是在向前走
if (index <= *lastIndex)
index = *lastIndex;
Point goal;
//防止index溢出
if (index < path.size())
goal = path[index];
else
index = path.size() - 1;
goal = path[index];
// 车身坐标系的x轴和目标点与车身坐标系原点连线的夹角
double alpha = atan2(goal.y - s.y, goal.x - s.x) - s.yaw;
if (s.speed < 0)
alpha = M_PI - alpha;
double lf = K * s.speed + L0; // 根据车速和道路曲率设置前视距离
// delta 即为纯跟踪算法的最终输出
double delta = atan2((2.0 * L_VEHICLE * sin(alpha)) / lf, 1.0);
*lastIndex = index; // 为下一个循环更新上一个目标点
return delta;
定义函数用于搜索最近的目标点：
//index为跟踪目标
int pure_pursuit::get_goal_index(const State &s, const vector <Point> &path)
vector<double> d;
for (int i = 0; i < path.size(); i++)
d.push_back(pow((s.x - path[i].x), 2) + pow((s.y - path[i].y), 2));//距离计算
int index = 0;
double d_min = d[0];
int dVecLen = d.size();
//找到距离车辆最近的路径点
for (int i = 0; i < dVecLen; i++)
if (d_min > d[i])
d_min = d[i];
index = i;
double l = 0;
double lf = K * s.speed + L0;
double dx_, dy_;
//积分法计算路径长度
while (l < lf && index < path.size())
dx_ = path[index + 1].x - path[index].x;
dy_ = path[index + 1].y - path[index].y;
l += sqrt(dx_ * dx_ + dy_ * dy_);
index++;
return index;
发送转角给线控系统执行：
bool pure_pursuit::track_follow_process(const vector <Point> &pathVet)
bool isjobDone = false;
//判断路径是否走完
if (goalIndex < rearIndex)
//获取纯跟踪输出的前轮转角
delta = pure_pursuit_control(vehicleState, pathVet, &goalIndex);
wheelAngle = delta * 180 / M_PI;
wheelAngle = -1 * wheelAngle; //前轮转角为弧度制，逆时针为正，AionLX车的顺时针转向为正，逆时针为负，刚好相反
//AionLX前轮转角最大为40°
if (wheelAngle > 40.0) wheelAngle = 40.0;
if (wheelAngle < -40.0) wheelAngle = -40.0;
//如果走完，则急停
if (goalIndex >= rearIndex)
isjobDone = true;
//发布前轮转角
std_msgs::Float32 tempAngleSet;
tempAngleSet.data = wheelAngle;
strAngle_pub.publish(tempAngleSet);
return isjobDone;
以上就是纯追踪算法的核心代码，经实车实验，平均误差小于10cm。如下左图，由于设置的最大前轮转角不够大，且前视距离设置较大，所以在急剧转角处出现了转向不足的情况；将最大前轮转角改为40°，前视距离改小后，如下右图，在大弯道也能准确循迹。
小结
纯追踪算法是一种几何路径跟踪算法，很适合新手上手使用。优点是很大地简化了车辆运动学和动力学模型，输入输出明确，在低速下能获得较好地控制效果；缺点是简化了太多的车辆运动学和动力学模型，所以只要速度和加速度提高，运动学动力学简化条件不再成立，就无法保证横向控制精度。
本项目由于是在园区内行驶，车速不会高于10km/h，所以纯追踪算法能够达到横向控制需求。但目前选择最佳前视距离的方法并不一定，本文将 视作纵向速度和路径曲率相关的一次函数，大的前视距离可以使控制更平滑，但在大转角处会转向不足；前视距离太小又会带来控制的震荡(前轮转角变动幅度很大)。 因此，影响横向控制精度地前视距离 的选取仍有待优化。
参考博客
无人驾驶汽车系统入门（十八）——使用pure pursuit实现无人车轨迹追踪_AdamShan的博客-CSDN博客自动驾驶（七十一）---------Pure Pursuit轨迹追踪_一实相印的博客-CSDN博客_自动驾驶轨迹跟踪
无人驾驶-控制-阿克曼模型_0->oo的博客-CSDN博客_阿克曼模型
CSDN 社区图书馆，开张营业！
深读计划，写书评领图书福利~
自动驾驶决策概况
文章目录1.第一章行为决策在自动驾驶系统架构中的位置2.行为决策算法的种类2.1基于规则的决策算法2.1.1决策树2.1.2有限状态机（FSM）2.1.3基于本体论（Ontologies-based）2.2基于统计的决策算法2.2.1贝叶斯网络（BN&#... 查看详情
自动驾驶决策控制与行为预测运动规划综述文章
有两篇博文，对自动驾驶的综述写的不错，自动驾驶决策控制及运动规划史上最详细最接地气综述，自动驾驶路径规划与行为预测最接地气详细综述美中不足的是，如果想要获取参考文献的话，需要付一些... 查看详情
自动驾驶技术综述2：自动驾驶决策规划模块算法介绍
一、前言：在自动驾驶整个软件框架中，决策规划模块有着重要的作用。决策规划模块一般也叫Planning模块，在整个自动驾驶系统中，Planning模块相当于人类驾驶员的大脑，它的上游是地图、导航、感知、预测... 查看详情
解析丨自动驾驶核心技术：感知决策与执行(中：决策篇)
本文将继续为大家解析自动驾驶所涉及到的核心技术，上篇《感知篇》内容可以查看历史原文：二、决策篇在一套相对成熟的自动驾驶技术体系中，如果将环境感知模块比作人的眼睛和耳朵，那么决策规划模块就相当于自动驾驶... 查看详情
基于三维gis平台的智慧园区建设方案
...中的不断深入，智慧城市建设也在不断深入的发展，智慧园区作为智慧城市重要组成部分，存在着建设内容多、周期长，运用存在延续性、技术不断的更新性，而且在建设的过程中涉及到的人员多与投资规模大等特征，需要将智... 查看详情
自动驾驶决策规划-控制方向2023届秋招总结
...深度强化学习与机器人控制。今年秋招投递的岗位主要是自动驾驶的决策规划或控制岗位，当然前期也投递了一些机器人公司的规划控制岗位和部分机器学习算法岗位。研究生期间，由于研究方向都是深度强化学习相关&#... 查看详情
解决方案|数字孪生园区运营决策系统
...城市建设”。近年来随着智慧城市建设的推进，智慧园区作为智慧城市的重要组成部分，智慧园区也迎来全新的建设浪潮。通过建设实现园区资源管理和企业服务更智慧、更高效、更低碳。在疫情反复、全球经济局势复... 查看详情
智能驾驶数据后处理分析利器—intework-vda(代码片段)
...驾驶研发厂商以传统的手动分析或借助非专业工具进行半自动处理数据为主，测试分析工作效率极低。由于路试并非按照预定工况进行顺序驾驶，各种场景往往是在采集过程中无规律地出现，因此给数据筛选和分析工作带来了许... 查看详情
自动驾驶⻋端算力:达到l4级自动驾驶要求并不遥远算力单位tops
1.⻋端算力:达到L4级自动驾驶要求并不遥远激光雷达方案:降低⻋辆环境感知的算法与算力需求纯视觉方案基于平面数据来识别出周围物体距离、轮廓、色彩，对算法、算力要求高，需要后端强大的数据中心训练算法多传... 查看详情
[autocars]自动驾驶汽车概述（上）
...联网公司的算法岗，结果误打误撞拿了互联网汽车公司的自动驾驶算法offer。想着新能源汽车与自动驾驶也是接下来几年的一个趋势，如果到时前景一般也可以跳去互联网，于是便签了下来。最近计划在入职之前补充点关于自动... 查看详情
智能驾驶数据后处理分析利器—intework-vda
...驾驶研发厂商以传统的手动分析或借助非专业工具进行半自动处理数据为主，测试分析工作效率极低。 由于路试并非按照预定工况进行顺序驾驶，各种场景往往是在采集过程中无规律地出现，因此... 查看详情
自动驾驶决策规划-控制方向学习资料总结（附相关资料的链接）
...ithub主页项目名称说明chhCpp学习C++仓库chhRobotics学习自动驾驶、控制理论相关仓库(python实现)chhRobotics_CPP学习自动驾驶、控制理论相关仓库(c++实现)chhML、chh-MachineLearning学习机器学习仓库chhRL学习强化学习仓库chhTricks存... 查看详情
自动驾驶规划控制软硬一体化控制器解决方案
...系统的硬件架构方案，包括核心主控系统、输入输出控制系统、显示模块、全景摄像模块和存储模块等各子功能模块，提出了360度全景摄像头和双屏后座娱乐显示子模块解决方案。系统实现了智能驾驶舱和高级驾驶辅助... 查看详情
避碰决策基于matlab多无人船舶避碰决策含matlab源码2348期
⛄一、多无人船舶避碰决策1无人驾驶船舶智能避碰决策面临的问题相比于车辆在陆地道路上的行驶,无人驾驶船舶航行的海洋环境更复杂、多变,在避碰智能决策方面面临如下2个难点。海洋环境复杂、多变。首先,在海洋环境下,风... 查看详情
[autocars]自动驾驶汽车概述（下）
本文将介绍自动驾驶汽车决策系统的相关技术，包含路线规划、行为选取、运动规划和控制子系统。1.路线规划 路线规划子系统负责计算出一条穿过道路网的路线，让自动驾驶汽车能从起始位... 查看详情
大模型多模态chatgpt+自动驾驶控制器设计方案
...对于文本的学习已经能够达到如此智能的地步，对于自动驾驶行业，ChatGPT能够为其带来什么呢？是否能够对困扰从业者多年的决策难题做出突破呢？自动驾驶中的决策难题在自动驾驶行业中，决策规划作为整... 查看详情
数据驱动的自动驾驶系统算法最新论文合集（27篇）
大家好，我是Charmve！应该说，自动驾驶的算法模块，基本都是数据驱动的训练模型要优于基于规则或者优化的，尤其是感知和预测。以下图（综述论文“ASurveyofAutonomousDriving:CommonPracticesandEmergingTechnologies”... 查看详情
自动驾驶6-2:几何横向控制lesson2:geometriclateralcontrol-purepursuit
...命令，然后我们将设计一个纯追踪控制器来为我们的自动驾驶汽车追踪环境中的参考路径。让我们开始吧。什么是几何路径跟踪控制器？平常，任何控制追踪参考路径仅使用 查看详情