Practical Robotics (PRAC)

Transcription

1 Practical Robotics (PRAC) A Mobile Robot Navigation System (1) - Sensor and Kinematic Modelling Nick Pears University of York, Department of Computer Science December 17, 2014 nep (UoY CS) PRAC Practical Robotics December 17, / 31

2 Practical Robotics (PRAC): Module outline - first half, Nick Pears and Alan Millard (labs) Lecture 1: A Mobile Robot Navigation System (1) - Sensor and Kinematic Modelling (Nick Pears, week 2) Lab 1a (week 2): Calibrating and characterising IR range sensors. Lab 1b (week 3): Calibrating and characterising the odometry system. Lecture 2: A Mobile Robot Navigation System (2) - Steering Control and Integrated Navigation System (Nick Pears, week 3) Lab 2a (week 4): A steering control system for wall following. Lab 2b (week 5): An integrated robot navigation system (time permitting!) nep (UoY CS) PRAC Practical Robotics December 17, / 31

3 Practical Robotics (PRAC): Module Outline - second half, Alan Millard and Adrian Bors Lecture 3 (Alan Millard, week 4): ROS and SLAM. Lab 3a (week 6): The Robot Operating System (ROS) Lab 3b (week 7): Swarm PI robot lab A Lab 3c (week 8): Swarm PI robot lab B Lecture 4 (Adrian Bors, week 5): Computer Vision (CV) for Mobile Robots (Optical Flow). Lab 4a (week 9): CV lab A Lab 4b (week 10): CV lab B nep (UoY CS) PRAC Practical Robotics December 17, / 31

4 Outline of this lecture The first part of this lecture will overview modelling in general and then discuss the kinematic modelling on the Pololu m3pi robot. 1 An overview of modelling in robotics. 2 Kinematic modelling of the robot. The second part of this lecture will look at each of the two main sensor systems that we will use 1 Calibrating, characterising and using the IR distance sensors in the navigation system (extereoceptive sensors). 2 Calibrating, characterising and using the odometry sensors in the navigation system (proprioceptive sensors). nep (UoY CS) PRAC Practical Robotics December 17, / 31

5 Lecture 1, part 1: What is robot modelling? 1 Kinematics modelling: mathematical representation of (i) the geometric layout of sensors and actuators, and the overall physical robot structure connecting these together and (ii) movement of the robot across all possible degrees of freedom. 2 Dynamics modelling: mathematical representation of how driving forces and torques in robot s actuators result in robot motion; again, over all possible degrees of freedom. 3 Sensor modelling: mathematical representation of the operation and characteristics of the robot s sensors. 4 Environment modelling: mathematical representation of the environment that the robot(s) operate in. 5 Multi-agent modelling: mathematical representation of other robots and other dynamic agents (eg. humans, pets). nep (UoY CS) PRAC Practical Robotics December 17, / 31

6 Why do we model? (1) If we model everything (robot, environment, other agents), we have a robot simulation. In terms of research: cheaper, easier, unsupervised, and (with enough compute power) possible to run faster than real-time, which is important for certain types of highly iterative algorithms (eg genetic algorithms). Beware of the gap between simulation and reality, due to unmodelled effects and inaccuracies in modelling: the reality gap. Modelling of the robot s kinematics and dynamics allows us to design control systems for smooth, efficient, accurate robot motion and actions (real or simulated). Often, direct manual measurement of model parameters to sufficient accuracy is difficult, and hence some form of model calibration procedures are required. nep (UoY CS) PRAC Practical Robotics December 17, / 31

7 Why do we model? (2) Sensor modelling allows us to characterise the behaviour of a sensor system, including operating range and measurement uncertainties due to various types of noise. Again, sensor parameters usually need to be determined by a calibration procedure. If calibration is possible through the normal operation of the robot, it is often called self calibration (eg cross referencing across several sensor systems). One aspect of autonomy is for a robot to learn its own models, at least parameters, sometimes even the overall model structure. nep (UoY CS) PRAC Practical Robotics December 17, / 31

8 Why do we model? (3) Environment modelling is essential for the autonomy of mobile platforms, as witnessed by the mapping aspect of SLAM algorithms (Simultaneous Localisation and Mapping). For a robot to behave intelligently in a space shared with multiple agents (other robots and humans), the robot must have a means of predicting the effect of its own behaviour within a dynamic environment, which may rapidly change over time, therefore multi-agent modelling is necessary. Look-ahead simulation can be used to implement short-term tactical behaviours. nep (UoY CS) PRAC Practical Robotics December 17, / 31

9 Kinematic modelling and steering control used in this study 1 Kinematic modelling of a differential wheeled robot (tank-like steering). 2 Controller that employs a potential field around a demand path allows recovery from large errors. Figure : The Pololu m3pi robot nep (UoY CS) PRAC Practical Robotics December 17, / 31

10 The robot frame and kinematic model parameters Differential drive is very simple to implement and three wheels do not require a suspension system. Kinematic model parameters: let a be the radius of the drive wheels and b be the wheelbase. i.e the distance between the wheels. Note also that there are potentially 4x3 sensor position parameters (discussed later). Infra red distance sensor x R R R R ( xs, ys, θs ) sensor coordinates in robot centred frame ω L y R θ R ω R a, drive wheel radius b, wheelbase Figure : The robot frame and kinematic parameters nep (UoY CS) PRAC Practical Robotics December 17, / 31

11 The robot frame and kinematic model parameters 1 Origin of the robot s coordinate system can be anywhere, natural choice is the midpoint of the drive wheels - why? 2 Note that the superscript R indicates a robot centred coorinate frame. 3 In this navigation system, there are three different coordinate frames: R - the robot centred frame (one frame). L - a local frame describing robot position relative to the start of some straight line path segment (n seg frames for n seg path segments). G - a global frame in which to describe path segments, targets (walls), and accumulated position via odometry (one frame). 4 Make sure you use the correct frame and know how to switch between frames using a rigid Euclidean transformation (rotation and 2D translation, reviewed later). nep (UoY CS) PRAC Practical Robotics December 17, / 31

12 Kinematic modelling of a differential drive 1 Let the angular velocities of the left and right wheels be ω L,R 2 Signs of angular velocities are defined for forward motion. 3 Looking from outside the robot, right wheel is positive clockwise, left is positive anticlockwise. 4 How do ω L,R relate to the robot s (tangential) speed, v, and turning radius r = 1 κ? (nb: both v and κ are signed scalars.) v θ r Figure : Robot on a constant radius, moves through angle θ in time t nep (UoY CS) PRAC Practical Robotics December 17, / 31

13 Kinematic modelling of a differential drive (r b 2 ) θ = aω L t (1) (r + b 2 ) θ = aω R t (2) Dividing the first equation (blue line, previous fig) by the second equation (red line, previous fig) gives Rearranging this equation (do it yourself later) 2r b 2r + b = ω L ω R (3) r = b(ω R + ω L ) 2(ω R ω L ) = b ω mean ω diff (4) nep (UoY CS) PRAC Practical Robotics December 17, / 31

14 Kinematic modelling of a differential drive Observation: the turning radius is independent of the drive wheel radius, a, as expected (?) Beware of th singularity (divide by zero) for a straight path (infinite radius), maybe it is better to computer turning curvature? κ = 1 r = 1 b ω diff ω mean (5) Note that κ = 0 for the same angular velocities, i.e. when ω diff = 0. However, now we have a singularity when turning on the spot, ω L = ω R hence ω mean = 0. Practical solution: compute value of ω mean and for values lower than some minimum, switch to turning radius representation. nep (UoY CS) PRAC Practical Robotics December 17, / 31

15 Kinematic modelling of a differential drive 1 We note that v = θr is linear in r and that the robot s origin is the midpoint between the two drive wheels. 2 This implies that the tangential speed, v, of the robot s origin along its curved path must be the average of the two tangential wheel speeds, as follows: v = v R + v L 2 = a(ω R + ω L ) 2 = aω mean (6) Thus the transformation (ω L, ω R ) (v, κ) from angular wheel speeds to tangential speed and turning curvature is v = a(ω R + ω L ) 2 = aω mean, κ = 2(ω R ω L ) b(ω R + ω L ) = ω diff bω mean. (7) nep (UoY CS) PRAC Practical Robotics December 17, / 31

16 Kinematic modelling of a differential drive 1 However, in our robot system, we want to specify the (signed scalars) robot speed (v) and steering curvature (κ), using software routines for velocity and steering control respectively. 2 Thus we need to invert the relation on the previous slide, in order to get the required mapping (v, κ) (ω R, ω L ) 3 To make v and κ independent of each other, we need to increase the speed of the right wheel by ω and decrease the left wheel speed by the same amount, relative to the mean angular wheel speed defined by the desired velocity, v. nep (UoY CS) PRAC Practical Robotics December 17, / 31

17 Kinematic modelling of a differential drive It is straightforward to show the relation below (prove it for yourself) ω = b (κv) (8) 2a hence the required relation (v, κ) (ω L, ω R ) is given by ω R = v κb (1 + a 2 ) (9) ω L = v κb (1 a 2 ) (10) For high positive curvatures, this may result in a negative value for ω L and the wheel rotation direction would need to be reversed. Similarly, for high negative curvatures, the right wheel rotation direction may need reversing. nep (UoY CS) PRAC Practical Robotics December 17, / 31

18 Lecture 1, part 2: Sensors used in this study 1 Infra-red (IR) distance sensors: extereoceptive not always available due to limited operating range errors fairly predictable and do not grow over time 2 Optical odometry: proprioceptive always available, errors much less predictable, not independent (x, y dependent on θ error) and grow with time 3 Note the complementary nature of these two sensor systems : they can be used to implement a predict-correct (odometry prediction, IR correction) localisation system. 4 Both systems require modelling : parameters to be found via direct measurement or calibration. nep (UoY CS) PRAC Practical Robotics December 17, / 31

19 The IR distance sensors that we will use Range sensor (10-80cm), with PSD (position sensitive detector), IRED (infrared emitting diode) and signal processing circuit. Reflectivity of the target object and environmental temperature don t strongly affect the measurement, due to the employed geometric triangulation method. Three connections, 5V voltage supply (30mA consumed), ground and analogue signal. Valid signal around 60mS after power up. Sample-and-hold arrangement with around 40mS hold interval, hence 25Hz is the approximate maximum sampling rate. nep (UoY CS) PRAC Practical Robotics December 17, / 31

20 IR distance sensor characteristics Output voltage is roughly in proportion to the position of the imaged IR spot on the PSD. This PSD position is roughly proportional to the inverse of the target s distance (a property of triangulation geometry). nep (UoY CS) PRAC Practical Robotics December 17, / 31

21 Triangulation geometry, analyse using similar triangles, z = fb p (11) target object illuminated spot depth, z IR emitter triangulation baseline, B lens, focal length f PSD with spot focussed at position, p p nep (UoY CS) PRAC Practical Robotics December 17, / 31

22 Triangulation geometry Image position, p and sensor range z are inversely proportional. For a small change in p (eg, due to sensor noise), what is the change in z? z p = fb p 2 = z2 fb (12) If random noise level on p was constant over all distances (z), measurement repeatability would drop as square of distance, just due to triangulation geometry. However, the signal drops as square of distance which, for a PSD, causes random noise on p to also increase with the square of distance. Combining both the geometric effect in (1) with the photometric effect in (2) results a ranging RMS error that increases with the fourth power of distance! nep (UoY CS) PRAC Practical Robotics December 17, / 31

23 IR sensor calibration How do we convert a raw voltage reading (via ADC) to a distance measurement? Choosing a method and finding that method s values or parameters is called sensor calibration. Possible methods: 1 Look up table voltage - distance with linear interpolation (piecewise linear sensor model). Look up table voltage - reciprocal distance with linear interpolation (more accurate?) 2 Fit a parametric function using least-squares model fitting Can use either the characteristic in (1) or (2) above using, for example, a polynomial (what order? linear, quadratic?) nep (UoY CS) PRAC Practical Robotics December 17, / 31

24 Local pose estimation with IR sensors 1 For local pose estimation, measure distance and orientation relative to target (wall). 2 This may be used to correct any errors in global position estimation (from odometry). 3 Need the position and orientation of the IR sensors in the robot s coordinate frame, (x R s, y R s, θ R s ) i for i = Assuming good sensor orientation alignment, and accurate symmetrical sensor distribution we may reduce from 12 to 2 parameters (α, β), see next slide. 5 In the practical labs, decide for yourself whether or not this regular rectangle simplifying assumption is acceptable in terms of accuracy in local pose estimation. nep (UoY CS) PRAC Practical Robotics December 17, / 31

25 Local pose estimation with IR sensors y L t d LF x R R R R ( xs, ys, θs ) sensor coordinates in robot centred frame y L t y L y R θ R θ L β α y L Note that y L is positive here θ L is negative here d LR Infra red distance sensor y L θ L x L local (path) frame where y L t θ L = tan 1 ( dlr d LF 2β ), y L = y L t d LR + α cos θ L (13) is the distance between the demand path and the target (wall). nep (UoY CS) PRAC Practical Robotics December 17, / 31

26 Odometry using optical shaft encoders 1 For every revolution of a drive wheel, n digital pulses are generated. 2 There are two square wave signal 90 degrees out of phase - phase indicates wheel s rotational direction (may not need this - why?). 3 How do we determine the robot s global position update based on the incremental change in drive wheel rotation over time t? 4 Suppose the changes in pulse counts over t are w L,R, then comparing to Eqn. 7, r = 1 κ = b( w R + w L ) 2( w R w L ) (14) θ = 2πa nb ( w R w L ) (15) nep (UoY CS) PRAC Practical Robotics December 17, / 31

27 Odometry using optical shaft encoders Odometry software function should consider two cases 1 When the (signed) returned count from each wheel is different. 2 When the (signed) return count from each wheel is the same, straight line, no rotation, hence r =. The first case is described overleaf, the second (simpler case) is left as an exercise. nep (UoY CS) PRAC Practical Robotics December 17, / 31

28 Odometry - position updates in the robot s own frame [ x R, y R ] = [r sin θ, r(1 cos θ)]; (16) r θ rsin θ x R y R r(1 cos θ ) nep (UoY CS) PRAC Practical Robotics December 17, / 31

29 Odometry - resolved into a global frame The coordinate changes in the robot s own frame need to be mapped into a global frame, y G G θ G x Global frame Resolve the red components yourself and check with eqns. θ r(1 cos θ ) r rsin θ θ G rsin θ rsin θ cos θ G sin θ G nep (UoY CS) PRAC Practical Robotics December 17, / 31

30 Odometry equations - in a global frame x G y G θ G i+1 x G y G θ G i i+1 x G x R = y + y (17) θ G θ i x G cos θ G sin θ G 0 r sin θ = y G + sin θ G cos θ G 0 r(1 cos θ) (18) θ G θ nep (UoY CS) PRAC Practical Robotics December 17, / 31

31 Closing slide This lecture will help you with the first two practical classes: Lab 1a (week 2): Calibrating, characterising and using the IR range sensors. Lab 1b (week 3): Calibrating, characterising and using the odometry sensors. Any questions? nep (UoY CS) PRAC Practical Robotics December 17, / 31