Watch this video: PID For Driving

Let’s use function get_rot function to generate roation matrix:

import numpy as np
"""
    rotation matrix in 2D
"""
def get_rot(theta):
    rot = np.array([[np.cos(theta),  -np.sin(theta)]
                    [np.sin(theta),   np.cos(theta)]])
    return rot

Notation Note: (2D case, only considering rotation about z axis)

• Pose: P = [x, y, θ]
• Pose of car B: P_B = [x_B, y_B, θ_B]
• Pose of car B w.r.t world frame: ^wP_B = [^wx_B, ^wy_B, ^wθ_B], or P^w_A.
• Rotation matrix of car B w.r.t world frame: ^wR_B = get_rot(^wθ_B) =

| cos(theta) -sin(theta) |
| sin(theta)  cos(theta) |

(theta here means: ^wθ_B)

• Rotation matrix of world frame w.r.t car B: ^BR_w = ^wR_B^-1 = get_rot(-^wθ_B) =

| cos(theta)  sin(theta) |
| -sin(theta) cos(theta) |

• Translation of car B w.r.t world frame: ^wt_B = [^wx_B, ^wy_B]

• Transformation matrix of car B w.r.t world frame:

  ^wT_A =

  ​ =

1. Given a planned trajectory T(t) to track(eg. when t = 1, T(1) = [x₁, y₁, θ₁]), w.r.t world frame. Given the current pose of our mushur car A, [x_A, y_B, θ_A]

2. Pick a specific reference point(Say it’s reference car B) on trajectory

3. Compute error to reference point

   • Translation: Cross track error

   

   • Rotation: Heading angle error, simply θ_A - θ_ref

4. Compute control law to minimize error