Agenda Path smoothing PID Graph slam

Path smoothing Until now we have worked with straight motions

Path smoothing Straight line

Path smoothing Smooth line

How do we smooth paths? X0 X0 X1 X1 X2 X2

How do we smooth paths? As we move X1 in we reduce the distance between X1 and X0 and X1 and X2. In other words as we move X1 the length of the red lines get smaller. So how do we figure out how to move X1 to reduce the length of the red lines? X2

How do we smooth paths? We subtract X1 from each of its neighbours and add the difference to X1 . In other word: X0 X1 X2 where α is a weight term that is used to control how quickly we move away from X1

How do we smooth paths? Quiz: what will happen if we repeatedly apply this update to our path? X0 X1 X2 We will end up with a straigth line

How do we smooth paths? Therefore we need a constraint to control the update to make it balance! To do this we will iteratively move the point back towards the original point X0 X1 Lets call the moving point Y1 to distinguish it from the original point X1 X2

How do we smooth paths? We now define the counter weight, which drags the point Y1 back to the original position X1 X0 X1 X2

Additional terms Distance to wall Fixed points Similarity of two following segments

Path smoothing Demo using gradient descent D:\Skole\AU-TekniskIT\Kalman\Min præsentation\Eksempel\Path smooth demo\Pathsmooth.m Juster alpha Juster beta Aktiver fixed points Aktiver gamma Vælg 3d

PID

PID controlling Desired path Use the error to control the steering Cross track error Desired path Ønsket er hurtigst muligt at komme ind på rette kurs og blive der. Use the error to control the steering

Closed loop control systems Use a measurement of output to control the input (Feedback)

PID control P depends on the present error, I on the accumulation of past errors, D is a prediction of future errors, e(t) = set point - process value

P-controlling Marginally stability unstable Perfectly damped Cross track error Perfectly damped Desired path

Error term The error term is derived by subtracting the feedback (i.e. current position) from the set point (i.e. desired position). e(t) = u(t) – y(t)

Proportional Term Simple proportional coefficient Kp is multiplied by the error term. Provides linear response to the error term.

Integral Term Integral coefficient Ki is multiplied by the error term and added to the sum of all previous integral terms. Provides response to accumulated error.

Derivative Term Derivative coefficient Kd is multiplied by the difference between the previous error and the current error. The derivative term slows the rate of change of the controller output

Effects of increasing a parameter independently Rise time Overshoot Settling time Steady-state error Stability Decrease Increase Small change Degrade Eliminate Minor change No effect in theory Improve if small

Discrete PID algorithm previous_error = setpoint - process_feedback integral = 0 while(1) { wait(dt) error = setpoint - process_feedback integral = integral + (error*dt) derivative = (error - previous_error)/dt output = (Kp*error) + (Ki*integral) + (Kd*derivative) previous_error = error }

Ziegler-Nichols Gains are first set to zero. P gain is increased until it reaches the ultimate gain, Ku, at which the output of the loop starts to oscillate. The oscillation period, Pu, are used to set the gains as shown: Control type Kp Ki Kd P 0.5Ku - PI 0.45Ku 1.2Kp/Pu PID 0.60Ku 2Kp/Pu KpPu/8

Demo 2D D:\Skole\AU-TekniskIT\Kalman\Min præsentation\Eksempel\PID demo\ PIDdemo.py D:\Skole\AU-TekniskIT\Kalman\Min præsentation\Eksempel\PID smooth\ PIDSmooth.py

= to be able to build a map of an environment and simultaneously localize within this map

Graph slam Is to use a graph whose nodes correspond to the poses of the robot at different points in time and whose edges represent constraints between the poses.

Graph slam problem To estimate the posterior probability of the robot’s path and the map given all the measurements plus an the initial position Assumption: The world is static

Constraints for graph slam: Initial localization constraint Relative motion constraints Relative measurement constraints Init: 2 Moving: 4 for hver QUIZ: how many constraints do we have in this figure i 1D? 6

Implementation of Graph based slam

Implementation of Graph based slam The best estimate for all the robot locations and landmark locations

Quiz: 1D Slam Which cells will be modified? What will the values in the cells then be?

Quiz: 1D Slam Which cells will be modified? What will the values in the cells then be?

Quiz: 1D Slam Which fields are not modifies?

Quiz: 1D slam Find the location of the landmarks

Graph slam Finds the most likely path along with the measured location of landmarks X0 X1 Graph-slam: definding our probability of a location given a sequence of such constraints Maximize the probability!

Simplifying the probability maximization Finds the most likely path along with the measured location of landmarks X0 X1 Z1 +

Demo with different measurement confidences D:\Skole\AU-TekniskIT\Kalman\Min præsentation\Eksempel\Slam\slam.py

References https://www.udacity.com http://www.comp.dit.ie/jkelleher/rtf/classmaterial/week10/PathSmoothing.pdf