Directorio de artículos
1. Modelado de problemas
Supongamos que una trayectoria de referencia se conoce como r 0 , r 1 , . . . , ri , . . . , rn r_0,r_1,...,r_i,...,r_nr0,r1,. . . ,ryo,. . . ,rnorte, esta trayectoria se puede generar arbitrariamente, porque puede haber puntos de inflexión en ángulo recto o puntos de inflexión en ángulo agudo, y el robot no puede moverse directamente a lo largo de esta trayectoria. Por lo tanto, es necesario generar una trayectoria práctica y factible x 0 , x 1 , . . . , xi , . . . xn x_0,x_1,...,x_i,...x_n que satisfaga las restricciones cinemáticas del robot. y quede lo más suave posible.X0,X1,. . . ,Xyo,. . . Xnorte, esta también es la variable que necesitamos optimizar, aquí la trayectoria { ri } \{r_i\}{ ryo} suma{ xi } \{x_i\}{ xyo} tienen la misma longitud. A partir de esto se establece la siguiente función de costos (función de costos \textbf{función de costos}función de costo ):
J = wx ∑ i = 0 n − 1 ( xi − ri ) 2 + wx ′ ∑ i = 0 n − 1 ( xi ′ ) 2 + wx ′ ′ ∑ i = 0 n − 1 ( xi ′ ′ ) 2 + wx ′ ′ ′ ∑ i = 0 n − 1 ( xi ′ ′ ′ ) 2 \mathcal{J}=w_x\sum^{n-1}_{i=0}(x_i-r_i)^2+w_{x' }\sum^{n-1}_{i=0}(x'_i)^2+w_{x''}\sum^{n-1}_{i=0}(x''_i)^ 2+w_{x'''}\sum^{n-1}_{i=0}(x''_i)^2j=wxyo = 0∑norte − 1( xyo−ryo)2+wX′yo = 0∑norte − 1( xi′)2+wX′ ′yo = 0∑norte − 1( xi′ ′)2+wX′ ′ ′yo = 0∑norte − 1( xi′ ′ ′)2
- wx ∑ i = 0 n − 1 ( xi − ri ) 2 w_x\sum^{n-1}_{i=0}(x_i-r_i)^2wx∑yo = 0norte − 1( xyo−ryo)2 : Minimizar la distancia entre la trayectoria generada y la trayectoria de referencia, es decir, hacer que la trayectoria generada siga la trayectoria de referencia tanto como sea posible, dondewx w_xwxes el peso.
- wx ′ ∑ i = 0 n − 1 ( xi ′ ) 2 w_{x'}\sum^{n-1}_{i=0}(x'_i)^2wX′∑yo = 0norte − 1( xi′)2 : Minimizar la primera derivada de la trayectoria, dondewx ′ w_x'wX′es el peso. En términos sencillos, { x } \{x\} Cuando { x } es una trayectoria, { x ′ } \{x'\}{ x′ }es la velocidad, y el propósito de este término es minimizar la velocidad. Al igual que al conducir, una línea recta es la trayectoria más corta y consume menos energía, girar el volante aleatoriamente distorsionará la trayectoria y aumentará el consumo de energía y el tiempo de movimiento.
- wx ∑ i = 0 n − 1 ( xi − ri ) 2 w_x\sum^{n-1}_{i=0}(x_i-r_i)^2wx∑yo = 0norte − 1( xyo−ryo)2 : Minimizar la segunda derivada de la trayectoria, dondewx ′ ′ w_x''wX′ ′es el peso. En términos sencillos, significa minimizar la aceleración.
- wx ∑ i = 0 n − 1 ( xi − ri ) 2 w_x\sum^{n-1}_{i=0}(x_i-r_i)^2wx∑yo = 0norte − 1( xyo−ryo)2 : Minimizar la tercera derivada de la trayectoria, dondewx ′ ′ ′ w_x'''wX′ ′ ′es el peso. En términos sencillos, significa minimizar el tirón (el término técnico debería llamarse $\textbf{Jerk}).
2. problema de calidad
El formato común es el siguiente:
min 1 2 x TP x + qxl ≤ A x ≤ u \begin{aligned} min \quad \frac{1}{2} x^T\mathcal{P}x+\mathcal{q} x \\ l\leq Ax \leq u \end{alineado}min _ _21XTPx __+qx _yo≤una x≤tu
donde P{P}P es unnxn nxnmatriz n x n , xxx esnortevector n- dimensional, qqq esmxn mxnmatriz m x n .
¿Por qué convertir al formato QP?
Debido a que el problema QP debe tener solución y actualmente varios solucionadores admiten la resolución de problemas QP, la velocidad es mucho más rápida que la de los problemas de optimización no lineal.
3. Conversión de formato
A continuación, convertimos nuestro problema al formato QP, principalmente para construir P \mathcal{P}Matriz P y q \mathcal{q}Matriz q , el siguiente contenido se refiere principalmente alcontrol de planificación de este artículo:Implementación en Python de Piecewise Jerk Path Optimizer. Aquí, el artículo de referencia unidimensional se expande a bidimensional, es decir,x = { x 0 , x 1 , . . . , xn } x=\{x_0,x_1, ..., x_n\}X={ x0,X1,. . . ,Xnorte} , en el quexi = { pxi , pyi , vxi , vyi , axi , ayi } x_i=\{p_{xi},p_{yi},v_{xi},v_{yi},a_{xi}, a_{ yy}\}Xyo={ pagxyo _,pagsi si,vxyo _,vsi si,axyo _,asi si} , correspondientes respectivamente axy xyColoque px p_xeje x ypagx或py p_ypagy, velocidad vx v_xvx或vy v_yvyy aceleración ax a_xax或 a y a_y ay。
Para simplificar la escala del problema QP, se supone aquí que la variable de optimización xxx contienenorten estados (correspondientes a la trayectoria de referenciarrla longitud de r ), y satisface la siguiente relación
- xi ′ = xi + 1 − xi Δ t x'_i=\frac{x_{i+1}-x_i}{\Delta{t}}Xi′=Δt _Xyo + 1−x _yo
- xi ′ ′ = xi + 1 ′ − xi ′ Δ t x''_i=\frac{x'_{i+1}-x'_i}{\Delta{t}}Xi′ ′=Δt _Xyo + 1′−x _i′
- xi ′ ′ ′ = xi + 1 ′ ′ − xi ′ ′ Δ t x''''_i=\frac{x''_{i+1}-x''_i}{\Delta{t}}Xi′ ′ ′=Δt _Xyo + 1′ ′−x _i′ ′
Coloque la última fórmula anterior en la función de costo J \mathcal{J}En J , lo general se puede ampliar:
J = wx ∑ i = 0 n − 1 [ ( xi ) 2 + ( ri ) 2 − 2 xiri ] + wx ′ ∑ i = 0 n − 1 ( xi ′ ) 2 + wx ′ ′ ∑ i = 0 n − 1 ( xi ′ ′ ) 2 + wx ′ ′ ′ Δ s 2 ∑ i = 0 n − 2 [ ( xi + 1 ′ ′ ) 2 + ( xi ′ ′ ) 2 − 2 xi + 1 ′ xi ′ ′ ] \mathcal{J}=w_x\sum^{n-1}_{i=0}[(x_i)^2+(r_i)^2-2x_ir_i]+w_{x'} \sum ^{n-1}_{i=0}(x'_i)^2+w_{x''}\sum^{n-1}_{i=0}(x''_i)^2 +\ frac{w_{x'''}}{\Delta s^2}\sum^{n-2}_{i=0}[(x''_{i+1})^2+(x '' _i)^2-2x''_{i+1}x''_i]j=wxyo = 0∑norte − 1[ ( xyo)2+( ryo)2−2x _yoryo]+wX′yo = 0∑norte − 1( xi′)2+wX′ ′yo = 0∑norte − 1( xi′ ′)2+Δs_ _2wX′ ′ ′yo = 0∑norte − 2[ ( xyo + 1′ ′)2+( xi′ ′)2−2x _yo + 1′ ′Xi′ ′]
Eliminar los términos constantes (y las variables de optimización xxx términos irrelevantes) se puede obtener:
J = wx ∑ i = 0 n − 1 [ ( xi ) 2 − 2 xiri ] + wx ′ ∑ i = 0 n − 1 ( xi ′ ) 2 + ( wx ′ ′ + 2 wx ′ ′ ′ Δ s 2 ) ∑ i = 0 n − 1 ( xi ′ ′ ) 2 − wx ′ ′ ′ Δ s 2 ( x 0 ′ ′ + xn − 1 ′ ′ ) − 2 wx ′ ′ ′ Δ s 2 ∑ i = 0 n − 2 ( xi + 1 ′ ′ xi ′ ′ ) \mathcal{J}=w_x\sum^{n-1}_{i=0}[(x_i)^2-2x_ir_i]+w_{x'}\sum^{n- 1}_{i=0}(x'_i)^2+(w_{x''}+\frac{2w_{x'''}}{\Delta s^2})\sum^{n-1 }_{i=0}(x''_i)^2-\frac{w_{x''}}{\Delta s^2}(x''_0+x''_{n-1}) -\frac{2w_{x'''}}{\Delta s^2}\sum^{n-2}_{i=0}(x''_{i+1}x''_i)j=wxyo = 0∑norte − 1[ ( xyo)2−2x _yoryo]+wX′yo = 0∑norte − 1( xi′)2+( wX′ ′+Δs_ _22 semanas _X′ ′ ′)yo = 0∑norte − 1( xi′ ′)2−Δs_ _2wX′ ′ ′( x0′ ′+Xnorte - 1′ ′)−Δs_ _22 semanas _X′ ′ ′yo = 0∑norte − 2( xyo + 1′ ′Xi′ ′)
其中
x = [ x 0 x 1 ⋮ xn − 1 x 0 ′ x 1 ′ ⋮ xn − 1 ′ x 0 ′ ′ x 1 ′ ′ ⋮ xn − 1 ′ ′ ] = [ px 0 xx 1 ⋮ pxn − 1 py 0 py 1 ⋮ pyn − 1 vx 0 vx 1 ⋮ vxn − 1 vy 0 vy 1 ⋮ vyn − 1 ax 0 ax 1 ⋮ axn − 1 ay 0 ay 1 ⋮ ayn − 1 ] ∈ R 6 n × 1 x=\begin{ bmatrix} x_0 \\[4pt] x_1\\[4pt]\vdots \\[4pt]x_{n-1}\\[4pt]x'_0\\[4pt]x'_1\\[4pt]\vdots \\[4pt]x'_{n-1}\\[4pt]x''_0\\[4pt]x''_1\\[4pt]\vdots\\[4pt]x''_{n- 1} \end{bmatrix} = \begin{bmatrix} p_{x_0} \\[4pt] x_{x_1}\\[4pt]\vdots \\[4pt]p_{x_{n-1}}\\[ 4pt]p_{y_0}\\[4pt]p_{y_1}\\[4pt]\vdots\\[4pt]p_{y_n-1}\\[4pt]v_{x_0}\\[4pt]v_{x_1 }\\[4pt]\vdots\\[4pt]v_{x_{n-1}}\\[4pt]v_{y_0}\\[4pt]v_{y_1}\\[4pt]\vdots\\[ 4pt]v_{y_{n-1}}\\[4pt]a_{x_0}\\[4pt]a_{x_1}\\[4pt]\vdots\\[4pt]a_{x_{n-1}} \\[4pt]a_{y_0}\\[4pt]a_{y_1}\\[4pt]\vdots\\[4pt]a_{y_{n-1}} \end{bmatrix}\en R^{6n \veces 1}X=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡X0X1⋮Xnorte − 1X0′X1′⋮Xnorte - 1′X0′ ′X1′ ′⋮Xnorte - 1′ ′⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡pagX0XX1⋮pagXnorte − 1pagy0pagy1⋮pagynorte− 1vX0vX1⋮vXnorte − 1vy0vy1⋮vynorte − 1aX0aX1⋮aXnorte − 1ay0ay1⋮aynorte − 1⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤∈R6n × 1 _
Construir PPP、qqqmatriz_ _
P p = [ wx 0 … 0 0 wx … 0 ⋮ ⋮ ⋱ ⋮ 0 0 … wx ] ∈ R n × n ， P v = [ wx ′ 0 … 0 0 wx ′ … 0 ⋮ ⋮ ⋱ ⋮ 0 0 … wx ′ ] ∈ R n × n P_p=\begin{bmatrix} w_x &0& \ldots&0\\0&w_x&\ldots&0\\\vdots&\vdots&\ddots&\vdots\\0&0&\ldots&w_x \end{bmatrix}\in R^{n \times n} ， P_{v}=\begin{bmatrix} w_{x'} &0& \ldots&0\\0&w_{x'}&\ldots&0\\\vdots&\vdots&\ddots&\vdots\\0&0&\ldots&w_{x'} \end{bmatrix}\in R^{n \times n}PAGp=⎣⎢⎢⎢⎡wx0⋮00wx⋮0……⋱…00⋮wx⎦⎥⎥⎥⎤∈Rnorte × norte，PAGv=⎣⎢⎢⎢⎡wX′0⋮00wX′⋮0……⋱…00⋮wX′⎦⎥⎥⎥⎤∈Rnorte × norte
P a = [ wx ′ ′ + wx ′ ′ ′ Δ s 2 0 … 0 0 − 2 wx ′ ′ ′ Δ s 2 wx ′ ′ + 2 wx ′ ′ ′ Δ s 2 … 0 0 ⋮ ⋮ ⋱ ⋮ ⋮ 0 0 … wx ′ ′ + 2 wx ′ ′ ′ Δ s 2 0 0 0 … − 2 wx ′ ′ ′ Δ s 2 wx ′ ′ + wx ′ ′ ′ Δ s 2 ] ∈ R n × n P_ {a}=\begin {bmatriz} w_{x''}+\frac{w_{x'''}}{\Delta s^2} &0& \ldots&0&0\\[8pt]-\frac{2w_{x''}}{\ Delta s^2}&w_{x''}+\frac{2w_{x'''}}{\Delta s^2}&\ldots&0&0\\[8pt]\vdots&\vdots&\ddots&\vdots&\vdots\\ [8pt]0&0&\ldots&w_{x''}+\frac{2w_{x'''}}{\Delta s^2}&0\\[8pt]0&0&\ldots&-\frac{2w_{x'''} }{\Delta s^2}&w_{x''}+\frac{w_{x'''}}{\Delta s^2} \end{bmatrix}\in R^{n \times n}PAGun=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡wX′ ′+Δs_ _2wX′ ′ ′−Δs_ _22 semanas _X′ ′ ′⋮000wX′ ′+Δs_ _22 semanas _X′ ′ ′⋮00……⋱……00⋮wX′ ′+Δs_ _22 semanas _X′ ′ ′−Δs_ _22 semanas _X′ ′ ′00⋮0wX′ ′+Δs_ _2wX′ ′ ′⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤∈Rnorte × norte
P = [ P p P p P v P v P a P a ] ∈ R 6 n × 6 n P=\begin{bmatrix} P_p & & & \\ & P_p \\ && P_v \\ &&& P_v \\ &&&& P_a \\ &&&&& P_a \\ end{bmatrix}\in R^{6n\times 6n}PAG=⎣⎢⎢⎢⎢⎢⎢⎡PAGpPAGpPAGvPAGvPAGunPAGun⎦⎥⎥⎥⎥⎥⎥⎤∈R6norte × 6norte _ _
q = − [ wxpx 0 wxxx 1 ⋮ wxpxn − 1 wxpy 0 wxpy 1 ⋮ wxpyn − 1 wx ′ vx 0 wx ′ vx 1 ⋮ wx ′ vxn − 1 wx ′ vy 0 wx ′ vy 1 ⋮ wx ′ vyn − 1 wx ′ ′ ax 0 wx ′ ′ ax 1 ⋮ wx ′ ′ axn − 1 wx ′ ′ ay 0 wx ′ ′ ay 1 ⋮ wx ′ ′ ayn − 1 ] ∈ R 1 × 6 nq=- \begin{bmatrix} w_xp_{x_0} \\[4pt] w_xx_{x_1}\\[4pt]\vdots \\[4pt]w_xp_{x_{n-1}}\\[4pt]w_xp_{y_0}\\[4pt]w_xp_{y_1}\\ [4pt]\vdots\\[4pt]w_xp_{y_n-1}\\[4pt]w_{x'}v_{x_0}\\[4pt]w_{x'}v_{x_1}\\[4pt]\ vdots\\[4pt]w_{x'}v_{x_{n-1}}\\[4pt]w_{x'}v_{y_0}\\[4pt]w_{x'}v_{y_1}\\ [4pt]\vdots\\[4pt]w_{x'}v_{y_{n-1}}\\[4pt]w_{x''}a_{x_0}\\[4pt]w_{x''} a_{x_1}\\[4pt]\vdots\\[4pt]w_{x''}a_{x_{n-1}}\\[4pt]w_{x''}a_{y_0}\\[4pt ]w_{x''}a_{y_1}\\[4pt]\vdots\\[4pt]w_{x''}a_{y_{n-1}} \end{bmatrix} \in R^{1 \ veces 6n}q=−⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎡wxpagX0wxXX1⋮wxpagXnorte − 1wxpagy0wxpagy1⋮wxpagynorte− 1wX′vX0wX′vX1⋮wX′vXnorte − 1wX′vy0wX′vy1⋮wX′vynorte − 1wX′ ′aX0wX′ ′aX1⋮wX′ ′aXnorte − 1wX′ ′ay0wX′ ′ay1⋮wX′ ′aynorte − 1⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎤∈R1 × 6 norte
Restricciones de igualdad
Para que la trayectoria generada satisfaga el estado de movimiento del robot, el modelo de movimiento del robot debe convertirse en las restricciones de ecuación correspondientes, es decir, l = A x = ul = Ax = uyo=una x=u。这里考虑如下运动模型：
xi + 1 = xi + xi ′ Δ txi + 1 ′ = xi ′ + xi ′ ′ Δ txi + 1 ′ ′ = xi ′ ′ + xi ′ ′ ′ Δ t \begin{aligned} x_{i+1}=x_i + x'_i\Delta t \\ x'_{i+1}=x'_i + x''_i\Delta t \\ x''_{i+1}=x ''_i + x'''_i\Delta t \\ \end{alineado}Xyo + 1=Xyo+Xi′Δt _Xyo + 1′=Xi′+Xi′ ′Δt _Xyo + 1′ ′=Xi′ ′+Xi′ ′ ′Δt _
Conviértalo a la forma matricial correspondiente. Aquí tomamos el espacio bidimensional como ejemplo, es decir, x = { x 0 , x 1 , . . . , xn } x=\{x_0,x_1, ..., x_n\}X={ x0,X1,. . . ,Xnorte} , en el quexi = { pxi , pyi , vxi , vyi , axi , ayi } x_i=\{p_{xi},p_{yi},v_{xi},v_{yi},a_{xi}, a_{ yy}\}Xyo={ pagxyo _,pagsi si,vxyo _,vsi si,axyo _,asi si} , correspondientes respectivamente axy xyColoque px p_xeje x ypagx或py p_ypagy, velocidad vx v_xvx或vy v_yvyy aceleración ax a_xax或 a y a_y ay。
AI = [ 1 − 1 0 … 0 0 1 − 1 … 0 ⋮ ⋮ ⋱ ⋱ ⋮ 0 0 … 1 − 1 ] ∈ R ( n − 1 ) × n A_{I}=\begin{bmatrix} 1 &-1& 0&\ldots&0 \\0& 1&-1&\ldots&0 \\\vdots&\vdots&\ddots&\ddots&\vdots \\0&0&\ldots&1&-1 \end{bmatrix}\in R^{(n-1) \times n}Ayo=⎣⎢⎢⎢⎡10⋮0− 11⋮00− 1⋱………⋱100⋮− 1⎦⎥⎥⎥⎤∈R( norte − 1 ) × norte，
A t = [ Δ t 0 0 … 0 0 Δ t 0 … 0 ⋮ ⋮ ⋱ ⋱ ⋮ 0 0 … Δ t 0 ] ∈ R ( n − 1 ) × n A_{t}=\begin{bmatrix} \Delta{ t} &0& 0&\ldots&0 \\0&\Delta{t} &0&\ldots&0 \\\vdots&\vdots&\ddots&\ddots&\vdots \\0&0&\ldots&\Delta{t} &0 \end{bmatrix}\in R^{ (n-1) \veces n}At=⎣⎢⎢⎢⎡Δt _0⋮00Δt _⋮000⋱………⋱Δt _00⋮0⎦⎥⎥⎥⎤∈R( norte − 1 ) × norte，
A z = [ 0 0 0 … 0 0 0 0 … 0 ⋮ ⋮ ⋱ ⋱ ⋮ 0 0 … 0 0 ] ∈ R ( n − 1 ) × n A_{z}=\begin{bmatrix}0 &0& 0&\ldots&0 \ \0&0 &0&\ldots&0 \\\vdots&\vdots&\ddots&\ddots&\vdots \\0&0&\ldots&0 &0 \end{bmatrix}\in R^{(n-1) \times n}Az=⎣⎢⎢⎢⎡00⋮000⋮000⋱………⋱000⋮0⎦⎥⎥⎥⎤∈R( norte − 1 ) × norte，
A eq = [ AIA z A t A z A z A z A z AIA z A t A z A z ] ∈ R ( 2 n − 2 ) × 6 n A_{eq}=\begin{bmatrix} A_I &A_z & A_t &A_z &A_z &A_z\\ A_z&A_{I} &A_z & A_{t} &A_z &A_z \end{bmatrix}\in R^{(2n-2) \times 6n}Aeq _=[AyoAzAzAyoAtAzAzAtAzAzAzAz]∈R( 2 norte - 2 ) × 6 norte
leq = ueq = [ 0 0 ⋮ 0 ] ∈ R 6 n × 1 l_{eq} = u_{eq}=\begin{bmatrix} 0 \\ 0 \\ \vdots \\ 0 \end{bmatrix}\in R ^{6n\veces 1}yoeq _=tueq _=⎣⎢⎢⎢⎡00⋮0⎦⎥⎥⎥⎤∈R6n × 1 _
4. Utilice OSQP para resolver problemas
El efecto es el siguiente: los puntos rojos son los puntos de ruta establecidos, el gris es la ruta ajustada por b-spline y el verde es el resultado de la optimización.
Sube el código directamente
import osqp
import numpy as np
import time
from scipy.interpolate import *
from scipy import sparse
import matplotlib.pyplot as plt
class PathOptimizer:
def __init__(self, points, ts, acc, d='2d'):
self.ctrl_points = points
self.ctrl_ts = ts
self.acc_max = acc
self.dimension = d
# 时间分配
self.seg_length = self.acc_max * self.ctrl_ts * self.ctrl_ts
self.total_length = 0
for i in range(self.ctrl_points.shape[0] - 1):
self.total_length += np.linalg.norm(self.ctrl_points[i + 1, :] - self.ctrl_points[i, :])
self.total_time = self.total_length/self.seg_length * self.ctrl_ts * 1.2
# 生成控制点B样条函数
time_list = np.linspace(0, self.total_time, self.ctrl_points.shape[0])
self.px_spline = InterpolatedUnivariateSpline(time_list, self.ctrl_points[:, 0])
self.py_spline = InterpolatedUnivariateSpline(time_list, self.ctrl_points[:, 1])
# 对B样条优化参考轨迹进行均匀采样
self.seg_time_list = np.arange(0, self.total_time, self.ctrl_ts)
self.px = self.px_spline(self.seg_time_list)
self.py = self.py_spline(self.seg_time_list)
# 优化问题权重参数
self.weight_pos = 5
self.weight_vel = 1
self.weight_acc = 1
self.weight_jerk = 0.1
start_time = time.perf_counter()
self.smooth_path = self.osqpSmooth()
print('OSQP in ' + self.dimension + ' Cost time: ' + str(time.perf_counter() - start_time))
pass
def osqpSmooth(self):
px0, py0 = self.px, self.py
vx0 = vy0 = vz0 = np.zeros(self.px.shape)
ax0 = ay0 = az0 = np.zeros(self.px.shape)
# x(0), x(1), ..., x(n - 1), x'(0), x'(1), ..., x'(n - 1), x"(0), x"(1), ..., x"(n - 1)
x0_total = np.hstack((px0, py0, vx0, vy0, ax0, ay0))
n = int(x0_total.shape[0] / 3) # 每种级别状态有几个变量,即x共n个，x'共n个，x"也是n个
Q_x = self.weight_pos * np.eye(n)
Q_dx = self.weight_vel * np.eye(n)
Q_zeros = np.zeros((n, n))
w_ddl = self.weight_acc
w_dddl = self.weight_jerk
if n % 2 != 0:
print('x0 input error.')
return
n_part = int(n/2) # 每部分有n_part个数据,即x_x(0), x_x(1), ..., x_x(n-1), x_y(0), x_y(1), ..., x_y(n-1), ...
Q_ddx_part = (w_ddl + 2 * w_dddl / self.ctrl_ts / self.ctrl_ts) * np.eye(n_part) \
- 2 * w_dddl / self.ctrl_ts / self.ctrl_ts * np.eye(n_part, k=-1)
Q_ddx_part[0][0] = w_ddl + w_dddl / self.ctrl_ts / self.ctrl_ts
Q_ddx_part[n_part-1][n_part-1] = w_ddl + w_dddl / self.ctrl_ts / self.ctrl_ts
np_zeros = np.zeros(Q_ddx_part.shape)
Q_ddx_l = np.vstack((Q_ddx_part, np_zeros))
Q_ddx_r = np.vstack((np_zeros, Q_ddx_part))
Q_ddx = np.hstack((Q_ddx_l, Q_ddx_r))
Q_total = sparse.csc_matrix(np.block([[Q_x, Q_zeros, Q_zeros],
[Q_zeros, Q_dx, Q_zeros],
[Q_zeros, Q_zeros, Q_ddx]
]))
p_total = - x0_total
p_total[:n] = self.weight_pos * p_total[:n]
# 动力学模型，所以是等式约束
# x(i+1) = x(i) + x'(i)△t
# x'(i+1) = x'(i) + x"(i)△t
AI_part = np.eye(n_part-1, n_part) - np.eye(n_part-1, n_part, k=1) # 计算同阶变量之间插值 (n-1) x n维
AT_part = self.ctrl_ts * np.eye(n_part-1, n_part) # 时间矩阵 (n-1) x n维
AZ_part = np.zeros([n_part-1, n_part]) # 全0矩阵 (n-1) x n维
# 起点为第一个点
A_init = np.zeros([4, x0_total.shape[0]])
A_init[0, 0] = A_init[1, n_part] = 1
A_init[2, n_part-1] = A_init[3, 2*n_part-1] = 1
A_l_init = A_u_init = np.array([x0_total[0], x0_total[n_part],
x0_total[n_part-1], x0_total[2*n_part-1]])
A_eq = sparse.csc_matrix(np.block([
[A_init],
[AI_part, AZ_part, AT_part, AZ_part, AZ_part, AZ_part],
[AZ_part, AI_part, AZ_part, AT_part, AZ_part, AZ_part],
[AZ_part, AZ_part, AI_part, AZ_part, AT_part, AZ_part],
[AZ_part, AZ_part, AZ_part, AI_part, AZ_part, AT_part]
]))
A_leq = A_ueq = np.zeros(A_eq.shape[0])
A_leq[:4] = A_ueq[:4] = A_l_init
# Create an OSQP object
prob = osqp.OSQP()
prob.setup(Q_total, p_total, A_eq, A_leq, A_ueq, alpha=1.0)
res = prob.solve()
return res.x[:]
class PathGenerator:
def __init__(self):
self.ctrl_points = np.array([[0.5, 0.5],
[0.5, 1],
[1.5, 1.],
[2., 2.],
[2.5, 2.5]])
self.ctrl_ts = 0.1 # 控制时间s
self.acc_max = 2 # 米/秒
self.ref_path = PathOptimizer(self.ctrl_points, self.ctrl_ts, self.acc_max)
if __name__ == '__main__':
pg = PathGenerator()
fig = plt.figure()
ax = plt.axes()
ax.scatter(pg.ctrl_points[:, 0], pg.ctrl_points[:, 1], color='red')
ax.plot(pg.ref_path.px, pg.ref_path.py, color='gray')
seg_num = pg.ref_path.px.shape[0]
ax.plot(pg.ref_path.smooth_path[:seg_num], pg.ref_path.smooth_path[seg_num:2*seg_num], color='green')
plt.show()