最优化理论与自动驾驶（十一）：基于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 = xself.y = yself.theta = theta  # 航向角self.v = v          # 速度def update(self, a, omega, dt):"""更新车辆状态a: 加速度omega: 角速度dt: 时间步长"""self.x += self.v * np.cos(self.theta) * dtself.y += self.v * np.sin(self.theta) * dtself.theta += omega * dtself.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/sdef 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.Qfk_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 @ uklxx = self.Qluu = self.Rlux = np.zeros((u_dim, x_dim))# 计算Q函数的二次近似Qx = lx + fx.T @ VxQu = lu + fu.T @ VxQxx = lxx + fx.T @ Vxx @ fxQuu = luu + fu.T @ Vxx @ fuQux = lux + fu.T @ Vxx @ fx# 计算控制增量Quu_inv = np.linalg.inv(Quu + np.eye(u_dim) * 1e-6)  # 加小值防止矩阵奇异k = -Quu_inv @ QuK = -Quu_inv @ Qux# 更新V函数Vx = Qx + K.T @ Quu @ k + K.T @ Qu + Qux.T @ kVxx = Qxx + K.T @ Quu @ K + K.T @ Qux + Qux.T @ K# 存储k和Kk_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:breakreturn 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  # xx_next[1] = x[1] + x[3] * np.sin(x[2]) * dt  # yx_next[2] = x[2] + u[1] * dt                 # thetax_next[3] = x[3] + u[0] * dt                 # vreturn x_nextdef linearize_dynamics(self, x, u, dt):"""线性化动力学模型，返回状态和控制的雅可比矩阵"""fx = np.eye(4)fx[0, 2] = -x[3] * np.sin(x[2]) * dtfx[0, 3] = np.cos(x[2]) * dtfx[1, 2] = x[3] * np.cos(x[2]) * dtfx[1, 3] = np.sin(x[2]) * dtfx[2, 2] = 1.0fx[3, 3] = 1.0fu = np.zeros((4, 2))fu[2, 1] = dt  # theta 对 omega 的偏导fu[3, 0] = dt  # v 对 a 的偏导return fx, fudef compute_total_cost(self, xs, us, trajectory, index):"""计算总的代价函数"""cost = 0.0for k in range(self.N):xk = xs[k]uk = us[k]x_ref = trajectory.get_reference(index + k)dx = xk - x_refcost += dx.T @ self.Q @ dx + uk.T @ self.R @ uk# 终端代价dx = xs[-1] - trajectory.get_reference(index + self.N)cost += dx.T @ self.Qf @ dxreturn cost# 主函数
def main():vehicle = Vehicle()trajectory = Trajectory()controller = iLQRController(N=50, max_iter=10, dt=0.1)dt = 0.1x_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)# 保存为GIFimageio.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 = xself.y = yself.theta = theta  # 航向角self.v = v          # 速度def update(self, a, omega, dt):"""更新车辆状态a: 加速度omega: 角速度dt: 时间步长"""self.x += self.v * np.cos(self.theta) * dtself.y += self.v * np.sin(self.theta) * dtself.theta += omega * dtself.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/sdef 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.Qfk_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 @ uklxx = self.Qluu = self.Rlux = np.zeros((u_dim, x_dim))# 计算Q函数的二次近似Qx = lx + fx.T @ VxQu = lu + fu.T @ VxQxx = lxx + fx.T @ Vxx @ fxQuu = luu + fu.T @ Vxx @ fuQux = lux + fu.T @ Vxx @ fx# 计算控制增量Quu_inv = np.linalg.inv(Quu + np.eye(u_dim) * 1e-6)  # 加小值防止矩阵奇异k = -Quu_inv @ QuK = -Quu_inv @ Qux# 更新V函数Vx = Qx + K.T @ Quu @ k + K.T @ Qu + Qux.T @ kVxx = Qxx + K.T @ Quu @ K + K.T @ Qux + Qux.T @ K# 存储k和Kk_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:breakreturn 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  # xx_next[1] = x[1] + x[3] * np.sin(x[2]) * dt  # yx_next[2] = x[2] + u[1] * dt                 # thetax_next[3] = x[3] + u[0] * dt                 # vreturn x_nextdef linearize_dynamics(self, x, u, dt):"""线性化动力学模型，返回状态和控制的雅可比矩阵"""fx = np.eye(4)fx[0, 2] = -x[3] * np.sin(x[2]) * dtfx[0, 3] = np.cos(x[2]) * dtfx[1, 2] = x[3] * np.cos(x[2]) * dtfx[1, 3] = np.sin(x[2]) * dtfx[2, 2] = 1.0fx[3, 3] = 1.0fu = np.zeros((4, 2))fu[2, 1] = dt  # theta 对 omega 的偏导fu[3, 0] = dt  # v 对 a 的偏导return fx, fudef compute_total_cost(self, xs, us, trajectory, index):"""计算总的代价函数"""cost = 0.0for k in range(self.N):xk = xs[k]uk = us[k]x_ref = trajectory.get_reference(index + k)dx = xk - x_refcost += dx.T @ self.Q @ dx + uk.T @ self.R @ uk# 终端代价dx = xs[-1] - trajectory.get_reference(index + self.N)cost += dx.T @ self.Qf @ dxreturn 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;         // 更新航向角thetav += 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);}// 计算每个轨迹点的航向角thetafor (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设置为0v.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，终端状态代价矩阵QfQ = 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;              // 更新thetax_next[3] += u[0] * dt;              // 更新速度vreturn 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;
}