当前位置：首页 > news >正文

用Python和cvxpy从零实现一个简易的自动驾驶轨迹跟踪器（附完整代码）

news 2026/4/22 6:29:36

用Python和cvxpy从零构建自动驾驶轨迹跟踪器：手把手实现模型预测控制

在自动驾驶系统的核心技术栈中，轨迹跟踪算法扮演着至关重要的角色。想象一下，当规划模块生成了一条理想路径后，控制模块需要像一位经验丰富的赛车手，精准地操纵车辆沿着预定路线行驶。这正是模型预测控制(Model Predictive Control, MPC)大显身手的领域——它不仅考虑当前时刻的控制决策，还能预见未来多个时间步的系统行为。

1. 环境准备与工具链配置

1.1 核心库选择与安装

实现MPC控制器需要以下Python生态的核心组件：

pip install numpy cvxpy matplotlib celluloid

各库的作用说明：

numpy：处理矩阵运算的基础库
cvxpy：凸优化求解的声明式建模工具
matplotlib：可视化跟踪效果
celluloid：生成动态演示图

提示：建议使用Python 3.8+环境，某些库的最新版本可能存在API变动

1.2 车辆运动学模型基础

我们采用简化的自行车模型来描述车辆运动：

ẋ = v * cos(ψ) ẏ = v * sin(ψ) ψ̇ = v * tan(δ)/L

其中关键参数：

(x,y)：车辆后轴中心坐标
ψ：航向角
v：车速
δ：前轮转向角
L：轴距

2. MPC控制器的数学建模

2.1 状态空间方程离散化

将连续模型离散化（采样时间dt=0.1s）：

def update_state(self, a, delta_f): self.x = self.x + self.v*math.cos(self.psi)*self.dt self.y = self.y + self.v*math.sin(self.psi)*self.dt self.psi = self.psi + self.v/self.L*math.tan(delta_f)*self.dt self.v = self.v + a*self.dt

2.2 代价函数设计

MPC的核心是最小化以下多目标函数：

min J = Σ(状态误差 + 控制量 + 控制变化率)

具体矩阵形式：

Q = np.diag([1, 1, 1]) # 状态权重 R = np.diag([0.1, 0.1]) # 控制量权重 Rd = np.diag([0.1, 0.1]) # 控制变化率权重

2.3 约束条件处理

实际车辆存在的物理限制：

约束类型	符号表示	典型值
最大转向角	δ
最大转向速率	Δδ
速度范围	v_min ≤ v ≤ v_max	0-20m/s

3. 参考轨迹生成策略

3.1 正弦波组合轨迹

self.refer_path[:,0] = np.linspace(0, 100, 1000) self.refer_path[:,1] = 2*np.sin(self.refer_path[:,0]/3.0) + 2.5*np.cos(self.refer_path[:,0]/2.0)

3.2 轨迹特征计算

通过差分法实时计算：

切线方向（用于航向角参考）
曲率（用于前轮转角参考）

dx = path[i+1,0] - path[i,0] dy = path[i+1,1] - path[i,1] yaw = math.atan2(dy, dx)

4. CVXPY优化求解实现

4.1 优化变量定义

x = cvxpy.Variable((NX, T + 1)) # 状态序列 u = cvxpy.Variable((NU, T)) # 控制序列

4.2 滚动时域优化框架

for t in range(T): cost += cvxpy.quad_form(u[:,t]-delta_ref[:,t], R) if t != 0: cost += cvxpy.quad_form(x[:,t]-xref[:,t], Q) # 添加系统动态约束 constraints += [x[:,t+1] == A@x[:,t] + B@u[:,t]]

4.3 实时闭环控制流程

获取当前车辆状态
计算未来N步的参考轨迹
求解MPC优化问题
应用第一个控制量
下一时间步重复

5. 可视化与调试技巧

5.1 实时绘图配置

plt.plot(ref_path[:,0], ref_path[:,1], '-.b', label="参考路径") plt.plot(x_hist, y_hist, '-r', linewidth=2, label="实际轨迹") plt.scatter(x_ref, y_ref, c='g', marker='*', label="目标点")