Minimum_jerk参考代码

1. 参考代码

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

def genQk(T_down, T_up):
    Q = np.zeros((6, 6))
    Q[3][4] = 72 * (T_up**2 - T_down**2)
    Q[3][5] = 120 * (T_up**3 - T_down**3)
    Q[4][5] = 360 * (T_up**4 - T_down**4)
    Q = Q + Q.T
    Q[3][3] = 36 * (T_up - T_down)
    Q[4][4] = 192 * (T_up**3 - T_down**3)
    Q[5][5] = 720 * (T_up**5 - T_down**5)
    return Q

def main():
    path = [[1., 3.], [3., 5.], [4., 2.], [2.5, 1.2], [2., -2.5]]
    path = np.array(path)
    x = path[:, 0]
    deltaT = 2.0
    T = np.linspace(0, deltaT * (len(x) - 1), len(x))

    K = 3
    n_order = 2 * K - 1 
    M = len(x) - 1 
    N = M * (n_order + 1) 
    
    Q = np.zeros((N, N))
    for idx in range(M):
        Qk = genQk(T[idx], T[idx + 1])
        Q[6 * idx : 6 * (idx + 1), 6 * idx : 6 * (idx + 1)] = Qk
        
    A0 = np.zeros((2 * K + M - 1, N))
    b0 = np.zeros(len(A0))
    for k in range(K):
        for i in range(k, 6):
            c = 1.0
            for j in range(k): 
                c *= (i - j)
            A0[2 * k][i] = c * T[0]**(i - k)
            A0[2 * k + 1][6 * (M - 1) + i] = c * T[M]**(i - k)
    b0[0] = x[0]
    b0[1] = x[M]
    
    for m in range(1, M):
        for i in range(6):
            A0[6 + m - 1][6 * m + i] = T[m]**i
        b0[6 + m - 1] = x[m]
        
    A1 = np.zeros((3 * (M - 1), N))
    b1 = np.zeros(len(A1))
    for m in range(M - 1):
        for k in range(3):     
            for i in range(k, 6):
                c = 1
                for j in range(k):
                    c *= (i - j)
                A1[3 * m + k][6 * m + i] = c * T[m + 1]**(i - k)
                A1[3 * m + k][6 * (m + 1) + i] = -c * T[m + 1]**(i - k)
    A = np.vstack((A0, A1))
    b = np.hstack((b0, b1))
    
    Q = matrix(Q)
    q = matrix(np.zeros(N))
    A = matrix(A)
    b = matrix(b)
    result = solvers.qp(Q, q, A = A, b = b)
    p_coff = np.array(result['x'])

    Pos, Vel, Acc, Time = [], [], [], []
    for k in range(M):
        t = np.linspace(T[k], T[k + 1], 100)
        coef = p_coff[6 * k: 6 * (k + 1)]
        coef = np.reshape(coef, (1, 6))
        for i in range(100):
            t_pos = np.array([1.0, t[i]**1, t[i]**2, t[i]**3, t[i]**4, t[i]**5])
            t_vel = np.array([0.0, 1.0, 2.0 * t[i], 3.0 * t[i]**2, 4.0 * t[i]**3, 5.0 * t[i]**4])
            t_acc = np.array([0.0, 0.0, 2.0, 6.0 * t[i], 12.0 * t[i]**2, 20.0 * t[i]**3])
            pos = np.dot(coef, t_pos)
            vel = np.dot(coef, t_vel)
            acc = np.dot(coef, t_acc)
            Pos.append(pos)
            Vel.append(vel)
            Acc.append(acc)
            Time.append(t[i])                        

    plt.subplot(3, 1, 1)
    plt.plot(Time, Pos)
    plt.xlabel("time(s)")
    plt.ylabel("position(m)")
    plt.subplot(3, 1, 2)
    plt.plot(Time, Vel)
    plt.xlabel("time(s)")
    plt.ylabel("velocity(m/s)")
    plt.subplot(3, 1, 3)
    plt.plot(Time, Acc)
    plt.xlabel("time(s)")
    plt.ylabel("accel(m/s^2)")
    plt.show()

if __name__ == "__main__":
    main()

 2. 运行

结果