轨迹规划-Constrained ILQR

YoungTimes

发布于 2023-11-13 14:24:29

1.8K0

发布于 2023-11-13 14:24:29

文章被收录于专栏：半杯茶的小酒杯半杯茶的小酒杯

1. General Planning Problem

典型的Motion Planning问题定义如下：

其中：

x_k

是State，

u_k

是Control Input；

k表示Time Step，N是Preview Horizon，J是Cost Function；

c_N

是Final Stage Cost，

c_k

是k时刻的Cost；

式1a)是非线性的车辆动力学方程；

式1b)是Initial State；

1c)和1d)是State和Control的非线性(Non-linear)和非凸(Non-Convex)约束。

SQP算法

SQP是一种解决非线性(Nonlinear)和非凸(Non-Convex)优化问题的通用方法，但是计算效率不高。

ILQR算法

标准的ILQR定义如下：

ILQR可以高效的解决非线性系统(Nonlinear System)的优化控制问题(Predictive Optimal Control Problem)，但它的缺点是不能处理复杂的约束，比如碰撞检测约束、加速度约束、交通规则约束、限速约束等等。

CILQR算法

用标准的ILQR方法解决General Motion Planning问题，需要解决两个问题：

General Motion Planning的State和Control是带约束的；
General Motion Planning的Cost是非二次型的；

这就是CILQR要解决的问题。

2. Constrained Iterative LQR

CILQR将Motion Planning的Constraints和Cost转换成二次型的Cost形式，然后对其应用ILQR算法。

Cost Function Quadratization

如果Cost Function

c_k^x

不是Quadratic的形式，那么通过在

x_k

处进行二阶泰勒展开将其转换为二次型形式。

Constraint Function Linearization

如果Constraint Function

f_k^x \leq 0

是非线性的，那么通过在

x_k

处进行一阶泰勒展开将其转换为线性形式。

Barrier Function Shaping

linearized Constraint Function转换为Barrier Function，将带约束的问题转换为非约束问题。并且指数形式的Barrier Function二阶可微，导数也很容易求解。

Constraint Function Quadratization

最后将Shaped Constraint Function转化为二次型的形式。

Barrier Function的一阶偏导：

Barrier Function的二阶偏导：

3、CILQR的应用

这里将具体的自动驾驶的运动规划问题定义如下，它是General Motion Planning问题的子问题。

其中：

式13a)是非线性的系统动力学方程(Dynamic Equation)；

式13c)是碰撞避免(Collision Avoidance)约束，其中C是Whole Space，

O_j(k)

是障碍物j是k时刻的占用空间。Collision Avoidance Constraints通常是Non-convex Inequality Constraint。

3.1 Vehicle Model

车辆的运动状态

x = [p^x, p^y, v, \theta]

，其中，

p^x

和

p^y

是Vehicle Position，v是Vehicle Velocity，

\theta

是Yaw Angle。

车辆动力学方程：

其中：

\dot{\theta} = \frac{v}{L} tan{\delta}

\delta

是前轮方向角。

动力学方程的离散表达

假设

t \in [T_s(k), T_s(k+1)]

，

时刻的车辆状态为

x(T_s(k)) = x_k = [p_k^x, p_k^y, v_k, \theta_k]^T

，k时刻的控制输入为

u_k = [\dot{v_k}, \dot{\theta_k}]

。

可以最终推导出k+1时刻的车辆状态：

3.2 Objective Function

目标函数如下：

Acceleration

c_k^{acc}

用于惩罚较大的加速度，保障乘车体验。

Yaw Rate

c_k^{yawr}

的Cost项用于惩罚Vehicle Direction的快速改变和横向加速度变化导致安全风险和驾驶体验。

Offset to Reference

c_k^{off}

的Cost惩罚主车偏移参考线的行为。

假设参考线的点序列：

x_r = [x_{r}^0, x_{r}^1, ..., x_{r}^N]

，常用的做法是直接计算对应点的距离。

c_k^{off} = (x_k - x_k^r)^T Q_k^r (x_k - x_k^r)

这种方法有一些缺陷。比如在下图的场景中，主车在一条弯曲的道路上，需要停止线A前停下来，行驶参考线

x_i^r

是道路的中心线。

在这种场景下，由于采用对应参考点的距离作为cost，参考点

x_{i+2}^r

、

x_{i+1}^r

会让优化轨迹偏离道路中心线，与预期的效果背离。

理想的Cost是惩罚主车偏移参考轨迹线的行为，而不是偏离对应参考点的行为。

参考中心线的一个可选项是道路中心线，但道路中心线不是连续可微，因此有不收敛的风险。

最终这里选择三次多项式S作为参考轨迹，规划轨迹到三次多项式的距离平方作为

c_k^{off}

。

Velocity Difference

c_k^{vel}

的Cost让主车保持相对高的通行速度，从而确定比较好的通行效率。

c_k^{vel} = w_{vel}(||v_{k||}|| - v_{des})^2

这些Cost函数都是二次函数，不需要额外处理。

3.3 Constraints

Acceleration Constraint

a_{high} > 0

是车辆引擎能够提供的最大加速度，

a_{low} < 0

是车辆刹车能够提供的最大减速度。

Yaw Rate Constraint

Yaw Rate与方向盘的转角(Steering Wheel Limit)限制有关。

其中，

\overline{s}

是Largest Yaw Rate，可以通过将largest steering wheel angle代入

\dot{\theta} = \frac{v}{l} tan{\delta}

计算得到。

Boundary Constraint

Boundary Constraint的场景很多。路口停车场景下，十字路口的停止线是一种Boundary Constrain。

在Car Following场景下，Front Vehicle可以看做一个移动的Boundary Constraint。

上面几个约束都是线性的，需要采用第2节CILQR的Barrier Function Shaping和Constraint Function Quadratization将其转化为二次形的形式。

Obstacle Avoidance Constraint

如下图所示，将道路上的障碍物按照椭圆建模，椭圆的长轴

a = l +vt_{safe} + s_{safe}

，短轴

b = w + s_{safe}

，其中，l是Vehicle Length，w是Vehicle Width，v是Vehicle Velocity，

t_{safe}

是车辆前方的安全距离，

s_{safe}

是safety margin。

确定长短轴之后，就可以确定障碍物的椭圆方程：

(x - x_o)^T P (x - x_o) = 1

在椭圆范围之外的区域就可以认为是安全的，因此该障碍物的Safe Constraint的定义就是：

(x - x_o)^T P (x - x_o) > 1

假设主车的位置是

x_k

，与障碍物的Safety Margin是r。主车相对于该障碍物的Safety Boundary如下图的红线所示。

红线区域不方便用解析几何表达，因此用一个长轴为a+r，短轴为b+r的椭圆来近似表达红线区域。

(x - x_o)^T P^{\prime} (x - x_o) = 1

最终得到主车与障碍物的碰撞检测约束：

f_k^x(x_k) = 1- (x_k - x_0)^T P^{\prime} (x_k - x_0) < 0

这个约束是非线性的，需要按照第2节CILQR的Constraint Function Linearization、Barrier Function Shaping和Constraint Function Quadratization将其转化为二次形的形式。

最后，不能简单把主车抽象为一个点，可以使用两个Circle来表达主车，两个Circle的圆心分别是主车前轴和后轴的中心。如下图所示。

假设车辆后轴的中心坐标为

p_{rear}=(p_x, p_y)

，那么前轴的中心坐标为

p_{front} = (p_x + \Delta cos \theta, p_y + \Delta sin \theta)

。

将这两个坐标带入椭圆方程，就可以判断主车是否会与障碍物发生碰撞。

通过CILQR的处理，将目标函数和约束条件都转化为符合ILQR的形式，最优利用ILQR算法得到最终的控制序列

[u_0, u_1, ..., u_{N-1}]

和轨迹序列

[x_0,x_1, ..., x_N]

效果演示

带约束的ILQR的在仿真中的一些测试场景。

静态障碍物避障场景

在变道跟车场景中，主车先加速变道，然后减速跟车。

变道跟车场景

在混合场景中，主车先加速超过前方慢速行驶的车辆，然后减速停车，等待对向车辆汇车。

混合场景

参考资料

1、Jianyu Chen, Wei Zhan, and Masayoshi Tomizuka. Constrained iterative lqr for on-road autonomous driving motion planning. In 2017 IEEE 20th International Conference on Intelligent Transportation Systems (ITSC), pages 1–7. IEEE, 2017.

2、https://github.com/pparmesh/Constrained_ILQR/tree/master

本文参与腾讯云自媒体分享计划，分享自微信公众号。

原始发表：2023-11-11，如有侵权请联系 cloudcommunity@tencent.com 删除

安全

本文分享自半杯茶的小酒杯微信公众号，前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体分享计划，欢迎热爱写作的你一起参与！

登录后参与评论

0 条评论

热度