# Steering Control Simple

Steering Control of Autonomous Vehicles in Obstacle Avoidance Maneuvers.

## Vehicle model

Nonlinear model

State vector

${\bf x} = \left[ \begin{array}{c} {\rm x}_1 \\ {\rm x}_2 \\ {\rm x}_3 \\ {\rm x}_4 \\ {\rm x}_5 \\ {\rm x}_6 \end{array} \right] = \left[ \begin{array}{c} x \\ y \\ \psi \\ v_{\rm T} \\ \alpha_{\rm T} \\ \dot{\psi} \end{array} \right]$

State equations

$\dot{{\rm x}}_1 = {\rm x}_4 \cos \left( {\rm x}_3 + {\rm x}_5 \right)$

$\dot{{\rm x}}_2 = {\rm x}_4 \sin \left( {\rm x}_3 + {\rm x}_5 \right)$

$\dot{{\rm x}}_3 = {\rm x}_6$

$\dot{{\rm x}}_4 = \frac{F_{y,{\rm F}} \sin \left( {\rm x}_5 - \delta \right) + F_{y,{\rm R}} \sin {\rm x}_5}{m_{T}}$

$\dot{{\rm x}}_5 = \frac{F_{y,{\rm F}} \cos \left( {\rm x}_5 - \delta \right) + F_{y,{\rm R}} \cos \alpha_{\rm T} - m_{T} {\rm x}_4 {\rm x}_6}{m_{T} {\rm x}_4}$

$\dot{{\rm x}}_6 = \frac{F_{y,{\rm F}} a \cos \delta - F_{y,{\rm R}} b}{I_{T}}$

Slip angles

$\alpha_{\rm F} = \arctan \left( \frac{v_{\rm T} \sin \alpha_{\rm T} + a \dot{\psi}}{ v_{\rm T} \cos \alpha_{\rm T}} \right) - \delta$

$\alpha_{\rm R} = \arctan \left( \frac{v_{\rm T} \sin \alpha_{\rm T} - b \dot{\psi}}{ v_{\rm T} \cos \alpha_{\rm T}} \right)$

Linear model

$\dot{x} = v_{\rm T}$

$\dot{y} = v_{{\rm T},0} \left( \psi + \alpha_{{\rm T}}\right)$

$\dot{\psi} = \dot{\psi}$

$\dot{v}_{\rm T} = 0$

$\dot{\alpha}_{\rm T} = \frac{F_{y,{\rm F}} + F_{y,{\rm R}}}{m_{T} v_{{\rm T},0}} - \dot{\psi}$

$\ddot{\psi} = \frac{a F_{y,{\rm F}} - b F_{y,{\rm R}}}{I_{T}}$

Neglecting equations of $$x$$ and $$v_T$$

$\left[ \begin{array}{c} \dot{y} \\ \dot{\psi} \\ \dot{\alpha}_T \\ \ddot{\psi} \end{array} \right] = \left[ \begin{array}{cccc} 0 & v_{T,0} & v_{T,0} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & -\frac{K_F+K_R}{m_T v_{T,0}} & - \frac{m_T v_{T,0} + \frac{a K_F - b K_R}{v_{T,0}}}{m_T v_{T,0}} \\ 0 & 0 & - \frac{a K_F - b K_R}{I_T} & - \frac{a^2 K_F + b^2 K_R}{I_T v_{T,0}} \end{array} \right] \left[ \begin{array}{c} y \\ \psi \\ \alpha_T \\ \dot{\psi} \end{array} \right] + \left[ \begin{array}{c} 0 \\ 0 \\ \frac{K_F}{m_T v_{T,0}} \\ \frac{a K_F}{I_T} \end{array} \right] \delta$

Slip angles

$\alpha_{{\rm F},lin} = \alpha_{{\rm T}} + \frac{a}{v_{{\rm T},0}} \dot{\psi} - \delta$

$\alpha_{{\rm F},lin} = \alpha_{{\rm T}} - \frac{b}{v_{{\rm T},0}} \dot{\psi}$

## Tire model

Typical characteristic curve and slip angle definition

Pacejka

$F_{y} = D \sin \left[ C \arctan{B \alpha - E( B \alpha -\arctan(B \alpha))} \right]$

Linear

$F_ y = K \alpha$

clear ; close all ; clc

slipAngle = (0:0.1:15)*pi/180;         % Slip angle [rad]

% Pacejka tire parameters
a0  = 1;
a1  = 0;
a2  = 800;
a3  = 10000;
a4  = 50;
a5  = 0;
a6  = 0;
a7  = -1;
a8  = 0;
a9  = 0;
a10 = 0;
a11 = 0;
a12 = 0;
a13 = 0;

TirePac = VehicleDynamicsLateral.TirePacejka();

Fz          = 4e+03;
camber      = 0;
TirePac.a0  = a0;
TirePac.a1  = a1;
TirePac.a2  = a2;
TirePac.a3  = a3;
TirePac.a4  = a4;
TirePac.a5  = a5;
TirePac.a6  = a6;
TirePac.a7  = a7;
TirePac.a8  = a8;
TirePac.a9  = a9;
TirePac.a10 = a10;
TirePac.a11 = a11;
TirePac.a12 = a12;
TirePac.a13 = a13;

muy0    = TirePac.a1 * Fz/1000 + TirePac.a2;
D       = muy0 * Fz/1000;
BCD     = TirePac.a3 * sin(2 * atan(Fz/1000/TirePac.a4))*(1-TirePac.a5 * abs(camber));

% Pneu linear equivalente

Ktire   = BCD * 180/pi;

TireLin     = VehicleDynamicsLateral.TireLinear();
TireLin.k   = Ktire;

% Lateral force
FyPac = TirePac.Characteristic(slipAngle, Fz, muy0/1000);
FyLin = TireLin.Characteristic(slipAngle);

% Graphics
g = VehicleDynamicsLateral.Graphics(TirePac);


Comparison of tire models

figure(1)
ax = gca;
set(ax, 'NextPlot', 'add', 'Box', 'on', 'XGrid', 'on', 'YGrid', 'on')
p = plot(slipAngle * 180/pi,-FyLin, 'Color', 'g', 'Marker', 's', 'MarkerFaceColor', 'g', 'MarkeredgeColor', 'k', 'MarkerSize', 7);
g.changeMarker(p, 10);
p = plot(slipAngle * 180/pi,-FyPac, 'Color', 'r', 'Marker', 'o', 'MarkerFaceColor', 'r', 'MarkeredgeColor', 'k', 'MarkerSize', 7);
g.changeMarker(p, 10);
xlabel('$$\alpha$$ [grau]', 'Interpreter', 'Latex')
ylabel('$$F_y$$ [N]', 'Interpreter', 'Latex')
l = legend('Linear', 'Pacejka');
set(l, 'Interpreter', 'Latex', 'Location', 'NorthWest')

%


## Plant model

Nonlinear vehicle + Pacejka tire

% Choosing vehicle
% System = VehicleDynamicsLateral.VehicleSimpleLinear();
VehiclePlant = VehicleDynamicsLateral.VehicleSimpleNonlinear();
% Defining vehicle parameters
VehiclePlant.mF0 = 700;
VehiclePlant.mR0 = 600;
VehiclePlant.IT = 10000;
VehiclePlant.lT = 3.5;
VehiclePlant.nF = 1;
VehiclePlant.nR = 1;
VehiclePlant.wT = 1.8;
VehiclePlant.muy = 1;
VehiclePlant.tire = TirePac;
VehiclePlant.deltaf = @ControlLaw;

disp(VehiclePlant)

% Choosing simulation
T       = 9;                     % Total simulation time             [s]
resol   = 500;                    % Resolution
TSPAN   = 0:T/resol:T;            % Time span                         [s]

simulator = VehicleDynamicsLateral.Simulator(VehiclePlant, TSPAN);
simulator.V0 = 16.7;

  VehicleSimpleNonlinear with properties:

mT: 1300
IT: 10000
a: 1.6154
b: 1.8846
mF0: 700
mR0: 600
lT: 3.5000
nF: 1
nR: 1
wT: 1.8000
muy: 1
tire: [1×1 VehicleDynamicsLateral.TirePacejka]
deltaf: @ControlLaw
deltar: 0
Fxf: 0
Fxr: 0



## Controller design

Vehicle parameters

mT  = 1300;
IT  = 10000;
a   = 1.6154;
b   = 1.8846;
vT0 = 16.7;
KF  = Ktire;
KR  = Ktire;


Linear system

A = [      0   vT0            vT0                         0                       ;...
0    0              0                          1                       ;...
0    0      -(KF+KR)/(mT*vT0)  -(mT*vT0+(a*KF-b*KR)/(vT0))/(mT*vT0)    ;...
0    0      -(a*KF-b*KR)/IT    -(a^2*KF+b^2*KR)/(IT*vT0)               ];

B = [   0                  ;...
0                  ;...
KF/(mT*vT0)        ;...
a*KF/IT            ];

C = [1 0 0 0];


A

disp(A)

         0   16.7000   16.7000         0
0         0         0    1.0000
0         0   -8.3915   -0.9324
0         0    2.4522   -3.3606



B

disp(B)

         0
0
4.1958
14.7147



C

disp(C)

     1     0     0     0



LQR design

Q = [   0.3 0 0 0 ;...
0   1 0 0 ;...
0   0 1 0 ;...
0   0 0 1 ];


Q

disp(Q)

R = 1;

    0.3000         0         0         0
0    1.0000         0         0
0         0    1.0000         0
0         0         0    1.0000



R

disp(R)

Klqr = lqr(A,B,Q,R);

     1



Klqr

disp(Klqr)

    0.5477    4.4651    1.0744    0.8169



Pole placement design

polos = [-6 -6.3 -6.7 -7];

Kplace = place(A,B,polos);


Kplace

disp(Kplace)

    0.7936    6.6882    1.6107    0.5090



Control law

$\delta = - {\bf K} {\bf z} + K_1 r$

## Double Lane Change Maneuver

% Simulation
simulator.Simulate();

g = VehicleDynamicsLateral.Graphics(simulator);
g.Frame();

% Adding the double lane change track to the frame figure
carWidth = 2;
LaneOffset = 3.5;

section1width = 1.1*carWidth + 0.25;
section3width = 1.2*carWidth + 0.25;
section5width = 1.3*carWidth + 0.25;

section1Inf = -section1width/2;
section1Sup = section1width/2;

section3Inf = section1Inf+LaneOffset;
section3Sup = section3Inf+section3width;
section3Center = (section3Inf+section3Sup)/2;

section5Inf = -section5width/2;
section5Sup = section5width/2;

% Section 1
plot([0 15],[section1Inf section1Inf],'k')            % linha inferior
plot([0 15],[section1Sup section1Sup],'k')            % linha superior
plot([0 15],[0 0],'k--')                % linha central
% Section 2
plot([15 45],[0 section3Center],'k--')  % linha central
% Section 3
plot([45 70],[section3Inf section3Inf],'k')        % linha inferior
plot([45 70],[section3Sup section3Sup],'k')        % linha superior
plot([45 70],[section3Center section3Center],'k--')               % linha central
% Section 4
plot([70 95],[section3Center 0],'k--')
% Section 5
plot([95 130],[section5Inf section5Inf],'k')
plot([95 130],[section5Sup section5Sup],'k')
plot([95 130],[0 0],'k--')

g.Animation();
g.Animation();
% g.Animation('html/SteeringControlSimple');       % Uncomment to save animation gif

% Retrieving states
XT      = simulator.XT;
YT      = simulator.YT;
PSI     = simulator.PSI;
VEL     = simulator.VEL;
ALPHAT  = simulator.ALPHAT;
dPSI    = simulator.dPSI;

x = [YT PSI ALPHAT dPSI];

u = zeros(length(TSPAN),1);
output = zeros(length(TSPAN),1);

LateralDisp = 3.6;

for ii = 1:length(TSPAN)
if XT(ii) <= 15
r = 0;
end
if XT(ii) > 15 && XT(ii) <= 70
r = LateralDisp;
end
if XT(ii) > 70
r = 0;
end

u(ii) = - Kplace*x(ii,:)' + Kplace(1)*r;

% Saturation
if abs(u(ii)) < 42*pi/180
output(ii) = u(ii);
else
output(ii) = sign(u(ii))*42*pi/180;
end
end

% States
f = figure;
set(f,'PaperUnits','centimeters')
set(f,'PaperPosition',[0 0 8.9 5])
PaperPos = get(f,'PaperPosition');
set(f,'PaperSize',PaperPos(3:4))
hold on; box on; grid on
plot(TSPAN,YT,'r')
plot(TSPAN,PSI,'g')
plot(TSPAN,ALPHAT,'b')
plot(TSPAN,dPSI,'c')
xlabel('Time [s]')
ylabel('States')
l = legend('$$y$$','$$\psi$$','$$\alpha_T$$','$$\dot{\psi}$$');
set(l,'Interpreter','Latex','Location','NorthEast')

% Steering input
f = figure;
set(f,'PaperUnits','centimeters')
set(f,'PaperPosition',[0 0 8.9 3.5])
PaperPos = get(f,'PaperPosition');
set(f,'PaperSize',PaperPos(3:4))
hold on; box on; grid on
plot(TSPAN,output*180/pi,'k')
xlabel('Time [s]')
y = ylabel('$$\delta [deg]$$');
set(y,'Interpreter','Latex')