Inverse Kinematics for Serial Kinematic Chains via Sum of Squares Optimization

Inverse kinematics is a fundamental problem for articulated robots: fast and accurate algorithms are needed for translating task-related workspace constraints and goals into feasible joint configurations. In general, inverse kinematics for serial kinematic chains is a difficult nonlinear problem, for which closed form solutions cannot be easily obtained. Therefore, computationally efficient numerical methods that can be adapted to a general class of manipulators are of great importance. % to motion planning and workspace generation tasks. In this paper, we use convex optimization techniques to solve the inverse kinematics problem with joint limit constraints for highly redundant serial kinematic chains with spherical joints in two and three dimensions. This is accomplished through a novel formulation of inverse kinematics as a nearest point problem, and with a fast sum of squares solver that exploits the sparsity of kinematic constraints for serial manipulators. Our method has the advantages of post-hoc certification of global optimality and a runtime that scales polynomialy with the number of degrees of freedom. Additionally, we prove that our convex relaxation leads to a globally optimal solution when certain conditions are met, and demonstrate empirically that these conditions are common and represent many practical instances. Finally, we provide an open source implementation of our algorithm.

Mixture Reduction on Matrix Lie Groups

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.

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

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.

Moving object tracking employing rigid body motion on matrix Lie groups

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.