先放两张笔记：
MPC模型预测控制，实际上是基于车辆模型构造二次规划的二次项矩阵、一次项矩阵，使其在满足约束下使目标函数最优。
代码讲解：
#主循环
for i in range(500):
robot_state = np.zeros(4)
robot_state[0] = ugv.x
robot_state[1] = ugv.y
robot_state[2] = ugv.psi
robot_state[3] = ugv.v
x0 = robot_state[0:3] #x0是当前状态 xref是预测状态 dref是控制输入
xref, target_ind, dref = reference_path.calc_ref_trajectory(
robot_state) #计算预测点
opt_v, opt_delta, opt_x, opt_y, opt_yaw = linear_mpc_control(
xref, x0, dref, ugv)
ugv.update_state(0, opt_delta[0]) # 加速度设为0，恒速
x_.append(ugv.x)
y_.append(ugv.y)
if np.linalg.norm(robot_state[0:2]-goal) <= 0.1:
print("reach goal")
break
#返回预测点
def calc_ref_trajectory(self, robot_state, dl=1.0):
"""计算参考轨迹点，统一化变量数组，便于后面MPC优化使用
参考自https://github.com/AtsushiSakai/PythonRobotics/blob/eb6d1cbe6fc90c7be9210bf153b3a04f177cc138/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
Args:
robot_state (_type_): 车辆的状态(x,y,yaw,v)
dl (float, optional): _description_. Defaults to 1.0.
Returns:
_type_: _description_
"""
e, k, ref_yaw, ind = self.calc_track_error(
robot_state[0], robot_state[1])
xref = np.zeros((NX, T + 1))
dref = np.zeros((NU, T))
ncourse = len(self.refer_path)
xref[0, 0] = self.refer_path[ind, 0]
xref[1, 0] = self.refer_path[ind, 1]
xref[2, 0] = self.refer_path[ind, 2]
# 参考控制量[v,delta]
ref_delta = math.atan2(L*k, 1)
dref[0, :] = robot_state[3]
dref[1, :] = ref_delta
travel = 0.0
for i in range(T + 1): #这里的作用应该是在最近路径点后再找T个点 算作预测状态
travel += abs(robot_state[3]) * dt
dind = int(round(travel / dl))
if (ind + dind) < ncourse:
xref[0, i] = self.refer_path[ind + dind, 0]
xref[1, i] = self.refer_path[ind + dind, 1]
xref[2, i] = self.refer_path[ind + dind, 2]
else:
xref[0, i] = self.refer_path[ncourse - 1, 0]
xref[1, i] = self.refer_path[ncourse - 1, 1]
xref[2, i] = self.refer_path[ncourse - 1, 2]
return xref, ind, dref
#计算最近的点误差并返回最近点坐标
def calc_track_error(self, x, y):
"""计算跟踪误差
Args:
x (_type_): 当前车辆的位置x
y (_type_): 当前车辆的位置y
Returns:
_type_: _description_ 返回当前点和曲线上的最近的点的误差 和曲线上那个最近点的曲率 航向角 和最近点的索引
"""
# 寻找参考轨迹最近目标点
d_x = [self.refer_path[i, 0]-x for i in range(len(self.refer_path))]
d_y = [self.refer_path[i, 1]-y for i in range(len(self.refer_path))]
d = [np.sqrt(d_x[i]**2+d_y[i]**2) for i in range(len(d_x))]
s = np.argmin(d) # 最近目标点索引
yaw = self.refer_path[s, 2]
k = self.refer_path[s, 3] #曲率
angle = normalize_angle(yaw - math.atan2(d_y[s], d_x[s]))
e = d[s] # 误差
if angle < 0:
e *= -1
return e, k, yaw, s
#MPC主函数
def linear_mpc_control(xref, x0, delta_ref, ugv):
"""
linear mpc control
xref: reference point 参考点
x0: initial state 当前状态点
delta_ref: reference steer angle 参考控制
ugv:车辆对象
returns: 最优的控制量和最优状态
"""
x = cvxpy.Variable((NX, T + 1))
u = cvxpy.Variable((NU, T)) #定义矩阵
cost = 0.0 # 代价函数
constraints = [] # 约束条件
for t in range(T):
cost += cvxpy.quad_form(u[:, t]-delta_ref[:, t], R) #quad_form是二次规划
if t != 0:
cost += cvxpy.quad_form(x[:, t] - xref[:, t], Q)
A, B, C = ugv.state_space(delta_ref[1, t], xref[2, t]) #传入角度yaw 控制输入中的转角控制量 生成离散后的状态空间表达式
constraints += [x[:, t + 1]-xref[:, t+1] == A @
(x[:, t]-xref[:, t]) + B @ (u[:, t]-delta_ref[:, t])]
cost += cvxpy.quad_form(x[:, T] - xref[:, T], Qf)
constraints += [(x[:, 0]) == x0]
constraints += [cvxpy.abs(u[0, :]) <= MAX_VEL]
constraints += [cvxpy.abs(u[1, :]) <= MAX_STEER]
prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints) #求解整数规划问题
prob.solve(solver=cvxpy.ECOS, verbose=False)
if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
opt_x = get_nparray_from_matrix(x.value[0, :])
opt_y = get_nparray_from_matrix(x.value[1, :])
opt_yaw = get_nparray_from_matrix(x.value[2, :])
opt_v = get_nparray_from_matrix(u.value[0, :])
opt_delta = get_nparray_from_matrix(u.value[1, :])
else:
print("Error: Cannot solve mpc..")
opt_v, opt_delta, opt_x, opt_y, opt_yaw = None, None, None, None, None,
return opt_v, opt_delta, opt_x, opt_y, opt_yaw总的思路就是先根据当前车辆点计算误差和一系列预测点，然后将预测点带入mpc函数中，MPC函数中使用了一个整数规划求解器，通过将误差cost和约束加入，用求解器求解出最优控制输入。