From 469d42f4ac7ea7979e45b450d8acc54ef22425c4 Mon Sep 17 00:00:00 2001 From: 12345qiupeng Date: Fri, 29 Nov 2024 14:19:06 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/MPC.py | 72 +++++++++++++++++++++++++++++------ src/simulation.py | 1 + src/spatial_bicycle_models.py | 7 ++-- 3 files changed, 66 insertions(+), 14 deletions(-) diff --git a/src/MPC.py b/src/MPC.py index a924ffc..4b68099 100644 --- a/src/MPC.py +++ b/src/MPC.py @@ -16,14 +16,15 @@ class MPC: ay_max): """ Constructor for the Model Predictive Controller. - :param model: bicycle model object to be controlled - :param N: time horizon | int - :param Q: state cost matrix - :param R: input cost matrix - :param QN: final state cost matrix - :param StateConstraints: dictionary of state constraints - :param InputConstraints: dictionary of input constraints - :param ay_max: maximum allowed lateral acceleration in curves + MPC的构造函数 + :param model: bicycle model object to be controlled 自行车模型对象用于控制 + :param N: time horizon | int 时间范围 + :param Q: state cost matrix 状态成本矩阵 + :param R: input cost matrix 输入成本矩阵 + :param QN: final state cost matrix 最终状态成本矩阵 + :param StateConstraints: dictionary of state constraints 状态约束的字典 + :param InputConstraints: dictionary of input constraints 输入约束的字典 + :param ay_max: maximum allowed lateral acceleration in curves 允许的最大横向加速度 """ # Parameters @@ -36,7 +37,7 @@ class MPC: self.model = model # Dimensions - self.nx = self.model.n_states + self.nx = self.model.n_states # 状态变量的数量 self.nu = 2 # Constraints @@ -44,52 +45,67 @@ class MPC: self.input_constraints = InputConstraints # Maximum lateral acceleration + # 最大横向加速度 self.ay_max = ay_max # Current control and prediction + # 当前控制和预测 self.current_prediction = None # Counter for old control signals in case of infeasible problem + # 如果问题不可行，旧控制信号的计数器 self.infeasibility_counter = 0 # Current control signals + # 当前控制信号 self.current_control = np.zeros((self.nu*self.N)) # Initialize Optimization Problem + # 初始化优化问题 self.optimizer = osqp.OSQP() def _init_problem(self): """ Initialize optimization problem for current time step. + 为当前时间步初始化优化问题 """ # Constraints + # 加载约束 umin = self.input_constraints['umin'] umax = self.input_constraints['umax'] xmin = self.state_constraints['xmin'] xmax = self.state_constraints['xmax'] # LTV System Matrices + # LTV系统矩阵 A = np.zeros((self.nx * (self.N + 1), self.nx * (self.N + 1))) B = np.zeros((self.nx * (self.N + 1), self.nu * (self.N))) # Reference vector for state and input variables + # 状态和输入变量的参考向量 ur = np.zeros(self.nu*self.N) xr = np.zeros(self.nx*(self.N+1)) # Offset for equality constraint (due to B * (u - ur)) + # 等式约束的偏移（由于B *（u - ur）） uq = np.zeros(self.N * self.nx) # Dynamic state constraints + # 动态状态约束 xmin_dyn = np.kron(np.ones(self.N + 1), xmin) xmax_dyn = np.kron(np.ones(self.N + 1), xmax) # Dynamic input constraints + # 动态输入约束 umax_dyn = np.kron(np.ones(self.N), umax) # Get curvature predictions from previous control signals + # 从先前的控制信号中获取曲率预测 kappa_pred = np.tan(np.array(self.current_control[3::] + self.current_control[-1:])) / self.model.length # Iterate over horizon + # 遍历时间范围 for n in range(self.N): # Get information about current waypoint + # 获取当前路标的信息 current_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n) next_waypoint = self.model.reference_path.get_waypoint(self.model.wp_id + n + 1) delta_s = next_waypoint - current_waypoint @@ -97,22 +113,27 @@ class MPC: v_ref = current_waypoint.v_ref # Compute LTV matrices + # 计算LTV矩阵 f, A_lin, B_lin = self.model.linearize(v_ref, kappa_ref, delta_s) A[(n+1) * self.nx: (n+2)*self.nx, n * self.nx:(n+1)*self.nx] = A_lin B[(n+1) * self.nx: (n+2)*self.nx, n * self.nu:(n+1)*self.nu] = B_lin # Set reference for input signal + # 设置输入信号的参考值 ur[n*self.nu:(n+1)*self.nu] = np.array([v_ref, kappa_ref]) # Compute equality constraint offset (B*ur) + # 计算等式约束偏移（B * ur） uq[n * self.nx:(n+1)*self.nx] = B_lin.dot(np.array ([v_ref, kappa_ref])) - f # Constrain maximum speed based on predicted car curvature + # 根据预测的汽车曲率限制最大速度 vmax_dyn = np.sqrt(self.ay_max / (np.abs(kappa_pred[n]) + 1e-12)) if vmax_dyn < umax_dyn[self.nu*n]: umax_dyn[self.nu*n] = vmax_dyn # Compute dynamic constraints on e_y + # 计算e_y的动态约束 ub, lb, _ = self.model.reference_path.update_path_constraints( self.model.wp_id+1, self.N, 2*self.model.safety_margin, self.model.safety_margin) @@ -122,31 +143,39 @@ class MPC: xmax_dyn[self.nx::self.nx] = ub # Set reference for state as center-line of drivable area + # 将状态的参考值设置为可驾驶区域的中心线 xr[self.nx::self.nx] = (lb + ub) / 2 # Get equality matrix + # 获取等式矩阵 Ax = sparse.kron(sparse.eye(self.N + 1), -sparse.eye(self.nx)) + sparse.csc_matrix(A) Bu = sparse.csc_matrix(B) Aeq = sparse.hstack([Ax, Bu]) # Get inequality matrix + # 获取不等式矩阵 Aineq = sparse.eye((self.N + 1) * self.nx + self.N * self.nu) # Combine constraint matrices + # 组合约束矩阵 A = sparse.vstack([Aeq, Aineq], format='csc') # Get upper and lower bound vectors for equality constraints + # 获取等式约束的上下限向量 lineq = np.hstack([xmin_dyn, np.kron(np.ones(self.N), umin)]) uineq = np.hstack([xmax_dyn, umax_dyn]) # Get upper and lower bound vectors for inequality constraints + # 获取不等式约束的上下限向量 x0 = np.array(self.model.spatial_state[:]) leq = np.hstack([-x0, uq]) ueq = leq # Combine upper and lower bound vectors + # 组合上下限向量 l = np.hstack([leq, lineq]) u = np.hstack([ueq, uineq]) # Set cost matrices + # 设置成本矩阵 P = sparse.block_diag([sparse.kron(sparse.eye(self.N), self.Q), self.QN, sparse.kron(sparse.eye(self.N), self.R)], format='csc') q = np.hstack( @@ -155,6 +184,7 @@ class MPC: -np.tile(self.R.diagonal(), self.N) * ur]) # Initialize optimizer + # 初始化优化器 self.optimizer = osqp.OSQP() self.optimizer.setup(P=P, q=q, A=A, l=l, u=u, verbose=False) @@ -162,28 +192,35 @@ class MPC: """ Get control signal given the current position of the car. Solves a finite time optimization problem based on the linearized car model. + 给定车辆的当前位置，获取控制信号。基于线性化的汽车模型解决有限时间优化问题。 """ # Number of state variables + # 状态变量的数量 nx = self.model.n_states nu = 2 # Update current waypoint + # 更新当前路标 self.model.get_current_waypoint() # Update spatial state + # 更新空间状态 self.model.spatial_state = self.model.t2s(reference_state= self.model.temporal_state, reference_waypoint= self.model.current_waypoint) # Initialize optimization problem + # 初始化优化问题 self._init_problem() # Solve optimization problem + # 解决优化问题 dec = self.optimizer.solve() try: # Get control signals + # 获取控制信号 control_signals = np.array(dec.x[-self.N*nu:]) control_signals[1::2] = np.arctan(control_signals[1::2] * self.model.length) @@ -191,18 +228,23 @@ class MPC: delta = control_signals[1] # Update control signals + # 更新控制信号 self.current_control = control_signals # Get predicted spatial states + # 获取预测的空间状态 x = np.reshape(dec.x[:(self.N+1)*nx], (self.N+1, nx)) # Update predicted temporal states + # 更新预测的时间状态 self.current_prediction = self.update_prediction(x) # Get current control signal + # 获取当前控制信号 u = np.array([v, delta]) # if problem solved, reset infeasibility counter + # 如果问题解决了，重置不可行计数器 self.infeasibility_counter = 0 except: @@ -213,6 +255,7 @@ class MPC: u = np.array(self.current_control[id:id+2]) # increase infeasibility counter + # 增加不可行计数器 self.infeasibility_counter += 1 if self.infeasibility_counter == (self.N - 1): @@ -225,23 +268,29 @@ class MPC: """ Transform the predicted states to predicted x and y coordinates. Mainly for visualization purposes. - :param spatial_state_prediction: list of predicted state variables - :return: lists of predicted x and y coordinates + 将预测的状态转换为预测的x和y坐标。主要用于可视化目的。 + :param spatial_state_prediction: list of predicted state variables 预测状态变量的列表 + :return: lists of predicted x and y coordinates 预测的x和y坐标的列表 """ # Containers for x and y coordinates of predicted states + # 预测状态的x和y坐标的容器 x_pred, y_pred = [], [] # Iterate over prediction horizon + # 遍历预测范围 for n in range(2, self.N): # Get associated waypoint + # 获取关联的路标 associated_waypoint = self.model.reference_path.\ get_waypoint(self.model.wp_id+n) # Transform predicted spatial state to temporal state + # 将预测的空间状态转换为时间状态 predicted_temporal_state = self.model.s2t(associated_waypoint, spatial_state_prediction[n, :]) # Save predicted coordinates in world coordinate frame + # 保存世界坐标系中的预测坐标 x_pred.append(predicted_temporal_state.x) y_pred.append(predicted_temporal_state.y) @@ -250,6 +299,7 @@ class MPC: def show_prediction(self): """ Display predicted car trajectory in current axis. + 在当前轴中显示预测的汽车轨迹。 """ if self.current_prediction is not None: diff --git a/src/simulation.py b/src/simulation.py index cf35e94..3b47b19 100644 --- a/src/simulation.py +++ b/src/simulation.py @@ -105,6 +105,7 @@ if __name__ == '__main__': v_max = 1.0 # m/s delta_max = 0.66 # rad ay_max = 4.0 # m/s^2 + # 控制量的约束，速度和转弯半径 InputConstraints = {'umin': np.array([0.0, -np.tan(delta_max)/car.length]), 'umax': np.array([v_max, np.tan(delta_max)/car.length])} StateConstraints = {'xmin': np.array([-np.inf, -np.inf, -np.inf]), diff --git a/src/spatial_bicycle_models.py b/src/spatial_bicycle_models.py index 0b770ca..f693b3a 100644 --- a/src/spatial_bicycle_models.py +++ b/src/spatial_bicycle_models.py @@ -26,9 +26,10 @@ class TemporalState: def __init__(self, x, y, psi): """ Temporal State Vector containing car pose (x, y, psi) - :param x: x position in global coordinate system | [m] - :param y: y position in global coordinate system | [m] - :param psi: yaw angle | [rad] + 状态向量，包含车辆姿态（x，y，psi） + :param x: x position in global coordinate system | [m] 全局坐标系中的 x 位置 + :param y: y position in global coordinate system | [m] 全局坐标系中的 y 位置 + :param psi: yaw angle | [rad] 偏航角 """ self.x = x self.y = y