手把手教做无人驾驶算法（二十六）--基于终端约束的MPC 控制

关于MPC终端约束问题，其实就是稳定性与可行性的分析，欢迎感兴趣的一起交流探讨，如有错误，还请指正，不胜感激。

基于终端约束的MPC控制，理论部分可以参考资料：

【Linear MPC入门】Model Predictive Control Algorithm, Feasibility and Stability - 知乎

这里，终端约束数据可以用数据驱动来代替，这样就可以把MPC与数据驱动相结合起来，但是好处不仅仅是为了与数据驱动联合，终端约束有什么好处呢？上面的参考资料也给出了参考。

对于一般MPC而言，相当于零点为终点，不过，使用Zero Terminal Constraint是非常苛刻的，导致可行域减少了很多。同时，MPC的预测的步数N越少，可行域也越少。

对于可行域使用凸多面体表示，如上图：

function [ invariantGoalSet ] = computeInvariantGoalSet( A, B, Q, R )[K,~,~] = dlqr(A,B,Q,R);Acl = A-B*K;
w_max = 0.05;
W = Polyhedron([w_max w_max; w_max -w_max; -w_max -w_max; -w_max w_max]);X{1} = zeros(size(Acl,1),1); % Initialize the set
for i = 1:10000Set = Acl*X{i} + W; % Propagate the uncertantySet.minHRep() % Compute minimal representationSet.minVRep() % Compute minimal representationX{i+1} = Set;% Check if the algorithm has covnerged if i > 1if (X{i+1}.contains(X{i})) && (X{i}.contains(X{i+1}))invariantGoalSet = X{i+1}; % Set invaraint to the current iteratedisp(['Invariant set computed in i = ',num2str(i),' iterations'])            breakendend
endend

下面给出一个例子：

这里，首先使用MPC去解除有效的解，即状态值，输入值，同时使用这些量可以计算出状态量，形成安全集，使用这些安全集SS作为终端约束以及代价约束，核心部分代码如下：

def addTrajectory(self, x, u):# Add the feasible trajectory x and the associated input sequence u to the safe setself.SS.append(copy.copy(x))self.uSS.append(copy.copy(u))# Compute and store the cost associated with the feasible trajectorycost = self.computeCost(x, u)self.Qfun.append(cost)# Initialize zVectorself.zt = np.array(x[self.ftocp.N])# Augment iteration counter and print the cost of the trajectories stored in the safe setself.it = self.it + 1print("Trajectory added to the Safe Set. Current Iteration: ", self.it)print("Performance stored trajectories: \n", [self.Qfun[i][0] for i in range(0, self.it)])def computeCost(self, x, u):# Compute the cost in a DP like strategy: start from the last point x[len(x)-1] and move backwardsfor i in range(0,len(x)):idx = len(x)-1 - iif i == 0:cost = [np.dot(np.dot(x[idx],self.Q),x[idx])]else:cost.append(np.dot(np.dot(x[idx],self.Q),x[idx]) + np.dot(np.dot(u[idx],self.R),u[idx]) + cost[-1])# Finally flip the cost to have correct orderreturn np.flip(cost).tolist()

上面为计算SS安全集以及代价值

# Terminal Constraint if SS not empty --> enforce the terminal constraintif SS is not None:constr += [SS * lambVar[:,0] == x[:,self.N], # Terminal state \in ConvHull(SS)np.ones((1, SS.shape[1])) * lambVar[:,0] == 1, # Multiplies \lambda sum to 1lambVar >= 0] # Multiplier are positive definite# Cost Functioncost = 0for i in range(0, self.N):# Running cost h(x,u) = x^TQx + u^TRu#ost += quad_form(x[:,i], self.Q) + norm(self.R**0.5*u[:,i])**2# cost += quad_form(x[:,i], self.Q) + quad_form(u[:,i], self.R)cost += norm(self.Q**0.5*x[:,i])**2 + norm(self.R**0.5*u[:,i])**2# Terminal cost if SS not emptyif SS is not None:cost += Qfun[0,:] * lambVar[:,0]  # It terminal cost is given by interpolation using \lambdaelse:cost += norm(self.Q**0.5*x[:,self.N])**2 # If SS is not given terminal cost is quadratic

这里代码是终端约束以及终端代价约束。

没有终端约束的轨迹：

带有终端约束的轨迹：

具体代码地址，这个代码为python 写的：

MPC/LinearLMPC at main · caokaifa/MPC · GitHub

手把手教做无人驾驶算法（二十六）--基于终端约束的MPC 控制相关推荐

教妹学Java(二十六)：static 关键字解析
你好呀,我是沉默王二,(目前是)CSDN 周排名前十的博客专家.这是<教妹学 Java>专栏的第二十六篇,今天我们来谈谈 Java 的 static 关键字--什么是静态变量?什么是静态方 ...
手把手教用matlab做无人驾驶（二十五）--基于控制障碍函数的MPC（MPC-CBF）
这边博客主要完成论文的内容:Safety-Critical Model Predictive Control with Discrete-Time Control Barrier Function,论 ...
【python数据挖掘课程】二十六.基于SnowNLP的豆瓣评论情感分析
这是<Python数据挖掘课程>系列文章,前面很多文章都讲解了分类.聚类算法,而这篇文章主要讲解如何调用SnowNLP库实现情感分析,处理的对象是豆瓣<肖申克救赎>的评论文本. ...
[Python人工智能] 二十六.基于BiLSTM-CRF的医学命名实体识别研究（上）数据预处理
这篇文章写得很冗余,但是我相信你如果真的看完,并且按照我的代码和逻辑进行分析,对您以后的数据预处理和命名实体识别都有帮助,只有真正对这些复杂的文本进行NLP处理后,您才能适应更多的真实环境,坚持!毕竟 ...
计算机网络教程网线制作,图吧小白教程篇二十六：手把手教你自制网线（夹网线水晶头）...
图吧小白教程篇二十六:手把手教你自制网线(夹网线水晶头) 2019-11-19 23:07:38 31点赞 309收藏 27评论创作立场声明:咕咕咕教程最后还是出了,不过咱现在用啥还是直接网购号 ...
OpenCV学习笔记（二十六）——小试SVM算法ml OpenCV学习笔记（二十七）——基于级联分类器的目标检测objdect OpenCV学习笔记（二十八）——光流法对运动目标跟踪Video Ope
OpenCV学习笔记(二十六)--小试SVM算法ml 总感觉自己停留在码农的初级阶段,要想更上一层,就得静下心来,好好研究一下算法的东西.OpenCV作为一个计算机视觉的开源库,肯定不会只停留在数字图 ...
无人驾驶汽车系统入门（二十六）——基于深度学习的实时激光雷达点云目标检测及ROS实现
无人驾驶汽车系统入门(二十六)--基于深度学习的实时激光雷达点云目标检测及ROS实现在前两篇文章中,我们使用PCL实现了在点云中对地面的过滤和点云的分割聚类,通常来说,在这两步以后我们将对分割出来的 ...
模板方法模式 Template method 行为型设计模式（二十六）
模板方法模式 Template method 上图为网上百度的一份简历模板截图相信大家都有求职的经历,那么必然需要简历,写简历的时候,很可能你会网上检索一份简历模板,使用此模板的格式,然后替换为你的 ...
C/C++基础讲解(二十六)之数值计算与趣味数学篇(打鱼还是晒网与怎样存钱以获取最大利息)
C/C++基础讲解(二十六)之数值计算与趣味数学篇(打鱼还是晒网与怎样存钱以获取最大利息) 程序之美前言很多时候,特别是刚步入大学的学子们,对于刚刚开展的计算机课程基本上是一团迷雾,想要弄明白其中 ...
JVM 学习笔记二十六、JVM监控及诊断工具-GUI篇
二十六.JVM监控及诊断工具-GUI篇 1.工具概述使用上一张命令行工具或组合能帮您获取目标Java应用性能相关的基础信息,但他们存在下列局限: (1)无法获取方法级别的分析数据,如方法间的调用关系 ...

手把手教做无人驾驶算法（二十六）--基于终端约束的MPC 控制

手把手教做无人驾驶算法（二十六）--基于终端约束的MPC 控制相关推荐

最新文章

热门文章