模型预测控制路径跟踪python语言实现

模型预测控制路径跟踪python语言实现，参考《无人驾驶车辆模型预测控制》以及网上其他代码。

import matplotlib.pyplot as plt
import numpy as np
from math import *
from cvxopt import matrix, solversclass MPC:def __init__(self):self.Np = 60  # 预测步长self.Nc = 60  # 控制步长self.dt = 0.1  # 时间间隔self.Length = 1.0  # 车辆轴距max_steer = 30 * pi / 180  # 最大方向盘打角max_steer_v = 15 * pi / 180  # 最大方向盘打角速度max_v = 8.7  # 最大车速max_a = 1.0  # 最大加速度# 目标函数相关矩阵self.Q = 50 * np.identity(3*self.Np)  # 位姿权重self.R = 100 * np.identity(2*self.Nc)  # 控制权重self.kesi = np.zeros((5, 1))self.A = np.identity(5)self.B = np.block([[np.zeros((3, 2))],[np.identity(2)]])self.C = np.block([[np.identity(3), np.zeros((3, 2))]])self.PHI = np.zeros((3*self.Np, 5))self.THETA = np.zeros((3*self.Np, 2*self.Nc))self.CA = (self.Np+1) * [self.C]self.H = np.zeros((2*self.Nc, 2*self.Nc))self.f = np.zeros((2*self.Nc, 1))# 不等式约束相关矩阵A_t = np.zeros((self.Nc, self.Nc))for p in range(self.Nc):for q in range(p+1):A_t[p][q] = 1A_I = np.kron(A_t, np.identity(2))# 控制量约束umin = np.array([[-max_v], [-max_steer]])umax = np.array([[max_v], [max_steer]])self.Umin = np.kron(np.ones((self.Nc, 1)), umin)self.Umax = np.kron(np.ones((self.Nc, 1)), umax)# 控制增量约束delta_umin = np.array([[-max_a * self.dt], [-max_steer_v * self.dt]])delta_umax = np.array([[max_a * self.dt], [max_steer_v * self.dt]])delta_Umin = np.kron(np.ones((self.Nc, 1)), delta_umin)delta_Umax = np.kron(np.ones((self.Nc, 1)), delta_umax)self.A_cons = np.zeros((2 * 2*self.Nc, 2*self.Nc))self.A_cons[0:2*self.Nc, 0:2*self.Nc] = A_Iself.A_cons[2*self.Nc:4*self.Nc, 0:2*self.Nc] = np.identity(2*self.Nc)self.lb_cons = np.zeros((2 * 2*self.Nc, 1))self.lb_cons[2*self.Nc:4*self.Nc, 0:1] = delta_Uminself.ub_cons = np.zeros((2 * 2*self.Nc, 1))self.ub_cons[2*self.Nc:4*self.Nc, 0:1] = delta_Umaxdef mpcControl(self, x, y, yaw, v, angle, tar_x, tar_y, tar_yaw, tar_v, tar_angle):  # mpc优化控制T = self.dtL = self.Length# 更新误差self.kesi[0][0] = x-tar_xself.kesi[1][0] = y-tar_yself.kesi[2][0] = self.normalizeTheta(yaw - tar_yaw)self.kesi[3][0] = v - tar_vself.kesi[4][0] = angle - tar_angle# 更新A矩阵self.A[0][2] = -tar_v * sin(tar_yaw) * Tself.A[0][3] = cos(tar_yaw) * Tself.A[1][2] = tar_v * cos(tar_yaw) * Tself.A[1][3] = sin(tar_yaw) * Tself.A[2][3] = tan(tar_angle) * T / Lself.A[2][4] = tar_v * T / (L * (cos(tar_angle)**2))# 更新B矩阵self.B[0][0] = cos(tar_yaw) * Tself.B[1][0] = sin(tar_yaw) * Tself.B[2][0] = tan(tar_angle) * T / Lself.B[2][1] = tar_v * T / (L * (cos(tar_angle)**2))# 更新CAfor i in range(1, self.Np+1):self.CA[i] = np.dot(self.CA[i-1], self.A)# 更新PHI和THETAfor j in range(self.Np):self.PHI[3*j:3*(j+1), 0:5] = self.CA[j+1]for k in range(min(self.Nc, j+1)):self.THETA[3*j:3*(j+1), 2*k: 2*(k+1)] = np.dot(self.CA[j-k], self.B)# 更新Hself.H = np.dot(np.dot(self.THETA.transpose(), self.Q),self.THETA) + self.R# 更新fself.f = 2 * np.dot(np.dot(self.THETA.transpose(), self.Q),np.dot(self.PHI, self.kesi))# 更新约束Ut = np.kron(np.ones((self.Nc, 1)), np.array([[v], [angle]]))self.lb_cons[0:2*self.Nc, 0:1] = self.Umin-Utself.ub_cons[0:2*self.Nc, 0:1] = self.Umax-Ut# 求解QPP = matrix(self.H)q = matrix(self.f)G = matrix(np.block([[self.A_cons],[-self.A_cons]]))h = matrix(np.block([[self.ub_cons],[-self.lb_cons]]))solvers.options['show_progress'] = Falsesol = solvers.qp(P, q, G, h)X = sol['x']# 输出结果v += X[0]angle += X[1]return v, angledef normalizeTheta(self, angle):  # 角度归一化while(angle >= pi):angle -= 2*piwhile(angle < -pi):angle += 2*pireturn angledef findIdx(self, x, y, cx, cy):  # 寻找欧式距离最近的点min_dis = float('inf')idx = 0for i in range(len(cx)):dx = x - cx[i]dy = y - cy[i]dis = dx**2 + dy**2if(dis < min_dis):min_dis = disidx = ireturn idxdef update(self, x, y, yaw, v, angle):  # 模拟车辆位置x += v * cos(yaw) * self.dty += v * sin(yaw) * self.dtyaw += v / self.Length * tan(angle) * self.dtreturn x, y, yawif __name__ == '__main__':cx = np.linspace(0, 200, 2000)cy = np.zeros(len(cx))dx = np.zeros(len(cx))ddx = np.zeros(len(cy))cyaw = np.zeros(len(cx))ck = np.zeros(len(cx))for i in range(len(cx)):cy[i] = cos(cx[i]/10)*cx[i]/10# 计算一阶导数for i in range(len(cx)-1):dx[i] = (cy[i+1] - cy[i])/(cx[i+1] - cx[i])dx[len(cx)-1] = dx[len(cx)-2]# 计算二阶导数for i in range(len(cx)-2):ddx[i] = (cy[i+2] - 2*cy[i+1] + cy[i]) / (0.5 * (cx[i+2] - cx[i]))**2ddx[len(cx)-2] = ddx[len(cx)-3]ddx[len(cx)-1] = ddx[len(cx)-2]# 计算偏航角for i in range(len(cx)):cyaw[i] = atan(dx[i])# 计算曲率for i in range(len(cx)):ck[i] = ddx[i] / (1 + dx[i]**2)**1.5# 初始状态x = 0.0y = 5.0yaw = 0.0v = 0.0angle = 0.0t = 0# 历史状态xs = [x]ys = [y]vs = [v]angles = [angle]ts = [t]# 实例化mpc = MPC()while(1):idx = mpc.findIdx(x, y, cx, cy)if(idx == len(cx)-1):breaktar_v = 30.0/3.6tar_angle = atan(mpc.Length * ck[idx])(v, angle) = mpc.mpcControl(x, y, yaw, v, angle,cx[idx], cy[idx], cyaw[idx], tar_v, tar_angle)(x, y, yaw) = mpc.update(x, y, yaw, v, angle)# 保存状态xs.append(x)ys.append(y)vs.append(v)angles.append(angle)t = t+0.1ts.append(t)# 显示plt.plot(cx, cy)plt.scatter(xs, ys, c='r', marker='*')plt.pause(0.01)  # 暂停0.01秒plt.clf()plt.close()plt.subplot(2, 1, 1)plt.plot(ts, vs)plt.subplot(2, 1, 2)plt.plot(ts, angles)plt.show()

模型预测控制路径跟踪python语言实现相关推荐

自动驾驶路径跟踪控制——纯追踪控制
文章目录 1.自行车模型(汽车二自由度模型) 注意点 Point1 Point2 2.纯追踪控制注意点 Point1 Point2 Point3 3.相关代码参考文献声明全局路径由一系 ...
斯坦福大学：极限工况下的无人驾驶路径跟踪｜厚势汽车
来源: 同济智能汽车研究所责任编辑:啜小雪文章译自 2017 年美国控制年会的会议论文原标题:Path-Tracking for Autonomous Vehicles at the Limit ...
无人车系统（十一）：轨迹跟踪模型预测控制(MPC)原理与python实现【40行代码】
前面介绍的PID,pure pursuit方法,Stanley方法都只是利用当前的系统误差来设计控制器.人们对这些控制器的设计过程中都利用了构建模型对无人车未来状态的估计(或者说利用模型估计未来的运动 ...
对南京地铁计价模型分析及最佳路径设计基于Python语言
离散数学技术报告(该报告不完整) 实验题目:对南京地铁计价模型分析及最佳路径设计基于Python语言关键词: 城市交通; 地理信息系统; Dijkstra算法; 约束条件; 路径分析一．引言截至 ...
python模型预测控制_【模型工具】耦合python和 SWMM的城市排水系统模型预测算法...
美国环境署EPA开发的SWMM模型具有开源性,这使其成为最为广泛使用的城市排水系统模型.但是SWMM是基于C语言开发的,二次开发困难较大.因此本文作者利用开源的PYSWMM程序包,利用python实现 ...
模型预测控制在路径规划中的应用
1.模型预测控制(Model Predictive Control) MPC的作用机理可以表述为:在每一个采样时刻,根据当前的测量信息,在线求解一个有限时间开环优化问题,并将得到的控制序列的第一个元素 ...
Python语言学习：利用python获取当前/上级/上上级目录路径(获取路径下的最后叶目录的文件名、合并两个不同路径下图片文件名等目录/路径案例、正确加载图片路径)之详细攻略
Python语言学习:利用python获取当前/上级/上上级目录路径(获取路径下的最后叶目录的文件名.合并两个不同路径下图片文件名等目录/路径案例.正确加载图片路径)之详细攻略目录利用python ...
Python语言编程学习：文件路径变量修改，利用os模块固定文件父路径，变换文件子路径实现代码
Python语言编程学习:文件路径变量修改,利用os模块固定文件父路径,变换文件子路径实现代码目录文件路径变量修改,利用os模块固定文件父路径,变换文件子路径实现代码
Python语言学习：创建/删除文件/文件夹、获取当前文件/文件夹路径(系统环境路径/目录)、获取当前文件夹下的所有子文件路径等代码(os系列用法)实现之详细攻略
Python语言学习:创建/删除文件/文件夹.获取当前文件/文件夹路径(系统环境路径/目录).获取当前文件夹下的所有子文件路径等代码(os系列用法)实现之详细攻略目录系统环境路径的设置 1.sys ...
【Python】Python语言学习：pip工具使用知识，模型保存pickle，PDF与docx相互转换处理...
2022年第01周. 这一周的Python语言学习,记录如下. 01 pip工具使用知识 1 pip是什么? pip是一个用Python写的用于安装和管理包的包管理系统.它连接一个叫做Python P ...

模型预测控制路径跟踪python语言实现

模型预测控制路径跟踪python语言实现相关推荐

最新文章

热门文章