自动驾驶之轨迹规划5——Apollo规划中的离散点曲线平滑数学原理

1. 前言

本文详细讲解Apollo的曲线平滑的数学原理,原文在《开发者说丨离散点曲线平滑原理》,参考Apollo代码在此,以下为整理内容。


2. 离散点曲线平滑的数学原理

如下图所示, P 0 P_0 P0 P 1 P_1 P1 P 2 P_2 P2 P 3 P_3 P3,…, P n P_n Pn, 一共 n + 1 n+1 n+1个离散点组成原始参考线。本节介绍一种方法通过对原始参考线上离散点的有限偏移对原始参考线进行平滑。
在这里插入图片描述


2.1. 优化变量

优化变量:离散点坐标 ( x i , y i ) (x_i,y_i) (xi,yi)


2.2. 优化目标

优化目标:平滑度、长度、相对原始点偏移量


2.3. 目标函数

目标函数 c o s t = c o s t 1 + c o s t 2 + c o s t 3 cost=cost_1+cost_2+cost_3 cost=cost1+cost2+cost3。其中 c o s t 1 cost_1 cost1为平滑度代价, c o s t 2 cost_2 cost2为长度代价, c o s t 3 cost_3 cost3为相对原始点偏移量代价。
在这里插入图片描述

c o s t 1 cost_1 cost1物理意义:如下图中红色向量 P 1 P 3 ⃗ \vec {P_1P_3} P1P3 的模,它可以理解为:向量 P 1 P 0 ⃗ \vec {P_1P_0} P1P0 和向量 P 1 P 2 ⃗ \vec {P_1P_2} P1P2 相加结果( P 1 P 3 ⃗ \vec {P_1P_3} P1P3 )的模平方。如果这三个点在一条直线上,那么这个值最小;三个点组成的两个线段的夹角 θ \theta θ越大,即曲线越趋于平直,也因此越平滑。
在这里插入图片描述
关于平滑度的表示除了上面提到的 c o s t 1 cost_1 cost1形式之外,还存在其他形式去表示,如下图所示,连续3点 P 0 P_0 P0 P 1 P_1 P1 P 2 P_2 P2,其中向量 P 0 P 1 ⃗ = ( x 1 − x 0 , y 1 − y 0 ) \vec {P_0P_1}=(x_1-x_0,y_1-y_0) P0P1 =(x1x0,y1y0) P 1 P 2 ⃗ = ( x 2 − x 1 , y 2 − y 1 ) \vec {P_1P_2}=(x_2-x_1,y_2-y_1) P1P2 =(x2x1,y2y1)之间的夹角 θ \theta θ
在这里插入图片描述
c o s θ cos\theta cosθ值越大, θ \theta θ越小, P 0 P_0 P0 P 1 P_1 P1 P 2 P_2 P2越接近直线,曲线越平滑。
在这里插入图片描述
因此, c o s t 1 cost_1 cost1可以表示为:
在这里插入图片描述

这两种代价形式也是开源Apollo代码中discretized_points_smoothing中cos_theta_ipopt_interface、fem_pos_deviation_ipopt_interface、fem_pos_deviation_osqp_interface、fem_pos_deviation_sqp_osqp_interface方法中对应用到的基本原理。

c o s t 2 cost_2 cost2物理意义:代表离散点之间距离平方之和。

c o s t 3 cost_3 cost3物理意义:代表优化后的离散点相对原始参考点偏移距离平方之和。


2.4. 约束

约束条件包含三个:位置约束,曲率约束,松弛变量。这里我们希望原始参考点的偏移量不应该太大,因此偏移量构成了第一个约束,如图中红色框所示把散点偏移量(X方向Y方向)给限制在一定的范围内。
在这里插入图片描述
位置约束
在这里插入图片描述
曲率约束
除此之外,我们在路径规划的时候通常需要考虑转弯半径的约束,也就是路径需要满足一定的曲率约束。关于曲率的计算我们的主要思想在这里采取是三点构成一个圆的思想去求解半径,具体原理如下图所示:
在这里插入图片描述

如图所示,假设 P 0 P_0 P0 P 1 P_1 P1 P 2 P_2 P2三点处在同一个圆上,当 θ \theta θ较小时,向量 P 1 P 0 ⃗ \vec {P_1P_0} P1P0 和向量 P 1 P 2 ⃗ \vec {P_1P_2} P1P2 的模近似等于弧长,因此有:
在这里插入图片描述
根据 O − P 0 − P 1 O-P_0-P_1 OP0P1等腰三角形几何关系有:
在这里插入图片描述
由于 ∣ P 1 P 0 ⃗ ∣ = ∣ P 1 P 2 ⃗ ∣ |\vec {P_1P_0}|=|\vec {P_1P_2}| P1P0 =P1P2 ,所以 C C C是的 P 1 P 3 P_1P_3 P1P3的中点,由此得:
在这里插入图片描述
根据 C − P 0 − P 1 C-P_0-P_1 CP0P1直角三角形几何关系有:
在这里插入图片描述
P 1 C P_1C P1C代入 P 1 P 3 ⃗ \vec {P_1P_3} P1P3 得:
在这里插入图片描述
θ 2 \theta_2 θ2代入得:
在这里插入图片描述
θ 1 \theta_1 θ1代入得:
在这里插入图片描述
我们希望路径曲率约束满足车辆运动学最小转弯半径的约束,也就是满足 P 1 P 3 ⃗ ≤ R m i n \vec {P_1P_3}≤R_{min} P1P3 Rmin即满足:
在这里插入图片描述
根据上式其中n-2个点中间点满足:
在这里插入图片描述
其中: △ s △ s s是散点之间平均长度, c u r c s t r cur_cstr curcstr是最大去率约束。
为了保证有解以及求解更快的收敛这里引入了大于0的松弛变量 s t a c k i stack_i stacki。因此:
在这里插入图片描述


2.5. 总结

目标函数
在这里插入图片描述
约束条件
在这里插入图片描述


附录_曲率约束的线性化

至此,作为一个优化问题,模型基本建立,通过观察我们可以知道目标函数为二次方程,但是很遗憾约束条件里面关于曲率约束是非线性的。此问题除了直接使用IPOPT求解之外,经过思考我们可以通过线性化方法把曲率约束转化成线性约束之后就可以把问题转化为二次优化问题使用OSQP加速求解。

假设:
在这里插入图片描述
泰勒展开:
在这里插入图片描述
线性化只保留一阶项:
在这里插入图片描述
F ( x ) F(x) F(x)原始参考点 x r e f x_{ref} xref处的值为:
在这里插入图片描述
F ( x ) F(x) F(x)原始参考点 x r e f x_{ref} xref处的导数为:
从曲率约束方程我们可以看到包含6个变量: x i − 1 x_{i-1} xi1 x i x_{i} xi x i + 1 x_{i+1} xi+1 y i − 1 y_{i-1} yi1 y i y_{i} yi y i + 1 y_{i+1} yi+1,对方程求导得:
在这里插入图片描述
其中包含6个变量: x i − 1 x_{i-1} xi1 x i x_{i} xi x i + 1 x_{i+1} xi+1 y i − 1 y_{i-1} yi1 y i y_{i} yi y i + 1 y_{i+1} yi+1
在这里插入图片描述
因此:
在这里插入图片描述
带入曲率约束方程得:
在这里插入图片描述

曲率线性化约束化简:
在这里插入图片描述

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值