Optimal path planning for nonholonomic robotics systems via parametric optimisation

Abstract. Motivated by the path planning problem for robotic systems this paper considers nonholonomic path planning on the Euclidean group of motions SE(n) which describes a rigid bodies path in n-dimensional Euclidean space. The problem is formulated as a constrained optimal kinematic control problem where the cost function to be minimised is a quadratic function of translational and angular velocity inputs. An application of the Maximum Principle of optimal control leads to a set of Hamiltonian vector field that define the necessary conditions for optimality and consequently the optimal velocity history of the trajectory. It is illustrated that the systems are always integrable when n = 2 and in some cases when n = 3. However, if they are not integrable in the most general form of the cost function they can be rendered integrable by considering special cases. This implies that it is possible to reduce the kinematic system to a class of curves defined analytically. If the optimal motions can be expressed analytically in closed form then the path planning problem is reduced to one of parameter optimisation where the parameters are optimised to match prescribed boundary conditions.This reduction procedure is illustrated for a simple wheeled robot with a sliding constraint and a conventional slender underwater vehicle whose velocity in the lateral directions are constrained due to viscous damping

Automatic Differentiation of Rigid Body Dynamics for Optimal Control and Estimation

Many algorithms for control, optimization and estimation in robotics depend on derivatives of the underlying system dynamics, e.g. to compute linearizations, sensitivities or gradient directions. However, we show that when dealing with Rigid Body Dynamics, these derivatives are difficult to derive analytically and to implement efficiently. To overcome this issue, we extend the modelling tool `RobCoGen' to be compatible with Automatic Differentiation. Additionally, we propose how to automatically obtain the derivatives and generate highly efficient source code. We highlight the flexibility and performance of the approach in two application examples. First, we show a Trajectory Optimization example for the quadrupedal robot HyQ, which employs auto-differentiation on the dynamics including a contact model. Second, we present a hardware experiment in which a 6 DoF robotic arm avoids a randomly moving obstacle in a go-to task by fast, dynamic replanning

Path planning for simple wheeled robots : sub-Riemannian and elastic curves on SE(2)

This paper presents a motion planning method for a simple wheeled robot in two cases: (i) where translational and rotational speeds are arbitrary and (ii) where the robot is constrained to move forwards at unit speed. The motions are generated by formulating a constrained optimal control problem on the Special Euclidean group SE(2). An application of Pontryagin’s maximum principle for arbitrary speeds yields an optimal Hamiltonian which is completely integrable in terms of Jacobi elliptic functions. In the unit speed case, the rotational velocity is described in terms of elliptic integrals and the expression for the position reduced to quadratures. Reachable sets are defined in the arbitrary speed case and a numerical plot of the time-limited reachable sets presented for the unit speed case. The resulting analytical functions for the position and orientation of the robot can be parametrically optimised to match prescribed target states within the reachable sets. The method is shown to be easily adapted to obstacle avoidance for static obstacles in a known environment

Dynamic Active Constraints for Surgical Robots using Vector Field Inequalities

Robotic assistance allows surgeons to perform dexterous and tremor-free procedures, but robotic aid is still underrepresented in procedures with constrained workspaces, such as deep brain neurosurgery and endonasal surgery. In these procedures, surgeons have restricted vision to areas near the surgical tooltips, which increases the risk of unexpected collisions between the shafts of the instruments and their surroundings. In this work, our vector-field-inequalities method is extended to provide dynamic active-constraints to any number of robots and moving objects sharing the same workspace. The method is evaluated with experiments and simulations in which robot tools have to avoid collisions autonomously and in real-time, in a constrained endonasal surgical environment. Simulations show that with our method the combined trajectory error of two robotic systems is optimal. Experiments using a real robotic system show that the method can autonomously prevent collisions between the moving robots themselves and between the robots and the environment. Moreover, the framework is also successfully verified under teleoperation with tool-tissue interactions.Comment: Accepted on T-RO 2019, 19 Page