参考线平滑算法

⼆、参考线平滑算法

三种平滑算法

离散点平滑分为两种，⼀种CosTheta，⼀种是FemPos 下⾯是对平滑算法的配置函数

         

平滑函数

在raw上⾯设置中间点，然后再进⾏平滑，在smotther中进⾏如何设置中间点

         

根据参考线在纵向的投影进⾏0.25m的采样，获取AnchorPoint，AnchorPoint包括路径点。裕度（周围可以平移的空间）。以及是不是强约束，如果是强约束需要满⾜裕度限制，⾸尾两个点都要满⾜，中间点不⽤。

Smooth函数

可以⼿动配置两种离散点平滑算法

输⼊参数为，Anchor Point的横纵向坐标以及裕度

         

⾸先对裕度进⾏收缩，然后调⽤Solve进⾏求解三种求解⽅式

         

考虑曲率约束为⾮线性的，使⽤Ipopt求解，不考虑曲率约束则为线性的，使⽤OSQP 求解

默认是不考虑曲率约束基于离散点的平滑

         

问题为⼆次优化问题，使⽤OSQP求解下⾯来看约束和代价函数

优化函数三部分组成，⼀部分是平滑度，⼀部分是⻓度，⼀部分是相对原始点便宜量

⻓度很容易理解，就是⻓度。cost误差也就是差值

cost smooth有两种计算⽅式

         

FemPos采⽤，和向量模的平⽅来表⽰，假设有三个点P0，P1以及P2，求出P1P0和P1P2的和向量P1PS，然后计算P1PS的模，如果模⽐较⼩说明⽐较平滑

         

CosTheta则采⽤相邻两个向量之间的夹⾓值，CosTheta值越⼤，夹⾓越⼩，也就是越接近直线

所以平滑度可以采⽤CosTheta的负值来表⽰两种约束条件

         

上⾯的约束是软约束，还需要添加硬约束，也就是横纵向裕度，添加box对参考点进

⾏限制，限制⽅法为在xy上⾯添加位置约束

         

此外还需要考虑曲率约束，Apollo通过近似假设计算，找到相邻的三个点，求出外接圆，

假设1：外接圆上的点两两之间均匀采样

假设2：相邻线段之间的夹⾓很⼩，相当于两个向量假设2：前两个条件可以推出弧⻓与弦⻓接近

然后计算出最⼤曲率cur_cstr，带⼊可以计算出约束⽅程

（三）U型路⼝仿真调试