Integrated guidance and feedback control for autonomous vehicle

Abstract

An integrated guidance and feedback control scheme for steering an underactuated vehicle through desired waypoints in three-dimensional space. The guidance and control algorithm takes as an input the desired trajectory for the translational motion that passes through the given waypoints, and autonomously generates the desired trajectory for the attitude based on the desired thrust direction to achieve the translational motion trajectory. A feedback control law is obtained to steer the underactuated vehicle towards the desired trajectories in translation and rotation.

Claims

1. A control system for an underactuated vehicle having a plurality of thrusters that provide a body-fixed thrust direction, comprising: a controller operably interconnected to the plurality of thrusters for selectively controlling each of the plurality of thrusters; wherein the controller is programmed to receive a starting position, at least one waypoint, and a final destination, to determine a smooth position trajectory that is continuous and at least twice differentiable for describing a flight path for the vehicle from the starting position through the at least one waypoint to the final designation, to determine a thrust vector that is continuous and differentiable and that converges to the smooth position trajectory in an asymptotically stable manner, to generate a unit vector that is continuous and differentiable and that is transverse to the thrust vector, to generate a smooth attitude trajectory from the thrust vector and the unit vector that is transverse to the thrust vector, and to generate a control torque that is continuous and tracks the smooth attitude trajectory and converges in finite time to the smooth attitude trajectory faster than the thrust vector converges to the position trajectory.

2. The control system of claim 1, wherein the controller is further programmed to selectively operate the plurality of thrusters of the vehicle according to the control thrust and control torque.

3. The control system of claim 2, wherein 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.

4. The control system of claim 3, wherein the controller is programmed to repeat the determination of the thrust vector, the unit vector that is transverse to the thrust vector, 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.

5. The control system of claim 4, wherein the controller is programmed to determine the thrust vector that tracks the smooth position trajectory by geometric control that guarantees nonlinear stability.

6. The control system of claim 5, wherein the attitude trajectory is obtained from the smooth position trajectory using the desired thrust vector direction and the transverse direction.

7. The control system of claim 6, wherein the controller is programmed to determine a control torque for the vehicle that guarantees finite-time stable tracking of the generated attitude trajectory and then provides the thrust vector that tracks the received smooth position trajectory.

8. The control system of claim 1, wherein the smooth position trajectory is determined by the continuous and twice differentiable time trajectory by using an interpolating function between the starting position, the at least one waypoint, and the final destination.

9. The control system of claim 1, wherein the smooth position trajectory is determined by a time trajectory generated using an optimal linear quadratic control scheme.

10. A method of controlling an underactuated vehicle having a plurality of thrusters, comprising: receiving a starting position, at least one waypoint, and a final destination; using a controller positioned on the vehicle to determine a smooth position trajectory that is continuous and at least twice differentiable, wherein the smooth position trajectory describes a flight path for the vehicle from the starting position through the at least one waypoint to the final destination; using the controller positioned on the vehicle and operably interconnected to the plurality of thrusters to generate a thrust vector that is continuous and differentiable and that converges to the smooth position trajectory in an asymptotically stable manner; using the controller on the vehicle to generate a unit vector continuous and differentiable and that is transverse to the thrust vector; using the controller on the vehicle to generate a smooth attitude trajectory from the thrust vector and the unit vector; using the controller on the vehicle to generate a control torque for the vehicle that converges to the smooth attitude trajectory in a finite-time and stable manner faster than the thrust vector converges to the position trajectory.

11. The method of claim 10, further comprising the step of using the controller to selectively operate the plurality of thrusters of the vehicle according to the control torque.

12. The method of claim 11, further comprising the step of determining a current position, an attitude, and a velocity of the vehicle after operating the thrusters.

13. The method of claim 12, further comprising the step of repeating the determination of the thrust vector, the unit vector that is transverse to the thrust vector, 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.

14. The method of claim 13, wherein the step of determining the thrust vector that tracks the smooth position trajectory is performed by geometric control that guarantees nonlinear asymptotic stability.

15. The method of claim 14, wherein the step of determining the attitude trajectory to track the received smooth position trajectory is performed by geometric control that guarantees nonlinear finite-time stability.

16. The method of claim 15, wherein the step of determining the control torque for the vehicle that provides the thrust vector and the attitude trajectory that tracks the smooth position trajectory is performed by finite-time stable attitude control.

Description

BRIEF DESCRIPTION OF THE SEVERAL VIEWS OF THE DRAWING(S)

(1) The present invention will be more fully understood and appreciated by reading the following Detailed Description in conjunction with the accompanying drawings, in which:

(2) FIG. 1 is a schematic of an exemplary underactuated vehicle to be guided through a set of waypoints according to the present invention;

(3) 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

(4) FIG. 3 is a graph of the attitude tracking error function according to the present invention;

(5) FIG. 4 is a graph of the angular velocity error according to the present invention;

(6) FIG. 5 is a graph of the position tracking error according to the present invention;

(7) FIG. 6 is a graph of the velocity tracking error function according to the present invention;

(8) FIG. 7 is a graph of the norm of position tracking error according to the present invention;

(9) FIG. 8 is a graph of the norm of velocity tracking error according to the present invention;

(10) FIG. 9 is a graph of the magnitude of total thrust according to the present invention; and

(11) FIG. 10 is a graph of the control torque according to the present invention;

(12) FIG. 11 is a flowchart of a control process for an underactuated vehicle according to the present invention.

DETAILED DESCRIPTION OF THE INVENTION

(13) 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.sub.1, g.sub.2, g.sub.3 . . . g.sub.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.sub.1, f.sub.2, f.sub.3, f.sub.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.

(14) 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'Alembert principle. Almost global asymptotic stability of the feedback system over its state space is shown analytically and verified through numerical simulations.

(15) Guidance Scheme

(16) Coordinate Frame Definition

(17) 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.sup.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:

(18) $\begin{array}{cc}g=\left[\begin{array}{cc}R& b\\ 0& 1\end{array}\right]\mathrm{SE}\left(3\right),& \left(1\right)\end{array}$
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.sup.3 with SO(3).
Trajectory Generation for Unmanned Vehicle

(19) The trajectory generation problem consists of creating an appropriately smooth trajectory through a given finite set of desired waypoints which the underactuated vehicle's trajectory is required to pass through. These waypoints are given in the inertial frame as follows:
b.sub.d.sub.1,b.sub.d.sub.2, . . . ,b.sub.d.sub.n.sup.3, with b.sub.d.sub.i=b.sub.d(t.sub.i)|.sup.3 and t.sub.1<t.sub.2< . . . <t.sub.n.(1)

(20) Here b.sub.d(t) gives the desired position trajectory on R.sup.3 parameterized by time. A time trajectory for the position that is continuous and twice differentiable (i.e., b.sub.d(t)=.sup.2(.sup.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.

(21) Let g.sub.d(f)SE(3) be the desired pose (position, b.sub.d and attitude, R.sub.d) generated by the guidance scheme. Then the desired velocities (translational, .sub.d and rotational, .sub.d) are given by .sub.d(t) that satisfies the kinematics

(22) $\begin{array}{cc}{\stackrel{.}{g}}_d\left(t\right)=g_d\left(t\right){{}_d\left(t\right)}^{},\mathrm{where}{g}_d\left(t\right)=R_d{b}_{d}01,{}_d={}_d{v}_d{}^6\mathrm{and}{}_d^{}={}_d^{}{v}_{d}00{.Math.}^{44}.& \left(2\right)\end{array}$

(23) Here ().sup.={.sup.x00|, .sup.3}, is a vector space isomorphism from R.sup.6 to the associated Lie algebra, and ().sup.x:.sup.3.fwdarw..sup.33 is the skew-symmetric cross-product operator that gives the vector space isomorphism between R.sup.3 and so(3):

(24) $\begin{array}{cc}{x}^{}={\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]}^{}=\left[\begin{array}{ccc}0& -x_3& x_2\\ x_3& 0& -x_1\\ -x_2& x_1& 0\end{array}\right].& \left(4\right)\end{array}$

(25) 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.sub.*+(g,)+Bu, u.sup.4, B.sup.64,(5)
where II denotes the mass (m) and inertia (J) properties of the vehicle given as

(26) $\begin{array}{cc}=\left[\begin{array}{cc}J& 0\\ 0& m{I}_3\end{array}\right]{66}^{}& \left(6\right)\end{array}$
and I.sub.3 is the 33 identity matrix.

(27) The vector of known (modeled) moments and forces is denoted (g,).sup.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:

(28) $B=\left[\begin{array}{c}{I}_4\\ 0_{24}\end{array}\right],$

(29) where I.sub.4 is the 44 identity matrix. The adjoint operator on se(3) and the co-adjoint operator (ad) are defined in matrix form as

(30) $\begin{array}{cc}{\mathrm{ad}}_{}=\left[\begin{array}{cc}-{}^{}& 0\\ -{}^{}& -{}^{}\end{array}\right]\mathrm{and}{\mathrm{ad}}_{}^{*}={\left({\mathrm{ad}}_{}\right)}^T,& \left(7\right)\end{array}$

(31) where ,R.sup.3 denote the rotational and translational velocities of the underactuated vehicle, respectively, in frame . The vector of control inputs u.sup.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

(32) Feedback Guidance and Tracking on TSE(3)

(33) The desired trajectory is generated in SE(3) and the tracking errors are expressed with respect to inertial and body fixed frames as follows,

(34) TABLE-US-00001 {tilde over (b)} := b b.sub.d Position tracking error in inertial frame x := R.sub.d.sup.T{tilde over (b)} Position tracking error in body fixed frame {tilde over (v)} := v v.sub.d = Velocity tracking error in inertial frame Q := R.sub.d.sup.TR Attitude tracking error := Q.sup.T.sub.d Angular velocity tracking error
Tracking Errors Expressed in Body Frame

(35) Tracking errors on TSE(3) are defined as follows:

(36) $\begin{array}{cc}g=\left[\begin{array}{cc}R& b\\ 0& 1\end{array}\right],g_d=\left[\begin{array}{cc}{R}_d& b_d\\ 0& 1\end{array}\right].& \left(8\right)\end{array}$

(37) Tracking error on SE(3):

(38) $\begin{array}{cc}h=g_d^{-1}g=\left[\begin{array}{cc}Q& x\\ 0& 1\end{array}\right],& \left(9\right)\end{array}$

(39) where Q=R.sub.dR and x=R.sub.d(bb.sub.d)=R.sub.d{tilde over (b)}. Therefore, the kinematics for the pose tracking error is:
{dot over (h)}=h.sup.,(10)

(40) where

(41) $\begin{array}{cc}{}^{}=\left[\begin{array}{cc}{}^{}& \\ 0& 0\end{array}\right],& \left(11\right)\end{array}$
and =Q.sub.d,=Q(.sub.d+.sub.d.sup.x+x). The dynamics for the tracking errors in velocities is:

(42) 0$\begin{array}{cc}m\stackrel{.}{}=-{\mathrm{fe}}_3+m\left\{{}^{}Q{v}_d-Q\left({\stackrel{.}{}}_d+{\stackrel{.}{}}_d^{}x+{}_d^{}\mathrm{Qv}\right)\right)\}+{m\left(+Q{v}_d\right)}^{}\left(+Q{}_d\right)+mgQ{R}_d{e}_3,& \left(12\right)\\ J\stackrel{.}{}=+J\left({}^{}Q{}_d-Q{\stackrel{.}{}}_d\right)-{\left(+Q{}_d\right)}^{}J\left(+Q{}_d\right).& \left(13\right)\end{array}$
Translational Motion Control in Inertial Frame

(43) Because the desired position trajectory, b.sub.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.sub.d. Note that =R and Q.sub.d=RR.sub.d.sub.d=R.sub.d, where .sub.d=R.sub.d.sub.d={dot over (b)}.sub.d. Define {tilde over (b)}:=bb.sub.d and {tilde over ()}:=.sub.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 ()}=.sub.d, m{dot over ({tilde over ()})}=mge.sub.3fr.sub.3+m{dot over (v)}.sub.d,(14)
where r.sub.3=Re.sub.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.sub.1=[1 0 0] e.sub.2=[0 1 0] and e.sub.3=[0 0 1], are the standard basis vectors (as column vectors) of R.sup.3. Note that fr.sup.3=.sub.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.sub.3=Re.sub.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.sub.3. This feedback guidance plus trajectory tracking control is detailed in the following development.

(44) Consider the following Lyapunov function for the desired translational motion:
V.sub.tr({tilde over (b)},{tilde over ()})=m{tilde over ()}{tilde over ()}+{tilde over (b)}P{tilde over (b)},(15)

(45) where P33 is a positive definite control gain matrix. Therefore, along the translational error dynamics (3.2),

(46) $\begin{array}{cc}{\stackrel{.}{V}}_{\mathrm{tr}}=m\tilde{v}\stackrel{.}{\stackrel{~}{v}}+\tilde{b}P\stackrel{.}{\stackrel{~}{b}}=\tilde{v}\left(m\stackrel{.}{\stackrel{~}{v}}+P\tilde{b}\right)=\tilde{v}\left({\mathrm{mge}}_3-{\mathrm{fr}}_3-m{\stackrel{.}{v}}_d+P\tilde{b}\right).& \left(16\right)\end{array}$

(47) It is desired that the control force vector satisfies:
fr.sub.3=.sub.c=mge.sub.3+P{tilde over (b)}+L.sub.{tilde over ()}m{dot over (v)}.sub.d,(17)
where L.sub..sup.33 is another positive definite control gain matrix. Therefore, the control law for the magnitude of this force vector is:
f=e.sub.3.sup.TR.sub.T.sub.c=e.sub.3.sup.TR.sub.T(mge.sub.3+P{tilde over (b)}+L.sub.(R.sub.d)m{dot over ()}.sub.d).(18)
However, to achieve stable tracking of the desired translational motion, the attitude has to be controlled such that the direction of Re.sub.3=r.sub.3 agrees with that specified by (17). This is done in the following subsection.
Generating Desired Attitude Trajectory

(48) 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.sub.d (the desired attitude) as follows:

(49) $\begin{array}{cc}{r}_{3d}=\frac{{\mathrm{mge}}_3+P\tilde{b}+L_v\tilde{v}-m{\stackrel{.}{v}}_d}{{\mathrm{mge}}_3+P\tilde{b}+L_v\tilde{v}-m{\stackrel{.}{v}}_d}=R_d{e}_3.& \left(19\right)\end{array}$

(50) Select an appropriate s.sub.d(t).sup.2(R.sup.3) such that it is transverse to r.sub.3d. Now compute

(51) $\begin{array}{cc}{r}_{2d}=\frac{r_{3d}{s}_d}{r_{3d}{s}_d}=R_d{e}_2,\mathrm{and}{r}_{1d}=r_{2d}{r}_{3d}=R_d{e}_1.& \left(20\right)\end{array}$

(52) The desired attitude trajectory is then given by:
R.sub.d=[r.sub.2dr.sub.3dr.sub.2dr.sub.3d].(21)

(53) Few methods to select s.sub.d(t) appropriately is described in the following results.

(54) If s.sub.d is selected as
s.sub.d=1r.sub.3d+e.sub.1, with >3,(22)
then s.sub.d is always transverse and never parallel to r.sub.3d.
Proof.

(55) The condition on in (22) comes from the following observation:

(56) $\begin{array}{cc}\begin{array}{c}{r}_{3d}{s}_d=r_{3d}\left(1r_{3d}+e_1\right)\\ =\left(r_{3d}{r}_{3d}\right)1-\left(r_{3d}1\right)r_{3d}+r_{3d}{e}_1\\ =1-\left(r_{3d}1\right)r_{3d}+r_{3d}{e}_1,\end{array}& \left(23\right)\end{array}$
because r.sub.3d.sup.2, i.e., it is a unit vector. Define (r.sub.3d)=1(r.sub.3d1)r.sub.3d so that:
r.sub.3ds.sub.d=(r.sub.3d)+r.sub.3de.sub.1.(24)
It can be verified that the components of the vector (r.sub.3d) are bounded in the closed interval [4/3,0]. Therefore, the first component of the vector r.sub.3ds.sub.d is bounded in the closed interval [4/3,]. If >3, then this component is always positive, and therefore r.sub.3ds.sub.d0. Therefore, the choice of s.sub.d given in (19) ensures that it is always transverse to the generated r.sub.3d.

(57) The following statement gives a simpler choice of s.sub.d in .sup.3 that is transverse to r.sub.3d.sup.3. It also gives a vector that is orthogonal to the given unit vector.

(58) Let r.sub.3d=a.sub.1a.sub.2a.sub.3.sup.2.sup.3 be a known unit vector as given in (16). The vector s_d=a_2+a_3

(59) $\begin{array}{cc}{s}_d=\left[\begin{array}{c}{a}_2+a_3\\ a_3-a_1\\ -a_1-a_2\end{array}\right]& \left(25\right)\end{array}$
is orthogonal to r.sub.3d.
Proof.

(60) This is easily verified by the property of scalar product (orthogonal projection) as follows:

(61) $\begin{array}{c}{r}_{3d}{s}_d=a_1\left(a_2+a_3\right)+a_2\left(a_3-a_1\right)+a_3\left(-a_1-a_2\right)\\ =a_1\left(a_2+a_3-a_2-a_3\right)+a_2\left(a_3-a_3\right)=0.\end{array}$

(62) This shows that the vector s.sub.d as defined by (25) is orthogonal to the given vector r.sub.3d.

(63) As the next step, a control torque is selected such that R.fwdarw.R.sub.d in finite time; this is done using an attitude tracking control scheme outlined in the next section.

(64) Feedback Tracking Control Schemes

(65) 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.

(66) Finite-Time Stable Attitude Tracking Control on TSO(3)

(67) 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.sup.(1/p)+b.sup.(1/p)(a+b).sup.(1/p).(26)

(68) This inequality is strict if both a and b are non-zero. This above inequality holds for all p>1 as f(x)=x.sup.(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.

(69) Let K=(k.sub.1,k.sub.2,k.sub.3), where k.sub.1>k.sub.2>k.sub.31. Define

(70) $\begin{array}{cc}{s}_K\left(Q\right)=\underset{i=1}{\overset{3}{.Math.}}{k}_i\left(Q{e}_i\right)e_i,& \left(27\right)\end{array}$

(71) such that tK,IQ=s.sub.K(Q). Here A,B=(AB), which makes K,IQ a Morse function defined on. Let be a closed subset containing the identity in its interior, defined by

(72) $\begin{array}{cc}S=\left\{Q\text{:}{Q}_{\mathrm{ii}}0\mathrm{and}{Q}_{\mathrm{ij}}{Q}_{\mathrm{ji}}0i,j\left\{1,2,3\right\},ij\right\}.& \left(28\right)\end{array}$

(73) Then for QS, we have
s.sub.K(Q).sup.Ts.sub.K(Q)tr(KKQ).(29)

(74) 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.

(75) Consider the attitude dynamics of Eq. (13) with s.sub.K(Q) as defined in (24). Define

(76) $\begin{array}{cc}{z}_K\left(Q\right)=\frac{s_K\left(Q\right)}{{\left(s_K\left(Q\right)s_K\left(Q\right)\right)}^{1-1/p}},\mathrm{and}& \left(30\right)\\ w\left(Q,w\right)=\underset{t}{-}{s}_K\left(Q\right)=\underset{i=1}{\overset{3}{.Math.}}{k}_i{e}_i\left({\mathrm{Qe}}_i\right),& \left(31\right)\end{array}$

(77) where P is as defined in Lemma 4.1. Further, let L.sub. be a positive definite control gain matrix such that L.sub.J is positive semidefinite, let k.sub.p>1 and define such that

(78) 0${}^p=\frac{{}_{L,min}}{{}_{J,\mathrm{ma}x}}>0.$

(79) Consider the feedback control law for given by

(80) $\begin{array}{cc}=J\left(Q{\stackrel{.}{}}_d-\frac{H\left(s_K\left(Q\right)\right)}{{\left(s_K\left(Q\right)s_K\left(Q\right)\right)}^{1-1/p}}w\left(Q,\right)\right)+{\left(Q{}_d\right)}^{}J\left(Q{}_d-z_K\left(Q\right)\right)+J\left(z_K\left(Q\right)Q{}_d\right)+J\left(+Q{}_d\right)z_K\left(Q\right)-k_p{s}_K\left(Q\right)-\frac{L_{}\left(Q,\right)}{{\left(\left(Q,\right)L_{}\left(Q,\right)\right)}^{1-1/p}},& \left(32\right)\\ \mathrm{where}& \\ \left(Q,\right)=+z_K\left(Q\right),& \left(33\right)\\ \mathrm{and}H\left(x\right)=I-\frac{2\left(1-1/p\right)}{\mathrm{xx}}\mathrm{xx}.& \left(34\right)\end{array}$

(81) Then the feedback attitude tracking error dynamics given by (13) is stabilized to (Q,)=(I,0) in finite time.

(82) Proof.

(83) Consider =z.sub.K(Q) and define the Morse-Lyapunov function K,IQ on SO(3). Then the time derivative of this Morse-Lyapunov function along the attitude kinematics is given by

(84) $\begin{array}{cc}\underset{t}{-}K,I-Q=s_K\left(Q\right)=-s_K\left(Q\right)z_K\left(Q\right)=-\frac{s_K\left(Q\right)s_K\left(Q\right)}{{\left(s_K\left(Q\right)s_K\left(Q\right)\right)}^{1-1/p}}=-{\left(s_K\left(Q\right)s_K\left(Q\right)\right)}^{1/p}-{\left(K,l-Q\right)}^{1/p},& \left(35\right)\end{array}$

(85) where we employed inequality (26) in Lemma 1. Therefore, when (Q,)=0, one can conclude that K,IQ.fwdarw.0 in finite time for all initial Q in the subset defined in Lemma 4.1, which yields Q.fwdarw.I in finite time once Q.

(86) The control law is then designed to ensure that (Q,).fwdarw.0 in finite time. Consider the Lyapunov function
V.sub.rot(Q,)=k.sub.pK,IQ+(Q,)J(Q,),(36)

(87) for the attitude dynamics of (13) with control law (32). The time derivative of this Lyapunov function along this feedback dynamics is given by

(88) $\begin{array}{cc}{\stackrel{.}{V}}_{rot}=k_p{s}_K\left(Q\right)+\left(Q,\right)J\stackrel{.}{}\left(Q,\right)=k_p{s}_K\left(Q\right)+\left[{}_c+J\left(Q{}_d-z_K\left(Q\right)\right)+J\left({}^{}Q{}_d-Q{\stackrel{.}{}}_d\right)+\frac{JH\left(s_K\left(Q\right)\right)}{{\left(s_K\left(Q\right)s_K\left(Q\right)\right)}^{1-1/p}}w\left(Q,\right)\right]& \left(37\right)\end{array}$

(89) After substituting the control law (32) into the expression (37) and carrying out several algebraic simplifications, one obtains
{dot over (V)}.sub.rot=k.sub.p(s.sub.K(Q)s.sub.K(Q)).sup.1/p((Q,)L.sub.(Q,)).sup.1/p(k.sub.pK,IQ).sup.1/p((Q,)J(Q,)).sup.1/p,

(90) for (Q,)x.sup.3, where is as defined in (28). After substituting inequality (26) into the above expression, one obtains

(91) $\begin{array}{cc}{\stackrel{.}{V}}_{rot}-{\left(k_{p}K,I-Q+\left(Q,\right)J\left(Q,\right)\right)}^{1/p}=-V_{\mathrm{rot}}^{1/p},& \left(38\right)\end{array}$

(92) which implies that the feedback attitude tracking control system is (locally) finite-time stable at (Q,)=(I,0).

(93) Note that the domain of attraction shown in the above analysis is (Q,)x.sup.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.

(94) Stability of the Overall Feedback System on TSE(3)

(95) 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.sub.d(t),R.sub.d(t),.sub.d(t),.sub.d(t)). Moreover, the domain of convergence is almost global over the state space.

(96) Proof.

(97) This attitude tracking control scheme given by Theorem 4.1 ensures that R(t)=R.sub.d(t) for time tT where T>0 is finite. Therefore, one obtains r.sub.3=Re.sub.3=r.sub.3d for tT, and from (18) and (19),
f=mge.sub.3+P{tilde over (b)}+L{tilde over ()}m{dot over ()}.sub.d, and
fr.sub.3=fr.sub.3d=mge.sub.3+P{tilde over (b)}+L.sub.{tilde over ()}m{dot over ()}.sub.d for tT.(39)

(98) Substituting (39) in (16) gives:

(99) $\begin{array}{cc}\begin{array}{c}{\stackrel{.}{V}}_{tr}=\tilde{v}\left(m{\mathrm{ge}}_3-f{r}_{3d}-m{\stackrel{.}{v}}_d+P\tilde{b}\right)\\ =-\tilde{v}{L}_v\tilde{v}0.\end{array}& \left(40\right)\end{array}$

(100) Since L.sub. is positive definite, {dot over (V)}.sub.tr=0 if and only if {tilde over ()}=0. Applying LaSalle's invariance principle, one can show that when {tilde over ()}=0, m{dot over ({tilde over ()})}=mge.sub.3fr.sub.3d+m{dot over ()}.sub.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)

(101) Note that the finite-time stability of the attitude control scheme guarantees that the desired thrust direction, r.sub.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

(102) Application to Autonomous UAV Navigation

(103) 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.sup.x,(41)
m{dot over ()}=mgRe.sub.3fe.sub.3,(42)
J{dot over ()}=J+(43)

(104) Each propeller can generate a thrust f.sub.i proportional to the square of the corresponding motor speed i.e., f.sub.i=k.sub.f.sub.i.sup.2, and the torque generated by each actuator is directly proportional to its thrust i.e., .sub.i=k.sub..sub.i.sup.2. The first and second axes (a.sup.1 and a.sub.2) of the body-fixed frame lie in the plane normal to the axes of the propellers. The total thrust, f=.sub.i=1.sup.4f.sub.i acts along the third axis a.sub.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 .sub.i according to

(105) $u={\stackrel{\_}{}}_1^2{\stackrel{\_}{}}_2^2{\stackrel{\_}{}}_3^2{\stackrel{\_}{}}_4^2,\mathrm{where}=-k_f-k_f-k_f-k_{f}0-k_{f}D0k_{f}D{k}_{f}D0-k_{f}D0-k_{}{k}_{}-k_{}{k}_{}{}^{44}$
is a constant invertible matrix for k.sub.f0 and k.sub.0. Then,

(106) $\begin{array}{cc}\left[\begin{array}{c}{\stackrel{\_}{}}_1^2\\ {\stackrel{\_}{}}_2^2\\ {\stackrel{\_}{}}_3^2\\ {\stackrel{\_}{}}_4^2\end{array}\right]=K^{-1}\left[\begin{array}{c}f\\ \end{array}\right]& \left(44\right)\end{array}$
Guidance Algorithm

(107) 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:

(108) TABLE-US-00002 PSEUDO CODE Integrated Guidance and Feedback Control procedure IGFC b.sub.d,f,R.sub.d, b.sub.d and f are in set Fix the inertial frame b = b.sub.xb.sub.yb.sub.z Initialize the UAV with respect to an inertial frame Select b.sub.d.sub.1,b.sub.d.sub.2, . . . ,b.sub.d.sub.n select desired waypoints in R.sup.3 Generate b.sub.d(t) = C.sup.2( .sup.3) Generate a smooth trajectory as a function of time in while t > t.sub.0 do f (18) Compute the UAV control thrust in r.sub.3d (19) desired thrust vector direction in the inertial frame for s.sub.d(t).sup.2 (R.sup.3) do s.sub.dr.sub.3d r.sub.2d (20) end for R.sub.d r.sub.2d r.sub.3d r.sub.2d r.sub.3d Generate the desired attitude trajectory (32) Compute attitude control torque such that R(t) .fwdarw. R.sub.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.sub.d,.sub.d,{dot over ()}.sub.d,R.sub.d,.sub.d,{dot over ()}.sub.d) end procedure
Numerical Validation

(109) Discretization of the UAV Dynamics

(110) 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'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.

(111) Let h0 denote the fixed time step size, such that h=t.sub.k+1t.sub.k. Then the discretized equations of motion obtained in the form of LGVI as,

(112) $\begin{array}{cc}{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+hmg{R}_{k+1}{e}_3-h{f}_k{e}_3,{}_{d_{k+1}}^{}=\frac{1}{h}\log \left(R_{d_k}{R}_{d_{k+1}}\right),J{}_{k+1}=F_{k}J{}_k+h{}_k,& \left(45\right)\end{array}$
where f.sub.kexp(h.sub.k.sup.x)SO(3) guarantees that R.sub.k evolves on SO(3).

(113) 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.sup.2. The helical desired trajectory and the initial conditions are given as follows
b.sub.d(t)=0.4 sin(t)0.6 cos(t)0.4t,
b(0)=100,
R(0)=I,
(0)=000; {dot over ()}(0)=000 and (0)=000.

(114) 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.sub.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.sup.1((tr(Q)1)).

(115) The numerical results are obtained after selecting the following gain values:
P=38I.sup.33; L.sub.=25I.sup.33
L.sub.=3.5I.sup.33; p=0.75; =0.04 and k.sub.p=4.5.
These gain values were selected after trial and error, and provide desirable transient response characteristics of the overall control scheme.

(116) 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>0. The attitude tracking error given by the principal angle, converges in finite time which indicates that R tracks R.sub.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.

(117) 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 and 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 tr 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 tr, vehicle 10 determines 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 as 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.

(118) 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.

(119) 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'Alembert principle. Almost global asymptotic stability of the feedback system over its state space is shown analytically and verified through numerical simulations.

(120) 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.

(121) 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.