代价函数
共由三部分组成:
平滑代价
长度代价
偏移代价
平滑代价
为三个点组成的两个向量之差模长,越短越平滑。
c
o
s
t
1
=
∑
i
=
1
N
−
1
(
(
x
i
−
1
+
x
i
+
1
−
2
x
i
)
2
+
(
y
i
−
1
+
y
i
+
1
−
2
y
i
)
2
)
=
[
x
i
−
1
y
i
−
1
x
i
y
i
x
i
+
1
y
i
+
1
]
⊤
[
1
0
−
2
0
1
0
0
1
0
−
2
0
1
−
2
0
4
0
−
2
0
0
−
2
0
4
0
−
2
1
0
−
2
0
1
0
0
1
0
−
2
0
1
]
[
x
i
−
1
y
i
−
1
x
i
y
i
x
i
+
1
y
i
+
1
]
\begin{aligned}cost_1={}&\sum_{i=1}^{N-1}((x_{i-1}+x_{i+1}-2x_i)^2+(y_{i-1}+y_{i+1}-2y_i)^2)\\ ={}&\left[\begin{matrix}x_{i-1}\\y_{i-1}\\x_{i}\\y_{i}\\x_{i+1}\\y_{i+1}\end{matrix}\right]^\top\left[\begin{matrix} 1&0&-2&0&1&0\\ 0&1&0&-2&0&1\\ -2&0&4&0&-2&0\\ 0&-2&0&4&0&-2\\ 1&0&-2&0&1&0\\ 0&1&0&-2&0&1\\ \end{matrix}\right]\left[\begin{matrix}x_{i-1}\\y_{i-1}\\x_{i}\\y_{i}\\x_{i+1}\\y_{i+1}\end{matrix}\right] \end{aligned}
cost1==i=1∑N−1((xi−1+xi+1−2xi)2+(yi−1+yi+1−2yi)2)
xi−1yi−1xiyixi+1yi+1
⊤
10−2010010−201−2040−200−2040−210−2010010−201
xi−1yi−1xiyixi+1yi+1
长度代价
c o s t 2 = ∑ i = 0 N − 1 ( ( x i − x i + 1 ) 2 + ( y i − y i + 1 ) 2 ) = [ x i y i x i + 1 y i + 1 ] ⊤ [ 1 0 − 1 0 0 1 0 − 1 − 1 0 1 0 0 − 1 0 1 ] [ x i y i x i + 1 y i + 1 ] \begin{aligned}cost_2={}&\sum_{i=0}^{N-1}((x_{i}-x_{i+1})^2+(y_{i}-y_{i+1})^2)\\ ={}&\left[\begin{matrix}x_{i}\\y_{i}\\x_{i+1}\\y_{i+1}\end{matrix}\right]^\top \left[\begin{matrix} 1&0&-1&0\\ 0&1&0&-1\\ -1&0&1&0\\ 0&-1&0&1 \end{matrix}\right]\left[\begin{matrix}x_{i}\\y_{i}\\x_{i+1}\\y_{i+1}\end{matrix}\right] \end{aligned} cost2==i=0∑N−1((xi−xi+1)2+(yi−yi+1)2) xiyixi+1yi+1 ⊤ 10−10010−1−10100−101 xiyixi+1yi+1
偏移代价
c o s t 2 = ∑ i = 0 N ( ( x i − x i r e f ) 2 + ( y i − y i r e f ) 2 ) = [ x i y i ] ⊤ [ 1 0 0 1 ] [ x i y i ] + 2 [ − x i r e f − y i r e f ] [ x i y i ] + c o n s t \begin{aligned}cost_2=\sum_{i=0}^{N}((x_{i}-x_{i\mathrm{ref}})^2+(y_{i}-y_{i\mathrm{ref}})^2) ={}&\left[\begin{matrix}x_{i}\\y_{i}\end{matrix}\right]^\top\left[\begin{matrix} 1&0\\ 0&1 \end{matrix}\right]\left[\begin{matrix}x_{i}\\y_{i}\end{matrix}\right]+2\left[\begin{matrix} -x_{i\mathrm{ref}}\\ -y_{i\mathrm{ref}} \end{matrix}\right]\left[\begin{matrix}x_{i}\\y_{i}\end{matrix}\right]+const \end{aligned} cost2=i=0∑N((xi−xiref)2+(yi−yiref)2)=[xiyi]⊤[1001][xiyi]+2[−xiref−yiref][xiyi]+const
总代价
c
o
s
t
=
1
2
∑
i
=
1
3
w
i
c
o
s
t
i
cost = \frac{1}{2}\sum_{i=1}^3w_icost_i
cost=21i=1∑3wicosti
w
i
w_i
wi是权重。
约束
位置约束(偏移约束)
x i ∈ [ x i r e f − x l , x i r e f + x u ] y i ∈ [ y i r e f − y l , y i r e f + y u ] \begin{aligned}x_i&\in[x_{i\mathrm{ref}}-x_l, x_{i\mathrm{ref}}+x_u]\\y_i&\in[y_{i\mathrm{ref}}-y_l, y_{i\mathrm{ref}}+y_u]\end{aligned} xiyi∈[xiref−xl,xiref+xu]∈[yiref−yl,yiref+yu]
曲率约束
如图,假设
P
i
,
i
=
0
,
1
,
2
P_i,i=0,1,2
Pi,i=0,1,2三点位于同一个圆上,当曲率越小弧长采样越小时,
θ
1
\theta_1
θ1较小,且此时弧长和弦长近似,有
θ
1
=
d
s
R
\theta_1=\frac{\mathrm{d}s}{R}
θ1=Rds
根据等腰三角形
O
P
0
P
1
OP_0P_1
OP0P1的几何关系有
θ
2
=
π
−
θ
1
2
\theta_2=\frac{\pi-\theta_1}{2}
θ2=2π−θ1
由于相邻边长相等,故平行四边形
P
0
P
1
P
2
P
3
P_0P_1P_2P_3
P0P1P2P3是菱形,因此有
∣
P
1
P
3
→
∣
=
2
∣
P
1
C
→
∣
=
2
d
s
cos
θ
2
=
2
d
s
sin
θ
1
2
=
d
s
2
R
=
d
s
2
κ
\begin{aligned}|\overrightarrow{P_1P_3}|=2|\overrightarrow{P_1C}|=2\mathrm{d}s\cos\theta_2=2\mathrm{d}s\sin\frac{\theta_1}{2}=\frac{\mathrm{d}s^2}{R}=\mathrm{d}s^2\kappa\end{aligned}
∣P1P3∣=2∣P1C∣=2dscosθ2=2dssin2θ1=Rds2=ds2κ
因此曲率约束
(
x
i
−
1
+
x
i
+
1
−
2
x
i
)
2
+
(
y
i
−
1
+
y
i
+
1
−
2
y
i
)
2
≤
(
d
s
2
κ
m
a
x
)
2
,
i
=
1
,
⋯
,
N
−
1
(x_{i-1}+x_{i+1}-2x_i)^2+(y_{i-1}+y_{i+1}-2y_i)^2\leq (\mathrm{d}s^2\kappa_\mathrm{max})^2, i=1,\cdots, N-1
(xi−1+xi+1−2xi)2+(yi−1+yi+1−2yi)2≤(ds2κmax)2,i=1,⋯,N−1
约束虽然有了,但不是仿射形式,需要线性化。令
f
i
=
(
x
i
−
1
+
x
i
+
1
−
2
x
i
)
2
+
(
y
i
−
1
+
y
i
+
1
−
2
y
i
)
2
X
i
=
[
x
i
−
1
y
i
−
1
x
i
y
i
x
i
+
1
y
i
+
1
]
⊤
,
i
=
1
,
⋯
,
N
\begin{aligned}f_i&=(x_{i-1}+x_{i+1}-2x_i)^2+(y_{i-1}+y_{i+1}-2y_i)^2\\ X_i&=\left[\begin{matrix}x_{i-1}&y_{i-1}&x_i&y_i&x_{i+1}&y_{i+1}\end{matrix}\right]^\top, i=1,\cdots, N\end{aligned}
fiXi=(xi−1+xi+1−2xi)2+(yi−1+yi+1−2yi)2=[xi−1yi−1xiyixi+1yi+1]⊤,i=1,⋯,N
一阶泰勒
f
i
=
f
(
X
i
r
e
f
)
+
f
′
(
X
i
r
e
f
)
(
X
i
−
X
i
r
e
f
)
+
⋯
,
i
=
1
,
⋯
,
N
−
1
f_i=f(X_{i\mathrm{ref}})+f'(X_{i\mathrm{ref}})(X_i-X_{i\mathrm{ref}})+\cdots, i=1,\cdots, N-1
fi=f(Xiref)+f′(Xiref)(Xi−Xiref)+⋯,i=1,⋯,N−1
其中
f
′
(
X
i
)
=
[
2
(
x
i
−
1
+
x
i
+
1
−
2
x
i
)
2
(
y
i
−
1
+
y
i
+
1
−
2
y
i
)
−
4
(
x
i
−
1
+
x
i
+
1
−
2
x
i
)
−
4
(
y
i
−
1
+
y
i
+
1
−
2
y
i
)
2
(
x
i
−
1
+
x
i
+
1
−
2
x
i
)
2
(
y
i
−
1
+
y
i
+
1
−
2
y
i
)
]
⊤
f'(X_{i})=\left[\begin{matrix}2(x_{i-1}+x_{i+1}-2x_i)\\ 2(y_{i-1}+y_{i+1}-2y_i)\\-4(x_{i-1}+x_{i+1}-2x_i)\\-4(y_{i-1}+y_{i+1}-2y_i)\\2(x_{i-1}+x_{i+1}-2x_i)\\2(y_{i-1}+y_{i+1}-2y_i)\end{matrix}\right]^\top
f′(Xi)=
2(xi−1+xi+1−2xi)2(yi−1+yi+1−2yi)−4(xi−1+xi+1−2xi)−4(yi−1+yi+1−2yi)2(xi−1+xi+1−2xi)2(yi−1+yi+1−2yi)
⊤
带入曲率约束方程,有
f
′
(
X
i
r
e
f
)
X
i
≤
(
d
s
2
κ
m
a
x
)
2
−
f
(
X
i
r
e
f
)
+
f
′
(
X
i
r
e
f
)
X
i
r
e
f
,
i
=
1
,
⋯
,
N
−
1
f'(X_{i\mathrm{ref}})X_i\leq (\mathrm{d}s^2\kappa_{\mathrm{max}})^2-f(X_{i\mathrm{ref}})+f'(X_{i\mathrm{ref}})X_{i\mathrm{ref}}, i=1,\cdots, N-1
f′(Xiref)Xi≤(ds2κmax)2−f(Xiref)+f′(Xiref)Xiref,i=1,⋯,N−1