回答

收藏

自动驾驶控制算法之车辆横向控制(project)(自动驾驶控制算法工程师招聘)

知识点 知识点 32 人阅读 | 0 人回复 | 2023-01-19

帖子摘要:本文为深蓝学院-自动驾驶控制与规划-第三章作业 目录 1 projection introduction 2 思路提示 2.1 ComputeControlCmd 2.2 ComputeLateral......
. z2 C8 m, n6 q8 G' o
5 x* V6 f! B* M, Z# b' E- P大家好,欢迎来到Java吧(www.java8.com),交流、学习Java技术、获取Java资源无任何套路,今天说一说:“自动驾驶控制算法之车辆横向控制(project)”& R9 j. y9 V; a  G) g

- G( }7 x& T8 C
- ]) W& ?$ c/ i* p2 D7 _        0 s( y, D5 B6 o( M5 v: u" o
                ; a% i) x* b* T( Y0 L
                    本文为深蓝学院-自动驾驶控制与规划-第三章作业
8 {5 Y  V7 B4 x$ c6 @ : j6 u5 o9 g0 W/ u) l
目录9 [! G0 w$ j! T% G

+ d( Y9 m( u& F7 y! ]& v: O1 projection introduction
9 I& V. P5 G  {8 N) O" {
) C( t& B; Y9 ]- D" W6 Q2 思路提示# S3 W0 J% ^' P# o* u* g3 F

) g8 B$ i, L1 I% Y0 s- i& S2.1 ComputeControlCmd
& R6 o6 C6 @9 V8 i. Z , K1 @* c: @% @9 B
2.2 ComputeLateralErrors
, o. ^' u6 Q: u  \' F
. c( ?! I5 U$ z( Z) g" Z6 C3 Corer Case9 G& o+ z! O1 _$ u. g0 u1 W
3 N* ]+ Q1 K9 U8 ]$ \- m+ a1 v; d
3.1 Low speed operation5 i, L4 V3 ?2 q" K- P( |$ f

$ ?/ H- k* _  K/ V( K5 {- X2 L& S! y3.2 Extra damping on heading' V7 ^0 N9 q  D. v3 Q" h9 f

# {% Y( r& I2 \) ]$ ^" J, p7 F3.3 Steer into constant radius curve! ~7 ~0 \  U; ~0 m2 W  P
& p# C5 q% S4 e% d0 N: J
4 ROS+LGSVL联合仿真
7 b" d& O+ e+ L3 P- a
; m" S4 I) s! ?4 U( g+ x& q, Q( Y5 深蓝学院-自动驾驶控制与规划-第三章-完整代码: N- N8 q4 m8 e1 O; y
% w  O) i, ]/ D0 B

5 y, G4 j  c) m5 {- }; \$ e1 projection introduction ' Z* `9 A3 {* M% n8 ?7 ~6 Q& n" ^
本项目希望大家根据Stanley算法实现车辆的横向控制结合上一章的PID算法实现轨迹跟踪。基本的系统框架已经给出仅需要完成stanley_control.cpp中todo部分。
. `/ k, j( `5 N9 E* B  C4 b
/ V2 \- b0 w1 B2 O3 }$ E给定的系统框架如下( W! M/ A. V$ m7 ]- O- W

* j/ F0 }) b9 C& U0 I- K[code]#include "stanley_control.h"
: \& [9 l  r; \) T#include   ?' M1 c; b0 y
#include ; }3 p0 U. n3 X' @9 ~/ v
#include 7 P4 o# i3 L. h. s: i! P+ h
#include
% z- O( ^& ~: n7 q* C5 `#include "Eigen/LU"
# ~8 H- L: P  o8 d/ M9 K" G, i+ J#include
6 O2 U! p! r$ R- N; Zusing namespace std;6 `, Z6 Q7 r  n' G
namespace shenlan {7 ]6 W7 m, q3 ^: @" Z9 F+ z
namespace control {
  [! h1 `- Q! n4 T# u* t$ Rdouble atan2_to_PI(const double atan2) {
( l- d* H$ r& Z) Y' _  return atan2 * M_PI / 180;
! D! D3 u/ Z) ~8 m}: M- ~+ F+ [3 A& z
double PointDistanceSquare(const TrajectoryPoint &point, const double x,: M$ r8 d' P* n9 [. m
                           const double y) {/ Y$ _& |0 w4 i
  const double dx = point.x - x;- m3 N8 E3 M' i( \  U# O
  const double dy = point.y - y;
" j% G1 h8 h5 h( j- @  return dx * dx + dy * dy;1 `) Z, k9 t5 _, `# m' r8 G3 D/ l
}+ j/ _* P* K" b6 `
void StanleyController::LoadControlConf() {4 W( M4 w7 T& ^4 K4 J  r$ m2 a
  k_y_ = 0.5;
8 q% c& F  ]5 j6 M  H5 t}
# S4 M( `# s4 G( E6 X// /** to-do **/ 计算需要的控制命令, 实现对应的stanley模型,并将获得的控制命令传递给汽车; X6 j; m( Y, M) W
// 提示在该函数中你需要调用计算误差
: Z# q2 ]- z9 K' }, dvoid StanleyController::ComputeControlCmd(
" Z8 ?4 X  {0 i% ^/ s- u    const VehicleState &vehicle_state,
+ `, Q2 V: k; f5 p2 I9 D0 i' O0 ~    const TrajectoryData &planning_published_trajectory, ControlCmd &cmd) {
' O+ f4 N5 P! N$ f  ! s. Y+ A4 o, S9 L" {. E
}
  B" O6 r0 S# j  V* X// /** to-do **/ 计算需要的误差包括横向误差纵向误差' M1 g+ n  M+ t( v* M) `
void StanleyController::ComputeLateralErrors(const double x, const double y,
+ y+ G! W' I& w& W- |6 O                                             const double theta, double &e_y,
3 h% t0 O* w2 O" v. y% t+ j                                             double &e_theta) {* z& E  v% U2 H# R1 a! H& b. ~
  5 q  V0 T, o8 s) v' U1 A
}
$ r! h$ T$ k. |TrajectoryPoint StanleyController::QueryNearestPointByPosition(const double x,1 {+ M; S8 P4 ?) Z# L
                                                               const double y) {
  F2 F3 e9 O7 Y2 h7 G+ G  double d_min = PointDistanceSquare(trajectory_points_.front(), x, y);% o9 m. C& l6 \) a6 V) u
  size_t index_min = 0;3 D0 j7 C/ I( w3 J8 m- N
  for (size_t i = 1; i  : g( P% P" _7 z4 k- ?! F  r8 V
2 思路提示
- o9 r$ K3 b5 A% Y: T* h( c从掌握stanley算法到用C++实现还需要思考不少问题
; p" A& C* \/ n! Q) K. Z- t
  d' ?$ C% |& R& R6 H7 c& F% v2.1 ComputeControlCmd ' @, z7 H2 i  R- \
ComputeControlCmd是计算控制命令的函数因此调用计算误差的函数得到航向误差与横向误差通过如下公式即可计算出车辆前轮转角6 B3 s/ I9 p5 r- i* o0 Z& L

7 _1 K2 \2 `7 M
178f867b05a84a199045781cf8890180.png (15.93 KB, 下载次数: 0, 售价: 3 金钱)
分享到:
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则