代码收藏家技术教程 2024-10-16

最优化理论与自动驾驶（十一）：基于iLQR的自动驾驶轨迹跟踪算法（c++和python版本）

最优化理论与自动驾驶（四）：iLQR原理、公式及代码演示

之前的章节我们介绍过，iLQR（迭代线性二次调节器）是一种用于求解非线性系统最优控制最优控制最优控制和规划问题的算法。本章节介绍采用iLQR算法对设定的自动驾驶轨迹进行跟踪，与第十章节纯跟踪算法采用同样跟踪轨迹，同时，我们仅对控制量的上下边界进行约束，使用简单的投影法进行约束（更详细的约束参考第九章 CILQR约束条件下的ILQR求解）。其实，iLQR可以直接进行轨迹规划，主要做法是将障碍物或者边界约束通过增广拉格朗日法将原始问题的约束条件通过拉格朗日乘子和惩罚项结合到代价函数中，逐步逼近最优解，这个将在后续章节进行讨论。

1. 问题定义

假设给定一个目标轨迹 $(x_{ref}, u_{ref})$ ，其中 $x_{ref}$ 是状态轨迹， $u_{ref}$ 是控制输入，任务是找到一组控制输入 $u_k$ 使得车辆从初始状态出发，最小化实际轨迹与目标轨迹之间的偏差。

(1) 状态方程

车辆的运动可以通过车辆动力学模型表示，通常有以下几种模型：

简化运动学模型：

$x_{k+1} = f(x_k, u_k)$

其中 $x_k$ 是车辆的状态， $u_k$ 是控制输入。

根据参考轨迹，将非线性系统模型在当前状态和控制输入附近进行线性化：

$f(x_k, u_k) \approx A_k \Delta x_k + B_k \Delta u_k + c_k$

其中 $A_k = \frac{\partial f}{\partial x}$ 和 $B_k = \frac{\partial f}{\partial u}$ 是对状态和控制的线性化， $\Delta x_k = x_k - x_{ref}$ ， $\Delta u_k = u_k - u_{ref}$ 。

(2) 代价函数

定义一个二次型代价函数，用于衡量实际轨迹与目标轨迹的偏差：

$J = \sum_{k=0}^{N-1} \left( (x_k - x_{ref})^T Q (x_k - x_{ref}) + (u_k - u_{ref})^T R (u_k - u_{ref}) \right) + (x_N - x_{ref})^T Q_f (x_N - x_{ref})$

其中 $Q$ 、 $R$ 、 $Q_f$ 分别是状态、控制输入和最终状态的权重矩阵。

(3) 反向递推

利用动态规划方法，从最终时刻 $N$ 开始，向前递推计算值函数的梯度和Hessian矩阵：

$V_x = \frac{\partial J}{\partial x}, \quad V_{xx} = \frac{\partial^2 J}{\partial x^2}$

同时计算控制增量的最优更新策略：

$\Delta u_k = K_k \Delta x_k + d_k$

其中 $K_k$ 是控制增量的反馈增益， $d_k$ 是前馈控制增量。

(4) 前向传播

基于反向递推得到的最优控制策略，在给定初始状态下，通过前向传播计算新的状态和控制轨迹，更新参考轨迹：

$x_{k+1} = f(x_k, u_k)$

重复步骤(1)至(4)直到收敛，得到最优控制序列。

2. 示例代码

import numpy as np
import math
import matplotlib.pyplot as plt
import imageio

# 车辆模型
class Vehicle:
    def __init__(self, x=0.0, y=0.0, theta=0.0, v=0.0):
        self.x = x
        self.y = y
        self.theta = theta  # 航向角
        self.v = v          # 速度

    def update(self, a, omega, dt):
        """
        更新车辆状态
        a: 加速度
        omega: 角速度
        dt: 时间步长
        """
        self.x += self.v * np.cos(self.theta) * dt
        self.y += self.v * np.sin(self.theta) * dt
        self.theta += omega * dt
        self.v += a * dt

# 轨迹
class Trajectory:
    def __init__(self):
        self.cx = np.linspace(0, 50, 500)
        self.cy = [np.sin(ix / 5.0) * ix / 2.0 for ix in self.cx]
        self.theta = [np.arctan2(self.cy[i+1] - self.cy[i], self.cx[i+1] - self.cx[i]) if i < len(self.cx)-1 else 0.0 for i in range(len(self.cx))]
        self.v = np.full_like(self.cx, 3.0)  # 目标速度为3 m/s

    def get_reference(self, index):
        """
        获取参考轨迹点
        """
        return np.array([self.cx[index], self.cy[index], self.theta[index], self.v[index]])

# iLQR控制器
class iLQRController:
    def __init__(self, N=50, max_iter=10, dt=0.1):
        self.N = N  # 控制时域长度
        self.max_iter = max_iter  # 最大迭代次数
        self.dt = dt  # 时间步长
        self.Q = np.diag([1.0, 1.0, 0.5, 0.1])  # 状态权重矩阵
        self.R = np.diag([0.1, 0.1])  # 控制权重矩阵
        self.Qf = self.Q * 10  # 终端状态权重矩阵

    def ilqr(self, vehicle, trajectory, index):
        """
        使用iLQR计算最优控制序列
        """
        # 初始化状态和控制序列
        x_dim = 4  # 状态维度 [x, y, theta, v]
        u_dim = 2  # 控制维度 [a, omega]
        xs = np.zeros((self.N + 1, x_dim))
        us = np.zeros((self.N, u_dim))
        
        # 初始状态
        xs[0] = np.array([vehicle.x, vehicle.y, vehicle.theta, vehicle.v])
        
        # 初始猜测控制序列（全零）
        us = np.zeros((self.N, u_dim))
        
        for iteration in range(self.max_iter):
            # 前向传播，计算状态轨迹
            for k in range(self.N):
                xs[k+1] = self.dynamics(xs[k], us[k], self.dt)
            
            # 计算代价函数梯度和Hessian矩阵
            Vx = self.Qf @ (xs[-1] - trajectory.get_reference(index + self.N))
            Vxx = self.Qf
            k_list = []
            K_list = []
            for k in reversed(range(self.N)):
                xk = xs[k]
                uk = us[k]
                x_ref = trajectory.get_reference(index + k)
                # 计算状态和控制的偏导数
                fx, fu = self.linearize_dynamics(xk, uk, self.dt)
                lx = self.Q @ (xk - x_ref)
                lu = self.R @ uk
                lxx = self.Q
                luu = self.R
                lux = np.zeros((u_dim, x_dim))
                # 计算Q函数的二次近似
                Qx = lx + fx.T @ Vx
                Qu = lu + fu.T @ Vx
                Qxx = lxx + fx.T @ Vxx @ fx
                Quu = luu + fu.T @ Vxx @ fu
                Qux = lux + fu.T @ Vxx @ fx
                # 计算控制增量
                Quu_inv = np.linalg.inv(Quu + np.eye(u_dim) * 1e-6)  # 加小值防止矩阵奇异
                k = -Quu_inv @ Qu
                K = -Quu_inv @ Qux
                # 更新V函数
                Vx = Qx + K.T @ Quu @ k + K.T @ Qu + Qux.T @ k
                Vxx = Qxx + K.T @ Quu @ K + K.T @ Qux + Qux.T @ K
                # 存储k和K
                k_list.insert(0, k)
                K_list.insert(0, K)
            # 更新控制序列并前向模拟
            x_new = np.copy(xs[0])
            xs_new = [x_new]
            us_new = []
            for k in range(self.N):
                du = k_list[k] + K_list[k] @ (x_new - xs[k])
                us_new.append(us[k] + du)
                x_new = self.dynamics(x_new, us_new[-1], self.dt)
                xs_new.append(x_new)
            xs = np.array(xs_new)
            us = np.array(us_new)
            # 判断收敛性
            cost = self.compute_total_cost(xs, us, trajectory, index)
            print(f"Iteration {iteration}, Cost: {cost}")
            if cost < 1e-6:
                break
        return us[0]  # 返回当前时刻的最优控制

    def dynamics(self, x, u, dt):
        """
        动力学模型
        """
        x_next = np.zeros_like(x)
        x_next[0] = x[0] + x[3] * np.cos(x[2]) * dt  # x
        x_next[1] = x[1] + x[3] * np.sin(x[2]) * dt  # y
        x_next[2] = x[2] + u[1] * dt                 # theta
        x_next[3] = x[3] + u[0] * dt                 # v
        return x_next

    def linearize_dynamics(self, x, u, dt):
        """
        线性化动力学模型，返回状态和控制的雅可比矩阵
        """
        fx = np.eye(4)
        fx[0, 2] = -x[3] * np.sin(x[2]) * dt
        fx[0, 3] = np.cos(x[2]) * dt
        fx[1, 2] = x[3] * np.cos(x[2]) * dt
        fx[1, 3] = np.sin(x[2]) * dt
        fx[2, 2] = 1.0
        fx[3, 3] = 1.0

        fu = np.zeros((4, 2))
        fu[2, 1] = dt  # theta 对 omega 的偏导
        fu[3, 0] = dt  # v 对 a 的偏导

        return fx, fu

    def compute_total_cost(self, xs, us, trajectory, index):
        """
        计算总的代价函数
        """
        cost = 0.0
        for k in range(self.N):
            xk = xs[k]
            uk = us[k]
            x_ref = trajectory.get_reference(index + k)
            dx = xk - x_ref
            cost += dx.T @ self.Q @ dx + uk.T @ self.R @ uk
        # 终端代价
        dx = xs[-1] - trajectory.get_reference(index + self.N)
        cost += dx.T @ self.Qf @ dx
        return cost

# 主函数
def main():
    vehicle = Vehicle()
    trajectory = Trajectory()
    controller = iLQRController(N=50, max_iter=10, dt=0.1)
    dt = 0.1
    x_history = []
    y_history = []
    total_time = len(trajectory.cx) - controller.N - 1

    # 创建图形
    fig, ax = plt.subplots()
    frames = []

    for t in range(total_time):
        # 获取当前最优控制
        u_opt = controller.ilqr(vehicle, trajectory, t)
        # 更新车辆状态
        vehicle.update(u_opt[0], u_opt[1], dt)
        # 记录轨迹
        x_history.append(vehicle.x)
        y_history.append(vehicle.y)

        # 绘制
        ax.cla()
        ax.plot(trajectory.cx, trajectory.cy, "-r", label="Reference Trajectory")
        ax.plot(x_history, y_history, "-b", label="Vehicle Trajectory")
        ax.set_xlim(0, 50)
        ax.set_ylim(-20, 25)
        ax.set_title(f"iLQR Trajectory Tracking - Step {t}")
        ax.set_xlabel("x [m]")
        ax.set_ylabel("y [m]")
        ax.grid(True)

        # 渲染当前帧
        fig.canvas.draw()
        image = np.frombuffer(fig.canvas.tostring_rgb(), dtype='uint8').reshape(fig.canvas.get_width_height()[::-1] + (3,))
        frames.append(image)

        # 实时显示
        plt.pause(0.001)

    # 保存为GIF
    imageio.mimsave('ilqr_trajectory_tracking.gif', frames, fps=10)
    plt.show()

if __name__ == '__main__':
    main()

2.1. 车辆模型 (Vehicle 类)

车辆模型采用一个简单的运动学模型，其中更新车辆的状态包括：

位置 (x, y)
航向角 (theta)
速度 (v)
通过给定加速度 (a) 和角速度 (omega)，每个时间步长内，车辆的状态都会更新。

class Vehicle:
    def __init__(self, x=0.0, y=0.0, theta=0.0, v=0.0):
        self.x = x
        self.y = y
        self.theta = theta  # 航向角
        self.v = v          # 速度

    def update(self, a, omega, dt):
        """
        更新车辆状态
        a: 加速度
        omega: 角速度
        dt: 时间步长
        """
        self.x += self.v * np.cos(self.theta) * dt
        self.y += self.v * np.sin(self.theta) * dt
        self.theta += omega * dt
        self.v += a * dt

2. 轨迹 (Trajectory 类)

轨迹类生成了一条目标轨迹，cx 和 cy 分别是参考轨迹的 x 和 y 轴坐标。车辆的目标是跟随这条正弦曲线，并在参考轨迹的每个点上都有相应的目标航向角 (theta) 和速度 (v)。

# 轨迹
class Trajectory:
    def __init__(self):
        self.cx = np.linspace(0, 50, 500)
        self.cy = [np.sin(ix / 5.0) * ix / 2.0 for ix in self.cx]
        self.theta = [np.arctan2(self.cy[i+1] - self.cy[i], self.cx[i+1] - self.cx[i]) if i < len(self.cx)-1 else 0.0 for i in range(len(self.cx))]
        self.v = np.full_like(self.cx, 3.0)  # 目标速度为3 m/s

    def get_reference(self, index):
        """
        获取参考轨迹点
        """
        return np.array([self.cx[index], self.cy[index], self.theta[index], self.v[index]])

3. iLQR 控制器 (iLQRController 类)

iLQR 控制器的主要职责是计算从当前时刻开始的最优控制序列，使得车辆的状态逼近目标轨迹。以下是关键步骤：

初始化：定义状态和控制权重矩阵 (Q, R, Qf) 以惩罚偏离目标状态和过大的控制输入。
前向传播：给定初始控制输入，模拟车辆状态随时间的演化。
反向递推：通过动态规划的方法，逐步计算代价函数的梯度和 Hessian，并优化控制策略。
线性化动力学：在当前状态下线性化系统的动力学方程，计算状态和控制的雅可比矩阵。
更新控制序列：通过更新控制增量，生成新的控制序列并前向传播，直到算法收敛。

# iLQR控制器
class iLQRController:
    def __init__(self, N=50, max_iter=10, dt=0.1):
        self.N = N  # 控制时域长度
        self.max_iter = max_iter  # 最大迭代次数
        self.dt = dt  # 时间步长
        self.Q = np.diag([1.0, 1.0, 0.5, 0.1])  # 状态权重矩阵
        self.R = np.diag([0.1, 0.1])  # 控制权重矩阵
        self.Qf = self.Q * 10  # 终端状态权重矩阵

    def ilqr(self, vehicle, trajectory, index):
        """
        使用iLQR计算最优控制序列
        """
        # 初始化状态和控制序列
        x_dim = 4  # 状态维度 [x, y, theta, v]
        u_dim = 2  # 控制维度 [a, omega]
        xs = np.zeros((self.N + 1, x_dim))
        us = np.zeros((self.N, u_dim))
        
        # 初始状态
        xs[0] = np.array([vehicle.x, vehicle.y, vehicle.theta, vehicle.v])
        
        # 初始猜测控制序列（全零）
        us = np.zeros((self.N, u_dim))
        
        for iteration in range(self.max_iter):
            # 前向传播，计算状态轨迹
            for k in range(self.N):
                xs[k+1] = self.dynamics(xs[k], us[k], self.dt)
            
            # 计算代价函数梯度和Hessian矩阵
            Vx = self.Qf @ (xs[-1] - trajectory.get_reference(index + self.N))
            Vxx = self.Qf
            k_list = []
            K_list = []
            for k in reversed(range(self.N)):
                xk = xs[k]
                uk = us[k]
                x_ref = trajectory.get_reference(index + k)
                # 计算状态和控制的偏导数
                fx, fu = self.linearize_dynamics(xk, uk, self.dt)
                lx = self.Q @ (xk - x_ref)
                lu = self.R @ uk
                lxx = self.Q
                luu = self.R
                lux = np.zeros((u_dim, x_dim))
                # 计算Q函数的二次近似
                Qx = lx + fx.T @ Vx
                Qu = lu + fu.T @ Vx
                Qxx = lxx + fx.T @ Vxx @ fx
                Quu = luu + fu.T @ Vxx @ fu
                Qux = lux + fu.T @ Vxx @ fx
                # 计算控制增量
                Quu_inv = np.linalg.inv(Quu + np.eye(u_dim) * 1e-6)  # 加小值防止矩阵奇异
                k = -Quu_inv @ Qu
                K = -Quu_inv @ Qux
                # 更新V函数
                Vx = Qx + K.T @ Quu @ k + K.T @ Qu + Qux.T @ k
                Vxx = Qxx + K.T @ Quu @ K + K.T @ Qux + Qux.T @ K
                # 存储k和K
                k_list.insert(0, k)
                K_list.insert(0, K)
            # 更新控制序列并前向模拟
            x_new = np.copy(xs[0])
            xs_new = [x_new]
            us_new = []
            for k in range(self.N):
                du = k_list[k] + K_list[k] @ (x_new - xs[k])
                us_new.append(us[k] + du)
                x_new = self.dynamics(x_new, us_new[-1], self.dt)
                xs_new.append(x_new)
            xs = np.array(xs_new)
            us = np.array(us_new)
            # 判断收敛性
            cost = self.compute_total_cost(xs, us, trajectory, index)
            print(f"Iteration {iteration}, Cost: {cost}")
            if cost < 1e-6:
                break
        return us[0]  # 返回当前时刻的最优控制

    def dynamics(self, x, u, dt):
        """
        动力学模型
        """
        x_next = np.zeros_like(x)
        x_next[0] = x[0] + x[3] * np.cos(x[2]) * dt  # x
        x_next[1] = x[1] + x[3] * np.sin(x[2]) * dt  # y
        x_next[2] = x[2] + u[1] * dt                 # theta
        x_next[3] = x[3] + u[0] * dt                 # v
        return x_next

    def linearize_dynamics(self, x, u, dt):
        """
        线性化动力学模型，返回状态和控制的雅可比矩阵
        """
        fx = np.eye(4)
        fx[0, 2] = -x[3] * np.sin(x[2]) * dt
        fx[0, 3] = np.cos(x[2]) * dt
        fx[1, 2] = x[3] * np.cos(x[2]) * dt
        fx[1, 3] = np.sin(x[2]) * dt
        fx[2, 2] = 1.0
        fx[3, 3] = 1.0

        fu = np.zeros((4, 2))
        fu[2, 1] = dt  # theta 对 omega 的偏导
        fu[3, 0] = dt  # v 对 a 的偏导

        return fx, fu

    def compute_total_cost(self, xs, us, trajectory, index):
        """
        计算总的代价函数
        """
        cost = 0.0
        for k in range(self.N):
            xk = xs[k]
            uk = us[k]
            x_ref = trajectory.get_reference(index + k)
            dx = xk - x_ref
            cost += dx.T @ self.Q @ dx + uk.T @ self.R @ uk
        # 终端代价
        dx = xs[-1] - trajectory.get_reference(index + self.N)
        cost += dx.T @ self.Qf @ dx
        return cost

4. 主循环 (main 函数)

主函数执行轨迹跟踪的模拟过程。每一步：

调用 iLQR 控制器生成最优控制输入。
根据控制输入更新车辆状态。
绘制当前的车辆轨迹与参考轨迹，并生成一帧图像。
最终，所有帧被保存为 GIF 文件，展示整个轨迹跟踪过程。

5. 执行结果

6. c++版本

在实际的工程开发中，一般采用c++代码进行开发。为了方便应用，将上述代码转为C++，涉及的矩阵运算主要采用eigen库（参考 C++教程（一）：超详细的C++矩阵操作和运算（附实例代码，与python对比））。同时为了方便展示动态运行过程，采用了python的matplotlib的c++库matplotlibcpp，也非常容易使用，直接include头文件即可。C++代码如下:

#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>
#include "matplotlibcpp.h"

namespace plt = matplotlibcpp;
using Eigen::MatrixXd;
using Eigen::VectorXd;

// 车辆模型类
class Vehicle {
public:
    double x, y, theta, v;

    // 车辆构造函数，初始化车辆的状态
    Vehicle(double x_init = 0.0, double y_init = 0.0, double theta_init = 0.0, double v_init = 0.0)
        : x(x_init), y(y_init), theta(theta_init), v(v_init) {}

    // 更新车辆状态，a为加速度，omega为角速度，dt为时间步长
    void update(double a, double omega, double dt) {
        x += v * cos(theta) * dt;    // 更新x坐标
        y += v * sin(theta) * dt;    // 更新y坐标
        theta += omega * dt;         // 更新航向角theta
        v += a * dt;                 // 更新速度v
    }
};

// 轨迹类，用于存储参考轨迹信息
class Trajectory {
public:
    std::vector<double> cx, cy, theta, v;

    // 轨迹构造函数，初始化参考轨迹
    Trajectory() {
        // 生成参考轨迹的x和y坐标
        for (double i = 0; i < 50; i += 0.1) {
            cx.push_back(i);
            cy.push_back(sin(i / 5.0) * i / 2.0);
        }
        // 计算每个轨迹点的航向角theta
        for (size_t i = 0; i < cx.size() - 1; ++i) {
            theta.push_back(atan2(cy[i+1] - cy[i], cx[i+1] - cx[i]));
        }
        theta.push_back(0.0);  // 最后一个点的theta设置为0
        v.assign(cx.size(), 3.0);  // 设置所有点的目标速度为3 m/s
    }

    // 获取指定索引处的参考状态向量[x, y, theta, v]
    VectorXd get_reference(size_t index) const {
        VectorXd ref(4);
        ref << cx[index], cy[index], theta[index], v[index];
        return ref;
    }
};

// iLQR（迭代线性二次调节器）控制器类
class iLQRController {
public:
    int N;             // 控制步数
    int max_iter;      // 最大迭代次数
    double dt;         // 时间步长
    MatrixXd Q, R, Qf; // 代价函数的权重矩阵

    // 控制器构造函数
    iLQRController(int N_input = 50, int max_iter_input = 10, double dt_input = 0.1)
        : N(N_input), max_iter(max_iter_input), dt(dt_input) {
        // 初始化状态代价矩阵Q，控制代价矩阵R，终端状态代价矩阵Qf
        Q = MatrixXd::Zero(4, 4);
        Q(0, 0) = 1.0; Q(1, 1) = 1.0; Q(2, 2) = 0.5; Q(3, 3) = 0.1;
        R = MatrixXd::Identity(2, 2) * 0.1;
        Qf = Q * 10.0;
    }

    // iLQR算法的实现，返回当前时刻的最优控制输入
    VectorXd ilqr(Vehicle &vehicle, const Trajectory &trajectory, size_t index) {
        int x_dim = 4;  // 状态维度 [x, y, theta, v]
        int u_dim = 2;  // 控制维度 [a, omega]
        std::vector<VectorXd> xs(N + 1, VectorXd::Zero(x_dim));  // 状态序列
        std::vector<VectorXd> us(N, VectorXd::Zero(u_dim));      // 控制序列

        // 初始化状态
        xs[0] << vehicle.x, vehicle.y, vehicle.theta, vehicle.v;

        // 迭代更新控制输入和状态
        for (int iter = 0; iter < max_iter; ++iter) {
            // 前向传播，基于当前控制序列预测状态序列
            for (int i = 0; i < N; ++i) {
                xs[i + 1] = dynamics(xs[i], us[i], dt);
            }

            // 反向传播，更新控制增量和反馈矩阵
            VectorXd Vx = Qf * (xs[N] - trajectory.get_reference(index + N));
            MatrixXd Vxx = Qf;

            std::vector<VectorXd> k_control_list(N, VectorXd::Zero(u_dim));  // 控制增量
            std::vector<MatrixXd> K_feedback_list(N, MatrixXd::Zero(u_dim, x_dim));  // 反馈矩阵

            for (int i = N - 1; i >= 0; --i) {
                VectorXd x_ref = trajectory.get_reference(index + i);
                std::pair<MatrixXd, MatrixXd> linearized = linearize_dynamics(xs[i], us[i], dt);
                MatrixXd fx = linearized.first;
                MatrixXd fu = linearized.second;

                // 计算代价梯度和Hessian矩阵
                VectorXd lx = Q * (xs[i] - x_ref);
                VectorXd lu = R * us[i];
                MatrixXd lxx = Q;
                MatrixXd luu = R;
                MatrixXd lux = MatrixXd::Zero(u_dim, x_dim);

                // 计算Q函数的二次近似
                VectorXd Qx = lx + fx.transpose() * Vx;
                VectorXd Qu = lu + fu.transpose() * Vx;
                MatrixXd Qxx = lxx + fx.transpose() * Vxx * fx;
                MatrixXd Quu = luu + fu.transpose() * Vxx * fu;
                MatrixXd Qux = lux + fu.transpose() * Vxx * fx;

                // 计算控制增量和反馈矩阵
                MatrixXd Quu_inv = Quu.inverse();
                VectorXd k_control = -Quu_inv * Qu;
                MatrixXd K_feedback = -Quu_inv * Qux;

                // 更新价值函数
                Vx = Qx + K_feedback.transpose() * Quu * k_control + K_feedback.transpose() * Qu + Qux.transpose() * k_control;
                Vxx = Qxx + K_feedback.transpose() * Quu * K_feedback + K_feedback.transpose() * Qux + Qux.transpose() * K_feedback;

                k_control_list[i] = k_control;
                K_feedback_list[i] = K_feedback;
            }

            // 更新控制序列并进行前向模拟
            std::vector<VectorXd> xs_new(N + 1, xs[0]);
            std::vector<VectorXd> us_new;

            for (int i = 0; i < N; ++i) {
                VectorXd du = k_control_list[i] + K_feedback_list[i] * (xs_new[i] - xs[i]);
                us_new.push_back(us[i] + du);
                xs_new[i + 1] = dynamics(xs_new[i], us_new.back(), dt);
            }
            xs = xs_new;
            us = us_new;

            // 判断是否收敛
            double cost = compute_total_cost(xs, us, trajectory, index);
            std::cout << "Iteration " << iter << ", Cost: " << cost << std::endl;
            if (cost < 1e-6) {
                break;
            }
        }
        return us[0];  // 返回当前时刻的最优控制输入
    }

    // 车辆动力学模型
    VectorXd dynamics(const VectorXd &x, const VectorXd &u, double dt) {
        VectorXd x_next = x;
        x_next[0] += x[3] * cos(x[2]) * dt;  // 更新x坐标
        x_next[1] += x[3] * sin(x[2]) * dt;  // 更新y坐标
        x_next[2] += u[1] * dt;              // 更新theta
        x_next[3] += u[0] * dt;              // 更新速度v
        return x_next;
    }

    // 线性化车辆动力学模型
    std::pair<MatrixXd, MatrixXd> linearize_dynamics(const VectorXd &x, const VectorXd &u, double dt) {
        MatrixXd fx = MatrixXd::Identity(4, 4);
        fx(0, 2) = -x[3] * sin(x[2]) * dt;
        fx(0, 3) = cos(x[2]) * dt;
        fx(1, 2) = x[3] * cos(x[2]) * dt;
        fx(1, 3) = sin(x[2]) * dt;

        MatrixXd fu = MatrixXd::Zero(4, 2);
        fu(2, 1) = dt;
        fu(3, 0) = dt;

        return {fx, fu};
    }

    // 计算总成本
    double compute_total_cost(const std::vector<VectorXd> &xs, const std::vector<VectorXd> &us, const Trajectory &trajectory, size_t index) {
        double cost = 0.0;
        for (int i = 0; i < N; ++i) {
            VectorXd dx = xs[i] - trajectory.get_reference(index + i);
            cost += (dx.transpose() * Q * dx)(0, 0) + (us[i].transpose() * R * us[i])(0, 0);
        }
        VectorXd dx_terminal = xs[N] - trajectory.get_reference(index + N);
        cost += (dx_terminal.transpose() * Qf * dx_terminal)(0, 0);
        return cost;
    }
};

// 主函数
int main() {
    Vehicle vehicle;  // 初始化车辆
    Trajectory trajectory;  // 初始化轨迹
    iLQRController controller(10, 10, 0.1);  // 初始化iLQR控制器
    double dt = 0.1;
    std::vector<double> x_history, y_history;  // 记录车辆轨迹

    for (size_t t = 0; t < trajectory.cx.size() - controller.N - 1; ++t) {
        VectorXd u_opt = controller.ilqr(vehicle, trajectory, t);  // 获取最优控制输入
        vehicle.update(u_opt[0], u_opt[1], dt);  // 更新车辆状态
        x_history.push_back(vehicle.x);  // 记录x坐标
        y_history.push_back(vehicle.y);  // 记录y坐标

        // 绘制车辆轨迹和参考轨迹
        plt::clf();
        plt::named_plot("Reference Trajectory", trajectory.cx, trajectory.cy, "-r");
        plt::named_plot("Vehicle Trajectory", x_history, y_history, "-b");
        plt::legend();
        plt::xlim(0, 50);
        plt::ylim(-20, 25);
        plt::title("iLQR Trajectory Tracking");
        plt::xlabel("x [m]");
        plt::ylabel("y [m]");
        plt::grid(true);
        plt::pause(0.001);  // 短暂暂停以动态展示
    }

    plt::show();  // 展示最终图形
    return 0;
}