MPE Mathematical Problems in Engineering 1563-5147 1024-123X Hindawi Publishing Corporation 10.1155/2014/464056 464056 Research Article Trajectory Evaluation of Rotor-Flying Robots Using Accurate Inverse Computation Based on Algorithm Differentiation http://orcid.org/0000-0001-5067-1169 He Yuqing Zhou Yingjun Han Jianda Yaman Fatih State Key Laboratory of Robotics, Shenyang Institute of Automation Chinese Academy of Sciences, Shenyang 110016 China cas.cn 2014 1382014 2014 26 03 2014 09 06 2014 11 06 2014 13 8 2014 2014 Copyright © 2014 Yuqing He et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Autonomous maneuvering flight control of rotor-flying robots (RFR) is a challenging problem due to the highly complicated structure of its model and significant uncertainties regarding many aspects of the field. As a consequence, it is difficult in many cases to decide whether or not a flight maneuver trajectory is feasible. It is necessary to conduct an analysis of the flight maneuvering ability of an RFR prior to test flight. Our aim in this paper is to use a numerical method called algorithm differentiation (AD) to solve this problem. The basic idea is to compute the internal state (i.e., attitude angles and angular rates) and input profiles based on predetermined maneuvering trajectory information denoted by the outputs (i.e., positions and yaw angle) and their higher-order derivatives. For this purpose, we first present a model of the RFR system and show that it is flat. We then cast the procedure for obtaining the required state/input based on the desired outputs as a static optimization problem, which is solved using AD and a derivative based optimization algorithm. Finally, we test our proposed method using a flight maneuver trajectory to verify its performance.

1. Introduction

The autonomous rotor-flying robot (RFR) is one of the frontier research topics in the field of robotics. Extensive research has been conducted on issues related to RFR, including flight control [1, 2], path/trajectory planning [3, 4], and intelligent navigation [5, 6].

Flight maneuvering is a very important ability for an RFR system because it can greatly extend the system’s field of applications. However, the flight maneuver control of an RFR is a challenging and absorbing problem in the field of autonomous RFR research . This can be explained from the following two points. On the one hand, the model of an RFR system is an extremely complicated structure [7, 8], designing a suitable controller for which is very difficult. On the other hand, flight maneuvering requires that both the interval states (i.e., the attitude angles and the angular rates) and the control surfaces to have a large scope variation (or even approach the largest permitted value) . This may produce a great deal of uncertainty and thus presents that this flight maneuvering is dangerous and difficult to be implemented and thus requires a much more robust controller than usual .

Most work on flight maneuver control, such as [11, 12], is based on successful flight demonstrations and grounded in learning techniques used to update the control laws. These methods have successfully pushed the envelope of what is possible with autonomous control, but they lack performance guarantees and are limited to situations where safety is not a critical concern. Thus, it is very important to assess the flight maneuvering ability of an RFR system prior to designing a controller and conducting tests. Some research has been conducted in this area. For instance, research in  is based on reachability analysis and explores aerobatic maneuvers and multiple vehicle collision avoidance of the RFR systems. This involves designing a sequence of modes and conducting safe transition from one to the next. However, reachability analysis is very time consuming, due to which the proposals in  are viable only for simplified models. Similarly,  involves a feasibility analysis of planned trajectories by using a greatly simplified dynamics model (without considering angular dynamics). Both the positions and attitude angles are predetermined and then used to evaluate the feasibility of the planned trajectories. Although the algorithm is simple and easily implemented, it is unfit for many planning algorithms since for which it is difficult to obtain the positions and attitude angles simultaneously.

In this paper, based on a full state dynamics model, we investigate the feasibility analysis problem of a flight maneuvering trajectory. This is actually an inverse computation problem, that is, to compute both the interval states and the inputs based on predetermined position, yaw angle, and their higher-order derivatives. In our work, we model it as a static optimization problem and then use a derivative-based optimization algorithm to obtain an accurate solution of it. We use algorithm differentiation (AD) to obtain precise higher-order derivatives of the nonlinear cost function. The advantages of our proposed algorithm are as follows: ( 1 ) only with desired positions and their derivatives, which is the case for many planning algorithm, the feasibility of the trajectories (including inner states and inputs) can be evaluated; ( 2 ) inverse computation can be very accurate because AD is a numerical algorithm that can precisely obtain the derivative of a nonlinear function. Thus, an accurate pointwise numerical feasibility analysis of planned trajectories becomes possible.

The remainder of this paper is organized as follows. We first present a model of the RFR system and show that it is flat by taking the three positions and the yaw angle as a group of flat outputs. This allows us to model the trajectory evaluation problem as an optimization problem by taking into account the dynamical model, which is solved by using the AD algorithm along with a derivative-based optimization algorithm. Finally, in order to test our proposed method, we use it to analyze a flight maneuvering trajectory.

2. Dynamical Model of the RFR System

Usually, the complete dynamical model of an RFR system can be divided into three parts: the actuator dynamics, the aerodynamics, and the rigid body dynamics. In this paper, only the rigid body dynamics as shown in the following equation (1) is considered : (1) [ p ˙ v ˙ p Θ ˙ ω ˙ b ] = [ v p 1 m R f b Ψ ω b J - 1 ( τ b - ω b × J ω b ) ] , where p = [ x y z ] T R 3 and v p = [ x ˙ y ˙ z ˙ ] R 3 are translational position and velocity vector of the RFR in the inertia frame; R is the rotation matrix between the body frame and the inertia frame; ω b is angular velocity vector; Θ = [ ϕ θ ψ ] T is Euler angle vector; m and J are, respectively, the mass and inertia matrix of the RFR; Ψ is the transformation matrix from angular velocity vector in body frame to angular velocity vector in inertia frame; and f b and τ b are force and moment of the RFR presented in body frame. Different kinds of RFR have very different aerodynamics, that is, the relationship between the force/moment and the control surfaces. Here we focus on the quadracopter flying robot, whose driving forces and moments can be denoted as follows: (2) f b = [ f x f y f z ] = [ 0 0 T M ] = [ 0 0 F 1 + F 2 + F 3 + F 4 ] , τ b = [ M x M y M z ] = [ l ( F 4 - F 2 ) l ( F 3 - F 1 ) l ( F 1 - F 2 + F 3 - F 4 ) ] , where F 1 , F 2 , F 3 , and F 4 are force produced by four rotors, respectively, and l is the distance between each rotor center and the RFR’s center of gravity (see Figure 1).

Thus, define inputs as (3) u = [ u 1 u 2 u 3 u 4 ] [ T ¨ M M x M y M z ] . System (1) can be rewritten as (4) x ¨ = cos ψ sin θ cos φ + sin ψ sin φ m T M , y ¨ = sin ψ sin θ cos φ - sin φ cos ψ m T M , z ¨ = - g + cos θ cos φ m T M , φ ¨ = I y - I z I x θ ˙ ψ ˙ + 1 I x u 2 , θ ¨ = I z - I x I y φ ˙ ψ ˙ + 1 I y u 3 , ψ ¨ = I x - I y I z θ ˙ φ ˙ + 1 I z u 4 , T ¨ M = u 1 .

It can be easily shown that system (4) is input-to-state feedback linearizable through defining the following state vector: (5) [ x y z x ˙ y ˙ z ˙ x ¨ y ¨ z ¨ x y z ψ ψ ˙ ] T . From (1) and (2), we have (6) p ( 4 ) = - 1 m R [ ω b × ( ω b × [ 0 0 T M ] ) ] + 1 m R [ 0 - T M 0 T M 0 0 0 0 0 ] × J - 1 ( τ b - ω b × J ω b ) - 2 m R { ω b × [ 0 0 T ˙ M ] T } - 1 m R [ 0 0 1 ] T T ¨ M , ψ ¨ = - [ 0 sin ϕ cos θ cos ϕ cos θ ] J - 1 ( ω b × J ω b - τ b ) + ( q ϕ ˙ cos ϕ - r ϕ ˙ sin ϕ ) cos θ × [ p + 2 q sin ϕ tan θ + 2 r cos ϕ tan θ ] and the system can be transformed into the following linear form: (7) p ( 4 ) = [ x ( 4 ) y ( 4 ) z ( 4 ) ] = [ v 1 v 2 v 3 ] , ψ ¨ = v 4 with the feedback linearization controller (8) u = [ g 2 g 3 ] - 1 ( [ v 1 v 2 v 3 v 4 ] - [ f 2 f 3 ] ) , where (9) f 2 = - 2 m T ˙ M R ( ω b × [ 0 0 1 ] ) - 1 m T M R [ ω b × ( ω b × [ 0 0 1 ] ) ] - 1 m T M R { [ J - 1 ( ω b × J ω b ) ] × [ 0 0 1 ] } ; g 2 = [ - 1 m R [ 0 0 1 ] T - T M m R [ 0 1 0 - 1 0 0 0 0 0 ] J - 1 ] ; f 3 = - [ 0 sin ϕ cos θ cos ϕ cos θ ] J - 1 ( ω b × J ω b ) + ( q cos ϕ - r sin ϕ ) × sec θ [ p + 2 q sin ϕ tan θ + 2 r cos ϕ tan θ ] ; g 3 = [ 0 [ 0 sin ϕ cos θ cos ϕ cos θ ] J - 1 ] .

That means system (1) is flat and that the [ p T , ψ ] are the corresponding flat outputs.

3. Automatic Differential Algorithm Based Inverse Computation

AD is a numerical method that is effective to accurately compute the derivatives of some complicated nonlinear function. In this section, we will briefly introduce how to conduct the inverse computation of a nonlinear flat system using AD algorithm .

Consider the following nonlinear system: (10) x ˙ = f ( x ) , y = h ( x ) .

It has been shown that the inverse computation problem of system (10), that is, to obtain a state vector x d with h ( x d ) = y d (a predefined output), is solvable if y = h ( x ) is a flat output of system (1) .

Derivatives of y with respect to x are necessary to effectively solve the preceding inverse problem. In the following contents of this section, the basic idea on how to obtain y / x using AD algorithm will be introduced.

For this purpose, we firstly use the following Taylor series to denote the state vector x ( t ) : (11) x ( t ) = x 0 + x 1 t + x 2 t 2 + + x d t d . Provided that f ( * ) is d times continuously differentiable, z = f ( x ) can also be expressed by Taylor series as follows: (12) z ( t ) = z 0 + z 1 t + z 2 t 2 + + z d t d + O ( t d + 1 ) .

It has been shown that each Taylor coefficient z i    ( i = 0,1 , , d ) is uniquely determined by the coefficients x i    ( i = 0,1 , , d ) as follows [15, 16]: (13) z 0 = f ( x 0 ) z 1 = f ( x 0 ) x 0 x 1 z 2 = f ( x 0 ) x 0 x 2 + 1 2 2 f ( x 0 ) x 0 x 0 x 1 x 1

Thus, if x i    ( i = 0,1 , , d ) in (11) are preknown, all z i    ( i = 0,1 , , d ) can be obtained based on some derivatives, which can be computed by basic “forward mode” of AD . Similarly, the following A j - i can also be derived using “reverse mode” of AD : (14) A j - i ( j i ) z j - i x 0 .

Furthermore, based on (10), we have (15) z ( t ) = z 0 + z 1 t + z 2 t 2 + + z d t d + O ( t d + 1 ) = f ( x ) = x ˙ = x 1 + 2 x 2 t + + d x d t d - 1 ; that is, (16) z i ( i + 1 ) x i + 1 , i = 0 , , d - 1 .

Through (16) and (13), we can easily compute all z i based on x 0 .

Define a new matrix as (17) B k d x k + 1 d x 0 = 1 k + 1 d z k d x 0 = 1 k + 1 j = 0 k z k x j d x j d x 0 .

From , we have (18) z k x j = z k - j x 0 .

Thus, B k can be iteratively obtained as follows: (19) B k = 1 k + 1 j = 0 k z k - j x 0 d x j d x 0 = 1 k + 1 ( A k + j = 0 k A k - j B j - 1 ) .

Furthermore, if we denote y ( t ) as (20) y ( t ) = y 0 + y 1 t + y 2 t 2 + + y d t d + O ( t d + 1 ) ,

then the same process can be used to compute y through the second equation of (10); that is, with a preknown x 0 , compute y i by forward mode and compute the following derivatives C j - i by reverse mode: (21) C j - i y j x i .

Also, it is not difficult to show that the derivatives of output with respect to the states can be denoted as follows: (22) D k d y k d x 0 = j = 0 k y k x j d x j d x 0 = C k + j = 1 k C k - j B j - 1 .

Up to now, we have shown that, with a predefined state x 0 , both the corresponding outputs y ( x 0 ) and its higher-order derivatives with respect to time, as well as the derivatives with respect to state x 0 (22), can be obtained through AD algorithm. These results can be further used to conduct the inverse computation of system (1), and the detailed steps are given as follows.

Initialization. d (the highest order of the outputs’ derivatives); γ (step length of searching algorithm); k = 0 (iterative number).

Step 1.

Compute the output vector Y ( x k ) using forward mode of AD algorithm at each time instant as a nonlinear map; that is, (23) Θ : x k Y ( x k ) ( y 0 ( x k ) y d ( x k ) ) .

Step 2.

Based on (22), AD can be used to obtain the partial derivative of Y with respect to x 0 ; that is, (24) Θ ( x k ) = Y x k = ( D 0 D d ) .

Step 3.

Update the state x using the following searching iteration: (25) x k + 1 x k - γ ( Ξ ( x k ) ) + Ξ ( x k ) = x k - γ ( Ξ ( x k ) ) + [ Θ ( x k ) - Y d ] = x k - γ ( Θ ( x 0 ) ) + [ Θ ( x k ) - Y d ] , where superscript “+” means the pseudoinverse of a matrix; that is, (26) Q + = ( Q T Q ) - 1 Q T , Ξ ( x ) = Θ ( x ) - Y d .

Step 4.

Go to Step 1 until the terminal conditions are satisfied.

If the system is nonautonomous, that is, it can be rewritten as following form, (27) x ˙ = f ( x , u ) , y = h ( x ) . We can first extend it into the following form: (28) x ˙ = f ( x , u ) , u ˙ = 0 , y = h ( x ) and the new system can be rewritten as (29) x - ˙ = F ( x - ) , y = H ( x - ) , where (30) x - = [ x u ] .

Thus, the inverse computation can be conducted using the preceding algorithm.

As for the proposed quadrocopter system introduced in Section 2, this algorithm can be directly utilized through defining the following output vector: (31) Y [ p T p ˙ T p ¨ T p T ( p ( t ) ) T ψ ψ ˙ ψ ¨ ] 18 × 1 T .

4. Evaluation of Circle Flying Maneuver

In this section, a so-called “Circle Maneuver,” that is, the RFR system flies along a circle as shown in the following equation (32) and Figure 2 , of the Quadrocopter is taken as an example to show the validity of the proposed algorithm. Consider (32) ψ d = π + t T × 2 π , x d = 50 sin ( t T × 2 π ) , y d = 50 cos ( t T × 2 π ) , z d = 0 , where T is the period of the circle maneuver.

Circle maneuver trajectory.

The parameters of the RFR system are as follows: (33) m = 9.5 kg J = [ 0.1364 0 0 0 0.5782 0 0 0 0.6306 ] N · m .

The trajectory feasibility is decided by some constrains on both inputs and states. Here the following constrains are considered.

Input Constrains. Consider (34) T M [ 49.4,190.8 ] N , M x [ - 25.5,25.5 ] N · m , M y [ - 40.2,40.2 ] N · m , M z [ - 34.6,34.6 ] N · m .

Angular Velocity Constrains. Consider (35) p , q [ - 0.4,0.4 ] rad / s , r [ - 1.45,1.45 ] rad / s .

Attitude Constrains. Consider (36) θ , ϕ [ - 0.5,0.5 ] rad .

In this paper, the software toolbox LIEDRIVERS  based on ADOL-C is used as the basic AD algorithm, and the optimization searching is implemented using the golden sector algorithm, instead of the direct derivative-based algorithm as in Section 4. During the simulations, we test the circle maneuvers with different period T , and the results are given in Figures 3, 4, and 5.

Control input: T ¨ M (a); M x (b); M y (c); M z (d).

State: ϕ (a); θ (b).

State: p (a); q (b); r (c); T M (d).

From these figures, we can obtain the following results.

For input constraints (Figures 3 and 5), only the maneuvering trajectories with T = 10 s are infeasible because the required dragging force is almost 210 N, larger than the maximum permitted value of 190.8 N, while the other inputs, that is, the three moments, are all much smaller than the maximum values.

For angular velocity constrains (Figure 5), again, only the maneuvering trajectories with T = 10 s are infeasible, and the maneuvering trajectories with T = 12.5 s are dangerous since the maximum angular velocity approaches the maximum value of the permitted angular velocity.

Based on the attitude constrains (Figure 4), the maneuvering trajectories with T = 14.3 s , 12.5 s, and 10 s are all infeasible.

Finally, from the preceding results, it can be seen that the circle maneuvers are really a highly dynamical maneuver, and the quantitative analysis prior to the flight test is useful and necessary to evaluate whether or not a maneuver trajectory is feasible or even dangerous.

5. Conclusions

In this paper, the flight maneuvering trajectories evaluation problem is researched, and the algorithm differentiation (AD) is used to realize the inverse computation of the rotor-flying robot (RFR) system. This paper starts from constructing nonlinear dynamical model of an RFR system. Then, we show that the RFR system model is flat taking translational positions and yaw angle as flat outputs. After that, the scheme of AD based inverse computation is introduced, that is, computing the internal states and inputs based on the desired flat outputs. Finally, with the proposed scheme, a typical flight maneuver, that is, the circle maneuver, is analyzed.

The advantages of our proposed algorithm are as follows: ( 1 ) only desired positions and their derivatives, which is the case for many planning algorithm, and the feasibility of the trajectories (including inner states and inputs) can be evaluated; ( 2 ) inverse computation can be very accurate because AD is a numerical algorithm that can precisely obtain the derivative of a nonlinear function. Thus, an accurate pointwise numerical feasibility analysis of planned trajectories becomes possible.

The method proposed in this paper can be used to not only evaluate the feasibility and validity of the maneuver flying trajectories, but also realize the tracking control algorithm, where the computed internal states and inputs are useful for attenuating the online computational burden of the tracking controller and thus improving the closed loop performance. This will be one of our future’s works.

Conflict of Interests

The authors declare that there is no conflict of interests regarding the publication of this paper.

Acknowledgments

The authors would like to present their thanks to Professor Klaus Röbenack at Technique University of Dresden (Germany) for the related discussion and suggestions on AD algorithm. This work is supported by the National Natural Science Foundation of China (Grants no. 61035005 and no. 61203340).

He Y. Q. Han J. D. Acceleration-feedback-enhanced robust control of an unmanned helicopter Journal of Guidance, Control, and Dynamics 2010 33 4 1236 1250 10.2514/1.45659 2-s2.0-77956379325 Gillula J. H. Hoffmann G. M. Vitus M. P. Tomlin C. J. Applications of hybrid reachability analysis to robotic aerial vehicles International Journal of Robotics Research 2011 30 3 335 354 10.1177/0278364910387173 2-s2.0-79952119221 Andert F. Adolf F. Goormann L. Dittrich J. Mapping and path planning in complex environments: an obstacle avoidance approach for an unmanned helicopter Proceedings of the IE E E International Conference on Robotics and Automation May 2011 Shanghai, China 745 750 10.1109/ICRA.2011.5979535 2-s2.0-84857364074 Flores G. Zhou S. T. Lozano R. Castillo P. A vision and GPS-based real time trajectory planning for a MAV in unknown and low-sunlight environments Journal of Intelligent Robotic Systems 2014 74 59 67 Bristeau P. Dorveaux E. Vissière D. Petit N. Hardware and software architecture for state estimation on an experimental low-cost small-scaled helicopter Control Engineering Practice 2010 18 7 733 746 10.1016/j.conengprac.2010.02.014 2-s2.0-77953609188 Mourikis A. I. Trawny N. Roumeliotis S. I. Johnson A. E. Ansar A. Matthies L. Vision-aided inertial navigation for spacecraft entry, descent, and landing IEEE Transactions on Robotics 2009 25 2 264 280 10.1109/TRO.2009.2012342 2-s2.0-67349093853 Bangura M. Mahony R. Nonlinear dynamic modeling for high performance control of a quadrotor Proceedings of the Australasian Conference on Robotics and Automation (ACRA '12) December 2012 Wellington, New Zealand 1 10 2-s2.0-84879951924 Hoffmann G. M. Huang H. Waslander S. L. Tomlin C. J. Precision flight control for a multi-vehicle quadrotor helicopter testbed Control Engineering Practice 2011 19 9 1023 1036 10.1016/j.conengprac.2011.04.005 2-s2.0-79960891417 Mueller M. W. Hehn M. D'Andrea R. A computationally efficient algorithm for state-to-state quadrocopter trajectory generation and feasibility verification Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS '13) 2013 Tokyo, Japan 3480 3486 Song D. Han J. Liu G. Active model-based predictive control and experimental investigation on unmanned helicopters in full flight envelope IEEE Transactions on Control Systems Technology 2013 21 4 1502 1509 10.1109/TCST.2012.2208968 2-s2.0-84879895947 Purwin O. D'Andrea R. Performing aggressive maneuvers using iterative learning control Proceedings of the IEEE International Conference on Robotics and Automation May 2009 Kobe, Japan 1731 1736 10.1109/ROBOT.2009.5152599 2-s2.0-70350350892 Lupashin S. Schöllig A. Sherback M. D'Andrea R. A simple learning strategy for high-speed quadrocopter multi-flips Proceedings of the IEEE International Conference on Robotics and Automation (ICRA '10) May 2010 Anchorage, Alaska, USA 1642 1648 10.1109/ROBOT.2010.5509452 2-s2.0-77955839332 Röbenack K. Vogel O. Computation of state and input trajectories for flat systems using automatic differentiation Automatica 2004 40 3 459 464 10.1016/j.automatica.2003.10.018 MR2145274 2-s2.0-1642525817 Andreas G. Andrea W. Evaluating Derivatives: Principles and Techniques of Algorithm Differentiation 2008 2nd Philadelphia, Pa, USA Society for Industrial and Applied Mathematics 10.1137/1.9780898717761 MR2454953 Christianson B. Reverse accumulation and accurate rounding error estimates for Taylor series coefficients Optimization Methods and Software 1992 1 1 81 94 10.1080/10556789208805508 2-s2.0-0026862533 Röbenack K. Automatic differentiation and nonlinear controller design by exact linearization Future Generation Computer Systems 2005 21 8 1372 1379 10.1016/j.future.2004.11.004 2-s2.0-24044480950 Röbenack K. Winkler J. Wang S. Q. LIEDRIVERS—a toolbox for the efficient computation of Lie derivatives based on the object-oriented algorithmic differentiation package ADOL-C Proceedings of the 4th International Workshop on Equation-Based Object-Oriented Modeling Languages and Tools 2011 Zurich, Switzerland ETH Zurich