参考线平滑算法详解（FEM-Based Path Smoothing）

参考线平滑算法详解（FEM-Based Path Smoothing）

目录

1. 算法概述

1.1 问题定义

输入：离散的原始参考路径点

{

(

x

i

r

e

f

,

y

i

r

e

f

)

}

i

=

0

n

−

1

\\{(x_i^{ref}, y_i^{ref})\\}_{i=0}^{n-1}

{(xiref​,yiref​)}i=0n−1​

输出：平滑后的路径点

{

(

x

i

,

y

i

)

}

i

=

0

n

−

1

\\{(x_i, y_i)\\}_{i=0}^{n-1}

{(xi​,yi​)}i=0n−1​

目标：在保持接近原始路径的同时，生成一条平滑、连续、曲率变化小的路径。

1.2 应用场景

在自动驾驶路径规划中，参考线平滑主要用于：

• 全局路径平滑：优化路径规划模块输出的折线路径
• 局部路径优化：消除DP动态规划产生的锯齿状路径
• 曲率优化：确保车辆可以平稳跟踪（减少急转弯）

1.3 核心思想

使用**有限元法（FEM）将路径点之间的连续性约束转化为代价函数，通过求解二次规划（QP）**问题得到最优平滑路径。

2. 数学理论基础

2.1 有限元法基本原理

有限元法将连续的路径用分段线性/多项式近似：

p

(

s

)

=

∑

i

=

0

n

−

1

N

i

(

s

)

⋅

p

i

(1)

p(s) = \\sum_{i=0}^{n-1} N_i(s) \\cdot p_i \\tag{1}

p(s)=i=0∑n−1​Ni​(s)⋅pi​(1)

其中：

• $p(s)$
• $N_i(s)$
• $p_i$

2.2 三点有限元估计

对于路径点

p

i

p_i

pi​，使用相邻三点的有限元估计：

p

^

i

=

p

i

−

1

+

p

i

+

1

2

(2)

\\hat{p}_i = \\frac{p_{i-1} + p_{i+1}}{2} \\tag{2}

p^​i​=2pi−1​+pi+1​​(2)

物理意义：如果路径是一条直线，则中间点应该是两端点的平均值。

偏差：

e

i

=

p

i

−

p

^

i

=

p

i

−

p

i

−

1

+

p

i

+

1

2

(3)

e_i = p_i – \\hat{p}_i = p_i – \\frac{p_{i-1} + p_{i+1}}{2} \\tag{3}

ei​=pi​−p^​i​=pi​−2pi−1​+pi+1​​(3)

2.3 路径长度估计

相邻两点之间的路径长度（一阶差分）：

Δ

p

i

=

p

i

+

1

−

p

i

(4)

\\Delta p_i = p_{i+1} – p_i \\tag{4}

Δpi​=pi+1​−pi​(4)

总路径长度：

L

=

∑

i

=

0

n

−

2

∥

Δ

p

i

∥

2

=

∑

i

=

0

n

−

2

(

p

i

+

1

−

p

i

)

T

(

p

i

+

1

−

p

i

)

(5)

L = \\sum_{i=0}^{n-2} \\|\\Delta p_i\\|^2 = \\sum_{i=0}^{n-2} (p_{i+1} – p_i)^T (p_{i+1} – p_i) \\tag{5}

L=i=0∑n−2​∥Δpi​∥2=i=0∑n−2​(pi+1​−pi​)T(pi+1​−pi​)(5)

3. 优化问题建模

3.1 代价函数

参考线平滑的总代价函数由三部分组成：

J

t

o

t

a

l

=

J

f

e

m

+

J

l

e

n

g

t

h

+

J

r

e

f

(6)

J_{total} = J_{fem} + J_{length} + J_{ref} \\tag{6}

Jtotal​=Jfem​+Jlength​+Jref​(6)

(1) 有限元偏差代价 $J_{fem}$

惩罚路径的非平滑性（曲率大的位置）：

J

f

e

m

=

w

f

e

m

∑

i

=

1

n

−

2

∥

p

i

−

p

i

−

1

+

p

i

+

1

2

∥

2

(7)

J_{fem} = w_{fem} \\sum_{i=1}^{n-2} \\left\\| p_i – \\frac{p_{i-1} + p_{i+1}}{2} \\right\\|^2 \\tag{7}

Jfem​=wfem​i=1∑n−2​

​pi​−2pi−1​+pi+1​​

​2(7)

物理意义：

• 如果 $p_i$
• 该项越小，路径越平滑（接近直线）

展开形式（针对点

p

i

p_i

pi​）：

∥

p

i

−

p

i

−

1

+

p

i

+

1

2

∥

2

=

∥

2

p

i

−

p

i

−

1

−

p

i

+

1

2

∥

2

(8)

\\left\\| p_i – \\frac{p_{i-1} + p_{i+1}}{2} \\right\\|^2 = \\left\\| \\frac{2p_i – p_{i-1} – p_{i+1}}{2} \\right\\|^2 \\tag{8}

​pi​−2pi−1​+pi+1​​

​2=

​22pi​−pi−1​−pi+1​​

​2(8)

=

1

4

(

p

i

−

1

−

2

p

i

+

p

i

+

1

)

T

(

p

i

−

1

−

2

p

i

+

p

i

+

1

)

(9)

= \\frac{1}{4} (p_{i-1} – 2p_i + p_{i+1})^T (p_{i-1} – 2p_i + p_{i+1}) \\tag{9}

=41​(pi−1​−2pi​+pi+1​)T(pi−1​−2pi​+pi+1​)(9)

这是路径的二阶差分，近似于曲率的平方。

(2) 路径长度代价 $J_{length}$

惩罚路径总长度（鼓励走捷径）：

J

l

e

n

g

t

h

=

w

l

e

n

g

t

h

∑

i

=

0

n

−

2

∥

p

i

+

1

−

p

i

∥

2

(10)

J_{length} = w_{length} \\sum_{i=0}^{n-2} \\|p_{i+1} – p_i\\|^2 \\tag{10}

Jlength​=wlength​i=0∑n−2​∥pi+1​−pi​∥2(10)

物理意义：

• 最小化路径长度，避免绕远路
• 与FEM项平衡：太平滑可能导致路径变长

(3) 参考点偏差代价 $J_{ref}$

惩罚偏离原始参考路径：

J

r

e

f

=

w

r

e

f

∑

i

=

0

n

−

1

∥

p

i

−

p

i

r

e

f

∥

2

(11)

J_{ref} = w_{ref} \\sum_{i=0}^{n-1} \\|p_i – p_i^{ref}\\|^2 \\tag{11}

Jref​=wref​i=0∑n−1​∥pi​−piref​∥2(11)

物理意义：

• 确保平滑后的路径不会偏离原始意图太远
• 例如：不能为了平滑而切到对向车道

3.2 二次型矩阵形式

将所有路径点堆叠成向量：

p

=

[

x

0

,

y

0

,

x

1

,

y

1

,

…

,

x

n

−

1

,

y

n

−

1

]

T

∈

R

2

n

(12)

\\mathbf{p} = [x_0, y_0, x_1, y_1, \\ldots, x_{n-1}, y_{n-1}]^T \\in \\mathbb{R}^{2n} \\tag{12}

p=[x0​,y0​,x1​,y1​,…,xn−1​,yn−1​]T∈R2n(12)

总代价函数可以写成标准二次型：

J

t

o

t

a

l

=

1

2

p

T

P

p

+

q

T

p

+

c

o

n

s

t

(13)

J_{total} = \\frac{1}{2} \\mathbf{p}^T \\mathbf{P} \\mathbf{p} + \\mathbf{q}^T \\mathbf{p} + const \\tag{13}

Jtotal​=21​pTPp+qTp+const(13)

其中

P

\\mathbf{P}

P 是 Hessian 矩阵（对称半正定），

q

\\mathbf{q}

q 是线性项。

4. 有限元法离散化

4.1 Hessian矩阵构造

对于

n

n

n 个路径点（

2

n

2n

2n 个变量），Hessian矩阵

P

∈

R

2

n

×

2

n

\\mathbf{P} \\in \\mathbb{R}^{2n \\times 2n}

P∈R2n×2n 的结构如下：

矩阵块结构（以6个点为例）

定义：

• $\mathbf{I}$
• $\mathbf{X}=w_{fem}\cdot \mathbf{I}$
• $\mathbf{Y}=w_{length}\cdot \mathbf{I}$
• $\mathbf{Z}=w_{ref}\cdot \mathbf{I}$
• $0$

则 Hessian 矩阵为（仅上三角，对称矩阵）：

P

=

[

X

+

Y

+

Z

−

2

X

−

Y

X

0

0

0

⋅

5

X

+

2

Y

+

Z

−

4

X

−

Y

X

0

0

⋅

⋅

6

X

+

2

Y

+

Z

−

4

X

−

Y

X

0

⋅

⋅

⋅

6

X

+

2

Y

+

Z

−

4

X

−

Y

X

⋅

⋅

⋅

⋅

5

X

+

2

Y

+

Z

−

2

X

−

Y

⋅

⋅

⋅

⋅

⋅

X

+

Y

+

Z

]

(14)

\\mathbf{P} = \\begin{bmatrix} \\mathbf{X}+\\mathbf{Y}+\\mathbf{Z} & -2\\mathbf{X}-\\mathbf{Y} & \\mathbf{X} & \\mathbf{0} & \\mathbf{0} & \\mathbf{0} \\\\ \\cdot & 5\\mathbf{X}+2\\mathbf{Y}+\\mathbf{Z} & -4\\mathbf{X}-\\mathbf{Y} & \\mathbf{X} & \\mathbf{0} & \\mathbf{0} \\\\ \\cdot & \\cdot & 6\\mathbf{X}+2\\mathbf{Y}+\\mathbf{Z} & -4\\mathbf{X}-\\mathbf{Y} & \\mathbf{X} & \\mathbf{0} \\\\ \\cdot & \\cdot & \\cdot & 6\\mathbf{X}+2\\mathbf{Y}+\\mathbf{Z} & -4\\mathbf{X}-\\mathbf{Y} & \\mathbf{X} \\\\ \\cdot & \\cdot & \\cdot & \\cdot & 5\\mathbf{X}+2\\mathbf{Y}+\\mathbf{Z} & -2\\mathbf{X}-\\mathbf{Y} \\\\ \\cdot & \\cdot & \\cdot & \\cdot & \\cdot & \\mathbf{X}+\\mathbf{Y}+\\mathbf{Z} \\end{bmatrix} \\tag{14}

P=

​X+Y+Z⋅⋅⋅⋅⋅​−2X−Y5X+2Y+Z⋅⋅⋅⋅​X−4X−Y6X+2Y+Z⋅⋅⋅​0X−4X−Y6X+2Y+Z⋅⋅​00X−4X−Y5X+2Y+Z⋅​000X−2X−YX+Y+Z​

​(14)

4.2 系数推导

以中间点

p

i

p_i

pi​（

2

≤

i

≤

n

−

3

2 \\leq i \\leq n-3

2≤i≤n−3）为例，展开代价函数：

FEM项贡献：

J

f

e

m

(

i

)

=

w

f

e

m

(

p

i

−

1

−

2

p

i

+

p

i

+

1

)

T

(

p

i

−

1

−

2

p

i

+

p

i

+

1

)

(15)

J_{fem}^{(i)} = w_{fem} (p_{i-1} – 2p_i + p_{i+1})^T (p_{i-1} – 2p_i + p_{i+1}) \\tag{15}

Jfem(i)​=wfem​(pi−1​−2pi​+pi+1​)T(pi−1​−2pi​+pi+1​)(15)

对

p

i

p_i

pi​ 的 Hessian：

∂

2

J

f

e

m

(

i

)

∂

p

i

2

=

4

w

f

e

m

I

(16)

\\frac{\\partial^2 J_{fem}^{(i)}}{\\partial p_i^2} = 4 w_{fem} \\mathbf{I} \\tag{16}

∂pi2​∂2Jfem(i)​​=4wfem​I(16)

但注意

p

i

p_i

pi​ 出现在三个FEM项中：

• $J_{fem}^{(i-1)}$
• $J_{fem}^{(i)}$
• $J_{fem}^{(i+1)}$

总贡献：

4

w

f

e

m

+

w

f

e

m

+

w

f

e

m

=

6

w

f

e

m

4w_{fem} + w_{fem} + w_{fem} = 6w_{fem}

4wfem​+wfem​+wfem​=6wfem​

长度项贡献：

J

l

e

n

g

t

h

(

i

)

=

w

l

e

n

g

t

h

[

(

p

i

−

p

i

−

1

)

T

(

p

i

−

p

i

−

1

)

+

(

p

i

+

1

−

p

i

)

T

(

p

i

+

1

−

p

i

)

]

(17)

J_{length}^{(i)} = w_{length} [(p_i – p_{i-1})^T(p_i – p_{i-1}) + (p_{i+1} – p_i)^T(p_{i+1} – p_i)] \\tag{17}

Jlength(i)​=wlength​[(pi​−pi−1​)T(pi​−pi−1​)+(pi+1​−pi​)T(pi+1​−pi​)](17)

对

p

i

p_i

pi​ 的 Hessian：

∂

2

J

l

e

n

g

t

h

∂

p

i

2

=

2

w

l

e

n

g

t

h

I

(18)

\\frac{\\partial^2 J_{length}}{\\partial p_i^2} = 2 w_{length} \\mathbf{I} \\tag{18}

∂pi2​∂2Jlength​​=2wlength​I(18)

参考点偏差项：

J

r

e

f

(

i

)

=

w

r

e

f

(

p

i

−

p

i

r

e

f

)

T

(

p

i

−

p

i

r

e

f

)

(19)

J_{ref}^{(i)} = w_{ref} (p_i – p_i^{ref})^T (p_i – p_i^{ref}) \\tag{19}

Jref(i)​=wref​(pi​−piref​)T(pi​−piref​)(19)

对

p

i

p_i

pi​ 的 Hessian：

∂

2

J

r

e

f

(

i

)

∂

p

i

2

=

w

r

e

f

I

(20)

\\frac{\\partial^2 J_{ref}^{(i)}}{\\partial p_i^2} = w_{ref} \\mathbf{I} \\tag{20}

∂pi2​∂2Jref(i)​​=wref​I(20)

总Hessian（对角块）：

P

i

i

=

(

6

w

f

e

m

+

2

w

l

e

n

g

t

h

+

w

r

e

f

)

I

(21)

\\mathbf{P}_{ii} = (6w_{fem} + 2w_{length} + w_{ref}) \\mathbf{I} \\tag{21}

Pii​=(6wfem​+2wlength​+wref​)I(21)

对应代码：fem_pos_deviation_osqp_interface.cpp 第172-173行：

col_index, 6.0 * weight_fem_pos_deviation_ +
2.0 * weight_path_length_ + weight_ref_deviation_

5. 二次规划求解

5.1 标准QP问题

优化问题最终形式：

min

⁡

p

1

2

p

T

P

p

+

q

T

p

s.t.

l

≤

A

p

≤

u

(22)

\\begin{aligned} \\min_{\\mathbf{p}} \\quad & \\frac{1}{2} \\mathbf{p}^T \\mathbf{P} \\mathbf{p} + \\mathbf{q}^T \\mathbf{p} \\\\ \\text{s.t.} \\quad & \\mathbf{l} \\leq \\mathbf{A} \\mathbf{p} \\leq \\mathbf{u} \\end{aligned} \\tag{22}

pmin​s.t.​21​pTPp+qTpl≤Ap≤u​(22)

其中：

• $\mathbf{P}$
• $\mathbf{q}$
• $\mathbf{A}$
• $\mathbf{l},\mathbf{u}$

5.2 线性项 $\mathbf{q}$

参考点偏差项展开：

J

r

e

f

=

w

r

e

f

∑

i

=

0

n

−

1

(

p

i

−

p

i

r

e

f

)

T

(

p

i

−

p

i

r

e

f

)

(23)

J_{ref} = w_{ref} \\sum_{i=0}^{n-1} (p_i – p_i^{ref})^T (p_i – p_i^{ref}) \\tag{23}

Jref​=wref​i=0∑n−1​(pi​−piref​)T(pi​−piref​)(23)

=

w

r

e

f

∑

i

=

0

n

−

1

(

p

i

T

p

i

−

2

p

i

T

p

i

r

e

f

+

(

p

i

r

e

f

)

T

p

i

r

e

f

)

(24)

= w_{ref} \\sum_{i=0}^{n-1} (p_i^T p_i – 2 p_i^T p_i^{ref} + (p_i^{ref})^T p_i^{ref}) \\tag{24}

=wref​i=0∑n−1​(piT​pi​−2piT​piref​+(piref​)Tpiref​)(24)

线性项系数：

q

i

=

−

2

w

r

e

f

p

i

r

e

f

(25)

q_i = -2 w_{ref} p_i^{ref} \\tag{25}

qi​=−2wref​piref​(25)

对应代码：fem_pos_deviation_osqp_interface.cpp 第218-219行：

q->push_back(–2.0 * weight_ref_deviation_ * ref_point_xy.first);
q->push_back(–2.0 * weight_ref_deviation_ * ref_point_xy.second);

5.3 约束条件

盒约束（Box Constraints）：限制平滑后的点不能偏离原始点太远

p

i

r

e

f

−

b

i

≤

p

i

≤

p

i

r

e

f

+

b

i

(26)

p_i^{ref} – b_i \\leq p_i \\leq p_i^{ref} + b_i \\tag{26}

piref​−bi​≤pi​≤piref​+bi​(26)

其中

b

i

b_i

bi​ 是第

i

i

i 个点的横向边界（lateral bound）。

特殊处理：

• 起点和终点：$b_0=b_{n-1}=0$
• 中间点：$b_i$

对应代码：discrete_points_reference_line_smoother.cpp 第25-26行：

anchorpoints_lateralbound.front() = 0.0; // 起点固定
anchorpoints_lateralbound.back() = 0.0; // 终点固定

6. 代码实现详解

6.1 算法流程

主流程（DiscretePointsReferenceLineSmoother::Smooth）

// discrete_points_reference_line_smoother.cpp: 8-51行
bool Smooth(const MatrixXd& raw_reference_line, MatrixXd &smoothed_reference_line) {
// 步骤1：提取原始参考点
std::vector<std::pair<double, double>> raw_point2d;
for (const auto& anchor_point : anchor_points_) {
raw_point2d.emplace_back(anchor_point.x, anchor_point.y);
anchorpoints_lateralbound.emplace_back(anchor_point.lateral_bound);
}

// 步骤2：归一化（相对于起点）
NormalizePoints(&raw_point2d);

// 步骤3：调用FEM优化求解器
std::vector<std::pair<double, double>> smoothed_point2d;
bool status = FemPosSmooth(raw_point2d, anchorpoints_lateralbound, &smoothed_point2d);

// 步骤4：反归一化（恢复全局坐标）
DeNormalizePoints(&smoothed_point2d);

// 步骤5：计算曲率、航向角等几何属性
referencePath(xy, smoothed_reference_line);

return true;
}

数学映射：

原始路径

→

归一化

局部坐标

→

QP求解

优化路径

→

反归一化

全局平滑路径

(27)

\\text{原始路径} \\xrightarrow{\\text{归一化}} \\text{局部坐标} \\xrightarrow{\\text{QP求解}} \\text{优化路径} \\xrightarrow{\\text{反归一化}} \\text{全局平滑路径} \\tag{27}

原始路径归一化

​局部坐标QP求解

​优化路径反归一化

​全局平滑路径(27)

6.2 Hessian矩阵构造（核心算法）

矩阵填充规则

第0, 1点（起点和第二个点）：

// fem_pos_deviation_osqp_interface.cpp: 145-159行
// 对角元素
P[0,0] = weight_fem + weight_length + weight_ref // X+Y+Z
P[1,1] = 5*weight_fem + 2*weight_length + weight_ref // 5X+2Y+Z

// 非对角元素
P[0,2] = –2*weight_fem – weight_length // -2X-Y
P[1,3] = –4*weight_fem – weight_length // -4X-Y

中间点

i

i

i（

2

≤

i

≤

n

−

3

2 \\leq i \\leq n-3

2≤i≤n−3）：

// fem_pos_deviation_osqp_interface.cpp: 162-176行
P[i,i–2] = weight_fem // X
P[i,i–1] = –4*weight_fem – weight_length // -4X-Y
P[i,i] = 6*weight_fem + 2*weight_length + weight_ref // 6X+2Y+Z

倒数第2, 1点：

// fem_pos_deviation_osqp_interface.cpp: 180-199行
// 对称于起点的处理
P[n–2,n–2] = 5*weight_fem + 2*weight_length + weight_ref
P[n–1,n–1] = weight_fem + weight_length + weight_ref

推导过程

以中间点

p

2

p_2

p2​ 为例（6点系统中的第3个点）：

FEM项：

J

f

e

m

=

w

X

[

(

p

0

−

2

p

1

+

p

2

)

2

+

(

p

1

−

2

p

2

+

p

3

)

2

+

(

p

2

−

2

p

3

+

p

4

)

2

]

(28)

J_{fem} = w_X \\left[ (p_0 – 2p_1 + p_2)^2 + (p_1 – 2p_2 + p_3)^2 + (p_2 – 2p_3 + p_4)^2 \\right] \\tag{28}

Jfem​=wX​[(p0​−2p1​+p2​)2+(p1​−2p2​+p3​)2+(p2​−2p3​+p4​)2](28)

对

p

2

p_2

p2​ 求二阶偏导：

∂

2

J

f

e

m

∂

p

2

2

=

w

X

(

1

+

4

+

1

)

=

6

w

X

(29)

\\frac{\\partial^2 J_{fem}}{\\partial p_2^2} = w_X(1 + 4 + 1) = 6w_X \\tag{29}

∂p22​∂2Jfem​​=wX​(1+4+1)=6wX​(29)

∂

2

J

f

e

m

∂

p

2

∂

p

1

=

w

X

(

−

2

−

2

)

=

−

4

w

X

(30)

\\frac{\\partial^2 J_{fem}}{\\partial p_2 \\partial p_1} = w_X(-2 – 2) = -4w_X \\tag{30}

∂p2​∂p1​∂2Jfem​​=wX​(−2−2)=−4wX​(30)

长度项：

J

l

e

n

g

t

h

=

w

Y

[

(

p

2

−

p

1

)

2

+

(

p

3

−

p

2

)

2

]

(31)

J_{length} = w_Y \\left[ (p_2 – p_1)^2 + (p_3 – p_2)^2 \\right] \\tag{31}

Jlength​=wY​[(p2​−p1​)2+(p3​−p2​)2](31)

对

p

2

p_2

p2​ 求二阶偏导：

∂

2

J

l

e

n

g

t

h

∂

p

2

2

=

2

w

Y

(32)

\\frac{\\partial^2 J_{length}}{\\partial p_2^2} = 2w_Y \\tag{32}

∂p22​∂2Jlength​​=2wY​(32)

∂

2

J

l

e

n

g

t

h

∂

p

2

∂

p

1

=

−

w

Y

(33)

\\frac{\\partial^2 J_{length}}{\\partial p_2 \\partial p_1} = -w_Y \\tag{33}

∂p2​∂p1​∂2Jlength​​=−wY​(33)

总Hessian（

p

2

p_2

p2​ 对应的块）：

P

22

=

6

w

X

+

2

w

Y

+

w

Z

(34)

\\mathbf{P}_{22} = 6w_X + 2w_Y + w_Z \\tag{34}

P22​=6wX​+2wY​+wZ​(34)

P

21

=

−

4

w

X

−

w

Y

(35)

\\mathbf{P}_{21} = -4w_X – w_Y \\tag{35}

P21​=−4wX​−wY​(35)

7. 可视化示意图

7.1 有限元估计示意图

原始路径（有锯齿）：

p₂ ●
/ \\
/ \\
/ \\
● ●
p₁ p₃

FEM估计：中点应该在p₁和p₃的中间

p̂₂ = (p₁ + p₃)/2

p₂ ● ← 实际位置
|
| ← 偏差 e₂
|
p̂₂ ● ← FEM估计位置
/ \\
/ \\
●──────●
p₁ p₃

平滑后的路径：

●
/ \\
/ \\ ← 更平滑的曲线
/ \\
● ●

7.2 三种代价的几何意义

(a) FEM代价 (b) 长度代价 (c) 参考偏差代价

● ● ●ref
\\ \\ |
\\ ← 曲率大 \\ ← 路径长 | ← 偏移
● ●―――――――● ●actual
/ 代价高 代价高 代价高
●

优化目标：平衡三者

●―――●―――● ← 太平滑但偏离原路径且过长

●⌒●⌒● ← 跟随原路径但不平滑

●～●～● ← 最优：平滑且不过度偏离

8. 代码实现对照

8.1 Hessian矩阵构造

理论公式：

P

i

i

=

6

w

f

e

m

+

2

w

l

e

n

g

t

h

+

w

r

e

f

(36)

\\mathbf{P}_{ii} = 6w_{fem} + 2w_{length} + w_{ref} \\tag{36}

Pii​=6wfem​+2wlength​+wref​(36)

对应代码：

// fem_pos_deviation_osqp_interface.cpp: 172-173行
columns[col_index].emplace_back(
col_index, 6.0 * weight_fem_pos_deviation_ +
2.0 * weight_path_length_ + weight_ref_deviation_);

8.2 线性项计算

理论公式：

q

i

=

−

2

w

r

e

f

⋅

p

i

r

e

f

(37)

q_i = -2 w_{ref} \\cdot p_i^{ref} \\tag{37}

qi​=−2wref​⋅piref​(37)

对应代码：

// fem_pos_deviation_osqp_interface.cpp: 218-219行
q->push_back(–2.0 * weight_ref_deviation_ * ref_point_xy.first);
q->push_back(–2.0 * weight_ref_deviation_ * ref_point_xy.second);

8.3 约束条件

理论公式：

p

i

r

e

f

−

b

i

≤

p

i

≤

p

i

r

e

f

+

b

i

(38)

p_i^{ref} – b_i \\leq p_i \\leq p_i^{ref} + b_i \\tag{38}

piref​−bi​≤pi​≤piref​+bi​(38)

对应代码：

// fem_pos_deviation_osqp_interface.cpp: 238-241行
upper_bounds->push_back(ref_point_xy.first + bounds_around_refs_[i]);
upper_bounds->push_back(ref_point_xy.second + bounds_around_refs_[i]);
lower_bounds->push_back(ref_point_xy.first – bounds_around_refs_[i]);
lower_bounds->push_back(ref_point_xy.second – bounds_around_refs_[i]);

8.4 归一化处理

理论意义：

将坐标原点移到起点，提高数值稳定性（避免处理很大的坐标值）。

对应代码：

// discrete_points_reference_line_smoother.cpp: 112-127行
void NormalizePoints(std::vector<std::pair<double, double>>* xy_points) {
zero_x_ = xy_points->front().first; // 记录起点x
zero_y_ = xy_points->front().second; // 记录起点y

// 所有点减去起点坐标
std::for_each(xy_points->begin(), xy_points->end(),
[this](std::pair<double, double>& point) {
point.first -= zero_x_; // x相对化
point.second -= zero_y_; // y相对化
});
}