Question

我试图用数字解决使用cvxpy（版本0.4.9）生成一维最小颠簸轨迹的问题。这是问题的基本陈述，

＆＃39; X＆＃39;是位置的向量作为时间的函数。＆＃39; d＆＃39;是用于计算加加速度的矩阵，＆＃39; A＆＃39;和＆＃39; b＆＃39;指定位置，速度和加速度的约束。 x [0] = 0，x [N] = 1.速度和加速度从0开始和结束。

我尝试使用CVXPY使用以下代码解决此问题。

import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import toeplitz
from scipy.linalg import block_diag
import cvxpy as cvx  # Version 0.4.9

# setup sample rate
N = 1000 # number of samples

# set up jerk matrix D
_row = np.hstack((np.array([[-1, 3, -3, 1]]), np.zeros((1,N-4))))
_col = np.vstack((np.array([[-1]]), np.zeros((N-4,1))))
D = toeplitz(_col, _row)

# set up constraint A*x = b
pos_i = np.hstack((np.array([[1]]), np.zeros((1,N-1))))
pos_f = np.hstack((np.zeros((1,N-1)), np.array([[1]])))

vel_i = np.hstack((np.array([[-1, 1]]), np.zeros((1,N-2))))
vel_f = np.hstack((np.zeros((1,N-2)), np.array([[-1, 1]])))

accl_i = np.hstack((np.array([[1,-2,1]]), np.zeros((1,N-3))))
accl_f = np.hstack((np.zeros((1,N-3)), np.array([[1,-2,1]])))

A = np.vstack((pos_i, pos_f, 
               vel_i, vel_f,
               accl_i, accl_f))
b = np.zeros((6,1))
b[0] = 0
b[1] = 1

# Construct the problem.
x = cvx.Variable(N)
objective = cvx.Minimize(cvx.sum_squares(1000 * D * x))
constraints = [A * x == b]
prob = cvx.Problem(objective, constraints)

# The optimal objective is returned by prob.solve().
result = prob.solve(verbose=True)

print("Problem status: {0}".format(prob.status))

pos = x.value[:, 0]
vel = 10 * np.diff(pos, axis=0)
accl = 10 * np.diff(vel, axis=0)
jerk = 10 * np.diff(accl, axis=0)

plt.subplot(221)
plt.plot(pos)
plt.title('Position')
plt.subplot(222)
plt.plot(vel)
plt.title('Velocity')
plt.subplot(223)
plt.plot(accl)
plt.title('Acceleration')
plt.subplot(224)
plt.plot(jerk)
plt.title('Jerk')
plt.tight_layout()
plt.show()

当 N = 100 时，我按预期得到以下输出。

ECOS 2.0.4 - (C) embotech GmbH, Zurich Switzerland, 2012-15. Web: www.embotech.com/ECOS

It     pcost       dcost      gap   pres   dres    k/t    mu     step   sigma     IR    |   BT
 0  +0.000e+00  -1.329e-04  +4e+03  3e-02  5e-06  1e+00  2e+03    ---    ---    1  1  - |  -  - 
 1  -1.866e-01  -9.469e-05  +4e+01  3e-04  5e-08  2e-01  2e+01  0.9890  1e-04   1  1  1 |  0  0
 2  -4.287e-03  -1.687e-05  +5e-01  3e-06  7e-10  4e-03  2e-01  0.9882  1e-04   2  2  2 |  0  0
 3  +2.401e-03  +3.685e-03  +7e-02  4e-07  1e-10  1e-03  4e-02  0.8562  3e-03   3  4  4 |  0  0
 4  +3.743e-02  +4.859e-02  +3e-01  3e-06  4e-11  1e-02  1e-01  0.9890  3e-01   7  9  9 |  0  0
 5  -2.330e-01  -1.977e-01  +4e-01  1e-06  1e-10  4e-02  2e-01  0.4759  6e-01   1  1  1 |  0  0
 6  +3.801e-03  +1.396e-02  +2e-01  5e-07  2e-11  1e-02  9e-02  0.9890  9e-02   1  1  1 |  0  0
 7  +5.710e-02  +5.937e-02  +4e-02  7e-07  4e-12  2e-03  2e-02  0.9890  4e-02   1  1  1 |  0  0
 8  +7.743e-02  +7.747e-02  +1e-03  3e-07  9e-12  5e-05  6e-04  0.9890  1e-03   8  9  9 |  0  0
 9  +8.119e-02  +8.124e-02  +4e-04  3e-07  8e-12  5e-05  2e-04  0.9095  1e-01   8  9  9 |  0  0
10  +8.086e-02  +8.094e-02  +1e-03  1e-07  5e-12  8e-05  6e-04  0.8810  5e-01   6  7  6 |  0  0
11  +8.263e-02  +8.267e-02  +1e-04  2e-07  4e-12  4e-05  8e-05  0.9890  9e-02   8  9  9 |  0  0
12  +8.323e-02  +8.324e-02  +2e-04  1e-07  3e-12  1e-05  9e-05  0.9890  2e-01   8  9  9 |  0  0
13  +8.354e-02  +8.355e-02  +6e-05  7e-08  2e-12  7e-06  4e-05  0.9890  1e-01   8  9  9 |  0  0
14  +8.375e-02  +8.375e-02  +2e-05  6e-08  2e-12  3e-06  1e-05  0.9535  1e-01   8  9  9 |  0  0
15  +8.374e-02  +8.374e-02  +5e-05  3e-08  1e-12  4e-06  3e-05  0.8755  4e-01   6  7  6 |  0  0
16  +8.383e-02  +8.383e-02  +6e-06  3e-08  1e-12  2e-06  4e-06  0.9890  9e-02   9  9  9 |  0  0
17  +8.385e-02  +8.385e-02  +8e-06  3e-08  8e-13  6e-07  4e-06  0.9890  2e-01   8  9  9 |  0  0
18  +8.387e-02  +8.387e-02  +2e-06  2e-08  6e-13  3e-07  1e-06  0.9890  8e-02   8  8  8 |  0  0
19  +8.388e-02  +8.388e-02  +1e-06  1e-08  4e-13  1e-07  8e-07  0.9890  1e-01   8  8  8 |  0  0
20  +8.388e-02  +8.388e-02  +6e-07  8e-09  3e-13  7e-08  4e-07  0.9890  1e-01   8  8  8 |  0  0
21  +8.389e-02  +8.389e-02  +3e-07  6e-09  2e-13  3e-08  2e-07  0.9890  1e-01   8  8  8 |  0  0
22  +8.389e-02  +8.389e-02  +2e-07  4e-09  1e-13  2e-08  1e-07  0.9890  2e-01   8  7  7 |  0  0
23  +8.389e-02  +8.389e-02  +5e-08  3e-09  1e-13  8e-09  3e-08  0.9786  9e-02   8  8  8 |  0  0
24  +8.389e-02  +8.389e-02  +1e-07  2e-09  6e-14  1e-08  7e-08  0.8905  4e-01   6  6  5 |  0  0
25  +8.389e-02  +8.389e-02  +1e-08  2e-09  6e-14  5e-09  1e-08  0.9890  1e-01   9  8  8 |  0  0
26  +8.389e-02  +8.389e-02  +3e-08  1e-09  4e-14  2e-09  1e-08  0.9890  3e-01   8  7  7 |  0  0
27  +8.389e-02  +8.389e-02  +5e-09  1e-09  3e-14  8e-10  3e-09  0.9890  7e-02   8  7  7 |  0  0

OPTIMAL (within feastol=1.0e-09, reltol=6.5e-08, abstol=5.5e-09).
Runtime: 0.006165 seconds.

Problem status: optimal

然而，当我改变 N = 1000 时，我得到一些奇怪的东西（问题状态仍然是最佳的）。

ECOS 2.0.4 - (C) embotech GmbH, Zurich Switzerland, 2012-15. Web: www.embotech.com/ECOS

It     pcost       dcost      gap   pres   dres    k/t    mu     step   sigma     IR    |   BT
 0  +0.000e+00  -1.295e-05  +4e+04  5e-02  4e-06  1e+00  2e+04    ---    ---    1  1  - |  -  - 
 1  -1.911e+00  -3.203e-05  +4e+02  6e-04  5e-08  2e+00  2e+02  0.9890  1e-04   1  1  1 |  0  0
 2  -4.228e-02  -5.084e-05  +5e+00  5e-06  5e-10  4e-02  2e+00  0.9890  1e-04   2  2  2 |  0  0
 3  -7.419e-04  -4.283e-05  +5e-02  5e-08  2e-11  7e-04  3e-02  0.9890  1e-04   3  5  5 |  0  0
 4  +1.401e-06  +1.169e-05  +6e-04  2e-08  1e-11  1e-05  3e-04  0.9890  1e-04   6  9  9 |  0  0
 5  +1.029e-05  +1.043e-05  +6e-06  1e-08  8e-12  1e-07  3e-06  0.9890  1e-04   4  6  6 |  0  0
 6  +8.446e-06  +8.448e-06  +1e-07  8e-09  7e-12  2e-09  6e-08  0.9890  1e-04   5  6  6 |  0  0
 7  +7.053e-06  +7.053e-06  +6e-09  7e-09  6e-12  7e-11  3e-09  0.9890  1e-03   5  5  5 |  0  0

OPTIMAL (within feastol=7.3e-09, reltol=8.0e-04, abstol=5.7e-09).
Runtime: 0.011601 seconds.

Problem status: optimal

为什么我会看到这样的差异？我做错了什么，我该怎么做才能解决这个问题？

当我将D缩放1000倍时，我的结果是不同的。

使用CVXPY的最小颠簸轨迹

0 个答案: