一般在Hybrid A*得到粗解之后，进行二次型优化，它只优化平面点列 (x_i,y_i) ，不优化 \theta,v,a 。目标是把折线路径变平滑、变短，同时用盒子约束躲开附近障碍，并用线性化曲率约束保证不超过自车最大转弯半径。
1.一般二次规划：
\begin{aligned}
\min_{X}\quad
&\frac12 X^\top P X + Q^\top X \\
\text{s.t.}\quad
& G X \le H \\
& A X = B
\end{aligned}
若 P\succeq 0 ，这是凸问题，局部最优即全局最优。本模块的 P 由三块 Gram 矩阵加权相加得到，均为半正定，因此整体凸。
QP 可以看成：在参考路径附近，用一个凸代理问题把折线修的平滑且安全。
2. 决策变量
输入路径有 n个点：
p_i^r = (x_i^r,\; y_i^r,\; \theta_i^r),\qquad i=0,\ldots,n-1
决策变量先排成 2n 维：
X =
\begin{bmatrix}
x_0 & y_0 & x_1 & y_1 & \cdots & x_{n-1} & y_{n-1}
\end{bmatrix}^\top
\in\mathbb{R}^{2n}
记第 i 个平面点 p_i=(x_i,y_i)\in\mathbb{R}^2 。
这部分我们考虑了对于轨迹曲率的约束，所以送进求解器的变量是带松弛的增广变量
\tilde X =
\begin{bmatrix} X \\ s \end{bmatrix}
\in\mathbb{R}^{3n-2},\qquad
s\in\mathbb{R}^{n-2},\quad s\ge 0
s_j 只服务于中间 n-2 个三点组的曲率软约束。
3. 目标函数：三项二次型
配置权重默认：这部分建议大case数据去测试对比
w_s=5,\; w_c=3,\; w_o=0.8,\; w_{\text{slack}}=1
未增广时的标量目标：
J(X)
=
w_s\,J_{\text{smooth}}(X)
+ w_c\,J_{\text{compact}}(X)
+ w_o\,J_{\text{offset}}(X)
一般的求解器形式都是 \tfrac12 X^\top P X + Q^\top X 所以有如下表示
P = 2\bigl(w_s S + w_c C + w_o I_{2n}\bigr),\qquad
Q = -2 w_o X^r
其中 X^r参考路径坐标，下面分别推导 S 和 C偏移的一次项。
3.1 平滑项：惩罚二阶差分
对相邻三点，离散 Laplacian / 二阶差分
\Delta^2 p_i
=
p_i - 2p_{i+1} + p_{i+2}
\in\mathbb{R}^2,
\qquad i=0,\ldots,n-3
平滑代价：
J_{\text{smooth}}(X)
=
\sum_{i=0}^{n-3}
\bigl\|p_i - 2p_{i+1} + p_{i+2}\bigr\|^2
几何含义：三点共线时 \Delta^2 p_i=0；三点间的折角越大该项就越大，它是离散曲线的弯曲代价，对应连续情形
\int \bigl\|p''(s)\bigr\|^2\,ds
的差分近似。
把局部向量写成 z_i=[p_i;\,p_{i+1};\,p_{i+2}]\in\mathbb{R}^6，则
\Delta^2 p_i
=
\begin{bmatrix} I_2 & -2I_2 & I_2 \end{bmatrix} z_i
\|\Delta^2 p_i\|^2
=
z_i^\top
\underbrace{
\begin{bmatrix}
I_2 & -2I_2 & I_2 \\
-2I_2 & 4I_2 & -2I_2 \\
I_2 & -2I_2 & I_2
\end{bmatrix}
}_{Q_{\text{local}}}
z_i
把 Q_{\text{local}}嵌进 2n\times 2n 的大矩阵对应位置，对所有 i 求和，得到 S：
J_{\text{smooth}} = X^\top S X,\qquad S\succeq 0
3.2 紧凑项：惩罚一阶差分
J_{\text{compact}}(X)
=
\sum_{i=0}^{n-2}
\|p_i - p_{i+1}\|^2
局部 Hessian：
\begin{bmatrix} I_2 & -I_2 \\ -I_2 & I_2 \end{bmatrix}
累加得到 C, J_{\text{compact}}=X^\top C X
没有这一项，平滑项会把点往两边“拉直拉开”，路径变长、采样间距失控。注意紧凑项的物理角色是张力：它缩短路径、切内角；端点固定时最小化 \sum_i\|p_i-p_{i+1}\|^2等价于让间距尽量均匀，所以它顺带也起到「均匀采样」的作用。真正把解钉在参考路径附近的是偏移项，不是紧凑项。
3.3 偏移项：贴住 Hybrid A*得到的粗解
J_{\text{offset}}(X)
=
\|X - X^r\|^2
=
X^\top X - 2(X^r)^\top X + \|X^r\|^2
常数 \|X^r\|^2 不影响最优解，丢掉后：
\frac12 X^\top (2w_o I) X + (-2w_o X^r)^\top X
=
w_o\bigl(\|X\|^2 - 2(X^r)^\top X\bigr)
三项合在一起：
J(X)
=
w_s\sum_i\|\Delta^2 p_i\|^2
+
w_c\sum_i\|p_i-p_{i+1}\|^2
+
w_o\sum_i\|p_i-p_i^r\|^2
直觉：
4. 等式约束：钉住首尾
A X = B,\qquad
A=
\begin{bmatrix}
I_2 & 0 & \cdots & 0 \\
0 & \cdots & 0 & I_2
\end{bmatrix}
\in\mathbb{R}^{4\times 2n}
B=
\begin{bmatrix}
x_0^r \\ y_0^r \\ x_{n-1}^r \\ y_{n-1}^r
\end{bmatrix}
即 p_0=p_0^r, p_{n-1}=p_{n-1}^r 。中间点自由。换挡拼接时，相邻段在切点处共享几何位置，但各自独立优化，因此切点本身被钉在 A* 点上。
带松弛时只是 A 右侧补零列： \tilde A= [A \;\; 0_{4\times(n-2)}]
5. 碰撞不等式：局部轴对齐盒子
QP 没有“车体多边形与障碍多边形不相交”这种非凸约束，而是对每个路径点给一个轴对齐盒子：
x_{\min,i}\le x_i\le x_{\max,i},\qquad
y_{\min,i}\le y_i\le y_{\max,i}
矩阵形式：
\begin{bmatrix} I_{2n} \\ -I_{2n} \end{bmatrix} X
\le
\begin{bmatrix} H_{\max} \\ -H_{\min} \end{bmatrix}
其中
H_{\max}=
\begin{bmatrix} x_{\max,0}\\ y_{\max,0}\\ \vdots \end{bmatrix},\quad
H_{\min}=
\begin{bmatrix} x_{\min,0}\\ y_{\min,0}\\ \vdots \end{bmatrix}
5.1 盒子怎么算
对参考点 p_i^r ：
用膨胀车体做 AABB，再外扩 expand_dis（默认 0.8 m），筛近邻障碍栅格点。
按 \theta_i^r 所在象限把空间分成车体前/后/左/右四个邻域。
对落入某邻域的障碍点，计算到对应车体边的距离 d ，再投影成水平/竖直余量：
d_{\perp}=\frac{|k x+b-y|}{\sqrt{1+k^2}},\qquad
\Delta x=\frac{d_{\perp}}{|\sin\theta|},\quad
\Delta y=\frac{d_{\perp}}{|\cos\theta|}
四个方向取最近障碍，得到允许移动量 x_{\min}^{\text{move}} 等。若附近无障碍，移动量退回 expand_dis。
世界系盒子：
\begin{aligned}
x_{\max,i} &= x_i^r + x_{\max}^{\text{move}} \\
x_{\min,i} &= x_i^r - x_{\min}^{\text{move}} \\
y_{\max,i} &= y_i^r + y_{\max}^{\text{move}} \\
y_{\min,i} &= y_i^r - y_{\min}^{\text{move}}
\end{aligned}
这是一种在参考姿态处冻结朝向后的局部线性避障。优化后 会按新折线重算，盒子并不跟着转，所以 QP 可行并不保证新姿态下矩形仍无碰。这是本模块最主要的模型误差之一。
补一句：冻结姿态下，真实的无碰可行域其实是若干半平面的交集（车体每条边对应一条），它本身就是线性的，加进 QP 不增加任何求解代价。
6. 曲率约束：从非线性到线性化 QP
6.1 离散曲率
对相邻三点，弦长记为L，二阶差分为 \Delta^2 p_{i-1}=p_{i-1}-2p_i+p_{i+1}，则
\kappa_i \approx \frac{\|p_{i+1}-2p_i+p_{i-1}\|}{L^2} = \frac{\|\Delta^2 p_{i-1}\|}{L^2}
这个近似在等弦长采样时其实是精确的：设相邻两弦的转角为 \alpha,则 \|\Delta^2 p\|=2L\sin(\alpha/2)，而三点外接圆的曲率恰为 \kappa=2\sin(\alpha/2)/L，两者相等。等价说法是，该约束就是转角上界
\alpha \le 2\arcsin\!\Bigl(\frac{L\,\kappa_{\max}}{2}\Bigr)
注意是「等弦长」而非「等弧长」
车辆曲率上界
\kappa_{\max}=\frac{1}{R_{\min}}=\frac{1}{L_w/\tan\sigma_{\max}+L_b/2}
这里要说明一句：后轴中心路径的运动学极限曲率是 \tan\sigma_{\max}/L_w，额外加 L_b/2 只是拿车体半宽换一点扫掠余量，是经验裕度而非纯运动学量；
希望 \kappa_i\le\kappa_{\max} ，即
\|p_{i+1}-2p_i+p_{i-1}\|
\le
\Delta s^2\,\kappa_{\max}
两边平方（两边非负，等价）：
F_i(X)
:=
\|p_{i+1}-2p_i+p_{i-1}\|^2
-
\bigl(\Delta s^2\kappa_{\max}\bigr)^2
\le 0
评论区