1. 无人驾驶轨迹跟踪基础与工具链搭建
在智能驾驶系统开发中,基于运动学的轨迹跟踪是路径规划与控制的基石。不同于复杂的动力学模型,运动学模型通过简化车辆运动特性,在保证精度的前提下显著降低了计算复杂度。我们采用Matlab 2021a作为算法开发平台,配合Carsim 2020进行高保真车辆动力学仿真,形成完整的开发验证闭环。
1.1 运动学模型核心假设
车辆运动学建模基于三个关键假设:
- 忽略轮胎侧偏特性,假设轮胎始终沿轮平面方向滚动
- 不考虑悬架系统运动对车身姿态的影响
- 假设地面与轮胎接触面具有理想摩擦条件
这些假设使得我们可以用简单的几何关系描述车辆运动。典型的自行车模型将四轮车辆简化为前后两轮,进一步将前后轮合并为单轮表示,最终形成如图1所示的简化模型结构。
注意:运动学模型在低速场景(通常<5m/s)下精度较高,当车速提升或需要精确控制时需切换至动力学模型
1.2 开发环境配置要点
Matlab与Carsim联合仿真需要特别注意以下配置细节:
- 接口配置:
matlab复制% Carsim初始化命令
csim_init = 'Carsim_Initialize';
[status, result] = system(csim_init);
if status ~= 0
error('Carsim初始化失败: %s', result);
end
- 参数同步机制:
- 采样时间必须严格对齐(建议0.01-0.05s)
- 单位系统统一采用SI标准单位制
- 坐标系定义遵循ISO标准(X向前,Y向左,Z向上)
- 实时数据交换:
建立共享内存区域用于传输:
- 车辆状态(位置、速度、航向角)
- 控制指令(转向角、加速度)
- 环境信息(参考路径、障碍物)
2. 车辆运动学模型深度解析
2.1 基础模型构建
考虑如图2所示的自行车模型,其状态方程可表示为:
code复制ẋ = v * cos(θ + β)
ẏ = v * sin(θ + β)
θ̇ = (v * cosβ / L) * tanδ
其中β为质心侧偏角,在纯运动学模型中通常设β=0。由此得到简化模型:
matlab复制function [x_dot, y_dot, theta_dot] = kinematic_model(v, delta, theta, L)
x_dot = v * cos(theta);
y_dot = v * sin(theta);
theta_dot = (v / L) * tan(delta);
end
2.2 模型改进与参数辨识
实际应用中需考虑以下改进:
- 转向系统延迟:
matlab复制% 一阶延迟模型
tau = 0.2; % 转向系统时间常数
delta_actual = delta_cmd * (1 - exp(-t/tau));
- 速度相关参数校准:
通过实车测试数据辨识得到速度-转向灵敏度关系:
matlab复制L_eff = L * (1 + 0.05*(v - 10)); % 速度补偿项
- 路面坡度补偿:
matlab复制g = 9.81;
phi = atan(slope); % 坡度角
v_dot = (F_tractive - R_x - m*g*sin(phi))/m;
3. 模型预测控制算法实现
3.1 预测模型构建
采用离散化处理,设采样时间为Ts:
matlab复制function X_next = predict_model(X, U, Ts, L)
% X = [x; y; theta]
% U = [v; delta]
x_next = X(1) + U(1)*cos(X(3))*Ts;
y_next = X(2) + U(1)*sin(X(3))*Ts;
theta_next = X(3) + (U(1)/L)*tan(U(2))*Ts;
X_next = [x_next; y_next; theta_next];
end
3.2 代价函数设计
综合考虑路径跟踪误差和控制平滑性:
matlab复制function J = cost_function(X_ref, U_ref, X_pred, U_pred, Q, R)
% Q = diag([q1, q2, q3]) 状态权重
% R = diag([r1, r2]) 控制权重
state_error = X_ref - X_pred;
control_error = U_ref - U_pred;
J = state_error'*Q*state_error + control_error'*R*control_error;
end
3.3 约束条件处理
考虑车辆物理限制:
- 转向角约束:
matlab复制delta_min = -30*pi/180; % 最小转向角
delta_max = 30*pi/180; % 最大转向角
- 转向速率约束:
matlab复制delta_dot_max = 15*pi/180; % 最大转向速率[rad/s]
- 加速度约束:
matlab复制a_min = -3.0; % 最大减速度[m/s^2]
a_max = 2.0; % 最大加速度[m/s^2]
4. 典型工况实现与调试
4.1 直线跟踪实现
直线作为基础工况,重点验证航向控制精度:
matlab复制ref_path = [linspace(0,100,1000)', zeros(1000,1)];
Kp_yaw = 1.5; % 航向角比例增益
for k = 1:length(ref_path)
yaw_error = atan2(ref_path(k,2)-y_curr, ref_path(k,1)-x_curr) - theta_curr;
delta = Kp_yaw * yaw_error;
% 应用转向约束
delta = max(min(delta, delta_max), delta_min);
% 更新车辆状态
[x_curr, y_curr, theta_curr] = update_state(x_curr, y_curr, theta_curr, v, delta, Ts, L);
end
4.2 圆形路径跟踪
验证横向控制性能:
matlab复制R = 20; % 圆半径[m]
for theta = 0:0.01:2*pi
ref_x = R*cos(theta);
ref_y = R*sin(theta);
ref_yaw = theta + pi/2;
% 计算横向误差
e_y = -(x_curr-ref_x)*sin(ref_yaw) + (y_curr-ref_y)*cos(ref_yaw);
% 前馈+反馈控制
delta_ff = atan(L/R); % 前馈转向
delta_fb = Kp_lat * e_y; % 反馈补偿
delta = delta_ff + delta_fb;
% 状态更新
[x_curr, y_curr, theta_curr] = update_state(x_curr, y_curr, theta_curr, v, delta, Ts, L);
end
4.3 双移线工况调试
双移线工况对控制器的响应速度提出挑战:
matlab复制% 双移线参考路径生成
x_ref = linspace(0, 150, 1000)';
y_ref = 1.5*sin(x_ref/15) + 0.5*sin(x_ref/5);
% MPC控制器参数
N = 10; % 预测步长
for k = 1:length(x_ref)-N
% 获取参考轨迹窗口
ref_window = [x_ref(k:k+N), y_ref(k:k+N)];
% 求解优化问题
U_opt = mpc_solver(current_state, ref_window);
% 应用控制量
[x_curr, y_curr, theta_curr] = update_state(x_curr, y_curr, theta_curr, U_opt(1), U_opt(2), Ts, L);
end
5. 工程实践中的关键问题
5.1 参数敏感性分析
通过蒙特卡洛仿真评估关键参数影响:
| 参数 | 允许偏差范围 | 跟踪误差变化率 |
|---|---|---|
| 轴距L | ±5% | +18%/-15% |
| 车速v | ±10% | +25% |
| 转向延迟τ | ±30ms | +40% |
| 轮胎半径 | ±2% | <5% |
5.2 实时性优化技巧
- 热启动技术:
matlab复制% 使用上一周期解作为初始猜测
options = optimoptions('fmincon','InitialGuess', U_prev);
- 模型简化:
- 减少预测步长(N=5~10)
- 采用线性时变模型近似
- 代码生成:
matlab复制% 将控制器编译为C代码
cfg = coder.config('lib');
codegen('mpc_controller.m', '-config', cfg);
5.3 典型故障排查指南
- 发散振荡:
- 检查预测时域与车速匹配关系:T_predict = NTs ≈ 1.5(L/v)
- 验证权重矩阵Q/R比值是否合理
- 稳态误差:
- 增加积分项或终端代价
- 检查参考路径曲率是否连续
- 求解失败:
- 放宽约束边界
- 检查雅可比矩阵条件数
- 验证初始猜测可行性
在实际项目中,我们发现车辆质量变化对控制性能影响显著。通过在线参数估计可提升适应性:
matlab复制% 递归最小二乘参数估计
P = 1e6*eye(2); % 初始协方差矩阵
lambda = 0.99; % 遗忘因子
for k = 1:N
phi = [a_cmd(k); -sign(v(k))]/m_nominal;
K = P*phi/(lambda + phi'*P*phi);
m_est = m_est + K*(a_actual(k) - phi'*m_est);
P = (eye(2) - K*phi')*P/lambda;
end
经过多个项目验证,这套方法在80km/h以下工况能保持横向误差<0.3m,航向误差<5°,满足大多数L2+级自动驾驶需求。对于更高性能要求,建议引入动力学模型补偿或前馈控制。