轨迹规划 ---- 二次规划

"""

$$$$       一维轨迹优化
####       2022-03-29
####       分段多项式拟合轨迹
####       将求多项式系数问题转换为QP问题
####       通过matrix, solvers求解

"""
import numpy as np
import matplotlib.pyplot as plt
from cvxopt import matrix, solvers


x = [1450, 1856.18787702, 2392] #, 2392, 1866, 1284, 1585, 2012, 2392, 2392, 2096.5, 1662, 1450]
# 分配时间
T = [0, 0.5625, 1.2] #, 1.31, 1.69, 2, 2.32, 2.56, 2.94, 3.26, 3.45, 3.63, 4]

########### 目标函数 ###########
######## 1/2xTQx + qTx ########
K = 4  # jerk为3阶导数，取K=3   snap为四阶导数，取K=4
n_order = 2 * K - 1  # 多项式阶数
M = len(x) - 1  # 轨迹的段数

N = M * (n_order + 1)  # 矩阵Q的维数


def getQk(T_down, T_up):
    Q = np.zeros((8, 8))
    Q[4][5] = 1440 * (T_up ** 2 - T_down ** 2)
    Q[4][6] = 2880 * (T_up ** 3 - T_down ** 3)
    Q[4][7] = 4920 * (T_up ** 4 - T_down ** 4)
    Q[5][6] = 10800 * (T_up ** 4 - T_down ** 4)
    Q[5][7] = 19680 * (T_up ** 5 - T_down ** 5)
    Q[6][7] = 49200 * (T_up ** 6 - T_down ** 6)
    Q = Q + Q.T  # Q为对称矩阵
    Q[4][4] = 576 * (T_up ** 1 - T_down ** 1)
    Q[5][5] = 4800 * (T_up ** 3 - T_down ** 3)
    Q[6][6] = 25920 * (T_up ** 5 - T_down ** 5)
    Q[7][7] = 100800 * (T_up ** 7 - T_down ** 7)
    return Q

Q = np.zeros((N, N))
for k in range(1, M + 1):
    Qk = getQk(T[k - 1], T[k])
    Q[(8 * (k - 1)): (8 * k), (8 * (k - 1)): (8 * k)] = Qk

Q = Q * 2  # 因为标准目标函数为： 1/2xTQx + qTx，所以要乘2

########### 等式约束 ###########
# 1.导数约束 Derivative Constraint
# 对于有 M+1 个路径点的轨迹，一共有 (2K+M-1) 个导数约束

# 添加首末状态约束(包括位置、速度、加速度、加加速度、加加加速度)
A0 = np.zeros((2 * (K + 1) + M - 1, N))    # 对于始末位置共有10个等式约束  中间有两个等式约束
b0 = np.zeros(len(A0))
for k in range(K+1):            # snap为四阶导数，取K=4
    for i in range(k, 8):
        c = 1
        for j in range(k):
            c *= (i - j)
        A0[0 + k * 2][i] = c * T[0] ** (i - k)
        A0[1 + k * 2][(M - 1) * 8 + i] = c * T[M] ** (i - k)
b0[0] = x[0]
b0[1] = x[M]
# 添加每段轨迹的初始位置约束   共 M-1 个约束
for m in range(1, M):
    for i in range(8):
        A0[8 + m + 1][m * 8 + i] = T[m] ** i
    b0[8 + m + 1] = x[m]
# 2.连续性约束 Continuity Constraint  (包括位置、速度、加速度、加加速度、加加加速度)
A1 = np.zeros(((M - 1) * 5, N))
b1 = np.zeros(len(A1))
for m in range(M - 1):
    for k in range(5):  # 最多两阶导数相等
        for i in range(k, 8):
            c = 1
            for j in range(k):
                c *= (i - j)
            index = m * 5 + k
            A1[index][m * 8 + i] = c * T[m + 1] ** (i - k)
            A1[index][(m + 1) * 8 + i] = -c * T[m + 1] ** (i - k)
A = np.vstack((A0, A1))
b = np.hstack((b0, b1))

########### 不等式约束 ###########
G0 = np.zeros((2 * M, N))    # 对于各段代码施加约束
H0 = np.zeros(len(G0))
#print(np.shape(G))
for m in range(M):
    for k in range(1,2):  # 一阶导约束
        for i in range(k, 8):
            c = 1
            for j in range(k):
                c *= (i - j)
            G0[0 + 2 * m][m * 8 + i] = -c * T[0 + m] ** (i - k)
            G0[1 + 2 * m][m * 8 + i] = -c * T[1 + m] ** (i - k)
    H0[0 + 2 * m] = -11
    H0[1 + 2 * m] = -11

G1 = np.zeros((2 * M, N))    # 对于各段代码施加约束
H1 = np.zeros(len(G1))
#print(np.shape(G))
for m in range(M):
    for k in range(2,3):     # 二阶导约束
        for i in range(k, 8):
            c = 1
            for j in range(k):
                c *= (i - j)
            G1[0 + 2 * m][m * 8 + i] = -c * T[0 + m] ** (i - k)
            G1[1 + 2 * m][m * 8 + i] = -c * T[1 + m] ** (i - k)
    H1[0 + 2 * m] = -11
    H1[1 + 2 * m] = -11

#print(np.shape(G))
G = np.vstack((G0, G1))
H = np.hstack((H0, H1))
# %% 解二次规划问题
# 目标函数
Q = matrix(Q)
q = matrix(np.zeros(N))
# 等式约束: Ax = b
A = matrix(A)
b = matrix(b)
G = matrix(G)
H = matrix(H)
result = solvers.qp(Q, q, A = A, b = b)
p_coff = np.asarray(result['x']).flatten()     # 多项式系数

# %% 可视化结果
Pos = []
Vel = []
Acc = []
Jerk = []
for k in range(M):
    t = np.linspace(T[k], T[k + 1], 100)
    t_pos = np.vstack((t ** 0, t ** 1, t ** 2, t ** 3, t ** 4, t ** 5, t ** 6, t ** 7))
    t_vel = np.vstack((t * 0, t ** 0, 2 * t ** 1, 3 * t ** 2, 4 * t ** 3, 5 * t ** 4, 6 * t ** 5, 7 * t ** 6))
    t_acc = np.vstack((t * 0, t * 0, 2 * t ** 0, 3 * 2 * t ** 1, 4 * 3 * t ** 2, 5 * 4 * t ** 3, 6 * 5 * t ** 4, 7 * 6 * t ** 5))
    t_jerk = np.vstack((t * 0, t * 0, 2 * t * 0, 3 * 2 * t ** 0, 4 * 3 * 2 * t ** 1, 5 * 4 * 3 * t ** 2, 6 * 5 * 4 * t ** 3, 7 * 6 * 5 * t ** 4))
    coef = p_coff[k * 8: (k + 1) * 8]
    coef = np.reshape(coef, (1, 8))
    pos = coef.dot(t_pos)
    vel = coef.dot(t_vel)
    acc = coef.dot(t_acc)
    jerk = coef.dot(t_jerk)
    Pos.append([t, pos[0]])
    Vel.append([t, vel[0]])
    Acc.append([t, acc[0]])
    Jerk.append([t, jerk[0]])

Pos = np.array(Pos)
Vel = np.array(Vel)
Acc = np.array(Acc)
Jerk = np.array(Jerk)

plt.subplot(4, 1, 1)
plt.plot(Pos[:, 0, :].T, Pos[:, 1, :].T)
# plt.title("position")
#plt.xlabel("time01(s)")
plt.ylabel("position(mm)")

plt.subplot(4, 1, 2)
plt.plot(Vel[:, 0, :].T, Vel[:, 1, :].T)
# plt.title("velocity")
#plt.xlabel("time01(s)")
plt.ylabel("velocity(mm/s)")

plt.subplot(4, 1, 3)
plt.plot(Acc[:, 0, :].T, Acc[:, 1, :].T)
# plt.title("accel")
#plt.xlabel("time01(s)")
plt.ylabel("accel(mm/s^2)")

plt.subplot(4, 1, 4)
plt.plot(Jerk[:, 0, :].T, Jerk[:, 1, :].T)
# plt.title("Jerk")
plt.xlabel("time01(s)")
plt.ylabel("Jerk(mm/s^3)")
plt.show()