羊村懒王

这个屌丝很懒，什么也没留下！

热门标签

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

作者：羊村懒王 | 2024-04-29 23:12:22

踩

自动驾驶规划控制(a*、pure pursuit、lqr算法,使用c++在ubuntu和ros环境下实现):

文章目录

1 目录概述
2 算法介绍

最近在学习自动驾驶规划控制相关内容，并着手用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）
1

实现思路：

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

算法流程：

见github

2.2 ROS（Gazebo仿真）

系统要求：ubuntu + ros +gazebo
1

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
1
2
3

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14

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

  // 当前点和目标点的距离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 < 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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215

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

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

Pure_pursuit仿真结果：

2.2.4 LQR横向控制算法