Tricycle Calibration

Overview

This project's aim is to calibrate the kinematic parameters and the sensor pose of a Tricycle robot.

1) Dataset

The dataset is composed by 2434 samples, each composed by the following elements:

• time ticks → time stamp of the measurement
• steering and traction ticks → reading of the encoders mounted on the robot
• robot global pose → odometry of the robot
• sensor global pose → odometry of the sensor

2) Kinematic Model

The vehicle is modeled like a FWD bicycle in SE(2), which is described by the following kinematic model: $$ \begin{cases} \begin{align} \Delta x_k &= \cos(\alpha_k)\cos(\Delta \theta_k) \cdot d_k \\ \Delta y_k &= \cos(\alpha_k)\sin(\Delta \theta_k) \cdot d_k \\ \Delta \theta_k &= \frac{\sin(\alpha_k)}{a_k} \cdot d_k \\ \Delta \phi_k &= \alpha_k \end{align} \end{cases} $$ where \(\alpha_k\) is the steering angle, \(d_k\) is the traction distance, and \(a_k\) is the axis length of the robot. The steering angle and the traction distance are obtained from the encoders readings that we have in the dataset. The first is measured through an absolute encoder while the latter from an incremental encoder.

Within the equations \(t_k\) and \(s_k\) represent the readings obtained from the incremental and absolute encoders, while \(t_{\text{max}}\) and \(s_{\text{max}}\) are the maximum values related to those ticks. The previous equations contain three of the four kinematic parameters that we would like to calibrate with Least Squares algorithm, which are \(K_s\), \(K_t\) and \(\delta_s\). The first two values represent how many radians and meters correspond to one tick, while the last one is the steering offset.

3) State and Measurement Definition

The state \(X\) of our problem is composed by the kinematic parameters and the sensor pose relative to the robot. $$ X = \left[ X_k \;|\; X_s \right] = \left[K_s \;\, K_t \;\, a \;\, \delta_s \;\, | \; {}^rT_s \right] \in \mathbb{R}^4 \times SE(2) $$ The euclidean parameterization of the state is the following: $$ \Delta_X = \left[ \Delta_{X_k} \;|\; \Delta_{X_s} \right] = \left[K_s \;\, K_t \;\, a \;\, \delta_s \;\, | \;\, {}^rx_s \;\, {}^ry_s \;\, {}^r\delta_{s}\right] \in \mathbb{R}^7 $$ Given that \(X\) is composed by a pose, which belongs to a manifold, we need to define the box-plus operator. $$ X \oplus \Delta_X = \Delta_X \cdot X $$

The measurement \(Z\) is defined as the displacement of the sensor from the current instant to the next one. This value is computed starting from the global pose of the sensor given by the dataset. The definition of \(Z\) is the following: $$ Z = \left[ \Delta_s \right] \in SE(2) $$ Where the displacement of the sensor is defined as: $$ \Delta_s = {}^wT_{s_i}^{-1} \cdot {}^wT_{s_{i+1}} $$ Since we are dealing again with a manifold object, we need to specify the implementation of the box-minus operator. $$ Z_1 \ominus Z_2 = Z_2^{-1} \cdot Z_1 $$ This operator will be used later in the definition of the error function.

The prediction function is defined as how much the sensor moved from the last iteration based on the kinematic model that we have previously specified. $$ h(X, \, \Delta_r) = {}^rT_{s}^{-1} \cdot \Delta_r \cdot {}^rT_{s} $$ In the previous equation, \({}^rT_{s}\) is the relative pose of the sensor that we want to calibrate while \(\Delta_r\)is the infinitesimal movement of the robot from the current instant to the next one.

4) Least Squares Algorithm

Here, \(e_j\) is the error on the \(j\)-th sample, and \(J_j\) is the Jacobian matrix. The Outliers are discarded according to the Huber Loss.

5) Error and Jacobian Computation

The error function specifies the discrepancy between the prediction of the displacement of the sensor and the measured displacement. Since we are dealing with two poses, hence manifold objects, we need to use the box-minus operator instead of a simple subtraction. $$ e(X) = h(X, \, \Delta_r) \ominus Z = Z^{-1} \cdot h(X, \, \Delta_r) $$ One of the key element of the Least Squares algorithm is the Jacobian of the error computed with respect to the state \(X\). $$ J(X) = \frac{\partial e(X)}{\partial X} = \left[ J_{k} \;\, J_{s} \right] \in \mathbb{R}^{3 \times 7} $$ This jacobian has been computed numerically rather than simbolically in the following way. $$ J_i(X) = \frac{e(X \oplus \boldsymbol{\epsilon}) - e(X \ominus \boldsymbol{\epsilon})}{2 \, \epsilon} $$ Where \(J_i(X)\) is the \(i\)-th columns of the jacobian matrix, which is related to the \(i\)-th component of the state \(X\). The variable \(\epsilon\) represent the small perturbation applied to the current state \(X\). Notice that the perturbation vector \(\boldsymbol{\epsilon} \) is composed by all zeros and one \(\epsilon\) in the \(i\)-th position, hence it perturbs only one variable of the state at time. The implementation of the jacobian has been done with the following function: $$ \begin{array}{l} \textbf{compute_jacobian}(X,\, \Delta_r, \, Z): \\[0.8em] \textbf{For} \;\; i = 1, \ldots, \text{max_iterations}: \\ \qquad X^{+} \gets X \oplus \boldsymbol{\epsilon} \\ \qquad X^{-} \gets X \ominus \boldsymbol{\epsilon} \\[0.4em] \qquad \textbf{if} \;\, i < 4 \;\, (\text{kinematic_parameters}): \\ \qquad \qquad \Delta_+ \gets \text{model_prediction}(X^{+}, \, s_j, \, t_j, \, t_{j+1}) \\ \qquad \qquad \Delta_- \gets \text{model_prediction}(X^{-}, \, s_j, \, t_j, \, t_{j+1}) \\ \qquad \qquad h_+ \gets h(X, \, \Delta_+) \ominus Z \\ \qquad \qquad h_- \gets h(X, \, \Delta_-) \ominus Z \\ \qquad \textbf{else} \;\, (\text{sensor_pose}): \\ \qquad \qquad h_+ \gets h(X^+, \, \Delta_+) \ominus Z \\ \qquad \qquad h_- \gets h(X^-, \, \Delta_-) \ominus Z \\[0.4em] \qquad e_+ \gets h_+ \ominus Z \\ \qquad e_- \gets h_- \ominus Z \\[0.4em] J_i \gets \dfrac{e_+ - e_-}{2 \epsilon} \end{array} $$ Where the prediction function is computed with a different perturbed parameter at each iteration. The first equation shows the computation of the prediction when one of the kinematic parameters is perturbed, while the second when one of the element of the sensor pose is perturbed. $$ \qquad \qquad h(X, \, \Delta_{+/-}) = {}^rT_{s}^{-1} \cdot \Delta_{+/-} \cdot {}^rT_{s} \\ \qquad \qquad h(X^{+/-}, \, \Delta_+) = {}^rT_{s_{+/-}}^{-1} \cdot \Delta_r \cdot {}^rT_{s_{+/-}} \\ $$

6) Results