Patent Publication Number: US-2020277053-A1

Title: Integrated guidance and feedback control for autonomous vehicle

Description:
BACKGROUND OF THE INVENTION 
     1. Field of the Invention 
     The present invention relates to unmanned vehicles and, more specifically, to guidance control having nonlinear stability as well as autonomous attitude control for improved robustness. 
     2. Description of the Related Art 
     Autonomous operation of an unmanned vehicle is considered an important topic of current research interest for several applications where remote human piloting is not feasible, such as security, agriculture and aquaculture, inspection of civilian infrastructure, space and underwater exploration, wildlife tracking and conservation, package delivery, and remote sensing. A critical aspect of reliable operations of unmanned vehicles is that of nonlinearly stable autonomous guidance and control with a large domain of attraction for robustness to external disturbances. This is particularly true for beyond visual-line-of-sight (BVLOS) operations that require safety and reliability in the presence of disturbances such as wind. The absence of nonlinear stability in such a situation may lead to failure and a crash of the vehicle. In addition, the attitude dynamics of the underactuated unmanned vehicle need to be controlled simultaneously with its translational dynamics. According, there is a need in the art for an approach to unmanned vehicle navigation that provides nonlinear stability as well as autonomous attitude control for improved robustness and that is discretized in time for easy computerized integration. 
     BRIEF SUMMARY OF THE INVENTION 
     The present invention is a control system for an underactuated vehicle having a plurality of thrusters that provide a body-fixed thrust direction. The control system provides improved robustness and is discretized in time for easy computerized integration. The system includes a controller operably interconnected to the plurality of thrusters for selectively controlling each of the plurality of thrusters and the controller is programmed to receive a position trajectory describing a flight path for the vehicle from a starting position to at least one waypoint, to determine a thrust vector needed to track the position trajectory, to determine a thrust direction that is transverse to the thrust vector, to determine an attitude trajectory to track the position trajectory from the thrust vector and thrust direction, and to determine a control torque for the vehicle that provides the thrust vector and the attitude trajectory needed to track the position trajectory. The controller is further programmed to selectively operate the plurality of thrusters of the vehicle according to the control thrust and control torque. The controller is further programmed to determine a current position, an attitude, a translational velocity, and an angular velocity of the vehicle after operating the plurality of thrusters. The controller is also programmed to repeat the determination of the thrust vector, the thrust direction, the attitude trajectory, the control torque, the operation of the thrusters, and the determination of the current position, attitude, translational velocity and angular velocity according to discrete time intervals. The controller is also programmed to determine the thrust vector that tracks the position trajectory by geometric control that guarantees nonlinear stability. The attitude trajectory tracks the position trajectory from the desired thrust vector direction and the thrust direction attitude trajectory guarantees nonlinear stability. The controller is programmed to determine a control torque for the vehicle that provides the thrust vector and the attitude trajectory that tracks the received position trajectory. 
     The present invention also comprises a method of controlling an underactuated vehicle having a plurality of thrusters that guarantees nonlinear stability. The steps of the method include receiving a position trajectory on the vehicle, wherein the position trajectory describes a flight path for the vehicle from a starting position to at least one waypoint, using a controller positioned on the vehicle and operably interconnected to the plurality of thrusters to determine a thrust vector that tracks the position trajectory, using the controller on the vehicle to determine a thrust direction that is transverse to the thrust vector, using the controller on the vehicle to determine an attitude trajectory that tracks the received position trajectory, and using the controller on the vehicle to determine a control torque for the vehicle that provides the thrust vector and the attitude trajectory that tracks the received position trajectory. The method further comprises the step of selectively operating the thrusters of the vehicle according to the control torque. The method also includes the step of determining the current position, attitude, and velocity of the vehicle after operating the thrusters. The method may also involve repeating the determination of the thrust vector, the thrust direction, the attitude trajectory, the control torque, the operation of the thrusters, and the determination of the current position, attitude, and velocity according to discrete time intervals. The step of determining the thrust vector that tracks the position trajectory is performed by geometric control that guarantees nonlinear stability. The step of determining the attitude trajectory to track the received position trajectory is performed by geometric control that guarantees nonlinear stability. The step of determining the control torque for the vehicle that provides the thrust vector and the attitude trajectory that tracks the received position trajectory is performed by finite-time stable attitude control. 
    
    
     
       BRIEF DESCRIPTION OF THE SEVERAL VIEWS OF THE DRAWING(S) 
       The present invention will be more fully understood and appreciated by reading the following Detailed Description in conjunction with the accompanying drawings, in which: 
         FIG. 1  is a schematic of an exemplary underactuated vehicle to be guided through a set of waypoints according to the present invention; 
         FIG. 2  is a graph of the desired and achieved trajectories of an underactuated vehicle guided through a set of waypoints according to the present invention; and 
         FIG. 3  is a graph of the attitude tracking error function according to the present invention; 
         FIG. 4  is a graph of the angular velocity error according to the present invention; 
         FIG. 5  is a graph of the position tracking error according to the present invention; 
         FIG. 6  is a graph of the velocity tracking error function according to the present invention; 
         FIG. 7  is a graph of the norm of position tracking error according to the present invention; 
         FIG. 8  is a graph of the norm of velocity tracking error according to the present invention; 
         FIG. 9  is a graph of the magnitude of total thrust according to the present invention; and 
         FIG. 10  is a graph of the control torque according to the present invention; 
         FIG. 11  is a flowchart of a control process for an underactuated vehicle according to the present invention. 
     
    
    
     DETAILED DESCRIPTION OF THE INVENTION 
     Referring to the figures, wherein like numerals refer to like parts throughout, there is seen in  FIG. 1  an underactuated vehicle  10  that is to be guided through a set of finite waypoint g 1 , g 2 , g 3  . . . g n . For the purposes of demonstrating the present invention, vehicle  10  is shown and described as having a fixed-body frame  12  having four actuators  14  that produce thrust vectors f 1 , f 2 , f 3 , f 4  that are separated by a scalar distance D from the axis of rotation to the center of vehicle  10 , an arrangement conventionally referred to as a quadcopter or small unmanned aerial vehicle (sUAV). Vehicle  10  includes a microcontroller  16  that controls thrust actuators  14  to follow a predetermined position trajectory  18  that described the intended flight path of vehicle  10  and can be prepared off-board and sent to microcontroller  16  or calculated directly on-board microcontroller  16  if desired. The present invention determines and implements the control of vehicle  10  so that executes the position trajectory  18 . 
     The present invention is thus an integrated guidance and feedback control scheme for steering an underactuated vehicle through desired waypoints in three-dimensional space. The underactuated vehicle is modeled as a rigid body with four control inputs. These control inputs actuate the three degrees of freedom of rotational motion and one degree of freedom of translational motion in a vehicle body-fixed coordinate frame. This actuation model is appropriate for a wide range of underactuated vehicles, including spacecraft with internal attitude actuators, vertical take-off and landing (VTOL) aircraft, fixed-wing multirotor unmanned aerial vehicles (UAVs), maneuverable robotic vehicles, etc. The guidance problem is developed on the special Euclidean group of rigid body motions, SE(3), in the framework of geometric mechanics, which represents the vehicle dynamics globally on this configuration manifold. The integrated guidance and control algorithm takes as an input the desired trajectory for the translational motion that passes through the given waypoints, and the desired trajectory for the attitude is generated autonomously based on the desired thrust direction to achieve the translational motion trajectory. A feedback control law is then obtained to steer the underactuated vehicle towards the desired trajectories in translation and rotation. This integrated guidance and control scheme takes into account known bounds on control inputs and generates a trajectory that is continuous and at least twice differentiable, which can be implemented with continuous and bounded control inputs. In an example, the integrated guidance and feedback control scheme is applied to an underactuated quadcopter UAV to autonomously generate a trajectory through a series of given waypoints in SE(3) and track the desired trajectory in finite time. The overall stability analysis of the feedback system is addressed. Discrete time models for the dynamics and control schemes of the UAV are obtained in the form of Lie group variational integrators using the discrete Lagrange-d&#39;Alembert principle. Almost global asymptotic stability of the feedback system over its state space is shown analytically and verified through numerical simulations. 
     Guidance Scheme 
     Coordinate Frame Definition 
     The configuration of an unmanned vehicle modeled as a rigid body is given by its position and orientation, which are together referred to as its pose. To define the pose of the vehicle, a coordinate frame β is fixed to its body and another coordinate frame   that is fixed in space and takes the role of an inertial coordinate frame. Let b∈ 3  denote the position vector of the origin of frame β with respect to frame   represented in frame  . Let R∈ denote the orientation, defined as the rotation matrix from frame β to frame  . The pose of the vehicle can be represented in matrix form as follows: 
     
       
         
           
             
               
                 
                   
                     g 
                     = 
                     
                       
                         [ 
                         
                           
                             
                               R 
                             
                             
                               b 
                             
                           
                           
                             
                               0 
                             
                             
                               1 
                             
                           
                         
                         ] 
                       
                       ∈ 
                       
                         SE 
                          
                         
                           ( 
                           3 
                           ) 
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   1 
                   ) 
                 
               
             
           
         
       
     
     where SE(3) is the six-dimensional Lie group of rigid body motions (translational and rotational) that is obtained as the semi-direct product of R 3  with SO(3). 
     Trajectory Generation for Unmanned Vehicle 
     The trajectory generation problem consists of creating an appropriately smooth trajectory through a given finite set of desired waypoints which the underactuated vehicle&#39;s trajectory is required to pass through. These waypoints are given in the inertial frame   as follows: 
         b   d     1     ,b   d     2     , . . . ,b   d     n   ∈ 3 , with  b   d     i     =b   d ( t   i )| 3  and  t   1   &lt;t   2   &lt; . . . &lt;t   n .  (1)
 
     Here b d (t) gives the desired position trajectory on R 3  parameterized by time. A time trajectory for the position that is continuous and twice differentiable (i.e., b d (t)= 2 ( 3 )) could be generated, for example, using interpolating functions between the waypoints. A time trajectory for the position can also be generated using standard linear quadratic control schemes, and this approach may generate smooth trajectories between waypoints. Once the desired position trajectory over time has been generated based on the given waypoints, one needs to generate a desired attitude trajectory such that the position trajectory is achieved. The procedure by which this is done utilizes the known actuation and the dynamics model, and is detailed in the following section. 
     Let g d (f)∈SE(3) be the desired pose (position, b d  and attitude, R d ) generated by the guidance scheme. Then the desired velocities (translational, ν d  and rotational, Ω d ) are given by ξ d (t) that satisfies the kinematics 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             g 
                             . 
                           
                           d 
                         
                          
                         
                           ( 
                           t 
                           ) 
                         
                       
                       = 
                       
                         
                           
                             g 
                             d 
                           
                            
                           
                             ( 
                             t 
                             ) 
                           
                         
                          
                         
                           
                             
                               ξ 
                               d 
                             
                              
                             
                               ( 
                               t 
                               ) 
                             
                           
                           ⋁ 
                         
                       
                     
                     , 
                     
                       
 
                     
                      
                     where 
                   
                    
                   
                     
 
                   
                    
                   
                     
                       
                         
                           g 
                           d 
                         
                          
                         
                           ( 
                           t 
                           ) 
                         
                       
                       = 
                       
                         
                           R 
                           d 
                         
                          
                         
                           b 
                           d 
                         
                          
                         01 
                       
                     
                     , 
                     
                       
 
                     
                      
                     
                       
                         ξ 
                         d 
                       
                       = 
                       
                         
                           
                             Ω 
                             d 
                           
                            
                           
                             v 
                             d 
                           
                         
                          
                         
                           ∈ 
                           6 
                         
                          
                         
                           
 
                         
                          
                         and 
                       
                     
                   
                    
                   
                     
 
                   
                    
                   
                     
                       ξ 
                       d 
                       ⋁ 
                     
                     = 
                     
                       
                         
                           Ω 
                           d 
                           × 
                         
                          
                         
                           v 
                           d 
                         
                          
                         0 
                          
                         
                             
                         
                          
                         0 
                       
                       ∈ 
                       
                         ⋐ 
                         
                           4 
                           × 
                           4 
                         
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   2 
                   ) 
                 
               
             
           
         
       
     
     Here (⋅) ν ={Ω x ν00∈|Ω, ν∈ 3 }, is a vector space isomorphism from R 6  to the associated Lie algebra, and (⋅) x : 3 →⊂ 3×3  is the skew-symmetric cross-product operator that gives the vector space isomorphism between R 3  and so(3): 
     
       
         
           
             
               
                 
                   
                     x 
                     × 
                   
                   = 
                   
                     
                       
                         [ 
                         
                           
                             
                               
                                 x 
                                 1 
                               
                             
                           
                           
                             
                               
                                 x 
                                 2 
                               
                             
                           
                           
                             
                               
                                 x 
                                 3 
                               
                             
                           
                         
                         ] 
                       
                       × 
                     
                     = 
                     
                       
                         [ 
                         
                           
                             
                               0 
                             
                             
                               
                                 - 
                                 
                                   x 
                                   3 
                                 
                               
                             
                             
                               
                                 x 
                                 2 
                               
                             
                           
                           
                             
                               
                                 x 
                                 3 
                               
                             
                             
                               0 
                             
                             
                               
                                 - 
                                 
                                   x 
                                   1 
                                 
                               
                             
                           
                           
                             
                               
                                 - 
                                 
                                   x 
                                   2 
                                 
                               
                             
                             
                               
                                 x 
                                 1 
                               
                             
                             
                               0 
                             
                           
                         
                         ] 
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   4 
                   ) 
                 
               
             
           
         
       
     
     In addition to the desired waypoints, the vehicle has to satisfy its known dynamics. Consider the “nominal” model of the dynamics for the underactuated vehicle as given by 
         {dot over (ξ)}= ad   ξ * ξ+φ( g ,ξ)+ Bu, u∈     ⊂     4   , B∈     6×4 ,  (5)
 
     where II denotes the mass (m) and inertia (J) properties of the vehicle given as 
     
       
         
           
             
               
                 
                   = 
                   
                     
                       [ 
                       
                         
                           
                             J 
                           
                           
                             0 
                           
                         
                         
                           
                             0 
                           
                           
                             
                               m 
                                
                               
                                   
                               
                                
                               
                                 I 
                                 3 
                               
                             
                           
                         
                       
                       ] 
                     
                     ∈ 
                     
                       
                         6 
                         × 
                         6 
                       
                     
                   
                 
               
               
                 
                   ( 
                   6 
                   ) 
                 
               
             
           
         
       
     
     and I 3  is the 3×3 identity matrix. 
     The vector of known (modeled) moments and forces is denoted φ(g,ξ)∈ 6 ; usually this is obtained from a known model. Note that the vehicle has four inputs for the six degrees of freedom of translational and rotational motion, as given by the control influence matrix B, which can be expressed in block matrix form as follows: 
     
       
         
           
             
               B 
               = 
               
                 [ 
                 
                   
                     
                       
                         I 
                         4 
                       
                     
                   
                   
                     
                       
                         0 
                         
                           2 
                           × 
                           4 
                         
                       
                     
                   
                 
                 ] 
               
             
             , 
           
         
       
     
     where I 4  is the 4×4 identity matrix. The adjoint operator on se(3) and the co-adjoint operator (adξ) are defined in matrix form as 
     
       
         
           
             
               
                 
                   
                     
                       ad 
                       ξ 
                     
                     = 
                     
                       
                         
                           [ 
                           
                             
                               
                                 
                                   - 
                                   
                                     Ω 
                                     × 
                                   
                                 
                               
                               
                                 0 
                               
                             
                             
                               
                                 
                                   - 
                                   
                                     ν 
                                     × 
                                   
                                 
                               
                               
                                 
                                   - 
                                   
                                     Ω 
                                     × 
                                   
                                 
                               
                             
                           
                           ] 
                         
                          
                         
                             
                         
                          
                         and 
                          
                         
                           
                               
                           
                            
                           
                               
                           
                         
                          
                         
                           ad 
                           ξ 
                           * 
                         
                       
                       = 
                       
                         
                           ( 
                           
                             ad 
                             ξ 
                           
                           ) 
                         
                         T 
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   7 
                   ) 
                 
               
             
           
         
       
     
     where Ω,ν∈R 3  denote the rotational and translational velocities of the underactuated vehicle, respectively, in frame β. The vector of control inputs u∈ ⊂   4  has to be in the set of admissible controls C and directly actuates the three degrees of rotational motion and one degree of translational motion. This actuation model is applicable to aerial, space and underwater vehicles and it is known that a rigid body is controllable with such actuation 
     Feedback Guidance and Tracking on TSE(3) 
     The desired trajectory is generated in SE(3) and the tracking errors are expressed with respect to inertial and body fixed frames as follows, 
     
       
         
           
               
               
               
             
               
                   
                   
               
             
            
               
                   
                 {tilde over (b)} := b − b d   
                 Position tracking error in inertial frame 
               
               
                   
                 x := R d   T {tilde over (b)} 
                 Position tracking error in body fixed frame 
               
               
                   
                 {tilde over (v)} := v − v d  =     
                 Velocity tracking error in inertial frame 
               
               
                   
                 Q := R d   T R 
                 Attitude tracking error 
               
               
                   
                 ω := Ω − Q T Ω d   
                 Angular velocity tracking error 
               
               
                   
                   
               
            
           
         
       
     
     Tracking Errors Expressed in Body Frame 
     Tracking errors on TSE(3) are defined as follows: 
     
       
         
           
             
               
                 
                   
                     g 
                     = 
                     
                       [ 
                       
                         
                           
                             R 
                           
                           
                             b 
                           
                         
                         
                           
                             0 
                           
                           
                             1 
                           
                         
                       
                       ] 
                     
                   
                   , 
                   
                     
                       g 
                       d 
                     
                     = 
                     
                       
                         [ 
                         
                           
                             
                               
                                 R 
                                 d 
                               
                             
                             
                               
                                 b 
                                 d 
                               
                             
                           
                           
                             
                               0 
                             
                             
                               1 
                             
                           
                         
                         ] 
                       
                       . 
                     
                   
                 
               
               
                 
                   ( 
                   8 
                   ) 
                 
               
             
           
         
       
     
     Tracking error on SE(3): 
     
       
         
           
             
               
                 
                   
                     h 
                     = 
                     
                       
                         
                           g 
                           d 
                           
                             - 
                             1 
                           
                         
                          
                         g 
                       
                       = 
                       
                         [ 
                         
                           
                             
                               Q 
                             
                             
                               x 
                             
                           
                           
                             
                               0 
                             
                             
                               1 
                             
                           
                         
                         ] 
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   9 
                   ) 
                 
               
             
           
         
       
     
     where Q=R d R and x=R d (b−b d )=R d {tilde over (b)}. Therefore, the kinematics for the pose tracking error is: 
         {dot over (h)}=hξ   ν ,  (10)
 
     where 
     
       
         
           
             
               
                 
                   
                     
                       ξ 
                       ⋁ 
                     
                     = 
                     
                       [ 
                       
                         
                           
                             
                               ω 
                               × 
                             
                           
                           
                             ν 
                           
                         
                         
                           
                             0 
                           
                           
                             0 
                           
                         
                       
                       ] 
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   11 
                   ) 
                 
               
             
           
         
       
     
     and ω=Ω−QΩ d ,υ=ν−Q(ν d +Ω d   x +x). The dynamics for the tracking errors in velocities is: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           m 
                            
                           
                             υ 
                             . 
                           
                         
                         = 
                         
                           
                             - 
                             
                               fe 
                               3 
                             
                           
                           + 
                           
                             m 
                              
                             
                               { 
                               
                                 
                                   
                                     ω 
                                     × 
                                   
                                    
                                   Q 
                                    
                                   
                                       
                                   
                                    
                                   
                                     v 
                                     d 
                                   
                                 
                                 - 
                                 
                                   Q 
                                    
                                   
                                     ( 
                                     
                                       
                                         
                                           ν 
                                           . 
                                         
                                         d 
                                       
                                       + 
                                       
                                         
                                           
                                             Ω 
                                             . 
                                           
                                           d 
                                           × 
                                         
                                          
                                         x 
                                       
                                       + 
                                       
                                         
                                           Ω 
                                           d 
                                           × 
                                         
                                          
                                         Qv 
                                       
                                     
                                     ) 
                                   
                                 
                               
                               ) 
                             
                           
                         
                       
                       } 
                     
                     + 
                     
                       
 
                     
                      
                     
                       
                         
                           m 
                            
                           
                             ( 
                             
                               υ 
                               + 
                               
                                 Q 
                                  
                                 
                                     
                                 
                                  
                                 
                                   v 
                                   d 
                                 
                               
                             
                             ) 
                           
                         
                         × 
                       
                        
                       
                         ( 
                         
                           ω 
                           + 
                           
                             Q 
                              
                             
                                 
                             
                              
                             
                               Ω 
                               d 
                             
                           
                         
                         ) 
                       
                     
                     + 
                     
                       m 
                        
                       
                           
                       
                        
                       g 
                        
                       
                           
                       
                        
                       Q 
                        
                       
                           
                       
                        
                       
                         R 
                         d 
                       
                        
                       
                         e 
                         3 
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   12 
                   ) 
                 
               
             
             
               
                 
                   
                     J 
                      
                     
                       ω 
                       . 
                     
                   
                   = 
                   
                     τ 
                     + 
                     
                       J 
                        
                       
                         ( 
                         
                           
                             
                               ω 
                               × 
                             
                              
                             Q 
                              
                             
                                 
                             
                              
                             
                               Ω 
                               d 
                             
                           
                           - 
                           
                             Q 
                              
                             
                                 
                             
                              
                             
                               
                                 Ω 
                                 . 
                               
                               d 
                             
                           
                         
                         ) 
                       
                     
                     - 
                     
                       
                         
                           ( 
                           
                             ω 
                             + 
                             
                               Q 
                                
                               
                                   
                               
                                
                               
                                 Ω 
                                 d 
                               
                             
                           
                           ) 
                         
                         × 
                       
                        
                       
                         
                           J 
                            
                           
                             ( 
                             
                               ω 
                               + 
                               
                                 Q 
                                  
                                 
                                     
                                 
                                  
                                 
                                   Ω 
                                   d 
                                 
                               
                             
                             ) 
                           
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   13 
                   ) 
                 
               
             
           
         
       
     
     Translational Motion Control in Inertial Frame 
     Because the desired position trajectory, b d (t), is generated in the inertial frame  , it is convenient to express the position and translational velocity tracking error dynamics (12) in this frame. As the translational dynamics is expressed in the inertial frame, the rotational dynamics is decoupled from the translational dynamics such that the translation control force is obtained in the inertial frame followed by the appropriate attitude control in body-fixed frame to track the desired trajectory, b d . Note that ν=Rν and Qν d =RR d ν d =Rν d , where ν d =R d ν d ={dot over (b)} d . Define {tilde over (b)}:=b−b d  and {tilde over (ν)}:=ν−ν d ={dot over ({tilde over (b)})}. Therefore, in inertial frame  , the position and translational velocity tracking error dynamics are: 
       {dot over ( {tilde over (b)} )}={tilde over (υ)}=υ−υ d   , m {dot over ( {tilde over (ν)} )}= mge   3   −fr   3   +m{dot over (v)}   d ,  (14)
 
     where r 3 =Re 3  is the third column vector of the rotation matrix R from frame β to frame  , which represents the true attitude of the body. Here e 1 =[1 0 0] e 2 =[0 1 0] and e 3 =[0 0 1], are the standard basis vectors (as column vectors) of R 3 . Note that fr 3 = φ   c  can be considered as the control force vector acting on the body, expressed in inertial frame. The magnitude of this vector is one of the control inputs f, which is designed as a feedback control law. The direction of this vector r 3 =Re 3  is designed as part of the desired trajectory for the attitude in SO(3), with the other two column vectors of the rotation matrix R obtained from the generated trajectory for r 3 . This feedback guidance plus trajectory tracking control is detailed in the following development. 
     Consider the following Lyapunov function for the desired translational motion: 
         V   tr ( {tilde over (b)} ,{tilde over (ν)})=½ m{tilde over (ν)}{tilde over (ν)}+ ½ {tilde over (b)}P{tilde over (b)},   (15)
 
     where P∈3×3 is a positive definite control gain matrix. Therefore, along the translational error dynamics (3.2), 
     
       
         
           
             
               
                 
                   
                     
                       V 
                       . 
                     
                     tr 
                   
                   = 
                   
                     
                       
                         m 
                          
                         
                           v 
                           ~ 
                         
                          
                         
                           
                             v 
                             ~ 
                           
                           . 
                         
                       
                       + 
                       
                         
                           b 
                           ~ 
                         
                          
                         P 
                          
                         
                           
                             b 
                             ~ 
                           
                           . 
                         
                       
                     
                     = 
                     
                       
                         
                           v 
                           ~ 
                         
                          
                         
                           ( 
                           
                             
                               m 
                                
                               
                                 
                                   v 
                                   ~ 
                                 
                                 . 
                               
                             
                             + 
                             
                               P 
                                
                               
                                 b 
                                 ~ 
                               
                             
                           
                           ) 
                         
                       
                       = 
                       
                         
                           
                             v 
                             ~ 
                           
                            
                           
                             ( 
                             
                               
                                 mge 
                                 3 
                               
                               - 
                               
                                 fr 
                                 3 
                               
                               - 
                               
                                 m 
                                  
                                 
                                   
                                     v 
                                     . 
                                   
                                   d 
                                 
                               
                               + 
                               
                                 P 
                                  
                                 
                                   b 
                                   ~ 
                                 
                               
                             
                             ) 
                           
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   16 
                   ) 
                 
               
             
           
         
       
     
     It is desired that the control force vector satisfies: 
         fr   3 = φ   c   =mge   3   +P{tilde over (b)}+L   ν   {tilde over (ν)}−m{dot over (v)}   d ,  (17)
 
     where L ν ∈ 3×3  is another positive definite control gain matrix. Therefore, the control law for the magnitude of this force vector is: 
         f=e   3   T   R   T   φ   c   =e   3   T   R   T ( mge   3   +P{tilde over (b)}+L   ν ( Rν−ν   d )− m{dot over (ν)}   d ).  (18)
 
     However, to achieve stable tracking of the desired translational motion, the attitude has to be controlled such that the direction of Re 3 =r 3  agrees with that specified by (17). This is done in the following subsection. 
     Generating Desired Attitude Trajectory 
     Given the desired control force vector in inertial frame as given by (17), one can generate a desired trajectory for the third column of R d  (the desired attitude) as follows: 
     
       
         
           
             
               
                 
                   
                     r 
                     
                       3 
                        
                       d 
                     
                   
                   = 
                   
                     
                       
                         
                           mge 
                           3 
                         
                         + 
                         
                           P 
                            
                           
                             b 
                             ~ 
                           
                         
                         + 
                         
                           
                             L 
                             v 
                           
                            
                           
                             v 
                             ~ 
                           
                         
                         - 
                         
                           m 
                            
                           
                             
                               v 
                               . 
                             
                             d 
                           
                         
                       
                       
                         
                           mge 
                           3 
                         
                         + 
                         
                           P 
                            
                           
                             b 
                             ~ 
                           
                         
                         + 
                         
                           
                             L 
                             v 
                           
                            
                           
                             v 
                             ~ 
                           
                         
                         - 
                         
                           m 
                            
                           
                             
                               v 
                               . 
                             
                             d 
                           
                         
                       
                     
                     = 
                     
                       
                         R 
                         d 
                       
                        
                       
                         
                           e 
                           3 
                         
                         . 
                       
                     
                   
                 
               
               
                 
                   ( 
                   19 
                   ) 
                 
               
             
           
         
       
     
     Select an appropriate s d (t)∈ 2 (R 3 ) such that it is transverse to r 3d . Now compute 
     
       
         
           
             
               
                 
                   
                     
                       
                         r 
                         
                           2 
                            
                           d 
                         
                       
                       = 
                       
                         
                           
                             
                               r 
                               
                                 3 
                                  
                                 d 
                               
                             
                             × 
                             
                               s 
                               d 
                             
                           
                           
                             
                               r 
                               
                                 3 
                                  
                                 d 
                               
                             
                             × 
                             
                               s 
                               d 
                             
                           
                         
                         = 
                         
                           
                             R 
                             d 
                           
                            
                           
                             e 
                             2 
                           
                         
                       
                     
                     , 
                     
                       
 
                     
                      
                     and 
                   
                    
                   
                     
 
                   
                    
                   
                     
                       r 
                       
                         1 
                          
                         
                             
                         
                          
                         d 
                       
                     
                     = 
                     
                       
                         
                           r 
                           
                             2 
                              
                             d 
                           
                         
                         × 
                         
                           r 
                           
                             3 
                              
                             d 
                           
                         
                       
                       = 
                       
                         
                           R 
                           d 
                         
                          
                         
                           
                             e 
                             1 
                           
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   20 
                   ) 
                 
               
             
           
         
       
     
     The desired attitude trajectory is then given by: 
         R   d =[ r   2d   ×r   3d   r   2d   r   3d ]∈.  (21)
 
     Few methods to select s d (t) appropriately is described in the following results. 
     If s d  is selected as 
         s   d =1× r   3d   +μe   1 , with μ&gt;3,  (22)
 
     then s d  is always transverse and never parallel to r 3d . 
     Proof. 
     The condition on μ in (22) comes from the following observation: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             r 
                             
                               3 
                                
                               d 
                             
                           
                           × 
                           
                             s 
                             d 
                           
                         
                         = 
                           
                          
                         
                           
                             r 
                             
                               3 
                                
                               d 
                             
                           
                           × 
                           
                             ( 
                             
                               
                                 1 
                                 × 
                                 
                                   r 
                                   
                                     3 
                                      
                                     d 
                                   
                                 
                               
                               + 
                               
                                 μ 
                                  
                                 
                                   e 
                                   1 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                          
                         
                           
                             
                               ( 
                               
                                 
                                   r 
                                   
                                     3 
                                      
                                     d 
                                   
                                 
                                  
                                 
                                   r 
                                   
                                     3 
                                      
                                     d 
                                   
                                 
                               
                               ) 
                             
                              
                             1 
                           
                           - 
                           
                             
                               ( 
                               
                                 
                                   r 
                                   
                                     3 
                                      
                                     d 
                                   
                                 
                                  
                                 1 
                               
                               ) 
                             
                              
                             
                               r 
                               
                                 3 
                                  
                                 d 
                               
                             
                           
                           + 
                           
                             μ 
                              
                             
                               r 
                               
                                 3 
                                  
                                 d 
                               
                             
                             × 
                             
                               e 
                               1 
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         
                           = 
                             
                            
                           
                             1 
                             - 
                             
                               
                                 ( 
                                 
                                   
                                     r 
                                     
                                       3 
                                        
                                       d 
                                     
                                   
                                    
                                   1 
                                 
                                 ) 
                               
                                
                               
                                 r 
                                 
                                   3 
                                    
                                   d 
                                 
                               
                             
                             + 
                             
                               μ 
                                
                               
                                   
                               
                                
                               
                                 r 
                                 
                                   3 
                                    
                                   d 
                                 
                               
                               × 
                               
                                 e 
                                 1 
                               
                             
                           
                         
                         , 
                       
                     
                   
                 
               
               
                 
                   ( 
                   23 
                   ) 
                 
               
             
           
         
       
     
     because r 3d ∈ 2 , i.e., it is a unit vector. Define ρ(r 3d )=1−(r 3d 1)r 3d  so that: 
         r   3d   ×s   d =ρ( r   3d )+μ r   3d   ×e   1 .  (24)
 
     It can be verified that the components of the vector ρ(r 3d ) are bounded in the closed interval [−4/3,0]. Therefore, the first component of the vector r 3d ×s d  is bounded in the closed interval [μ−4/3,μ]. If μ&gt;3, then this component is always positive, and therefore r 3d ×s d ≠0. Therefore, the choice of s d  given in (19) ensures that it is always transverse to the generated r 3d . 
     The following statement gives a simpler choice of s d  in  3  that is transverse to r 3d ∈ 3 . It also gives a vector that is orthogonal to the given unit vector. 
     Let r 3d =a 1 a 2 a 3 ∈ 2 ⊂ 3  be a known unit vector as given in (16). The vector s_d=a_2+a_3 
     
       
         
           
             
               
                 
                   
                     s 
                     d 
                   
                   = 
                   
                     [ 
                     
                       
                         
                           
                             
                               a 
                               2 
                             
                             + 
                             
                               a 
                               3 
                             
                           
                         
                       
                       
                         
                           
                             
                               a 
                               3 
                             
                             - 
                             
                               a 
                               1 
                             
                           
                         
                       
                       
                         
                           
                             
                               - 
                               
                                 a 
                                 1 
                               
                             
                             - 
                             
                               a 
                               2 
                             
                           
                         
                       
                     
                     ] 
                   
                 
               
               
                 
                   ( 
                   25 
                   ) 
                 
               
             
           
         
       
     
     is orthogonal to r 3d . 
     Proof. 
     This is easily verified by the property of scalar product (orthogonal projection) as follows: 
     
       
         
           
             
               
                 
                   
                     
                       r 
                       
                         3 
                          
                         d 
                       
                     
                      
                     
                       s 
                       d 
                     
                   
                   = 
                     
                    
                   
                     
                       
                         a 
                         1 
                       
                        
                       
                         ( 
                         
                           
                             a 
                             2 
                           
                           + 
                           
                             a 
                             3 
                           
                         
                         ) 
                       
                     
                     + 
                     
                       
                         a 
                         2 
                       
                        
                       
                         ( 
                         
                           
                             a 
                             3 
                           
                           - 
                           
                             a 
                             1 
                           
                         
                         ) 
                       
                     
                     + 
                     
                       
                         a 
                         3 
                       
                        
                       
                         ( 
                         
                           
                             - 
                             
                               a 
                               1 
                             
                           
                           - 
                           
                             a 
                             2 
                           
                         
                         ) 
                       
                     
                   
                 
               
             
             
               
                 
                   = 
                     
                    
                   
                     
                       
                         
                           a 
                           1 
                         
                          
                         
                           ( 
                           
                             
                               a 
                               2 
                             
                             + 
                             
                               a 
                               3 
                             
                             - 
                             
                               a 
                               2 
                             
                             - 
                             
                               a 
                               3 
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           a 
                           2 
                         
                          
                         
                           ( 
                           
                             
                               a 
                               3 
                             
                             - 
                             
                               a 
                               3 
                             
                           
                           ) 
                         
                       
                     
                     = 
                     0. 
                   
                 
               
             
           
         
       
     
     This shows that the vector s d  as defined by (25) is orthogonal to the given vector r 3d . 
     As the next step, a control torque is selected such that R→R d  in finite time; this is done using an attitude tracking control scheme outlined in the next section. 
     Feedback Tracking Control Schemes 
     It is known that finite-time stability is more robust to external disturbances to the dynamics than asymptotic or exponential stability. Having a finite-time stable attitude control scheme also theoretically guarantees that the overall combined feedback attitude and position tracking control scheme is asymptotically stable, unlike conventional schemes which only guarantee convergence and not stability. This is because the desired thrust direction required for the position control is achieved in finite time, as shown in the second part of this section. The first part of this section details the attitude tracking control scheme and its stability properties, while the second part gives the stability result for the entire (translational and rotational) motion control scheme. 
     Finite-Time Stable Attitude Tracking Control on TSO(3) 
     Here we provide a finite-time stable attitude control scheme that is continuous and can be implemented with actuators commonly used for unmanned vehicles like rotorcraft and fixed-wing UAVs. The following two lemmas are used to prove the main result. Let a and b be non-negative real numbers and let p∈(1,2). Then 
         a   (1/p)   +b   (1/p) ≥( a+b ) (1/p) .  (26)
 
     This inequality is strict if both a and b are non-zero. This above inequality holds for all p&gt;1 as f(x)=x (1/p)  is a concave function and therefore subadditive. For the finite-time attitude tracking scheme, only the case where p∈(1,2) is required. 
     Let K=(k 1 ,k 2 ,k 3 ), where k 1 &gt;k 2 &gt;k 3 ≥1. Define 
     
       
         
           
             
               
                 
                   
                     
                       
                         s 
                         K 
                       
                        
                       
                         ( 
                         Q 
                         ) 
                       
                     
                     = 
                     
                       
                         ∑ 
                         
                           i 
                           = 
                           1 
                         
                         3 
                       
                        
                       
                         
                           
                             k 
                             i 
                           
                            
                           
                             ( 
                             
                               Q 
                                
                               
                                 e 
                                 i 
                               
                             
                             ) 
                           
                         
                         × 
                         
                           e 
                           i 
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   27 
                   ) 
                 
               
             
           
         
       
     
     such that t−K,I−Q=ωs K (Q). Here A,B=(AB), which makes K,I−Q a Morse function defined on. Let ⊂ be a closed subset containing the identity in its interior, defined by 
     
       
         
           
             
               
                 
                   S 
                   = 
                   
                     
                       { 
                       
                         
                           Q 
                           ∈ 
                           
                             
                               
                                 : 
                               
                                
                               
                                 Q 
                                 ii 
                               
                             
                             ≥ 
                             
                               0 
                                
                               
                                   
                               
                                
                               and 
                                
                               
                                   
                               
                                
                               
                                 Q 
                                 ij 
                               
                                
                               
                                 Q 
                                 ji 
                               
                             
                             ≤ 
                             
                               0 
                                
                               
                                 
 
                               
                                
                               
                                 ∀ 
                                 i 
                               
                             
                           
                         
                         , 
                         
                           j 
                           ∈ 
                           
                             { 
                             
                               1 
                               , 
                               2 
                               , 
                               3 
                             
                             } 
                           
                         
                         , 
                         
                           i 
                           ≠ 
                           j 
                         
                       
                       } 
                     
                     . 
                   
                 
               
               
                 
                   ( 
                   28 
                   ) 
                 
               
             
           
         
       
     
     Then for Q∈S, we have 
         s   K ( Q ) T   s   K ( Q )≥ tr ( K−KQ ).  (29)
 
     The finite-time attitude tracking control scheme and its proof of stability are given as follows. Note that this control scheme is continuous (indeed smooth) in time. This is unlike sliding mode control schemes that cannot be implemented with actuators (like rotors) that can only provide continuous control inputs. 
     Consider the attitude dynamics of Eq. (13) with s K (Q) as defined in (24). Define 
     
       
         
           
             
               
                 
                   
                     
                       
                         z 
                         K 
                       
                        
                       
                         ( 
                         Q 
                         ) 
                       
                     
                     = 
                     
                       
                         
                           s 
                           K 
                         
                          
                         
                           ( 
                           Q 
                           ) 
                         
                       
                       
                         
                           ( 
                           
                             
                               
                                 s 
                                 K 
                               
                                
                               
                                 ( 
                                 Q 
                                 ) 
                               
                             
                              
                             
                               
                                 s 
                                 K 
                               
                                
                               
                                 ( 
                                 Q 
                                 ) 
                               
                             
                           
                           ) 
                         
                         
                           1 
                           - 
                           
                             1 
                             / 
                             p 
                           
                         
                       
                     
                   
                   , 
                   and 
                 
               
               
                 
                   ( 
                   30 
                   ) 
                 
               
             
             
               
                 
                   
                     
                       w 
                        
                       
                         ( 
                         
                           Q 
                           , 
                           w 
                         
                         ) 
                       
                     
                     = 
                     
                       
                         
                           - 
                           t 
                         
                          
                         
                           
                             s 
                             K 
                           
                            
                           
                             ( 
                             Q 
                             ) 
                           
                         
                       
                       = 
                       
                         
                           ∑ 
                           
                             i 
                             = 
                             1 
                           
                           3 
                         
                          
                         
                           
                             k 
                             i 
                           
                            
                           
                             e 
                             i 
                           
                           × 
                           
                             ( 
                             
                               ω 
                               × 
                               
                                 Qe 
                                 i 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   31 
                   ) 
                 
               
             
           
         
       
     
     where P is as defined in Lemma 4.1. Further, let L Ω  be a positive definite control gain matrix such that L Ω −J is positive semidefinite, let k p &gt;1 and define κ such that 
     
       
         
           
             
               κ 
               p 
             
             = 
             
               
                 
                   σ 
                   
                     L 
                     , 
                     
                         
                     
                      
                     
                       m 
                        
                       
                           
                       
                        
                       i 
                        
                       
                           
                       
                        
                       n 
                     
                   
                 
                 
                   σ 
                   
                     J 
                     , 
                     
                         
                     
                      
                     
                       ma 
                        
                       
                           
                       
                        
                       x 
                     
                   
                 
               
               &gt; 
               0. 
             
           
         
       
     
     Consider the feedback control law for τ given by 
     
       
         
           
             
               
                 
                   
                     τ 
                     = 
                     
                       
                         J 
                          
                         
                           ( 
                           
                             
                               Q 
                                
                               
                                   
                               
                                
                               
                                 
                                   Ω 
                                   . 
                                 
                                 d 
                               
                             
                             - 
                             
                               
                                 
                                   κ 
                                    
                                   
                                       
                                   
                                    
                                   
                                     H 
                                      
                                     
                                       ( 
                                       
                                         
                                           s 
                                           K 
                                         
                                          
                                         
                                           ( 
                                           Q 
                                           ) 
                                         
                                       
                                       ) 
                                     
                                   
                                 
                                 
                                   
                                     ( 
                                     
                                       
                                         
                                           s 
                                           K 
                                         
                                          
                                         
                                           ( 
                                           Q 
                                           ) 
                                         
                                       
                                        
                                       
                                         
                                           s 
                                           K 
                                         
                                          
                                         
                                           ( 
                                           Q 
                                           ) 
                                         
                                       
                                     
                                     ) 
                                   
                                   
                                     1 
                                     - 
                                     
                                       1 
                                       / 
                                       p 
                                     
                                   
                                 
                               
                                
                               
                                 w 
                                  
                                 
                                   ( 
                                   
                                     Q 
                                     , 
                                     ω 
                                   
                                   ) 
                                 
                               
                             
                           
                           ) 
                         
                       
                       + 
                       
                         
                           
                             ( 
                             
                               Q 
                                
                               
                                   
                               
                                
                               
                                 Ω 
                                 d 
                               
                             
                             ) 
                           
                           × 
                         
                          
                         
                           J 
                            
                           
                             ( 
                             
                               
                                 Q 
                                  
                                 
                                     
                                 
                                  
                                 
                                   Ω 
                                   d 
                                 
                               
                               - 
                               
                                 κ 
                                  
                                 
                                     
                                 
                                  
                                 
                                   
                                     z 
                                     K 
                                   
                                    
                                   
                                     ( 
                                     Q 
                                     ) 
                                   
                                 
                               
                             
                             ) 
                           
                         
                       
                       + 
                       
                         κ 
                          
                         
                             
                         
                          
                         
                           J 
                            
                           
                             ( 
                             
                               
                                 
                                   z 
                                   K 
                                 
                                  
                                 
                                   ( 
                                   Q 
                                   ) 
                                 
                               
                               × 
                               Q 
                                
                               
                                   
                               
                                
                               
                                 Ω 
                                 d 
                               
                             
                             ) 
                           
                         
                       
                       + 
                       
                         κ 
                          
                         
                             
                         
                          
                         
                           J 
                            
                           
                             ( 
                             
                               ω 
                               + 
                               
                                 Q 
                                  
                                 
                                     
                                 
                                  
                                 
                                   Ω 
                                   d 
                                 
                               
                             
                             ) 
                           
                         
                         × 
                         
                           
                             z 
                             K 
                           
                            
                           
                             ( 
                             Q 
                             ) 
                           
                         
                       
                       - 
                       
                         
                           k 
                           p 
                         
                          
                         
                           
                             s 
                             K 
                           
                            
                           
                             ( 
                             Q 
                             ) 
                           
                         
                       
                       - 
                       
                         
                           
                             L 
                             Ω 
                           
                            
                           
                             Ψ 
                              
                             
                               ( 
                               
                                 Q 
                                 , 
                                 ω 
                               
                               ) 
                             
                           
                         
                         
                           
                             ( 
                             
                               
                                 Ψ 
                                  
                                 
                                   ( 
                                   
                                     Q 
                                     , 
                                     ω 
                                   
                                   ) 
                                 
                               
                                
                               
                                 L 
                                 Ω 
                               
                                
                               
                                 Ψ 
                                  
                                 
                                   ( 
                                   
                                     Q 
                                     , 
                                     ω 
                                   
                                   ) 
                                 
                               
                             
                             ) 
                           
                           
                             1 
                             - 
                             
                               1 
                               / 
                               p 
                             
                           
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   32 
                   ) 
                 
               
             
             
               
                 
                   
                       
                   
                    
                   where 
                 
               
               
                 
                     
                 
               
             
             
               
                 
                   
                       
                   
                    
                   
                     
                       
                         Ψ 
                          
                         
                           ( 
                           
                             Q 
                             , 
                             ω 
                           
                           ) 
                         
                       
                       = 
                       
                         ω 
                         + 
                         
                           κ 
                            
                           
                               
                           
                            
                           
                             
                               z 
                               K 
                             
                              
                             
                               ( 
                               Q 
                               ) 
                             
                           
                         
                       
                     
                     , 
                   
                 
               
               
                 
                   ( 
                   33 
                   ) 
                 
               
             
             
               
                 
                   
                       
                   
                    
                   
                     
                       and 
                        
                       
                           
                       
                        
                       
                         H 
                          
                         
                           ( 
                           x 
                           ) 
                         
                       
                     
                     = 
                     
                       I 
                       - 
                       
                         
                           
                             2 
                              
                             
                               ( 
                               
                                 1 
                                 - 
                                 
                                   1 
                                   / 
                                   p 
                                 
                               
                               ) 
                             
                           
                           xx 
                         
                          
                         
                           xx 
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   34 
                   ) 
                 
               
             
           
         
       
     
     Then the feedback attitude tracking error dynamics given by (13) is stabilized to (Q,ω)=(I,0) in finite time. 
     Proof. 
     Consider ω=−κz K (Q) and define the Morse-Lyapunov function K,I−Q on SO(3). Then the time derivative of this Morse-Lyapunov function along the attitude kinematics is given by 
     
       
         
           
             
               
                 
                   
                     
                       - 
                       t 
                     
                      
                     K 
                   
                   , 
                   
                     
                       I 
                       - 
                       Q 
                     
                     = 
                     
                       
                         ω 
                          
                         
                           
                             s 
                             K 
                           
                            
                           
                             ( 
                             Q 
                             ) 
                           
                         
                       
                       = 
                       
                         
                           
                             - 
                             κ 
                           
                            
                           
                               
                           
                            
                           
                             
                               s 
                               K 
                             
                              
                             
                               ( 
                               Q 
                               ) 
                             
                           
                            
                           
                             
                               z 
                               K 
                             
                              
                             
                               ( 
                               Q 
                               ) 
                             
                           
                         
                         = 
                         
                           
                             
                               - 
                               κ 
                             
                              
                             
                               
                                 
                                   
                                     s 
                                     K 
                                   
                                    
                                   
                                     ( 
                                     Q 
                                     ) 
                                   
                                 
                                  
                                 
                                   
                                     s 
                                     K 
                                   
                                    
                                   
                                     ( 
                                     Q 
                                     ) 
                                   
                                 
                               
                               
                                 
                                   ( 
                                   
                                     
                                       
                                         s 
                                         K 
                                       
                                        
                                       
                                         ( 
                                         Q 
                                         ) 
                                       
                                     
                                      
                                     
                                       
                                         s 
                                         K 
                                       
                                        
                                       
                                         ( 
                                         Q 
                                         ) 
                                       
                                     
                                   
                                   ) 
                                 
                                 
                                   1 
                                   - 
                                   
                                     1 
                                     / 
                                     p 
                                   
                                 
                               
                             
                           
                           = 
                           
                             
                               - 
                               
                                 
                                   κ 
                                    
                                   
                                     ( 
                                     
                                       
                                         
                                           s 
                                           K 
                                         
                                          
                                         
                                           ( 
                                           Q 
                                           ) 
                                         
                                       
                                        
                                       
                                         
                                           s 
                                           K 
                                         
                                          
                                         
                                           ( 
                                           Q 
                                           ) 
                                         
                                       
                                     
                                     ) 
                                   
                                 
                                 
                                   1 
                                   / 
                                   p 
                                 
                               
                             
                             ≤ 
                             
                               - 
                               
                                 
                                   κ 
                                    
                                   
                                     ( 
                                     
                                       K 
                                       , 
                                       
                                         l 
                                         - 
                                         Q 
                                       
                                     
                                     ) 
                                   
                                 
                                 
                                   1 
                                   / 
                                   p 
                                 
                               
                             
                           
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   35 
                   ) 
                 
               
             
           
         
       
     
     where we employed inequality (26) in Lemma 1. Therefore, when Ψ(Q,ω)=0, one can conclude that K,I−Q→0 in finite time for all initial Q in the subset ⊂ defined in Lemma 4.1, which yields Q→I in finite time once Q∈. 
     The control law is then designed to ensure that Ψ(Q,ω)→0 in finite time. Consider the Lyapunov function 
         V   rot ( Q ,ω)= k   p   K,I−Q +ω( Q ,ω) J Ψ( Q ,ω),  (36)
 
     for the attitude dynamics of (13) with control law (32). The time derivative of this Lyapunov function along this feedback dynamics is given by 
     
       
         
           
             
               
                 
                   
                     
                       V 
                       . 
                     
                     
                       r 
                        
                       o 
                        
                       t 
                     
                   
                   = 
                   
                     
                       
                         
                           k 
                           p 
                         
                          
                         ω 
                          
                         
                           
                             s 
                             K 
                           
                            
                           
                             ( 
                             Q 
                             ) 
                           
                         
                       
                       + 
                       
                         
                           Ψ 
                            
                           
                             ( 
                             
                               Q 
                               , 
                               ω 
                             
                             ) 
                           
                         
                          
                         J 
                          
                         
                             
                         
                          
                         
                           
                             Ψ 
                             . 
                           
                            
                           
                             ( 
                             
                               Q 
                               , 
                               ω 
                             
                             ) 
                           
                         
                       
                     
                     = 
                     
                       
                         
                           k 
                           p 
                         
                          
                         ω 
                          
                         
                           
                             s 
                             K 
                           
                            
                           
                             ( 
                             Q 
                             ) 
                           
                         
                       
                       + 
                       
                         Ψ 
                          
                         
                           [ 
                           
                             
                               τ 
                               c 
                             
                             + 
                             
                               J 
                                
                               Ω 
                               × 
                               
                                 ( 
                                 
                                   
                                     Q 
                                      
                                     
                                       Ω 
                                       d 
                                     
                                   
                                   - 
                                   
                                     κ 
                                      
                                     
                                       
                                         z 
                                         K 
                                       
                                        
                                       
                                         ( 
                                         Q 
                                         ) 
                                       
                                     
                                   
                                 
                                 ) 
                               
                             
                             + 
                             
                               J 
                                
                               
                                 ( 
                                 
                                   
                                     
                                       ω 
                                       × 
                                     
                                      
                                     Q 
                                      
                                     
                                       Ω 
                                       d 
                                     
                                   
                                   - 
                                   
                                     Q 
                                      
                                     
                                         
                                     
                                      
                                     
                                       
                                         Ω 
                                         . 
                                       
                                       d 
                                     
                                   
                                 
                                 ) 
                               
                             
                             + 
                             
                               
                                 
                                   κ 
                                    
                                   J 
                                    
                                   
                                     H 
                                      
                                     
                                       ( 
                                       
                                         
                                           s 
                                           K 
                                         
                                          
                                         
                                           ( 
                                           Q 
                                           ) 
                                         
                                       
                                       ) 
                                     
                                   
                                 
                                 
                                   
                                     ( 
                                     
                                       
                                         
                                           s 
                                           K 
                                         
                                          
                                         
                                           ( 
                                           Q 
                                           ) 
                                         
                                       
                                        
                                       
                                         
                                           s 
                                           K 
                                         
                                          
                                         
                                           ( 
                                           Q 
                                           ) 
                                         
                                       
                                     
                                     ) 
                                   
                                   
                                     1 
                                     - 
                                     
                                       1 
                                       / 
                                       p 
                                     
                                   
                                 
                               
                                
                               
                                 w 
                                  
                                 
                                   ( 
                                   
                                     Q 
                                     , 
                                     ω 
                                   
                                   ) 
                                 
                               
                             
                           
                           ] 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   37 
                   ) 
                 
               
             
           
         
       
     
     After substituting the control law (32) into the expression (37) and carrying out several algebraic simplifications, one obtains 
         {dot over (V)}   rot   =−k   p κ( s   K ( Q ) s   K ( Q )) 1/p −(ψ( Q ,ω) L   Ω Ψ( Q ,ω)) 1/p ≤−κ( k   p   K,I−Q ) 1/p −κ(Ψ( Q ,ω) J Ψ( Q ,ω)) 1/p ,
 
     for (Q,ω)∈x 3 , where ⊂ is as defined in (28). After substituting inequality (26) into the above expression, one obtains 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           V 
                           . 
                         
                         
                           r 
                            
                           o 
                            
                           t 
                         
                       
                       ≤ 
                       
                         - 
                         
                           
                             κ 
                              
                             
                               ( 
                               
                                 
                                   
                                     k 
                                     p 
                                   
                                    
                                   K 
                                 
                                 , 
                                 
                                   I 
                                   - 
                                   Q 
                                   + 
                                   
                                     
                                       Ψ 
                                        
                                       
                                         ( 
                                         
                                           Q 
                                           , 
                                           ω 
                                         
                                         ) 
                                       
                                     
                                      
                                     J 
                                      
                                     
                                       Ψ 
                                        
                                       
                                         ( 
                                         
                                           Q 
                                           , 
                                           ω 
                                         
                                         ) 
                                       
                                     
                                   
                                 
                               
                               ) 
                             
                           
                           
                             1 
                             / 
                             p 
                           
                         
                       
                     
                     = 
                     
                       
                         - 
                         κ 
                       
                        
                       
                         V 
                         rot 
                         
                           1 
                           / 
                           p 
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   38 
                   ) 
                 
               
             
           
         
       
     
     which implies that the feedback attitude tracking control system is (locally) finite-time stable at (Q,ω)=(I,0). 
     Note that the domain of attraction shown in the above analysis is (Q,ω)∈x 3 . The rest of the proof to show almost global finite-time stability of the attitude feedback control is identical to conventional proofs in the art, and is omitted here for the sake of brevity. 
     Stability of the Overall Feedback System on TSE(3) 
     The following statement outlines the stability of the overall feedback system with the control laws (32) and (18). The overall feedback control system given by the tracking error kinematics (10) and dynamics (13)-(14) is asymptotically stable for the generated state trajectory (b d (t),R d (t),ν d (t),Ω d (t))⊂. Moreover, the domain of convergence is almost global over the state space. 
     Proof. 
     This attitude tracking control scheme given by Theorem 4.1 ensures that R(t)=R d (t) for time t≥T where T&gt;0 is finite. Therefore, one obtains r 3 =Re 3 =r 3d  for t≥T, and from (18) and (19), 
         f=mge   3   +P{tilde over (b)}+Lν{tilde over (ν)}−m{dot over (ν)}   d , and 
         fr   3   =fr   3d   =mge   3   +P{tilde over (b)}+L   ν   {tilde over (ν)}−m{dot over (ν)}   d  for  t≥T.   (39)
 
     Substituting (39) in (16) gives: 
     
       
         
           
             
               
                 
                   
                     
                       
                         
                           
                             V 
                             . 
                           
                           
                             t 
                              
                             r 
                           
                         
                         = 
                           
                          
                         
                           
                             v 
                             ~ 
                           
                            
                           
                             ( 
                             
                               
                                 m 
                                  
                                 
                                     
                                 
                                  
                                 
                                   ge 
                                   3 
                                 
                               
                               - 
                               
                                 f 
                                  
                                 
                                   r 
                                   
                                     3 
                                      
                                     d 
                                   
                                 
                               
                               - 
                               
                                 m 
                                  
                                 
                                   
                                     v 
                                     . 
                                   
                                   d 
                                 
                               
                               + 
                               
                                 P 
                                  
                                 
                                   b 
                                   ~ 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                          
                         
                           
                             
                               - 
                               
                                 v 
                                 ~ 
                               
                             
                              
                             
                               L 
                               v 
                             
                              
                             
                               v 
                               ~ 
                             
                           
                           ≤ 
                           0. 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   40 
                   ) 
                 
               
             
           
         
       
     
     Since L ν  is positive definite, {dot over (V)} tr =0 if and only if {tilde over (ν)}=0. Applying LaSalle&#39;s invariance principle, one can show that when {tilde over (ν)}=0, m{dot over ({tilde over (ν)})}=mge 3 −fr 3d +m{dot over (ν)} d =0, and therefore P{tilde over (b)}=0, and thus {tilde over (b)}=0. Therefore, the tracking errors for the translational motion converge to ({tilde over (b)},{tilde over (ν)})=(0,0) 
     Note that the finite-time stability of the attitude control scheme guarantees that the desired thrust direction, r 3d , is achieved in finite time. This in turn ensures that the desired position trajectory is asymptotically tracked and the overall attitude and position tracking control system is asymptotically stable. The almost global domain of convergence also provides robustness to disturbances. Moreover, the continuous control schemes give here can be implemented with rotorcraft UAV that have actuators that can only provide continuous control forces and torques. 
     Example 
     Application to Autonomous UAV Navigation 
     For underactuated vehicle  10  seen in  FIG. 1 , the continuous equations of motion of the quadcopter UAV may be written as: 
         {dot over (R)}=RΩ   x ,  (41)
 
         m{dot over (ν)}=mgRe   3   −fe   3 ,  (42)
 
         J{dot over (Ω)}=JΩ×Ω+τ   (43)
 
     Each propeller can generate a thrust f i  proportional to the square of the corresponding motor speed i.e., f i =k f   ω   i   2 , and the torque generated by each actuator is directly proportional to its thrust i.e., τ i =k τ   ω   i   2 . The first and second axes (a 1  and a 2 ) of the body-fixed frame   lie in the plane normal to the axes of the propellers. The total thrust, f=Σ i=1   4 f i  acts along the third axis −a 3  of the body-fixed frame  . For such an UAV as shown in  FIG. 1 , the control input vector u=fτ can be expressed in terms of the actuator speeds  ω   i  according to 
     
       
         
           
             
                 
             
              
             
               
                 u 
                 = 
                 
                   
                     
                       ω 
                       _ 
                     
                     1 
                     2 
                   
                    
                   
                     
                       ω 
                       _ 
                     
                     2 
                     2 
                   
                    
                   
                     
                       ω 
                       _ 
                     
                     3 
                     2 
                   
                    
                   
                     
                       ω 
                       _ 
                     
                     4 
                     2 
                   
                 
               
               , 
               
                 where 
                  
                 
                   
 
                 
                 = 
                 
                   
                     
                       - 
                       
                         k 
                         f 
                       
                     
                     - 
                     
                       k 
                       f 
                     
                     - 
                     
                       k 
                       f 
                     
                     - 
                     
                       
                         k 
                         f 
                       
                        
                       0 
                     
                     - 
                     
                       
                         k 
                         f 
                       
                        
                       D 
                        
                       0 
                        
                       
                         k 
                         f 
                       
                        
                       D 
                        
                       
                         k 
                         f 
                       
                        
                       D 
                        
                       0 
                     
                     - 
                     
                       
                         k 
                         f 
                       
                        
                       D 
                        
                       0 
                     
                     - 
                     
                       
                         k 
                         τ 
                       
                        
                       
                         k 
                         τ 
                       
                     
                     - 
                     
                       
                         k 
                         τ 
                       
                        
                       
                         k 
                         τ 
                       
                     
                   
                    
                   
                     ∈ 
                     
                       4 
                       × 
                       4 
                     
                   
                 
               
             
           
         
       
     
     is a constant invertible matrix for k f ≠0 and k τ ≠0. Then, 
     
       
         
           
             
               
                 
                   
                     [ 
                     
                       
                         
                           
                             
                               ω 
                               _ 
                             
                             1 
                             2 
                           
                         
                       
                       
                         
                           
                             
                               ω 
                               _ 
                             
                             2 
                             2 
                           
                         
                       
                       
                         
                           
                             
                               ω 
                               _ 
                             
                             3 
                             2 
                           
                         
                       
                       
                         
                           
                             
                               ω 
                               _ 
                             
                             4 
                             2 
                           
                         
                       
                     
                     ] 
                   
                   = 
                   
                     
                       K 
                       
                         - 
                         1 
                       
                     
                      
                     
                       [ 
                       
                         
                           
                             f 
                           
                         
                         
                           
                             τ 
                           
                         
                       
                       ] 
                     
                   
                 
               
               
                 
                   ( 
                   44 
                   ) 
                 
               
             
           
         
       
     
     Guidance Algorithm 
     The objective of the integrated trajectory generation and control is to navigate the UAV from an initial pose to a final desired pose in SE(3), through a finite set of desired waypoints. Pseudocode of the integrated guidance and feedback control algorithm that generates a trajectory that passes through the waypoints and satisfies the kinematics and dynamics is given below: 
     
       
         
           
               
             
               
                   
               
             
            
               
                 PSEUDO CODE 
               
               
                 Integrated Guidance and Feedback Control 
               
               
                 procedure IGFC b d ,f,R d ,τ b d  and f are in    
               
               
                 set    
               
               
                 Fix the inertial frame b = b x b y b z   
               
               
                 Initialize the UAV with respect to an inertial frame    
               
               
                 Select b d     1   ,b d     2   , . . . ,b d     n    select desired waypoints in R 3   
               
               
                 Generate b d (t) = C 2 (     3 ) 
               
               
                 Generate a smooth trajectory as a function of time in 
               
               
                 while t &gt; t 0  do f ← (18) 
               
               
                 Compute the UAV control thrust in    r 3 d ← (19) 
               
               
                 desired thrust vector direction in the inertial frame 
               
               
                  for s d (t)ϵ 2  (R 3 ) do s d ãr 3d  r 2d  ← (20) 
               
               
                  end for 
               
               
                  R d  ← r 2d  × r 3d  r 2d  r 3d   
               
               
                 Generate the desired attitude trajectory τ ← (32) 
               
               
                 Compute attitude control torque such that R(t) → R d (t) in finite time 
               
               
                  end while 
               
               
                  return (b,R,ν) 
               
               
                 Implement the integrated guidance and feedback control on TSE(3)  
               
               
                 with feedback of (b,R,ν) and knowledge of (b d ,ν d ,{dot over (ν)} d ,R d ,Ω d ,{dot over (Ω)} d ) 
               
               
                 end procedure 
               
               
                   
               
            
           
         
       
     
     Numerical Validation 
     Discretization of the UAV Dynamics 
     The continuous equations of motion are discretized in the form of Lie Group Variational Integrator (LGVI) for digital implementation by applying the discrete Lagrange-d&#39;Alembert principle. The LGVI preserves the structure of the configuration space, which in this case is the Lie group SE(3), without any reprojection or parameterization. The LGVI schemes also have good energy-momentum properties when the dynamical model is that of a conservative or nearly conservative system. The discrete model obtained for the present invention is a Lie group variational integrator. 
     Let h≠0 denote the fixed time step size, such that h=t k+1 −t k . Then the discretized equations of motion obtained in the form of LGVI as, 
     
       
         
           
             
               
                 
                   
                     
                       R 
                       
                         k 
                         + 
                         1 
                       
                     
                     = 
                     
                       
                         R 
                         k 
                       
                        
                       
                         F 
                         k 
                       
                     
                   
                   , 
                   
                     
 
                   
                    
                   
                     
                       b 
                       
                         k 
                         + 
                         1 
                       
                     
                     = 
                     
                       
                         h 
                          
                         
                           R 
                           k 
                         
                          
                         
                           v 
                           k 
                         
                       
                       + 
                       
                         b 
                         k 
                       
                     
                   
                   , 
                   
                     
 
                   
                    
                   
                     
                       m 
                        
                       
                         v 
                         
                           k 
                           + 
                           1 
                         
                       
                     
                     = 
                     
                       
                         m 
                          
                         
                           F 
                           k 
                         
                          
                         
                           v 
                           k 
                         
                       
                       + 
                       
                         h 
                          
                         m 
                          
                         g 
                          
                         
                           R 
                           
                             k 
                             + 
                             1 
                           
                         
                          
                         
                           e 
                           3 
                         
                       
                       - 
                       
                         h 
                          
                         
                           f 
                           k 
                         
                          
                         
                           e 
                           3 
                         
                       
                     
                   
                   , 
                   
                     
 
                   
                    
                   
                     
                       Ω 
                       
                         d 
                         
                           k 
                           + 
                           1 
                         
                       
                       × 
                     
                     = 
                     
                       
                         1 
                         h 
                       
                        
                       
                         log 
                          
                         
                           ( 
                           
                             
                               R 
                               
                                 d 
                                 k 
                               
                             
                              
                             
                               R 
                               
                                 d 
                                 
                                   k 
                                   + 
                                   1 
                                 
                               
                             
                           
                           ) 
                         
                       
                     
                   
                   , 
                   
                     
 
                   
                    
                   
                     
                       J 
                        
                       
                           
                       
                        
                       
                         Ω 
                         
                           k 
                           + 
                           1 
                         
                       
                     
                     = 
                     
                       
                         
                           F 
                           k 
                         
                          
                         J 
                          
                         
                           Ω 
                           k 
                         
                       
                       + 
                       
                         h 
                          
                         
                           τ 
                           k 
                         
                       
                     
                   
                   , 
                 
               
               
                 
                   ( 
                   45 
                   ) 
                 
               
             
           
         
       
     
     where f k ≈exp(hΩ k   x )∈SO(3) guarantees that R k  evolves on SO(3). 
     The integrated guidance and feedback control scheme is numerically simulated for an UAV quadcopter of mass, m=4.34 kg; J=diag 0.8200.08450.1377 kgm 2 . The helical desired trajectory and the initial conditions are given as follows 
         b   d ( t )=0.4 sin(π t )0.6 cos(π t )0.4 t,  
 
         b (0)=100, 
         R (0)= I,    
       ν(0)=000; {dot over (ν)}(0)=000 and Ω(0)=000.
 
     The numerical simulation is performed for five seconds, t=5 with a time step size of h=0.01, using the LGVI routine given in (54) for the choice of s d  as given in proposition 3.3. The results of the numerical simulation are summarized in  FIG. 10 . The attitude error function Φ is parameterized as principle rotation angle, in terms of Q as given by, 
       Φ=cos −1 (½( tr ( Q )−1)).
 
     The numerical results are obtained after selecting the following gain values: 
         P= 38× I   3×3   ; L   ν =25× I   3×3  
 
         L   Ω =3.5× I   3×3   ; p= 0.75; κ=0.04 and  k   p =4.5.
 
     These gain values were selected after trial and error, and provide desirable transient response characteristics of the overall control scheme. 
     The time trajectory of the UAV tracking the desired trajectory is shown in  FIG. 2  and it is inferred that the trajectory converges to the desired values in finite time and remains stable for all time, t&gt;0. The attitude tracking error given by the principal angle, Φ converges in finite time which indicates that R tracks R d  as shown in  FIG. 3 . The angular velocity error is plotted in  FIG. 4 , and it is shown to converge to a small bounded neighborhood of the origin. The position tracking error is shown in  FIG. 5  and the velocity tracking error in  FIG. 6 . The tracking performance shows the errors converging to zero. Thus, the control scheme of the present invention tracks the position and remains stable even with the large initial position error with the norm of the position and velocity tracking errors shown in  FIGS. 7 and 8 . The total magnitude of the thrust force is less than 50 Newtons as shown in  FIG. 9  and the corresponding control torque is shown in  FIG. 10 ; these control inputs are reasonable and within the capabilities of the four propellers of the UAV as given by (53). From the simulation results, it can be inferred that the integrated guidance and control scheme takes the UAV from an initial pose to a desired final pose and the overall feedback system is stable. 
     Referring to  FIG. 11 , the present invention includes a method of controlling an underactuated vehicle  10  implemented on-board, such as in a microcontroller  16 , that is provided with control over all thrusters  14 . The flight control method commences with the initialization  20  of underactuated vehicle  10  with respect to an inertial frame. As discussed above, this involves using the appropriate paradigm for the particular underactuated vehicle representing its thrush and control characteristics. Next, a smooth position trajectory passing through the desired waypoints is loaded from an external database  22  for use initialized underactuated vehicle  10 . At its simplest, the trajectory may be from a starting position to a final destination of include one or more waypoints that vehicle  10  must progress to prior to completing a flight. The determination of the best trajectory given a finite set of waypoint may be determined by conventional off-board systems and provided to vehicle  10 . Alternatively, vehicle  10  could be programmed to determine a smooth position trajectory through the set of waypoints using conventional approaches implemented in the on-board microcontroller. Once the smooth position trajectory has been loaded, a check  24  is performed to determine whether time t is between a start time to and a finish time t f  during which vehicle  10  must accomplish the desired trajectory. If check  24  is false, underactuated vehicle  10  has completed its flight by tracking the desired trajectory  26  and can stop  28 . If check  24  determines that time t is between the start time to and the finish time t f , vehicle  10  determined the desired state  30 . Next, trajectory tracking errors are calculated  32  and vehicle  10  determines the required thrust vector to track the desired position trajectory  34 . From this information, a vector is constructed in a direction that is transverse to the desired thrust vector  36 . Next, a desired attitude trajectory is generated  38 . As explained above, the attitude trajectory is non-linearly stable and can therefore improve the efficiency of the flight control as well as the stability of vehicle  10  in adverse conditions. The control torque is then evaluated to track the desired attitude trajectory  40  and applied by vehicle  10 . For example, the appropriate thrust is provided to the thrusters  14  of vehicle  10  as established in initialization step  20  so that vehicle  10  is flown along the trajectory provided in step  24  using the appropriate attitude trajectory. The current state of vehicle  10  is then determined  42  (i.e., current position, attitude, and body velocity of underactuated vehicle  10  are determined to monitor the progress of vehicle  10 ) and control returns to check  24 . Location may be provided by a number of conventional processes, such a global positioning systems (GPS), WiFi location, or even external telemetry. The feedback control of  FIG. 11  is repeated in discrete time until vehicle  10  is determined to have completed its route by check  24 . 
     As described above, trajectory tracking control (both position and attitude), it is performed using geometric control that guarantees nonlinear (Lyapunov) stability. The stability of the overall control loop and convergence to the desired attitude and position trajectories is guaranteed by the finite-time stability of the attitude control scheme, which converges to the desired attitude (and direction transverse to the thrust direction) in finite time. The finite-time stable attitude control generates the desired thrust direction to track the given position trajectory. As this converges in finite time, the desired thrust direction is exactly followed after this convergence occurs. This ensures that the position trajectory converges to the desired position. 
     The present invention thus provides an integrated guidance and feedback control scheme for steering an underactuated vehicle through desired waypoints in three-dimensional space. The underactuated vehicle is modeled as a rigid body with four control inputs. These control inputs actuate the three degrees of freedom of rotational motion and one degree of freedom of translational motion in a vehicle body-fixed coordinate frame. This actuation model is appropriate for a wide range of underactuated vehicles, including spacecraft with internal attitude actuators, vertical take-off and landing (VTOL) aircraft, fixed-wing multirotor unmanned aerial vehicles (UAVs), maneuverable robotic vehicles, etc. The guidance problem is developed on the special Euclidean group of rigid body motions, SE(3), in the framework of geometric mechanics, which represents the vehicle dynamics globally on this configuration manifold. The integrated guidance and control algorithm selects the desired trajectory for the translational motion that passes through the given waypoints, and the desired trajectory for the attitude based on the desired thrust direction to achieve the translational motion trajectory. A feedback control law is then obtained to steer the underactuated vehicle towards the desired trajectories in translation and rotation. This integrated guidance and control scheme takes into account known bounds on control inputs and generates a trajectory that is continuous and at least twice differentiable, which can be implemented with continuous and bounded control inputs. In an example, the integrated guidance and feedback control scheme is applied to an underactuated quadcopter UAV to autonomously generate a trajectory through a series of given waypoints in SE(3) and track the desired trajectory in finite time. The overall stability analysis of the feedback system is addressed. Discrete time models for the dynamics and control schemes of the UAV are obtained in the form of Lie group variational integrators using the discrete Lagrange-d&#39;Alembert principle. Almost global asymptotic stability of the feedback system over its state space is shown analytically and verified through numerical simulations. 
     The present invention thus provides a stable and robust system for the UAV to track three-dimensional position trajectory that: (i) generates a feedback control force along the UAV body-fixed thrust direction to track the desired position trajectory in 3D; (ii) ensures that the control force is within the thrust generation capabilities of the UAV; and (iii) has a control force that leads to convergence in finite time to the desired position trajectory. The present invention also provides a method for autonomously generating the desired attitude trajectory for UAVs that: (i) generates a trajectory for the required thrust direction along the third body axis to track the desired position trajectory; and (ii) generates the other two body-fixed coordinate axes orthogonal to the generated thrust direction. The present invention further provides a stable and robust attitude trajectory tracking control method that: (i) generates a three-axis control torque to track the three-dimensional attitude motion trajectory; (ii) provides a control torque that is within the torque generation capabilities of the UAV; and (iii) has a control torque that converges in finite time to the attitude trajectory faster than the control force converges to the position trajectory. The stable and robust position and attitude control schemes are discretized in time for embedded computer implementation such that they are stable and robust in discrete time. 
     As described above, the present invention may be a system, a method, and/or a computer program associated therewith and is described herein with reference to flowcharts and block diagrams of methods and systems. The flowchart and block diagrams illustrate the architecture, functionality, and operation of possible implementations of systems, methods, and computer programs of the present invention. It should be understood that each block of the flowcharts and block diagrams can be implemented by computer readable program instructions in software, firmware, or dedicated analog or digital circuits. These computer readable program instructions may be implemented on the processor of a general purpose computer, a special purpose computer, or other programmable data processing apparatus to produce a machine that implements a part or all of any of the blocks in the flowcharts and block diagrams. Each block in the flowchart or block diagrams may represent a module, segment, or portion of instructions, which comprises one or more executable instructions for implementing the specified logical functions. It should also be noted that each block of the block diagrams and flowchart illustrations, or combinations of blocks in the block diagrams and flowcharts, can be implemented by special purpose hardware-based systems that perform the specified functions or acts or carry out combinations of special purpose hardware and computer instructions.