### 实现基于模型预测控制（MPC）的小车路径跟踪 为了使小车能够沿着预定的轨迹移动，在允许的误差范围内精确地跟随设定路径，可以采用模型预测控制（MPC）。该方法依赖于对未来一段时间内的行为做出最优决策的能力。具体来说，就是根据当前的状态估计未来的行为，并调整控制量使得系统的响应尽可能接近期望的目标。 #### 小车运动学模型建立 考虑到两轮差动驱动机器人，其动力学特性可以通过简单的自行车模型来描述。此模型假设前轴中心点的速度矢量方向始终指向转向角的方向。对于此类系统而言，状态变量通常包括位置坐标\( (x, y) \)，航向角度 \( \theta \)，以及线速度 \( v \)[^1]。 ```python import numpy as np def bicycle_model(x, u, dt=0.1): """ Bicycle model for differential drive robot. Parameters: x : array_like Current state vector [x(m), y(m), yaw(rad), v(m/s)]. u : array_like Input vector [acceleration(m/s²), steering_angle(rad)]. dt : float Time step. Returns: Updated state vector after applying inputs over time interval `dt`. """ F = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 0]]) B = np.array([[(u[1]*np.cos(x[2]) - u[0]*np.sin(x[2]))*dt**2/2], [(u[1]*np.sin(x[2]) + u[0]*np.cos(x[2]))*dt**2/2], [u[1]*dt], [u[0]*dt]]) return (F @ x.reshape(-1, 1) + B).flatten() ``` #### 构造代价函数 在MPC框架下，需要定义一个合适的成本函数用于评估不同动作序列的好坏程度。这里考虑最小化实际行驶路线与理想参考路径之间的偏差，同时也要惩罚过大的加速度变化以保持平稳运行[^2]。 ```python from scipy.optimize import minimize def cost_function(u_seq, *args): """Cost function to be minimized by MPC.""" ref_path, init_state, Np = args # Initialize variables total_cost = 0 current_state = init_state.copy() for k in range(Np): # Predict next states using the dynamic model predicted_states = [] for i in range(k+1): pred_x = bicycle_model(current_state, u_seq[i]) predicted_states.append(pred_x) # Calculate tracking error at each prediction horizon point errors = [np.linalg.norm(ref_path[k+i][:2]-pred[:2])**2 \ for i,pred in enumerate(predicted_states)] # Add up all individual costs within this iteration window total_cost += sum(errors) # Move forward one step along reference trajectory if k < len(ref_path)-1: current_state = ref_path[k+1] return total_cost ``` #### 执行优化过程并获取最佳控制指令 有了上述准备之后，就可以调用SciPy中的`minimize()`函数来进行数值最优化计算了。它会尝试找到一组能使总费用最低化的输入参数\[a_0,\delta_0\],[a_1,\delta_1\],...,[a_{N_p-1},\delta_{N_p-1}]作为最终输出给执行器使用的命令值。 ```python def mpc_controller(ref_path, initial_state, Np=10): """ Model predictive controller that outputs optimal control actions based on given path and vehicle's status. Args: ref_path(list of lists or ndarray): Reference waypoints [[x,y,...]] defining desired travel route. initial_state(array-like): Initial condition [x_init, y_init, theta_init, vel_init]. Np(int): Prediction horizon length indicating how many steps ahead we look when planning future moves. Yields: Generator yielding successive pairs of acceleration & steering angle commands intended to guide the car towards its goal efficiently while adhering closely enough to planned course. """ bounds=[[-1., 1.],[-np.pi/6,np.pi/6]] res=minimize(cost_function, x0=np.zeros((Np,2)), method='SLSQP', jac=False, options={'disp': False}, constraints=(), bounds=bounds*(Np), args=(ref_path,initial_state,Np)) yield from zip(*res.x.reshape((-1,2)).T.tolist()) ``` 通过这种方式，实现了基于模型预测控制理论下的路径追踪功能。每次迭代都会重新计算新的最优解集合作为下一步行动指南，从而确保整个行程过程中都能较好地贴合预设线路的要求。