Траектория следования с использованием CVXPy

Question

У меня есть траектория векторов состояний, где каждый вектор состояния состоит из [x, y, theta, alpha]. Мой вход управления phi . Я хочу сгладить мою текущую траекторию, используя регулятор LQR, где моя функция стоимости будет выглядеть примерно так:

Проблема в том, что динамикагрузовика являются нелинейными, и я хочу линеаризовать их по моей текущей траектории (и после каждой итерации линеаризовать по новой найденной траектории). Динамика моего грузовика следующая (вы можете считать константы v, l и lt):

Проблема в том, что мне нужнополучить матрицу A и матрицу B для каждой точки траектории, но я не уверен, правильно ли я линеаризирую динамику (dt изменяется между различными точками A, поскольку моя траектория, которой нужно следовать, не имеет равноотстоящих точек).

Я закодировал это в CvxPx, но независимо от того, насколько я установил стоимость отклонения от конечной желаемой позиции, полученные результаты сильно отклоняются(в основном идите в другом совершенно другом направлении), поэтому что-то не так. Я прилагаю код CvxPy.

def cvxpy_problem(self, trajectory):
    """
    :param trajectory: Dictionary with the set of states for the trajectory, the dt between each state and v_trailer
    (speed of vehicle at every step)
    """
    # Form and solve control problem.
    import cvxpy as cp
    n = 4
    m = 1
    T = 100
    x = cp.Variable((n, T + 1))
    u = cp.Variable((m, T))
    cost = 0
    lt = 12.086
    l = 3.6
    x_original = np.vstack(
        (trajectory["x"][0:T+1], trajectory["y"][0:T+1], trajectory["theta"][0:T+1], trajectory["alpha"][0:T+1]))
    u_original = trajectory["phi"][0:T]
    constr = []
    for t in range(1,T):
        cost += 10*cp.sum_squares(x[:, t] - x_original[:, t]) + 0.5*cp.sum_squares(u[:, t-1])
        A = np.array([[1, 0, -np.sin(trajectory["theta"][t]), 0], [0, 1, np.cos(trajectory["theta"][t]), 0],
                      [0, 0, 1, 0],
                      [0, 0, 0, 1 - np.cos(trajectory["alpha"][t]) / lt]]) * trajectory["v_trailer"][t] * \
            trajectory["delta_t"][t]
        B = np.array([0, 0, 1 / (l * np.cos(trajectory["phi"][t]) ** 2),
                      -1 / (l * np.cos(trajectory["phi"][t]) ** 2)]) * trajectory["v_trailer"][t] * \
            trajectory["delta_t"][t]
        constr += [x[:,t+1] == A@(x[:,t]-x_original[:,t]) + B@(u[:,t]-u_original[t]),
                   cp.norm(u[:, t], 'inf') <= 0.55]  # constraint on control
    # sums problem objectives and concatenates constraints.
    cost += 1000*cp.sum_squares(x[:, -1] - x_original[:, -1])
    constr += [x[:, 0] == x_original[:, 0]]  # initial and final position are fixed
    problem = cp.Problem(cp.Minimize(cost), constr)
    problem.solve(solver=cp.ECOS)

Проблема в том, что, хотя я получаю результат, он не будет полностью заполнять мою нелинейную динамику, так как оптимизатор просто получает их линеаризованную версию. Я прилагаю некоторые результаты, начиная с [40,14,1.56,8.5e-3] и до конечной цели [42,18.5,0.71,0.70]. Синяя линия соответствует траектории, а оранжевая линия - результаты.