不确定场景下的规划

*该笔记基于自动驾驶之心课程整理

1.Contingency Planning

假设自车需要左转，在邻车道有个对向 onComing 的车，那么自车有两种方案：
1.加速在近处先拐过去—> 激进决策
2.减速，让对向车过去再拐过去 —> 保守解决
在做这两个决策之前有一段公共距离为红色箭头可以进行选择，这种方法叫做 Tree Planning

Contingency Planning SL

目标函数：

$$\begin{array}{rl}& \arg \underset{\tau }{\min }{J}_{\mathrm{shared}}({\tau }_{0:t_b})+\sum _{\lambda \in \Lambda }p(\lambda )J_{\mathrm{conting}}({\tau }_{t_b:T},\lambda )\\ & \text{s.t. }{g}_j(\tau )\le b_j,j=1,\dots ,n\end{array}$$

$$J_{\mathrm{seg}}(\tau_{a:b}) = w_{l} \sum_{i=a}^{b} l_{i}^2

• w_{l’} \sum_{i=a}^{b} {l’_{i}}^2
• w_{l’‘} \sum_{i=a}^{b} {l’'_{i}}^2
• w_{l’‘’} \sum_{i=a}^{b-1} {l’‘’_{i}}^2$$

前面代表公共路径中的目标函数，目标函数就是二次型。后面 $J_{\mathrm{conting}}({\tau }_{t_b:T})$ 是每个路径下的目标函数 $p(\lambda )$ 是每个路径的概率

优化变量

这个和我们的约束差不多

约束

l 相邻点之间的积分约束为：
$$\begin{array}{rl}{l}_{i+1}^{\prime \prime \prime }& =l_i^{\prime \prime }+{\int }_0^{\Delta s}{l}_{i\to i+1}^{\prime \prime \prime }ds=l_i^{\prime \prime }+l_i^{\prime \prime \prime }\ast \Delta s\\ l_{i+1}^{\prime \prime }& =l_i^{\prime }+{\int }_0^{\Delta s}{l}_i^{\prime \prime \prime }(s)ds=l_i^{\prime }+l_i^{\prime \prime \prime }\ast \Delta s+\frac{1}{2}\ast l_i^{\prime \prime \prime \prime }\ast \Delta s^2\\ l_{i+1}& =l_i+{\int }_0^{\Delta s}{l}_i^{\prime \prime }(s)ds\\ & =l_i+l_i^{\prime \prime }\ast \Delta s+\frac{1}{2}\ast l_i^{\prime \prime \prime }\ast \Delta s^2+\frac{1}{6}\ast l_i^{\prime \prime \prime \prime }\ast \Delta s^3\end{array}$$
$$x_{i+1}=A{x}_i+B{u}_i$$

$$\mathbf{A}=\left[\begin{array}{ccc}1& \Delta s& \frac{1}{2}\Delta s^2\\ 0& 1& \Delta s\\ 0& 0& 1\end{array}\right],\mathbf{B}=\left[\begin{array}{c}\frac{1}{6}\Delta s^3\\ \frac{1}{2}\Delta s^2\\ 1\end{array}\right]$$

对于每个 Branch 上的每个点的约束为
$$\begin{array}{rl}{x}_{i+1}& =A{x}_i+B{u}_i,\\ x_{i+1}^{\lambda }& =A{x}_i^{\lambda }+B{u}_i,\end{array}i=0,1,\dots ,\text{branch\_idx}-1i=\text{branch\_idx},\dots ,N,\lambda \in \Lambda$$

初始状态约束及 boundary 约束：
这里的 boundary 约束会根据不同的 scene 使用不同的 boundary

目标函数，约束汇总：

Contingency Speed Planning (ST)

2.iLQR

数学建模

代价函数

$$\begin{array}{rl}& x^{\ast },u^{\ast }=\arg \underset{x,u}{\min }\left\{\varphi (x_N)+\sum _{k=0}^{N-1}{L}^k(x_k,u_k)\right\}\\ & \text{s.t.}{x}_{k+1}=f^k(x_k,u_k),k=0,1,\dots ,N-1\\ & x_0=x_{start}\end{array}$$

对于一条轨迹来说，假设有 5 个点 $x_0-x_4$ 代表状态值，点与点之间有一个控制量 $u_0-u_3$ 也就是转向角度和加速度。iLQR 最终的目标就是优化 ${\sum }_{k=0}^{N-1}{L}^k(x_k,u_k)$ $x$ 和 $u$
对于终点 $x_N$ 不需要再对它的下一步进行控制，所以没有对应的 $u$ ，也就是 $u_N=0$ 所以 $\varphi (x_N)$ 代表终点状态代价

状态转移方程 $x_{k+1}=f^k(x_k,u_k)$ 可以得到下一个目标点的位置

c 代表每一步的目标函数，下面是目标函数的展开：
$$\underset{u_1,\dots ,u_T}{\min }c({\mathbf{x}}_1,{\mathbf{u}}_1)+c(f({\mathbf{x}}_1,{\mathbf{u}}_1),{\mathbf{u}}_2)+\cdots +c(f(f(\cdots )),{\mathbf{u}}_T)$$
$x_1$ 代表起始位置，$u$ 代表控制变量。这里的 $x_2$ 是通过 $x_2=f(x_1,u_1)$ 得到的，同理 $x_3$ 也是由 $x_1$ 推到来，所以 $f$ 是相互嵌套的
状态转移方程线性化的表达如下：

$$f(x_t,u_t)=F_t\left[\begin{array}{c}{x}_t\\ u_t\end{array}\right]+f_t$$
这是一个线性化的系统动态模型，描述状态如何从t演变到 t+1
$F_t$ :状态和控制输入的线性变换矩阵
$f_t$:线性模型的偏置项

$$c({\mathbf{x}}_t,{\mathbf{u}}_t)=\frac{1}{2}{\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]}^{\top }{\mathbf{C}}_t\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]+{\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]}^{\top }{\mathbf{c}}_t\text{quadratic}$$
将目标函数转换为泰勒二次展开式，包含二次项和线性项。在iLQR（迭代线性二次调节器）算法中，将代价函数（或系统动态）转换为二次型（或线性）的核心思想是局部近似。
$$C_T=\left[\begin{array}{c}{C}_{x_T,x_T}\\ C_{u_T,x_T}\end{array},\begin{array}{c}{C}_{x_T,u_T}\\ C_{u_T,u_T}\end{array}\right]$$
$C_t$ ：时间步t的成本函数的二次项系数矩阵（Hessian矩阵）
$c_t$：时间步t的成本函数的线性项系数向量

最优控制求解

$$Q(x_T,u_T)=\text{const}+\frac{1}{2}{\left[\begin{array}{c}{x}_T\\ u_T\end{array}\right]}^{\top }{C}_T\left[\begin{array}{c}{x}_T\\ u_T\end{array}\right]+{\left[\begin{array}{c}{x}_T\\ u_T\end{array}\right]}^{\top }{c}_T$$
类似强化学习，$x$ 看做状态，$u$ 看做控制量，将其转换为状态价值函数，也就是在 $x_T$ 时刻采取控制量 $u_T$ 下的代价加上之后所有时间步长的最优代价之和。const 任何不会影响优化过程的常数偏置。

$$V(x_T)=\underset{u_T}{\min }Q(x_T,u_T)$$
$V(x_T)$ 是在 $x_T$ 下最优策略代价函数，如果这里的 u 取的是最优控制的 u ,那么 $V(x_T)={\min }_{u_T}Q(x_T,u_T)$

求最优控制就是求哪一个 u 可以使得 Q 最小。所以将 u 看做变量 x 看做常数，对 u 求导，当 Q = 0 时求出极值点，公式如下：
$$\begin{array}{rl}& {\nabla }_{{\mathbf{u}}_T}Q({\mathbf{x}}_T,{\mathbf{u}}_T)={\mathbf{C}}_{u_T,x_T}{\mathbf{x}}_T+{\mathbf{C}}_{u_T,u_T}{\mathbf{u}}_T+{\mathbf{c}}_{u_T}=0\\ & {\mathbf{u}}_T=-{\mathbf{C}}_{u_T,u_T}^{-1}\left({\mathbf{C}}_{u_T,x_T}{\mathbf{x}}_T+{\mathbf{c}}_{u_T}\right)\end{array}$$
$${\mathbf{u}}_T={\mathbf{K}}_r{\mathbf{x}}_T+{\mathbf{k}}_r$$
$${\mathbf{c}}_T=\left[\begin{array}{c}{\mathbf{c}}_{xT}\\ {\mathbf{c}}_{uT}\end{array}\right]$$
$$\begin{array}{rl}{\mathbf{K}}_T& =-{\mathbf{C}}_{u_T,u_T}^{-1}{\mathbf{C}}_{u_T,x_T}\\ {\mathbf{k}}_T& =-{\mathbf{C}}_{u_T,u_T}^{-1}{\mathbf{c}}_{u_T}\end{array}$$
$C_T$ 为设计的代价函数系数的矩阵.
$K$ 代表反馈项的系数
$k$ 代表前馈项的系数

那么将上面 $u$ 的结果带入到 $Q$ 中就可以得到状态价值函数 V：

将 $V$ 的表达式展开得到：

将上面的展开式转换为二次型：

其中 $V$ 和 $v$ 的表达式为：

通过上面的计算可以看到我们最终需要计算 $V(x_T)$ 的值。得到了 $V_T$ 和 $u_T$ 就可以算出 $V_{T-1}$

（1）得到 $x_T$ 的值
根据状态转移方程得到下面的公式
$$f({\mathbf{x}}_{T-1},{\mathbf{u}}_{T-1})={\mathbf{x}}_T={\mathbf{F}}_{T-1}\left[\begin{array}{c}{\mathbf{x}}_{T-1}\\ {\mathbf{u}}_{T-1}\end{array}\right]+{\mathbf{f}}_{T-1}$$

（2）得到在 $T-1$ 时间下的 Q
该公式包含两个部分，前半部分代表该状态即时价值，也就是强化学习的 $R$ ，后半部分代表 $V(X_T)$ 的价值
强化学习状态动作价值函数公式：$Q(T-1)=R(T-1)+V(T)$

（3）$V(x_T)$ 最终公式
$Q(T-1)=R(T-1)+V(T)$
根据上面的计算可以得到 $V(x_T)$ 最终二次型公式

上面就是 iLQR 反馈计算 $u$ 的过程

LQR 前馈和反馈步骤

反馈

反馈就是从 T 时刻往后退推到 1 时刻，在这个过程中可以求出 u 的 （k,K）的值

$$\begin{array}{rl}\delta {\mathbf{u}}^{\ast }& =\underset{\delta \mathbf{u}}{\mathrm{argmin}}Q(\delta \mathbf{x},\delta \mathbf{u})=-{\mathbf{Q}}_{uu}^{-1}({\mathbf{Q}}_u+{\mathbf{Q}}_{ux}\delta \mathbf{x})\\ \mathbf{k}& =-{\mathbf{Q}}_{uu}^{-1}{\mathbf{Q}}_u\\ \mathbf{K}& =-{\mathbf{Q}}_{uu}^{-1}{\mathbf{Q}}_{ux}\end{array}$$

$$\begin{array}{rl}\Delta V(i)& =-\frac{1}{2}{\mathbf{Q}}_u^{\top }{\mathbf{Q}}_{uu}^{-1}{\mathbf{Q}}_u\\ {\mathbf{V}}_x(i)& ={\mathbf{Q}}_x-{\mathbf{Q}}_{ux}^{\top }{\mathbf{Q}}_{uu}^{-1}{\mathbf{Q}}_u\\ {\mathbf{V}}_{xx}(i)& ={\mathbf{Q}}_{xx}-{\mathbf{Q}}_{ux}^{\top }{\mathbf{Q}}_{uu}^{-1}{\mathbf{Q}}_{ux}\end{array}$$

前馈

前向传播是从初始状态开始，通过一系列时间步骤生成整个轨迹的过程。有了初始化的 K 的值就可以算 U ，然后在根据状态转移方程计算 x

iLQR

iLQR 相当于在 LQR 的基础上进行二次泰勒展开，那么状态转移方程为：
（1）LQR

（2）iLQR

上式中 $x_{hat}$ 代表 $x_t$ 的参考点，那么就可以用参考点的值推算出 $x_t$ 的值。这里 $x_{hat}$ 一般都是用上一个轨迹点。
那么 ILQR 可以理解为计算 $\delta u$ 的结果

（3）iLQR 的整体流程

$$\begin{array}{rl}\hat{\mathbf{x}}(1)& =\mathbf{x}(1)\\ \hat{\mathbf{u}}(i)& =\mathbf{u}(i)+\mathbf{k}(i)+\mathbf{K}(i)(\hat{\mathbf{x}}(i)-\mathbf{x}(i))\\ \hat{\mathbf{x}}(i+1)& =\mathbf{f}(\hat{\mathbf{x}}(i),\hat{\mathbf{u}}(i))\end{array}$$

常见论文中的推导公式

上面的公式是推导公式，下面是论文中常见的表达
$$J_0(\mathbf{x},\mathbf{U})=\sum _{i=0}^{N-1}\ell ({\mathbf{x}}_i,{\mathbf{u}}_i)+{\ell }_f({\mathbf{x}}_N),$$
整体代价函数为前面每个边的代价 + 终点代价
$${\mathbf{u}}^{\ast }(\mathbf{x})\equiv \arg \underset{\mathbf{u}}{\min }{J}_0(\mathbf{x},\mathbf{U})$$
最优控制问题就是想求代价最小找到最优控制量 $U$
$$V(\mathbf{x},i)=\underset{\mathbf{u}}{\min }\left[\ell (\mathbf{x},\mathbf{u})+V(f(\mathbf{x},\mathbf{u}),i+1)\right]$$

上面公式为 Cost-to-go function，该值等于当前状态的代价 + 往后一步的代价。
对于 iLQR 的目标函数表达为：
$$Q(\delta x,\delta u)=\ell (x+\delta x,u+\delta u,i)+V(f(x+\delta x,u+\delta u),i+1)-V(f(x,u),i+1)$$
$$\approx \frac{1}{2}{\left[\begin{array}{c}1\\ \delta x\\ \delta u\end{array}\right]}^T\left[\begin{array}{ccc}0& Q_x^T& Q_u^T\\ Q_x& Q_{xx}& Q_{xu}\\ Q_u& Q_{ux}& Q_{uu}\end{array}\right]\left[\begin{array}{c}1\\ \delta x\\ \delta u\end{array}\right]$$
上面公式求得就是 $\delta x,\delta u$ 的代价，这两个是基于 x,u 的增量，含义就是添加增量后的代价 - 没有添加增量时的代价

$$\begin{array}{rl}{Q}_x& =l_x+f_x^{\top }{V}_x^{\prime }\\ Q_u& =l_u+f_u^{\top }{V}_x^{\prime }\\ Q_{xx}& =l_{xx}+f_x^{\top }{V}_{xx}^{\prime }{f}_x+V_x^{\prime }\cdot f_{xx}\\ Q_{uu}& =l_{uu}+f_u^{\top }{V}_{xx}^{\prime }{f}_u+V_x^{\prime }\cdot f_{uu}\\ Q_{ux}& =l_{ux}+f_u^{\top }{V}_{xx}^{\prime }{f}_x+V_x^{\prime }\cdot f_{ux}\end{array}$$

iLQR 在 Contingency Planning 的使用

（1）反馈
对于 tree model 来说在计算 Q 时只需要将每个 branch 的 Q 乘一个系数再进行累加

（2）前馈
对于前馈来说每一个点都会计算出一个 (k,K) 那么改点根据自己的 (k,K) 的值生成轨迹

Risk-Aware Contingency Planning

RACP: Risk-Aware Contingency Planning with Multi-Modal Predictions

1.Collision Chance Constraints

这里是根据自车和障碍物之间的距离计算出来的碰撞概率

$$\mathbb{P}(\Vert x_k^v-{\delta }_k^o\Vert \le r)\le 1-\delta$$
对于上式来说，$x_k^v$ 代表自车当前位置，${\delta }_k^o$ 代表目标车的位置
风险概率的定义：
$${\mathcal{R}}_k^o\left(x_k^d\right)={\mathcal{C}}_k^o\left(x_k^d\right){\mathcal{S}}_k^o\left(x_k^d\right),\forall k,o,d$$
上式中 $R_k^o$ 代表风险概率，$C_k^o$ 代表碰撞的风险，$S_k^o$ 代表碰撞后的严重程度，其中 C 的计算和 S 的计算如下所示：

$$C_k^o\left({\mathbf{x}}_k^d\right)=\underset{({\mathbf{x}}_k^d,y_k^d)\in D}{\iint }{f}_k^o(\mathbf{x},\mathbf{y})d\mathbf{x}d\mathbf{y},\forall k,o,d$$
C 的计算使用碰撞概率密度的积分得到的。
$$S_k^o\left(x_k^d\right)=\frac{m^v}{m^v+m^o}{\left({\left(v_k^v\right)}^2+{\left(v_k^o\right)}^2-2v_k^v{v}_k^o\cos \alpha \right)}^{\frac{1}{2}}$$

对于一条轨迹远处的点风险更小，近处的点风险更大更应该关注，所以下面的公式在每个点中乘了一个概率表现每个点的重要程度
$${\mathcal{R}}_k^o\left({\mathbf{x}}_k^d\right)={\left(\gamma \right)}^k{\mathcal{C}}_k^o\left({\mathbf{x}}_k^d\right){\mathcal{S}}_k^o\left({\mathbf{x}}_k^d\right),\forall k,o,d$$

3.Game Theory Based Interactive Planning

iLQGame

iLQR 和 iLQGame 的区别

iLQR 在进行多 agent 迭代时是非常耗时的，如下图所示当存在多个 agent 时，当求 agent 1 时需要将 agent 2,agent 3 状态固定，求 agengt 2 时需要将 agent1 agent2 状态固定，迭代的求不同 agent 的轨迹。

iLQGame 就是将所有 agent 的求解都放到一个公式中

动力学方程：

$$\dot{\mathbf{x}}=\mathbf{f}(t,\mathbf{x},{\mathbf{u}}_{1:N})$$
$$x=\left[\begin{array}{c}\\ x1\\ y1\\ v1\\ \theta 1\\ x2\\ y2\\ v2\\ \theta 2\\ x3\\ y3\\ v3\\ \theta 3\end{array}\right]$$
x 是多个子系统状态的联合状态向量（concatenated states）假设这里有 3 辆车，例如多辆车的联合状态（位置、速度等）。u表示所有玩家（如 N 辆车）的联合控制输入。f 是非线性函数，描述状态如何随时间演化（如车辆动力学）。

代价函数：

$$J_i({\mathbf{u}}_{1:N}(\cdot ))\triangleq {\int }_0^T{g}_i(t,\mathbf{x}(t),{\mathbf{u}}_{1:N}(t))dt$$
$$J=J_1+J_2+J_3$$
每个玩家 i 有自己的成本函数 $J_i$ ，成本的公式为 $g_i$ ，最后所有玩家的目标函数累加求 u （假设这里有 3 个玩家）。其中 g 可以代表一条轨迹的平滑度，与其他轨迹的碰撞风险等 cost 。

如下图所示车和人的动态方程如下，也就是下一步各子状态的增益，根据目标动态方程可以计算出目标函数也就是 Objectives 下的公式

目标–纳什均衡

$$\begin{array}{rl}{J}_i^{\ast }& \triangleq J_i({\gamma }_1^{\ast },\dots ,{\gamma }_{i-1}^{\ast },{\gamma }_i^{\ast },{\gamma }_{i+1}^{\ast },\dots ,{\gamma }_N^{\ast })\\ & \le J_i({\gamma }_1^{\ast },\dots ,{\gamma }_{i-1}^{\ast },{\gamma }_i,{\gamma }_{i+1}^{\ast },\dots ,{\gamma }_N^{\ast }),\forall i\in [N]\end{array}$$
目标是找到每个玩家的时变状态反馈策略 ${\gamma }_i^{\ast }$,使得在纳什均衡下，任何玩家单方面偏离策略 ${\gamma }_i$ 都无法降低自己的成本。
这个 ${\gamma }_i^{\ast }$ 和 $u_i^{\ast }$ 类似，在求 $u^{\ast }$ 时 $u^{\ast }=k+K\Delta x$，${\gamma }_i^{\ast }=\alpha +P\Delta x$
在自动驾驶中，每辆车根据其他车的策略（如变道、加速）选择最优反馈控制（如跟车或避让）。
均衡时，所有玩家的策略互为最优响应（Best Response)

数学推导：

将转移方程线性化表示：
$$\begin{array}{r}{\mathbf{x}}_{t+1}={\mathbf{A}}_t{\mathbf{x}}_t+\sum _{j=1}^N{\mathbf{B}}_t^j{\mathbf{u}}_t^j\end{array}$$

在 $u$ 的前面为什么还有一个累加的符号，并不是将其他 agent 的 $u$ 也累加到自车，是因为 B 是一个分块矩阵，当计算 agent1 时其他 agent 的系数是 0，这里最终累加的只有自己的 $u$

$$J^i=\frac{1}{2}\sum _{t=1}^T\left[({\mathbf{x}}_t^{\top }{\mathbf{Q}}_t^i+2{\mathbf{q}}_t^{i\top }){\mathbf{x}}_t+\sum _{j=1}^N({\mathbf{u}}_t^{j\top }{\mathbf{R}}_t^{ij}+2{\mathbf{r}}_t^{ij\top }){\mathbf{u}}_t^j\right]$$

求解器：

求解在 t 时刻的最优价值函数
$$\begin{array}{rl}{Q}_t^i(x_t,u_t)& =\underset{u_t^i}{\min }\left\{\frac{1}{2}((x_t^T{Q}_t^i+2q_t^{iT})x_t+\sum _{j=1}^N(u_t^{jT}{R}_t^{ij}+2r_t^{ijT}\left)u_t^j\right)+V_{t+1}^i(x_{t+1})\right\}\\ & =\underset{u_t^i}{\min }\left\{\frac{1}{2}((x_t^T{Q}_t^i+2q_t^{iT})x_t+\sum _{j=1}^N(u_t^{jT}{R}_t^{ij}+2r_t^{ijT}\left)u_t^j\right)+V_{t+1}^i(A_t{x}_t+\sum _{j=1}^N{B}_t^j{u}_t^j)\right\},\end{array}$$

转换为价值函数如下：
$$V_t^i(x_t)=\frac{1}{2}\left(x_t^T{Z}_t^i+2{\zeta }_t^{iT}\right)x_t+n_t^i,\text{\hspace{1em}(5)}$$

将上面的 Q 公式中值替换为 V 的表达式如下：

根据上面的公式对第 i 个 agent 进行求导，也就是对 $d_u^{t}i$ 进行求导
$$\begin{array}{r}0=R_t^{ii}{u}_t^i+r_t^{ii}+B_t^{i\top }{Z}_{t+1}^i\left(A_t{x}_t+\sum _{j=1}^N{B}_t^j{u}_t^j\right)+B_t^{i\top }{\zeta }_{t+1}^i.\end{array}$$

求得最优控制量最终表达形式如下：
$$\begin{array}{r}{u}_t^{i\ast }=-P_t^i{x}_t-{\alpha }_t^i.\end{array}$$

最终目的就是求 P 和 α 的值。这 u 的表达式放在求导的表达式中：

$$\begin{array}{rl}0=& -R_t^{ii}\left(P_t^i{x}_t+{\alpha }_t^i\right)+r_t^{ii}\\ & +B_t^{i\top }{Z}_{t+1}^i\left(A_t{x}_t-\sum _{j=1}^N{B}_t^j\left(P_t^j{x}_t+{\alpha }_t^j\right)\right)\\ & +B_t^{i\top }{\zeta }_{t+1}^i.\end{array}$$
将 x 单独提出后：

对于上面的公式假设两个子公式的值都是 0。那么分别求出 P 和 α 的表达式：

$$\begin{array}{rl}\left(R_t^{ii}+B_t^{i\top }{Z}_{t+1}^i{B}_t^i\right)P_t^i+B_t^{i\top }{Z}_{t+1}^i\sum _{j\ne i}{B}_t^j{P}_t^j& =B_t^{i\top }{Z}_{t+1}^i{A}_t,\\ \left(R_t^{ii}+B_t^{i\top }{Z}_{t+1}^i{B}_t^i\right){\alpha }_t^i+B_t^{i\top }{Z}_{t+1}^i\sum _{j\ne i}{B}_t^j{\alpha }_t^j& =B_t^{i\top }{\zeta }_{t+1}^i+r_t^{ii}.\end{array}$$

由于 $B$，$Z$ 都是已知变量，那么在求 $P$ 和 $\alpha$ 矩阵时可以转换成求 $SX=Y$ 的形式，其中 $S,Y$ 都是已知矩阵。最后的 $P，\alpha$ 就是 $X$

通过上面步骤求出了 u，将 u 带入 Q(x,u) 中可以得到 V(x) 的表达式：

$$\begin{array}{rl}{V}_t^i(x_t)& =\frac{1}{2}\left((x_t^{\top }{Q}_t^i+2q_t^{i\top })x_t+\sum _{j=1}^N\left((P_t^j{x}_t+{\alpha }_t^j{)}^{\top }{R}_t^{ij}-2r_t^{ij\top }\right)(P_t^j{x}_t+{\alpha }_t^j)\right)\\ & +\frac{1}{2}\left({\left(A_t{x}_t-\sum _{j=1}^N{B}_t^j(P_t^j{x}_t+{\alpha }_t^j)\right)}^{\top }{Z}_{t+1}^i+2{\zeta }_{t+1}^{i\top }\right)\left(A_t{x}_t-\sum _{j=1}^N{B}_t^j(P_t^j{x}_t+{\alpha }_t^j)\right)\\ & +n_{t+1}^i\\ & =\frac{1}{2}{x}_t^{\top }\left[Q_t^i+\sum _{j=1}^N{P}_t^{j\top }{R}_t^{ij}{P}_t^j+{\left(A_t-\sum _{j=1}^N{B}_t^j{P}_t^j\right)}^{\top }{Z}_{t+1}^i\left(A_t-\sum _{j=1}^N{B}_t^j{P}_t^j\right)\right]x_t\\ & +\frac{1}{2}\left[q_t^i+\sum _{j=1}^N\left(P_t^{j\top }{R}_t^{ij}{\alpha }_t^j-P_t^{j\top }{r}_t^{ij}\right)+{\left(A_t-\sum _{j=1}^N{B}_t^j{P}_t^j\right)}^{\top }\left({\zeta }_{t+1}^i-Z_{t+1}^i\sum _{j=1}^N{B}_t^j{\alpha }_t^j\right)\right]x_t\\ & +\frac{1}{2}\left[\sum _{j=1}^N({\alpha }_t^{j\top }{R}_t^{ij}-2r_t^{ij\top }){\alpha }_t^j-{\left(2{\zeta }_{t+1}^i-Z_{t+1}^i\sum _{j=1}^N{B}_t^j{\alpha }_t^j\right)}^{\top }\sum _{j=1}^N{B}_t^j{\alpha }_t^j\right]\\ & +n_{t+1}^i.\end{array}$$

那么就像前面的公式将其转换为 $Z，\zeta$ 表示为：

$$\begin{array}{r}{F}_t=A_t-\sum _{j=1}^N{B}_t^j{P}_t^j\end{array}$$
$${\beta }_t=-\sum _{j=1}^N{B}_t^j{\alpha }_t^j$$

$$\begin{array}{rl}{Z}_t^i& =Q_t^i+\sum _{j=1}^N{P}_t^{j\top }{R}_t^{ij}{P}_t^j+F_t^{\top }{Z}_{t+1}^i{F}_t,Z_{T+1}^i=0,\\ {\zeta }_t^i& =q_t^i+\sum _{j=1}^N\left(P_t^{j\top }{R}_t^{ij}{\alpha }_t^j-P_t^{j\top }{r}_t^{ij}\right)+F_t^{\top }\left({\zeta }_{t+1}^i+Z_{t+1}^i{\beta }_t\right),{\zeta }_{T+1}^i=0,\\ n_t^i& =\frac{1}{2}\left[\sum _{j=1}^N\left({\alpha }_t^{j\top }{R}_t^{ij}{\alpha }_t^j-2r_t^{ij\top }{\alpha }_t^j\right)-{\left(2{\zeta }_{t+1}^i-Z_{t+1}^i\sum _{j=1}^N{B}_t^j{\alpha }_t^j\right)}^{\top }\sum _{j=1}^N{B}_t^j{\alpha }_t^j\right]+n_{t+1}^i,n_{T+1}^i=0.\end{array}$$
在上面的公式中 agent j 对 agent i 的 u 影响几乎可以看做没有，所以可以将这些值看做 0 ，那么公式可以简化如下：

$$\begin{array}{rl}Z& =Q+F_{}^{\top }ZF,\\ \zeta & =q+F^{\top }({\zeta }_{t+1}+Z_{t+1}+\beta )\end{array}$$

iLQGame 整体流程：

backward 相关代码

backward 是从后往前计算

for (int kk = num_time_steps_ - 2; kk >= 0; kk--) {

得到线性矩阵和二次型矩阵：

const auto& lin = linearization[kk];
const auto& quad = quadraticization[kk];

上面有说最终求解的是公式 $SX=Y$ 那么就要构建每个 agent 的 $SY$ 矩阵

在数学推导章节需要计算每一个 agent 和其他 agent 的 $S$

Dimension cumulative_udim_row = 0;
    for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++) {
      // // Check Nash existence condition (sufficient, not necessary).
      // Eigen::LLT<MatrixXf> llt(quad[ii].control.find(ii)->second.hess +
      //                          lin.Bs[ii].transpose() * Zs_[ii] *
      //                          lin.Bs[ii]);
      // CHECK(llt.info() != Eigen::NumericalIssue);

      // Intermediate variable to store B[ii]' * Z[ii].
      // 因为公式中有很多 BiZi 的计算，所以先计算出来
      const MatrixXf BiZi = lin.Bs[ii].transpose() * Zs_[kk + 1][ii];

      Dimension cumulative_udim_col = 0;
      // 填充 S 矩阵
      for (PlayerIndex jj = 0; jj < dynamics_->NumPlayers(); jj++) {
        Eigen::Ref<MatrixXf> S_block =
            S_.block(cumulative_udim_row, cumulative_udim_col,
                     dynamics_->UDim(ii), dynamics_->UDim(jj));
        // 填充 S 矩阵时分为对角和非对角的情况
        if (ii == jj) {
          // Does player ii's cost depend upon player jj's control?
          const auto control_iter = quad[ii].control.find(ii);
          CHECK(control_iter != quad[ii].control.end())
              << "Player " << ii << " is missing a control Hessian.";
          // BiZiBi+Rii
          S_block = BiZi * lin.Bs[ii] + control_iter->second.hess;
        } else {
          // BiZiBj
          S_block = BiZi * lin.Bs[jj];
        }

        // Increment cumulative_udim_col.
        cumulative_udim_col += dynamics_->UDim(jj);
      }

      // Set appropriate blocks of Y.
      // Y 的第一列
      Y_.block(cumulative_udim_row, 0, dynamics_->UDim(ii), dynamics_->XDim()) =
          BiZi * lin.A;
      // Y 的第二列
      Y_.col(dynamics_->XDim())
          .segment(cumulative_udim_row, dynamics_->UDim(ii)) =
          lin.Bs[ii].transpose() * zetas_[kk + 1][ii] +
          quad[ii].control.at(ii).grad;

      // Increment cumulative_udim_row.
      cumulative_udim_row += dynamics_->UDim(ii);
    }
    
    // 对 S 进行修改，自适应正则化（确保S矩阵正定）
    if (adaptive_regularization_) {
      // Regularize `S` to have positive eigenvalues using the Gershgorin circle
      // theorem (https://en.wikipedia.org/wiki/Gershgorin_circle_theorem). That
      // is, for column i, compute the 1-norm of non-diagonal entries and ensure
      // that the ii^th entry of `S` is greater than that norm by adding some
      // amount to that diagonal entry.
      for (size_t ii = 0; ii < S_.cols(); ii++) {
        const float radius = S_.col(ii).lpNorm<1>() - std::abs(S_(ii, ii));
        const float eval_lo = S_(ii, ii) - radius;

        constexpr float min_eval = 1e-3;
        if (eval_lo < min_eval) S_(ii, ii) += radius + min_eval;
      }
    }

求解 $SX=Y$

// 对 S 进行修改，自适应正则化（确保S矩阵正定）
    if (adaptive_regularization_) {
      // Regularize `S` to have positive eigenvalues using the Gershgorin circle
      // theorem (https://en.wikipedia.org/wiki/Gershgorin_circle_theorem). That
      // is, for column i, compute the 1-norm of non-diagonal entries and ensure
      // that the ii^th entry of `S` is greater than that norm by adding some
      // amount to that diagonal entry.
      for (size_t ii = 0; ii < S_.cols(); ii++) {
        const float radius = S_.col(ii).lpNorm<1>() - std::abs(S_(ii, ii));
        const float eval_lo = S_(ii, ii) - radius;

        constexpr float min_eval = 1e-3;
        if (eval_lo < min_eval) S_(ii, ii) += radius + min_eval;
      }
    }

    // Solve linear matrix equality S X = Y.
    // NOTE: not 100% sure that this avoids dynamic memory allocation.
    // 求解 X 的值
    X_ = S_.householderQr().solve(Y_);

将 X 的值填充到 strategs 中

// Set strategy at current time step.
    // 将求解出来的 P alpha 赋值给策略
for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++) {
  strategies[ii].Ps[kk] = Ps_[ii];
  strategies[ii].alphas[kk] = alphas_[ii];
    }

更新 Z 和 $\zeta$:

// Compute F and beta. 更新 Ft 和 β
 F_ = lin.A;
 beta_ = VectorXf::Zero(dynamics_->XDim());
 for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++) {
   F_ -= lin.Bs[ii] * Ps_[ii]; //  F_t = A_t - \sum_{j=1}^N B_t^j P_t^j
   beta_ -= lin.Bs[ii] * alphas_[ii]; // \beta_t = - \sum_{j=1}^N B_t^j \alpha_t^j
 }

 // Update Zs and zetas.
 for (PlayerIndex ii = 0; ii < dynamics_->NumPlayers(); ii++) {
   zetas_[kk][ii] =
       (F_.transpose() * (zetas_[kk + 1][ii] + Zs_[kk + 1][ii] * beta_) +
        quad[ii].state.grad)
           .eval(); // \zeta &= q + F^\top (\zeta_{t+1} + Z_{ t+1} + \beta)
   Zs_[kk][ii] =
       (F_.transpose() * Zs_[kk + 1][ii] * F_ + quad[ii].state.hess).eval(); // Z &= Q + F_{}^\top Z F
for (const auto& Rij_entry : quad[ii].control) {
  const PlayerIndex jj = Rij_entry.first;
  const MatrixXf& Rij = Rij_entry.second.hess;
  const VectorXf& rij = Rij_entry.second.grad;
  zetas_[kk][ii] += Ps_[jj].transpose() * (Rij * alphas_[jj] - rij); // zeta 的剩余部分
  Zs_[kk][ii] += Ps_[jj].transpose() * Rij * Ps_[jj]; // Z 的剩余部分
      }

AL-iLQR

具体来说，该方法通过引入拉格朗日乘子，将约束条件整合到目标函数中，并通过逐步增加惩罚项来逼近或违反约束条件。

$$\begin{array}{rl}{\mathcal{L}}_A(x,\lambda )& =f(x)+{\lambda }^{\top }c(x)+\frac{1}{2}c(x{)}^{\top }{I}_{\mu }c(x),\\ \text{s.t.}& c(x)=0.\end{array}$$

求解器调用代码

求解器迭代调用

// 求解器迭代，如果求解器超过最大迭代次数或者超时则不再进入 while
while (num_iterations < params_.max_solver_iters && !has_converged &&
       elapsed < max_runtime - timer_.RuntimeUpperBound()) {
  // Start loop timer.
  timer_.Tic();

将转移方程线性化：

// 如果 x_t=1 = f(x_t,u_t)不是线性的，将其线性化
if (!problem_->Dynamics()->TreatAsLinear())
  ComputeLinearization(current_operating_point, &linearization_);

调用求解器：

// 调用刚才 iLQGame 求解器，得到策略
current_strategies = lq_solver_->Solve(
     linearization_, cost_quadraticization_,
     problem_->InitialState() - current_operating_point.xs.front(),
     &delta_xs, &costates);

调整步长更新策略，防止 cost 越来越大，无法收敛：

if (!ModifyLQStrategies(delta_xs, costates, &current_strategies,
                            &current_operating_point, &has_converged)) {
      // Maybe emit warning if exiting early.
      VLOG(1) << "Solver exited due to linesearch failure.";

      // Handle success flag.
      if (success) *success = false;

      return log;
}

计算当前 cost ：

// Compute total costs and check if we've converged.
// 计算这次迭代下的累计 cost 
TotalCosts(current_operating_point, &total_costs);

1h37

相关数学

Hessian 矩阵

在 iLQR 目标函数公式中
$$c({\mathbf{x}}_t,{\mathbf{u}}_t)=\frac{1}{2}{\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]}^{\top }{\mathbf{C}}_t\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]+{\left[\begin{array}{c}{\mathbf{x}}_t\\ {\mathbf{u}}_t\end{array}\right]}^{\top }{\mathbf{c}}_t\text{quadratic}$$
其中 Ct 就是 Hessian 矩阵，当代价函数中存在如绝对值项，非线性项，iLQR 会按当前点做泰勒展开，泰勒展开里的“二次项系数矩阵”就是 Hession 矩阵

相关参考

• https://blog.youkuaiyun.com/qq_36497771/article/details/140370616