规划控制复现：Apollo LQR横向控制（C++/simulink实现）

本文在前文已经搭建好的ROS-C++规划控制验证平台中进行LQR算法的复现，理论部分详见于：

规划控制复现：Apollo LQR横向控制（算法原理推导与流程）_apollo 规划控制-优快云博客

Prescan-Carsim-ROS的仿真平台搭建详见于：

简单硬件在环搭建（ROS+Prescan+Carsim+simulink)_prescan ros-优快云博客

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;
end

2.2 LQR增益矩阵K的计算

在本算法中，为节省计算开销，采用查表的方式对速度进行插值，采样5000个点，先通过不同的输入速度Vx进行离线计算，再差值得到LQR的增益矩阵K：

离线计算算法如下，调用matlab的LQR求解器对LQR问题进行求解：

k=zeros(5000,4);
vx_break_point=zeros(1,5000);
for i=1:5000
    vx_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.1
        k=[0,0,0,0];
    else
       
        k=[k1,k2,k3,k4];
    end
end

2.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);%apollo
    projection_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]);
        // 更新全局的 steer
        double 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-优快云博客

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);
    // 计算ephi
    double ephi     = sin(phi - projection_point_thetar);
    // 计算s_dot
    double 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;
}