# `main.tex` 第 841--843 行详解

本文档解释 `eq:joint_psi_discrete` 为什么能同时影响两条路径、有限支撑核为什么会产生稀疏 Hessian，以及第 843 行所谓“块对角结构”到底是什么意思。

## 1. 先把决策变量拆开

联合优化变量写成

$$
\mathbf z=
\begin{bmatrix}
\mathbf z_{\mathrm{park}}\\
\mathbf z_{\mathrm{exit}}
\end{bmatrix}.
$$

其中，$\mathbf z_{\mathrm{park}}$ 包含泊入路径的全部配置点状态、区间控制量和弧长步长，$\mathbf z_{\mathrm{exit}}$ 同理。每条路径的变量维数为

$$
n_{\mathrm{park}}=5M_{\mathrm{park}},
\qquad
n_{\mathrm{exit}}=5M_{\mathrm{exit}},
$$

所以 $\mathbf z\in\mathbb R^{n_z}$，其中

$$
n_z=n_{\mathrm{park}}+n_{\mathrm{exit}}.
$$

这里的 $5M_\bullet$ 来自 $M_\bullet$ 个四维状态、$M_\bullet-1$ 个控制量以及一个弧长步长。

第 $i$ 个泊入路径位置是

$$
\mathbf p_i^{\mathrm{park}}
=
\begin{bmatrix}x_i^{\mathrm{park}}\\y_i^{\mathrm{park}}\end{bmatrix},
$$

它只是 $\mathbf z_{\mathrm{park}}$ 中第 $i$ 个状态的位置分量，并不是额外的决策变量。定义状态到位置的选择矩阵

$$
\mathbf S_p=
\begin{bmatrix}
1&0&0&0\\
0&1&0&0
\end{bmatrix}.
$$

再定义作用于整个联合变量 $\mathbf z$ 的常数选择矩阵

$$
\mathbf E_i^{\mathrm{park}}\in\mathbb R^{2\times n_z},
\qquad
\mathbf E_j^{\mathrm{exit}}\in\mathbb R^{2\times n_z}.
$$

$\mathbf E_i^{\mathrm{park}}$ 在泊入路径第 $i$ 个四维状态对应的列上放置 $\mathbf S_p$，其余元素均为零；$\mathbf E_j^{\mathrm{exit}}$ 在驶离路径第 $j$ 个四维状态对应的列上放置 $\mathbf S_p$，其余元素均为零。因此

$$
\boxed{
\mathbf p_i^{\mathrm{park}}
=\mathbf E_i^{\mathrm{park}}\mathbf z,
\qquad
\mathbf p_j^{\mathrm{exit}}
=\mathbf E_j^{\mathrm{exit}}\mathbf z.}
$$

若用从 1 开始的元素下标表示，则 $\mathbf E_i^{\mathrm{park}}$ 选取 $\mathbf z$ 的第 $4(i-1)+1$、$4(i-1)+2$ 个元素；$\mathbf E_j^{\mathrm{exit}}$ 选取第

$$
n_{\mathrm{park}}+4(j-1)+1,
\qquad
n_{\mathrm{park}}+4(j-1)+2
$$

个元素。

因此，只要目标函数同时包含 $\mathbf p_i^{\mathrm{park}}$ 和 $\mathbf p_j^{\mathrm{exit}}$，它就是整个 $\mathbf z$ 的函数。

## 2. 将交互项显式写成 $\mathbf z$ 的函数

为简化记号，定义点对选择矩阵

$$
\mathbf D_{ij}
=\mathbf E_i^{\mathrm{park}}-\mathbf E_j^{\mathrm{exit}}
\in\mathbb R^{2\times n_z}.
$$

于是两个配置点的相对位置和距离分别为

$$
\mathbf r_{ij}(\mathbf z)
=\mathbf p_i^{\mathrm{park}}-\mathbf p_j^{\mathrm{exit}}
=\mathbf D_{ij}\mathbf z,
$$

$$
d_{ij}(\mathbf z)
=\|\mathbf D_{ij}\mathbf z\|.
$$

正文实现将两个弧长步长冻结为粗解给出的常数。记

$$
c_0=\frac{w_\varPsi}{v_0}
\Delta s_{\mathrm{park}}^{(0)}
\Delta s_{\mathrm{exit}}^{(0)},
$$

其中上标 $(0)$ 表示粗解值。离散交互项便可以显式写成

$$
\boxed{
J_\varPsi(\mathbf z)
=c_0\sum_{i=1}^{M_{\mathrm{park}}}
\sum_{j=1}^{M_{\mathrm{exit}}}
K\!\left(
\|\mathbf D_{ij}\mathbf z\|;
\rho_\varPsi,m_\varPsi
\right).}
$$

把核函数的分段定义代入后，还可以完全展开为

$$
J_\varPsi(\mathbf z)
=c_0\sum_{i=1}^{M_{\mathrm{park}}}
\sum_{j=1}^{M_{\mathrm{exit}}}
\begin{cases}
\displaystyle
\left(
1-\frac{\|\mathbf D_{ij}\mathbf z\|}{\rho_\varPsi}
\right)^{m_\varPsi},
& \|\mathbf D_{ij}\mathbf z\|<\rho_\varPsi,\\[8pt]
0,
& \|\mathbf D_{ij}\mathbf z\|\geq\rho_\varPsi.
\end{cases}
$$

至此，表达式右侧只剩下决策变量 $\mathbf z$、常数选择矩阵 $\mathbf D_{ij}$ 以及固定参数，不再把 $\mathbf p_i^{\mathrm{park}}$ 和 $\mathbf p_j^{\mathrm{exit}}$ 当成独立符号。这里使用 $\mathbf D_{ij}$ 而不使用 $\mathbf A_{ij}$，是为了避免和正文凸走廊约束中的 $\mathbf A_k$ 混淆。

对于每个点对 $(i,j)$，定义

$$
\mathbf r_{ij}(\mathbf z)
=\mathbf D_{ij}\mathbf z,
\qquad
d_{ij}(\mathbf z)=\|\mathbf D_{ij}\mathbf z\|.
$$

当 $0<d_{ij}(\mathbf z)<\rho_\varPsi$ 时，核函数为

$$
K(d_{ij}(\mathbf z))=
\left(1-\frac{d_{ij}(\mathbf z)}{\rho_\varPsi}\right)^{m_\varPsi},
$$

它随距离增大而减小。因此，最小化 $J_\varPsi$ 会倾向于增大两条路径之间的距离。

## 3. 为什么两条路径会“同时被推开”

对一个有效点对 $(i,j)$，有

$$
K'(d)
=-\frac{m_\varPsi}{\rho_\varPsi}
\left(1-\frac{d}{\rho_\varPsi}\right)^{m_\varPsi-1}.
$$

### 3.1 先求距离对位置的导数

令

$$
\mathbf p_i^{\mathrm{park}}
=\begin{bmatrix}x_i\\y_i\end{bmatrix},
\qquad
\mathbf p_j^{\mathrm{exit}}
=\begin{bmatrix}\bar x_j\\\bar y_j\end{bmatrix}.
$$

则相对位置为

$$
\mathbf r_{ij}
=\mathbf p_i^{\mathrm{park}}-\mathbf p_j^{\mathrm{exit}}
=\begin{bmatrix}
x_i-\bar x_j\\
y_i-\bar y_j
\end{bmatrix},
$$

距离为

$$
d_{ij}=\|\mathbf r_{ij}\|
=\sqrt{(x_i-\bar x_j)^2+(y_i-\bar y_j)^2}.
$$

对泊入点的两个坐标分别求偏导：

$$
\frac{\partial d_{ij}}{\partial x_i}
=\frac{x_i-\bar x_j}{d_{ij}},
\qquad
\frac{\partial d_{ij}}{\partial y_i}
=\frac{y_i-\bar y_j}{d_{ij}}.
$$

将两个分量写成列向量，就是

$$
\nabla_{\mathbf p_i^{\mathrm{park}}}d_{ij}
=
\begin{bmatrix}
\partial d_{ij}/\partial x_i\\
\partial d_{ij}/\partial y_i
\end{bmatrix}
=\frac{1}{d_{ij}}
\begin{bmatrix}
x_i-\bar x_j\\
y_i-\bar y_j
\end{bmatrix}
=\frac{\mathbf r_{ij}}{d_{ij}}.
$$

几何上，$\mathbf r_{ij}/d_{ij}$ 是从驶离点指向泊入点的单位方向向量：泊入点沿这个方向移动会增大两点距离，沿相反方向移动会减小距离。

### 3.2 再使用链式法则

核函数不是直接以 $\mathbf p_i^{\mathrm{park}}$ 为自变量，而是通过标量距离 $d_{ij}$ 依赖位置：

$$
\mathbf p_i^{\mathrm{park}}
\longrightarrow
d_{ij}
\longrightarrow
K(d_{ij}).
$$

因此多元链式法则给出

$$
\frac{\partial K(d_{ij})}
{\partial \mathbf p_i^{\mathrm{park}}}
=\frac{\mathrm dK}{\mathrm dd}(d_{ij})
\nabla_{\mathbf p_i^{\mathrm{park}}}d_{ij}
=K'(d_{ij})\frac{\mathbf r_{ij}}{d_{ij}}.
$$

这就是文档中的目标公式。它由两部分相乘得到：

$$
\underbrace{K'(d_{ij})}_{\text{距离增大时核代价的变化率}}
\quad\times\quad
\underbrace{\frac{\mathbf r_{ij}}{d_{ij}}}_{\text{距离对泊入位置的方向导数}}.
$$

而

$$
\frac{\partial K(d_{ij})}
{\partial \mathbf p_j^{\mathrm{exit}}}
=-K'(d_{ij})\frac{\mathbf r_{ij}}{d_{ij}}.
$$

驶离点前面的负号来自相对位置向量的定义。对每一对泊入点和驶离点，定义

$$
\mathbf r_{ij}
=\mathbf p_i^{\mathrm{park}}-\mathbf p_j^{\mathrm{exit}}.
$$

为了看清这个偏导，令两个二维位置分别写成

$$
\mathbf p_i^{\mathrm{park}}
=\begin{bmatrix}x_i\\y_i\end{bmatrix},
\qquad
\mathbf p_j^{\mathrm{exit}}
=\begin{bmatrix}\bar x_j\\\bar y_j\end{bmatrix}.
$$

于是

$$
\mathbf r_{ij}
=\begin{bmatrix}
x_i-\bar x_j\\
y_i-\bar y_j
\end{bmatrix}.
$$

这里的“对向量求偏导”严格来说是 Jacobian 矩阵，因此逐分量计算可得

$$
\begin{aligned}
\frac{\partial \mathbf r_{ij}}
{\partial \mathbf p_j^{\mathrm{exit}}}
&=
\begin{bmatrix}
\dfrac{\partial(x_i-\bar x_j)}{\partial \bar x_j}
&\dfrac{\partial(x_i-\bar x_j)}{\partial \bar y_j}\\[6pt]
\dfrac{\partial(y_i-\bar y_j)}{\partial \bar x_j}
&\dfrac{\partial(y_i-\bar y_j)}{\partial \bar y_j}
\end{bmatrix}\\[4pt]
&=
\begin{bmatrix}-1&0\\0&-1\end{bmatrix}
=-\mathbf I_2.
\end{aligned}
$$

其中 $\mathbf I_2$ 是二维单位矩阵。这个结果也可以用增量来理解：若驶离点发生一个小位移 $\Delta\mathbf p_j^{\mathrm{exit}}$，而泊入点保持不变，则

$$
\Delta\mathbf r_{ij}
=\Delta\mathbf p_i^{\mathrm{park}}
-\Delta\mathbf p_j^{\mathrm{exit}}
=-\Delta\mathbf p_j^{\mathrm{exit}}.
$$

由于这里固定泊入点，$\Delta\mathbf p_i^{\mathrm{park}}=\mathbf 0$。也就是说，驶离点沿某个方向移动时，从驶离点指向泊入点的相对向量沿相反方向变化，这正是 Jacobian 中出现负号的原因。将它代入链式法则，就得到

$$
\frac{\partial K(d_{ij})}
{\partial \mathbf p_j^{\mathrm{exit}}}
=-K'(d_{ij})\frac{\mathbf r_{ij}}{d_{ij}}.
$$

上述距离梯度推导要求 $d_{ij}>0$；当两个点精确重合时，$\|\mathbf r_{ij}\|$ 在零点没有唯一梯度，需要使用平滑距离或在实现中单独处理该点对。

直接对联合变量 $\mathbf z$ 求导，则一个有效点对的梯度为

$$
\nabla_{\mathbf z}
K\!\left(\|\mathbf D_{ij}\mathbf z\|\right)
=K'(d_{ij})
\frac{\mathbf D_{ij}^{\mathsf T}\mathbf D_{ij}\mathbf z}{d_{ij}}.
$$

定义当前有限支撑域内的有效点对集合

$$
\mathcal I(\mathbf z)=
\left\{(i,j)\ \middle|\
0<\|\mathbf D_{ij}\mathbf z\|<\rho_\varPsi
\right\},
$$

便得到完整交互项关于联合决策变量的梯度：

$$
\boxed{
\nabla_{\mathbf z}J_\varPsi(\mathbf z)
=c_0\sum_{(i,j)\in\mathcal I(\mathbf z)}
K'\!\left(\|\mathbf D_{ij}\mathbf z\|\right)
\frac{\mathbf D_{ij}^{\mathsf T}\mathbf D_{ij}\mathbf z}
{\|\mathbf D_{ij}\mathbf z\|}.}
$$

由于

$$
\mathbf D_{ij}
=\mathbf E_i^{\mathrm{park}}-\mathbf E_j^{\mathrm{exit}},
$$

$\mathbf D_{ij}^{\mathsf T}$ 会把同一个二维点对梯度以相反符号写回 $\mathbf z_{\mathrm{park}}$ 和 $\mathbf z_{\mathrm{exit}}$ 对应的位置分量。这就是 $J_\varPsi(\mathbf z)$ 在一次联合求解中同时更新两条路径的代数原因。

两个梯度大小相等、方向相反。因为 $K'(d)<0$，梯度下降时：

- 泊入点 $\mathbf p_i^{\mathrm{park}}$ 沿远离驶离点的方向移动；
- 驶离点 $\mathbf p_j^{\mathrm{exit}}$ 沿远离泊入点的方向移动。

这就是第 841 行所说的“两条路径可在同一次求解中相互推离”。它并不是先固定一条路径再移动另一条，而是同一个目标函数对两组决策变量同时提供梯度。

需要注意，梯度形式是对称的，不代表最终位移一定相等。两条路径还分别受到引导场、长度、平滑性和走廊约束的作用；哪条路径更容易移动，取决于这些项共同形成的“刚度”。

## 4. Hessian 的“跨路径耦合块”是什么

将联合变量按两条路径分块后，目标函数 Hessian 的结构可以概念性地写成

$$
\nabla^2_{\mathbf z\mathbf z}J=
\begin{bmatrix}
H_{\mathrm{pp}} & H_{\mathrm{pe}}\\
H_{\mathrm{ep}} & H_{\mathrm{ee}}
\end{bmatrix}.
$$

这里：

- $H_{\mathrm{pp}}$ 描述泊入变量之间的二阶关系；
- $H_{\mathrm{ee}}$ 描述驶离变量之间的二阶关系；
- $H_{\mathrm{pe}}$ 和 $H_{\mathrm{ep}}$ 描述泊入变量与驶离变量之间的二阶关系，称为跨路径耦合块。

若目标函数可以完全分离为

$$
J(\mathbf z)=J_{\mathrm{park}}(\mathbf z_{\mathrm{park}})
+J_{\mathrm{exit}}(\mathbf z_{\mathrm{exit}}),
$$

那么

$$
H_{\mathrm{pe}}=H_{\mathrm{ep}}=0,
$$

整个 Hessian 按两条路径呈块对角结构。加入 $J_\varPsi$ 后，一个核项同时依赖 $\mathbf p_i^{\mathrm{park}}$ 和 $\mathbf p_j^{\mathrm{exit}}$，所以相应的跨路径二阶导数一般不为零。

对于单个点对，可以把它的 Hessian 贡献写成

$$
\begin{bmatrix}
G_{ij} & -G_{ij}\\
-G_{ij} & G_{ij}
\end{bmatrix},
$$

其中 $G_{ij}$ 是关于二维相对位置 $\mathbf r_{ij}$ 的 $2\times2$ 二阶导数矩阵。非对角位置的 $-G_{ij}$ 正是该点对造成的跨路径耦合。

## 5. 为什么有限支撑核会让跨路径块稀疏

核函数满足

$$
K(d)=0,\qquad d\geq\rho_\varPsi.
$$

在严格满足 $d_{ij}>\rho_\varPsi$ 的区域内，$K(d_{ij})$ 是常数零，因此它的梯度和 Hessian 也都是零。于是：

- 若点对 $(i,j)$ 的距离小于 $\rho_\varPsi$，它在跨路径块中产生非零项；
- 若点对 $(i,j)$ 的距离大于 $\rho_\varPsi$，它不产生任何导数项。

例如，两条路径各有 4 个点，理论上共有 $4\times4=16$ 个点对。假设只有

$$
(1,1),\ (2,2),\ (2,3),\ (3,3),\ (4,4)
$$

这 5 对点的距离小于作用半径，那么跨路径块中只有与这 5 对点有关的位置非零，其余 11 对均为零。这就是第 841 行所说的“并非稠密的 $M_{\mathrm{park}}\times M_{\mathrm{exit}}$ 块”。

这里的 $M_{\mathrm{park}}\times M_{\mathrm{exit}}$ 是按“配置点对”描述的概念尺寸。由于每个位置点有 $x,y$ 两个分量，实际位置 Hessian 的跨路径块尺寸是

$$
2M_{\mathrm{park}}\times2M_{\mathrm{exit}},
$$

每个有效点对通常对应一个 $2\times2$ 小块。

## 6. “其余部分均为块对角结构”是什么意思

第 843 行的“块对角”是按两条路径分成两个大块而言，并不是说单条路径内部的矩阵是对角矩阵。

### 6.1 单条路径内部仍然存在耦合

配置约束

$$
\mathbf x_{k+1}=\mathbf x_k+
\frac{\Delta s}{2}
\left[\mathbf f(\mathbf x_k,\sigma_k)
+\mathbf f(\mathbf x_{k+1},\sigma_k)\right]
$$

同时依赖 $\mathbf x_k$、$\mathbf x_{k+1}$ 和 $\sigma_k$。所以，单条路径内部相邻配置点之间存在耦合，其 Jacobian/Hessian 通常是带状矩阵，而不是对角矩阵。

### 6.2 但这些约束不连接两条路径

泊入路径的配置约束只包含 $\mathbf z_{\mathrm{park}}$，驶离路径的配置约束只包含 $\mathbf z_{\mathrm{exit}}$。因此按两条路径分块后，约束 Jacobian 具有如下形式：

$$
J_{\mathrm{independent}}=
\begin{bmatrix}
B_{\mathrm{park}}&0\\
0&B_{\mathrm{exit}}
\end{bmatrix}.
$$

其中 $B_{\mathrm{park}}$、$B_{\mathrm{exit}}$ 内部可以是带状的，并不要求是对角的。

同样地：

- 曲率与曲率变化率上界只约束各自路径；
- 泊入走廊约束只作用于泊入路径；
- 驶离走廊约束只作用于驶离路径；
- $J_\ell$、$J_U$ 和 $J_\kappa$ 都可以分别写成泊入部分与驶离部分之和。

所以，在不考虑 $J_\varPsi$ 时，两条路径的数值结构基本可以分开处理。

## 7. 关于装载位约束的一处重要问题

第 843 行称：“唯一的另一处跨路径耦合来自装载位处共享同一位姿”。结合当前论文第 335--347 行的定义，这句话并不严格准确。

当前边界条件写成

$$
\boldsymbol\eta^{\mathrm{park}}(s_{\mathrm{park}})
=\boldsymbol\eta_{\mathrm{load}},
$$

$$
\boldsymbol\eta^{\mathrm{exit}}(0)
=\boldsymbol\eta_{\mathrm{load}},
$$

而正文明确说明 $\boldsymbol\eta_{\mathrm{load}}$ 是由上层系统给出的固定值。若它确实是常量，那么这只是两组分别等于同一常量的边界约束：

$$
h_{\mathrm{park}}(\mathbf z_{\mathrm{park}})=0,
\qquad
h_{\mathrm{exit}}(\mathbf z_{\mathrm{exit}})=0.
$$

第一组不含 $\mathbf z_{\mathrm{exit}}$，第二组不含 $\mathbf z_{\mathrm{park}}$，所以它们不会在 Jacobian 或 Hessian 中产生跨路径非零块。两条路径在几何上共享同一个已知端点，但在导数结构上没有直接耦合。

在当前问题定义下，固定的 $\boldsymbol\eta_{\mathrm{load}}$ 不会形成跨路径导数耦合。只有在另外重新定义约束时，才可能形成真正的跨路径约束，例如：

1. 直接施加

   $$
   \boldsymbol\eta^{\mathrm{park}}(s_{\mathrm{park}})
   -\boldsymbol\eta^{\mathrm{exit}}(0)=0;
   $$

2. 由上层模型额外引入一个待优化的共享位姿变量，并让两条路径的边界条件同时依赖该变量。这个做法已经改变了当前论文中“装载位姿由上层系统给定”的问题定义。

因此，按照论文当前“装载位姿固定”的问题定义，真正的跨路径导数耦合只来自 $J_\varPsi$。第 843 行建议相应修改。

## 8. 可微性表述还需要哪些条件

第 841 行称 $J_\varPsi$ 对 $\mathbf z$ 整体可微，这需要附加条件：

- 当两个路径点完全重合，即 $d_{ij}=0$ 时，欧氏范数 $\|\mathbf r_{ij}\|$ 的梯度没有唯一方向；当前核函数在 $d=0$ 处的径向导数通常又不为零，因此复合函数在重合点处不可微。
- 在截断点 $d=\rho_\varPsi$ 处，若 $m_\varPsi>1$，一阶导数可连续衔接到零；若还希望二阶导数连续，通常需要 $m_\varPsi>2$。

这不一定造成实际求解失败，因为车辆路径点通常不会精确重合，且参数可以选择足够高的 $m_\varPsi$。但论文若要声称“整体可微”，最好明确参数条件，并对 $d=0$ 使用平滑距离，例如

$$
d_\varepsilon=\sqrt{\|\mathbf r\|^2+\varepsilon^2}.
$$

## 9. 用一句话重新理解第 841--843 行

这几行的核心意思是：

> 大部分目标项和约束都只在各自路径内部产生带状导数结构；交互项 $J_\varPsi$ 同时依赖两条路径，因此能在一次 NLP 求解中推动双方分离。由于核函数只作用于距离小于 $\rho_\varPsi$ 的点对，交互项产生的跨路径 Hessian 块通常是稀疏的。

按照当前“装载位姿为固定常量”的定义，不应再把装载位边界条件称为跨路径导数耦合。

## 10. 建议替换原文的表述

可以将第 841--843 行改写为：

> 式 (joint\_psi\_discrete) 同时依赖泊入与驶离路径的位置变量，故其关于两组变量的梯度均非零，两条路径可在同一次求解中相互推离。由有限支撑核的定义，仅当配置点对的距离小于 $\rho_\varPsi$ 时，该点对才对梯度及 Hessian 产生贡献；因此，交互项所形成的跨路径 Hessian 块仅在空间上相近的配置点对处非零。除 $J_\varPsi$ 外，其余目标项与约束均只作用于单条路径内部：配置约束在各自路径内耦合相邻配置点，形成带状结构；物理上界及走廊约束也不连接两条路径。由于 $\boldsymbol\eta_\mathrm{load}$ 是给定常量，两条路径分别等于该位姿的边界条件不产生跨路径导数耦合。因此，联合问题仅通过稀疏的 $J_\varPsi$ 块发生跨路径耦合。