309 lines
12 KiB
Python
309 lines
12 KiB
Python
import numpy as np
|
||
import osqp
|
||
from scipy import sparse
|
||
import matplotlib.pyplot as plt
|
||
|
||
# Colors
|
||
PREDICTION = '#BA4A00'
|
||
|
||
##################
|
||
# MPC Controller #
|
||
##################
|
||
|
||
|
||
class MPC:
|
||
def __init__(self, model, N, Q, R, QN, StateConstraints, InputConstraints,
|
||
ay_max):
|
||
"""
|
||
Constructor for the Model Predictive Controller.
|
||
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
|
||
self.N = N # horizon
|
||
self.Q = Q # weight matrix state vector
|
||
self.R = R # weight matrix input vector
|
||
self.QN = QN # weight matrix terminal
|
||
|
||
# Model
|
||
self.model = model
|
||
|
||
# Dimensions
|
||
self.nx = self.model.n_states # 状态变量的数量
|
||
self.nu = 2
|
||
|
||
# Constraints
|
||
self.state_constraints = StateConstraints
|
||
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
|
||
kappa_ref = current_waypoint.kappa
|
||
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)
|
||
xmin_dyn[0] = self.model.spatial_state.e_y
|
||
xmax_dyn[0] = self.model.spatial_state.e_y
|
||
xmin_dyn[self.nx::self.nx] = lb
|
||
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(
|
||
[-np.tile(self.Q.diagonal(), self.N) * xr[:-self.nx],
|
||
-self.QN.dot(xr[-self.nx:]),
|
||
-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)
|
||
|
||
def get_control(self):
|
||
"""
|
||
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)
|
||
v = control_signals[0]
|
||
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:
|
||
|
||
print('Infeasible problem. Previously predicted'
|
||
' control signal used!')
|
||
id = nu * (self.infeasibility_counter + 1)
|
||
u = np.array(self.current_control[id:id+2])
|
||
|
||
# increase infeasibility counter
|
||
# 增加不可行计数器
|
||
self.infeasibility_counter += 1
|
||
|
||
if self.infeasibility_counter == (self.N - 1):
|
||
print('No control signal computed!')
|
||
exit(1)
|
||
|
||
return u
|
||
|
||
def update_prediction(self, spatial_state_prediction):
|
||
"""
|
||
Transform the predicted states to predicted x and y coordinates.
|
||
Mainly for visualization purposes.
|
||
将预测的状态转换为预测的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)
|
||
|
||
return x_pred, y_pred
|
||
|
||
def show_prediction(self):
|
||
"""
|
||
Display predicted car trajectory in current axis.
|
||
在当前轴中显示预测的汽车轨迹。
|
||
"""
|
||
|
||
if self.current_prediction is not None:
|
||
plt.scatter(self.current_prediction[0], self.current_prediction[1],
|
||
c=PREDICTION, s=30)
|
||
|