ADORe
ADORe is a modular open source software library and toolkit for decision making, planning, control and simulation of automated vehicles
linear_tracking_controller.h
Go to the documentation of this file.
1 /********************************************************************************
2  * Copyright (C) 2017-2020 German Aerospace Center (DLR).
3  * Eclipse ADORe, Automated Driving Open Research https://eclipse.org/adore
4  *
5  * This program and the accompanying materials are made available under the
6  * terms of the Eclipse Public License 2.0 which is available at
7  * http://www.eclipse.org/legal/epl-2.0.
8  *
9  * SPDX-License-Identifier: EPL-2.0
10  *
11  * Contributors:
12  * Daniel Heß - initial API and implementation
13  ********************************************************************************/
14 
15 #pragma once
16 
17 #include <adore/mad/adoremath.h>
20 #include <algorithm>
21 #include <iostream>
25 
26 
27 namespace adore
28 {
29  namespace fun
30  {
39  {
40  private:
43  double m_ex;
44  double m_ey;
45  double m_Ix;
46  double m_Iy;
49  public:
56  :m_pVehParameters(pVehicle),m_pCtrlParameters(pControl)
57  {
58  m_ex=0.0;m_ey=0.0;
59  m_Ix=0.0;m_Iy=0.0;
60  m_use_integrator=false;
61  m_reset_integrator=false;
62  }
64  double get_ex(){return m_ex;}
66  double get_ey(){return m_ey;}
67 
68  void setUseIntegrator(bool value){m_use_integrator=value;}
69 
70  void resetIntegrator(bool value){m_reset_integrator=value;}
71 
79  {
80  // read the required parameters
81  const double k0x = m_pCtrlParameters->getK0x();
82  const double k1x = m_pCtrlParameters->getK1x();
83  const double key = m_pCtrlParameters->getKey();
84  const double kepsi = m_pCtrlParameters->getKepsi();
85  const double keomega = m_pCtrlParameters->getKeomega();
86  const double kIx = m_pCtrlParameters->getKIx();
87  const double kIy = m_pCtrlParameters->getKIy();
88 
89  const double ex_static = m_pCtrlParameters->getExStatic();
90  const double ey_static = m_pCtrlParameters->getEyStatic();
91 
92  const double b = m_pVehParameters->get_b();//rear axle to cog
93 
94  // extract variables
95  const double psi = x.getPSI();
96  const double X = x.getX()+std::cos(psi)*(b-m_pVehParameters->get_observationPointForPosition()); //position of cog
97  const double Y = x.getY()+std::sin(psi)*(b-m_pVehParameters->get_observationPointForPosition()); //position of cog
98  const double vx = x.getvx();
99  const double omega = x.getOmega();
100 
101  const double psis = xref.getPSI();
102  const double Xs = xref.getX()+std::cos(psis)*b; //set position of cog
103  const double Ys = xref.getY()+std::sin(psis)*b; //set position of cog
104  const double vxs = xref.getvx();
105  const double omegas = xref.getOmega();
106  const double axs = xref.getAx();
107  const double deltas = xref.getDelta();
108 
109  const double eomega = omega - omegas;
110  const double ev = vx-vxs;
111  /* angle transformation to prevent angle modulo errors*/
112  const double cpsi = (std::cos)(psi);
113  const double spsi = (std::sin)(psi);
114  const double cpsis = (std::cos)(psis);
115  const double spsis = (std::sin)(psis);
116  //transformation (negative rotation) into reference coordinates
117  const double tcpsi = cpsis * cpsi + spsis * spsi;
118  const double tspsi = -spsis * cpsi + cpsis * spsi;
119  const double epsi = (std::atan2)(tspsi,tcpsi);
120 
121 
122 
123  if( vxs<0.0 )
124  {
125  u.setAcceleration(-2.0);
126  u.setSteeringAngle( 0.0 );
127  }
128  else
129  {
130  //compute error terms
131  m_ex = std::cos(psis)*(X-Xs)+std::sin(psis)*(Y-Ys) + ex_static;
132  m_ey =-std::sin(psis)*(X-Xs)+std::cos(psis)*(Y-Ys) + ey_static;
133  if(m_use_integrator)
134  {
135  m_Ix += m_ex*0.01;
136  m_Iy += m_ey*0.01;
137  }
138 
139  if(vx<0.05 || m_reset_integrator)
140  {
141  m_Ix = 0.0;
142  m_Iy = 0.0;
143  }
144 
145  //ouput resulting control:
146  double a_out = axs-kIx*m_Ix-k0x*m_ex-k1x*ev;
147  double delta_out = deltas -kIy*m_Iy-key*m_ey -kepsi*epsi -keomega*eomega;
150  }
151  }
152  };
153  }
154 }
Definition: linear_tracking_controller.h:39
LinearTrackingController(adore::params::APVehicle *pVehicle, adore::params::APTrajectoryTracking *pControl)
Definition: linear_tracking_controller.h:55
double m_Ix
lateral tracking error
Definition: linear_tracking_controller.h:45
double m_ex
Definition: linear_tracking_controller.h:43
double get_ex()
returns the longitudinal tracking error
Definition: linear_tracking_controller.h:64
adore::params::APTrajectoryTracking * m_pCtrlParameters
Definition: linear_tracking_controller.h:42
double m_ey
longitudinal tracking error
Definition: linear_tracking_controller.h:44
double get_ey()
returns the lateral tracking error
Definition: linear_tracking_controller.h:66
void compute_control_input(const VehicleMotionState9d &x, const PlanarVehicleState10d &xref, MotionCommand &u)
Definition: linear_tracking_controller.h:78
bool m_use_integrator
integrator in lateral direction
Definition: linear_tracking_controller.h:47
bool m_reset_integrator
Definition: linear_tracking_controller.h:48
adore::params::APVehicle * m_pVehParameters
Definition: linear_tracking_controller.h:41
void setUseIntegrator(bool value)
Definition: linear_tracking_controller.h:68
double m_Iy
integrator in longitudinal direction
Definition: linear_tracking_controller.h:46
void resetIntegrator(bool value)
Definition: linear_tracking_controller.h:70
Definition: motioncommand.h:26
void setAcceleration(double acceleration)
Definition: motioncommand.h:51
void setSteeringAngle(double steeringAngle)
Definition: motioncommand.h:41
abstract class containing parameters to configure the behaviour of the trajactory tracking controller
Definition: ap_trajectory_tracking.h:26
virtual double getDeltaMin() const =0
the minimum controllable steering angle
virtual double getEyStatic() const =0
static trajectory tracking offset in lateral direction, which should be compensated by tracking contr...
virtual double getKepsi() const =0
lateral control gain for yaw angle error epsi
virtual double getAxMax() const =0
hard coded maximum longitudinal acceleration
virtual double getK0x() const =0
returns P control gain for longitudinal direction
virtual double getDeltaMax() const =0
the maximum controllable steering angle
virtual double getK1x() const =0
returns D control gain for longitudinal direction
virtual double getKey() const =0
lateral control gain for lateral error ey
virtual double getAxMin() const =0
hard coded minimum longitudinal acceleration
virtual double getKIx() const =0
returns I control gain for longitudinal direction
virtual double getKeomega() const =0
lateral control gain for yaw rate error eomega
virtual double getExStatic() const =0
static trajectory tracking offset in longitudinal direction, which should be compensated by tracking ...
virtual double getKIy() const =0
returns I control gain for lateral direction
abstract class for vehicle configuration related paremeters
Definition: ap_vehicle.h:29
virtual double get_b() const =0
rear axle to cog
virtual double get_observationPointForPosition() const =0
interval< T > atan2(interval< T > y, interval< T > x)
Definition: intervalarithmetic.h:234
interval< T > cos(interval< T > x)
Definition: intervalarithmetic.h:225
T bound(T lb, T value, T ub)
Definition: adoremath.h:569
interval< T > sin(interval< T > x)
Definition: intervalarithmetic.h:204
x
Definition: adore_set_goal.py:30
Definition: areaofeffectconverter.h:20
This struct holds the motion state of the vehicle in 9d.
Definition: vehiclemotionstate9d.h:39
Definition: planarvehiclestate10d.h:41
double getvx() const
Definition: planarvehiclestate10d.h:65
double getX() const
Definition: planarvehiclestate10d.h:62
double getDelta() const
Definition: planarvehiclestate10d.h:69
double getPSI() const
Definition: planarvehiclestate10d.h:64
double getOmega() const
Definition: planarvehiclestate10d.h:67
double getY() const
Definition: planarvehiclestate10d.h:63
double getAx() const
Definition: planarvehiclestate10d.h:68