本文在前文已经搭建好的ROS-C++规划控制验证平台中进行LQR算法的复现，理论部分详见于：
规划控制复现：Apollo LQR横向控制（算法原理推导与流程）_apollo 规划控制-CSDN博客
Prescan-Carsim-ROS的仿真平台搭建详见于：
简单硬件在环搭建（ROS+Prescan+Carsim+simulink)_prescan ros-CSDN博客
1.算法流程
LQR的核心在于求解里卡提方程，计算最优的反馈增益K矩阵：
其中，K矩阵为：
在计算得到K矩阵后，计算Frenet坐标系下的航向误差和横向误差，得到误差矩阵：
最终计算得到的是前轮转角：
故横向控制算法流程如下：
下面就分模块对每一个部分进行编写。
2.simulink/matlab代码编写
取自《忠厚老实的老王》，这里搬运代码并对代码作部分详解
横向控制模块如上图所示，输入为自车的状态量：x,y,Vx,Vy，航向角，以及规划出来的期望点xr,yr,θr,kappar_r(道路曲率），横向控制模块子模块实现如下：
2.1 预测模块
此模块主要用于预测一个时间步长之后的车辆状态：
数学原理如下：
matlab代码如下：
function [pred_x_ctrl,pred_y_ctrl,pred_yaw_ctrl,pred_vx_ctrl,pred_vy_ctrl,pred_yaw_dot_ctrl] = fcn(x,y,phi,vx,vy,phi_dot,ts)pred_x_ctrl=x+vx*ts*cos(phi)-vy*ts*sin(phi);pred_y_ctrl=y+vy*ts*cos(phi)+vx*ts*sin(phi);pred_yaw_ctrl=phi;pred_vx_ctrl=vx;pred_vy_ctrl=vy;pred_yaw_dot_ctrl=phi_dot;
end2.2 LQR增益矩阵K的计算
在本算法中，为节省计算开销，采用查表的方式对速度进行插值，采样5000个点，先通过不同的输入速度Vx进行离线计算，再差值得到LQR的增益矩阵K：
离线计算算法如下，调用matlab的LQR求解器对LQR问题进行求解：
k=zeros(5000,4);
vx_break_point=zeros(1,5000);
for i=1:5000vx_break_point(i)=0.01*i;A=[0,1,0,0;0,(cf+cr)/(m*vx_break_point(i)),-(cf+cr)/m,(la*cf-lb*cr)/(m*vx_break_point(i));0,0,0,1;0,(la*cf-lb*cr)/(Iz*vx_break_point(i)),-(la*cf-lb*cr)/Iz,(la*la*cf+lb*lb*cr)/(Iz*vx_break_point(i))];B=[0;-cf/m;0;-la*cf/Iz];
LQR_Q=1*[LQR_Q1,0,0,0;0,LQR_Q2,0,0;0,0,LQR_Q3,0;0,0,0,LQR_Q4];k(i,:)=lqr(A,B,LQR_Q,LQR_R);
end
LQR_K1=k(:,1)';
LQR_K2=k(:,2)';
LQR_K3=k(:,3)';
LQR_K4=k(:,4)';将得到的K矩阵作为查表模块得到：
当速度值过小时，采用如下模块将K矩阵计算结果判定为0：
展开得到：
function k = fcn(k1,k2,k3,k4,vx)if abs(vx)<0.1k=[0,0,0,0];elsek=[k1,k2,k3,k4];end
end2.3 误差计算模块
此模块根据前文得到的误差公式，计算Frenet坐标系下的横向误差与航向误差：
模块展开如下：
function [err,es,s_dot] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)tor=[cos(thetar);sin(thetar)];nor=[-sin(thetar);cos(thetar)];d_err=[x-xr;y-yr];ed=nor'*d_err;es=tor'*d_err;%projection_point_thetar=thetar(dmin);%apolloprojection_point_thetar=thetar+kappar*es;ed_dot=vy*cos(phi-projection_point_thetar)+vx*sin(phi-projection_point_thetar);%%%%%%%%%ephi=sin(phi-projection_point_thetar);%%%%%%%%%ss_dot=vx*cos(phi-projection_point_thetar)-vy*sin(phi-projection_point_thetar);s_dot=ss_dot/(1-kappar*ed);ephi_dot=phi_dot-kappar*s_dot;err=[ed;ed_dot;ephi;ephi_dot];
end
2.4 前馈计算模块
此模块用于计算前馈
展开得到：
function forword_angle = fcn(vx,a,b,m,cf,cr,k,kr)forword_angle=kr*(a+b-b*k(3)-(m*vx*vx/(a+b))*((b/cf)+(a/cr)*k(3)-(a/cr)));
end
2.5 输出计算模块
此模块用于将前面计算得到的反馈矩阵与误差矩阵作乘法，在前馈误差的基础上得到最终的反馈：
function angle = fcn(k,err,forword_angle)angle=-k*err+forword_angle;
end
3.C++/ROS环境下的算法代码编写
3.1 ROS节点代码编写
头文件Init_Ros.h：
#ifndef INIT_ROS_H
#define INIT_ROS_H
#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Accel.h>
#include <std_msgs/Float64.h>
#include <fstream>
#include <sstream>
#include <vector>
#include <string>
#include <cmath>
#include <limits>class InitRos
{
public://定义ROS初始化参考路径全局变量std::vector<double> gloabel_X; // 全局 X 坐标轨迹值std::vector<double> gloabel_Y; // 全局 Y 坐标轨迹值std::vector<double> globel_Angle; // 航向角值std::vector<std::vector<double>> matrix; // 油门刹车查找表double current_speed;// 车辆数据double host_vehicle_speed_;double host_yaw_rate_;double host_heading_;double host_x_;double host_y_;double host_Vx_;double host_Vy_;double host_ax_;double host_ay_;double curr_time_;double ts=0.1;//输出double steering_;double throttle_;double brake_;// 读取文件的函数std::vector<double> readData(const std::string &filePath);std::vector<std::vector<double>> readMatrix(const std::string &filePath, int rows, int cols);//定义ROS初始化函数InitRos(); // 构造函数void run(); // 运行 ROS 循环//定义ROS消息类型geometry_msgs::Pose2D msg;;//输出控制消息
private:// ROS 节点句柄和订阅器ros::NodeHandle nh_;ros::Subscriber pose_sub_;ros::Subscriber velocity_sub_;ros::Subscriber Vxy_sub_;ros::Subscriber Axy_sub_;ros::Subscriber time_sub_;//发布节点ros::Publisher control_pub_;// 位置回调函数void chatterCallback(const geometry_msgs::Pose2D& Pose_msg);// 速度回调函数void velocityCallback(const geometry_msgs::Twist::ConstPtr& msg);//Vxy速度回调函数void Vector_velocityCallback(const geometry_msgs::Twist::ConstPtr& Vxy_msg);//axy加速度回调函数void AccCallback(const geometry_msgs::Accel::ConstPtr& Axy_msg);//绝对时间回调函数void timeCallback(const geometry_msgs::Pose2D& time_msg);};
#endif // VEHICLE_DATA_RECEIVER_H
在构造函数中初始化ROS节点：
#include "Init_Ros.h"
#include "EM_plan.h"
#include "LQR.h"
InitRos::InitRos()
{// 初始化订阅器velocity_sub_ = nh_.subscribe("/vel_cmd", 10, &InitRos::velocityCallback, this);pose_sub_ = nh_.subscribe("/simulink_pose", 10, &InitRos::chatterCallback, this);Vxy_sub_ = nh_.subscribe("/vxy_info", 10, &InitRos::Vector_velocityCallback, this);Axy_sub_ = nh_.subscribe("/Axy_info", 10, &InitRos::AccCallback, this);time_sub_ = nh_.subscribe("/time_info", 10, &InitRos::timeCallback, this);// 初始化发布器control_pub_ = nh_.advertise<geometry_msgs::Pose2D>("/control_cmd", 100);// 读取轨迹值和查找表std::string filePathX = "/home/jetson/ROS/R2/yahboomcar_ws/src/ROS_Prescan/prescanpp/src/gloable_X.txt";gloabel_X = readData(filePathX);std::string filePathY = "/home/jetson/ROS/R2/yahboomcar_ws/src/ROS_Prescan/prescanpp/src/gloable_Y.txt";gloabel_Y = readData(filePathY);std::string filePathAngle = "/home/jetson/ROS/R2/yahboomcar_ws/src/ROS_Prescan/prescanpp/src/globel_Angle.txt";globel_Angle = readData(filePathAngle);std::string filePathMatrix = "/home/jetson/ROS/R2/yahboomcar_ws/src/ROS_Prescan/prescanpp/src/tablebr.txt";matrix = readMatrix(filePathMatrix, 1001, 261);}std::vector<double> InitRos::readData(const std::string &filePath)
{std::ifstream file(filePath);std::vector<double> data;if (!file.is_open()) {ROS_ERROR("无法打开文件: %s", filePath.c_str());return data;}std::string line;while (std::getline(file, line)) {std::istringstream iss(line);double value;if (iss >> value) {data.push_back(value);} else {ROS_WARN("读取数据失败: %s", line.c_str());}}file.close();return data;
}std::vector<std::vector<double>> InitRos::readMatrix(const std::string &filePath, int rows, int cols)
{std::ifstream file(filePath);std::vector<std::vector<double>> matrix(rows, std::vector<double>(cols));if (!file.is_open()) {ROS_ERROR("无法打开文件: %s", filePath.c_str());return matrix;}std::string line;for (int i = 0; i < rows; ++i) {if (!std::getline(file, line)) {ROS_ERROR("读取文件时出错或文件行数不足");return matrix;}std::istringstream iss(line);for (int j = 0; j < cols; ++j) {if (!(iss >> matrix[i][j])) {ROS_ERROR("读取数据时出错或文件列数不足: 行 %d 列 %d", i, j);return matrix;}}}file.close();return matrix;
}
//ROSrun主函数
void InitRos::run()
{ros::Rate rate(1000);EM_plan em_plan;LQRController lqrController;while (ros::ok()){// 寻找索引值int index = em_plan.findClosestPoint(gloabel_X, gloabel_Y, host_x_, host_y_);//寻找期望点em_plan.xr_ = gloabel_X[index]; em_plan.yr_ = gloabel_Y[index];//航向角计算std::vector<double> theta_vector = em_plan.calculateHeading(gloabel_X,gloabel_Y);//曲率计算std::vector<double> kappa_vector = em_plan.calculateHeading(gloabel_X,gloabel_Y);//ROS_INFO("索引: %f", gloabel_Y[index]);//ROS_INFO("索引: %f", gloabel_X[index]);//预测模块更新lqrController.predictState(host_x_,host_y_,host_heading_,host_Vx_,host_Vy_,host_yaw_rate_);//主计算函数Eigen::MatrixXd _matrix_A_bar_ = lqrController._matrix_A_bar;Eigen::MatrixXd _matrix_B_bar_ = lqrController._matrix_B_bar;Eigen::MatrixXd _matrix_Q_ = lqrController._matrix_Q; // Q矩阵Eigen::MatrixXd _matrix_R_ = lqrController._matrix_R; // R矩阵int _iter_max_ = lqrController._iter_max; // 最大迭代次数double _tolerance_ = lqrController._tolerance; // 迭代精度lqrController.Compute_LQR(_matrix_A_bar_,_matrix_B_bar_,_matrix_Q_,_matrix_R_,_iter_max_,_tolerance_);//误差计算函数double pred_x = lqrController.pred_x_ctrl_;double pred_y = lqrController.pred_y_ctrl_;double pred_yaw = lqrController.pred_yaw_ctrl_;double pred_vx = lqrController.pred_vx_ctrl_;double pred_vy = lqrController.pred_vy_ctrl_;double pred_yaw_dot = lqrController.pred_yaw_dot_ctrl_;double xrr = em_plan.xr_;double yrr = em_plan.yr_;lqrController.err_Compute(pred_x,pred_y,pred_yaw,pred_vx,pred_vy,pred_yaw_dot,xrr, yrr,theta_vector[index],kappa_vector[index]);// 更新全局的 steerdouble steer = lqrController.steer_Compute(kappa_vector[index]);double dt = 0.1;msg.x = throttle_; // 油门开度msg.y = brake_; // 刹车压力steering_ = -steer*180/3.14;msg.theta = steering_; // 方向盘扭矩control_pub_.publish(msg);//ROS_INFO("索引: %f", steering_);ros::spinOnce();rate.sleep();}
}void InitRos::chatterCallback(const geometry_msgs::Pose2D& Pose_msg)
{host_x_ = Pose_msg.x;host_y_ = Pose_msg.y;host_heading_ = Pose_msg.theta;}void InitRos::velocityCallback(const geometry_msgs::Twist::ConstPtr& msg)
{host_vehicle_speed_ = msg->linear.x;host_yaw_rate_ = msg->angular.z;}void InitRos::Vector_velocityCallback(const geometry_msgs::Twist::ConstPtr& Vxy_msg)
{host_Vx_ = Vxy_msg->linear.x; // 获取车辆前进速度host_Vy_ = Vxy_msg->linear.y; // 获取横向速度//ROS_INFO("Received Vxy: Vx_ = %f, Vy_ = %f", host_Vx_, host_Vy_);
}void InitRos::AccCallback(const geometry_msgs::Accel::ConstPtr& Axy_msg)
{host_ax_ = Axy_msg->linear.x; // 获取车辆前进速度host_ay_ = Axy_msg->linear.y; // 获取横向速度//ROS_INFO("Received ACC: ax_ = %f, ay_ = %f", host_ax_,host_ay_);
}
//绝对时间回调函数
void InitRos::timeCallback(const geometry_msgs::Pose2D& time_msg)
{curr_time_=time_msg.x;//ROS_INFO("Received time: time= %f", curr_time_);
}全局路径的TXT文件获取，详见上一章节：
简单硬件在环搭建（ROS+Prescan+Carsim+simulink)_prescan ros-CSDN博客
3.2 LQR主算法模块编写
算法流程依然和之前一样，只不过将matlab模块转换为了CPP函数，头文件LQR.h：
#ifndef LQR_H
#define LQR_H
#include <Eigen/Dense>class LQRController {
public://构造函数LQRController();//预测函数void predictState(double& host_x, double& host_y, double& host_heading,double& host_Vx, double& host_Vy, double& host_yaw_rate);//LQR主计算函数bool Compute_LQR(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R,const int iter_max,const double tolerance);//误差计算函数Eigen::VectorXd err_Compute(double x_e,double y_e,double phi,double vx_e,double vy_e,double yaw_e,double xr, double yr, double thetar,double kappar);//前馈与最终输出计算函数double steer_Compute(double kappa);//设置Q矩阵//设置R矩阵// 预测结果double pred_x_ctrl_;double pred_y_ctrl_;double pred_yaw_ctrl_;double pred_vx_ctrl_;double pred_vy_ctrl_;double pred_yaw_dot_ctrl_;int _iter_max; // 最大迭代次数double _tolerance; // 迭代精度// lqr参数int _matrix_size;Eigen::MatrixXd _matrix_A; // 状态矩阵Eigen::MatrixXd _matrix_A_bar; // 离散化的状态矩阵Eigen::MatrixXd _matrix_B; // 输入矩阵Eigen::MatrixXd _matrix_B_bar; // 离散化的矩阵Eigen::MatrixXd _matrix_K; // 反馈矩阵Eigen::VectorXd _matrix_err; // 误差向量Eigen::MatrixXd _matrix_Q; // Q矩阵Eigen::MatrixXd _matrix_R; // R矩阵double ts;
private:// 车辆参数double _cf; // 前轮侧偏刚度系数double _cr; // 后轮侧偏刚度系数double _m; // 质量double _vx; // 沿着车身轴线的速度double _Iz; // 车身转动惯量double _a; // 质心到车前轴的距离double _b; // 质心到车后轴的距离double _steer_ratio; // 方向盘转角到轮胎转角之间的比值系数};#endif // LQR_H
主函数模块编写：
#include "Init_Ros.h"
#include "LQR.h"
//构造函数初始化
LQRController::LQRController()
{//车辆静态参数初始化_cf = -155494.663;_cr = -155494.663;_m = 1845.0;_a = 2.852/2.0;_b = 2.852/2.0;_Iz = _a*_a*(_m/2.0) + _b*_b*(_m/2.0);_steer_ratio = 16.0;//lqr静态矩阵初始化_matrix_size = 4;_matrix_A = Eigen::MatrixXd::Zero(_matrix_size,_matrix_size);_matrix_A_bar = Eigen::MatrixXd::Zero(_matrix_size,_matrix_size);_matrix_B = Eigen::MatrixXd::Zero(_matrix_size,1);_matrix_B_bar = Eigen::MatrixXd::Zero(_matrix_size,1);_matrix_K = Eigen::MatrixXd::Zero(1,_matrix_size);_matrix_err = Eigen::VectorXd::Zero(_matrix_size);_matrix_Q = Eigen::MatrixXd::Identity(_matrix_size,_matrix_size);_matrix_R = Eigen::MatrixXd::Identity(1,1);//A矩阵初始化_matrix_A(0,1) = 1.0;_matrix_A(1,1) = (_cf + _cr)/(_m * _vx);_matrix_A(1,2) = -(_cf + _cr)/(_m);_matrix_A(1,3) = (_a*_cf - _b*_cr)/(_m * _vx);_matrix_A(2,3) = 1.0;_matrix_A(3,1) = (_a*_cf - _b*_cr)/(_Iz * _vx);_matrix_A(3,2) = -(_a*_cf - _b*_cr)/(_Iz);_matrix_A(3,3) = (_a*_a*_cf + _b*_b*_cr)/(_Iz * _vx);//B矩阵初始化_matrix_B(1,0) = -_cf/_m;_matrix_B(3,0) = -_a*_cf/_Iz;//默认权重_matrix_R(0,0) = 0.1;//输入权重_matrix_Q(0,0) = 2.0;//横向距离误差权重_matrix_Q(1,1) = 0.0;//横向距离误差导数权重_matrix_Q(2,2) = 1.0;//横向角度误差权重_matrix_Q(3,3) = 0.0;//横向角度误差导数权重//迭代次数与容差_iter_max = 1500;_tolerance = 0.01;ts = 0.01;//前向欧拉法离散化矩阵auto I = Eigen::MatrixXd::Identity(_matrix_size,_matrix_size);_matrix_A_bar = (I + _matrix_A*ts);_matrix_B_bar = _matrix_B * ts;//初始化预测模块pred_x_ctrl_ = 0;pred_y_ctrl_ = 0;pred_yaw_ctrl_ = 0;pred_vx_ctrl_ = 0;pred_vy_ctrl_ = 0;pred_yaw_dot_ctrl_ = 0;}
//预测函数初始化
void LQRController::predictState(double& host_x, double& host_y, double& phi,double& host_Vx, double& host_Vy, double& host_yaw_rate)
{pred_x_ctrl_ = host_x+ host_Vx* ts * cos(phi) - host_Vy * ts * sin(phi);pred_y_ctrl_ = host_y + host_Vy * ts * cos(phi) + host_Vx * ts * sin(phi);pred_yaw_ctrl_ = phi;pred_vx_ctrl_ = host_Vx;pred_vy_ctrl_ = host_Vy;pred_yaw_dot_ctrl_ = host_yaw_rate;
}
//LQR主计算函数
bool LQRController::Compute_LQR(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B, const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R,const int iter_max,const double tolerance)
{//判断输入矩阵的维数是否正确if(A.rows()!=A.cols() || B.rows()!=A.rows() || Q.rows()!=Q.cols() || Q.rows()!=A.rows() || R.cols()!=B.cols()){return false;}auto matrix_size = A.rows();Eigen::MatrixXd P;Eigen::MatrixXd P_next;Eigen::MatrixXd AT;Eigen::MatrixXd BT;//初始化P = Eigen::MatrixXd::Zero(matrix_size,matrix_size);//P初始化成Q矩阵也可以，因为P初始化为0矩阵的话，第一次迭代P_next就是Q矩阵P_next = Eigen::MatrixXd::Zero(matrix_size,matrix_size);AT = A.transpose();BT = B.transpose();//迭代计算黎卡提方程for (int i = 0; i < iter_max; i++){ P_next = Q + AT*P*A3 - AT*P*B*(R+BT*P*B).inverse()*BT*P*A;if (fabs((P_next-P).maxCoeff()) < tolerance){ROS_INFO("success");_matrix_K = (R + BT*P*B).inverse()*(BT*P*A);return true;}//ROS_INFO("false");//第一次写忘记写P = P_next;}return false;
}
//误差计算函数
Eigen::VectorXd LQRController::err_Compute(double x_e,double y_e,double phi,double vx_e,double vy_e,double yaw_e,double xr, double yr, double thetar,double kappar)
{// 创建向量Eigen::Vector2d tor(cos(thetar), sin(thetar)); // 方向向量Eigen::Vector2d nor(-sin(thetar), cos(thetar)); // 法向量Eigen::Vector2d d_err(x_e - xr, y_e - yr); // 位置误差// 计算误差double ed = nor.transpose() * d_err; // 误差沿法向量double es = tor.transpose() * d_err; // 误差沿切向量// 投影点double projection_point_thetar = thetar + kappar * es;// 计算误差导数double ed_dot = y_e * cos(phi - projection_point_thetar) + x_e * sin(phi - projection_point_thetar);// 计算ephidouble ephi = sin(phi - projection_point_thetar);// 计算s_dotdouble ss_dot = vx_e * cos(phi - projection_point_thetar) - vy_e * sin(phi - projection_point_thetar);double s_dot = ss_dot / (1 - kappar * ed);double ephi_dot = yaw_e - kappar * s_dot;// 将误差组合成向量Eigen::VectorXd err(4); // 创建一个四维向量err << ed, ed_dot, ephi, ephi_dot;return err;
}
//控制输出量计算
double LQRController::steer_Compute(double kappa)
{//前馈计算 if (! Compute_LQR(_matrix_A_bar,_matrix_B_bar,_matrix_Q,_matrix_R,_iter_max,_tolerance)){_matrix_K = Eigen::MatrixXd::Zero(1,_matrix_size);}double k3 = _matrix_K(0,2);double k_m = kappa;double delta_f = k_m*(_a + _b - _b*k3 - (_m*_vx*_vx/(_a+_b)) * (_b/_cf + _a*k3/_cr - _a/_cr));//最终控制输出量计算double u = (_matrix_K*_matrix_err)[0] + delta_f;//限制前轮转角double max_u = 60.0 * M_PI / 180.0;u = std::min(std::max(u,-max_u),max_u);double steer = -u;//前轮转角计算出负值应该是右转，所以有一个负号。double steer_Angel = std::min(std::max(steer,-10.0),10.0);return steer_Angel;
}3.3 用于测试的上层规划模块
由于规划模块还未集成到本项目中，故先采用离散全局路径来作为规划点，EM_Plan.h如下：
#include "LQR.h"
#include "Init_Ros.h"
class EM_plan
{
public:EM_plan();//计算距离double calculateDistance(double X, double Y, double X1, double Y1);//查找最近的索引点int findClosestPoint(const std::vector<double>& X,const std::vector<double>& Y, double currentX, double currentY);//计算理想曲率std::vector<double> calculateCurvature(const std::vector<double>& X,const std::vector<double>& Y); //计算理想航向角std::vector<double> calculateHeading(const std::vector<double>& X,const std::vector<double>& Y);double xr_;double yr_;double thetar_;double kappar_;private:};函数实现如下：
#include "EM_plan.h"
//构造函数
EM_plan::EM_plan()
{xr_ = 0;yr_ = 0;thetar_ = 0;kappar_ = 0;
}
//计算距离
double EM_plan::calculateDistance(double X, double Y, double X1, double Y1) {return sqrt((X - X1) * (X - X1) + (Y - Y1) * (Y - Y1));}
//计算最近的索引点
int EM_plan::findClosestPoint(const std::vector<double>& X, const std::vector<double>& Y, double currentX, double currentY)
{int closestIndex = -1;double minDistance = std::numeric_limits<double>::max();for (size_t i = 0; i < X.size(); ++i) {double distance = calculateDistance(X[i], Y[i], currentX, currentY);if (distance < minDistance) {minDistance = distance;closestIndex = i;}}return closestIndex+15;
}//计算理想曲率
std::vector<double> EM_plan::calculateCurvature(const std::vector<double>& X,const std::vector<double>& Y)
{std::vector<double> headings;for (size_t i = 1; i < X.size(); ++i) {double dx = X[i] - X[i - 1];double dy = Y[i] - Y[i - 1];double heading = atan2(dy, dx);headings.push_back(heading);}return headings;
}
//计算理想航向角
std::vector<double> EM_plan::calculateHeading(const std::vector<double>& X,const std::vector<double>& Y)
{std::vector<double> curvatures;for (size_t i = 1; i < X.size() - 1; ++i) {double dx1 = X[i] - X[i - 1];double dy1 = Y[i] - Y[i - 1];double dx2 = X[i + 1] - X[i];double dy2 = Y[i + 1] - Y[i];double curvature = (dy2 / dx2 - dy1 / dx1) / (1 + (dy1 / dx1) * (dy1 / dx1));curvatures.push_back(curvature);}return curvatures;
}
3.4 主函数
main.cpp如下：
#include "Init_Ros.h"
#include "LQR.h"
#include "EM_plan.h"
#include <ros/ros.h>
int main(int argc, char** argv)
{ros::init(argc, argv, "vehicle_data_receiver_node");InitRos RosPrescan;RosPrescan.run();return 0;
}
