文章目录

1 目录概述2 算法介绍2.1 Astart改进2.2 ROS（Gazebo仿真）2.2.1 使用Gazebo仿真需要安装的功能包2.2.2 创建工作空间 catkin_ws2.2.3 Pure_pursuit算法2.2.4 LQR横向控制算法

​ 最近在学习自动驾驶规划控制相关内容，并着手用c++和ros编写相关算法，代码部分见/NeXTzhao/planning.git，下面是对github内容的一些说明。

1 目录概述

routing_planning/Astart改进

针对A*算法做出优化：加入靠近路沿的启发函数，并对生成的轨迹点做了均值滤波处理，使轨迹更加平滑。

routing_planning/ros/src

ros工作空间中，purepursuit功能包使用purepursuit算法对spline生成的样条曲线进行了路径跟踪。lqr_steering功能包使用lqr算法对生成的五次多项式轨迹进行横向路径跟踪。

purepursuit算法:原理很简单，网上很多资料也比较多

LQR控制算法主要参考b站up主

2 算法介绍

2.1 Astart改进

编译：g++ -std=c++11 xxx.cpp -o xx $(pkg-config --cflags --libs opencv) （需要安装opencv）

实现思路：

先用opencv将图片做灰度处理，再做二值化，将像素保存到vector二维数组作为地图，设置起点和终点，调用AStart算法(改进版：加入路沿代价函数)找到一条路径，由于算法会导致路径出现锯齿状，故用均值滤波对路径点做平滑处理。

算法流程：

见github

2.2 ROS（Gazebo仿真）

系统要求：ubuntu + ros +gazebo

2.2.1 使用Gazebo仿真需要安装的功能包

sudo apt-get install -y ros-(对应的ros版本，例如kinetic，下面两个同理)-gazebo-ros-controlsudo apt-get install -y ros-kinetic-ros-control ros-kinetic-ros-controllerssudo apt-get install -y ros-kinetic-gazebo-ros-control

2.2.2 创建工作空间 catkin_ws

1.创建src文件，放置功能包源码：mkdir -p ~/catkin_ws/src2.进入src文件夹cd ~/catkin_ws/src3.将路径ros/src下的功能包复制粘贴到创建的src目录下4.初始化文件夹catkin_init_workspace5.编译工作空间catkin_makecd ~/catkin_ws/catkin_make

2.2.3 Pure_pursuit算法

实现思路

运用spline插值进行简单轨迹生成编写pure_pursuit纯路径跟踪算法，对生成的轨迹进行跟踪

代码部分

/*** @file purepursuit.cpp*/#include <ros/ros.h>#include <algorithm>#include <cassert>#include <cmath>#include <fstream>#include <iostream>#include <string>#include <vector>#include <geometry_msgs/Twist.h>#include <nav_msgs/Path.h>#include <tf/tf.h>#include <tf/transform_broadcaster.h>#include "geometry_msgs/PoseStamped.h"#include "cpprobotics_types.h"#include "cubic_spline.h"#define PREVIEW_DIS 3 //预瞄距离#define Ld 1.868 //轴距using namespace std;using namespace cpprobotics;ros::Publisher purepersuit_;ros::Publisher path_pub_;nav_msgs::Path path;float carVelocity = 0;float preview_dis = 0;float k = 0.1;// 计算四元数转换到欧拉角std::array<float, 3> calQuaternionToEuler(const float x, const float y,const float z, const float w) {std::array<float, 3> calRPY = {(0, 0, 0)};// roll = atan2(2(wx+yz),1-2(x*x+y*y))calRPY[0] = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));// pitch = arcsin(2(wy-zx))calRPY[1] = asin(2 * (w * y - z * x));// yaw = atan2(2(wx+yz),1-2(y*y+z*z))calRPY[2] = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));return calRPY;}cpprobotics::Vec_f r_x_;cpprobotics::Vec_f r_y_;int pointNum = 0; //保存路径点的个数int targetIndex = pointNum - 1;/*方案一*/// vector<int> bestPoints_ = {pointNum - 1};/*方案二*/vector<float> bestPoints_ = {0.0};//计算发送给模型车的转角void poseCallback(const geometry_msgs::PoseStamped &currentWaypoint) {auto currentPositionX = currentWaypoint.pose.position.x;auto currentPositionY = currentWaypoint.pose.position.y;auto currentPositionZ = 0.0;auto currentQuaternionX = currentWaypoint.pose.orientation.x;auto currentQuaternionY = currentWaypoint.pose.orientation.y;auto currentQuaternionZ = currentWaypoint.pose.orientation.z;auto currentQuaternionW = currentWaypoint.pose.orientation.w;std::array<float, 3> calRPY =calQuaternionToEuler(currentQuaternionX, currentQuaternionY,currentQuaternionZ, currentQuaternionW);/*************************************************************************************************// 方案一：通过累加路径距离，和预瞄距离进行比较以及夹角方向// 寻找匹配目标点for (int i = 0; i < pointNum; i++) {float lad = 0.0; //累加前视距离float next_x = r_x_[i + 1];float next_y = r_y_[i + 1];lad += sqrt(pow(next_x - currentPositionX, 2) +pow(next_y - currentPositionY, 2));// cos(aAngle)判断方向float aAngle =atan2(next_y - currentPositionY, next_x - currentPositionX) -calRPY[2];if (lad > preview_dis && cos(aAngle) >= 0) {targetIndex = i + 1;bestPoints_.push_back(targetIndex);break;}}// 取容器中的最大值int index = *max_element(bestPoints_.begin(), bestPoints_.end());***************************************************************************************************//**************************************************************************************************/// 方案二:通过计算当前坐标和目标轨迹距离，找到一个最小距离的索引号int index;vector<float> bestPoints_;for (int i = 0; i < pointNum; i++) {// float lad = 0.0;float path_x = r_x_[i];float path_y = r_y_[i];// 遍历所有路径点和当前位置的距离，保存到数组中float lad = sqrt(pow(path_x - currentPositionX, 2) +pow(path_y - currentPositionY, 2));bestPoints_.push_back(lad);}// 找到数组中最小横向距离auto smallest = min_element(bestPoints_.begin(), bestPoints_.end());// 找到最小横向距离的索引位置index = distance(bestPoints_.begin(), smallest);int temp_index;for (int i = index; i < pointNum; i++) {//遍历路径点和预瞄点的距离，从最小横向位置的索引开始float dis =sqrt(pow(r_y_[index] - r_y_[i], 2) + pow(r_x_[index] - r_x_[i], 2));//判断跟预瞄点的距离if (dis < preview_dis) {temp_index = i;} else {break;}}index = temp_index;/**************************************************************************************************/float alpha =atan2(r_y_[index] - currentPositionY, r_x_[index] - currentPositionX) -calRPY[2];// 当前点和目标点的距离Idfloat dl = sqrt(pow(r_y_[index] - currentPositionY, 2) +pow(r_x_[index] - currentPositionX, 2));// 发布小车运动指令及运动轨迹if (dl > 0.2) {float theta = atan(2 * Ld * sin(alpha) / dl);geometry_msgs::Twist vel_msg;vel_msg.linear.x = 3;vel_msg.angular.z = theta;purepersuit_.publish(vel_msg);// 发布小车运动轨迹geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.pose.position.x = currentPositionX;this_pose_stamped.pose.position.y = currentPositionY;geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(theta);this_pose_stamped.pose.orientation.x = currentQuaternionX;this_pose_stamped.pose.orientation.y = currentQuaternionY;this_pose_stamped.pose.orientation.z = currentQuaternionZ;this_pose_stamped.pose.orientation.w = currentQuaternionW;this_pose_stamped.header.stamp = ros::Time::now();this_pose_stamped.header.frame_id = "world";path.poses.push_back(this_pose_stamped);} else {geometry_msgs::Twist vel_msg;vel_msg.linear.x = 0;vel_msg.angular.z = 0;purepersuit_.publish(vel_msg);}path_pub_.publish(path);}void velocityCall(const geometry_msgs::TwistStamped &carWaypoint) {// carVelocity = carWaypoint.linear.x;carVelocity = carWaypoint.twist.linear.x;preview_dis = k * carVelocity + PREVIEW_DIS;}void pointCallback(const nav_msgs::Path &msg) {// geometry_msgs/PoseStamped[] posespointNum = msg.poses.size();// auto a = msg.poses[0].pose.position.x;for (int i = 0; i < pointNum; i++) {r_x_.push_back(msg.poses[i].pose.position.x);r_y_.push_back(msg.poses[i].pose.position.y);}int main(int argc, char **argv) {//创建节点ros::init(argc, argv, "pure_pursuit");//创建节点句柄ros::NodeHandle n;//创建Publisher，发送经过pure_pursuit计算后的转角及速度purepersuit_ = n.advertise<geometry_msgs::Twist>("/smart/cmd_vel", 20);path_pub_ = n.advertise<nav_msgs::Path>("rvizpath", 100, true);// ros::Rate loop_rate(10);path.header.frame_id = "world";// 设置时间戳path.header.stamp = ros::Time::now();geometry_msgs::PoseStamped pose;pose.header.stamp = ros::Time::now();// 设置参考系pose.header.frame_id = "world";ros::Subscriber splinePath = n.subscribe("/splinepoints", 20, pointCallback);ros::Subscriber carVel = n.subscribe("/smart/velocity", 20, velocityCall);ros::Subscriber carPose = n.subscribe("/smart/rear_pose", 20, poseCallback);ros::spin();return 0;}

操作步骤：（新开终端窗口）

source devel/setup.shroslaunch car_model spawn_car.launchroslaunch purepursuit splinepath.launch roslaunch purepursuit purepursuit.launchrviz 中add /splinepoints /rvizpath /smart（在rviz显示中，红色为小车运动轨迹，绿色为规划模块给出的轨迹）

Pure_pursuit仿真结果：

2.2.4 LQR横向控制算法

实现思路

运用五次多项式生成控制算法所需要的轨迹编写lqr路径跟踪算法，对轨迹进行跟踪控制

代码部分

/*** @file frenetlqr.cpp*/#include <stdio.h>#include <Eigen/Eigen>#include <array>#include <fstream>#include <iostream>#include <string>#include <vector>#include <geometry_msgs/PoseStamped.h>#include <geometry_msgs/Twist.h>#include <nav_msgs/Path.h>#include <tf/tf.h>#include <tf/transform_broadcaster.h>#include "cpprobotics_types_double.h"#include "frenet_path_double.h"#include "quintic_polynomial_double.h"#define DT 0.1 // time tick [s]using namespace cpprobotics;ros::Publisher frenet_lqr_;ros::Publisher path_pub_;ros::Publisher trajectory_path;nav_msgs::Path path;nav_msgs::Path trajectorypath;/**************************************************************************/// t-t0经历的时间double T = 50;double xend = 80.0;double yend = 30.0;// 起始状态std::array<double, 3> x_start{0.0, 0.0, 0.0};std::array<double, 3> x_end{xend, 0.0, 0.0};// 终点状态std::array<double, 3> y_start{0.0, 0.0, 0.0};std::array<double, 3> y_end{yend, 0.0, 0.0};/**************************************************************************//*** 整车参数及状态*/// 纵向速度double vx = 0.01;// 横向速度double vy = 0; //质心侧偏角视为不变// 轮胎侧偏刚度double cf = -65494.663, cr = -115494.663;// 前后悬架载荷double mass_fl = 500, mass_fr = 500, mass_rl = 520, mass_rr = 520;double mass_front = mass_fl + mass_fr;double mass_rear = mass_rl + mass_rr;double m = mass_front + mass_rear;// 最大纵向加速度double max_lateral_acceleration = 5.0;// 最大制动减速度double standstill_acceleration = -3.0;// 轴距double wheel_base = 3.8;// 前轴中心到质心的距离double a = wheel_base * (1.0 - mass_front / m);// 后轴中心到质心的距离double b = wheel_base * (1.0 - mass_rear / m);// 车辆绕z轴转动的转动惯量double Iz = std::pow(a, 2) * mass_front + std::pow(b, 2) * mass_rear;// 轮胎最大转角(rad)double wheel_max_degree = 0.6;/**************************************************************************//*** @brief 计算四元数转换到欧拉角* @return std::array<double, 3>*/std::array<double, 3> calQuaternionToEuler(const double x, const double y,const double z, const double w) {std::array<double, 3> calRPY = {(0, 0, 0)};// roll = atan2(2(wx+yz),1-2(x*x+y*y))calRPY[0] = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));// pitch = arcsin(2(wy-zx))calRPY[1] = asin(2 * (w * y - z * x));// yaw = atan2(2(wx+yz),1-2(y*y+z*z))calRPY[2] = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));return calRPY;}/**************************************************************************//*** @brief 规划路径**/FrenetPath fp;void calc_frenet_paths() {// 纵向QuinticPolynomial lon_qp(x_start[0], x_start[1], x_start[2], x_end[0],x_end[1], x_end[2], T);// 横向QuinticPolynomial lat_qp(y_start[0], y_start[1], y_start[2], y_end[0],y_end[1], y_end[2], T, xend);for (double t = 0; t < T; t += DT) {double x = lon_qp.calc_point_x(t);double xd = lon_qp.calc_point_xd(t);double xdd = lon_qp.calc_point_xdd(t);fp.t.push_back(t);fp.x.push_back(x);fp.x_d.push_back(xd);fp.x_dd.push_back(xdd);double y_x_t = lat_qp.calc_point_y_x(x);double y_x_d = lat_qp.calc_point_y_x_d(x);double y_x_t_d = lat_qp.calc_point_y_t_d(y_x_d, xd);double y_x_dd = lat_qp.calc_point_y_x_dd(x);double y_x_t_dd = lat_qp.calc_point_y_t_dd(y_x_dd, xd, y_x_d, xdd);fp.y.push_back(y_x_t);fp.y_d.push_back(y_x_t_d);fp.y_dd.push_back(y_x_t_dd);// 压入航向角// fp.threat.push_back(lat_qp.calc_point_thetar(y_x_t_d, xd));// 压入曲率fp.k.push_back(lat_qp.calc_point_k(y_x_dd, y_x_d));// fp.k.push_back(lat_qp.calc_point_k(y_x_t_dd, y_x_t_d, xdd, xd));}int num = fp.x.size();for (int i = 0; i < num; i++) {double dy = fp.y[i + 1] - fp.y[i];double dx = fp.x[i + 1] - fp.x[i];fp.threat.push_back(lat_qp.calc_point_thetar(dy, dx));}// 最后一个道路航向角和前一个相同// fp.threat.push_back(fp.threat.back());}/**************************************************************************//*** @brief 寻找匹配点及距离最短的点* @return int*/int index_ = 0;int findTrajref(double current_post_x, double current_post_y) {int numPoints = fp.x.size();// double dis_min = std::pow(fp.x[0] - current_post_x, 2) +// std::pow(fp.y[0] - current_post_y, 2);double dis_min = std::numeric_limits<double>::max();int index = 0;for (int i = index; i < numPoints; i++) {double temp_dis = std::pow(fp.x[i] - current_post_x, 2) +std::pow(fp.y[i] - current_post_y, 2);// printf("dis_min,temp_dis=%f,%f \n", dis_min, temp_dis);if (temp_dis < dis_min) {dis_min = temp_dis;index = i;}}index_ = index;// printf("index,numPoints=%d,%d \n", index, numPoints);return index;}/*** @brief 计算误差err和投影点的曲率* 1.先遍历找到匹配点* 2.通过匹配点近似求解投影点* 2.1 由投影点得到对应的航向角和曲率* @return std::array<double, 5>*/std::array<double, 5> cal_err_k(double current_post_x, double current_post_y,std::array<double, 3> calRPY) {std::array<double, 5> err_k;int index = findTrajref(current_post_x, current_post_y);// 找到index后，开始求解投影点// Eigen::Vector2f tor;Eigen::Matrix<double, 2, 1> tor;tor << cos(fp.threat[index]), sin(fp.threat[index]);// Eigen::Vector2f nor;Eigen::Matrix<double, 2, 1> nor;nor << -sin(fp.threat[index]), cos(fp.threat[index]);// Eigen::Vector2f d_err;Eigen::Matrix<double, 2, 1> d_err;d_err << current_post_x - fp.x[index], current_post_y - fp.y[index];double phi = calRPY[2];// nor.transpose()对nor转置double ed = nor.transpose() * d_err;// double ed = -vx*sin();double es = tor.transpose() * d_err;// 投影点的threat角度double projection_point_threat = fp.threat[index] + fp.k[index] * es;// double phi = fp.threat[index];double ed_d = vy * cos(phi - projection_point_threat) +vx * sin(phi - projection_point_threat);// 计算ephi// double ephi = sin(phi - projection_point_threat);double ephi = phi - projection_point_threat;// 计算s_ddouble s_d = (vx * cos(phi - projection_point_threat) -vy * sin(phi - projection_point_threat)) /(1 - fp.k[index] * ed);double phi_d = vx * fp.k[index];double ephi_d = phi_d - fp.k[index] * s_d;// 计算投影点曲率kdouble projection_point_curvature = fp.k[index];err_k[0] = ed;err_k[1] = ed_d;err_k[2] = ephi;err_k[3] = ephi_d;err_k[4] = projection_point_curvature;return err_k;}/*** @brief 求解k系数* 1.首先用迭代法解黎卡提方程得到参数得到p矩阵* 2.将p带入k得到k值* 2.将得到的k带入u(n)=-kx(n)得到u也就是转角的控制量* @return Eigen::RowVector4cf*/Eigen::Matrix<double, 1, 4> cal_dlqr(Eigen::Matrix4d A,Eigen::Matrix<double, 4, 1> B,Eigen::Matrix4d Q,Eigen::Matrix<double, 1, 1> R) {// 设置最大循环迭代次数int numLoop = 200;// 设置目标极小值double minValue = 10e-10;Eigen::Matrix4d p_old;p_old = Q;/*************************************//*** 离散化状态方程**/double ts = 0.001;Eigen::Matrix4d eye;eye.setIdentity(4, 4);Eigen::Matrix4d Ad;Ad = (eye - ts * 0.5 * A).inverse() * (eye + ts * 0.5 * A);Eigen::Matrix<double, 4, 1> Bd;Bd = B * ts;/*************************************/for (int i = 0; i < numLoop; i++) {// B.inverse()求逆Eigen::Matrix4d p_new = Ad.transpose() * p_old * Ad -Ad.transpose() * p_old * Bd *(R + Bd.transpose() * p_old * Bd).inverse() *Bd.transpose() * p_old * Ad +Q;// p.determinant()求行列式// if (std::abs((p_old - p_new).determinant()) <= minValue) {// cwiseAbs()求绝对值、maxCoeff()求最大系数if (fabs((p_new - p_old).maxCoeff()) < minValue) {p_old = p_new;break;}p_old = p_new;}Eigen::Matrix<double, 1, 4> k;// Eigen::RowVector4f;// 当两个超出范围的浮点数（即INF）进行运算时，运算结果会成为NaN。k = (R + Bd.transpose() * p_old * Bd).inverse() * Bd.transpose() * p_old * Ad;return k;}/*** @brief 计算k值** @param err_k* @return Eigen::Matrix<double, 1, 4>*/Eigen::Matrix<double, 1, 4> cal_k(std::array<double, 5> err_k) {Eigen::Matrix4d A;A << 0, 1, 0, 0, 0, (cf + cr) / (m * vx), -(cf + cr) / m,(a * cf - b * cr) / (m * vx), 0, 0, 0, 1, 0,(a * cf - b * cr) / (Iz * vx), -(a * cf - b * cr) / Iz,(a * a * cf + b * b * cr) / (Iz * vx);// Eigen::Vector4f B;Eigen::Matrix<double, 4, 1> B;B << 0, -cf / m, 0, -a * cf / Iz;// Eigen::Matrix4f Q;// // 设置成单位矩阵Eigen::Matrix4d Q;// Q.setIdentity(4, 4);Q(0, 0) = 60;Q(1, 1) = 1;Q(2, 2) = 1;Q(3, 3) = 1;Eigen::Matrix<double, 1, 1> R;R(0, 0) = 35.0;// MatrixXd矩阵只能用(),VectorXd不仅能用()还能用[]Eigen::Matrix<double, 1, 4> k = cal_dlqr(A, B, Q, R);return k;}/*** @brief 计算前馈环节* @return double*/double cal_forword_angle(Eigen::Matrix<double, 1, 4> k,std::array<double, 5> err_k) {double k3 = k[2];// 不足转向系数double kv = b * m / (cf * wheel_base) - a * m / (cr * wheel_base);//投影点的曲率final_path.k[index]double point_curvature = err_k[4];double forword_angle =wheel_base * point_curvature + kv * vx * vx * point_curvature -k3 * (b * point_curvature - a * m * vx * vx * point_curvature / cr / b);return forword_angle;}/*** @brief 计算前轮转角u*/double cal_angle(Eigen::Matrix<double, 1, 4> k, double forword_angle,std::array<double, 5> err_k) {Eigen::Matrix<double, 4, 1> err;err << err_k[0], err_k[1], err_k[2], err_k[3];double angle = -k * err + forword_angle;return angle;}/*** @brief 限制前轮最大转角*/double limitSterringAngle(double value, double bound1, double bound2) {if (bound1 > bound2) {std::swap(bound1, bound2);}if (value < bound1) {return bound1;} else if (value > bound2) {return bound2;}return value;}/*** @brief 统一调用各个子函数* @return double*/double theta_angle(double currentPositionX, double currentPositionY,std::array<double, 3> cal_RPY) {std::array<double, 5> err_k =cal_err_k(currentPositionX, currentPositionY, cal_RPY);Eigen::Matrix<double, 1, 4> k = cal_k(err_k);double forword_angle = cal_forword_angle(k, err_k);double tempangle = cal_angle(k, forword_angle, err_k);double angle =limitSterringAngle(tempangle, -wheel_max_degree, wheel_max_degree);printf("angle,forword_angle=%.3f,%.3f\n", angle, forword_angle);return angle;}void velocityCall(const geometry_msgs::TwistStamped &carWaypoint) {//错误写法 carVelocity = carWaypoint.linear.x;vx = carWaypoint.twist.linear.x;}void poseCallback(const geometry_msgs::PoseStamped &currentWaypoint) {double currentPositionX = currentWaypoint.pose.position.x;double currentPositionY = currentWaypoint.pose.position.y;double currentPositionZ = 0.0;double currentQuaternionX = currentWaypoint.pose.orientation.x;double currentQuaternionY = currentWaypoint.pose.orientation.y;double currentQuaternionZ = currentWaypoint.pose.orientation.z;double currentQuaternionW = currentWaypoint.pose.orientation.w;std::array<double, 3> cal_RPY =calQuaternionToEuler(currentQuaternionX, currentQuaternionY,currentQuaternionZ, currentQuaternionW);double theta = theta_angle(currentPositionX, currentPositionY, cal_RPY);int numpoints = fp.x.size();if (index_ < numpoints - 2) {geometry_msgs::Twist vel_msg;vel_msg.linear.x = 8;vel_msg.angular.z = theta;frenet_lqr_.publish(vel_msg);geometry_msgs::PoseStamped this_pose_stamped;this_pose_stamped.pose.position.x = currentPositionX;this_pose_stamped.pose.position.y = currentPositionY;geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(theta);this_pose_stamped.pose.orientation.x = currentQuaternionX;this_pose_stamped.pose.orientation.y = currentQuaternionY;this_pose_stamped.pose.orientation.z = currentQuaternionZ;this_pose_stamped.pose.orientation.w = currentQuaternionW;this_pose_stamped.header.stamp = ros::Time::now();this_pose_stamped.header.frame_id = "world";trajectorypath.poses.push_back(this_pose_stamped);trajectory_path.publish(trajectorypath);} else {geometry_msgs::Twist vel_msg;vel_msg.linear.x = 0;vel_msg.angular.z = 0;frenet_lqr_.publish(vel_msg);}}int main(int argc, char **argv) {//创建节点ros::init(argc, argv, "lqr");//创建节点句柄ros::NodeHandle a;//创建Publisher，发送经过lqr计算后的转角及速度frenet_lqr_ = a.advertise<geometry_msgs::Twist>("/smart/cmd_vel", 20);//初始化五次多项式轨迹calc_frenet_paths();int Num = fp.x.size();for (int i = 0; i < Num; i++) {printf("x,y,th,k,i=%.3f,%.3f,%.3f,%f,%d \n", fp.x[i], fp.y[i],fp.threat[i],fp.k[i], i);}/**************************************************************/// 发布规划轨迹path_pub_ = a.advertise<nav_msgs::Path>("rviz_path", 20, true);path.header.frame_id = "world";path.header.stamp = ros::Time::now();geometry_msgs::PoseStamped pose;pose.header.stamp = ros::Time::now();pose.header.frame_id = "world";int sNum = fp.x.size();for (int i = 0; i < sNum; i++) {pose.pose.position.x = fp.x[i];pose.pose.position.y = fp.y[i];path.poses.push_back(pose);}path_pub_.publish(path);/**************************************************************///发布小车运动轨迹trajectory_path = a.advertise<nav_msgs::Path>("trajector_ypath", 20, true);trajectorypath.header.frame_id = "world";trajectorypath.header.stamp = ros::Time::now();/**************************************************************/ros::Subscriber carVel = a.subscribe("/smart/velocity", 20, velocityCall);ros::Subscriber carPose = a.subscribe("/smart/rear_pose", 20, poseCallback);ros::spin();return 0;};

操作步骤：（新开终端窗口）

source devel/setup.shroslaunch car_model spawn_car.launchroslaunch lqr_steering frenet_lqr.launch rviz 中add /trajector_ypath /rviz_path /smart（在rviz显示中，红色为小车运动轨迹，绿色为规划模块给出的轨迹）

LQR仿真结果：