Kinematic state estimation and motion planning for stochastic nonholonomic systems using the exponential map

Abstract

SUMMARYA nonholonomic system subjected to external noise from the environment, or internal noise in its own actuators, will evolve in a stochastic manner described by an ensemble of trajectories. This ensemble of trajectories is equivalent to the solution of a Fokker-Planck equation that typically evolves on a Lie group. If the most likely state of such a system is to be estimated, and plans for subsequent motions from the current state are to be made so as to move the system to a desired state with high probability, then modeling how the probability density of the system evolves is critical. Methods for solving Fokker-Planck equations that evolve on Lie groups then become important. Such equations can be solved using the operational properties of group Fourier transforms in which irreducible unitary representation (IUR) matrices play a critical role. Therefore, we develop a simple approach for the numerical approximation of all the IUR matrices for two of the groups of most interest in robotics: the rotation group in three-dimensional space, SO(3), and the Euclidean motion group of the plane, SE(2). This approach uses the exponential mapping from the Lie algebras of these groups, and takes advantage of the sparse nature of the Lie algebra representation matrices. Other techniques for density estimation on groups are also explored. The computed densities are applied in the context of probabilistic path planning for kinematic cart in the plane and flexible needle steering in three-dimensional space. In these examples the injection of artificial noise into the computational models (rather than noise in the actual physical systems) serves as a tool to search the configuration spaces and plan paths. Finally, we illustrate how density estimation problems arise in the characterization of physical noise in orientational sensors such as gyroscopes.

Kalman filtering with a class of geometric state equality constraints

“…rather than (15), then it is said to be right-equivariant, and one should use a right-invariant EKF (RIEKF), see [4]. The results remain entirely valid then, by symmetry.…”

“…However, due to the nonlinear nature of the navigation equations, and in particular to the fact the orientation of the aircraft (i.e., the attitude) does not live in a vector space, the EKF may have some shortcomings. This has motivated the development of alternative filters, especially for attitude estimation, see e.g., [12], [19], [6], [15], [13], [10], [16], [20], [7].…”

Robotic Needle Steering: Design, Modeling, Planning, and Image Guidance

“…However, a needle that is repeatedly inserted into a medium, such as a gelatin used to simulate tissue [70], will demonstrate an ensemble of slightly different trajectories. A simple stochastic model [54,55] is obtained by adding noise to the two input parameters in the ideal model:…”

“…The trajectories will be slightly different from each other due to uncertainty that may exist in the control mechanism and the interaction between the needle and the tissue. Park et al [54,55] developed such a path planning method for needle steering that actively utilizes this stochastic behavior of the flexible needles. This algorithm is an adapted version of the path-of-probability (POP) algorithm in [25].…”

“…Rapid evaluation of the probability function is required for fast path planning. Specifically, the probability density is estimated by a Gaussian function [55,57], and the mean and covariance are estimated using error propagation techniques developed for the motion groups [68,69]. …”