NeuPAN代码解读

2. 优化问题构建

加粗代表变量

2.1 `nrmp`

定义变量variable_definition

$\bm{d}$ 安全距离尺寸为 $1 \times T$
$\bm{s}$ 状态尺寸为 $3 \times (T+1)$ ，代表 $t_0 - T$
$\bm{u}$ 控制输入尺寸为 $3 \times T$ ，代表 $t_1 - T$

定义参数parameter_definition

$s$ 预测状态尺寸为 $3 \times (T+1)$
$q \circ s^\diamondsuit$ 权重乘以参考状态尺寸为 $3 \times (T+1)$
$p \circ u^\diamondsuit$ 权重乘以参考控制输入尺寸为 $3 \times (T+1)$
$A$ 运动学方程中的状态转移矩阵尺寸为 $T \times (3 \times 3)$
$B$ 运动学方程中的输入移矩阵尺寸为 $T \times (3 \times 2)$
$C$ 运动学方程中的扰动向量尺寸为 $T \times (3 \times 1)$
$\lambda ^\top$ 尺寸为 $T \times (M \times 2)$ ， $M$ 代表最大点云数量
$\lambda ^\top p + \mu ^\top h$ 尺寸为 $T \times (M \times 1)$
$q$ 状态的权重系数尺寸为 $1$ 或 $3 \times 1$ ，取决于给参数输入的格式
$p$ 控制输入的权重系数尺寸为 $1$
$\eta$ 安全距离正则化的权重系数尺寸为 $1$
$d_{\max}$ 安全距离的上限尺寸为 $1$
$d_{\min}$ 安全距离的下限尺寸为 $1$

构建约束

$\bm{s}_{t+1} = A_t \bm{s}_t + B_t \bm{u}_t + C_t, \forall t$
$\bm{s}_{0} = s_0$
$a_{\min} \leq \bm{u}_{t+1} - \bm{u}_t \leq a_{\max}$
$u_{\min} \leq \bm{u}_t \leq u_{\max}$
$d_{\min} \leq \bm{d}_t \leq d_{\max}$

代价函数为

$\begin{align*} & \operatorname{cost} : \\ & \sum_{h=t}^{t+H} \lVert q \circ \bm{s}_h - (q \circ s^\diamondsuit)_h \rVert_2^2 + \sum_{h=t}^{t+H} \lVert p \circ \bm{u}_h - (p \circ u^\diamondsuit)_h \rVert_2^2 \\ & + \frac{b_k}{2} \lVert \bm{s}_h - s^{\text{ref}}_h \rVert_2^2 - \eta \sum_{h=t}^{t+H} \bm{d}_h + \frac{\rho}{2} \sum_{t=0}^{N} \lVert \min(I_{t,m} , 0) \rVert_2^2 \\ & \text{where }I_{t,m} = \lambda^{i \top}_t \bm{s}_t -( \lambda ^\top p + \mu ^\top h)^i_t - \bm{d}_t \end{align*}$

可以不考虑 $E$ 函数，因为在DUNE部分求解出的结果应该是满足约束的，同时每次迭代时轨迹变化不大，那么 $E$ 函数便可以视为零

为参数赋值generate_parameter_value（赋值的是tensor）

$s$ 将上次求出的最优控制值输入到运动学模型中得到预测状态
$q \circ s^\diamondsuit$ 权重系数乘以参考路径上的状态
$p \circ u^\diamondsuit$ 权重乘以参考控制输入
$A$ 将运动学方程线性化处理，用预测状态作为矩阵参数
$B$ 将运动学方程线性化处理，用预测状态作为矩阵参数
$C$ 将运动学方程线性化处理，用预测状态作为矩阵参数
$\lambda ^\top$ DUNE求解出的参数值
$\lambda ^\top p + \mu ^\top h$ DUNE求解出的参数值与点云和物体自身参数相乘
$q$ 构造函数输入或者通过update函数更新
$p$ 构造函数输入或者通过update函数更新
$\eta$ 构造函数输入或者通过update函数更新
$d_{\max}$ 构造函数输入或者通过update函数更新
$d_{\min}$ 构造函数输入或者通过update函数更新

2. 优化问题构建

2.1 nrmp

2.1 `nrmp`