1. ilqr

ILQR算法是基于nominal trajectory$(\tilde{x},\tilde{u})$来优化求解的。ILQR是求解状态变量和控制变量的增量序列$(\delta x^{\ast },\delta u^{\ast })$求解轨迹的局部最优值。

1.1 无约束轨迹优化问题形式

$$\begin{array}{rl}& x^{\ast },u^{\ast }=\underset{x,u}{\mathrm{argmin}}[\sum _0^{N-1}{L}^k(x_k,u_k)+L_F(x_N)]\\ & s.t.\{\begin{array}{rl}{x}_{k+1}& =f(x_k,u_k)\\ x_0& =x_{start}\end{array}\end{array}$$

1.2 Backward Pass

$V^k$是第$k(k\in [0,N])$的最优 cost-to-go，根据Bellman方程有：
$\{\begin{array}{rl}{V}^k(x_N)& =L_F(x_N)\\ V^k(x_k)& =\underset{u_k}{\min }[L^k(x_k,u_k)+V^{k+1}(f(x_k,u_k))]\end{array}$
根据Bellman方程的等式右侧公式，定义Perturbation如下：
$P^k(\delta x,\delta u)\triangleq L^k({\tilde{x}}_k+\delta x_k,{\tilde{u}}_k+\delta u_k)-L^k({\tilde{x}}_k,{\tilde{u}}_k)+V^{k+1}(f({\tilde{x}}_k+\delta x_k,{\tilde{u}}_k+\delta u_k))-V^{k+1}(f({\tilde{x}}_k,{\tilde{u}}_k))$
其中，$P^k(0,0)=0$，使用二阶泰勒展开：
$P^k(\delta x,\delta u)\approx \frac{1}{2}{\left[\begin{array}{c}1\\ \delta x\\ \delta u\end{array}\right]}^T\left[\begin{array}{ccc}0& (P_x^k{)}^T& (P_u^k{)}^T\\ P_x^k& P_{xx}^k& P_{xu}^k\\ P_u^k& P_{ux}^k& P_{uu}^k\end{array}\right]\left[\begin{array}{c}1\\ \delta x\\ \delta u\end{array}\right]=\frac{1}{2}(\delta x{P}_x^k+\delta u{P}_u^k+(P_x^k{)}^T\delta x+\delta x{P}_{xx}^k\delta x+\delta u{P}_{ux}^k\delta x+(P_u^k{)}^T\delta u+\delta x{P}_{xu}^k\delta u+\delta u{P}_{uu}^k\delta u)$

$\frac{\partial P}{\partial (\delta u)}=\frac{1}{2}(P_u^k+P_{ux}^k\delta x+P_u^k+P_{xu}^k\delta x+2P_{uu}^k\delta u)=P_u^k+P_{ux}^k\delta x+P_{uu}^k\delta u$
$P^k$是标准二次型，因此满足一阶条件下，$P^k$取到最小值，因此令$\frac{\partial P}{\partial (\delta u)}=P_u^k+P_{ux}^k\delta x+P_{uu}^k\delta u=0$，
可得$\delta u=-(P_{uu}^k{)}^{-1}(P_u^k+P_{ux}^k\delta x)$
其中：
$\{\begin{array}{l}\begin{array}{rl}{P}_x^k& =L_x^k+f_x^T{V}_x^{k+1}\\ P_u^k& =L_u^k+f_u^T{V}_x^{k+1}\\ P_{xx}^k& =L_{xx}^k+f_x^T{V}_{xx}^{k+1}{f}_x+V_x^{k+1}{f}_{xx}\\ P_{uu}^k& =L_{uu}^k+f_u^T{V}_{xx}^{k+1}{f}_u+V_x^{k+1}{f}_{uu}\\ P_{ux}^k& =L_{ux}^k+f_u^T{V}_{xx}^{k+1}{f}_x+V_x^{k+1}{f}_{ux}\end{array}\end{array}$
由于$V^{k+1}(x_{k+1})=V^{k+1}(f^k(x,u))$，因此链式求导$V$都是对$x$求偏导。对于ILQR来说，系统的二阶导数为$0$，即忽略$f_{xx},f_{uu},f_{ux}$。在DDP中，二阶导数不可忽略，这也是ILQR和DDP唯一的区别。
另外，在多数问题下，$L_{ux}=0$。
令${\nabla }_{\delta u_k}{P}^k=0$，可得：$\delta u_k^{\ast }=-(P_{uu}^k{)}^{-1}(P_u^k+P_{ux}^k\delta x_k^{\ast })=K_k\delta x_k+d_k$。
将$\delta u_k^{\ast }$的结果带入$P^k$可得：
$\underset{\delta u_k}{\min }{P}^k(\delta x,\delta u)=-\frac{1}{2}(P_u^k{)}^T(P_{uu}^k{)}^{-1}{P}_u^k+((P_x^k{)}^T-(P_u^k{)}^T(P_{uu}^k{)}^{-1}{P}_{ux}^k)\delta x+\frac{1}{2}\delta x_k^T((P_{xx}^k{)}^T-(P_{ux}^k{)}^T(P_{uu}^k{)}^{-1}{P}_{ux}^k)\delta x=\delta V^k$
因为：$V^k(x)+\delta V^k(x)=V^k(x+\delta x)\approx V^k(x_k)+\frac{\partial V^k}{\partial x}{|}_{x_k}(x-x_k)+\frac{1}{2}(x-x_k{)}^T\frac{{\partial }^2{V}^k}{\partial x^2}(x-x_k)$，可以得到：$\delta V^k(x_k)\approx V_x^k\delta x_k+\frac{1}{2}\delta x_k^T{V}_x^{k}x\delta x_k$，因此：
$\{\begin{array}{l}\begin{array}{rl}\Delta V^k& =-\frac{1}{2}(P_u^k{)}^T(P_{uu}^k{)}^{-1}{P}_u^k=\frac{1}{2}{d}_k^T{P}_{uu}{d}_k+d_k^T{P}_u\\ V_x^k& =(P_x^k{)}^T-(P_u^k{)}^T(P_{uu}^k{)}^{-1}{P}_{ux}^k=P_x+K_k^T{P}_{uu}{d}_k+K_k^T{P}_u+P_{ux}^T{d}_k\\ V_{xx}^k& =(P_{xx}^k{)}^T-(P_{ux}^k{)}^T(P_{uu}^k{)}^{-1}{P}_{ux}^k=P_{xx}+K_k^T{P}_{uu}{K}_k+K_k^T{P}_{ux}+P_{ux}^T{K}_k\end{array}\end{array}$
$V_x^k,V_{xx}^k$用在$P^{k-1}$中。
在Backward Pass中计算得到不是$\delta u^{\ast }$，而是反馈增益和开环增益序列。

1.3 Forward Pass

$\{\begin{array}{l}\begin{array}{rl}{x}_0& =x_{start}\\ u_k& ={\tilde{u}}_k+d_k+K_k(x_k-{\tilde{x}}_k)\\ x_{k+1}& =f(x_k,u_k)\end{array}\end{array}$
其中，$x_k,{\tilde{x}}_k$分别是两次迭代计算得到的轨迹。
Line Search
论文[13]提出，和所有的非线性优化问题一样，在梯度下降方向进行线性搜索来保证cost有足够的下降。使用backtracking line search计算参数$\alpha$应用在反馈中。
$z=\frac{(J(X,U)-J(\tilde{X},\tilde{U}))}{-\Delta V(\alpha )}$
其中，$\Delta V(\alpha )={\sum }_{k=0}^{N-1}\frac{1}{2}{\alpha }^2{d}_k^T{P}_{uu}{d}_k+\alpha d_k^T{P}_u$。（类似Armijo Conditon，保证每一步迭代充分下降。）
如果$z\in [{\beta }_1,{\beta }_2]$，一般为$[1e-4,10]$范围内，则认为此次迭代求解结果可以接受。否则，需要更新$\alpha =\gamma \alpha ,0<\gamma <1$，一般$\gamma =0.5$，重新进行Forward pass。

1.4 Regularization

论文[13]同时提出，当Line search超出最大迭代次数，或者cost超出最大阈值，应该放弃Forward pass，而增大backward pass中的正则项$\rho$。
$\delta u_k^{\ast }=-(P_{uu}^k+\rho I{)}^{-1}(P_u^k+P_{ux}^k\delta x_k^{\ast })=K_k\delta x_k+d_k$
增大$\rho$使$K_k$更加相似单位矩阵，使梯度下降方向更加相似Newton方向。
当$P_{uu}^k$奇异矩阵时，也可以增大正则项$\rho$，在这种情况下，Backward pass需要从头开始计算，当Backward pass成功后减小$\rho$，一般$\rho \in [1.5,2.0]$。

2. cilqr

CILQR是将约束转换为cost的方式，然后进行ILQR计算。这里约束形式表示为：
$\{\begin{array}{l}\begin{array}{rl}{g}^k(x_k,u_k)& <0\\ g^N(x_N)& <0\end{array}\end{array}$

2.1 Barrier function

Barrier function是一种被广泛采用的处理方式。将barrier function加到cost function中。

2.1.1 Exponential function

论文[1]采用了指数形式的barrier function处理约束。
$b^k=q_1{e}^{q_2{g}^k(x_k,u_k)}$
其中，$q_1,q_2$是控制barrier function的参数，用来近似indicator function。

2.1.2 Logarithmic function

指数形式的barrier function有几个问题：
● 不能保证 hard constraint；
● $q_1,q_2$难以调节，太小会违反约束，太大会使梯度和Hessian在边界处剧烈变化，造成ill-conditions。
论文[2]提出了对数形式的barrier function：
$b(g(x,u))=-\frac{1}{t}log(-g(x,u))$
其中，$t>0$是调节参数。增大$t$可以逼近indicator function。随着迭代进行，$t$逐渐增大。

2.1.3 ill-conditions

Penalty method的惩罚权重$\sigma$越来越大，或者Barrier method的惩罚权重$\sigma$越来越小时，无约束最优解越来越逼近有约束最优解。但是当无约束最优越来越逼近有约束最优解时，无约束优化问题的函数越来越病态。Penalty Method的曲率（条件数）从约束外部趋近无穷大，Barrier Method的曲率（条件数）从约束内部趋近无穷大。Hessian的最大奇异值变成无界的了。那么，使用梯度的优化算法会面临收敛越来越慢的情况。使用Sequential策略可以缓解这个问题，但是不能永远解决这个问题。
但是，在工程上，约束条件是有物理意义的，精度要求不需要异常的高，Penalty method和Barrier method可以很快的收敛。

2.1.4 Relaxation

但是，上述方法要求初始轨迹必须满足所有的不等式约束。论文[3]修改了barrier function，可以降低对初始解的要求：
$b_{relax}(g)=\{\begin{array}{ll}-\frac{1}{t}log(-g)& (g<-ϵ)\\ \frac{k-1}{tk}[(\frac{-g-kϵ}{(k-1)ϵ}{)}^k-1]-\frac{1}{t}log(ϵ)& (g\ge -ϵ)\end{array}$
论文中描述，$k=2$保证$C^2$连续，$ϵ,t$是大于$0$的任意数，选择$t=5$，$ϵ$和约束的类型有关。
论文[7]提出了另一种松弛的对数形式的barrier function。写法不同，其实是一样的。都是用的06年的一篇论文。

2.2 Augmented Lagrangian

使用Barrier function的方法主要有以下几个问题：

1. 对约束的惩罚函数有很大的权重，cost function在约束边界处急剧下降，会有weak condition的情况，甚至会产生奇异矩阵；
2. Barrier function的方法，是在约束范围内迭代优化，需要轨迹的初始解（nominal trajectory）在约束范围内。现在的一些方法通过修改barrier function可以处理初始解轨迹不满足约束的情况；
3. Barrier function无法处理等式约束。虽然，一般的行车轨迹规划没有等式约束，但是个别情况下，仍会出现等式约束；
   论文[13]详细描述了AL求解ILQR/DDP的过程，论文[6]使用AL-ilqr方法进行轨迹规划。
   也是一种Relaxation方法。

2.2.1 Augmented Lagrangian process

$J=f(x)+{\lambda }^{T}c(x)+\frac{1}{2}c(x{)}^T{I}_{\mu }c(x)$
其中，$\lambda$是拉格朗日乘子，$\mu$是惩罚系数，并且有：
$I_{\mu }=\{\begin{array}{ll}0& \text{if }{c}_i(x)<0\wedge {\lambda }_i=0,i\in \mathcal{I}\\ {\mu }_i& \text{otherwise }\end{array}$
求解过程为：

1. 保持$\lambda$和$\mu$为常量，求解$\underset{x}{\min }J(x,\lambda ,\mu )$；
2. 更新Lagrange multipliers：
   ${\lambda }_i^{+}=\{\begin{array}{ll}{\lambda }_i+{\mu }_i{c}_i(x^{\ast })& i\in \mathcal{E}\\ \max (0,{\lambda }_i+{\mu }_i{c}_i(x^{\ast }))& i\in \mathcal{I}\end{array}$
3. 更新惩罚系数：${\mu }^{+}=\varphi \mu ,\varphi >1$；
4. 检查约束是否满足；
5. 判断是否收敛；
   其中，$\mathcal{E}$和$\mathcal{I}$分别是等式约束和不等式约束，一般$2\le \varphi \le 10$。

2.2.2 AL-ilqr

2.2.2.1 Backward Pass

优化目标函数为：
$J=L_N(x_N)+({\lambda }_N+\frac{1}{2}{c}_N(x_N)I_{\mu ,N})c_N(x_N)+{\sum }_{k=0}^{N-1}[L_k(x_k,u_k)+({\lambda }_k+\frac{1}{2}{c}_k(x_k,u_k)I_{\mu ,k})c_k(x_k,u_k)]$
同样的，根据Bellman方程有：
$\{\begin{array}{rl}{V}^k(x_N){|}_{\lambda ,\mu }& =L_F(x_N)\\ V^k(x_k){|}_{\lambda ,\mu }& =\underset{u_k}{\min }[L^k(x_k,u_k)+V^{k+1}(f(x_k,u_k)){|}_{\lambda ,\mu }]\end{array}$
同样的，使用泰勒二阶展开近似$V^k(x)$的Perturbation，有：
$P^k(\delta x,\delta u)\approx \frac{1}{2}{\left[\begin{array}{c}1\\ \delta x\\ \delta u\end{array}\right]}^T\left[\begin{array}{ccc}0& (P_x^k{)}^T& (P_u^k{)}^T\\ P_x^k& P_{xx}^k& P_{xu}^k\\ P_u^k& P_{ux}^k& P_{uu}^k\end{array}\right]\left[\begin{array}{c}1\\ \delta x\\ \delta u\end{array}\right]$
其中：
$\{\begin{array}{l}\begin{array}{rl}{P}_x^k& =L_x^k+f_x^T{V}_x^{k+1}+c_x^T(\lambda +I_{\mu }c)\\ P_u^k& =L_u^k+f_u^T{V}_x^{k+1}+c_u^T(\lambda +I_{\mu }c)\\ P_{xx}^k& =L_{xx}^k+f_x^T{V}_{xx}^{k+1}{f}_x+V_x^{k+1}{f}_{xx}+c_x^T{I}_{\mu }{c}_x\\ P_{uu}^k& =L_{uu}^k+f_u^T{V}_{xx}^{k+1}{f}_u+V_x^{k+1}{f}_{uu}+c_u^T{I}_{\mu }{c}_u\\ P_{ux}^k& =L_{xx}^k+f_x^T{V}_{xx}^{k+1}{f}_x+V_x^{k+1}{f}_{ux}+c_u^T{I}_{\mu }{c}_x\end{array}\end{array}$
可以看出，AL-ilqr和常规的ilqr是相似的，只是在backward pass公式中明确的写出了$c(x)$相关的参数，常规ilqr是隐式表达了。
同样的，可以求出最优控制增量：$\delta u_k^{\ast }=-(P_{uu}^k+\rho I{)}^{-1}(P_u^k+P_{ux}^k\delta x_k^{\ast })=K_k\delta x_k+d_k$
其中：
$\{\begin{array}{l}\begin{array}{rl}\Delta V^k& =\frac{1}{2}{d}_k^T{P}_{uu}{d}_k+d_k^T{P}_u\\ V_x^k& =P_x+K_k^T{P}_{uu}{d}_k+K_k^T{P}_u+P_{ux}^T{d}_k\\ V_{xx}^k& =P_{xx}+K_k^T{P}_{uu}{K}_k+K_k^T{P}_{us}+P_{ux}^T{K}_k\\ V_x^N& =L_x^N+(c_N{)}_x^T(\lambda +I_{\mu ,N}{c}_N)\\ V_{xx}^N& =L_{xx}^N+(c_N{)}_x^T{I}_{\mu ,N}(c_N{)}_x\end{array}\end{array}$

2.2.2.2 Forward Pass

和前面所述一致。

2.2.2.3 Augmented Lagrangian update

${\lambda }_i^{+}=\{\begin{array}{ll}{\lambda }_i+{\mu }_i{c}_i(x^{\ast })& i\in \mathcal{E}\\ \max (0,{\lambda }_i+{\mu }_i{c}_i(x^{\ast }))& i\in \mathcal{I}\end{array}$
${\mu }^{+}=\varphi \mu$
论文[13]指出，通过大量的不同的启发式方法更新拉格朗日乘子和惩罚系数对比，在外部循环中更新的效果最好。在一些问题中，只更新拉格朗日乘子，或者更新active constraints对应的惩罚系数，会得到更好的性能，但是基本的方法对大范围的问题可以得到最好的性能。

3. cost function

CILQR的目标函数是ilqr问题的优化函数和不等式约束转化的cost部分的和。
$$\begin{array}{rl}& x^{\ast },u^{\ast }=\underset{x,u}{\mathrm{argmin}}[\sum _0^{N-1}(L^k(x_k,u_k)+b(g^k(x_k,u_k)))+L_F(x_N)+b(g^N(x_N))]\\ & s.t.\{\begin{array}{rl}{x}_{k+1}& =f(x_k,u_k)\\ x_0& =x_{start}\end{array}\end{array}$$

3.1 control cost

控制量的cost，使轨迹更加平滑和平顺。

3.2 tracking cost

对全局求解的粗轨迹的cost。

3.3 adjusting cost

论文[11]提出对上一次迭代的轨迹做cost，避免相邻两次迭代得到轨迹差距太大。

4. constraints

这里的约束是指不等式约束，包括状态变量的约束、控制变量的约束和避障约束。

4.1 状态约束

$x_{low}\le x\le x_{high}$

4.2 控制约束

$u_{low}\le u\le u_{high}$

4.3 避障约束

对于自动驾驶轨迹规划问题，避障约束是主要的问题，也有多种处理形式。

4.3.1 Mass point

论文[4,5]将主车和障碍物车辆当作质点，是两个点之间的欧式距离大于安全阈值。
$g^k(x_k,u_k)=d_{safe}-||p_k-p_o||\le 0$
其中，$p_k,p_o$分别是主车和障碍物的位置。

4.3.2 Circles Representation

论文[6,7]采用等效圆的方式近似主车和障碍物车辆的形状。不同的是论文[6]采用的两个等效圆近似，论文[7]的研究对象是小型移动机器人，因此采用了一个等效圆。
$\{\begin{array}{l}{d}^i-d_u^i\ge W_{ego}/2\\ d_l^i-d^i\ge W_{ego}/2\\ d_{obs,j,k}^i\ge r_{obs}+r_{ego}\end{array}$
其中，$d_u,d_l$是道路的上下边界，$d^i$是主车到道路边界的距离，但是论文中没有解释怎么计算$d^i$。$x_{ego,f}^i$的位置和$x_{ego,r}^i$满足几何约束。

4.3.3 Circles and Ellipse Representation

论文[1,8]将主车近似为两个等半径$r$的圆形。将障碍物无车辆近似为一个椭圆，并将椭圆按照主车等效圆半径拓展，长轴是其速度方向，$a=l+v\times t_{safe}+s_{safe}+r$，短轴$b=w+s_{safe}+r$。
$g=1-(x_k-x_o{)}^{T}RP{R}^T(x_k-x_o)<0$
其中，$P=diag(\frac{1}{a},\frac{1}{b}),R=\left[\begin{array}{cc}\cos {\theta }_o& -\sin {\theta }_o\\ \sin {\theta }_o& \cos {\theta }_o\end{array}\right]$。

这么做优点：
● 在结构化道路场景中，障碍物的圆形近似并不能表达障碍物车辆的纵向运动；
● 椭圆是凸的，平滑的；
● 椭圆符合高斯分布；

4.3.4 Rectangle and Ellipse Representation

论文[9]提出将主车处理为rectangle，将障碍物处理为椭圆，公式描述同4.3.3，并没有介绍主车rectangle的处理方式。
推测为：

1. 障碍物的椭圆不需要以车辆等效圆半径膨胀；
2. 车辆Rectangle轮廓上的采样点的坐标$p_c$和后轴非线性关系可以得到；
3. 约束$p_c$在障碍物的椭圆外面；

4.3.5 Rectangle and Polygon Representation

论文[2,10]提出将主车处理为rectangle，将障碍物车辆处理为Polygon，显然Ploygon比圆形或者椭圆的精度更高。两个凸多边形的距离计算参考论文[11]。
论文[11]中没有描述主车的处理方式，推测和4.3.4中处理方式一样。

论文[11]提出了CFS算法，是一种迭代算法，根据参考轨迹$x^k$计算凸的走廊优化求解得到$x^{k+1}$，然后根据$x^{k+1}$计算凸走廊求解直至收敛。

4.3.5.1 障碍物表达式

O j = { x ∈ R 2 : φ j ( x ) < 0 } , ∂ O j = { x ∈ R 2 : φ j ( x ) = 0 } {\mathcal{O}}_j = \{ x \in \R^2: \varphi_j(x) < 0 \}, \partial {\mathcal{O}}_j = \{ x \in \R^2: \varphi_j(x) =0 \} Oj​={x∈R2:φj​(x)<0},∂Oj​={x∈R2:φj​(x)=0}
其中，${\mathcal{O}}_j$障碍物$j$的内部空间，$\partial {\mathcal{O}}_j$表示障碍物$j$的边界。将障碍物分为三种形式，并给出了各自的表达式。

4.3.5.1.1 Convex obstacle

${\phi }_j:=\{\begin{array}{ll}{\min }_{y\in \partial {\mathcal{O}}_j}||x-y||& x\notin {\mathcal{O}}_j\\ -{\min }_{y\in \partial {\mathcal{O}}_j}||x-y||& x\in {\mathcal{O}}_j\end{array}$

4.3.5.1.2 Boundary obstacle

${\phi }_j:=f(p_1)-p_2$

4.3.5.1.3 Non-convex obstacle

${\phi }_j:=\{\begin{array}{ll}{\min }_{y\in \partial {\hat{\mathcal{O}}}_j}[||x-y||+||y-\delta (y)||]& x\notin {\hat{\mathcal{O}}}_j\\ -{\min }_{y\in \partial {\hat{\mathcal{O}}}_j}[||x-y||-||y-\delta (y)||]& x\in {\hat{\mathcal{O}}}_j\end{array}$
其中，${\hat{\mathcal{O}}}_j$是最小凸包络。

4.3.5.2 避障约束

x ∈ Γ = ⋂ j , q { x ∈ R 2 h : φ j ( l q ( x ) ) ≥ 0 } = x ∈ ⋂ j = 1 N Γ j = x ∈ ⋂ j = 1 N F j x \in \varGamma = \mathop{\bigcap}\limits_{j,q} \{ x \in \R^{2h} : \varphi_j(l_q(x)) \ge 0 \} = x \in \mathop{\bigcap}\limits_{j = 1}^{N} {\varGamma}_j = x \in \mathop{\bigcap}\limits_{j = 1}^{N} {\mathcal{F}}_j x∈Γ=j,q⋂​{x∈R2h:φj​(lq​(x))≥0}=x∈j=1⋂N​Γj​=x∈j=1⋂N​Fj​
其中，$h$是规划时域，即需要规划的轨迹点的数目。$l_q(x):{\mathbb{R}}^{2h}\to {\mathbb{R}}^2$，即选择规划轨迹上的某个点。

4.3.5.2.1 Convex obstacle

F j ( x r ) : = { x : φ j ( x ) + ∇ ^ φ j ( x r ) ( x − x r ) ≥ 0 } {\mathcal{F}}_j (x^r) := \{ x : \varphi_j(x) + \hat{\nabla} \varphi_j(x^r) (x - x^r) \ge 0 \} Fj​(xr):={x:φj​(x)+∇^φj​(xr)(x−xr)≥0}
其中，$\hat{\nabla }{\phi }_j(x^r)$是${\phi }_j$的梯度方向。

4.3.5.2.2 Non-convex obstalce

F j ( x r ) : = { x : φ j ( x ) + ∇ ^ φ j ( x r ) ( x − x r ) ≥ 1 2 ( x − x r ) R H j ∗ ( x − x r ) } {\mathcal{F}}_j (x^r) := \{ x : \varphi_j(x) + \hat{\nabla} \varphi_j(x^r) (x - x^r) \ge \frac{1}{2} (x - x^r)^R H^*_j (x - x^r) \} Fj​(xr):={x:φj​(x)+∇^φj​(xr)(x−xr)≥21​(x−xr)RHj∗​(x−xr)}
其中，$H_j^{\ast }$是${\phi }_j$的Hessian下边界。

4.3.5.3 Examples

4.3.5.3.1 Convex obstacle

多边形障碍物的四个顶点分别为$a=(1,1),b=(-1,1),c=(-1,-1),d=(1,-1)$，有：
φ ( x ) = { max ⁡ { − 1 − p 1 , p 1 − 1 , p 2 − 1 , − 1 − p 2 } ∣ ∣ x ∣ ∣ ∞ ≤ 1 min ⁡ { ∣ ∣ x − a ∣ ∣ , ∣ ∣ x − b ∣ ∣ , ∣ ∣ x − c ∣ ∣ , ∣ ∣ x − d ∣ ∣ } ∣ p 1 ∣ > 1 , ∣ p 2 ∣ > 1 min ⁡ { ∣ p 1 − 1 ∣ , ∣ p 1 + 1 ∣ } ∣ p 1 ∣ > 1 , ∣ p 2 ∣ < 1 min ⁡ { ∣ p 2 − 1 ∣ , ∣ p 2 + 1 ∣ } ∣ p 1 ∣ < 1 , ∣ p 2 ∣ > 1 \varphi(x) = \begin{cases} \max \{ -1 - p_1, p_1 - 1, p_2 - 1, -1 -p_2 \} & ||x||_{\infin} \le 1 \\ \min \{ || x - a||, || x - b||, || x - c||, || x - d|| \} & |p_1| > 1, |p_2| > 1 \\ \min \{ |p_1 - 1|, |p_1 + 1| \} & |p_1| > 1, |p_2| < 1 \\ \min \{ |p_2 - 1|, |p_2 + 1| \} & |p_1| < 1, |p_2| > 1 \\ \end{cases} φ(x)=⎩ ⎨ ⎧​max{−1−p1​,p1​−1,p2​−1,−1−p2​}min{∣∣x−a∣∣,∣∣x−b∣∣,∣∣x−c∣∣,∣∣x−d∣∣}min{∣p1​−1∣,∣p1​+1∣}min{∣p2​−1∣,∣p2​+1∣}​∣∣x∣∣∞​≤1∣p1​∣>1,∣p2​∣>1∣p1​∣>1,∣p2​∣<1∣p1​∣<1,∣p2​∣>1​
参考点$x^r=(-1.5,-1.5)$，并且$\phi (x^r)=\frac{\sqrt{2}}{2}$和$\nabla \phi (x^r)=\frac{\sqrt{2}}{2}[-1,-1]$，因此有：
F ( x r ) : = { x : φ j ( x ) + ∇ ^ φ j ( x r ) ( x − x r ) ≥ 0 } = { x : 2 2 ( − p 1 − p 2 + 2 ) ≥ 0 } \mathcal{F} (x^r) := \{ x : \varphi_j(x) + \hat{\nabla} \varphi_j(x^r) (x - x^r) \ge 0 \} = \{ x : \frac{\sqrt{2}}{2} (-p_1 - p_2 + 2) \ge 0 \} F(xr):={x:φj​(x)+∇^φj​(xr)(x−xr)≥0}={x:22 ​​(−p1​−p2​+2)≥0}
则，在$x^r$处的凸的可行域如蓝色区域。

4.3.5.3.2 Non-convex obstalce

非凸障碍物的下边界为$p_2=-(p_1{)}^2$。
φ ( x ) = { max ⁡ { − ( p 1 ) 2 − p 2 , p 1 − 1 , p 2 − 1 , − 1 − p 2 } ∣ ∣ x ∣ ∣ ∞ ≤ 1 , p 2 ≥ − ( p 1 ) 2 min ⁡ { ∣ ∣ x − a ∣ ∣ , ∣ ∣ x − b ∣ ∣ , ∣ ∣ x − c ∣ ∣ , ∣ ∣ x − d ∣ ∣ } ∣ p 1 ∣ > 1 , ∣ p 2 ∣ > 1 min ⁡ { ∣ p 1 − 1 ∣ , ∣ p 1 + 1 ∣ } ∣ p 1 ∣ > 1 , ∣ p 2 ∣ < 1 p 2 − 1 p 2 > 1 − ( p 1 ) 2 − p 2 p 2 < − ( p 1 ) 2 \varphi(x) = \begin{cases} \max \{ -(p_1)^2 - p_2, p_1 - 1, p_2 - 1, -1 -p_2 \} & ||x||_{\infin} \le 1, p_2 \ge -(p_1)^2 \\ \min \{ || x - a||, || x - b||, || x - c||, || x - d|| \} & |p_1| > 1, |p_2| > 1 \\ \min \{ |p_1 - 1|, |p_1 + 1| \} & |p_1| > 1, |p_2| < 1 \\ p_2 - 1 & p_2 > 1 \\ -(p_1)^2 - p_2 & p_2 < -(p_1)^2 \\ \end{cases} φ(x)=⎩ ⎨ ⎧​max{−(p1​)2−p2​,p1​−1,p2​−1,−1−p2​}min{∣∣x−a∣∣,∣∣x−b∣∣,∣∣x−c∣∣,∣∣x−d∣∣}min{∣p1​−1∣,∣p1​+1∣}p2​−1−(p1​)2−p2​​∣∣x∣∣∞​≤1,p2​≥−(p1​)2∣p1​∣>1,∣p2​∣>1∣p1​∣>1,∣p2​∣<1p2​>1p2​<−(p1​)2​
参考点$x^r=(0,-1)$，并且$\phi (x^r)=1$和$\nabla \phi (x^r)=[0,-1],-H^{\ast }=[-2,0;0,0]$，因此有：
F ( x r ) : = { x : φ j ( x ) + ∇ ^ φ j ( x r ) ( x − x r ) ≥ 1 2 ( x − x r ) R H j ∗ ( x − x r ) } = { x : − p 2 − ( p 1 ) 2 ≥ 0 } \mathcal{F} (x^r) := \{ x : \varphi_j(x) + \hat{\nabla} \varphi_j(x^r) (x - x^r) \ge \frac{1}{2} (x - x^r)^R H^*_j (x - x^r) \} = \{ x : -p_2 - (p_1)^2 \ge 0 \} F(xr):={x:φj​(x)+∇^φj​(xr)(x−xr)≥21​(x−xr)RHj∗​(x−xr)}={x:−p2​−(p1​)2≥0}

4.3.5.4 凸走廊的可行性

当存在的障碍物都是凸的，并且没有相交时，在$x^r$处必然可以找到凸走廊。反之，则不然。

4.3.6 Minkowski Sum of Polygons

论文[12]基于Minkowski sum的理论（GJK）来处理碰撞约束：

1. 将ego的box沿着obstacle的box移动，得到Minkowski sum的多边形$p_{MS}$；
   

2. 找到$p_{MS}$距离主车最近的顶点$O$；
   

3. 在顶点$O$分为五个区域，计算每个区域到ego的距离；
   文章没有对计算细节有太多的描述。

4.4 车道线约束

对于结构化道路场景下的自动驾驶轨迹规划问题，车道线约束也是一个重要的约束条件。一般在大曲率道路下，左右车道线形成的行驶区域是非凸的。

4.4.1 polylines

论文[4]分别将左右车道线离散为$20$段长度为$5m$的直线段，将车辆约束在左右对应的直线段内。论文[3]也是类似的处理。
论文[15]同样将车道线近似为 polylines (多条线段)。

4.4.2 polynomial function

论文[11]将左右车道线分别拟合为一条多项式，将轨迹约束在两个多项式之间。

但是，在大曲率，例如环岛，无法使用一条多项式曲线表达车道线。

5. Reference

1. Constrained iterative LQR for on-road autonomous driving motion planning
2. Autonomous Driving Motion Planning With Constrained Iterative LQR
3. Integrating Higher-Order Dynamics and Roadway-Compliance into Constrained ILQR-based Trajectory Planning for Autonomous Vehicles
4. Iterative LQR with Discrete Barrier States for Efficient Collision Avoidance of Autonomous Vehicle
5. Decentralized iLQR for Cooperative Trajectory Planning of Connected Autonomous Vehicles via Dual Consensus ADMM
6. Model Predictive Control of Autonomous Vehicles With Integrated Barriers Using Occupancy Grid Maps
7. Adaptive High-Order Control Barrier Function-Based Iterative LQR for Real Time Safety-Critical Motion Planning
8. Safe Planning for Self-Driving Via Adaptive Constrained ILQR
9. Alternating Direction Method of Multipliers for Constrained Iterative LQR in Autonomous Driving
10. Constrained iterative lqg for real-time chance-constrained gaussian belief space planning
11. The convex feasible set algo rithm for real time optimization in motion planning
12. Real Time Motion Planning Using Constrained Iterative Linear Quadratic Regulator for On-Road Self-Driving
13. AL-iLQR Tutorial
14. Trajectory Planning for BERTHA - a Local, Continuous Method
15. Improved Trajectory Planning for On-Road Self-Driving Vehicles Via Combined Graph Search, Optimization & Topology Analysis