在之前的博客中我们提到了Apollo OSQP Solver构造二次规划问题的形式, 该种构造方式称为Non-Condensed Format,简单的来说, 我们在构造二次规划的优化问题的时候将输入量和状态量一起当做决策变量和, 之前的仅仅把状态变量当做决策变量的形式叫做Condensed Format. Apollo在构造二次规划问题的时候是没有考虑对控制的增量进行惩罚的, 如果我们希望输入更加平滑,需要对控制的变化率进行惩罚, 另外我们把终端状态约束单独提出来, 我们需要使用如下优化目标函数:
arg min x , u ( x N − x r e f ) T Q x N ( x N − x r e f ) ⏞ J x N + ∑ k = 0 N p − 1 ( x − x r e f ) T Q x ( x k − x r e f ) ⏞ J x + ∑ k = 0 N p − 1 ( u − u r e f ) T Q u ( u k − u r e f ) ⏞ J u + ∑ k = 0 N p − 1 Δ u k T Q Δ u Δ u k ⏞ J Δ u s u b j e c t t o u m i n ≤ u k ≤ u m a x x m i n ≤ x k ≤ x m a x Δ u m i n ≤ Δ u k ≤ Δ u m a x x 0 = x ˉ u − 1 = u ˉ \arg \min_{x,u} \overbrace{(x_N-x_{ref})^{T}Q_{x_N}(x_N-x_{ref})}^{J_{x_N}} + \overbrace{\sum_{k=0}^{N_{p}-1}(x - x_{ref})^{T}Q_{x}(x_{k} - x_{ref})}^{J_{x}} + \\ \overbrace{\sum_{k=0}^{N_{p}-1}(u - u_{ref})^{T}Q_{u}(u_{k} - u_{ref})}^{J_{u}} +\\ \overbrace{\sum_{k=0}^{N_{p} -1} \Delta u_k^{T}Q_{\Delta u} \Delta u_k}^{J_{\Delta u}} \\ subject~to \\ u_{min} \leq u_k \leq u_{max}\\ x_{min} \leq x_k \leq x_{max} \\ \Delta u_{min} \leq \Delta u_k \leq \Delta u_{max} \\ x_0 = \bar x\\ u_{-1} = \bar u argx,umin(xN−xref)TQxN(xN−xref)
JxN+k=0∑Np−1(x−xref)TQx(xk−xref)
Jx+k=0∑Np−1(u−uref)TQu(uk−uref)
Ju+k=0∑Np−1ΔukTQΔuΔuk
JΔusubject toumin≤uk≤umaxxmin≤xk≤xmaxΔumin≤Δuk≤Δumaxx0=xˉu−1=uˉ
其中 Δ u k = u k − u k − 1 \Delta u_k = u_k - u_{k-1} Δuk=uk−uk−1,
如果我们同时选取状态量和输入量作为优化问题的决策变量, 我们优化问题的决策变量为:
x = [ x 0 x 1 … x N p ] , u = [ u 0 u 1 … u N p − 1 ] x = \begin{bmatrix} x_0 & x_1 & \dots & x_{N_{p}} \end{bmatrix},~ u = \begin{bmatrix} u_0 & u_1 & \dots & u_{N_{p-1}} \end{bmatrix} x=[x0x1…xNp], u=[u0u1…uNp−1]
我们需要将上述优化问题转换为标准的二次规划形式
min 1 2 x T P x + q T x s u b j e c t t o l ≤ A x ≤ u \min \frac{1}{2} x^{T} P x + q^{T} x \\ subject~to ~l \leq Ax \leq u min21xTPx+qTxsubject to l≤Ax≤u
目标函数部分转化
下面我们需要将优化函数的下述三个部分转换成二次规划的形式
J x = 1 2 [ x 0 T x 1 T … x N p − 1 T ] [ Q x 0 0 … … 0 0 Q x 0 0 … 0 0 0 ⋱ ⋱ ⋱ 0 0 0 ⋱ ⋱ ⋱ 0 0 0 ⋱ ⋱ ⋱ 0 0 0 0 0 Q x 0 0 0 0 0 0 Q x ] [ x 0 x 1 ⋮ x N p − 1 ] + [ − x r e f T Q x − x r e f T Q x … − x r e f T Q x ] [ x 0 x 1 ⋮ x N p − 1 ] J_{x} = \dfrac{1}{2} \begin{bmatrix} x_{0}^{T}&x_{1}^{T}&\dots&x_{N_{p} - 1}^{T} \end{bmatrix} \begin{bmatrix} Q_{x} &0 &0 & \dots & \dots & 0\\ 0 & Q_{x} &0 &0 & \dots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &\ddots & \ddots &\ddots & 0\\ 0 & 0 &0 &0 & Q_{x} &0\\ 0 & 0 &0 &0 & 0&Q_{x}\\ \end{bmatrix} \begin{bmatrix} x_{0} \\ x_{1}\\ \vdots \\ x_{N_{p} - 1} \end{bmatrix} + \\ \begin{bmatrix} -x_{ref}^{T}Q_{x}&-x_{ref}^{T}Q_{x}&\dots&-x_{ref}^{T}Q_{x} \end{bmatrix} \begin{bmatrix} x_{0} \\ x_{1} \\ \vdots \\ x_{N_{p}-1} \end{bmatrix} Jx=21[x0Tx1T…xNp−1T]⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡Qx0000000Qx0000000⋱⋱⋱00…0⋱⋱⋱00……⋱⋱⋱Qx0000000Qx⎦⎥⎥⎥⎥
最低0.47元/天 解锁文章
1282
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
