Types of Localization

Types of Frame and Transformation

Reference

Reference

LiDAR Preprocessing

Reference

Acceleration and Brake Calibration (Polaris GEM Vehicles)

Raw Vehicle Command Converter (Polaris GEM Vehicles)

Steer Offset Estimator (Polaris GEM Vehicles)

Reference

Pure Pursuit Lateral Controller (Kinematic Bicycle Model)

\(\theta\): heading of vehicle or yaw angle of vehicle
\(\dot{\theta}\): rotation rate of vehicle or yaw rate of vehicle
\(\delta_f\): front steering angle
\(L\): wheelbase
\(l_d\): look-ahead distance
\(\alpha\): angle between the vehicle’s heading vector and the look-ahead vector
\(e_{l_d}\): crosstrack error, \(e_{l_d}(t) = l_d sin(\alpha(t))\)
\(\kappa\): path curvature
\(v_x\): vehicle speed in the direction of its x-axis
\(k_{dd}\): gain of pure pursuit controller, \(l_d = k_{dd}v_x(t)\)

$$\delta_f(t) = tan^{-1}(\frac{2Lsin(\alpha(t))}{l_d}) = tan^{-1}(\frac{2Lsin(\alpha(t))}{k_{dd}v_x(t)})$$

$$\delta_f(t) = tan^{-1}(\frac{2L}{l^2_d}e_{l_d}(t))$$

Stanley Lateral Controller (Kinematic/Dynamic Bicycle Model)

\(\theta\): heading of vehicle or yaw angle of vehicle
\(\dot{\theta}\): rotation rate of vehicle or yaw rate of vehicle
\(\theta_{e}\): heading error
\(\delta_f\): front steering angle, \(\delta_f \in [\delta_{min}, \delta_{max}]\)
\(L\): wheelbase
\(e\): crosstrack error
\(k\): controller gain

LQR Tracker with Feed Forward Term (Dynamic Bicycle Model)

\(\theta\): heading of vehicle or yaw angle of vehicle
\(v\) and \(a\): velocity of the center of mass and acceleration of center of mass
\(v_x\) and \(v_y\): longitudinal velocity and lateral velocity
\(a_x\) and \(a_y\): longitudinal acceleration and lateral acceleration, \(a_y = {v_x^2}/{R} = \kappa v_x^2\)
\(v_f\) and \(v_r\): velocity of front wheel and velocity of rear wheel
\(F_{yf}\) and \(F_{yr}\): lateral force of front/rear tire, \(F_{yf}=[2]C_{\alpha f}\alpha_f\), \(F_{yr}=[2]C_{\alpha r}\alpha_r\)
\(\alpha_f\) and \(\alpha_r\): slip angle of front/rear tire
\(C_{\alpha f}\) and \(C_{\alpha r}\): cornering stiffness of front/rear tire
\(l_f\) and \(l_r\): distance from front/rear wheel to the center of mass
\(\theta_{vf}\) and \(\theta_{vr}\): velocity angle of front/rear tire
\(\theta\): yaw angle in absolute coordinate
\(\beta\): slip angle of the center of mass in vehicle body coordinate
\(\theta+\beta\): heading angle in absolute coordinate
\(e_d\): lateral error, \(e_d\) is positive when road is on the left side of the vehicle

$$\sum F_y = m a_y = F_{yf}cos(\delta_f) + F_{yr}$$ $$\sum M = I \ddot{\theta} = F_{yf}cos(\delta_f) l_f – F_{yr} l_r$$ $$a_y = \frac{d^2y}{dt^2} = \ddot{y} + v_x\frac{v_x}{R}= \ddot{y} + v_x \dot{\theta} = \dot{v_y} + v_x \dot{\theta}$$ $$ma_y = C_{\alpha f}\alpha_f + C_{\alpha r}\alpha_r = m(\dot{v_y} + v_x \dot{\theta}) = C_{\alpha f} (\delta_f – \frac{\dot{\theta}l_f+v_y}{v_x}) + C_{\alpha r} (\frac{\dot{\theta}l_r-v_y}{v_x})$$ $$\dot{v_y} = \frac{C_{\alpha f}}{m} \delta_f + (\frac{C_{\alpha r} l_r – C_{\alpha f} l_f}{m v_x} – v_x)\dot{\theta} – \frac{C_{\alpha f} + C_{\alpha r}}{m v_x} v_y$$ $$\ddot{\theta} = \frac{C_{\alpha f} l_f}{I} \delta_f – \frac{C_{\alpha f} l_f^2 + C_{\alpha r} l_r^2}{I v_x} \dot{\theta} + \frac{C_{\alpha r} l_r – C_{\alpha f} l_f}{I v_x} v_y$$

$$ \begin{bmatrix} \ddot{y} \\ \ddot{\theta} \\ \end{bmatrix} = \begin{bmatrix} – \frac{C_{\alpha f} + C_{\alpha r}}{m v_x} & \frac{C_{\alpha r} l_r – C_{\alpha f} l_f}{m v_x} – {v_x} \\ \frac{C_{\alpha r} l_r – C_{\alpha f} l_f}{I v_x} & – \frac{C_{\alpha f} l_f^2 + C_{\alpha r} l_r^2}{I v_x} \\ \end{bmatrix} \begin{bmatrix} \dot{y} \\ \dot{\theta} \\ \end{bmatrix} % + % \begin{bmatrix} \frac{C_{\alpha f}}{m} \\ \frac{C_{\alpha f} l_f}{I} \\ \end{bmatrix} % \delta_f $$ $$ \begin{bmatrix} \dot{e}_d \\ \ddot{e}_d \\ \dot{e}_{\theta} \\ \ddot{e}_{\theta} \\ \end{bmatrix} = \begin{bmatrix} {0} & {1} & {0} & {0} \\ {0} & – \frac{C_{\alpha f} + C_{\alpha r}}{m v_x} & \frac{C_{\alpha f} + C_{\alpha r}}{m} & \frac{C_{\alpha r} l_r – C_{\alpha f} l_f}{m v_x} \\ {0} & {0} & {0} & {1} \\ {0} & \frac{C_{\alpha r} l_r – C_{\alpha f} l_f}{I v_x} & -\frac{C_{\alpha r} l_r – C_{\alpha f} l_f}{I} & – \frac{C_{\alpha f} l_f^2 + C_{\alpha r} l_r^2}{I v_x} \\ \end{bmatrix} \begin{bmatrix} {e}_d \\ \dot{e}_d \\ {e}_{\theta} \\ \dot{e}_{\theta} \\ \end{bmatrix} $$ $$ + % \begin{bmatrix} {0} \\ \frac{C_{\alpha f}}{m} \\ {0} \\ \frac{C_{\alpha f} l_f}{I} \\ \end{bmatrix} % \delta_f % + % \begin{bmatrix} {0} \\ \frac{C_{\alpha r} l_r – C_{\alpha f} l_f}{m v_x} – {v_x} \\ {0} \\ – \frac{C_{\alpha f} l_f^2 + C_{\alpha r} l_r^2}{I v_x} \\ \end{bmatrix} % \dot{\theta}_r $$ $$ \dot{X} = AX + B_1 \delta_f + B_2 \dot{\theta}_r \quad where \quad X = \begin{bmatrix} {e}_d \\ \dot{e}_d \\ {e}_{\theta} \\ \dot{e}_{\theta} \\ \end{bmatrix} $$

$$ J = \sum_{k=0}^{\infty} X^T[k] Q X[k] + u[k]^T R u[k] \quad w.r.t. \quad x[k+1] = A_dx[k]+B_du[k] $$ where \(A_d = (I – \frac{Adt}{2})^{-1}(I + \frac{Adt}{2})\) and \(B_d = Bdt\)

Riccati Equation
$$ P_{k-1} = Q + A_d^TP_kA_d-A_d^TP_kB_d(R+B_d^TP_kB_d)^{-1}B_d^TP_kA_d $$ Feed Forward Term
The state space model for the closed-loop system under state feedback
$$ \dot{X} = AX + B_1 \delta_f + B_2 \dot{\theta}_r = (A- B_1)K X + B_2 \dot{\theta}_r $$ The closed-loop system with feedforward term \(\delta_{ff}\) can be written as $$ \dot{X} = AX + B_1 \delta_f + B_2 \dot{\theta}_r = (A- B_1)K X + B_1\delta_{ff} + B_2 \dot{\theta}_r $$ where \(u = \delta_f = -K_{1\times4}X_{4\times1} + \delta_{ff}\)

Model Predictive Control (Dynamic Bicycle Model)

Reference

Camera Perception (Temporary Demo, AutowareClass2020)

\(D\): aperture diameter
\(f\): focal length
\(N\): aperture f-number (e.g., f/2.8, f/4)

$$D = \frac{f}{N}$$

$$FoV = 2arctan(\frac{sensor\_dimension}{2f})$$

\(K\): intrinsic camera parameters
\([R|t]\): extrinsic camera parameters
\(s\): extrinsic camera parameters
\(f_x,f_y\): focal lengths of the camera in the x- and y-direction in pixels
\(c_x,c_y\): principal point coordinates, \(c_x\) and \(c_y\) are close to the center of the image
\(p_i\): 2D image point in homogeneous coordinates \([u, v, 1]^T\)
\(p_w\): 3D image point in the world in homogeneous coordinates \([x, y, z, 1]^T\)

$$P = K[R|t] = \begin{bmatrix} f_x & s & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}[R|t]$$

$$p_i = \begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = Pp_w = K[R|t]p_w = \begin{bmatrix} f_x & s & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} r_{11} & r_{12} & r_{13} & t_x \\ r_{21} & r_{22} & r_{23} & t_y \\ r_{31} & r_{32} & r_{33} & t_z \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix}$$

Radar Perception (Temporary Demo, AutowareClass2020)

Radar can measure the distance, angle, velocity, and cross-section to/of objects to detect and classify different objects.

LiDAR Perception

Object Detection & Recognition with LiDAR and Camera on GEM e4

Reference

Reference