文章目录

1 目录概述
2 算法介绍
- 2.1 Astart改进
- 2.2 ROS（Gazebo仿真）
- - 2.2.1 使用Gazebo仿真需要安装的功能包
  - 2.2.2 创建工作空间 catkin_ws
  - 2.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/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.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横向控制算法