1. Agenda
Path smoothing
PID
Graph slam

2. Path smoothing
Until now we have worked with straight motions

3. Path smoothing
Straight line

4. Path smoothing
Smooth line

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

6. 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

7. 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

8. 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

9. 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

10. 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

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

12. 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

13. PID

14. 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

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

16. 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

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

18. 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)

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

20. 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.

21. 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

22. 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

23. 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 }

24. 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

25. 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

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

27. 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.

28. 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

29. 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

30. Implementation of Graph based slam

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

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

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

34. Quiz: 1D Slam
Which fields are not modifies?

35. Quiz: 1D slam
Find the location of the landmarks

36. 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!

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

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

39. References https://www.udacity.com