当前位置：网站首页>Quadratic programming based on osqp

Quadratic programming based on osqp

2022-07-27 16:39:00 【An audience who doesn't understand music】

List of articles

1. Problem modeling
2. QP problem
3. format conversion
- structure $P$ 、 $q$ matrix
- Equality constraints
4. Use OSQP Solve the problem

1. Problem modeling

Suppose a reference trajectory is known to be $r_0,r_1,...,r_i,...,r_n$ , This track can be generated arbitrarily , Because there may be a right angle inflection point or an acute angle inflection point , The robot cannot move directly along this trajectory . therefore , It is necessary to generate a path that satisfies the kinematic constraints of the robot 、 As smooth as possible, a practical trajectory $x_0,x_1,...,x_i,...x_n$ , This is also the variable we need to optimize , Here trajectory ${r_i\}$ and ${x_i\}$ Same length . The following cost function is established （ $\textbf{cost function}$ ）：

$\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)^2$

$w_x\sum^{n-1}_{i=0}(x_i-r_i)^2$ ： Minimize the distance between the generated track and the reference track , That is, let the generated track follow the reference track as much as possible , among $w_x$ For weight .
$w_{x'}\sum^{n-1}_{i=0}(x'_i)^2$ ： Minimize the first derivative of the trajectory , among $w_x'$ For weight . Popular point , ${x\}$ When it is a track , ${x'\}$ For speed , The purpose of this item is to minimize speed . It's like driving , The shortest trajectory along a straight line , Minimum energy consumption , Turning the steering wheel randomly will distort the track , Increase energy consumption and mobile time .
$w_x\sum^{n-1}_{i=0}(x_i-r_i)^2$ ： Minimize the second derivative of the trajectory , among $w_x''$ For weight . Popular point , Is to minimize acceleration .
$w_x\sum^{n-1}_{i=0}(x_i-r_i)^2$ ： Minimize the third derivative of the trajectory , among $w_x'''$ For weight . Popular point , Is to minimize acceleration （ The technical term should be $\textbf{Jerk}）.

2. QP problem

The common format is as follows ：
$\begin{aligned} min \quad \frac{1}{2} x^T\mathcal{P}x+\mathcal{q}x \\ l\leq Ax \leq u \end{aligned}$

among ${P}$ It's a $n x n$ matrix , $x$ by $n$ Dimension vector , $q$ by $m x n$ Matrix .

Why convert to QP Format ？

because QP The problem must be solvable , And at present, all kinds of solvers support solving QP problem , The speed is much faster than that of nonlinear optimization problem .

3. format conversion

Let's turn our question into QP Format , The main thing is to build $\mathcal{P}$ Matrix and $\mathcal{q}$ matrix , The following content mainly refers to this article Planning control :Piecewise Jerk Path Optimizer Of python Realization . Here, the one-dimensional of the reference article is extended to two-dimensional , namely $x=\{x_0,x_1, ..., x_n\}$ , Each of these States $x_i=\{p_{xi},p_{yi},v_{xi},v_{yi},a_{xi},a_{yi}\}$ , Corresponding to $x y$ Position on the shaft $p_x$ or $p_y$ 、 Speed $v_x$ or $v_y$ And acceleration $a_x$ or $a_y$ .

In order to simplify the QP The scale of the problem , Here we assume that the optimization variables $x$ contain $n$ Status （ Respectively corresponding to the reference track $r$ The length of ）, And satisfy the following relationship

$x'_i=\frac{x_{i+1}-x_i}{\Delta{t}}$
$x''_i=\frac{x'_{i+1}-x'_i}{\Delta{t}}$
$x'''_i=\frac{x''_{i+1}-x''_i}{\Delta{t}}$

Bring the last formula above into the cost function $\mathcal{J}$ in , And expand it to get ：
$\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]$

Remove the constant term （ And optimization variables $x$ Irrelevant items ） Available ：

$\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)$

among
$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} \in R^{6n \times 1}$

structure $P$ 、 $q$ matrix

$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}$

$P_{a}=\begin{bmatrix} 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}$

$P=\begin{bmatrix} P_p & & & \\ & P_p \\ && P_v \\ &&& P_v \\ &&&& P_a \\ &&&&& P_a \\ \end{bmatrix}\in R^{6n \times 6n}$

$\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 \times 6n}$

Equality constraints

In order to make the generated trajectory meet the moving state of the robot , The motion model of the robot needs to be transformed into the corresponding equality constraints , namely $l = A x = u$ . Here consider the following motion model ：
$\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{aligned}$

Convert to corresponding matrix form , Here to 2 Take dimensional space as an example , namely $x=\{x_0,x_1, ..., x_n\}$ , Each of these States $x_i=\{p_{xi},p_{yi},v_{xi},v_{yi},a_{xi},a_{yi}\}$ , Corresponding to $x y$ Position on the shaft $p_x$ or $p_y$ 、 Speed $v_x$ or $v_y$ And acceleration $a_x$ or $a_y$ .

$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}$ ,

$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) \times 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}$ ,

$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}$

$l_{eq} = u_{eq}=\begin{bmatrix} 0 \\ 0 \\ \vdots \\ 0 \end{bmatrix}\in R^{6n \times 1}$

4. Use OSQP Solve the problem

The effect is as follows , The red dot is the set path point , Grey is b The path of spline fitting , Green is the result of optimization .

Go straight to the code

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

        #  Time allocation 
        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

        #  Generate control points B Spline function 
        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])

        #  Yes B Spline optimization reference trajectory for uniform sampling 
        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)

        #  Optimize the weight parameters of the problem 
        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)   #  There are several variables for each level state , namely x common n individual ,x' common n individual ,x" It's also n individual 
        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)   #  Each part has n_part Data , namely 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]

        #  The kinetic model , So it's an equality constraint 
        # 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)  #  Calculate the interpolation between variables of the same order  (n-1) x n dimension 
        AT_part = self.ctrl_ts * np.eye(n_part-1, n_part)   #  Time matrix  (n-1) x n dimension 
        AZ_part = np.zeros([n_part-1, n_part])  #  whole 0 matrix  (n-1) x n dimension 

        #  The starting point is the first point 
        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  #  Control time s
        self.acc_max = 2    #  rice / second 

        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()