Autonomous navigation vehicle algorithms

Introduction

A great resource is F1Tenth, https://f1tenth.org/

See also https://mushr.io/tutorials/tuning/ and https://racecar.mit.edu/platform

Safety Concerns

• Real-life problems
• Sensors
• Failure modes

Automatic breaking AEB

Automatic emergency breaking AEB

• Detect objects
• Find range, velocity, heading
• Determine critical objects. Time to collision TTC.

• False positive: Nobody will buy a system with these
• False negative: kills innocent people

Stop the vehicle before colliding.

Sensors

• Camera
  • Structured light 3d scanner camera
  • Stereo camera
  • Monocular camera
• Radar
• Ultrasonic
• Lidar
  • Planar lidar (Hokuyo 30LX)
  • 3d lidar
  • solid state lidar (Velodyne velarray)
• Odometry

Reactive Methods

Follow the Gap

Follow the gap

Furthest distance?

• Misreading
• Too small gap

Gap: Series of at least $n$ consecutive hits that pass some distance threshold $R$.

• Works good for holonomic robots. Also good if only sparse obstacles.
• No car's dimensions, no safety.
• What is the threshold radius $R$?

Avoid the nearest obstacle every timestep.

1. Find the nearest point: add a safety bubble with radius $r_{b}$ around it.
2. Set all distance points inside the safety bubble to zero (0).
3. Find the max gaps of consecutive nonzeros
4. Choose the best max gap of all. Naive: The furthest max-gap.

Consider changing the speed.

References

• https://panxiaofan.github.io/myf1tenth/2021/01/24/lab4.html
• F1/10 Autonomous Racing - Montreal Grand Prix Winning Team https://www.youtube.com/watch?v=ctTJHueaTcY&t=368s
• F1Tenth Autonomous Racing Cars(Lab 4) https://www.youtube.com/watch?v=4HOsg2SRabw

Bug algorithms

Original bug algorithm published around 1986.

Assume local knowledge of the environment and a global goal.

TangentBug

• Distance $\rho (x,\theta )$ function, $x\in \mathbb {R} ^{2}$ and $\theta \in [0,2\pi ]$.

Saturated distance function for range $R$ $\rho _{R}(x,\theta )={\begin{cases}\rho (x,\theta ),{\text{ if }}\rho (x,\theta )<R\\\infty ,{\text{ otherwise}}\end{cases}}$

Problems

• Requires knowledge about the distance to the goal; beacon setup or similar

References

• https://www.cs.cmu.edu/~motionplanning/lecture/Chap2-Bug-Alg_howie.pdf
• https://cs.gmu.edu/~plaku/teaching/LecRoboBugAlgorithms.pdf

Artificial Potential Fields

Gradient descent

${\begin{matrix}{\text{If }}&\nabla U(q_{i})\neq 0:\\&q_{i+1}=q_{i}-\alpha _{i}\nabla U(q_{i})\end{matrix}}$

Problems

• Local minima

References

• https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
• https://www.coursera.org/learn/robotics-motion-planning

Mapping and localization

SLAM

https://f1tenth-coursekit.readthedocs.io/en/latest/lectures/ModuleC/lecture07.html

Filtering (Bayesian)

Simulator repo https://github.com/f1tenth/f110_ros/tree/master/f110_simulator

1. State estimation
2. Probability and Bayes rule
3. Recursive Bayes filter
4. Variants of Bayes filter:
5. Tuning particles

1 State estimation. Motor u, output y. System matrix A, Output matrix C, input matrix B.

${\begin{aligned}{\dot {x}}&=Ax+Bu\\y&=Cx\end{aligned}}$

Dead reckoning: the current state using previous state and estimates of varibales (speed, heading, time etc). Noise; the approximate distribution of the position.

Observation: correction.

2 Probability and Bayes rule. Conditional probability $P(B|A)$ the chance of B when A has already happened. $P(B|A)={\frac {P(A|B)P(B)}{P(A)}}$.

3 Recursive Bayes filter. Hidden Markov Model. Action model $p(x_{t}|u_{t},x_{t-1})$, sensor model $p(o_{t}|x_{t})$.

Step 1. Prediction with the control input (like in dead reckoning)

${\begin{aligned}bel(x_{t})&=P(x_{t}|o_{1:t-1},u_{1:t})\\&=\int P(x_{t}|x_{t-1},o_{1:t-1},u_{1:t}bel(x_{t-1})d(x_{t-1})\end{aligned}}$

Step 2. Correction with the observation (Bayes) \begin{align} bel_{posterior}(x_t) = \dots \end{align} </math>

Labs from F1tenth

1. Wall following https://f1tenth-coursekit.readthedocs.io/en/latest/assignments/labs/lab3.html#doc-lab3

More references

• Quaternions: Steven M. LaValle - Virtual reality lectures https://www.youtube.com/playlist?list=PL_ezWOhnpakMojiJGm-YiCz5zr4GpuLG_
• Names of the coordinates: https://www.ros.org/reps/rep-0105.html
• Ziegler-Nichols method for PID https://en.wikipedia.org/wiki/Ziegler%E2%80%93Nichols_method
• Harmonic potential field path planning for high speed vehicles https://folk.ntnu.no/skoge/prost/proceedings/acc08/data/papers/0383.pdf
• Robotic Motion Planning: Potential Functions https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
• https://fab.cba.mit.edu/classes/865.21/topics/path_planning/robotic.html
• https://web.ics.purdue.edu/~rvoyles/Classes/ROSprogramming/Lectures/PathPlanning.pdf