QP优化方法(凸优化问题)

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+r00,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} P0S+n,i=0,1...m,ARp×n

在无约束的情况下,最小二乘问题即二次规划QP问题: m i n i m i z e ∣ ∣ A x − b ∣ ∣ 2 2 minimize ||Ax-b||_2^2 minimizeAxb22
其解析解为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+ai1s+ai2s2+ai3s3+ai4s4++ai5s5,(0sdi)然后定义每个样条段的优化目标函数: 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=1n(w10di(fi)2(s)ds+w20di(fi)2(s)ds+w30di(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.21xTHx+fTxLBxUBAeqx=beqAxb

2.约束条件
a) 初始点约束
b) 终点约束
c) 平滑节点约束
d) 点采样边界约束

  • 4
    点赞
  • 17
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

秃头队长

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值