当前位置: 首页 > news >正文

【Autoware规控】mpc_follower模型预测控制节点

文章目录

    • 1. 技术原理
    • 2. 代码实现

1. 技术原理

MPC,即Model Predictive Control(模型预测控制),是一种基于动态模型的控制算法。MPC算法通过建立系统的数学模型,根据当前状态和一定时间内的预测,优化未来的控制输入,从而实现对系统的控制。

MPC算法主要分为以下几个步骤:

1. 建立数学模型:根据系统的物理特性,建立状态空间模型或者传递函数模型。
2. 预测状态:根据当前状态,利用建立的数学模型对未来一段时间内的状态进行预测。
3. 生成控制输入:根据预测的状态和控制目标,利用最优化算法生成控制输入。
4. 执行控制:根据生成的控制输入,执行控制。
5. 更新状态:根据执行的控制输入,更新系统状态,并进入下一次预测和控制循环。

基于模型预测控制的轨迹跟踪算法对未来轨迹的预测和处理多目标约束条件的能力较强。主要体现在:能够考虑系统的非线性和时变性,适用于各种复杂系统的控制;能够考虑多个控制目标,并在它们之间进行平衡和优化;能够对约束条件进行有效的处理,例如系统的输入和输出限制、状态变量的可行性等。

MPC算法可以用于实现车辆的路径跟踪和速度控制。具体地,利用车辆的动态模型,预测未来一段时间内的车辆状态(例如位置、速度、加速度等),并根据预测结果生成最优的车辆控制输入(例如方向盘转角、油门踏板位置、刹车踏板位置等),从而实现对车辆的精确控制。MPC算法还可以考虑车辆与周围环境的交互,例如与其他车辆、行人和路标的交互,从而实现更加安全和高效的自动驾驶。

2. 代码实现

在Autoware中,MPC算法主要实现在mpc_follower节点中。该节点接收/vehicle_status、/vehicle_cmd和/trajectory等消息,其中/vehicle_status消息包括车辆状态信息(例如位置、速度、方向等),/vehicle_cmd消息包括车辆控制指令(例如方向盘转角、油门踏板位置、刹车踏板位置等),/trajectory消息包括规划的车辆轨迹。通过对这些消息的处理,mpc_follower节点可以计算出最优的车辆控制指令,并将其发送给/vehicle_cmd话题,从而实现对车辆的控制。

在实现MPC控制的过程中,需要定义车辆的动态模型、代价函数以及约束条件等。可以通过编辑mpc_param.yaml文件来配置MPC控制的参数。

在这里插入图片描述

mpc_follower_core.h

#include <vector>
#include <iostream>
#include <limits>
#include <chrono>
#include <unistd.h>
#include <deque>#include <ros/ros.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64MultiArray.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <visualization_msgs/Marker.h>
#include <tf2/utils.h>#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/LU>#include <autoware_msgs/ControlCommandStamped.h>
#include <autoware_msgs/Lane.h>
#include <autoware_msgs/VehicleStatus.h>#include "mpc_follower/mpc_utils.h"
#include "mpc_follower/mpc_trajectory.h"
#include "mpc_follower/lowpass_filter.h"
#include "mpc_follower/vehicle_model/vehicle_model_bicycle_kinematics.h"
#include "mpc_follower/vehicle_model/vehicle_model_bicycle_dynamics.h"
#include "mpc_follower/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.h"
#include "mpc_follower/qp_solver/qp_solver_unconstr.h"
#include "mpc_follower/qp_solver/qp_solver_unconstr_fast.h"
#include "mpc_follower/qp_solver/qp_solver_qpoases.h"/** * @class MPC-based waypoints follower class* @brief calculate control command to follow reference waypoints*/
class MPCFollower
{
public:/*** @brief constructor*/MPCFollower();/*** @brief destructor*/~MPCFollower();private:ros::NodeHandle nh_;                    //!< @brief ros node handleros::NodeHandle pnh_;                   //!< @brief private ros node handleros::Publisher pub_steer_vel_ctrl_cmd_; //!< @brief topic publisher for control commandros::Publisher pub_twist_cmd_;          //!< @brief topic publisher for twist commandros::Subscriber sub_ref_path_;          //!< @brief topic subscriber for reference waypointsros::Subscriber sub_pose_;              //!< @brief subscriber for current poseros::Subscriber sub_vehicle_status_;    //!< @brief subscriber for currrent vehicle statusros::Timer timer_control_;              //!< @brief timer for control command computationMPCTrajectory ref_traj_;                                   //!< @brief reference trajectory to be followedButterworth2dFilter lpf_steering_cmd_;                     //!< @brief lowpass filter for steering commandButterworth2dFilter lpf_lateral_error_;                    //!< @brief lowpass filter for lateral error to calculate derivatieButterworth2dFilter lpf_yaw_error_;                        //!< @brief lowpass filter for heading error to calculate derivatieautoware_msgs::Lane current_waypoints_;                    //!< @brief current waypoints to be followedstd::shared_ptr<VehicleModelInterface> vehicle_model_ptr_; //!< @brief vehicle model for MPCstd::string vehicle_model_type_;                           //!< @brief vehicle model type for MPCstd::shared_ptr<QPSolverInterface> qpsolver_ptr_;          //!< @brief qp solver for MPCstd::string output_interface_;                             //!< @brief output command typestd::deque<double> input_buffer_;                          //!< @brief control input (mpc_output) buffer for delay time conpemsation/* parameters for control*/double ctrl_period_;              //!< @brief control frequency [s]double steering_lpf_cutoff_hz_;   //!< @brief cutoff frequency of lowpass filter for steering command [Hz]double admisible_position_error_; //!< @brief stop MPC calculation when lateral error is large than this value [m]double admisible_yaw_error_deg_;  //!< @brief stop MPC calculation when heading error is large than this value [deg]double steer_lim_deg_;            //!< @brief steering command limit [rad]double wheelbase_;                //!< @brief vehicle wheelbase length [m] to convert steering angle to angular velocity/* parameters for path smoothing */bool enable_path_smoothing_;     //< @brief flag for path smoothingbool enable_yaw_recalculation_;  //< @brief flag for recalculation of yaw angle after resamplingint path_filter_moving_ave_num_; //< @brief param of moving average filter for path smoothingint path_smoothing_times_;       //< @brief number of times of applying path smoothing filterint curvature_smoothing_num_;    //< @brief point-to-point index distance used in curvature calculationdouble traj_resample_dist_;      //< @brief path resampling interval [m]struct MPCParam{int prediction_horizon;                         //< @brief prediction horizon stepdouble prediction_sampling_time;                //< @brief prediction horizon perioddouble weight_lat_error;                        //< @brief lateral error weight in matrix Qdouble weight_heading_error;                    //< @brief heading error weight in matrix Qdouble weight_heading_error_squared_vel_coeff;  //< @brief heading error * velocity weight in matrix Qdouble weight_steering_input;                   //< @brief steering error weight in matrix Rdouble weight_steering_input_squared_vel_coeff; //< @brief steering error * velocity weight in matrix Rdouble weight_lat_jerk;                         //< @brief lateral jerk weight in matrix Rdouble weight_terminal_lat_error;               //< @brief terminal lateral error weight in matrix Qdouble weight_terminal_heading_error;           //< @brief terminal heading error weight in matrix Qdouble zero_ff_steer_deg;                       //< @brief threshold that feed-forward angle becomes zerodouble delay_compensation_time;                //< @brief delay time for steering input to be compensated};MPCParam mpc_param_; // for mpc design parameterstruct VehicleStatus{std_msgs::Header header;    //< @brief headergeometry_msgs::Pose pose;   //< @brief vehicle posegeometry_msgs::Twist twist; //< @brief vehicle velocitydouble tire_angle_rad;      //< @brief vehicle tire angle};VehicleStatus vehicle_status_; //< @brief vehicle statusdouble steer_cmd_prev_;     //< @brief steering command calculated in previous perioddouble lateral_error_prev_; //< @brief previous lateral error for derivativedouble yaw_error_prev_;     //< @brief previous lateral error for derivative/* flags */bool my_position_ok_; //< @brief flag for validity of current posebool my_velocity_ok_; //< @brief flag for validity of current velocitybool my_steering_ok_; //< @brief flag for validity of steering angle/*** @brief compute and publish control command for path follow with a constant control period*/void timerCallback(const ros::TimerEvent &);/*** @brief set current_waypoints_ with receved message*/void callbackRefPath(const autoware_msgs::Lane::ConstPtr &);/*** @brief set vehicle_status_.pose with receved message */void callbackPose(const geometry_msgs::PoseStamped::ConstPtr &);/*** @brief set vehicle_status_.twist and vehicle_status_.tire_angle_rad with receved message*/void callbackVehicleStatus(const autoware_msgs::VehicleStatus &msg);/*** @brief publish control command calculated by MPC* @param [in] vel_cmd velocity command [m/s] for vehicle control* @param [in] acc_cmd acceleration command [m/s2] for vehicle control* @param [in] steer_cmd steering angle command [rad] for vehicle control* @param [in] steer_vel_cmd steering angle speed [rad/s] for vehicle control*/void publishControlCommands(const double &vel_cmd, const double &acc_cmd,const double &steer_cmd, const double &steer_vel_cmd);/*** @brief publish control command as geometry_msgs/TwistStamped type* @param [in] vel_cmd velocity command [m/s] for vehicle control* @param [in] omega_cmd angular velocity command [rad/s] for vehicle control*/void publishTwist(const double &vel_cmd, const double &omega_cmd);/*** @brief publish control command as autoware_msgs/ControlCommand type* @param [in] vel_cmd velocity command [m/s] for vehicle control* @param [in] acc_cmd acceleration command [m/s2] for vehicle control* @param [in] steer_cmd steering angle command [rad] for vehicle control*/void publishCtrlCmd(const double &vel_cmd, const double &acc_cmd, const double &steer_cmd);/*** @brief calculate control command by MPC algorithm* @param [out] vel_cmd velocity command* @param [out] acc_cmd acceleration command* @param [out] steer_cmd steering command* @param [out] steer_vel_cmd steering rotation speed command*/bool calculateMPC(double &vel_cmd, double &acc_cmd, double &steer_cmd, double &steer_vel_cmd);/* debug */bool show_debug_info_;      //!< @brief flag to display debug inforos::Publisher pub_debug_filtered_traj_;        //!< @brief publisher for debug inforos::Publisher pub_debug_predicted_traj_;       //!< @brief publisher for debug inforos::Publisher pub_debug_values_;               //!< @brief publisher for debug inforos::Publisher pub_debug_mpc_calc_time_;        //!< @brief publisher for debug inforos::Subscriber sub_estimate_twist_;         //!< @brief subscriber for /estimate_twist for debuggeometry_msgs::TwistStamped estimate_twist_; //!< @brief received /estimate_twist for debug/*** @brief convert MPCTraj to visualizaton marker for visualization*/void convertTrajToMarker(const MPCTrajectory &traj, visualization_msgs::Marker &markers,std::string ns, double r, double g, double b, double z);/*** @brief callback for estimate twist for debug*/void callbackEstimateTwist(const geometry_msgs::TwistStamped &msg) { estimate_twist_ = msg; }
};

以上。

相关文章:

【Autoware规控】mpc_follower模型预测控制节点

文章目录1. 技术原理2. 代码实现1. 技术原理 MPC&#xff0c;即Model Predictive Control&#xff08;模型预测控制&#xff09;&#xff0c;是一种基于动态模型的控制算法。MPC算法通过建立系统的数学模型&#xff0c;根据当前状态和一定时间内的预测&#xff0c;优化未来的控…...

成果VR虚拟3D展厅让内容更丰富饱满

随着数字技术的不断发展和普及&#xff0c;数字化展厅成为了一种重要的展示形式。线上虚拟展厅作为数字化展示的一种新形式&#xff0c;采用虚拟现实技术&#xff0c;能够克服时空限制&#xff0c;打破传统展览业的展示模式&#xff0c;为用户提供更加丰富、立体、沉浸式的展览…...

【CE进阶】lua脚本使用

▒ 目录 ▒&#x1f6eb; 导读需求开发环境1️⃣ 脚本窗口Lua ScriptLua EngineAuto assemble2️⃣ 全局变量3️⃣ 进程当前打开的进程ID系统的进程列表系统的顶部窗口列表4️⃣ 线程5️⃣ 输入设备6️⃣ 屏幕7️⃣ 剪贴板&#x1f6ec; 文章小结&#x1f4d6; 参考资料&#x…...

【vue2】近期bug收集与整理02

⭐【前言】 在使用vue2构建页面时候&#xff0c;博主遇到的问题难点以及最终的解决方案。 &#x1f973;博主&#xff1a;初映CY的前说(前端领域) &#x1f918;本文核心&#xff1a;博主遇到的问题与解决思路 ⭐数据枚举文件的使用 同后端那边发送请求的时&#xff0c;请求返…...

2. 01背包问题

文章目录QuestionIdeasCodeQuestion 有 N 件物品和一个容量是 V 的背包。每件物品只能使用一次。 第 i 件物品的体积是 vi &#xff0c;价值是 wi 。 求解将哪些物品装入背包&#xff0c;可使这些物品的总体积不超过背包容量&#xff0c;且总价值最大。 输出最大价值。 输入…...

【Docker】CAdvisor+InfluxDB+Granfana容器监控

文章目录原生命令 docker stats容器监控3剑客CIGCAdvisorInfluxDBGranfanacompose容器编排&#xff0c;一套带走新建目录新建3件套组合的 docker-compose.yml检查配置&#xff0c;有问题才有输出 docker-compose config -q启动docker-compose文件 docker-compose up -d测试浏览…...

k8s 部署nginx 实现集群统一配置,自动更新nginx.conf配置文件 总结

k8s 部署nginx 实现集群统一配置&#xff0c;自动更新nginx.conf配置文件 总结 大纲 1 nginx镜像选择2 创建configmap保存nginx配置文件3 使用inotify监控配置文件变化4 Dockerfile创建5 调整镜像原地址使用阿里云6 创建deploy部署文件部署nginx7 测试使用nginx配置文件同步&…...

动态内存管理(上)——“C”

各位CSDN的uu们你们好呀&#xff0c;今天&#xff0c;小雅兰的内容是动态内存管理噢&#xff0c;下面&#xff0c;让我们进入动态内存管理的世界吧 为什么存在动态内存分配 动态内存函数的介绍 malloc free calloc realloc 常见的动态内存错误 为什么存在动态内存分配 我们已…...

GPT-4发布,这类人才告急,大厂月薪10W+疯抢

ChatGPT最近彻底火出圈&#xff0c;各行各业都在争相报道&#xff0c;甚至连很多官媒都下场“跟风”。ChatGPT的瓜还没吃完&#xff0c;平地一声雷&#xff0c;GPT-4又重磅发布&#xff01; 很多小伙伴瑟瑟发抖&#xff1a;“AI会不会跟自己抢饭碗啊&#xff1f;” 关于“如何…...

MySQL数据库实现主主同步

前言 MySQL主主同步实际上是在主从同步的基础上将从数据库也提升成主数据库&#xff0c;让它们可以互相读写数据库&#xff0c;从数据库变成主数据库&#xff1b;主从相互授权连接&#xff0c;读取对方binlog日志并更新到本地数据库的过程,只要对方数据改变&#xff0c;自己就…...

JavaScript传参的6种方式

JavaScript传参的方式1. 传递基本类型参数2. 传递对象类型参数3. 使用解构赋值传递参数4. 使用展开运算符传递参数5. 使用可选参数6. 使用剩余参数JavaScript是一门非常灵活的语言&#xff0c;其参数传递方式也同样灵活。在本篇文章中&#xff0c;会详细介绍JavaScript中的参数…...

蓝桥之统计子矩阵

样例说明 满足条件的子矩阵一共有 19 , 包含: 大小为 11 的有 10 个。 大小为 12 的有 3 个。 大小为13 的有 2 个。 大小为 14 的有 1 个。 大小为 21 的有 3 个。 前缀和二维数组 前缀和暴力搜索 import java.util.*; public class Main{private static int ans0;pub…...

Java的基础面试题

一.java基础1.JDK和JRE有什么区别&#xff1f;JDK是java开发工具包&#xff0c;JRE是java运行时环境&#xff08;包括Java基础类库&#xff0c;java虚拟机&#xff09;2.和equals的区别是什么&#xff1f;比较的是两者的地址值&#xff0c;equals比较的是两者的内容是否一样3.两…...

J1939故障码诊断说明

1&#xff1a;1939整体协议说明 这里主要说明1939不同的协议&#xff0c;对应不同的网络分层 注意了&#xff0c;这里只进行文档解析说明&#xff0c;具体查看去搜素协议的关键字进行理解 2&#xff1a;DMx和FMI 说明 想知道每个代号的具体含义&#xff0c;可以去 saeJ1939…...

XCPC第十三站,贪心问题

一.区间选点 我们采取这样的策略来选点&#xff1a;step&#xff08;1&#xff09;将区间按照右端点的大小从小到大排序&#xff1b;step&#xff08;2&#xff09;从前往后依次枚举每个区间&#xff0c;如果当前区间中已经包含点&#xff0c;直接pass&#xff0c;否则选当前区…...

一文让你吃透 Vue3中的组件间通讯 【一篇通】

文章目录前情回顾前言1. 父组件 > 子组件通讯传递2. 子组件 > 父组件通讯传递3. 爷孙组件&#xff0c;后代组件通讯数据总结前情回顾 在本专栏前一章节中&#xff0c;我为大家带来了 Vue3 新特性变化上手指南 的归纳梳理&#xff0c;主要介绍了 Vue3 的 Proxy 响应式原理…...

EVE遭遇大规模DDOS攻击,玩家和官方都傻眼了

如果你恰好是一名《星战前夜》&#xff08;EVE&#xff09;的国际服玩家&#xff08;虽然这个几率很小&#xff09;&#xff0c;又恰好因为疫情一直待在家里&#xff0c;那你就真是倒霉透顶了。因为从1月底开始&#xff0c;EVE的服务器就一直受到大规模的DDOS攻击&#xff0c;而…...

【数据结构】二叉树及相关习题详解

新年新气象! 祝大家兔年 财源滚滚! 万事胜意! 文章目录前言1. 树的一些基础概念1.1 树的一些基本概念1.2 树的一些重要概念2. 二叉树的一些基本概念2.1 二叉树的结构2.2 两种特殊的二叉树3. 二叉树的性质4. 二叉树的存储5. 二叉树的基本操作5.1 构造一棵二叉树5.2 二叉树的遍历…...

锂电池充电的同时也能放电吗?

大家应该都有这样经历&#xff0c;我们的手机在充电的同时也能边使用&#xff0c;有的同学就会说了&#xff0c;这是因为手机电池在充电的同时也在放电。如果这样想我们可能就把锂电池类比了一个蓄水池&#xff0c;以为它在进水的同时也能出水&#xff0c;其实这个比喻是错误的…...

通信工程考研英语复试专有名词翻译

中文英文频分多址Frequency Division Multiple Access码分多址Code Division Multiple Access时分多址Time Division Multiple Access移动通信mobile communication人工智能artificial intelligence水声通信Middle-Range Uwa Communication正交频分复用Orthogonal frequency di…...

KubeSphere 容器平台高可用:环境搭建与可视化操作指南

Linux_k8s篇 欢迎来到Linux的世界&#xff0c;看笔记好好学多敲多打&#xff0c;每个人都是大神&#xff01; 题目&#xff1a;KubeSphere 容器平台高可用&#xff1a;环境搭建与可视化操作指南 版本号: 1.0,0 作者: 老王要学习 日期: 2025.06.05 适用环境: Ubuntu22 文档说…...

wordpress后台更新后 前端没变化的解决方法

使用siteground主机的wordpress网站&#xff0c;会出现更新了网站内容和修改了php模板文件、js文件、css文件、图片文件后&#xff0c;网站没有变化的情况。 不熟悉siteground主机的新手&#xff0c;遇到这个问题&#xff0c;就很抓狂&#xff0c;明明是哪都没操作错误&#x…...

Swift 协议扩展精进之路:解决 CoreData 托管实体子类的类型不匹配问题(下)

概述 在 Swift 开发语言中&#xff0c;各位秃头小码农们可以充分利用语法本身所带来的便利去劈荆斩棘。我们还可以恣意利用泛型、协议关联类型和协议扩展来进一步简化和优化我们复杂的代码需求。 不过&#xff0c;在涉及到多个子类派生于基类进行多态模拟的场景下&#xff0c;…...

UDP(Echoserver)

网络命令 Ping 命令 检测网络是否连通 使用方法: ping -c 次数 网址ping -c 3 www.baidu.comnetstat 命令 netstat 是一个用来查看网络状态的重要工具. 语法&#xff1a;netstat [选项] 功能&#xff1a;查看网络状态 常用选项&#xff1a; n 拒绝显示别名&#…...

C++中string流知识详解和示例

一、概览与类体系 C 提供三种基于内存字符串的流&#xff0c;定义在 <sstream> 中&#xff1a; std::istringstream&#xff1a;输入流&#xff0c;从已有字符串中读取并解析。std::ostringstream&#xff1a;输出流&#xff0c;向内部缓冲区写入内容&#xff0c;最终取…...

c#开发AI模型对话

AI模型 前面已经介绍了一般AI模型本地部署&#xff0c;直接调用现成的模型数据。这里主要讲述讲接口集成到我们自己的程序中使用方式。 微软提供了ML.NET来开发和使用AI模型&#xff0c;但是目前国内可能使用不多&#xff0c;至少实践例子很少看见。开发训练模型就不介绍了&am…...

C#学习第29天:表达式树(Expression Trees)

目录 什么是表达式树&#xff1f; 核心概念 1.表达式树的构建 2. 表达式树与Lambda表达式 3.解析和访问表达式树 4.动态条件查询 表达式树的优势 1.动态构建查询 2.LINQ 提供程序支持&#xff1a; 3.性能优化 4.元数据处理 5.代码转换和重写 适用场景 代码复杂性…...

STM32---外部32.768K晶振(LSE)无法起振问题

晶振是否起振主要就检查两个1、晶振与MCU是否兼容&#xff1b;2、晶振的负载电容是否匹配 目录 一、判断晶振与MCU是否兼容 二、判断负载电容是否匹配 1. 晶振负载电容&#xff08;CL&#xff09;与匹配电容&#xff08;CL1、CL2&#xff09;的关系 2. 如何选择 CL1 和 CL…...

深度学习之模型压缩三驾马车:模型剪枝、模型量化、知识蒸馏

一、引言 在深度学习中&#xff0c;我们训练出的神经网络往往非常庞大&#xff08;比如像 ResNet、YOLOv8、Vision Transformer&#xff09;&#xff0c;虽然精度很高&#xff0c;但“太重”了&#xff0c;运行起来很慢&#xff0c;占用内存大&#xff0c;不适合部署到手机、摄…...

使用python进行图像处理—图像变换(6)

图像变换是指改变图像的几何形状或空间位置的操作。常见的几何变换包括平移、旋转、缩放、剪切&#xff08;shear&#xff09;以及更复杂的仿射变换和透视变换。这些变换在图像配准、图像校正、创建特效等场景中非常有用。 6.1仿射变换(Affine Transformation) 仿射变换是一种…...