IROS2019国际学术会议论文集1801

Robust Trajectory Planning for a Multirotor against Disturbance based on Hamilton-Jacobi Reachability Analysis Hoseong Seo, Donggun Lee, Clark Youngdong Son, Claire J. Tomlin, and H. Jin Kim AbstractEnsuring safety in trajectory planning of multiro- tor systems is an essential element for risk-free operation. Even if the generated trajectory is known to be safe in the planning phase, unknown disturbance during an actual operation can lead to a dangerous situation. This paper proposes safety- guaranteed receding horizon planning against unknown, but bounded, disturbances. We fi rst characterize forward reachable set (FRS) of the system, the set of states after a certain duration considering all possible disturbances, using Hamilton- Jacobi (HJ) reachability analysis. To compute the FRSs in real-time, we conservatively approximate the true FRS and perform ellipsoidal parameterization on the FRSs. Using the FRSs, we can plan a robust trajectory that avoids risky regions and rapidly re-plan the trajectory when the system encounters sudden disturbance. The proposed method is validated through an experiment of avoiding obstacles in a wind. I. INTRODUCTION Safety guarantee is an important topic in fully utilizing high maneuverability of multirotor systems for a broad range of applications. Such importance has led to recent works on generating safe fl ight trajectories for multirotor systems in cluttered environments. The authors of 1 proposed a method for planning a time-parameterized trajectory in a confi ned space. With the aid of a sampling-based planner, collision-free waypoints were specifi ed and used to enforce the planned trajectory to pass through those waypoints. Sim- ilar approaches were proposed in 2, 3 with consideration of a collision cost in optimizing the fl ight path. Some works tried to decompose a cluttered space into free space and occupied space. The free space was modeled as rectangular grids 4 or maximal ellipsoids 5 and then considered as inequality constraints in trajectory optimization. Although many existing works on trajectory planning offer appealing solutions for safe fl ight, the generated trajectory may not be suffi cient to guarantee safety if affected by un- known disturbances in actual environments. Various sources This material is based upon work supported by the Ministry of Trade, Industry however, our work presents a closed-form solution of the reachable set as ellipsoids. C. Outline We formulate a problem and defi ne the error FRS in Section II. Section III describes the analytic solution of the HJ reachability analysis and characterizes FRSs of tracking errors. A conservative, but computationally effi cient, approx- imation on the FRSs and the FRS-based robust planning al- gorithm are described in Section IV. Experimental validation and conclusion are followed in Sections V and VI. II. PROBLEMFORMULATION We consider the following simplifi ed 8-state multirotor dy- namics with external disturbance in the acceleration channel: r = F m cos sin sin cos cos + 0 0 G +w, = d, = d, (1) where r R3is the position of the multirotor, m R is the mass of the multirotor, F R is the total thrust exerted by the multirotor, and R are the roll and pitch angles of the multirotor, G R is the gravitational acceleration, d and dare the desired roll and pitch rates, and w Rnwis the mass-normalized external disturbance with nw= 3. It is assumed that the heading angle of the multirotor can always be maintained as zero, and roll/pitch rates track the desired command suffi ciently well. The state and input are defi ned as x = r r Rnx, and u = F d d Rnuwith nx= 8 and nu= 3. The dynamics can be written as x = f(x(t),u(t),w(t). The FRS X (t) Rnxis the set of all states to which a system can be derived from the given initial set of states X0 Rnxafter a certain duration t 0, considering all possible inputs and disturbances 14. For general nonlinear systems, exact computation of the FRSs, as well as its propagation law, is intractable. We linearize the system around nominal states and inputs, which are assumed to be known, to reduce the complexity in computing the FRSs. For multirotor systems, the assumption on known nominal values is not restrictive because we can take advantage of a structural property such as the differential fl atness 15, 16. Let us denote the nominal states and inputs as x(t) Rnx and u(t)Rnu. The dynamics of the error state e(t):=x(t) x(t) with linearization around x(t) and u(t) can be expressed as follows. e(t) = A(t)e(t)+B(t)(u(t)u(t)+D(t)w(t) +(e(t),u(t)u(t),w(t), (2) where A(t):= f/x|( x, u,0)Rnxnx, B(t):= f/u|( x, u,0) Rnxnu, D(t):= f/w|( x, u,0)Rnxnware linearized system matrices at the nominal states and inputs. To simplify our analysis, we make the following assumptions. Assumption 1. The linearization error Rnxis negligible near x(t). Assumption 2. The control policy is composed of an affi ne feedback and a feedforward term as u(t) = K(t)e(t)+u(t),(3) where K(t) Rnunx, and the feedback gains are known a prior to compute the FRSs. Note that Assumption 1 can be removed if some conservative approximations on such as nonlinearity bounders 13 are explicitly considered in the error dynamics, but it is out of the scope of this work. The resultant error dynamics can be expressed as e(t) = (t)e(t)+D(t)w(t),(4) where (t) = A(t) + B(t)K(t). We now defi ne the FRS E(t) Rnxof the error system (4) as E(t) = y ? ? ? ? ? ? ? 0,t, w() W, e() = ()e()+D()w(), e(0) E0, y = e(t) ,(5) where E0is the initial set of error states, and W Rnwis 3151 an admissible set for disturbances. Note that the FRS of the original system (1) is the translation of the error state FRS with x(t), namely X (t) = E(t)+x(t). In order to plan the safety-guaranteed fl ight trajectory of the multirotor, we formulate the following optimization problem that can be solved in a receding horizon manner: min u(t) lf(x(T)+ Z T 0 l(x(t),u(t)dt s.t x(t) = f(x(t),u(t),0), x(0) = x0, c(X (t) 0, X (t) = E(t)+x(t) with E(t) = g(E(t), E(0) = E0, (6) where x0is the initial state, l is the stage cost, lfis the terminal cost, c represents the nonlinear constraints such as collision avoidance, g represents the propagation of the FRS E(t), and T 0 is the prediction horizon. Note that the constraints c in (6) ensure that the system avoids risky regions whenever disturbances are bounded in W. From Assumption 2, we can divide the robust trajectory planning problem into two steps: the computation of E(t) and the optimization on x(t),u(t) considering E(t). Given the initial guess on x(t) and u(t), E(t) is computed and then used to fi nd the optimal x(t) and u(t) which robustly satisfy the constraints. The next section presents how to fi nd analytic expressions of E(t) based on the reachability analysis. III. HAMILTON-JACOBIREACHABILITYANALYSIS This section aims to fi nd the maximal reachable set of the error state considering adversary disturbances. To com- pute the error state FRS E(t), we consider an optimization problem of the reachability with the level set approach 14. A. The reachability problem We fi rst defi ne a convex function h, which satisfi es E0= y|h(y) 0. The function h encodes information to deter- mine whether an initial error is inside or outside E0. Consider a cost functional J defi ned as J(e,w,t) = h(e(t)+ Z t 0 L(w()d,(7) where the function L is the running cost detailed later. The value function V is defi ned as the minimum of J among all possible disturbances, namely V(e,t) = min w J(e,w,t).(8) Since the worst case disturbance is expected to increase tracking errors, the disturbance tries to minimize J so that the zero level set of the value V can be expanded as much as possible. The Hamilton-Jacobi equation of (8) can be derived from the dynamic programming principle 17. The initial value HJ PDE is determined as V t +max w ?V e e(t)L(w(t) ? = 0 V(e,0) = h(e(0). (9) To consider the admissible set of disturbance, we use the following function as the running cost L, L(w(t) = ( 0w(t) W w(t) / W. (10) The Hamiltonian H is defi ned as the maximum value of the inner product of the error dynamics and the value gradient := V/e Rnx, H(,t) = max wW ?(t)e(t)+D(t)w?. (11) The optimal disturbance w?(t) can steer an error state in the zero level set of V(e,0) to the zero level set of V(e,t). Con- sequently, the error state FRS after time t can be represented as E(t) = e|V(e,t) 0.(12) B. Computation of the value function The HJ reachability analysis provides the quantitative expression of the FRS. But it requires the solution of the initial value PDE, which is generally not tractable for high- dimensional systems. This section aims to fi nd a tractable solution of the HJ PDE with the following assumption. Assumption 3. The time-varying system matrix (t) can be treated as a constant for a suffi ciently small duration 0,t. For convenience, we drop the time argument and use as the constant system matrix during 0,t. We now consider the following coordinate transform as proposed in 18, 19 (t) = exp(t)e(t),(13) and the dynamics of the transformed state is (t) = exp(t)D(t)w(t).(14) For the system in the above form, the generalized Hopf formula 19 provides the solution for the value function as V(,t) = min ? h (0)+ Z t 0 H(,)d ? , (15) where the superscript represents a convex conjugate of a function, and the subscript represents the correspond- ing expression in the transformed coordinate, for instance h(e(t) = h(exp(t)(t) = h(t). The Hamiltonian in (15) is H(,t) = max wW ?exp(t)D(t)w?. (16) Note that the Hamiltonian in -coordinate does not contain state variables, unlike the original Hamiltonian. Let us consider the following quadratic initial cost defi ned in the error space h(e(0) = eQ1 0 e1,(17) 3152 where Q0 Snx +. Consequently, the initial set of error states can be expressed as the following ellipsoid, E0= ? Q 1 2 0v ? ? ?|v|2 2 1 ? ,v Rnx.(18) Note that the initial sets of the error states and the trans- formed states are identical since (0) = e(0). Using the defi nition of the Fenchel-Legendre transform, the convex conjugate of h(t), can be computed as h (t) = 1 4 exp(t)Q0exp(t) +1. (19) Before computing the Hamiltonian, we make the following assumption on the set of disturbances. Assumption 4. Each channel of the disturbance is bounded, i.e. |wi()| wi R, 0,t where the subscript i repre- sents the i-th element of a vector with i = 1,.,nw. The optimization in (16) results the following Hamiltonian H(,t) = nw i=1 |Di(t)|(20) with the optimal disturbance w? i(t) = sign ? Di(t) ? wi,(21) where Di(t) := exp(t)Di(t) wi Rnx, and Di(t) is the i-th column of D(t). Substituting (19) and (20) into (15) yields the value function expressed in space and its convex conjugate as V(,t) = min ?V (,t) ?, V (,t) = 1 4 Q0 + nw i=1 Z t 0 |Di()|d +1. (22) C. Characterization of the FRS The following proposition presents an explicit expression for the FRS based on the value function described in (22). Proposition 1. Consider the dynamic system (14) with the initial set of states (18). Let E(t) be a set-valued function. If the value function satisfi es (22), then E(t) = ( Q 1 2 0v+ nw i=1 Z t 0 Di()sgn ? vQ 1 2 0 Di() ? d ) is the FRS of system (14) after time t and |v|2 2 1. Proof. We use the property of Fenchel-Legendre transform 20 that the derivative of a convex function is the optimal argument of its conjugate. Let ? Rnxbe the minimum argument of (22) as V(,t) = ?V ( ?,t). (23) Since ?is the minimizer and V (,t) is convex on , we evaluate the fi rst-order optimality condition and obtain 0 = ? V (,t) ? ? ?=? = 1 2Q0 ?+ nw i=1 Z t 0 Di()sign ? ?Di() ? d. (24) Substituting (24) into (23) yields the other expression for the value function as V(?,t) = 1 4 ?Q0?1. (25) It is apparent that V(?,t) 0 ? ? 2Q 1 2 0 v ? ? ?|v|2 2 1 ? ,(26) where v:=(1/2)Q1/2 0 ?Rnx. By composing (24) and (26), we obtain V(,t) 0 ( Q 1 2 0v+ nw i=1 Z t 0 Di()sgn ? vQ 1 2 0 Di() ? d ? ? ?|v|2 2 1 ) . As mentioned in (12), the FRS is the sub-zero level set of the value function. Therefore, the set in the above expression represents the FRS E(t). ? Note that E(t) can be interpreted as the Minkowski sum of sets as E(t) = E0D(t),D(t) = nw M i=1 Di(t), Di(t) = ?Z t 0 Di()sgn ? vQ 1 2 0 Di() ? d ? ? ?|v|2 2 1 ? , (27) where Diis the reachable set due to the i-th channel of disturbance wi(t). Although we analyze how the FRS E(t) is composed of, how this can be computed is still obscure since numerical integration is required to compute Di(t). Moreover, the numerical computation of the Minkowski sum of sets is intractable as the number of state increases. Methods for overcoming those limitations are discussed in the next section. IV. PROPAGATION OF THEFORWARDREACHABLESET This section discusses an effi cient way to describe an ap- proximation of the error FRS. We fi rst fi gure out an ellipsoid which encloses Di(t) of (27). Then, we compose ellipsoidal regions to fi nd the resultant ellipsoid which encompasses the original FRS expressed in (12). Finally, the propagation law for the ellipsoidal FRSs is derived and used for the robust trajectory planning problem. A. Ellipsoidal approximation on Di(t) The following proposition presents the expression for the conservatively approximated region of Di(t). Proposition 2. Let Qi(t) Snx + be a shape matrix of an ellipsoid for i 1,nw. The set Di(t) is encompassed by Qi(t) if Qi(t) =t Z t 0 ? Di()Di()+I ? d, where is an arbitrary positive scalar, and I is the nxnx identity matrix. Proof. It suffi ces to show d(t)Qi(t)1d(t)1 0,d(t) Di(t).(28) 3153 Substituting Qi(t) into (28) yields t d(t) ?Z t 0 ? Di()Di()+I ? d ?1 d(t) 0.(29) Note that the left-hand side of (29) is the Schur complement of the following matrix S(t) = ? td(t) d(t) Rt 0 ? Di()Di()+I?d ? . Let m()=sgn(vQ 1 2 0 Di (), 0,t. We fi rst consider the case when m() is constant, and then the case when m() varies over . Case 1) Suppose that m() = 1, 0,t. Then, d(t) = Rt 0 Di()d. Now S(t) can be expressed as S(t) = Z t 0 s()d,s() = ? 1 Di() Di() Di()Di()+I ? . Note that s() and S(t) are positive defi nite matrices, and the right bottom block of S(t) is also positive defi nite. From the positive semi-defi niteness condition, the Schur complement of S(t) is determined as positive semi-defi nite, which vali- dates the inequality of (29). When m() = 1, 0,t, the same result can be derived similarly. Case 2) Let t0 0,t be the moment when m() changes. Assume that m() = 1 if Di() Di()Di()+I ? , b() = ? 1Di() Di() Di()Di()+I ? . Since all matrices that compose S(t) are positive defi nite, the Schur complement of S(t), i.e. the left-hand side of (29), is positive semi-defi nite from the property of the Schur complement. ? Fig. 2.Illustrative result of the approximation on D(t) for a 2-state system: e1= e2+w1+0.5w2, e2= e1e2+0.4w1+w2with w = 1.1 1.5. The black dotted line represents the boundary of Di(t). The blue, red, and green lines represent the approximation Qi(t) with = 0.1, = 0.01, and = 0.0001 respectively. The computation of Qi(t) does not require any numerical integration. Considering that Di(t)=exp(t)Di(t) wi, Qi(t) can be expressed as Qi(t)t2I = Z t 0 exp()Ni(t)exp()d, Ni(t) =t w2 iDi(t)Di(t), and thus Qi(t) is the solution of the following Lyapunov equation: (Qi(t)t2I)(Qi(t)t2I) = exp(t)Ni(t)exp(t)Ni(t). (30) Note that the positive scalar can be selected suffi ciently small in order to compute a less conservative approximation on Di(t) as illustrated in Fig. 2. B. Propagation of the ellipsoidal FRSs This section provides an expression for the ellipsoid that encompasses the error FRS and how this ellipsoid varies according to time. Recall from (27) that the reachable set propagated by disturbances D(t) is the Minkowski sum of Di(t). So, D(t) can be conservatively approximated by the Minkowski sum of Qi(t) as Di(t) B(Qi(t) nw M i=1 Di(t) nw M i=1 B(Qi(t), where B(Q)Rnxis the region occupied by an ellipsoid Q. We utilize the formula for the ellipsoidal bounding of the Minkowski sum of ellipsoids 21 which represents nw M i=1 B(Qi(t) B(Qd(t) Qd(t) = nw i=1 Qi(t) ri s.t. nw i=1 ri= 1. (31) Since our interest is to get a conservative and compact approximation, riin (31) is selected so that the trace of Qd(t) is as small as possible. A simple optimization on the trace of Qd(t) yields Qd(t) = nw i=1 p tr(Qi(t) ! nw i=1 Qi(t) ptr(Q i(t) ! .(32) An ellipsoidal approximation on the reachable set in -space, E (t) of (27), satisfi es E(t) B(Q0 Qd(t). Considering the coordinate transform (13), the error state FRS E(t) is approximated with an ellipsoid Q(t) Snx + as E(t) B(Q(t) where Q(t) = exp(t) ? 1+ b a ? Q0+ ? 1+ a b ? Qd(t) ? exp(t), (33) with a = ptr(Q 0) and b = ptr(Q d(t). Until now we have derived the error FRS after a small duration t, starting from the initial ellipsoid Q0. By using the computed Q(t) as an initial ellipsoid at each t, the propagated ellipsoid after a small time dt, namely Q(t+dt), can be consecutively derived using (30), (32), and (33). 3154 Note that the proposed propagation law for the ellipsoidal error FRSs does not require any computationally burden- some procedure, such as a numerical optimization. This is because we have started from the analytic solution of the HJ PDE, and then sequentially approximate the solution sets as conser