3、你真的把MPC搞懂了吗

2024-04-25 22:36
文章标签 真的 搞懂 mpc

本文主要是介绍3、你真的把MPC搞懂了吗,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

        MPC是模型预测控制(Model Predictive Control)的缩写,也叫做滚动时域控制(receding horizon control)。在机器人领域,MPC主要应用于运动控制和路径规划、轨迹跟踪。

一、组成部分:
        1、模型:要解决问题的对象所在领域的模型,数学上表现为问题对象的状态和输入控制量。以小车轨迹跟踪为例,包括小车的位置、方向,控制小车需要的转向和速度,以及与运动相关的公式,如路程、速度、时间、加速度相关的公式。
        2、预测:根据已有的模型、系统当前的状态、期望的状态和未来的控制量,来预测系统未来的输出。接上例,由小车当前的位置和方向,需要到达的目标位置,未来施加的速度和转向角,来预测小车下一时刻的位置、方向和速度、转向角。一般会输出未来一个时间段上的多个值,这个未来时间段就叫预测区间(Predictive Horizon)。
        3、控制:将预测的输出与期望的输出做比较,要使得这个差别最小化,这就是代价函数。施加一些约束条件,通过优化的方法,优化出未来控制量,使得代价函数最小。最后只取多个输出值中的第一个值。接上例,执行小车代价函数后,得到多个时刻的位置、方向和速度、转向角,只取第一个时刻的值来控制小车,保证汽车始终保持在道路中央。

二、案例说明 

1、以无人车轨迹跟踪为例,建立模型;

C, Q, Qf, R矩阵等;

X上面1点是路程一次求导为速度,2点是加速度;

三、一言不合(说不清楚)上代码

将江湖流传的代码进行了面向对象的封装,并删繁就简,去掉了许多不便理解的无用的边角料算法与变量,修复了一些bug,加上了详细得令人发指的注解,希望能一码绝后尘,

  • Q,Qf:状态偏差代价矩阵(最终),不同的运动模型值不一样;
  •         R:输入偏差代价矩阵,不同的运动模型值不一样;
    • 2、求状态偏差Ẋ和控制量偏差u。

      x,y,fai=平面坐标,偏航角;v,delta=速度,前轮转角;

      其中ẋ是当前状态的平面x坐标, xr是期望状态的平面x坐标,其它类推;

    • 3、计算代价函数与约束

  • N:时间范围(Time Horizon),即预测的区间,包含N个时间段;
  • Ẋ(k)TQẊ(k):衡量状态偏差
  • u(k)TRu(k):衡量输入大小
  • Ẋ(N)TQfẊ(k):衡量最终状态偏差
  • X(k+1)是在k+1时刻的系统状态,uk是k时刻的控制量。

三、一言不合(说不清楚)上代码

        把江湖上流传最广的MPC代码进行了面向对象的封装,修正了一些小bug,去掉了一些令人难以理解的边角料变量和小算法,完美贴合了第二部分的理论,特别是加入了令人详细得发指的注解,希望能一码绝后码,成为全网最易理解的源码实现,拿走不谢,不解可询。

from celluloid import Camera
import numpy as np
import matplotlib
matplotlib.use('TkAgg') # matplotlib切换图形界面显示终端TkAgg
import matplotlib.pyplot as plt
import cvxpy
import math
import sys#一、无人车轨迹跟踪运动学模型
class Vehicle:def __init__(self):  # 车辆self.x = 0  # 初始xself.y = -4  # 初始yself.psi = 0  # 初始航向角self.v = 2  # 初始速度self.av = 1 # 加速度,为0就是恒速;self.L = 2 # 车辆轴距,单位:mself.dt = 0.1  # 时间间隔,单位:sself.R = np.diag([0.1, 0.1])  # [[0.1,0],[0,0.1]]  input cost matrix, 控制区间的输入权重,输入代价矩阵,Ru(k)self.Q = np.diag([1, 1, 1])  # state cost matrix 预测区间的状态偏差,给定状态代价矩阵, Qx(k)self.Qf = np.diag([1, 1, 1])  # state final matrix = 最终状态代价矩阵self.MAX_STEER = np.deg2rad(45.0)  # maximum steering angle [rad] 最大转向角self.MAX_VEL = 100.0  # maximum accel [m/s]  最大速度def update_state(self, delta_f):self.x = self.x+self.v*math.cos(self.psi)*self.dtself.y = self.y+self.v*math.sin(self.psi)*self.dtself.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dtself.v = self.v + self.av * self.dtdef get_state(self):return self.x, self.y, self.psi, self.vdef state_space(self, ref_delta, ref_yaw):"""Args: ref_delta (_type_): 参考的转角控制量;  ref_yaw (_type_): 参考的偏航角 """A = np.matrix([[1.0, 0.0, -self.v * self.dt*math.sin(ref_yaw)],[0.0, 1.0, self.v * self.dt*math.cos(ref_yaw)],[0.0, 0.0, 1.0]])B = np.matrix([[self.dt*math.cos(ref_yaw), 0],[self.dt*math.sin(ref_yaw), 0],[self.dt*math.tan(ref_delta)/self.L, self.v*self.dt /(self.L*math.cos(ref_delta)*math.cos(ref_delta))]])C = np.eye(3)  # 3x3的单位矩阵return A, B, C# 二、道路模型,虚拟道路上1000个点,给出每个点的位置(x坐标, y坐标,轨迹点的切线方向, 曲率k)
class VPath:def __init__(self, util):self.refer_path = np.zeros((1000, 4))self.refer_path[:, 0] = np.linspace(0, util.x_xis, 1000)  # x 间隔起始点、终止端,以及指定分隔值总数,x的间距为0.1self.refer_path[:, 1] = 2*np.sin(self.refer_path[:, 0]/3.0) + 2.5*np.cos(self.refer_path[:, 0]/2.0)  # y# 使用差分的方式计算路径点的一阶导和二阶导,从而得到切线方向和曲率for i in range(len(self.refer_path)):if i == 0:dx = self.refer_path[i+1, 0] - self.refer_path[i, 0]dy = self.refer_path[i+1, 1] - self.refer_path[i, 1]ddx = self.refer_path[2, 0] + self.refer_path[0, 0] - 2*self.refer_path[1, 0]ddy = self.refer_path[2, 1] + self.refer_path[0, 1] - 2*self.refer_path[1, 1]elif i == (len(self.refer_path)-1):dx = self.refer_path[i, 0] - self.refer_path[i-1, 0]dy = self.refer_path[i, 1] - self.refer_path[i-1, 1]ddx = self.refer_path[i, 0] + self.refer_path[i-2, 0] - 2*self.refer_path[i-1, 0]ddy = self.refer_path[i, 1] + self.refer_path[i-2, 1] - 2*self.refer_path[i-1, 1]else:dx = self.refer_path[i+1, 0] - self.refer_path[i, 0]dy = self.refer_path[i+1, 1] - self.refer_path[i, 1]ddx = self.refer_path[i+1, 0] + self.refer_path[i-1, 0] - 2*self.refer_path[i, 0]ddy = self.refer_path[i+1, 1] + self.refer_path[i-1, 1] - 2*self.refer_path[i, 1]self.refer_path[i, 2] = math.atan2(dy, dx)  # yawself.refer_path[i, 3] = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))  # 曲率k计算# 三、MPC
class MPC:def __init__(self):self.NX = 3  # 状态x = x, y, yaw = x,y坐标,偏航角self.NU = 2  # 输入变量u = [v, delta] = [速度,前轮转角]self.T = 8  # horizon length  预测区间=时间范围=8个dt"""找出车辆当前实际位置(x,y)与距离道路最近的点返回结果:将计算出的横向误差 e、曲率 k、最近目标位置所在路径段处的航向角yaw 和最近目标位置所在路径段的下标 s"""def calc_track_error(self, util, path, x, y):# 计算小车当前位置与参考路径上每个点之间的距离,找到距离小车最近的参考路径点,将该点的下标保存为 sd_x = [path.refer_path[i, 0]-x for i in range(len(path.refer_path))]d_y = [path.refer_path[i, 1]-y for i in range(len(path.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 = path.refer_path[s, 2]k = path.refer_path[s, 3]  # 将参考路径上距离小车最近的点的曲率 k 作为小车所在路径段的曲率# 将小车当前位置与距离最近的参考路径点之间的连线,与参考路径在该点处的方向角之差,作为小车当前位置与参考路径之间的方向角误差 angle。angle = util.normalize_angle(yaw - math.atan2(d_y[s], d_x[s]))e = d[s]  # 将小车当前位置与参考路径上距离最近的点之间的距离作为小车的横向误差 eif angle < 0:  # 根据 angle 的符号将横向误差 e 取正或取负:如果 angle 小于 0,则将横向误差 e 取负e *= -1return k, s"""由当前小车的位置实际值(x,y),取离道路最近的几个目标值参数:robot_state是车辆的当前状态(x,y,yaw,v); 返回值:xref=3行9列共九段预测区间的(x,y,yaw), ind=当前路径段的下标, dref=二行八列(v, 前轮转角)"""def calc_ref_trajectory(self, util, vehicle, path, robot_state):# 曲率 k、小车所在路径段的下标 sk, ind = self.calc_track_error(util, path, robot_state[0], robot_state[1])# 初始化参考轨迹:定义一个 3 行 T+1=9 列的数组 xref,用于存储参考轨迹。将第一列的值设为当前小车位置所在路径段的值(x,y,yaw)xref = np.zeros((self.NX, self.T + 1))ncourse = len(path.refer_path)  # 1000# 参考控制量,由车辆轴距和曲率,计算前轮转角ref_delta = math.atan2(vehicle.L*k, 1)# 二行八列 的第 0 行所有列车速,第 1 行所有列是转角dref = np.zeros((self.NU, self.T))dref[0, :] = robot_state[3]dref[1, :] = ref_deltatravel = 0.0for i in range(self.T + 1):if (ind + i) < ncourse:xref[0, i] = path.refer_path[ind + i, 0]  # x坐标xref[1, i] = path.refer_path[ind + i, 1]  # y坐标xref[2, i] = path.refer_path[ind + i, 2]  # v速度return xref, ind, dref"""通过二次线性规划算法,由几个目标点的状态和控制量,计算未来最优的几个点的状态和控制量xref: reference point=shape(3,9)=(x,y,yaw; 0~8)x0: initial state,(x,y,yaw), delta_ref: reference steer angle 参考转向角 =(2,8)=(v,转角;0~7 )ugv:车辆对象mpc控制的代价函数 minJ(U)=sum(u^T R u + x^T Q x + x^T Qf x)约束条件 x(k+1) = Ax(k) + Bu(k) + Cx(k)=[x-xr, y-yr, yaw-yawr]u(k)=[v-vr, delta-deltar]x(0)=x0u(k)<=umax"""def linear_mpc_control(self, util, vehicle, xref, x0, delta_ref):x = cvxpy.Variable((self.NX, self.T + 1))  # 定义状态变量9维向量x,具体数值不确定u = cvxpy.Variable((self.NU, self.T))      # 定义控制变量 u=[速度,前轮转角]cost = 0.0  # 代价函数constraints = []  # 约束条件for t in range(self.T):cost += cvxpy.quad_form(u[:, t] - delta_ref[:, t], vehicle.R)  # 衡量输入大小=u^T R uif t != 0:cost += cvxpy.quad_form(x[:, t] - xref[:, t], vehicle.Q)  # 衡量状态偏差=x^T Q xA, B, C = vehicle.state_space(delta_ref[1, t], xref[2, t]) # (转角,偏航角)constraints += [x[:, t + 1]-xref[:, t+1] == A @ (x[:, t]-xref[:, t]) + B @ (u[:, t]-delta_ref[:, t])]cost += cvxpy.quad_form(x[:, self.T] - xref[:, self.T], vehicle.Qf)  # 衡量最终状态偏差=x^T Qf xconstraints += [(x[:, 0]) == x0]constraints += [cvxpy.abs(u[0, :]) <= vehicle.MAX_VEL]constraints += [cvxpy.abs(u[1, :]) <= vehicle.MAX_STEER]# 定义了一个“问题”,“问题”函数里填写凸优化的目标,目前的目标就是cost最小prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)# 求解,运行完这一步才能确定x的具体数值prob.solve(solver=cvxpy.ECOS, verbose=False)# # prob.value储存的是minimize(cost)的值,就是优化后目标的值; 查看变量x使用x.valueif prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:opt_x = util.get_nparray_from_matrix(x.value[0, :])opt_y = util.get_nparray_from_matrix(x.value[1, :])opt_yaw = util.get_nparray_from_matrix(x.value[2, :])opt_v = util.get_nparray_from_matrix(u.value[0, :])opt_delta = util.get_nparray_from_matrix(u.value[1, :])else: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# 工具类
class Util:def __init__(self):self.x_xis = 100  # x轴的长度100,共1000个点# 展示动图def draw(self, ugv, path, mpc):x_ = []y_ = []fig = plt.figure(1)  # 图像编号1for i in range(sys.maxsize):robot_state = np.zeros(4)  # [0, 0, 0, 0]robot_state[0] = ugv.xrobot_state[1] = ugv.yrobot_state[2] = ugv.psirobot_state[3] = ugv.vx0 = robot_state[0:3]xref, target_ind, dref = mpc.calc_ref_trajectory(self, ugv, path, robot_state)opt_v, opt_delta, opt_x, opt_y, opt_yaw = mpc.linear_mpc_control(self, ugv, xref, x0, dref)# 速度v与x,y坐标不需要传递,只能按车辆指定的速度来计算ugv.update_state(opt_delta[0])x_.append(ugv.x)y_.append(ugv.y)plt.cla()  # cla清理当前的axes,以下分别绘制蓝色-.线,红色-线,绿色o点plt.plot(path.refer_path[:, 0], path.refer_path[:, 1], "-.b",  linewidth=1.0, label="course")plt.plot(x_, y_, "-g", label="trajectory")plt.plot(x_, y_, ".r", label="target")plt.grid(True)  # 显示网格线 1=True=默认显示;0=False=不显示plt.pause(0.001) # 图形会间隔0.001秒后重新绘制if ugv.x > self.x_xis:  # 判断是否到达最后一个点break# Normalize an angle to [-pi, pi]def normalize_angle(self, angle):while angle > np.pi:angle -= 2.0 * np.piwhile angle < -np.pi:angle += 2.0 * np.pireturn angledef get_nparray_from_matrix(self, x):return np.array(x).flatten()if __name__=='__main__':util = Util()ugv = Vehicle()path = VPath(util)mpc = MPC()util.draw(ugv, path, mpc)

这篇关于3、你真的把MPC搞懂了吗的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/935975

相关文章

【数据结构】——原来排序算法搞懂这些就行,轻松拿捏

前言:快速排序的实现最重要的是找基准值,下面让我们来了解如何实现找基准值 基准值的注释:在快排的过程中,每一次我们要取一个元素作为枢纽值,以这个数字来将序列划分为两部分。 在此我们采用三数取中法,也就是取左端、中间、右端三个数,然后进行排序,将中间数作为枢纽值。 快速排序实现主框架: //快速排序 void QuickSort(int* arr, int left, int rig

为什么现在很多人愿意选择做债务重组?债重组真的就这么好吗?

债务重组,起初作为面向优质企业客户的定制化大额融资策略,以其高效周期著称,一个月便显成效。然而,随着时代的车轮滚滚向前,它已悄然转变为负债累累、深陷网贷泥潭者的救赎之道。在此路径下,个人可先借助专业机构暂代月供,经一段时间养护征信之后,转向银行获取低成本贷款,用以替换高昂网贷,实现利息减负与成本优化的双重目标。 尽管债务重组的代价不菲,远超传统贷款成本,但其吸引力依旧强劲,背后逻辑深刻。其一

CSP-J基础之数学基础 初等数论 一篇搞懂(一)

文章目录 前言声明初等数论是什么初等数论历史1. **古代时期**2. **中世纪时期**3. **文艺复兴与近代**4. **现代时期** 整数的整除性约数什么样的整数除什么样的整数才能得到整数?条件:举例说明:一般化: 判断两个数能否被整除 因数与倍数质数与复合数使用开根号法判定质数哥德巴赫猜想最大公因数与辗转相除法计算最大公因数的常用方法:举几个例子:例子 1: 计算 12 和 18

CSP-J基础之数学基础 初等数论 一篇搞懂(二)

文章目录 前言算术基本定理简介什么是质数?举个简单例子:重要的结论:算术基本定理公式解释:举例: 算术基本定理的求法如何找出质因数:举个简单的例子: 重要的步骤:C++实现 同余举个例子:同余的性质简介1. 同余的自反性2. 同余的对称性3. 同余的传递性4. 同余的加法性质5. 同余的乘法性质 推论 总结 前言 在计算机科学和数学中,初等数论是一个重要的基础领域,涉及到整数

写给大数据开发:你真的“慢“了吗?揭秘技术与职场的平衡艺术

你是否曾经在深夜里,面对着一个棘手的数据处理问题,感到无比沮丧?或者在一次重要的项目汇报中,突然语塞,无法清晰地表达你的技术方案?作为一名大数据开发者,这些场景可能再熟悉不过。但别担心,因为你并不孤单。让我们一起探讨如何在这个瞬息万变的行业中,既磨练技术利刃,又培养职场软实力。 目录 技术与时间的赛跑1. 长远视角的重要性2. 复利效应在技能学习中的应用 跨界思维:数据结构教我们的职场智

【数据结构】你真的学会了二叉树了吗,来做一做二叉树的算法题及选择题

文章目录 1. 二叉树算法题1.1 单值二叉树1.2 相同的树1.3 另一棵树的子树1.4 二叉树的遍历1.5 二叉树的构建及遍历 2. 二叉树选择题3. 结语 1. 二叉树算法题 1.1 单值二叉树 https://leetcode.cn/problems/univalued-binary-tree/description/ 1.2 相同的树 https://leet

Matlab实现MPC算法

模型预测控制(Model Predictive Control, MPC)是一种先进的过程控制方法,它使用模型来预测系统未来的行为,并基于这些预测来优化控制动作。在Matlab中实现MPC算法通常涉及到使用Matlab的MPC Toolbox,我们可以考虑一个线性时不变系统的简化版本。 以下是一个简单的MPC算法实现,用于控制一个线性系统,使其状态达到期望的设定点。我们将使用一个简单的线性系统模

Flink: 两个递归彻底搞懂operator chain

《2021年最新版大数据面试题全面开启更新》 operator chain是指将满足一定条件的operator 链在一起,放在同一个task里面执行,是Flink任务优化的一种方式,在同一个task里面的operator的数据传输变成函数调用关系,这种方式减少数据传输过程。常见的chain例如:source->map->filter,这样的任务链可以chain在一起,那么其内部是如何决定

5分钟搞懂什么是Nginx?

一、What's nginx?         1、在服务器上可以组装网页并且可以响应(response)浏览器http请求(request)的软件。         2、支持负载均衡。         3、支持静态网页缓存         那么nginx可以扮演的角色为(每个角色可以单独为一台服务器):                 1、web server(组装网页)

4所适合职场人读的海外在线硕士院校,真的靠谱!

不用参加硕士研究生统考、不用出国、线上上网课就能读全球排名前列院校的硕士研究生。 这让很多想自我提升,但是因为工作繁忙没时间准备国内硕士联考、也没时间出国留学,以及一些毕业多年,应试能力下降、多次联考不过的职场人士有些蠢蠢欲动。 但是这个领域实在过于陌生,很多同学不了解、也没有靠谱的了解渠道,看到各种世界排名靠前院校的海外硕士的招生信息,心动过后也只能继续观望。 不出国门、免考试,就能获取海