Rigid body attitude estimation based on the Lagrange-d'Alembert principle

Estimation of rigid body attitude and angular velocity without any knowledge of the attitude dynamics model is treated using the Lagrange-d'Alembert principle from variational mechanics. It is shown that Wahba's cost function for attitude determination from two or more non-collinear vector measurements can be generalized and represented as a Morse function of the attitude estimation error on the Lie group of rigid body rotations. With body-fixed sensor measurements of direction vectors and angular velocity, a Lagrangian is obtained as the difference between a kinetic energy-like term that is quadratic in the angular velocity estimation error and an artificial potential obtained from Wahba's function. An additional dissipation term that depends on the angular velocity estimation error is introduced, and the Lagrange-d'Alembert principle is applied to the Lagrangian with this dissipation. A Lyapunov analysis shows that the state estimation scheme so obtained provides stable asymptotic convergence of state estimates to actual states in the absence of measurement noise, with an almost global domain of attraction. These estimation schemes are discretized for computer implementation using discrete variational mechanics. A first order Lie group variational integrator is obtained as a discrete-time implementation. In the presence of bounded measurement noise, numerical simulations show that the estimated states converge to a bounded neighborhood of the actual states.

[1]  Vijay Kumar,et al.  Vision-Based State Estimation and Trajectory Control Towards High-Speed Flight with a Quadrotor , 2013, Robotics: Science and Systems.

[2]  Vijay Kumar,et al.  Vision-based state estimation for autonomous rotorcraft MAVs in complex environments , 2013, 2013 IEEE International Conference on Robotics and Automation.

[3]  Amit K. Sanyal,et al.  An Almost Global Tracking Control Scheme for Maneuverable Autonomous Vehicles and its Discretization , 2011, IEEE Transactions on Automatic Control.

[4]  Carlos Silvestre,et al.  A Nonlinear GPS/IMU based observer for rigid body attitude and position estimation , 2008, 2008 47th IEEE Conference on Decision and Control.

[5]  A. Sanyal.  Optimal Attitude Estimation and Filtering Without Using Local Coordinates Part I: Uncontrolled and Deterministic Attitude Dynamics , 2005, 2006 American Control Conference.

[6]  Daewon Lee,et al.  Geometric nonlinear PID control of a quadrotor UAV on SE(3) , 2013, 2013 European Control Conference (ECC).

[7]  D. Bernstein,et al.  Inertia-Free Spacecraft Attitude Tracking with Disturbance Rejection and Almost Global Stabilization , 2009 .

[8]  Robert E. Mahony,et al.  Recursive attitude estimation in the presence of multi-rate and multi-delay vector measurements , 2015, 2015 American Control Conference (ACC).

[9]  Bijoy K. Ghosh,et al.  Pose estimation using line-based dynamic vision and inertial sensors , 2003, IEEE Trans. Autom. Control..

[10]  Alexander B. Miller,et al.  Tracking of the UAV trajectory on the basis of bearing-only observations , 2014, 53rd IEEE Conference on Decision and Control.

[11]  Timothy W. McLain,et al.  Relative Navigation Approach for Vision-Based Aerial GPS-Denied Navigation , 2013, Journal of Intelligent & Robotic Systems.

[12]  Philippe Martin,et al.  Non-Linear Symmetry-Preserving Observers on Lie Groups , 2007, IEEE Transactions on Automatic Control.

[13]  K. S. Amelin,et al.  An algorithm for refinement of the position of a light UAV on the basis of Kalman filtering of bearing measurements , 2014 .

[14]  A. Tayebi,et al.  Inertial measurements Based dynamic attitude estimation and velocity-free attitude stabilization , 2011, Proceedings of the 2011 American Control Conference.

[15]  Paul William Coote,et al.  Near-optimal deterministic filtering on the unit circle , 2009, Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference.

[16]  H. C. Corben,et al.  Classical Mechanics (2nd ed.) , 1961 .

[17]  Rita Cunha,et al.  A nonlinear position and attitude observer on SE(3) using landmark measurements , 2010, Syst. Control. Lett..

[18]  N. McClamroch,et al.  Lie group variational integrators for the full body problem , 2005, math/0508365.

[19]  G. Wahba.  A Least Squares Estimate of Satellite Attitude , 1965 .

[20]  Amit K. Sanyal,et al.  Attitude State Estimation with Multirate Measurements for Almost Global Attitude Feedback Tracking , 2012 .

[21]  Alexander B. Miller,et al.  Stochastic control of light UAV at landing with the aid of bearing-only observations , 2015, International Conference on Machine Vision.

[22]  H. D. Black,et al.  A passive system for determining the attitude of a satellite , 1964 .

[23]  Mohammad Zamani,et al.  Deterministic Attitude and Pose Filtering, an Embedded Lie Groups Approach , 2013 .

[24]  N. McClamroch,et al.  A lie group variational integrator for the attitude dynamics of a rigid body with applications to the 3D pendulum , 2005, Proceedings of 2005 IEEE Conference on Control Applications, 2005. CCA 2005..

[25]  Amit K. Sanyal,et al.  Discrete-time rigid body attitude state estimation based on the discrete Lagrange-d'Alembert principle , 2015, 2015 American Control Conference (ACC).

[26]  João Pedro Hespanha,et al.  Minimum-energy state estimation for systems with perspective outputs , 2006, IEEE Transactions on Automatic Control.

[27]  J. Stuelpnagel,et al.  A Least Squares Estimate of Satellite Attitude (Grace Wahba) , 1966 .

[28]  J. Marsden,et al.  Discrete mechanics and variational integrators , 2001, Acta Numerica.

[29]  Donald E. Kirk,et al.  Optimal control theory : an introduction , 1970 .

[30]  Taeyoung Lee,et al.  Global optimal attitude estimation using uncertainty ellipsoids , 2006, Syst. Control. Lett..

[31]  Robert E. Mahony,et al.  Minimum-Energy Filtering for Attitude Estimation , 2013, IEEE Transactions on Automatic Control.

[32]  Robert E. Mahony,et al.  Near-Optimal Deterministic Filtering on the Rotation Group , 2011, IEEE Transactions on Automatic Control.

[33]  F. Markley.  Attitude determination using vector observations and the singular value decomposition , 1988 .

[34]  R. Mortensen.  Maximum-likelihood recursive nonlinear filtering , 1968 .

[35]  Ernest Barany,et al.  Rigid body motion estimation based on the Lagrange-d'Alembert principle , 2015, 2015 54th IEEE Conference on Decision and Control (CDC).

[36]  F. Landis Markley,et al.  Attitude Filtering on SO(3) , 2006 .

[37]  J. W. Humberston.  Classical mechanics , 1980, Nature.

[38]  Vijay Kumar,et al.  Comparison of an attitude estimator based on the Lagrange-d'Alembert principle with some state-of-the-art filters , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[39]  Robert E. Mahony,et al.  Nonlinear Complementary Filters on the Special Orthogonal Group , 2008, IEEE Transactions on Automatic Control.

[40]  Dmitry P. Nikolaev,et al.  UAV Control on the Basis of 3D Landmark Bearing-Only Observations , 2015, Sensors.

[41]  Jerrold E. Marsden,et al.  Introduction to Mechanics and Symmetry: A Basic Exposition of Classical Mechanical Systems , 1999 .

[42]  A. Sanyal,et al.  Almost Global Robust Attitude Tracking Control of Spacecraft in Gravity , 2008 .

[43]  Amit K. Sanyal,et al.  Determination of relative motion of a space object from simultaneous measurements of range and range rate , 2014, 2014 American Control Conference.

[44]  Robert E. Mahony,et al.  Observer design on the Special Euclidean group SE(3) , 2011, IEEE Conference on Decision and Control and European Control Conference.

[45]  D. H. S. Maithripala,et al.  An intrinsic observer for a class of simple mechanical systems on a Lie group , 2004, Proceedings of the 2004 American Control Conference.

[46]  A. Bloch,et al.  Nonholonomic Mechanics and Control , 2004, IEEE Transactions on Automatic Control.

[47]  Ravi N. Banavar,et al.  Almost global attitude stabilization of a rigid body for both internal and external actuation schemes , 2013, 2013 European Control Conference (ECC).

[48]  L. Einkemmer.  Structure preserving numerical methods for the Vlasov equation , 2016, 1604.02616.

[49]  Robert E. Mahony,et al.  Observers for invariant systems on Lie groups with biased input measurements and homogeneous outputs , 2015, Autom..

[50]  N. McClamroch,et al.  Rigid-Body Attitude Control , 2011, IEEE Control Systems.

[51]  Axel Barrau,et al.  An intrinsic Cramér-Rao bound on SO(3) for (dynamic) attitude filtering , 2015, 2015 54th IEEE Conference on Decision and Control (CDC).