自动驾驶决策规划算法第二章——Apollo EM Planner理论篇
前置学习内容： 【自动驾驶】【零基础】基础自动驾驶控制算法笔记
注意：最好学习前置控制算法，因为决策规划仿真中需要用到
决策规划算法第一章链接
自动驾驶决策规划算法第一章_免费教学录影带的博客-CSDN博客
https://blog.csdn.net/m0_62664599/article/details/126517025?csdn_share_tail=%7B%22type%22%3A%22blog%22%2C%22rType%22%3A%22article%22%2C%22rId%22%3A%22126517025%22%2C%22source%22%3A%22m0_62664599%22%7D
决策规划算法实践篇链接
https://blog.csdn.net/m0_62664599/article/details/127243697https://blog.csdn.net/m0_62664599/article/details/127243697
目录
一、决策规划仿真平台搭建
1、软件安装
2、前置准备工作
二、上参考线模块
1、决策规划总体概览（粗略）
2、参考线平滑算法
3、算法优化
4、代码实践部分
三、路径决策算法
1、确定规划起点
2、动态规划
3、控制接口，轨迹拼接
4、代码实践部分
一、决策规划仿真平台搭建
1、软件安装
老王的github主页：VincentWong3 (github.com)
下载：automatic-driving-decision-and-planning-for-matlab
如果github下载慢，可以用下面链接下载：
automatic-driving-decision-and-planning-for-matlab-main.zip - 蓝奏云
仿真需要以下软件：注意版本也要对应
1、Matlab 2020a或者b
2、CarSim 2019.1
3、Visual Studio 2019
4、PreScan 8.5
软件安装包获取：https://wwn.lanzout.com/iJrMm0chcyja （密码:gph2）
仿真采用PreScan+Matlab+CarSim的联合仿真
PreScan提供环境搭建，传感器模型等等。CarSim提供车辆动力学模型。Matlab主要是写算法和跑仿真的工具。此外需要C++的编译器，使用VS2019。
注意：CarSim这里要有Matlab2020a，然后选择它
如果没有的话，参考以下博客
【carsim2019和matlab2021a联合仿真matlab not found问题的解决】_维维维维维维的博客-CSDN博客_carsim找不到matlab
2、前置准备工作
打开prescan，新建一个实验，名为testplanner
选择Actor，将Audi A8拖出来
接下来吧location改成(0,0)，不用管
接下来启动CarSim选择右下角的这个选项
选择文件为Prescan里面的cpar文件，里面包含了一些底盘、转向，轮胎等等数据
路径：C:\Users\Public\Documents\Experiments\DemoTemplates\Demo_PreScanCarSim3D
接下来选择吧cpar解压放在那里，在PreScan的实验文件夹中新建一个DynamicModel文件夹确定
接下来CarSim会导入数据，
回到PreScan，对车右键，选择第一个
选择Dynamics，然后User spcified
选择这个文件
接下来吧CarSim的模型仿真频率改成1000Hz（至少200Hz以上，推荐1000因为和CarSim自带的一样不用改），PreScan的不用改，20就行
点击build按钮，会把prescan的模型生成simulink模型
build完打开matlab，不能直接打开，需要在桌面右下角打开
第一次打开时间比较长
打开后将matlab工作路径改成刚才新建的testplanner
testplanner_cs.slx为prescan build出来的模型，打开这个模型
左上角Regenerate作用：在Prescan中改动后需要build然后点一下Regenerate
点击车，进入里面去，
然后点击车辆动力学模型左下角箭头
如果没有的话，这么解决
这里的simfile.sim我们要手动绑定一下
点击CarSim的send to simulink
会在C:\Users\Public\Documents\Experiments\DynamicModel生成一个simfile
将C:/Users/Public/Documents/Experiments/DynamicModel/simfile.sim填入
输入输出设置
接下来去CarSim设置输入输出
前三个是方向盘转角，油门和制动压力。从第四个开始是坡度，从第16-23个是路面附着系数。
打开prescan自带的模型
在C:\Users\Public\Documents\Experiments\DemoTemplates\Demo_PreScanCarSim3D里面
打开之后进入Audi这个，然后打开动力学模型，
把输入输出复制进去
输入为 1 1 1 12 8，也就是23个输入，对应CarSim的输入，分别是方向盘转角，油门和制动压力，12个坡度，以及8个路面附着系数，使用mux做一个接口。
这里用in1和ground和constant模块，附着系数注意为向量ones(8,1)*9.0，然后坡度设置为接地，即0。
原本的CarSim输出有35个，没有ax，ay，这里添加进去作为第36和37的输出
然后点击send to simulink
进入模型，给输出增加一个28的demux
最后两个接口接出来，输出出去，然后加上9.8的增益
回到外面，设置四个输出，vx，vy，ax，ay，然后连上去
模型搭建完毕，接下来适配算法
打开github的文件，
将这三个文件复制到prescan的testplanner里面
回到prescan里面regene一下
打开emplanner_init.m运行一下，获得控制需要的参数，纵向pid横向lqr和油门刹车标定表都获得了
打开github下载的EMPlanner_cs.slx，把这部分复制到模型内
再把输入输出接口粘贴过来
这样就算搭建完毕了
下面对每个模块进行简介
（1）定位模块：把prescan自身传感器的数据输出出去，这里不研究定位算法
（2）决策规划模块：主要完成这一部分
（3）控制模块
下来是控制模块，输入是定位和规划信息，输出要输入给车辆的油门和刹车以及转角信息
转向不足的积分补偿
再把转向不足模块输出出的前轮转角转化为方向盘转角
转角关系在这里
（4）横向LQR模块（包含在控制模块里）
预测模块以及离线LQR的查表（根据车辆vx得到LQR的结果K）
（5）纵向双pid模块（包含在控制模块内）
这里标定表给好了，直接用就行
控制算法运行需要提前运行EMplanner那个文件让变量放到工作区内
规划时间是30S，
仿真时间也30S
准备就绪后就点击运行
这个时候运行会报错：
解决办法
可以尝试在Export: Outputs右下角Optional equations添加
DEFINE_OUTPUT mod1_L1 = 0; - ; mod1_L1
DEFINE_OUTPUT mod2_L1 = 0; - ; mod2_L1
DEFINE_OUTPUT mod3_L1 = 0; - ; mod3_L1
DEFINE_OUTPUT mod4_L1 = 0; - ; mod4_L1
DEFINE_OUTPUT mod1_R1 = 0; - ; mod1_R1
DEFINE_OUTPUT mod2_R1 = 0; - ; mod2_R1
DEFINE_OUTPUT mod3_R1 = 0; - ; mod3_R1
DEFINE_OUTPUT mod4_R1 = 0; - ; mod4_R1
DEFINE_OUTPUT mod1_L2 = 0; - ; mod1_L2
DEFINE_OUTPUT mod2_L2 = 0; - ; mod2_L2
DEFINE_OUTPUT mod3_L2 = 0; - ; mod3_L2
DEFINE_OUTPUT mod4_L2 = 0; - ; mod4_L2
DEFINE_OUTPUT mod1_R2 = 0; - ; mod1_R2
DEFINE_OUTPUT mod2_R2 = 0; - ; mod2_R2
DEFINE_OUTPUT mod3_R2 = 0; - ; mod3_R2
DEFINE_OUTPUT mod4_R2 = 0; - ; mod4_R2
EQ_OUT MOD1_L1 = (ABS(FY_L1)/FZ_REF(1,1))*MUY_L1;
EQ_OUT MOD2_L1 = SIN(MIN(ABS(ALPHL_L1), 15/DR))*ABS(VXCENL1);
EQ_OUT MOD3_L1 = (ABS(FX_L1)/FZ_REF(1,1))*MUX_L1;
EQ_OUT MOD4_L1 = MIN(ABS(KAPPL_L1), 1)*ABS(VXCENL1);
EQ_OUT MOD1_R1 = (ABS(FY_R1)/FZ_REF(1,2))*MUY_R1;
EQ_OUT MOD2_R1 = SIN(MIN(ABS(ALPHL_R1), 15/DR))*ABS(VXCENR1);
EQ_OUT MOD3_R1 = (ABS(FX_R1)/FZ_REF(1,2))*MUX_R1;
EQ_OUT MOD4_R1 = MIN(ABS(KAPPL_R1), 1)*ABS(VXCENR1);
EQ_OUT MOD1_L2 = (ABS(FY_L2)/FZ_REF(2,1))*MUY_L2;
EQ_OUT MOD1_L2 = SIN(MIN(ABS(ALPHL_L2), 15/DR))*ABS(VXCENL2);
EQ_OUT MOD3_L2 = (ABS(FX_L2)/FZ_REF(2,1))*MUX_L2;
EQ_OUT MOD4_L2 = MIN(ABS(KAPPL_L2), 1)*ABS(VXCENL2);
EQ_OUT MOD1_R2 = (ABS(FY_R2)/FZ_REF(2,2))*MUY_R2;
EQ_OUT MOD2_R2 = SIN(MIN(ABS(ALPHL_R2), 15/DR))*ABS(VXCENR2);
EQ_OUT MOD3_R2 = (ABS(FX_R2)/FZ_REF(2,2))*MUX_R2;
EQ_OUT MOD4_R2 = MIN(ABS(KAPPL_R2), 1)*ABS(VXCENR2);
下面去prescan设置一个跟随车的视角，选择custom
点右上角，然后改变XYZ的位置
接下来build然后regenerate再run，在visviewer里面humanview
可以正常跑起来，接下来运行testctrl.m
蓝色为规划轨迹，黄色为跟踪的轨迹，仿真平台搭建成功
二、上参考线模块
代码为EMPlanner_v0.1
automatic-driving-decision-and-planning-for-matlab-main.zip - 蓝奏云
打开EMPlanner_cs.slx，注意不能直接复制粘贴全部
相比于上面的模型更改如下：
首先是控制模块的双pid改为离散pid
其次是加入调度系统用来调控控制信号和规划信号的更新频率
接下来是理论部分：参考线 refrence line
1、决策规划总体概览（粗略）
已有导航路径，决策规划流程
①定位+导航：生成参考线
②静态障碍物投影到以参考线为坐标轴的frenet坐标系上
③决策算法对障碍物做决策（往左绕，往右绕，忽略）开辟最优的凸空间，然后搜索轨迹交给控制执行
如果往左绕，就在紫色空间中搜索。往右就蓝色搜索。这里有个评价标准
④规划算法在凸空间中搜索出最优路径
决策选择了蓝色空间，规划再搜索出一个路径
⑤后处理，在规划轨迹中选一个点，坐标转换成Cartisian，输出给控制去跟踪
这个流程比较粗略，暂时没考虑动态障碍物
2、参考线平滑算法
参考线是解决方案，解决路径过长，不平滑的问题
（1）过长的路径不利于坐标转换（找匹配点，计算s）
（2）过长的路径，障碍物投影可能不唯一。（图中绿色障碍物到两线距离都一样，不好判断。）
（3）不平滑。图中蓝色点导数不连续
解决方案：参考线
参考线如何生成：每个规划周期内，找到车在导航路径上的投影点，以投影点为坐标原点，往后取30m，往前取150m范围内的点，做平滑，平滑后的点的集合称为参考线
参考线平滑算法：点与点之间越接近直线越平滑，否则越不平滑
以这三个点做向量
是衡量平滑与不平滑的标准，越小，越平滑
但是参考线不是越平滑越好
如图紫色的线，虽然平滑了，但是几何形状差距太大。
那么如何衡量几何形状呢？
如果这三个长度
加起来越小，说明越接近原路径几何
此外希望长度尽可能比较均匀紧凑
紧凑性的衡量标准如下：（这里以三个点举例，后面扩展到n个点）
综上，平滑算法的数学表达如下
问题：已知
，未知
，求
Cost function 定义如下：（这里以三个点举例，后面扩展到n个点）
平滑代价项的简单说明： 两个向量和的平方
希望代价函数越小越好，越平滑越紧凑越好
目的是选择合适的让代价函数越小
这是一个典型的二次规划问题
（1）将平滑代价函数写成二次规划形式：
如果是n个点的形式
2n行，2n-4列的矩阵记为
平滑代价可以写成
其中：
（2）紧凑代价
直接写n个点
那么有紧凑代价：
（3）几何相似代价
几何相似代价：
其中
综上所述，有总的Cost function
与二次规划标准式
对比有
下面是约束
我们希望x与xref不要太远
这里的buff给的是0.1
此外还有曲率约束（防止曲率过大），一般是不需要的，曲率约束是非线性约束，处理起来比较麻烦
曲率约束一般与车的最大侧向加速度有关，可以放到后面考虑
下面看下曲率怎么算
这里就可以看到l是非线性的，取模这一步得开根号
3、算法优化
上面讲的平滑算法在Apollo中叫做Fem smoother
优点：优化变量较少
缺点：无法保证曲率是连续的，添加曲率约束求解较麻烦
此外还有优化一阶和二阶导数的方法，这种方法则优化变量更多，二次规划规模比较大。
方法流程
（1）找匹配点
（2）采样（前150，后30）
（3）Fem smooth平滑
参考线平滑算法主要难在实现上，而不是理论上。需要很快的计算，不能太慢。写出来不难，但是优化的很快是非常难的。
做出来不难，做好很难。在工程上很难实现。
期望：快，节省计算量
原因：决策规划流程
参考线是决策规划的基础和前提，必须要快。
怎样才能快呢？答：方法不唯一
优化思路：
（1）减少规划频率，规划算法每100ms执行一次，控制算法每10ms执行一次（调度系统）
（2）充分利用上一个规划周期的结果
如何快速找到每个周期的匹配点？
普通做法：每个规划周期都做遍历，从路径上第一个点一直遍历到最后一个点，找到举例车最近的
这种做法太慢，导航路径很可能很长，遍历很费时
如图，这里 
表示当前车的位置到参考线上的第i个点的距离
以上个周期的match point 为起点做遍历，一旦有
，立刻退出遍历li对应的点作为本周期的匹配点
tips，判断遍历的方向的方法
判断d\tau与0对比，就可以判断遍历的方向
本质有点像梯度下降
此方法可以大大加速找匹配点算法
问题：只适用于在上个匹配点结果附近只有一个极小值点
如图所示，落入下面的绿点，实际匹配点是在上面那个绿点
实际一般不会出现这种情况，规划周期是100ms，即使以50m/s运动，车也只是走了5m，5m内道路不太可能出现这么扭曲的几何，在上个规划周期的匹配点上一般只有一个极小值
万一出现这种情况，使用双保险：increase count变量记录l增加的次数
那么如果检查到了另外的极小值怎么办呢
在Github算法里
第一次运行时，遍历
不是第一次运行的话，把上次的匹配点当成起始点，往后找3个点如果没发现另外极小值就可以
（2）点不够的情况
要保持总长度不变，即二次规划的矩阵规模不变，
（3）轨迹拼接问题
因为上次规划和这次规划只是100ms，所以必然有大量重复区域，两次规划之间的参考线有重复部分，这里没有必要再算一遍
具体做法如下：
计算这个周期需要拼接多少点，把上个周期的两个点去掉，这个周期新加的两个点加进去。前面的点上个周期平滑过了，已经有结果了，所以这个周期只需要平滑新的点就可以了
只需要优化新加进来的点即可，前面不需要动的两个点视为等式约束，后面为不等式约束，
这样优化二次规划计算量就会少很多
此外还有个问题，如何快速的找到车在全局路径下的投影，如果找不到投影没法往前150m往后30m这个操作。
4、代码实践部分
首先打开prescan，打开上面弄好的模型
使用prescan打开matlab
进入文件夹，运行emplanner_init.m，把变量放在工作区
打开testplanner_cs.slx
接下来打开Carsim，Send to Simulink一下，然后matlab simulink regenerate一下
接下来run一下
运行testctrl.m脚本，发现实际曲线和规画曲线贴在一起，说明环境配置没问题
接下来要把全局路径模块做进去，一般要使用定位和地图实现，这里不做定位地图，用其他办法。
回到prescan，造一个道路出来
选择300m的直路
选择400m弯道
选择300m弯道
接下来导入matlab，使用这个工具
把所有道路连接点点一下
到最后按ESC退出获得这条全局路径
接下来用一个巧妙地办法获取这条路径的信息，使用一辆新的车，让他沿着这个路跑，把它的定位信息放到matlab工作区内再保存成mat文件，记为路径信息。
拖一辆车进来，路径变蓝绿色松手
自动变到起点说明正确
右键车辆选择object configuration，然后调节速度为10
选择2D Simple
选择 Path Follwer
设置完毕，点击build，然后regenerate
进入第二个车，设置两个采样频率为0,1的to workspace
连接到x和y
把自己的模块注释掉，时间改成300s，然后run
使用prescan可以看到道路采集车在跑，这里右键选择top view
得到路径数据
接下来在工作区输入以下两行代码，获取只有路径点信息的变量
global_path_x = global_path_x_original.data
global_path_y = global_path_y_original.data另存为mat文件
接下来在emplanner_init.m中加上加载全局路径的代码
把采集车注释掉，取消注释我们的模块
接下来写routine模块：
把global_path_x使用constant导入，然后使用goto模块
创建子模块
使用bus和from模块，将数据导出
使用goto把routine模块导出
这里routine模块制作完毕
下面开始写参考线算法，这里老王建议明白原理后自己写一遍，不要照抄
进入decision&planning module，删掉test planning及其输入输出
然后添加俩输入
然后把外面的goto去掉，直接用线连接，上面的location info也是直接连接
然后使用bus selector，把两个info都输出出来
找到定位坐标在全局路径上的投影，然后通过投影去提取参考线，然后在做平滑
第一步，先找投影，前面已经有找车的投影的算法，但是障碍物的投影不一样，障碍物可以看成由很多点组成，需要找一个通用的找投影的方法，可以批量处理一大堆点的投影
新建一个模块写函数来计算投影点
代码流程：
如果是第一次运行，从第一个点执行遍历，找到距离当前点最小的路径点作为匹配点，接下来把匹配点转化为投影点。首先求出匹配点位矢以及车的位矢，可以获得车到匹配点的位矢d，然后得到匹配点与投影点的弧长（纵向误差）
，再乘以
，得到es向量，
。投影点的位矢就等于匹配点加上es向量，投影点的heading，用老王的方法得到
，投影点的曲率等于匹配点的曲率，然后，记录下当前匹配点在所有路径中的index。不是第一次运行的话，从上次记录的匹配点开始遍历，先判断遍历方向，求出
，大于0往下接着遍历，小于0则往前遍历，然后跟第一次运行一样获取匹配点，再转换为投影点。
function [match_point_index_set,proj_x_set,proj_y_set,proj_heading_set,proj_kappa_set] = fcn(x_set,y_set,path_x,path_y,path_heading,path_kappa)
% 该函数是一个通用函数，找到x_set,y_set在path_x,path_y的匹配点的编号，
% 以及投影点在直角坐标系下的(x,y,theta,kappa)
% 最多可以处理128个点
% 初始化
match_point_index_set = ones(128,1)*nan;
proj_x_set = ones(128,1)*nan;
proj_y_set = ones(128,1)*nan;
proj_heading_set = ones(128,1)*nan;
proj_kappa_set = ones(128,1)*nan;
persistent is_first_run; % 判断是否第一次运行
persistent init_search_index_set;
if isempty(is_first_run) % 如果是第一次运行
is_first_run = 0; % 标记为0
init_search_index_set = ones(128,1)*nan;
for i = 1:length(x_set) % 遍历所有的点
if isnan(x_set(i)) % 如果为nan就break掉
break;
else
increase_count = 0;
min_distance = 1e10;
for j = 1:length(path_x) % 遍历所有的离散路径点
distance = (x_set(i)-path_x(j))^2 + (y_set(i)-path_y(j))^2; % 计算当前点到路径的的距离
if (min_distance > distance)
min_distance = distance; % 找到距离路径最小的距离
init_search_index_set(i)=j; % 更新这个点的index
increase_count = 0;
else
increase_count = increase_count + 1; % 否则遍历次数+1
end
if increase_count > 50 %如果遍历了50个点，l一直增大，就break掉
break;
end
end
match_point_index = init_search_index_set(i); % 获取当前点的匹配点的index
match_point_index_set(i) = match_point_index; % 再放到match_point_index_set内
% 获取匹配点的位矢
vector_match_point = [path_x(match_point_index);path_y(match_point_index)];
% 获取匹配点矢量的切向方向向量
vector_match_point_direction = [cos(path_heading(match_point_index));sin(path_heading(match_point_index))];
vector_host_point = [x_set(i);y_set(i)]; % 获取当前点的位矢
% 获取向量d，即匹配点到当前点的向量
vector_d = vector_host_point - vector_match_point;
% d = （误差位矢*匹配点切向量） * 匹配点切向量
vector_err = (vector_d'* vector_match_point_direction) * vector_match_point_direction;
% 投影点位矢 = 误差位矢 + 匹配点位矢
vector_proj = vector_err + vector_match_point;
proj_x_set(i) = vector_proj(1); % 获取投影点x坐标
proj_y_set(i) = vector_proj(2); % 获取投影点y坐标
% 投影点方向 = 匹配点方向 + 匹配点曲率 * （误差位矢*匹配点切向量）
proj_heading_set(i) = path_heading(match_point_index) + path_kappa(match_point_index) * (vector_d'* vector_match_point_direction);
proj_kappa_set(i) = path_kappa(match_point_index); % 投影点曲率等于匹配点曲率
end
end
else % 如果不是第一次运行
min_distance = inf;
for i = 1:length(x_set)
if isnan(x_set(i))
break;
else
increase_count = 0;
start_index = init_search_index_set(i); % 从上次的匹配点开始遍历
% 获取上次匹配点位矢以及切向量
vector_m_init = [path_x(start_index);path_y(start_index)];
vector_direction_init = [cos(path_heading(start_index));sin(path_heading(start_index))];
% 判断遍历方向，ds = (d*tau) * tau，大于0说明要往后面遍历，小于0则往前面遍历
ds = ([x_set(1);y_set(1)]-vector_m_init)'*vector_direction_init;
if ds > 0
for j = start_index:length(path_x) % 找到距离最小的点作为匹配点
distance = (x_set(i)-path_x(j))^2 + (y_set(i)-path_y(j))^2;
if (min_distance > distance)
min_distance = distance;
init_search_index_set(i)=j;
increase_count = 0;
else
increase_count = increase_count + 1;
end
% 如果接下来有三个点和车的距离越升越大，则认为三个点前面的那个点为匹配点
if increase_count > 3
break;
end
end
else
for j = start_index:-1:1 % 换个遍历方向而已，其他一样
distance = (x_set(i)-path_x(j))^2 + (y_set(i)-path_y(j))^2;
if (min_distance > distance)
min_distance = distance;
init_search_index_set(i)=j;
increase_count = 0;
else
increase_count = increase_count + 1;
end
if increase_count > 3
break;
end
end
end
% 和前面一样，将匹配点转化为投影点
match_point_index = init_search_index_set(i);
match_point_index_set(i) = match_point_index;
vector_match_point = [path_x(match_point_index);path_y(match_point_index)];
vector_match_point_direction = [cos(path_heading(match_point_index));sin(path_heading(match_point_index))];
vector_host_point = [x_set(i);y_set(i)];
vector_d = vector_host_point - vector_match_point;
vector_err = (vector_d'* vector_match_point_direction) * vector_match_point_direction;
vector_proj = vector_err + vector_match_point;
proj_x_set(i) = vector_proj(1);
proj_y_set(i) = vector_proj(2);
proj_heading_set(i) = path_heading(match_point_index) + path_kappa(match_point_index) * (vector_d'* vector_match_point_direction);
proj_kappa_set(i) = path_kappa(match_point_index);
end
end
end然后还需要一个函数来计算曲率，可以计算离散的路径点每个点的曲率大小
该算法使用公式：
，求出每两个离散的路径点之间的曲率。其中
和ds的计算都用中点欧拉法
代码如下
function [path_heading,path_kappa] = fcn(path_x,path_y)
%该函数将计算path在切线方向与x轴的夹角和曲率
% 输入：path_x，y路径坐标
% 输出：path heading kappa 路径的heading和曲率
% 原理：heading = arctan(dy/dx);
% kappa = dheading/ds;
% ds = (dx^2 + dy^2)^0.5;
% 注意，角度的多值性会带来很大的麻烦，需要慎重处理
% 例：x 与x+2pi往往代表同一个角度，这种多值性在计算曲率会带来麻烦
% 比如，原本是(0.1 - (-0.1))/ds，但如果计算出的-0.1多了一个2pi
% kappa就等于(0.1 - (-0.1 + 2*pi))/ds，曲率就会变得非常大
% 还有若采用中点欧拉法计算heading, (即使用(y2-y1)/(x2-x1)的反正切计算一个角度，
% 用(y1 - y0)/(x1 - x0)的反正切又计算一个角度，然后加起来除以2)，如果精确值是(a1+a2)/2，
% 但最终计算结果可能是（a1+a2+2pi）/2
% 还有tan(x) = tan(x + pi) 所以arctan(tan(x))可能等于x，也可能等于x + pi
% 角度的处理非常麻烦，而且角度处理不当往往是许多奇怪错误的源头
dx = diff(path_x); % 函数diff可以求出两个x_i之间的差值，作为微分dx
dy = diff(path_y);
% 用中点欧拉法要比向前欧拉法向后欧拉法更精确，但是也可能计算出的结果比精确值多一个pi
% 在这里对dx，dy做中点欧拉法，使用四象限反正切计算角度
dx_pre = [dx(1);dx];
dx_after = [dx;dx(end)];
dx_final = (dx_pre + dx_after)/2;
dy_pre = [dy(1);dy];
dy_after = [dy;dy(end)];
dy_final = (dy_pre + dy_after)/2;
ds_final = sqrt(dx_final .^2 + dy_final.^2);
path_heading = atan2(dy_final,dx_final);
dheading = diff(path_heading);
dheading_pre = [dheading(1);dheading];
dheading_after = [dheading;dheading(end)];
dheading_final = (dheading_pre + dheading_after)/2;
%为了防止dheading出现多一个2pi的错误，假设真实的dheading较小，用sin(dheading)近似dheading
path_kappa = sin(dheading_final) ./ ds_final;
end
投影点的信息我们暂时用不到，通过匹配点的编号，把参考线取出来
需要一个函数来提取出集合中第一个元素，即车当前位置对应的匹配点index
function y = fcn(u)
y = u(1);然后根据匹配点来提取参考线的未平滑的初值，用于后面二次规划
算法流程：获取当前匹配点前面30，后面150个点的索引起点和终点。
如图，如果前面不足30个点，直接用索引1；如果后面不足150个点，就把起点的index设为终点index-180。因为要确保参考线有180个点，确保二次规划的未知数数量固定。
function [refrenceline_x_init,refrenceline_y_init] = fcn(host_match_point_index, global_path_x, global_path_y)
%该函数将在全局路径上提取refrenceline的未平滑的初值
% 输入：host_match_point_index 自车的位置在全局路径的匹配点的编号
% global_path_x, global_path_y 全局路径数据
% 输出：refrenceline_x_init,refrenceline_y_init 未平滑的参考线的xy坐标
%由于global path 是每1m取一个点，所以从匹配点往后取30个点，往前取150个点即可，一共181个点
% 后面的点不够的话就用前面的补，前面的点不够的话就用后面的点补，保证总数是181个
% 索引初始化
start_index = -1;
%判断后面前面的点是否足够多
if host_match_point_index - 30<1
%匹配点后面的点太少了，不够30个
start_index = 1;
elseif host_match_point_index + 150 > length(global_path_x)
%匹配点前面的点太少了，不够150个
start_index = length(global_path_x) - 180;
else
%匹配点正常情况
start_index = host_match_point_index - 30;
end
% 提取refrenceline
refrenceline_x_init = global_path_x(start_index:start_index+180);
refrenceline_y_init = global_path_y(start_index:start_index+180);
end接下来写二次规划的平滑算法，系数设置如下：平滑cost系数为2，紧凑系数设置为3，和原参考线相似系数设置为2，然后自变量xy的上下界为±0.2
看算法具体代码怎么写的。
输入参考线的x，y以及各个系数，上下界。输出优化后的参考线x，y坐标。
写出Cost function中的矩阵H以及f
添加约束，然后使用quadprog求解，得到优化后的refrenceline
function [referenceline_x,referenceline_y] = ...
fcn(referenceline_x_init,referenceline_y_init,w_cost_smooth,w_cost_length,w_cost_ref,x_lb,x_ub,y_lb,y_ub)
% 该函数将平滑referenceline
% 使用二次规划
% 0.5x'Hx + f'x = min
% lb < x < ub
% 输入referenceline x,y,init未平滑的参考线
% 输出referenceline_x，referenceline_y 平滑后的参考线
%在这里为了简化，不使用参考线拼接算法
% 计算量问题通过调度解决，规划周期为100ms，只要在100ms计算完成就可以
%如果到最后计算速度成问题在优化，目前暂时不使用拼接
%simulink无法直接使用自带的二次规划函数，需要从外部引入
coder.extrinsic("quadprog");
%二次规划形式
% H1 = w_cost_smooth*(A1' * A1) + w_cost_length*(A2' * A2) + w_cost_ref*(A3' * A3)
% f = -2 * w_cost_ref * referenceline_init
% 0.5x'Hx + f'x = 0.5 * x'(2H1)*x + f'x
% A1 = [1 0 -2 0 1 0
% 0 1 0 -2 0 1
% 1 0 -2 0 1 0
% 0 1 0 -2 0 1
% .......................
% A2 = [1 0 -1 0
% 0 1 0 -1
% 1 0 -1 0
% 0 1 0 -1
% .......................
%Ａ3为单位矩阵
%设置求解矩阵规模，因为参考线输出点一共181个，每个点有两个坐标，所以求解矩阵规模是362个
n = 181;
%referenceline初始化
referenceline_x = zeros(n,1);
referenceline_y = zeros(n,1);
A1 = zeros(2 * n - 4, 2 * n);
A2 = zeros(2 * n - 4, 2 * n);
A3 = eye(2 * n , 2 * n);
f = zeros(2 * n,1);
lb = zeros(2 * n,1);
ub = zeros(2 * n,1);
for i = 1:n
f(2*i - 1) = referenceline_x_init(i);
f(2*i) = referenceline_y_init(i);
lb(2*i - 1) = f(2*i - 1) + x_lb;
ub(2*i - 1) = f(2*i - 1) + x_ub;
lb(2*i ) = f(2*i ) + y_lb;
ub(2*i ) = f(2*i ) + y_ub;
end
for j = 1:2*n - 5
A1(i,i) = 1;
A1(i,i+2) = -2;
A1(i,i+4) = 1;
A1(i+1,i+1) = 1;
A1(i+1,i+3) = -2;
A1(i+1,i+5) = 1;
end
for k= 1:2*n - 3
A2(i,i) = 1;
A2(i,i+2) = -1;
A2(i+1,i+1) = 1;
A2(i+1,i+3) = -1;
end
H1 = w_cost_smooth * (A1' * A1) + w_cost_length*(A2' * A2) + w_cost_ref*(A3' * A3);
H = 2 * H1;
%用referenceline的初值作为二次规划的迭代起点
X0 = f;
f = -2 * w_cost_ref *f;
%二次规划求解
X = quadprog(H,f,[],[],[],[],lb,ub,X0);
%将结果输出到referenceline中
for i = 1:n
referenceline_x(i) = X(2*i - 1);
referenceline_y(i) = X(2*i );
end
end参考线模块输出信息为info和dubug，info包含优化后的参考线xy坐标，debug包含所有其他变量。使用goto和from标记输出出去。
退出去将整个模块打包重命名为referenceline provider
接下来把referenceline发给控制，进行简单的验证
首先使用一个模块生成desire的信息，让控制去跟踪
这里也用到前面的计算曲率函数
把参考线输入进去，输出为desire点的信息
即获取匹配点集合的index=1的index，即车所在位置匹配点的index，再获取对应优化的参考线上面的点作为desire的信息
function [xr,yr,thetar,kappar] = fcn(host_match_point_index,path_x,path_y,path_heading,path_kappa)
index = host_match_point_index(1);
xr = path_x(index + 1);
yr = path_y(index + 1);
thetar = path_heading(index + 1);
kappar = path_kappa(index + 1);
end
至此代码写的差不多了，运行测试
run发现运行速度非常慢，因为这个是以0.1ms再进行二次规划
接下来对二次规划的代码进行优化：
进入二次规划的函数，主要改动为：声明一个变量存储是否第一次运行，以及存储一下上一次优化前后结果。从第二次运行开始，如果本次需要优化的参考线起点和上次需要优化的参考线起点相同的话，直接拿上次优化后的结果作为本次优化后的结果输出，否则执行原来的优化操作。这样可以在一定程度上减少了执行二次规划quadprog的次数。
function [referenceline_x,referenceline_y] = ...
fcn(referenceline_x_init,referenceline_y_init,w_cost_smooth,w_cost_length,w_cost_ref,x_lb,x_ub,y_lb,y_ub)
% 该函数将平滑referenceline
% 使用二次规划
% 0.5x'Hx + f'x = min
% lb < x < ub
% 输入referenceline x,y,init未平滑的参考线
% 输出referenceline_x，referenceline_y 平滑后的参考线
%在这里为了简化，不使用参考线拼接算法
% 计算量问题通过调度解决，规划周期为100ms，只要在100ms计算完成就可以
%如果到最后计算速度成问题在优化，目前暂时不使用拼接
%simulink无法直接使用自带的二次规划函数，需要从外部引入
coder.extrinsic("quadprog");
%二次规划形式
% H1 = w_cost_smooth*(A1' * A1) + w_cost_length*(A2' * A2) + w_cost_ref*(A3' * A3)
% f = -2 * w_cost_ref * referenceline_init
% 0.5x'Hx + f'x = 0.5 * x'(2H1)*x + f'x
% A1 = [1 0 -2 0 1 0
% 0 1 0 -2 0 1
% 1 0 -2 0 1 0
% 0 1 0 -2 0 1
% .......................
% A2 = [1 0 -1 0
% 0 1 0 -1
% 1 0 -1 0
% 0 1 0 -1
% .......................
%Ａ3为单位矩阵
%设置求解矩阵规模，因为参考线输出点一共181个，每个点有两个坐标，所以求解矩阵规模是362个
n = 181;
%referenceline初始化
referenceline_x = zeros(n,1);
referenceline_y = zeros(n,1);
A1 = zeros(2 * n - 4, 2 * n);
A2 = zeros(2 * n - 4, 2 * n);
A3 = eye(2 * n , 2 * n);
f = zeros(2 * n,1);
lb = zeros(2 * n,1);
ub = zeros(2 * n,1);
persistent is_first_run;
persistent pre_result_x;
persistent pre_result_y;
persistent pre_referenceline_x_init;
persistent pre_referenceline_y_init
if isempty(is_first_run)
is_first_run = 0;
for i = 1:n
f(2*i - 1) = referenceline_x_init(i);
f(2*i) = referenceline_y_init(i);
lb(2*i - 1) = f(2*i - 1) + x_lb;
ub(2*i - 1) = f(2*i - 1) + x_ub;
lb(2*i ) = f(2*i ) + y_lb;
ub(2*i ) = f(2*i ) + y_ub;
end
for j = 1:2*n - 5
A1(i,i) = 1;
A1(i,i+2) = -2;
A1(i,i+4) = 1;
A1(i+1,i+1) = 1;
A1(i+1,i+3) = -2;
A1(i+1,i+5) = 1;
end
for k= 1:2*n - 3
A2(i,i) = 1;
A2(i,i+2) = -1;
A2(i+1,i+1) = 1;
A2(i+1,i+3) = -1;
end
H1 = w_cost_smooth * (A1' * A1) + w_cost_length*(A2' * A2) + w_cost_ref*(A3' * A3);
H = 2 * H1;
%用referenceline的初值作为二次规划的迭代起点
X0 = f;
f = -2 * w_cost_ref *f;
%二次规划求解
X = quadprog(H,f,[],[],[],[],lb,ub,X0);
%将结果输出到referenceline中
for i = 1:n
referenceline_x(i) = X(2*i - 1);
referenceline_y(i) = X(2*i );
end
% 把参考线x，y坐标存起来
pre_result_x = referenceline_x;
pre_result_y = referenceline_y;
% 把参考线优化前的x，y坐标存起来
pre_referenceline_x_init = referenceline_x_init;
pre_referenceline_y_init = referenceline_y_init;
else
% 如果上次参考线优化前的起点x，y坐标和这次相同
if pre_referenceline_x_init(1) == referenceline_x_init(1) && ...
pre_referenceline_y_init(1) == referenceline_y_init(1)
% 起点相同，可以认为本周期和上周期的referenceline_init一模一样，就直接复用上个周期的结果
referenceline_x = pre_result_x;
referenceline_y = pre_result_y;
else
%起点不同，重复第一次运行的逻辑
for i = 1:n
f(2*i - 1) = referenceline_x_init(i);
f(2*i) = referenceline_y_init(i);
lb(2*i - 1) = f(2*i - 1) + x_lb;
ub(2*i - 1) = f(2*i - 1) + x_ub;
lb(2*i ) = f(2*i ) + y_lb;
ub(2*i ) = f(2*i ) + y_ub;
end
for j = 1:2*n - 5
A1(i,i) = 1;
A1(i,i+2) = -2;
A1(i,i+4) = 1;
A1(i+1,i+1) = 1;
A1(i+1,i+3) = -2;
A1(i+1,i+5) = 1;
end
for k= 1:2*n - 3
A2(i,i) = 1;
A2(i,i+2) = -1;
A2(i+1,i+1) = 1;
A2(i+1,i+3) = -1;
end
H1 = w_cost_smooth * (A1' * A1) + w_cost_length*(A2' * A2) + w_cost_ref*(A3' * A3);
H = 2 * H1;
%用referenceline的初值作为二次规划的迭代起点
X0 = f;
f = -2 * w_cost_ref *f;
%二次规划求解
X = quadprog(H,f,[],[],[],[],lb,ub,X0);
%将结果输出到referenceline中
for i = 1:n
referenceline_x(i) = X(2*i - 1);
referenceline_y(i) = X(2*i );
end
pre_result_x = referenceline_x;
pre_result_y = referenceline_y;
pre_referenceline_x_init = referenceline_x_init;
pre_referenceline_y_init = referenceline_y_init;
end
end
end可以发现运行速度提升很多，但还是比较慢
下面使用调度系统进行优化，让规划以100ms执行一次，控制以
回到A8的界面，使用chart创建一个状态机
查看迭代步长，这里是1ms
下面点开chart，add一个data
添加两个data命名为ij，然后初始值改成1
添加两个event，然后调成output
有两个输出代表正确
下面进入chart，
拖成这样
添加条件，每100ms调用一次planning
接下来加上控制
俩goto设置输出
规划和控制模块都加上trigger
然后回到上一层，给两个trigger加上from
此时完成调度系统
在运行发现速度提升不少
至此，参考线算法模块搭建完毕
三、路径决策算法
上一节的参考线模块提供了光滑的frenet坐标轴
1、确定规划起点
避障还是得在自然坐标系下做，host向参考线投影为坐标原点建立自然坐标系，
所有障碍物都投影到自然坐标系下面，生成SL图
自车的位置为（0,l0），因为其在原点上
接下来就开始规划，第一步是确定规划的起点
定位得到的host_x，host_y，投影referenceline，得到SL坐标(0,l0)，以此点为路径规划的起点，这么做是错误的。
因为考虑控制是不完美的
规划以100ms周期执行，在上个周期已经规划出轨迹
从控制的角度来看，规划的轨迹是割裂的，一段一段不连续的
正确的做法是
判断current location与pre-period规划结果是否误差太大
如果误差太大，就以current location来投影referenceline，以（0,l0）为规划起点
若差别不大，要么当前点投影上个周期轨迹得到P点，要么干脆直接拿当前点当P点
以（s0,l0）作为规划的起点而不是定位的坐标作为起点，效果怎么样
从控制的角度来看，就是一小截一小截的多段首尾连续的轨迹拼接而成
如果用定位作为轨迹的起点
SL图
搜索出一条最优路径
最优一般有以下三个标准
（1）平滑
（2）和障碍物保持适当的距离
（3）贴近参考线
可以用二次规划解决
cost function：平滑代价+参考线距离代价+障碍物距离代价
很遗憾：避障问题的二次规划是非凸的，cost function有多个极小值点
比如，下面这个车绕树的问题，
代价函数如下
对于非凸问题，没有一个完美的解决方案
方法：离散化，启发式搜索出一个粗解，以粗解为基础优化出最终解
撒点离散化，然后在粗解附近迭代求解，看往左绕还是往右绕最小，如果找到一个最小值往右绕的凸空间，我们就以往右绕的凸空间作为二次规划的凸空间。
这种方法并不完美，会搜到次优解，但是对于自动驾驶也够用了
在离散空间下的最优路径是什么，此最优路径将开辟一个凸空间，在此凸空间上做二次规划
离散空间（图1）的最优路径称为粗解（图2），粗解开辟凸空间（图3），在此凸空间上优化出最终解
如何在凸空间上优化出最终解？ 答：二次规划
如何在离散空间上找到粗解？ 答：动态规划
下面先说动态规划
连接每两个点的曲线是什么？
答：五次多项式
五次多项式6个系数，求
例：对于起点和下个点
路径规划的起点一般有heading约束，边界条件为：
如果对于中间点，怎么确定边界条件
更简单，认为导数为0即可
因为这个解是粗解，所以长得什么样无所谓，是用来判断的，开辟凸空间用。
这样就获得了一个离散点组成的路径
如何评价路径的好坏？cost function
cost function可以设置为：一二三阶导数求和的平滑代价，再加上障碍物距离代价，参考线距离代价
下面说一下为什么这么设置
（1）平滑：越像直线越平滑，直线最短
所以期望
最小，等价于
最小，但是积分处理起来麻烦，所以离散化成求和
最小
二阶和三阶求和主要考虑到曲率和曲率的变化率，不希望扭来扭曲，曲率变化太快
（2）障碍物距离代价
g(x)这么设置：在d1范围外为0，在（d2，d1）范围内线性增长，在（0，d1）内为无穷大
或者
其实就是惩罚函数
2、动态规划
下面来说如何找最短路径
每个路径都有对应的cost，问题变为找图的最短路径问题
计算机：dijstra算法，自动驾驶没必要
使用动态规划的方法解决这个问题
记cost（P0,Pij）表示P0到Pij点花费的最小代价
可以得到第二层的最小cost
第三层
可以得到第三层的最小cost
前面已经算过
所以cost(P0,P13)可由子问题推出
第j层
状态转移方程
得到P0到任一点的最小距离
如果要算第五层
动态规划计算出
，选最小的作为动态规划的输出
3、控制接口，轨迹拼接
现在的控制接口（控制输入）为
规划为100ms，控制周期为10ms，也就是说控制有9个周期都在跟踪同一个点，这样的效果不好，所以在规划周期和控制周期不同步的时候，不能直接用这个输入
想想
是怎么来的
规划：规划出了trajectory，包含了一系列的
规划周期为100ms，在trajectory找到t=100ms所对应的
（找不到就找100ms相邻的2个时间，做插值），作为规划输出发出去
在规控周期同步时，无问题。不同步时，有问题
因为规划已经规划出了10ms,20ms,......,100ms的轨迹，但只把100ms后的点发了出去，必然造成控制效果变差
所以：原代码中的控制接口不可取，需要改进
改造：规划直接发一条带时间的轨迹给控制
规划输出
控制每10ms搜索trajectory
控制根据当前控制周期的时间，搜索轨迹，找到当前时间对应的
，输出给控制算法去跟踪
方法很简单，细节是魔鬼
问题一：轨迹的时间是相对时间还是绝对时间？
例：16:00:00开始规划，16:00:10规划完成，规划出了4秒轨迹
有三种发给控制的选择
（1）trajectory-t = （0,......,4）相对时间
（2）trajectory-t = （16:00:00,......,16:04:00）绝对时间
（3）trajectory-t = （16:00:00,......,16:04:10）绝对时间
23区别在于规划的起点
到底是相对时间还是绝对时间？
推荐发绝对时间，因为做拼接，做控制，绝对时间更直观方便
例：16:00 规划了4秒
本周期16:00:10 首先获得本周期的时间（16:00:10），直接查找上一时刻规划的16:00:10对应的点
对于控制：16:00:13，查trajectory对应的16:00:13的点，输出给算法进行跟踪
问题二：规划起点
设在本周期规划开始的时间为T
规划起点是T，还是T+100ms?
答：T+100ms
为什么？
控制频率比规划快，在[T, T+100ms]内，此时规划未计算完成，那么控制跟踪的是哪个轨迹
答：上个周期的轨迹
从控制看来
如果以T+100ms为规划起点
这里引用一篇文章：
百度 Apollo 轨迹规划技术分享笔记 - 知乎 (zhihu.com)
如果误差不大的话，就以T+100ms作为规划起点，如果误差较大的话，就用车辆动力学外推100ms后的状态作为规划起点，这就是轨迹拼接算法
解释为什么不能用定位的点作为规划起点
控制不完美，用定位做就会导致曲线不连续
解决办法：轨迹拼接
2022年10月10日
请等待后续更新