QP问题为二次规划问题: m i n i m i z e : 1 2 x T P 0 x + q 0 T x + r 0 minimize:{1 \over 2}x^TP_0x+q_0^Tx+r_0 minimize:21xTP0x+q0Tx+r0 s u b j c e t t o : q 0 T x + r 0 ≤ 0 , i = 1 , 2... m subjcet to: q_0^Tx+r_0 \leq 0,i=1,2...m subjcetto:q0Tx+r0≤0,i=1,2...m A x = b Ax=b Ax=b P 0 ∈ S + n , i = 0 , 1... m , A ∈ R p × n P_0 \in S_+^n,i=0,1...m,A \in R^{p \times n} P0∈S+n,i=0,1...m,A∈Rp×n
在无约束的情况下,最小二乘问题即二次规划QP问题:
m
i
n
i
m
i
z
e
∣
∣
A
x
−
b
∣
∣
2
2
minimize ||Ax-b||_2^2
minimize∣∣Ax−b∣∣22
其解析解为A的伪逆,具体来说是其左逆。
所谓伪逆是对矩阵逆的一种推广,满足一定性质的矩阵都可以成为矩阵A的伪逆,对于欠定方程组而言,使用伪逆可以给出最小范数解,使用QR方法则只是给出无数个可行解中的一个!
对于一个A的转置矩阵 A ′ A' A′同型的矩阵X,并且满足: A X A = A , X A X = X AXA=A,XAX=X AXA=A,XAX=X,此时称矩阵X为矩阵A的伪逆,也称广义逆矩阵。
二次规划(QP)样条路径优化(参考apollo github文档)
首先明确个基本问题:1.目标函数 2.约束条件!
1.目标函数
路径
s
s
s定义在station-lateral坐标系中,
s
s
s的变化区间从车辆的当前位置点到默认路径的长度。
将路径
s
s
s划分为
n
n
n样条段
i
i
i,每段路径
l
l
l用一个多项式(默认5阶)来表示,每个样条段
i
i
i都有沿着参考线的累加距离
d
i
d_i
di:
l
=
f
i
(
s
)
=
a
i
0
+
a
i
1
∗
s
+
a
i
2
∗
s
2
+
a
i
3
∗
s
3
+
a
i
4
∗
s
4
+
+
a
i
5
∗
s
5
,
(
0
≤
s
≤
d
i
)
l=f_i(s)=a_{i0}+a_{i1}*s+a_{i2}*s^2+a_{i3}*s^3+a_{i4}*s^4++a_{i5}*s^5,(0 \leq s \leq d_i)
l=fi(s)=ai0+ai1∗s+ai2∗s2+ai3∗s3+ai4∗s4++ai5∗s5,(0≤s≤di)然后定义每个样条段的优化目标函数:
c
o
s
t
=
∑
i
=
1
n
(
w
1
⋅
∫
0
d
i
(
f
i
′
)
2
(
s
)
d
s
+
w
2
⋅
∫
0
d
i
(
f
i
′
′
)
2
(
s
)
d
s
+
w
3
⋅
∫
0
d
i
(
f
i
′
′
′
)
2
(
s
)
d
s
)
cost = \sum_{i=1}^{n} \Big( w_1 \cdot \int\limits_{0}^{d_i} (f_i')^2(s) ds + w_2 \cdot \int\limits_{0}^{d_i} (f_i'')^2(s) ds + w_3 \cdot \int\limits_{0}^{d_i} (f_i^{\prime\prime\prime})^2(s) ds \Big)
cost=i=1∑n(w1⋅0∫di(fi′)2(s)ds+w2⋅0∫di(fi′′)2(s)ds+w3⋅0∫di(fi′′′)2(s)ds)最后,将cost函数转换为QP公式:
m
i
n
i
m
i
z
e
1
2
⋅
x
T
⋅
H
⋅
x
+
f
T
⋅
x
s
.
t
.
L
B
≤
x
≤
U
B
A
e
q
x
=
b
e
q
A
x
≥
b
\begin{aligned} minimize & \frac{1}{2} \cdot x^T \cdot H \cdot x + f^T \cdot x \\ s.t. \qquad & LB \leq x \leq UB \\ & A_{eq}x = b_{eq} \\ & Ax \geq b \end{aligned}
minimizes.t.21⋅xT⋅H⋅x+fT⋅xLB≤x≤UBAeqx=beqAx≥b
2.约束条件
a) 初始点约束
b) 终点约束
c) 平滑节点约束
d) 点采样边界约束