Collision Avoidance Using Linear Quadratic Control in Satellite Formation Flying
- Author: Mok Sung-Hoon, Choi Yoon-Hyuk, Bang Hyo-Choong
- Organization: Mok Sung-Hoon; Choi Yoon-Hyuk; Bang Hyo-Choong
- Publish: International Journal Aeronautical and Space Sciences Volume 11, Issue4, p351~359, 15 Dec 2010
This paper proposes a linear system control algorithm with collision avoidance in multiple satellites. Consideration of collision avoidance is augmented by adding a weighting term in the cost function of the original tracking problem in linear quadratic control (LQC). Because the proposed algorithm relies on a similar solution procedure to the original LQC, its inherent advantages,including gain-robustness and optimality, are preserved. To confirm and visualize the derived algorithm, a simple example of two-vehicle motion in the two-dimensional plane is illustrated. In addition, the proposed collision avoidance control is applied to satellite formation flying, and verified by numerical simulations.
Collision avoidance , Linear quadratic control , Tracking problem , Satellite formation flying
Linear quadratic control (LQC) is a well-known control algorithm in the area of linear systems control. LQC originated from mean-square filtering, researched by Wiener in 1940s (Wiener, 1948). The LQC problem is based upon linear systems, and the cost function of the performance is constructed in the form of quadratic terms which track performance and control efforts. An objective in LQC entails cost minimizing, and the corresponding control input solution can be obtained by the well-known Riccati equation.In the solution procedure, various methods can be used; the Hamilton-Jacobi optimization equation is used to derive the Riccati equation.
The obtained solution provides gain-robustness and global optimality performance (Dorato et al., 1995). Additionally,because of such advantages, the LQC algorithm has been applied to various areas such as ground-vehicles, missions,aircraft, and spacecraft (Fravolini et al., 2003; Gribble, 1993;Lovren and Tomic, 1994; Plumlee and Bevly, 2004; Psiaki,2001; Ridgely et al., 1987).
The LQC solution was originally derived in a regulating problem, which is called the linear quadratic regulator(LQR). The regulating problem specifies that the desired state is zero, and the corresponding controller maintains the vehicle state at zero. To apply the LQC solution in a tracking problem, which has a non-zero desired trajectory, the disturbance-rejection problem was considered by modifying system dynamics (Anderson and Moore, 1990; Dorato et al.,1995).
This study proposes a control algorithm that adds collision avoidance to the original LQC tracking problem. To achieve this goal, an additional term is inserted into the cost function to account for a collision avoidance effect. A detailed derivation of the proposed algorithm is introduced in Section 3. One of the important changes in the new approach is the off-diagonal term of the weight matrix on state. This term is not zero, which is a measure taken in order to implement the collision avoidance constraint. To prevent equalization of the position between vehicles, the cost function should consider the correlation and dependency between state variables. As a result, the weight matrix on states becomes a non-diagonal matrix, which is easy to implement by using commercial software tools such as MATLAB.
After performing the derivation of the optimal control history for a multiple-vehicle case, the proposed algorithm is applied to a satellite formation flying (SFF) mission. SFF is a combined phrase consisting of the words satellite and formation flying. SFF has recently received much attention(Clohessy and Wiltshire, 1960; Hadaegh et al., 2000;McCambish et al., 2009; Speyer, 1979; Xing et al., 2000).As with other concerning satellite missions, all guidance,navigation, and control theories are necessary for SFF.Additionally, among guidance and control parts, the collision avoidance problem is an important issue because multiple satellites are operating.
Many researchers have studied the collision avoidance problem in SFF. Slater et al. (2006) referred to the collision probability function, and used an impulsive control to maintain a safe distance between spacecrafts. Wang and Schaub (2008) attempted to avoid collision using electrostatic (coulomb) forces with a Lyapunov-based control law. And, Lim et al. (2005) used potential functions to prevent collision. This paper proposes a collision avoidance algorithm by modifying the original LQC tracking problem.The conventional LQC provides desirable qualities such as gain-robustness and optimality; these properties can be preserved in the proposed algorithm because the proposed algorithm is based on the LQC problem. In addition, an optimal solution can be obtained by simple numerical integration only.
This paper is presented in the following manner. In Section 2, introductory material of LQR is presented. The main control algorithm and its derivation are introduced in Section 3. Finally, the algorithm’s application to SFF with numerical simulation is discussed in Section 4.
In this chapter, the LQC problem is briefly reviewed.Before proceeding, the general LQR algorithm including disturbance rejection will be introduced. In the next chapter,a collision avoidance measurement term is augmented to the linear control framework introduced here.
The LQC problem originated from a mean-square filtering algorithm by Wiener. The LQC algorithm manages a linear system, and its performance is measured in quadratic form.A linear system is a system in which the system equation is constructed by linear operators. Quadratic form indicates cost terms, like the tracking performance of states and control effort of an actuator, are represented by the quadratic errors.
Vis a performance measure, x and u are the state and control input of corresponding linear system x˙= Ax+ Bu, Qand R are the weight matrix to performance. An optimal control input history u(τ) which minimizes the performance measure can be found using an optimality principle also referred to as the dynamic programming approach (Bellman,1952).
The above equation is the Hamilton-Jacobi optimization equation. Additionally,
l(x, u, t)is a cost function which is integrated along time V=∫tT l(x, u, t) dτ for obtaining the performance measure, and f(x, u, t)represents the differential state x˙. In addition, V*( x, t) is a minimum value of the performance measure obtained by the optimal input history. V*( x, t) may have various forms for different system equations f(x)and cost functions l(x, u, t).
Firstly, we will consider a simple case, where
l(x, u, t)=xTQx+ uT Ru and f(x, u, t)= Ax+Bu. In this case, the optimal control input is solved as a state feedback control, u*(t)= ？Kx(t), and the minimum performance measure can be represented as V*(x, t)= xTP(t)x. By substituting the derived forms into Hamilton-Jacobi optimization equation, the Riccati equation and the corresponding optimal control law are obtained.
Disturbance rejection problems are characterized by an additional disturbance term
w(t)in a differential state equation, i.e., f(x, u, t)= Ax+Bu+w(t). Thus, the LQR solution previously introduced is a special solution to the disturbance rejection problem with w(t)=0. As previously noted, the minimum performance measure V* (x, t)takes a different form. Linearly state-dependent terms and state-independent terms are augmented, as V*= xT P(t)x+2bT (t)x+c(t). By substituting the new performance measure form into the Hamilton-Jacobi equation, one can obtain the following differential equations.
In addition, the disturbance rejection problem is similar to the tracking problem. The tracking problem requires a nonzero desired state,
xd(t)≠0. To formulate the tracking problem as a regulating problem, one can use an additional term ξ (t)=x(t)-xd(t), which represents tracking errors. In addition, through the following procedure, the tracking problem can be reduced to a disturbance-rejection regulating problem (Dorato et al., 1994).
In multiple vehicles control, such as airplanes and spacecrafts, collision between each vehicle should be avoided. This avoidance guidance is known as ‘Collision Avoidance’, and associated research has been conducted for many years. In this chapter, an additional cost term will be added to the pre-defined cost function
l(x, u, t)in order to consider collision avoidance. First, our analysis focuses solely on regulating problem, which is described as xd(t)=0. Then, in the following sub-chapter, a general tracking problem is introduced, which is the xd(t)≠0 case for linear optimal control.
The regulating problem, which is when
xd(t)=0, regulates a vehicle’s position to the zero state, generally represented by zero position and zero velocity. In a strict sense, the regulating problem has a paradox for the collision avoidance concept because a vehicle’s position travels toward the same position at the final time xpos(T)=0, x1(T)=x2(T), and collision eventually occurs. However, as a sub-work for simple derivation of the case, we will first study the regulating problem.
To add the collision avoidance effect into multiple vehicle guidance, we will augment additional cost terms to the cost function,
lCA (x, u, t)is a modified cost function which considers the collision avoidance issue from the original cost function l(x, u, t)by adding the second terms on the R.H.S. in the above equation. The (xi-xj)T(xi-xj)term indicates the square of distance between vehicles, and the term qCA, ij is used as a weighted term for collision avoidance.
For a simple case, the number of vehicles is assumed to be two, which means
N=2; and, the vehicles are considered to be moving along a one-dimensional axis. The necessary information can be summarized as four state variables, x (t)=[x1 x2 x˙1 x˙2]T. Finally, we add collision avoidance terms to the cost function using Eq. (8),
A1, A2and A3are weight terms for each cost. A1 is a weight that regulates the performance of vehicle 1, A2is a weight that regulates the performance of vehicle 2, and A3is a weight term that considers the collision avoidance between the vehicles. Note that in the above equation we do not consider the weight terms of the velocities, such as Ax1x˙12. We also omit a cost for the control quantity uT Ru for simplicity. In addition, the above equation can be represented in matrix notation,
Qnois a weight matrix for regulating, which is an original weight matrix of LQR with no collision avoidance, QCAis a weight matrix for avoiding collision avoidance. We can see that off-diagonal entries of original weight matrix become non-zero after collision avoidance is considered. Finally, after this matrix formulation, the optimal input history u*(t)can be obtained by using the Hamilton-Jacobi optimization equation as given in Eqs. (3) and (4).
This sub-chapter investigates the tracking problem. As noted before, the tracking problem has a non-zero desired state
xd(t)≠0, which is different from the regulating problem in which xd(t)=0. Because of this change, a simple Riccati equation cannot be used, and its optimal control solution has a similar form to the disturbance rejection problem given in Eqs. (5) and (6). The detailed derivation procedure of the optimal control history is explored.
Firstly, for simplicity, it is assumed again that there are two vehicles, and they move along a one-dimensional axis. Four state variables represent the system status as before. However, the vehicles’ tracking errors from the desired trajectory ξ
(t)=x(t)-xd(t)is adopted as state variables, rather than the vehicles’ positions and velocities directly. The tracking problem can then be converted into a regulating problem as given by Eq. (7). And, a new state representing the tracking error becomes ξ (t)=[x1-x1d x2-x2d x˙1-x˙1d x˙2-x˙2d]T.
However, the vehicles’ tracking errors from the desired trajectory ξ(t)=x(t)-xd(t) is adopted as state variables, rather than the vehicles’ positions and velocities directly. The tracking problem can then be converted into a regulating problem as given by Eq. (7). And, a new state representing the tracking error becomes ξ(t)=[x1-x1d x2-x2d x˙1-x˙1d x˙2-x˙2d]T .
Because state variables are different, the cost function
l(ξ, u, t)should also be changed
(t)and x2d (t)are the desired trajectories of each vehicle.
In Eq. (11), it should be noted that the last term, which is used to prevent collision avoidance, has a quadratic form with a negative sign. This form is must be applied to the original LQC algorithm which is derived in a quadratic manner. In general collision avoidance research, rather than a quadratic term, an inverse of the distance term like (xi？xj)？1 or a potential function with an exponential term is used to prevent collision. In this manner, collision could be avoided regardless of the shape of the desired trajectory. However, an optimal property such as the minimizing control effort is hard to obtain when those terms are used. In addition, if optimal equations could be constructed, the equations are not easily solved. In this paper, the proposed algorithm does not guarantee collision avoidance for some cases in which different trajectories or systems are desired, and confirmation of the obtained control input could be needed. However, the proposed algorithm is based on the conventional LQC problem, so the proposed algorithm inherently preserves gain-robustness and optimality, and an optimal solution can be obtained by simple numerical integration.
We can see that Eq. (11) is similar to Eq. (9) which was derived in the regulating problem. But, different in state variables exist. The x1, x2 terms in Eq. (9) are replaced by ξ1=x1？x1d, ξ2=x2？x2d in Eq. (11). Because of this change, the cost function with tracking problem, Eq. (11), cannot be formed with quadratic terms, ξT
Qξ, while it was possible in Eqs. (9) and (10). Thus, additional terms are needed in the tracking problem to generalize the cost function.
r(t)and s(t)terms are added. The unknown time-independent matrix Q, time-varying terms r(t), and s(t)can be obtained by expanding and comparing the first and the third lines of the above equations,
From Eq. (12) and Eq. (13), one can express the collision avoidance effect as a combination of a matrix and vector. Finally, the optimal input history is derived using the Hamilton-Jacobi optimization equation, Eq. (2). Because this problem manages the tracking problem, the following form for the performance measure is introduced, i.e.,
V*(ξ, t)=ξ TP (t)+2 bT (t)ξ+ c(t). By substituting all of the terms into Eq. (2), the following equations are derived.
After expanding the above equations and collecting the quadratic terms, linear terms and state-independent terms respectively, the results can be summarized as,
w(t)comes from a desired trajectory produced by converting the tracking problem into the regulating problem, w(t)=Axd？xd. And, the corresponding optimal control u*(t)is equivalent to the original tracking problem solution.
Eq. (15) shows similar results to the original disturbance rejection solution, Eq. (5). A comparison of the two results is summarized in Table 1. One can see that there is an additional term ？
r(t)in the b(t)equation for the solution of collision avoidance considered cell. This term changes the b(t) history, consequently affecting the control history u*(t).
Until now, cases involving two-vehicles have only been considered, rather than the multiple-vehicle cases in which N≥3. However, we can generalize the results of the above sub-chapters into a multiple vehicle case without complex derivation.
Firstly, note that the solution for the performance measure coefficients
P(t), b(t), and c(t)in Eq. (15) are not changed, as the number of vehicles change. The weight matrix and coefficients of the cost function l(ξ, u), which are Q, r(t)and s(t)in Eq. (13), are changed.
To visualize the tendency of the change in
Q, r(t), s(t), a three-vehicle case solution is represented in Eq. (17). We can see that Eq. (13) and Eq. (17) exhibit similarities in the results.
Finally, a generalized form of the multiple-vehicle case can be obtained, as shown in Eqs. (18) and (19).
Before applying the derived control algorithm to real practical problems, such as SFF, a simple problem is analyzed to demonstrate the validity of the control law. In a two-dimensional space, two-vehicles are used, requiring no additional forces except control acceleration
u(t). Then, the system kinematics equations are formulated as given by Eq. (20).
where x=[x1 x2 y1 y2 x˙2 x˙2 y˙1 y˙2]T and
u=[ux1 ux2 uy1 uy2]. In the problem formulation, the cost function coefficients and initial conditions are defined in Table 2.
Figures 1-4 display the simulation results of this condition. Fig. 1 shows a vehicle trajectory from its initial position to the final position. The solid line represents a trajectory with the original tracking algorithm, and the dashed line represents the trajectory with the derived algorithm, considering collision avoidance between the vehicles. To prevent collision between vehicles, vehicles do not follow a pre-determined desired trajectory. In Fig. 2, one can confirm that the distance between vehicles becomes larger when collision avoidance is added. In addition, Figs. 3 and 4 show corresponding tracking errors between the present position and the desired trajectory.
Figure 5 shows the change of vehicle trajectory as the collision avoidance weighting parameter
A3 increases. As the weighting parameter A3 increases, collision avoidance between vehicles becomes more important than the tracking of a desired trajectory. Thus, the distance between the
vehicles tends to become longer in the time history.
In this chapter, the control law derived in the previous chapter is applied to a SSF problem. SSF has been investigated for many years, and the Clohessy and Wiltshire (CW) equation is generally employed as the linearized equation for relative dynamics. By using the CW equation, an application of collision avoidance algorithm is demonstrated, and the corresponding simulation results are presented.
SSF is commonly constructed by a chief satellite and a deputy satellite. The amount of deputy satellites present can be more than one, and the actual number is determined by mission characteristics. The coordinated frame (localvertical-local-horizontal [LVLH] frame) has an origin at the center of the chief satellite and the direction of each axis is drawn in Fig. 6. The
(x, y)axis together make an in-plane motion, and z axis represents an out-of-plane motion of the deputy satellite. Eq. (21) shows the general non-linear relative equations of motion.
(x, y, z) are the components of the relative position of the deputy satellite in the LVLH frame, θc is the true latitude angle of the chief satellite, and rc stands for the radius of the chief satellite.
Eq. (21) has non-linear terms such as a square term. CW developed a linearized equation of Eq. (21) using assumptions such that the chief satellite travels in a circular orbit (Clohessy and Wiltshire, 1960).
Eq. (22) is the CW equation, also known as Hill’s equation. In Eq. (22), the term stands for the angular rate of the chief satellite. Using the linear equation of SSF, CW equation, one can apply the derived linear control algorithm that were presented in the previous chapter.
The application of the linear optimal control algorithm derived in the previous chapter is explored in this sub-chapter. A two-dimensional space,
(x, y)plane, is employed for the simulation. Because in-plane motion (x, y)is de-coupled from the out-of-plane motion z axis, the in-plane motion only case is selected. Three satellites are assumed for the simulation, with final positions producing a circular formation around a chief satellite.
Details for the cost function coefficients and the initial condition of each satellite are presented in Table 3. Figures 7-10 represent resultant plotting of the SSF control simulation. Figure 7 presents satellite trajectories from initial positions to final mission destinations. No weighting term was added for the collision avoidance in the Fig. 7 simulation, but Fig. 8 considers collision avoidance for trajectory generation. In Fig. 8, satellites enter the final circular formation by detouring non-direct guidance as Fig. 7, to prevent collision between satellites.
Figures 9 and 10 present the corresponding distance between satellites as well as the acceleration input. Figure 9 shows the distance between each satellite, which is
d1-2, d2-3, d1-3. One can see that the distances became longer than the original tracking history case. Finally, Fig. 10 displays the control acceleration input history in each axis for the first satellite.
In this paper, a linear control algorithm was derived for the tracking problem by solving the Hamilton-Jacobi optimization equation. To avoid collision between vehicles, an additional weighting term was introduced when constructing the cost function. The corresponding optimal control solution maintained a similar form as the original LQR tracking problem, which does not consider collision avoidance. The only difference was that extra terms were added in the differential equations of the performance measure coefficients. After deriving the control history solution, a simple example involving two vehicles was considered. In addition, the application of the control algorithm to SSF mission was analyzed, and numerical simulation was presented. Using this proposed algorithm, SSF problems as well as other problems involving multiple-vehicles could consider collision avoidance in the tracking problems.
[Table 2.] Simulation parameters
[Fig. 1.] Vehicle trajectory.
[Fig. 2.] Tracking error in X-axis.
[Fig. 3.] Distance between vehicles.
[Fig. 4.] Tracking error in Y-axis.
[Fig. 5.] Vehicle trajectory along various collision avoidance weighting A3
[Fig. 6.] Local-vertical-local-horizontal frame.
[Table 3.] Simulation parameters
[Fig. 7.] Satellite trajectory without collision avoidance.
[Fig. 8.] Satellite trajectory with collision avoidance.
[Fig. 9.] Distance between satellites.
[Fig. 10.] Control acceleration of satellite No.1.