Control del Movimiento
Introducción
• En esta lección analizaremos algunas estrategias de control del
movimiento de un robot. Concretamente:
–
–
–
–
Moverse a un punto.
Seguir una línea.
Seguir una trayectoria.
Moverse a una pose específica.
• La lección se basa en el capítulo 4, apartado 4.2 del libro de P. Corke
“Robotics, vision and Control”, 2ª ed.
the rear axle. The configuration of the vehicle is represented by the generalized coordinates q = (x, y, θ ) ∈ C where C ⊂ R2 × S1.
Rudolph
(1764–1834)
waswhich
a German
TheAckermann
dashed lines show
the direction along
the wheelsinventor
cannot move,born
the at Schneeberg, in Saxony. For
of no motion,
and these
intersect
a point known
as the Instantaneous
Center a saddler like his father. For a ti
cial lines
reasons
he was
unable
to atattend
university
and became
of Rotation (ICR). The reference point of the vehicle thus follows a circular path and
worked
as avelocity
saddler
and coach-builder and in 1795 established a print-shop and drawing-s
its angular
is
in London. He published a popular magazine “The Repository of Arts, Literature, Comm
(4.1)
Manufactures, Fashion and Politics” that included an eclectic
mix of articles on water pump
lighting,
and lithographic presses, along with fashion plates and furniture designs. He manufa
and by simple geometry the turning radius is RB = L / tan γ where L is the length of
the vehicle
or wheel base. As
we would
expect the turning
circle increases
with vehicle
paper
for landscape
and
miniature
painters,
patented
a method for waterproofing cloth an
length. The steering angle γ is typically limited mechanically and its maximum value
per dictates
and built
a factory
to produce it. He is buried in Kensal Green Cemetery, Lo
the minimum
value ofin
RBChelsea
.
In 1818 Ackermann took out British patent 4212 on behalf of the German inventor G
Vehicle coordinate system. The coordinate system that we will use, and a common one for vehicles
Lankensperger
for
mechanism
which
ensures
of all sorts is that the
x-axisaissteering
forward (longitudinal
motion), the y-axis
is to the left
side (lateral that the steered wheels move on
motion) which implies that the z-axis is upward. For aerospace and underwater applications the
with az-axis
common
center. The same scheme was proposed and tested by Erasmus Darwin (g
is often downward and the x-axis is forward.
father of Charles) in the 1760s. Subsequent refinement by the Frenchman Charles Jeanta
to the mechanism used in cars to this day which is known as Ackermann steering.
Modelo de Robot (bicicleta)
• Denominaremos pose del robot a su
localización y orientación.
• Supondremos un modelo de robot
Arcs withque
smoothly
como el mostrado,
nosvarying
llevaradius.
a
Dubbins and Reeds-Shepp paths comdefinirla a partir
de un vector de tres
prises constant radius circular arcs and
dimensiones en
coordenadas
straight
line segments. del
mundo:
P = [x, y, θ]T
• Supondremos que el robot se
comanda a partir de dos señales:
– Velocidad lineal (v).
– Ángulo de giro del volante: γ.
For a fixed steering wheel angle the car moves along a circular arc. For this
curves on roads are circular arcs or clothoids! which makes life easier for the
since constant or smoothly varying steering wheel angle allow the car to follow th
Fig. 4.1.
Note that RF > RB which means the front wheel must follow
a longer path and th
Bicycle model of a car. The car
is shown in light grey, and
the
rotate more quickly than the back wheel. When a four-wheeled
vehicle
goes ar
bicycle approximation is dark
The vehicle’s body frame
corner the two steered wheels follow circular paths ofgrey.
different
radii
and theref
is shown
in red, and the
world
coordinate frame in blue. The
angles of the steered wheels γ L and γ R should be very slightly
different.
This is a
steering wheel angle
is γ and
the velocity of the back wheel,
in thewhich
x-direction, results
is v. The two in lower w
by the commonly used Ackermann steering mechanism
wheel axes are extended as
dashed lines and
intersect at on corners w
tear on the tyres. The driven wheels must rotate at different
speeds
the Instantaneous Center of
Rotation (ICR) and the distance
why a differential gearbox is required between the motor
and the driven wheel
from the ICR to the back and
front wheels is R and R respecThe velocity of the robot in the world frame is (v cos
tively θ , v sin θ ) and combin
Eq. 4.1 we write the equations of motion as
B
F
• Con esto, podríamos definir un
modelo cinemático del robot. Las
ecuaciones de movimiento del robot
en coordenadas del mundo son:
From Sharp 1896
This model is referred to as a kinematic model since it describes the velocities of the
but not the forces or torques that cause the velocity. The rate of change of heading Ë is
to as turn rate, heading rate or yaw rate and can be measured by a gyroscope. It can
deduced from the angular velocity of the nondriven wheels on the left- and right-han
Modelo de Robot (bicicleta)
102
Chapter 4 · Mobile Robot Vehicles
Fig. 4.2.
Simulink model sl_lanechange
that results in a lane changing
maneuver. The pulse generator drives the steering angle left
then right. The vehicle has a default wheelbase L = 1
>> out
Simulink.SimulationOutput:
t: [504x1 double]
y: [504x4 double]
from which we can retrieve the simulation time and other variables
Fig. 4.3. Simple lane changing maneuver. a Vehicle response as a
function of time, b motion in the
xy-plane, the vehicle moves in the
positive x-direction
Control del Robot
• Moverse a un Punto:
104
Chapter 4 · Mobile Robot Vehicles
– Nos queremos mover a un punto del plano
P* = [x*, y*]T
– En las diferentes etapas, el robot debería
implementar la siguiente acción de control
(regulador proporcional):
• v* = kv
(x − x*)2 + (y* − y)2
−1 y*
−y
• θ* = tan
x* − x
• γ = Kh(θ* − θ)
which is shown in Fig. 4.5 for a number of starting poses. In each case the vehicle h
moved forward and turned onto a path toward the goal point. The final part of each pa
is a straight line and the final orientation therefore depends on the starting point.
4.1.1.2
l
Following a Line
Another useful task for a mobile robot is to follow a line on the plane! defined
ax + by + c = 0. This requires two controllers to adjust steering. One controller
Ejercicio
• Hacer una simulación (python) basada en el modelo anterior de robot
de la trayectoria que sigue para ir desde una posición inicial a una
final, ambas pedidas por teclado.
Nota
• En la resta de ngulos para el control de la orientaci n, se recomienda usar la
siguiente funci n que devuelve el ngulo entre –pi y pi
import numpy as np
def angdiff(th1, th2):
d = th1 - th2
d = np.mod(d+np.pi, 2*np.pi) - np.pi
return d
.
.
ó
á
á
ó
:
• Igualmente, para calcular arcotangentes usar la función atan2(Y, X) para
obtener un ángulo considerando los cuatro cuadrantes
Control del Robot
• Seguir una Trayectoria:
Fig. 4.10. The Simulink® model
sl_pursuit drives the vehicle
to follow an arbitrary moving target using pure pursuit. In this example the vehicle follows a point
moving around a unit circle with
a frequency of 0.1 Hz
– Se trata de seguir una secuencia de puntos
(x*, y*) en el plano xy.
– Se supone que recibimos un punto en cada
instante de muestreo.
– Para cada punto de la trayectoria calculamos
un error:
• e=
(x* − x)2 + (y* − y)2 − d*
v* = Kve + edt
∫
y* − y
• θ* = atan
x* − x
• γ = Kh(θ* − θ)
•
Fig. 4.11. Simulation results from
pure pursuit. a Path of the robot
in the xy-plane. The black dashed
line is the circle to be tracked and
the blue line in the path followed
by the robot. b The speed of the
vehicle versus time
Control del Robot
ax + by + c = 0
• Seguir una línea:
!
Fig. 4.6. The Simulink model
– Dos controladores
paradrives
el the
giro
sl_driveline
ve- del
hicle along a line. The line paramvolante:
eters (a, b, c) are set in the workspace variable L. Red blocks have
Kd > that
0 you can adjust to
• αd = − Kd d,parameters
d<0
x − 2y + 4 = 0
investigate the effect on perfor– siendo d la
distancia ortogonal de la
mance
posición del robot a la línea
–
d=
ax + by + c
a2 + b2
d>0
• Ajustar la orientación del robot paralela
a la línea:
Fig. 4.7.
−a
– θ* = tan
b
– αh = Kh(θ* − θ), Kh > 0
Simulation
results from different
−1
initial poses for the line (1, −2, 4)
•
and an initial pose
γ = αd + αh = − Kd d + Kh(θ* − θ)>>
θ* = atan2(−1, − 2) ≈ − 153o
x0 = [8 5 pi/2];
and then simulate the motion
>> r = sim('sl_driveline');
Para que vaya hacia la derecha, todo encaja perfectamente si especi camos la curva como -x+2y-4=0
fi
The vehicle’s path for a number of different starting poses is shown in Fig. 4.7.
76
angle of
respect to
α is the a
with resp
Chapter 4 · Mobile Robot Vehicles
which results in
Moverse a una pose específica
and assumes the goal {G} is in front of the
vehicle.
The linear control law
76
Chapter 4 · Mobile Robot Vehicles
ρ=
Δ2x + Δ2y
drives the robot
β ) = (0, 0, 0). The intuition behind
Δy to a unique equilibrium! at (ρ,Fig.α,
4.12.
−1
Polar coordinate notation for the
= tan
− θthat the terms k ρ and k α drive
thisαcontroller
is
the
robot
along a line toward {G}
bicycle model
vehicle
moving
ρ
α
Δx
toward a goal pose: ρ is the
to the goal,
β is the
while the term kβ β rotates the line so that β →distance
0. The
closed-loop
system
angle of the goal vector with
β =−θ−α
respect to the world frame, and
The control law introduces a discon
nuity at ρ = 0 which satisfies Brocke
theorem.
α is the angle of the goal vector
with respect to the vehicle frame
and assumes the goal {G} is in front of the vehicle. The linear control law
which results in
is stable so long as
and assumes the goal {G} is in front of the vehicle. The linear control law
Fig. 4.12.
Polar coordinate notation for the
bicycle model vehicle moving
toward a goal pose: ρ is the
distance to the goal, β is the
angle of the goal vector with
respect to the world frame, and
α is the angle of the goal vector
with respect to the vehicle frame
drives the robot to a unique equilibrium! at (ρ, α, β ) = (0, 0, 0). The intuition behind
thisdrivescontroller
is that the terms kρ ρ and kα α drive the robot along a line toward {G}
the robot to a unique equilibrium at (ρ, α, β ) = (0, 0, 0). The intuition behind
this controller is that the terms k ρ and k α drive the robot along a line toward {G}
while
the term kβ βso thatrotates
the line so that β → 0. The closed-loop system
while the term k β rotates the line
β → 0. The closed-loop system
!
ρ
α
The control law introduces a discontinuity at ρ = 0 which satisfies Brockett’s
theorem.
β
which results in
is stable so long as
The contro
nuity at ρ
theorem.
Opcional
• Si queréis experimentar con las simulaciones que se muestran en el
libro, podéis instalar una toolbox para Matlab/Simulink:
• https://petercorke.com/toolboxes/robotics-toolbox/
• Existe un port a python https://pypi.org/project/robopy/.