Navigation

Navigation based on the trajectory

package: fub_navigation

Input: sample_map_origin_map.txt

Output: matrix1.npy : forces' matrix in npy format

Run make_force_matrixs.py:

  python make_force_matrixs.py

Variables:

 map_size_x        600 cmm
 map_size_y        400 cm
 resolution        10 cm

Look a head = 0.2 meter

Look a head = dynamic 0.2 + 1/(1*distance +1) meter

Look a head = 0 meter

a potential field map for the second lane Look a head = dynamic 0.2 + 1/(0.5*distance +1) meter

Test example:

Run plot_desired_steering.py:

  python plot_desired_steering.py

Load the "matrixDynamic" and find the steering angle

Navigation using the force matrix

source code: control.py

The controller formula when fx is positive:

   Kp = 4.0
   steering angle = Kp * arctan(fy/(fx*2.5))

When fx is negative the maximum steering angle is used, and sign of the steering angle is defined based on sign of fy.

Run:

  python control.py

Load the "matrixDynamic" and publish the desired steering angle and speed
Desired trajectory:Green path, Car odometry: red arrows

Load the "matrix20cm" and publish the desired steering angle and speed
Desired trajectory:Green path, Car odometry: red arrows

Load the "matrixDynamic" and publish the desired steering angle and speed
Desired trajectory:Green path, Car odometry: red arrows

Simulation video1 Initial position of the car is (0,0).

Simulation video3: Moving backward when f_x in car coordinate is negative.

Simulation video2 When the car is on the lane, but rotated 180 degrees, with maximum steering.

Simulation video4: When the car is on the lane, but rotated 180 degrees, without maximum steering.

Simulation video5: Lane changing.