自动驾驶规划控制（Ａ*、pure pursuit、LQR算法，使用c++在ubuntu和ros环境下实现）

2024-06-26 21:49| 来源: 网络整理| 查看: 265

文章目录 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编写相关算法，代码部分见https://github.com/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-control sudo apt-get install -y ros-kinetic-ros-control ros-kinetic-ros-controllers sudo apt-get install -y ros-kinetic-gazebo-ros-control 2.2.2 创建工作空间 catkin_ws 1.创建src文件，放置功能包源码： mkdir -p ~/catkin_ws/src 2.进入src文件夹 cd ~/catkin_ws/src 3.将路径ros/src下的功能包复制粘贴到创建的src目录下 4.初始化文件夹 catkin_init_workspace 5.编译工作空间catkin_make cd ~/catkin_ws/ catkin_make 2.2.3 Pure_pursuit算法

实现思路

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

代码部分

/** * @file purepursuit.cpp */ #include #include #include #include #include #include #include #include #include #include #include #include #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 calQuaternionToEuler(const float x, const float y, const float z, const float w) { std::array 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 bestPoints_ = {pointNum - 1}; /*方案二*/ vector bestPoints_ = {0.0}; //计算发送给模型车的转角 void poseCallback(const geometry_msgs::PoseStamped ¤tWaypoint) { 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 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 bestPoints_; for (int i = 0; i //遍历路径点和预瞄点的距离，从最小横向位置的索引开始 float dis = sqrt(pow(r_y_[index] - r_y_[i], 2) + pow(r_x_[index] - r_x_[i], 2)); //判断跟预瞄点的距离 if (dis break; } } index = temp_index; /**************************************************************************************************/ float alpha = atan2(r_y_[index] - currentPositionY, r_x_[index] - currentPositionX) - calRPY[2]; // 当前点和目标点的距离Id float 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[] poses pointNum = msg.poses.size(); // auto a = msg.poses[0].pose.position.x; for (int i = 0; i //创建节点 ros::init(argc, argv, "pure_pursuit"); //创建节点句柄 ros::NodeHandle n; //创建Publisher，发送经过pure_pursuit计算后的转角及速度 purepersuit_ = n.advertise("/smart/cmd_vel", 20); path_pub_ = n.advertise("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.sh roslaunch car_model spawn_car.launch roslaunch purepursuit splinepath.launch roslaunch purepursuit purepursuit.launch rviz 中add /splinepoints /rvizpath /smart（在rviz显示中，红色为小车运动轨迹，绿色为规划模块给出的轨迹）

Pure_pursuit仿真结果：

2.2.4 LQR横向控制算法

实现思路

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

代码部分

/** * @file frenetlqr.cpp */ #include #include #include #include #include #include #include #include #include #include #include #include #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 x_start{0.0, 0.0, 0.0}; std::array x_end{xend, 0.0, 0.0}; // 终点状态 std::array y_start{0.0, 0.0, 0.0}; std::array 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 */ std::array calQuaternionToEuler(const double x, const double y, const double z, const double w) { std::array 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 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::max(); int index = 0; for (int i = index; i 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 */ std::array cal_err_k(double current_post_x, double current_post_y, std::array calRPY) { std::array err_k; int index = findTrajref(current_post_x, current_post_y); // 找到index后，开始求解投影点 // Eigen::Vector2f tor; Eigen::Matrix tor; tor // 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()) p_old = p_new; break; } p_old = p_new; } Eigen::Matrix 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 */ Eigen::Matrix cal_k(std::array err_k) { Eigen::Matrix4d A; A Eigen::Matrix err; err std::swap(bound1, bound2); } if (value return bound2; } return value; } /** * @brief 统一调用各个子函数 * @return double */ double theta_angle(double currentPositionX, double currentPositionY, std::array cal_RPY) { std::array err_k = cal_err_k(currentPositionX, currentPositionY, cal_RPY); Eigen::Matrix 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 ¤tWaypoint) { 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 cal_RPY = calQuaternionToEuler(currentQuaternionX, currentQuaternionY, currentQuaternionZ, currentQuaternionW); double theta = theta_angle(currentPositionX, currentPositionY, cal_RPY); int numpoints = fp.x.size(); if (index_ 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("/smart/cmd_vel", 20); //初始化五次多项式轨迹 calc_frenet_paths(); int Num = fp.x.size(); for (int i = 0; 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("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.sh roslaunch car_model spawn_car.launch roslaunch lqr_steering frenet_lqr.launch rviz 中add /trajector_ypath /rviz_path /smart　（在rviz显示中，红色为小车运动轨迹，绿色为规划模块给出的轨迹）

LQR仿真结果： lqr

【本文地址】

公司简介

联系我们