本文为深蓝学院-自动驾驶控制与规划-第三章作业
目录
3.3 Steer into constant radius curve
1 projection introduction
本项目希望大家根据Stanley算法实现车辆的横向控制，结合上一章的PID算法实现轨迹跟踪。基本的系统框架已经给出，仅需要完成stanley_control.cpp中todo部分。
给定的系统框架如下
-
#include "stanley_control.h"
-
-
#include <algorithm>
-
#include <iomanip>
-
#include <utility>
-
#include <vector>
-
#include "Eigen/LU"
-
#include <math.h>
-
-
using
namespace std;
-
-
namespace shenlan {
-
namespace control {
-
-
double atan2_to_PI(const double atan2) {
-
return atan2 * M_PI /
180;
-
}
-
-
double PointDistanceSquare(const TrajectoryPoint &point, const double x,
-
const
double y) {
-
const
double dx = point.x - x;
-
const
double dy = point.y - y;
-
return dx * dx + dy * dy;
-
}
-
-
void StanleyController::LoadControlConf() {
-
k_y_ =
0.5;
-
}
-
-
// /** to-do **/ 计算需要的控制命令, 实现对应的stanley模型,并将获得的控制命令传递给汽车
-
// 提示，在该函数中你需要调用计算误差
-
void StanleyController::ComputeControlCmd(
-
const VehicleState &vehicle_state,
-
const TrajectoryData &planning_published_trajectory, ControlCmd &cmd) {
-
-
}
-
-
// /** to-do **/ 计算需要的误差，包括横向误差，纵向误差
-
-
void StanleyController::ComputeLateralErrors(const double x, const double y,
-
const
double theta,
double &e_y,
-
double &e_theta) {
-
-
}
-
-
TrajectoryPoint StanleyController::QueryNearestPointByPosition(const double x,
-
const
double y) {
-
double d_min =
PointDistanceSquare(trajectory_points_.
front(), x, y);
-
size_t index_min =
0;
-
-
for (
size_t i =
1; i < trajectory_points_.
size(); ++i) {
-
double d_temp =
PointDistanceSquare(trajectory_points_[i], x, y);
-
if (d_temp < d_min) {
-
d_min = d_temp;
-
index_min = i;
-
}
-
}
-
// cout << " index_min: " << index_min << endl;
-
//cout << "tarjectory.heading: " << trajectory_points_[index_min].heading << endl;
-
theta_ref_ = trajectory_points_[index_min].heading;
-
-
return trajectory_points_[index_min];
-
}
-
-
}
// namespace control
-
}
// namespace shenlan
2 思路提示
从掌握stanley算法到用C++实现还需要思考不少问题
2.1 ComputeControlCmd
ComputeControlCmd是计算控制命令的函数，因此调用计算误差的函数，得到航向误差与横向误差，通过如下公式即可计算出车辆前轮转角
但是这里还需要考虑车辆转向角有范围，当超过范围则需要取边值
-
//计算需要的控制命令, 实现对应的stanley模型,并将获得的控制命令传递给汽车
-
void StanleyController::ComputeControlCmd(
-
const VehicleState &vehicle_state,
-
const TrajectoryData &planning_published_trajectory, ControlCmd &cmd) {
-
trajectory_points_=planning_published_trajectory.trajectory_points;
-
double e_y=
0.0;
-
double e_theta=
0.0;
-
cout<<
"vehicle_state.heading:" <<vehicle_state.heading <<endl;
-
ComputeLateralErrors(vehicle_state.x,vehicle_state.y,vehicle_state.heading,e_y,e_theta);
-
cout<<
"e_y:" <<e_y <<endl;
-
cout<<
"e_theta:" <<e_theta <<endl;
-
cmd.steer_target=e_theta+std::
atan2(k_y_*e_y,vehicle_state.velocity);
-
if (cmd.steer_target>M_PI/
4.0){
-
cmd.steer_target=M_PI/
4.0;
-
}
-
else
if(cmd.steer_target<-M_PI/
4.0){
-
cmd.steer_target=-M_PI/
4.0;
-
}
-
}
我还把 vehicle_state.heading、e_y、e_theta 输出，便于观察误差是否能快速收敛到0并且稳态误差较小，同时设定最大转角为45度（根据实际车辆可以调整该参数）
2.2 ComputeLateralErrors
这一部分需要计算航向误差与横向误差
航向误差：离车辆前轮中心最近点的切线与车辆纵轴之间的夹角，但是我们需要小心此处的正负
横向误差：离车辆前轮中心最近点与车辆前轮中心之间的距离，同时这里也会涉及到正负
在理解算法时可能没有关注到上述两个量的正负，但是用代码实现时，这是非常重要的问题
横向误差：因为当一辆车在参考轨迹的右侧和左侧，转向角必须做出相反的反应，这里通过向量叉乘判断车在参考轨迹的哪一侧
航向误差：会涉及到的问题就是需要把两角之差限制到-pi~pi，因为原始角度会出现差2*k*pi是代表一样的角度，导致把多出来的2*k*pi附加到转向角中
-
//计算需要的误差，包括横向误差，纵向误差
-
void StanleyController::ComputeLateralErrors(const double x, const double y,
-
const
double theta,
double &e_y,
-
double &e_theta) {
-
TrajectoryPoint target_point=
QueryNearestPointByPosition(x,y);
-
-
e_y=
sqrt((target_point.x-x)*(target_point.x-x)+(target_point.y-y)*(target_point.y-y));
-
double judge=(target_point.y-y)*
cos(target_point.heading)-(target_point.x-x)*
sin(target_point.heading);
-
if (judge>
0){
-
e_y=-e_y;
-
}
-
e_theta=theta-target_point.heading;
-
if(e_theta>M_PI){
-
e_theta-=
2.0*M_PI;
-
}
-
else
if(e_theta<-M_PI){
-
e_theta+=
2.0*M_PI;
-
}
-
return;
-
}
3 Corer Case
3.1 Low speed operation
不适用于停车场景，当倒车时，相反的速度会导致横向误差变化大，使得转向角不够连续变化，不稳定，同时当速度中噪声占比较大，也会对此产生较大影响
因此常常在分母加上一个 soft 项，让分母恒正同时受噪声干扰小
3.2 Extra damping on heading
为了使系统更稳定，可以考虑给航向误差加上一个PD控制
3.3 Steer into constant radius curve
当轨迹追踪一个曲率较大的曲线时，往往需要增加前馈项去增加轨迹跟踪的效果
4 ROS+LGSVL联合仿真
这里是联合仿真的链接
自动驾驶stanley控制（ROS+LGSVL联合仿真）_哔哩哔哩_bilibili
5 深蓝学院-自动驾驶控制与规划-第三章-完整代码
链接自取
百度网盘链接：https://pan.baidu.com/s/1fZEUzXlCGs4bY1oF9Mlp_w?pwd=wotf
提取码：wotf
深蓝学院-自动驾驶控制与规划-第三章作业-完整代码-深度学习文档类资源-CSDN文库
转载：https://blog.csdn.net/weixin_65089713/article/details/128732810