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

190 阅读 0 评论 126 点赞

我是靠谱客的博主忐忑航空，这篇文章主要介绍自动驾驶规划控制（Ａ*、pure pursuit、LQR算法，使用c++在ubuntu和ros环境下实现）1 目录概述2 算法介绍，现在分享给大家，希望可以做个参考。

文章目录

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改进

复制代码

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

实现思路：

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

算法流程：

见github

2.2 ROS（Gazebo仿真）

复制代码

1
2
系统要求：ubuntu + ros +gazebo

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

复制代码

1
2
3
4
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
2
3
4
5
6
7
8
9
10
11
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 <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 ¤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<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
/**
* @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
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 <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_d
double 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;
// 计算投影点曲率k
double 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,%.3fn", 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<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;
};

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
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
/**
* @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_d
double 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;
// 计算投影点曲率k
double 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,%.3fn", 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;
};

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

复制代码

1
2
3
4
5
6
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