Showing papers by "Ivan Petrović published in 2017"

Receding Horizon Control for Convergent Navigation of a Differential Drive Mobile Robot

Abstract: A receding horizon control (RHC) algorithm for convergent navigation of a differential drive mobile robot is proposed. Its objective function utilizes a local-minima-free navigation function to measure the cost-to-goal over the robot trajectory. The navigation function is derived from the path-search algorithm over a discretized 2-D search space. The proposed RHC navigation algorithm includes a systematic procedure for the generation of feasible control sequences. The optimal value of the objective function is employed as a Lyapunov function to prove a finite-time convergence of the discrete-time nonlinear closed-loop system to the goal state. The developed RHC navigation algorithm inherits fast replanning capability from the $D$ * search algorithm, which is experimentally verified in changing indoor environments. The performance of the developed RHC navigation algorithm is compared with the state-of-the-art sample-based motion planning algorithm based on lattice graphs, which is combined with a trajectory tracking controller. The RHC navigation algorithm produces faster motion to the goal with significantly lower computational costs and it does not need any controller tuning to cope with diverse obstacle configurations.

Extrinsic 6DoF calibration of 3D LiDAR and radar

Abstract: Environment perception is a key component of any autonomous system and is often based on a heterogeneous set of sensors and fusion thereof, for which extrinsic sensor calibration plays fundamental role. In this paper, we tackle the problem of 3D LiDAR-radar calibration which is challenging due to low accuracy and sparse informativeness of the radar measurements. We propose a complementary calibration target design suitable for both sensors, thus enabling a simple, yet reliable calibration procedure. The calibration method is composed of correspondence registration and a two-step optimization. The first step, reprojection error based optimization, provides initial estimate of the calibration parameters, while the second step, field of view optimization, uses additional information from the radar cross section measurements and the nominal field of view to refine the parameters. In the end, results of the experiments validated the proposed method and demonstrated how the two steps combined provide an improved estimate of extrinsic calibration parameters.

Human motion estimation on Lie groups using IMU measurements

Abstract: This paper proposes a new algorithm for human motion estimation using inertial measurement unit (IMU) measurements. We model the joints by matrix Lie groups, namely the special orthogonal groups SO(2) and SO(3), representing rotations in 2D and 3D space, respectively. The state space is defined by the Cartesian product of the rotation groups and their velocities and accelerations, given a kinematic model of the articulated body. In order to estimate the state, we propose the Lie Group Extended Kalman Filter (LG-EKF), thus explicitly accounting for the non-Euclidean geometry of the state space, and we derive the LG-EKF recursion for articulated motion estimation based on IMU measurements. The performance of the proposed algorithm is compared to the EKF based on Euler angle parametrization in both simulation and real-world experiments. The results show that for motion near gimbal lock regions, which is common for shoulder movement, the proposed filter is a significant improvement over the Euler angles EKF.

Time-optimal velocity planning along predefined path for static formations of mobile robots

Abstract: This paper is concerned with the problem of finding a time-optimal velocity profile along the predefined path for static formations of mobile robots in order to traverse the path in shortest time and to satisfy, for each mobile robot in the formation, velocity, acceleration, tip over and wheel slip prevention constraints. Time-optimal velocity planning is achieved using so called bang-bang control where minimum and maximum accelerations of the formation are alternating. The developed trajectory planning algorithm is demonstrated on the formation of differential drive mobile robots.

Score matching based assumed density filtering with the von Mises-Fisher distribution

Abstract: Bayesian filters are often used in statistical inference and consist of recursively alternating between two steps: prediction and correction. Most commonly the Gaussian distribution is used within the Bayes filtering framework, but other distributions, which could model better the nature of the estimated phenomenon like the von Mises-Fisher distribution on the unit sphere, have also been subject of research interest. However, the von Mises-Fisher filter requires approximations since the prediction step does not yield an another von Mises-Fisher distribution. Furthermore, other advanced filtering methods require approximating a mixture of distributions with just a single component. In this paper we propose to use the score matching within the context of Bayesian assumed density filtering inlieu of the more common moment matching. Moment matching functions by assuming the type of the resulting distribution and then matching its moments with the prior distribution, which in the end minimizes the Kullback-Leibler divergence. Score matching also assumes the resulting distribution type, but finds optimal parameters by minimizing the relative Fisher information. In the paper we show that the score matching procedure results with identical performance, but with simpler equations that, unlike moment matching, do not require tedious numerical methods. In the end, we corroborate theoretical results by running the moment and score matching based filters for single and multiple object tracking on a large number of randomly generated trajectories on the unit sphere.

Mixture Reduction on Matrix Lie Groups

Abstract: Many physical systems evolved on matrix Lie groups and mixture filtering designed for such manifolds represent an inevitable tool for challenging estimation problems. However, mixture filtering faces the issue of a constantly growing number of components, hence requiring appropriate mixture reduction techniques. In this letter, we propose a mixture reduction approach for distributions on matrix Lie groups, called the concentrated Gaussian distributions (CGDs). This entails appropriate reparameterization of CGD parameters to compute the KL divergence, pick and merge the mixture components. Furthermore, we also introduce a multitarget tracking filter on Lie groups as a mixture filtering study example for the proposed reduction method. In particular, we implemented the probability hypothesis density filter on matrix Lie groups. We validate the filter performance using the optimal subpattern assignment metric on a synthetic dataset consisting of 100 randomly generated multitarget scenarios.

Mixture Reduction on Matrix Lie Groups

Abstract: Many physical systems evolve on matrix Lie groups and mixture filtering designed for such manifolds represent an inevitable tool for challenging estimation problems. However, mixture filtering faces the issue of a constantly growing number of components, hence require appropriate mixture reduction techniques. In this letter we propose a mixture reduction approach for distributions on matrix Lie groups, called the concentrated Gaussian distributions (CGDs). This entails appropriate reparametrization of CGD parameters to compute the KL divergence, pick and merge the mixture components. Furthermore, we also introduce a multitarget tracking filter on Lie groups as a mixture filtering study example for the proposed reduction method. In particular, we implemented the probability hypothesis density filter on matrix Lie groups. We validate the filter performance using the optimal subpattern assignment metric on a synthetic dataset consisting of 100 randomly generated multitarget scenarios.

Human Intention Recognition in Flexible Robotized Warehouses based on Markov Decision Processes

Abstract: The rapid growth of e-commerce increases the need for larger warehouses and their automation, thus using robots as assistants to human workers becomes a priority. In order to operate efficiently and safely, robot assistants or the supervising system should recognize human intentions. Theory of mind (ToM) is an intuitive conception of other agents’ mental state, i.e., beliefs and desires, and how they cause behavior. In this paper we present a ToM-based algorithm for human intention recognition in flexible robotized warehouses. We have placed the warehouse worker in a simulated 2D environment with three potential goals. We observe agent’s actions and validate them with respect to the goal locations using a Markov decision process framework. Those observations are then processed by the proposed hidden Markov model framework which estimated agent’s desires. We demonstrate that the proposed framework predicts human warehouse worker’s desires in an intuitive manner and in the end we discuss the simulation results.

Revival of filtering based SLAM? Exactly sparse delayed state filter on Lie groups

Abstract: Simultaneous localization and mapping (SLAM) is a core element of every autonomous mobile robot. The underlying engine of a SLAM system is its back-end, which aims at optimally estimating the trajectory and map of the environment based on sensor data abstractions. Over the past decade, SLAM solutions based on graph optimization approaches prevailed over the filtering based solutions, since they dominated in performance over a wider range of applications. In this paper we propose a novel filtering based SLAM back-end based on the exactly sparse delayed state filter (ESDSF) derived on Lie groups (LG-ESDSF). The proposed filter retains all the good characteristics of the classic ESDSF, but also respects the state space geometry by employing filtering equations directly on Lie groups. We have compared our SLAM system with two current state-of-the-art SLAM solutions, namely ORB-SLAM and LSD-SLAM, on the KITTI vision benchmark suite. Test results show that the proposed SLAM based on the LG-ESDSF back-end can achieve same level of accuracy as the methods based on the graph optimization techniques, while maintaining lower computation times.

On wrapping the Kalman filter and estimating with the SO(2) group

Abstract: This paper analyzes directional tracking in 2D with the extended Kalman filter on Lie groups (LG-EKF). The study stems from the problem of tracking objects moving in 2D Euclidean space, with the observer measuring direction only, thus rendering the measurement space and object position on the circle---a non-Euclidean geometry. The problem is further inconvenienced if we need to include higher-order dynamics in the state space, like angular velocity which is a Euclidean variables. The LG-EKF offers a solution to this issue by modeling the state space as a Lie group or combination thereof, e.g., SO(2) or its combinations with Rn. In the present paper, we first derive the LG-EKF on SO(2) and subsequently show that this derivation, based on the mathematically grounded framework of filtering on Lie groups, yields the same result as heuristically wrapping the angular variable within the EKF framework. This result applies only to the SO(2) and SO(2)xRn LG-EKFs and is not intended to be extended to other Lie groups or combinations thereof. In the end, we showcase the SO(2)xR2 LG-EKF, as an example of a constant angular acceleration model, on the problem of speaker tracking with a microphone array for which real-world experiments are conducted and accuracy is evaluated with ground truth data obtained by a motion capture system.

Dense Disparity Estimation in Ego-motion Reduced Search Space

Abstract: Depth estimation from stereo images remains a challenge even though studied for decades. The KITTI benchmark shows that the state-of-the-art solutions offer accurate depth estimation, but are still computationally complex and often require a GPU or FPGA implementation. In this paper we aim at increasing the accuracy of depth map estimation and reducing the computational complexity by using information from previous frames. We propose to transform the disparity map of the previous frame into the current frame, relying on the estimated ego-motion, and use this map as the prediction for the Kalman filter in the disparity space. Then, we update the predicted disparity map using the newly matched one. This way we reduce disparity search space and flickering between consecutive frames, thus increasing the computational efficiency of the algorithm. In the end, we validate the proposed approach on real-world data from the KITTI benchmark suite and show that the proposed algorithm yields more accurate results, while at the same time reducing the disparity search space.

Cooperative Cloud SLAM on Matrix Lie Groups

Abstract: In this paper we present a Cooperative Cloud SLAM on Matrix Lie Groups (\(\text {C}^2\text {LEARS}\)), which enables efficient and accurate execution of simultaneous localization and environment mapping, while relying on integration of data from multiple agents. Such fused information is then used to increase mapping accuracy of every agent itself. In particular, the agents perform only computationally simpler tasks including local map building and single trajectory optimization. At the same time, the efficient execution is ensured by performing complex tasks of global map building and multiple trajectory optimization on a standalone cloud server. The front-end part of \(\text {C}^2\text {LEARS}\) is based on a planar SLAM solution, while the back-end is implemented using the exactly sparse delayed state filter on matrix Lie groups (LG-ESDSF). The main advantages of the front-end employing planar surfaces to represent the environment are significantly lower memory requirements and possibility of the efficient map exchange between agents. The back-end relying on the LG-ESDSF allows for efficient trajectory optimization utilizing sparsity of the information form and exploiting higher accuracy supported by representing the state on Lie groups. We demonstrate \(\text {C}^2\text {LEARS}\) on a real-world experiment recorded on the ground floor of our faculty building.

Determination of wagonaxels quality by NDT methods

Abstract: U radu se obrađuje problematika utvrđivanja pojave gresaka u unutrasnjosti osovine. Ovakva uocena ostecenja osovina poznata su pod nazivom kovacka zvijezda koje mogu dovesti do raspadanja osovine, uzrokujuci teske posljedice u željeznickom prometu. U okviru istraživanja provedena je detaljna ultrazvucna kontrola pregleda vagonskih osovina u mreži hrvatskih željeznica, s ciljem smanjenja opasnosti od pojave napuknuca osovina uslijed umora materijala. Dobiveni rezultati mogu poslužiti kao podloga za izradu tehnickih propisa za održavanje željeznickih strojeva u mreži hrvatskih željeznica.

Moving object tracking employing rigid body motion on matrix Lie groups

Abstract: In this paper we propose a novel method for estimating rigid body motion by modeling the object state directly in the space of the rigid body motion group SE(2). It has been recently observed that a noisy manoeuvring object in SE(2) exhibits banana-shaped probability density contours in its pose. For this reason, we propose and investigate two state space models for moving object tracking: (i) a direct product SE(2)xR3 and (ii) a direct product of the two rigid body motion groups SE(2)xSE(2). The first term within these two state space constructions describes the current pose of the rigid body, while the second one employs its second order dynamics, i.e., the velocities. By this, we gain the flexibility of tracking omnidirectional motion in the vein of a constant velocity model, but also accounting for the dynamics in the rotation component. Since the SE(2) group is a matrix Lie group, we solve this problem by using the extended Kalman filter on matrix Lie groups and provide a detailed derivation of the proposed filters. We analyze the performance of the filters on a large number of synthetic trajectories and compare them with (i) the extended Kalman filter based constant velocity and turn rate model and (ii) the linear Kalman filter based constant velocity model. The results show that the proposed filters outperform the other two filters on a wide spectrum of types of motion.