Improved Tracking and Docking of Industrial Mobile Robots Through UKF Vision-Based Kinematics Calibration

Performing an open-loop movement, or docking, for an industrial mobile robot (IMR), is a common necessary procedure when relying on environmental sensors is not possible. This procedure precision and outcome, solely depend on the IMR forward kinematic and odometry correctness, which is tied to the kinematics parameters, depending on the IMR kind. Calibrating the kinematic parameters of an IMR is a time consuming and mandatory procedure, since the mechanical tolerances and the assembly procedure may introduce a large variation from the nominal parameters. Furthermore, calibration inaccuracies might introduce severe inconsistencies in tasks such as localization, mapping, and navigation in general. In this work, we focus on the so-called kinematic parameter calibration. We propose the use of the unscented Kalman filter to perform a calibration procedure of the geometrical kinematic parameters of a mobile platform. The mobile platform is externally tracked during the calibration phase, using a fixed temporary external sensor that retrieves the position of a visual tag fixed to the platform. The unscented Kalman filter, using the calibration phase collected data, estimates the enlarged system state, which is comprised of the parameters that have to be estimated, the platform odometry and the visual tag position. The method can either be used online, to identify parameters and monitor their value while the system is operating, or offline, on logged data. We validate this method on two different devices, a 4 mecanum-wheel IMR, and a Turtlebot 3, using a camera to track the movement trough a reference chessboard, for then comparing the original path to its corrected version.


I. INTRODUCTION
Generally, industrial mechanical systems need to have a good parameter calibration to perform accordingly to the standard, hence, a calibration procedure is needed. This calibration procedure, or parameter estimation, is obtainable by means of general purpose algorithms, or specifically tailored methods for particular mechanical systems. Concerning IMRs, the value of the kinematic parameters incorporated in the model, hugely modify the performance of the system in all of its uses, from the mapping phase, where the IMR position infers the map conception, in navigation, where the planning algorithms use the IMR kinematics to devise a path and control the robot movements, and in localization, where the relative position and velocity are used to estimate the displacement progression in an environment. The overall impact The associate editor coordinating the review of this manuscript and approving it for publication was Halil Ersin Soken . of the IMR odometry correctness arises in specific cases such as docking, in which we cannot rely on localization algorithms (e.g. highly de-structured environments, dynamic environments or lacking of localization sensors), and high precision relative displacements are performed in open loop on the IMR kinematic model. Generally, the constant parameters that are taken into account in an IMR kinematic are the wheels radius, which is usually the same for all the wheels, and a geometric length that depends of the IMR structure (e.g omnidirectional mobile platform, car-like, three wheels). Several works related to the general subject of odometry calibration are present in the literature, some focusing on the intrinsic kinematic parameter calibration, some on the calibration of extrinsic parameters related to mounted sensors, or both problems simultaneously. The majority of the works present are designed for differential drive IMRs, while few are specific to omnidirectional mobile platforms, furthermore, most of the methods are offline and based on the repetition of fixed calibration paths. Offline calibration is more commonly employed in standard robotics, but is not suitable to systems where the kinematic parameters change significantly over time (inflatable wheels, robots with great dynamic loads, re-configurable mobile platforms), while online calibration requires real time capability in the measure system and in the computation, which might be hard to accomplish for complex vision related measure systems.
Works dealing with the topic of parameter estimation and calibration are vastly present in literature, for generic robotic chains, many classical methods have been devised, from a classical Denavit -Hartenberg approach [1], to rejectingsampling methods [2], virtually closed kinematic chains [3], and Kalman filtering based methods, such as in [4], [5], where Kalman theory is used alongside a particle filter to estimate serial manipulator parameters. Recent works [6]- [8] apply visual based measurement system for the estimation of manipulator parameters, [9] deals with a complete AXB = YCZ problem, while deep learning based methods have also been used extensively on the subject [10], [11]. For an extensive survey on the overall topic, refer to [12]. Despite IMRs can be treated as a generic kinematic chains, many works focus on the different kinds existing. On the subject, systematic odometry errors have been initially studied by [13]- [18], while more recent works on the subject use iterative learning approaches [19], least squares estimation [20], and 'Effective Kinematic Parameters' [21]. All the mentioned methods, follow a trajectory based optimization, where the aim is to minimize the positional error on a given goal path that is repeated constantly, and where the information needed are related only to the end points. In such works, the calibration performance is affected by the path parameters, hence the number of trials and the length of the paths [22], while the paths have to be manually designed and the measurements of the final error carried out manually, with the possibility to bring inaccuracies and measurement errors. Many works take in consideration a possible kinematic flaw, such has an unequal wheel diameter [23]- [25], generic optimization methods tackle the model inaccuracies by means of corrective coefficients [26], [27], while dynamic wheel model and lateral dynamics and taken into account in [28]. Specific works have also been carried out regarding the type of IMR: carlike robot [29]- [31], differential drive [22], [32] and omnidirectional robots [25], [27], [33]- [35]. Many methods based on Kalman filtering algorithms have been used, where the localization and calibration are solved as a single combined task, using the available sensors on the IMR [36]- [38]. The advantage in using Kalman filtering techniques is that the calibration can be depleted online, while the mobile platform performs its tasks. For an extensive survey on IMRs odometry calibration methods, refer to [39]. The contribution of this paper is to propose the usage of an external sensor system to track the IMR state, using a visual tag fixed on it, while reading the values of the wheels encoders, in order to perform a model parameter identification using the collected data, by means of an unscented Kalman filter. The usage of an external visual sensor as a tracking method has many advantages on the calibration system, nowadays industrial cameras reach a high level of fidelity, given by the combination of high resolution, advanced calibration techniques, and high frame rate, which allows a smooth tracking of the IMR movement. Furthermore, this method doesn't require the design of a specific path, nor requires hardware changes to the IMR, making it easily integrable in every scenario. In order to identify the parameters, a system model that includes the mobile platform position, velocity, and the relative tag position is needed, where the unknown relative position between the reference tag and the mobile platform reference system corresponds to an hand-eye problem, which parameters will be included in the system model and estimated.

A. MOBILE PLATFORM KINEMATIC AND ODOMETRY
Consider a generic IMR as shown in Fig 1, where W is the globally fixed world frame, MR is the frame fixed to the IMR, and is the transformation matrix, composed by the rotational R W MR and translational t W MR = [p x p y p z ] T parts, from the MR frame to the W . The robot position in the world is described by its translational and rotational components, hence the six element vector [p x , p y , p z , φ 1 , φ 2 , φ 3 ] = p W , prompt by the requirement of describing also rough and uneven terrains, uneven wheels diameters, and other environment irregularities.
The IMR model generally includes the j-th wheel radius r j ,with r = [r 0 , . . . , r j ], a geometric distance depending on the robot kind l, a general purpose parameter vector q, and the wheels rotational speedθ , composed by a j number of elementsθ = [θ 0 , . . . ,θ j ], computed by the wheels encoder sensors.
Depending on the specifics, the wheels radius might be assumed equal, or different for each wheel. Due to the digital nature of IMR controllers and encoders, the system will be described with a discrete time model. The IMR velocities v and ω at time t are expressed with respect to the MR frame as the vector as computed by the forward kinematic of the mobile robot: where the function f depends on the mobile platform type(e.g. for car-like robots v t y = 0), while the same velocities referred to the W are: where [·] X is the skew operator. Referred to the frame W at time t + 1, the position and the orientation (odometry) of the IMR is then computed as the discrete-time integral given dt as the update time, using position, orientation, linear velocity and angular velocity at time t (passing from angular velocity to Cardan angles is done accordingly to [40]).

B. UNSCENTED KALMAN FILTER
Consider a discrete time nonlinear dynamical system: where t is the time index, ζ is the state vector, u is the system input, y is the system output, and e and b are generic constant parameters. The model includes v and n which are respectively the state noise, in order to include model uncertainties, and the measurement noise, both are zero-mean white noises with known covariance matrix. Both the system dynamic model F and the output function G, are known, and include some unknown constant parameters that have to be estimated. In order to use the Kalman filtering theory to estimate the constant parameters, they have to be given a fake dynamic, and to be included in the state vector: where x is the enlarged state, and the parameters e and b, that have to be estimated, do not have an evolution because their value at instant t + 1 is the same as at instant t. Kalman filtering theory can be optimally used in linear dynamical systems to estimate the state value, while, in case of a nonlinear system, variants of the original filter exist, such as the extended Kalman filter (EKF) or the Unscented Kalman filter (UKF). Considering the strong non-linearity of the system, and the additional complexity brought by the hand-eye problem, an UKF is employed, due to its increased reliability in non-linear system identification [41], and robustness to parameter initial value [42]. While the inaccuracies of the EKF, come from the linearization of the dynamic model, the UKF keeps the original nonlinear dynamic model, using the unscented transform methodology [43] to propagate the state uncertainty. 1 Following [41], denote x ∈ R L ,x, P x as an L dimensional random variable, its mean value, and its co-variance matrix respectively, and denote y = g(x) as a generic nonlinear map. The distribution of y, its mean valueȳ and its co-variance P y can be approximated using a number 2L + 1 of sigma row vectors χ i , which form a matrix χ, and associated weights W i as following The set of vectors χ i and weights W i are computed as where (.) i extracts the i-th row of the resulting matrix, and λ = µ 2 (L + κ)−L is the gamma vectors scaling parameter, with µ, ν and κ tuned accordingly to the process. The UKF, extends the unscented transform, including it into the Kalman filter iterative estimation, the UKF steps are reported in algorithm 1, for an extensive survey, refer to [41].

III. METHODOLOGY
The system is comprised by an IMR endowed with wheel encoders, a visual tag fixed on the platform, and an external tracking camera to estimate the position of the visual tag, as shown in Fig 1. The system model includes the IMR kinematic and odometry, the tag frame position, the fixed transformation between the IMR frame and the tag frame, and the geometrical parameters related to the kinematic formulation.
Four reference systems are defined prior to the model, the frame W , a fixed global frame to which the mobile platforms position are referred, equal to MR, the mobile platform frame, at the starting time, and VW , the visual world fixed reference system, equal to V , the tag frame, at starting time, refer to Fig 2 for a system representation. The tag position results in where H V MR is the constant transformation matrix between the MR frame and the tag frame V , and T W MR|t is the odometry transformation.
x a is the concatenation of original state and noise variables 4: P v is the process noise co-variance matrix 5: P n i the measurement noise co-variance matrix 6: x − k is the optimal prediction of such variable.

23:
κ ← P x k ,y k P −1 y kỹk 24:x k ←x − k + κ(y k −ŷ − k ) 25: In order to properly include the H V MR and T WV V matrix in the model, its formulation from the Euler xyz angles is used: Hence, defining h and c as where the last elements are the constant parameters that have to be estimated: The measurable output y t of the system is the visual tag position, computed by an external tracking system as shown in Fig. 3. In order to retrieve the position of the mobile tag frame by means of the external sensor, the vision tag displacement at time t is always referred to the position at instant 0, using the following: The parameters are then estimated online, or offline, using the output readings and encoders value to evolve the model dynamic.

IV. TEST CASES A. DESIGN OF EXPERIMENTS
Tests have been performed in a 2D planar environment, with two different IMRs, one omnidirectional and one with differential drive, in order to assess its effectiveness in two VOLUME 9, 2021  different scenarios. As a reference, the omniwheel platform is shown in Fig. 4, while the differential drive Turtlebot 3 waffle is shown in Fig. 5. The parameters to be estimated are the radius of the platform wheels, considered to be equal for wheels of the same platform, and a geometric parameter, which is l xy for the omniwheel platform, and the parameter l d for the differential drive. The parameters are initialized with the CAD values for the omniwheel internally developed platform, and with data-sheet values for the differential drive commercial platform. The platforms have been teleoperated, using a joystick as a remote manual speed controller, along 10 different paths, each with an average duration of 50 seconds, as a reference, the nominal paths performed by the omnidirectional robot are shown in Fig. 6, we refer to the nominal path as the one measured by the external camera.
In order to ease the estimation, we suppose the tag frame to be planar to the platform xy frame plane (i.e. α, β = 0), and for both the frames to have the same z component in the world frame (i.e. h z = 0). All the needed data are logged during the runs, and then used offline to evolve the dynamic of the discrete-time system, at every step(time instant), and improve the estimation of the state, repeating the overall estimation procedure for a number of iterations. The data is shuffled between each estimation trial, where half of the data are used to simulate the model and estimate the parameters, while the other half to validate the trial and assess improvement. Parameters relative to the UKF are tuned according to [43], [44], specifically the values relative to the Unscented transform are: number of points (14), α(0.1), β(2.0) and κ(-11). Regarding the initial covariances of the UKF, the numerous trials we performed offline suggested that the initial values mostly affect the convergence time, and not the actual estimation value. Specifically, the value of the state covariance matrix P is set to 0.01 * I , where I is the identity matrix, the state noise covariance matrix Q is set to 0.01 * I , aside from the values corresponding to parameters that have to be estimated, which is set to zero, while the output noise covariance matrix R is 0.0025 * I , which has been approximately computed using the camera resolution, the camera field of view and the average distance from camera to the tracked object. The identification algorithm is run in two different forms: • parallelly, performing an estimation for each of 5 estimation data sets, using 10 iterations of the whole data per run, for then taking the average of the values. In order to assess the convergence to common values of the estimations on the data sets.
• on the 5 data sets in series, for 10 iterations, for having a single estimate of the parameters furthermore, the outcomes are compared, to establish the more appropriate approach to the problem. Each estimation is repeated 50 times, randomly shuffling the estimation and validation paths in order to have statistically relevant results.
The IMRs controllers are synchronized with the camera acquisition system using the NTP protocol. Both robots are moved at a maximum velocity of 0.15 m s , while externally tracked at 30Hz, using a calibrated Kinect V2 camera, hence allowing a maximum displacement of 0.005m per frame. Specifically, the Kinect V2 camera has a 1920 x 1080 RGB resolution, and, most importantly, adopt a global shutter, which reduces blurs and improves the measure quality. The wheels encoders are then down-sampled at 30 Hz in order to have the same time scale. The relative position between the tag and the camera is computed using the perspective-n-point algorithm in the OpenCv libraries [45]. For the algorithm development, Python3 has been used, while the UKF library is provided by [46]. The estimation algorithm has been run using an Intel Core i7-7700HQ processor. Given this setup, the running time of a single estimation instance on the offline data, for both methods, was of 35 seconds. In order to measure the performance improvements, 2 indexes will be used, one that takes in account the whole movement along the paths, and one which aim is to measure the docking precision, that relies only on the final position after a path is performed. We define a path P . = {p 0 , . . . , p N p } as an ordered collection of IMR positions p, where each position is composed by two translational parts(x, y) and one rotational part(ω), being our experiment setup modelled in a 2D environment. Then, we will define a nominal path P n , as the one measured by the external camera, an original path P o , as the one computed using the initial parameters values by the IMR kinematics, and a corrected P c path, as the one re-computed with the newly estimated parameters. In order to measure the improvement given by the estimation of the parameters, we compare the different paths, using the summation of the euclidean distance between each j − th point P(j) = p j in P o and P c ,  with respect to the j − th point in P n as: (19) where N p is the equal number of points of the paths, for then computing the percentage improvement of the corrected path as

B. TEST CASE A -OMNIWHEEL MOBILE PLATFORM
The test is performed on an omniwheel mobile platform, endowed with 4 mecanum wheels with an axis of rotation at 45 • to the wheel plane and at 45 • to the axle line, each wheel has a high frequency (1kHz) encoder to compute the speed and position. For this specific robot, equations 2 and 3 take this form: where l xy is half-distance between front wheels and rear wheels plus half-distance between left wheels and the right  wheels, refer to [47] for a complete survey on mobile robot kinematics.
Regarding the parallel estimation approach, Fig. 7 shows the convergence of the radius estimation, while Fig. 8 shows the relation between the nominal, original and corrected paths for the test dataset, in the same way, for the series approach, Fig. 9 shows the convergence of the radius and l xy estimations, while Fig. 10 shows the paths comparison.

C. TEST CASE B -DIFFERENTIAL DRIVE MOBILE PLATFORM
The test is performed on a Turtlebot3 Waffle commercial mobile platform, endowed with 2 active wheels (100Hz encoders) and 2 ball casters.    For this specific robot, equation 2 takes this form: (21) where l d is the distance between left wheel and right wheel, refer to [47] for a complete survey on mobile robot kinematics.
An example of the radius value trend during the parallel estimation in this case is shown in Fig. 11.

D. RESULTS
The results are presented in Table 2 for the omniwheel robot, and in Table 3 for the Turtlebot3 waffle, where Std refers to the standard deviation of the estimated parameter, and σ c ,r and l . are the mean values, over the 50 trials. While the overall odometry error decreased in both cases, it is noticeable that the series estimation brought better results in both the experiments. The estimation in the series case, is more precise, due to higher σ c , and more reliable, due to a smaller Std r and Std l . . In order to assess the docking capability improvement, we also compare the final position distance between the nominal path, the original computed  odometry and the corrected one, using the same concept as in 20, the results are shown in 1. The overall resulted accuracy is higher in the omniwheel robot, due to a better manufacture and devices quality.

V. CONCLUSION AND FURTHER DEVELOPMENT
In this work, a novel method to improve the IMR docking quality and calibrate the kinematic parameters of a mobile platform is presented. An Unscented Kalman Filter is used to estimate the parameters of the dynamic model of the mobile platform, tracked using an external camera, and wheels encoders. The method is tested on two different IMRs, gathered results show a positive outcome. In order to improve the method, future works will focus on filtering the data, to raise the value of information-rich data, and filter out the data that is deemed not important. This approach can be pursued using the Fisher information theory. Furthermore, the method will be tested in a 3D scenario, including problematics related to 3D outdoor navigation and mobile platforms with suspensions. Other improvements can be done to lessen the computation time, in order to make the algorithm more suitable for online use. Since 2015, he has been coordinating the activity of the Robot Motion Control and Robotized Processes Laboratory, CNR-STIIMA. His research interest includes control techniques for industrial manipulators in advanced application requiring the interaction robot-environment, such as technological tasks, or robothuman operator, such as workspace sharing, teach-by-demonstration. He is involved in researches for accurate elastic modeling and dynamic calibration of industrial robots. VOLUME 9, 2021