自动驾驶控制算法之车辆横向控制(project)

本文为深蓝学院-自动驾驶控制与规划-第三章作业

目录

1 projection introduction

2 思路提示

2.1 ComputeControlCmd

2.2 ComputeLateralErrors

3 Corer Case

3.1 Low speed operation

3.2 Extra damping on heading

3.3 Steer into constant radius curve

4 ROS+LGSVL联合仿真

5 深蓝学院-自动驾驶控制与规划-第三章-完整代码


1 projection introduction

本项目希望大家根据Stanley算法实现车辆的横向控制,结合上一章的PID算法实现轨迹跟踪。基本的系统框架已经给出,仅需要完成stanley_control.cpp中todo部分。

给定的系统框架如下

#include "stanley_control.h"

#include 
#include 
#include 
#include 
#include "Eigen/LU"
#include 

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:" <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

这一部分需要计算航向误差与横向误差

航向误差:离车辆前轮中心最近点的切线与车辆纵轴之间的夹角,但是我们需要小心此处的正负

横向误差:离车辆前轮中心最近点与车辆前轮中心之间的距离,同时这里也会涉及到正负

自动驾驶控制算法之车辆横向控制(project)_第1张图片

在理解算法时可能没有关注到上述两个量的正负,但是用代码实现时,这是非常重要的问题

横向误差:因为当一辆车在参考轨迹的右侧和左侧,转向角必须做出相反的反应,这里通过向量叉乘判断车在参考轨迹的哪一侧

航向误差:会涉及到的问题就是需要把两角之差限制到-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联合仿真

自动驾驶控制算法之车辆横向控制(project)_第2张图片

这里是联合仿真的链接

自动驾驶stanley控制(ROS+LGSVL联合仿真)_哔哩哔哩_bilibili

5 深蓝学院-自动驾驶控制与规划-第三章-完整代码

链接自取

百度网盘链接:https://pan.baidu.com/s/1fZEUzXlCGs4bY1oF9Mlp_w?pwd=wotf 
提取码:wotf 

深蓝学院-自动驾驶控制与规划-第三章作业-完整代码-深度学习文档类资源-CSDN文库

你可能感兴趣的:(自动驾驶控制算法,自动驾驶,人工智能,c++)