feat: 添加注释

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
+        MPC的构造函数
-        :param N: time horizon | int
+        :param model: bicycle model object to be controlled 自行车模型对象用于控制
-        :param Q: state cost matrix
+        :param N: time horizon | int 时间范围
-        :param R: input cost matrix
+        :param Q: state cost matrix 状态成本矩阵
-        :param QN: final state cost matrix
+        :param R: input cost matrix 输入成本矩阵
-        :param StateConstraints: dictionary of state constraints
+        :param QN: final state cost matrix 最终状态成本矩阵
-        :param InputConstraints: dictionary of input constraints
+        :param StateConstraints: dictionary of state constraints 状态约束的字典
-        :param ay_max: maximum allowed lateral acceleration in curves
+        :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
+        将预测的状态转换为预测的x和y坐标。主要用于可视化目的。
-        :return: lists of predicted x and y coordinates
+        :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:

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]),

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]
+        状态向量，包含车辆姿态（x，y，psi）
-        :param y: y position in global coordinate system | [m]
+        :param x: x position in global coordinate system | [m] 全局坐标系中的 x 位置
-        :param psi: yaw angle | [rad]
+        :param y: y position in global coordinate system | [m] 全局坐标系中的 y 位置
+        :param psi: yaw angle | [rad] 偏航角
         """
         self.x = x
         self.y = y