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 poses 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 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 $$
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) = {}^rT_{s}^{-1} \cdot \Delta_r \cdot {}^rT_{s} $$
4) Error and Jacobian Computation
The error function specifies the discrepancy between the prediction of the displacement of the sensor and the measured displacement. $$ e(X) = h(X) \ominus Z = Z^{-1} \cdot h(X) $$ 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} $$
5) Least Squares Algorithm
6) Results
The algorithm has been tested with different parameters, but the following one are the one that leads to a better solution of the problem. The following plots show the behavior of the error squared norm and the number of outliers over all the simulation.
