程序代写代做代考 algorithm python matlab CMP3103M AMR 4 - Control.key

CMP3103M AMR 4 – Control.key

 
CMP3103M AMR

Dr. Marc Hanheide
MOTION & CONTROL

go to: http://lncn.eu/cmp3103m/

http://lncn.eu/cmp3103m/

QUIZ

‣ Why would you use HSV colour space?

A. not too sensitive to illumination changes

B. resembles human vision system

C. physically plausible model (direct analogy to wave lengths)

D. more efficient to compute than RGB

E. can represent all displayable colours, also those that RGB can’t

WHAT IS WRONG WITH
THIS CODE?

‣ What is wrong?

A. node is initialised
after Subscriber
and Publisher are
created

B. Illegal constructor
for Publisher

C. an import
statement is missing

D. tries publishing
double on String-
typed topic

import rospy
from std_msgs.msg import String
from sensor_msgs.msg import LaserScan

class FirstSub:
def __init__(self):
self.sub = rospy.Subscriber(‘/turtlebot_2/scan’,
LaserScan,
callback=self.callback)
self.pub = rospy.Publisher(‘/out’, String)

def callback(self, msg):
for range in msg.ranges:
if range < 1.0: s = String() s.data = 1.0 self.pub.publish(s) fp = FirstSub() rospy.init_node("firstsubscriber") rospy.spin() ! WHAT IS WRONG WITH THIS CODE? import rospy from std_msgs.msg import String from sensor_msgs.msg import LaserScan class FirstSub: def __init__(self): self.sub = rospy.Subscriber(‘/turtlebot_2/scan', LaserScan, callback=self.callback) self.pub = rospy.Publisher(‘/out’, String) def callback(self, msg): for range in msg.ranges: if range < 1.0: s = String() s.data = "ALERT" self.pub.publish(s) rospy.init_node("firstsubscriber") fp = FirstSub() rospy.spin() ‣ What is wrong? A. node is initialised after Subscriber and Publisher are created B. Illegal constructor for Publisher C. an import statement is missing D. tries publishing double on String- typed topic ! SYLLABUS ‣ Introduction to Robotics ‣ Robot Programming ‣ Robot Sensing & Vision ‣ Robot Control ‣ Robot Behaviours ‣ Control Architectures ‣ Navigation Strategies ‣ Map Building ROBOTIC AGENT environment think / reason sense act ! MOBILE ROBOT CONTROL ‣ Move a robot in a desired way ‣ position control ‣ move to XY ‣ follow a path ‣ behaviours ‣ follow the corridor ‣ avoid obstacles ‣ Position control ‣ open and closed loop control ‣ What Voltage/Current/Force to put on your motors? ! THE TWIST pub = rospy.Publisher( ’/cmd_vel’, Twist, queue_size=1 ) t = Twist() pub.publish(t) ? ! MOVEMENT COMMANDS ‣ Dependent on the wheel configuration ‣ Omni-directional Drive (Rovio) ‣ Differential Drive (Turtlebot/Roomba) ! CONTROLLING TURTLEBOT ‣ Which speeds to turn each wheel to move in desired direction? ‣ That corresponds to the Force -> Current for every motor

‣ This is a control problem!

FOR MOTION CONTROL

Model

Engineering

Kinematics / Dynamics control-parameter

Algorithm

control signal => actuators

KINEMATICS

Model

Kinematics / Dynamics

AD-HOC MODEL

‣ Drive in a straight line: calculate the number of steps required
to perform a distance unit e.g. 1m

‣ different for each speed value (non(?)-linear dependency)

‣ and other factors (e.g. surface type)

‣ Use this value to calculate the number of steps required for a
desired distance

‣ Similar with pure rotations/angles

FORWARD MODEL

INVERSE MODEL

AD-HOC FORWARD MODEL
IS EASY?

THE WHEEL

c = 2πr

A SIMPLE ROBOT WITH ONE
WHEEL: FORWARD MODEL

v = r ω

Spinning at 60 degrees per second

Degrees… Radiants?

ω = (60 / 360) * 2π
= 1.0471975511965976

AND WITH TWO WHEELS?

v = 1/2 r (ωl+ωr)
but ωl = ωr when going

straight

WHAT’S THE INVERSE MODEL?

v = r ω
ω = v/r

Make your turtlebot go forward
with v = 1m/s with r = 6cm

AD-HOC FORWARD MODEL
IS EASY?

Hint: Assume both wheels moving same speed again!

d r
a

a = r/d (ωl-ωr)

v = 0
v = 1/2 r (ωl+ωr)
=> ωl=-ωr

FORWARD MODEL

?
a = 1/d r (ωl-ωr)
v = 1/2 r (ωl+ωr)

d r

INVERSE MODEL

?
a = 1/d r (ωl-ωr)
v = 1/2 r (ωl+ωr)

ωl = (v + (d * a)/2)/r

d r
ωr = (v – (d * a)/2)/r

PYTHON TIME

d r

PYTHON TIME

a = 1/d r (ωl-ωr)
= (rωl – rωr) / d

v = 1/2 r (ωl+ωr)
= (rωl+rωr) / 2

d r

PYTHON TIME

d r

ωl = (v + (d * a) /2) / r

ωr = (v – (d * a) /2) / r

PYTHON TIME
Forward and inverse Kinematics for Turtlebot
We define two functions forward_kinemantics and inverse_kinematics respectively. They allow to
convert from wheel speeds to robot motion and from desired robot motion to wheel speeds.

v denotes the linear velocity of the robot
a denotes the angular velocity of the robot
w_l is the angular velocity of the left wheel
w_r is the angular velocity of the right wheel

unit are m for geometry, m/s for linear velocity and rad/s for angular velocity

In [8]:

import math
#from geometry_msgs.msg import Twist

# some estimates for the robot gemorty
wheel_radius = 0.05 # 5 cm radius of wheel
robot_radius = 0.25 # 25 cm radio of base

# computing the forward kinematics for a differential drive
def forward_kinematics(w_l, w_r):
c_l = wheel_radius * w_l
c_r = wheel_radius * w_r
v = (c_l + c_r) / 2
a = (c_l – c_r) / robot_radius
return (v, a)

# computing the inverse kinematics for a differential drive
def inverse_kinematics(v, a):
c_l = v + (robot_radius * a) / 2
c_r = v – (robot_radius * a) / 2
w_l = c_l / wheel_radius
w_r = c_r / wheel_radius
return (w_l, w_r)

In [13]:

# try out forward kinematics, both wheels turning at `2pi rad/s` (one full turn
per second)

(v, a) = forward_kinematics(2*math.pi, 2*math.pi)
print “v = %f,\ta = %f” % (v, a)

v = 0.314159, a = 0.000000

PYTHON TIME

Forward and inverse Kinematics for Turtlebot
We define two functions forward_kinemantics and inverse_kinematics respectively. They allow to
convert from wheel speeds to robot motion and from desired robot motion to wheel speeds.

v denotes the linear velocity of the robot
a denotes the angular velocity of the robot
w_l is the angular velocity of the left wheel
w_r is the angular velocity of the right wheel

unit are m for geometry, m/s for linear velocity and rad/s for angular velocity

In [8]:

import math
#from geometry_msgs.msg import Twist

# some estimates for the robot gemorty
wheel_radius = 0.05 # 5 cm radius of wheel
robot_radius = 0.25 # 25 cm radio of base

In [13]:

# try out forward kinematics, both wheels turning at `2pi rad/s` (one full turn
per second)

(v, a) = forward_kinematics(2*math.pi, 2*math.pi)
print “v = %f,\ta = %f” % (v, a)

v = 0.314159, a = 0.000000

In [15]:

# try out forward kinematics, one wheel turning at `2pi rad/s` (one full turn pe
r second), the other `-2pi rad/s`

(v, a) = forward_kinematics(2*math.pi, -2*math.pi)
print “v = %f,\ta = %f” % (v, a)

In [16]:

# inverse kinematics:

(w_l, w_r) = inverse_kinematics(1.0, 0.0)
print “w_l = %f,\tw_r = %f” % (w_l, w_r)

# this should give us again the desired values:
(v, a) = forward_kinematics(w_l, w_r)
print “v = %f,\ta = %f” % (v, a)

In [ ]:

v = 0.000000, a = 2.513274

w_l = 20.000000, w_r = 20.000000
v = 1.000000, a = 0.000000

CONTROLLING ROVIO

‣ Which speeds to turn each wheel to move in desired direction?

‣ That corresponds to the Voltage for every motor

‣ This is a control problem!

‣ Rovio has “universal wheels” or “omni wheel”

‣ simple design

‣ significant slippage

‣ improvement: “Mecanum wheel”

‣ Straight line

‣ Pure rotation

USING THE KINEMATIC
MODEL

forward kinematics

INVERSE KINEMATICS FOR
ROVIO

‣ General (omni-drive) velocity control example

ROVIO KINEMATICS IN
MATLAB

30°
-30°

90°

ROVIO KINEMATICS IN
MATLAB

FOR MOTION CONTROL

Model

Engineering

Kinematics / Dynamics control-parameter

Algorithm

control signal => actuators

OPEN LOOP CONTROL

‣ Task for the controller :

‣ generate control signals u so that y = y*

Model 
e.g. kinematics

Input 
(e.g. drive command)

Output 
(e.g. robot’s position/vel)

Reference  
(e.g. desired position/vel)

Controller Robot

YOU ARE ROVIO!

‣ close your eyes and hit the spot! 
 
 
 

‣ … and that’s not even really open-loop control

‣ Proprioception: “one’s own” and perception, is the sense of the
relative position of neighbouring parts of the body and strength
of effort being employed in movement.

http://en.wikipedia.org/wiki/Sense

OPEN LOOP CONTROL

‣ No measurements – no feedback

‣ can rely on a model only

‣ better models – smaller errors, however the errors accumulate
over time

‣ no possibility of correcting the errors since they are unknown

‣ how to deal with unexpected events, changes, obstacles, etc.?

OPEN LOOP CONTROL
MOVE IN A SQUARE

CLOSED LOOP CONTROL

‣ Task for the controller :

‣ generate control signals u that will keep error e as small as
possible (0 would be the best)

Input 
(e.g. drive command)

Output 
(e.g. robot’s position/vel)

Reference  
(e.g. desired position/vel)

Error

Controller

Sensor

Robot

Measurement 
(e.g. measured position)

DRIVE DISTANCE – FEEDBACK
CONTROL

‣ Repeat

‣ issue DriveForward command

‣ read odometry (proprioception again) and accumulate the total
distance travelled

‣ stop if the total distance is greater than the specified value 
 
 

‣ Smaller time intervals – smaller errors

CORRIDOR FOLLOWING

‣ Scenario:

‣ two sensors measuring distance to the wall dl
and dr

‣ robot moves constantly forward (v)

‣ controller affects the angular speed only  
(u = ω)

‣ Controller task:

‣ keep dl = dr

dl dr
ω
v

+ –

SIMPLE CONTROLLER

‣ Loop:

‣ measure e = dl – dr

‣ if e > 0 then ω = +K

‣ if e < 0 then ω = -K ‣ ω = K sign(e) ‣ In Rovio language: ‣ if e > 0 then RotateLeft(5)

‣ if e < 0 then RotateRight(5) constant  parameter K = 5 dl dr ω v + - ! BANG-BANG CONTROLLER ‣ Control input depends only on the sign of the error K = 1 K = 10K = 5 ! PROPORTIONAL CONTROLLER ‣ Change ω proportionally to the error value ‣ ω = e*Kp ‣ small e – small correction ‣ large e – large correction ‣ Result ‣ smoother actions and smaller errors ‣ Kp parameter ‣ large – faster reaction ‣ small – slower ‣ optimal value: smooth behaviour, robust to changes proportional  gain ! VISION-BASED CONTROL ‣ Object state – in our case: x position and size ‣ Error - difference between the   desired and current state ‣ Control input u: ‣ Spin – to adjust the x position ‣ DriveForward - to adjust the size current state: information from   the object detector desired state FOR MOTION CONTROL Model Engineering Kinematics / Dynamics control-parameter Algorithm control signal => actuators

NOW FOR THE REAL STUFF

Morgan Quigley, Brian Gerkey & And William D. Smart. (2015)
Programming Robots with ROS [Chapter 12]

RESOURCES

‣ https://github.com/osrf/rosbook (https://github.com/osrf/
rosbook/tree/master/followbot)
‣ catkin_init_workspace
‣ git clone https://github.com/osrf/rosbook.git
‣ catkin_make

‣ Morgan Quigley, Brian Gerkey & And William D. Smart.
(2015) Programming Robots with ROS [Chapter 12]

https://github.com/osrf/rosbook
https://github.com/osrf/rosbook/tree/master/followbot
https://github.com/osrf/rosbook.git

PROBLEMS WITH
PROPORTIONAL CONTROLLER?

‣ What’s the problem?

PID CONTROLLER

Proportional:
corrects the actual

error

Differential:
slows the rate of change
(reduces overshooting)

Integral:
accelerates rate of

change

PID CONTROLLER

IMPLEMENTATION EXAMPLES
Bang-bang controller

IMPLEMENTATION EXAMPLES
Proportional controller

APPLICATIONS – EXAMPLES

‣ Balancing Robot

‣ Person Following 
 
 

ANY OTHER CONTROL
PROBLEMS YOU COULD

CONTROL THEORY

‣ Optimal control, minimise

‣ errors

‣ energy

‣ response time

‣ Design and analysis of controllers

‣ linear (P, PD, PID controllers)

‣ non-linear

‣ adaptive controllers

‣ changing parameter values in
time

‣ models

‣ kinematics and dynamics in
robotics

OUT OF CONTROL?

Related Posts