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

article/2024/6/24 18:37:48

文章目录

    • 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; }
};

以上。


http://www.ngui.cc/article/show-1007656.html

相关文章

【蓝桥杯集训·每日一题】AcWing 3662. 最大上升子序列和

文章目录一、题目1、原题链接2、题目描述二、解题报告1、思路分析2、时间复杂度3、代码详解三、知识风暴树状数组一、题目 1、原题链接 3662. 最大上升子序列和 2、题目描述 给定一个长度为 n 的整数序列 a1,a2,…,an。 请你选出一个该序列的严格上升子序列&#xff0c;要求所…

Oracle 启动后一会儿就挂掉故障处理—ORA-600 17182----惜分飞

一例正常运行的数据库突然节点不停重启(因为是rac,启动一会儿就crash,然后又被crs给启动起来,然后有crash,依次循环),告警日志类似:Fri Mar 24 13:36:07 2023QMNC started with pid124, OS id188397 ARC3: Archival startedARC0: STARTING ARCH PROCESSES COMPLETECompleted: A…

JNI原理及常用方法概述

1.1 JNI(Java Native Interface) 提供一种Java字节码调用C/C的解决方案&#xff0c;JNI描述的是一种技术。 1.2 NDK(Native Development Kit) Android NDK 是一组允许您将 C 或 C&#xff08;“原生代码”&#xff09;嵌入到 Android 应用中的工具&#xff0c;NDK描述的是工具集…

【PC自动化测试-3】GUI对象检查工具

1&#xff0c;Inspect.exe (C:\Program Files(x86)\Windows Kits\10\bin\x64)Inspect.exe 是Microsoft创建的另外一个很棒的工具。它包含在Windows SDK中&#xff0c;因此可以在x64 Windows上的一下位置找到它上传资源https://download.csdn.net/download/qq_26086231/87530399…

Golang流媒体实战之二:回源

欢迎访问我的GitHub 这里分类和汇总了欣宸的全部原创(含配套源码)&#xff1a;https://github.com/zq2599/blog_demos 本篇概览 今天的实战是流传输过程中的常见功能&#xff1a;回源如下图&#xff0c;lal(源站)和lal(拉流节点)代表两台电脑&#xff0c;上面都部署了lalVLC在…

面试官:vue2和vue3的区别有哪些

目录 多根节点&#xff0c;fragment&#xff08;碎片&#xff09; Composition API reactive 函数是用来创建响应式对象 Ref toRef toRefs 去除了管道 v-model的prop 和 event 默认名称会更改 vue2写法 Vue 3写法 vue3组件需要使用v-model时的写法 其他语法 1. 创…

Android SDK对应版本

前言 很多时候看到某个版本都无法对应起来&#xff0c;需要去网上查找&#xff0c;这里做个记录&#xff0c;方便查找对应版本。 平台版本SDK版本版本名称13.0T(33)Android 13 (Android Tiramisu)12LSv2(32)Android 12L (Android Sv2)12.0S(31)Android 12 (Android S)11.0R(3…

图解WebView -- (1) WebView概述

前言 目前各移动应用或多或少都内嵌了Web网页&#xff0c;在Android开发中&#xff0c;就不可避免的使用本系列的主角——WebView。 一、WebView 是什么&#xff1f; WebView是Android 展示Web网页的控件&#xff0c;类似于应用提供一个内置的浏览器&#xff0c;在应用内实现…

【Mongoose笔记】SOCKS5 服务器

【Mongoose笔记】socks5 服务器 简介 Mongoose 笔记系列用于记录学习 Mongoose 的一些内容。 Mongoose 是一个 C/C 的网络库。它为 TCP、UDP、HTTP、WebSocket、MQTT 实现了事件驱动的、非阻塞的 API。 项目地址&#xff1a; https://github.com/cesanta/mongoose学习 下…

重启Android后SystemProperties属性变化

重启Android后SystemProperties属性变化1、SystemProperties属性加载2、PropertySet条件限制3、SystemProperties属性变化android12-release1、SystemProperties属性加载 查看 SystemProperties属性加载 属性映射区域LoadPath("/dev/__properties__/property_info")…