Optical Design and Engineering

Combined line-of-sight error and angular position to generate feedforward control for a charge-coupled device–based tracking loop

[+] Author Affiliations
Tao Tang, Huaxiang Cai, Yongmei Huang, Ge Ren

Chinese Academy of Sciences, Key Laboratory of Optical Engineering, Chengdu 610209, China

Chinese Academy of Sciences, The Institute of Optics and Electronics, Chengdu 610209, China

Opt. Eng. 54(10), 105107 (Oct 15, 2015). doi:10.1117/1.OE.54.10.105107
History: Received May 2, 2015; Accepted September 9, 2015
Text Size: A A A

Open Access Open Access

Abstract.  A feedforward control based on data fusion is proposed to enhance closed-loop performance. The target trajectory as the observed value of a Kalman filter is recovered by synthesizing line-of-sight error and angular position from the encoder. A Kalman filter based on a Singer acceleration model is employed to estimate the target velocity. In this control scheme, the control stability is influenced by the bandwidth of the Kalman filter and time misalignment. The transfer function of the Kalman filter in the frequency domain is built for analyzing the closed loop stability, which shows that the Kalman filter is the major factor that affects the control stability. The feedforward control proposed here is verified through simulations and experiments.

Figures in this Article

Two-axis (azimuth and elevation) gimbals with a charge-coupled device (CCD)-based control system can be used for monitoring and positioning as well as tracking an interesting target.13 The basic mechanism and configuration is a two-axis gimbal equipped mainly with motor, encoder, CCD, and other components.2,3 Unlike the previous control problem, the tracker of CCD cannot provide the target trajectory or the target velocity, but only line-of-sight (LOS) error. A direct feedback loop is usually utilized to control LOS. High control bandwidth (BW) facilitates good closed loop performance. However, the time delay is the major reason to restrict the BW in a CCD-based tracking loop. In general, there are three factors causing the time delay to the closed loop system: exposure time of the CCD, image process time, and transmit time. The time delay cannot be cut to zero, which results in an ineffectiveness of a high BW. A feedforward control, such as a rate-aided control, is introduced into a CCD-based tracking system to reduce the LOS error.4,5 The target trajectory either can be recovered through sensor fusion, including a rangefinder, the CCD, encoder, and rate sensor, or can be given. Experiments verify rate the feedforward control to be effective for improving the tracking performance, especially for a maneuvering target tracking. Motivated by pervious research,6,7 an improved feedforward control is proposed in this paper. This method only combines the CCD and encoder for data fusion to recover the target trajectory as an observed value of the Kalman filter, which can produce the target velocity to implement the feedforward control. The previous researches usually focused on how to optimize the Kalman filter or how to make precise models for better estimation of the target rate.810 However, if the target trajectory is unknown, it is necessary to consider the closed loop stability when a Kalman filter is used to estimate the target velocity. To analyze the closed loop stability, the transfer function of a Kalman filter in the frequency domain is built.

Section 2 presents a detailed introduction to a feedforward control, mainly describing the CCD-based control structure and the implementation of the feedforward control. Section 3 focuses on parameters’ design, specifically in terms of the proportional-integral (PI) controller and the Kalman filter. Section 4 discusses and analyzes the system stability. Section 5 sets up experiments to verify this method. Concluding remarks are presented in Sec. 6.

A classical feedforward control is shown in Fig. 1, where Q(s) is the feedforward controller, C(s) is the position controller, and P(s) is the control plant. The time delay eT0s characterizes the CCD in the control system, although it may be rough.

Graphic Jump Location
Fig. 1
F1 :

classical feedforward control.

The feedforward control not only has little influence on the closed loop stability, but also contributes a lot to the control performance. However, the tracking sensor cannot provide the target trajectory or target speed, but only an LOS error in a CCD-based servo control system. Considering R=E+Y, the equivalent control structure of Fig. 1 is depicted in Fig. 2.

Graphic Jump Location
Fig. 2
F2 :

An equivalent feedforward control.

This control structure in Fig. 2 is practical. The target trajectory can be recovered by combining the LOS error and the angular position of the gimbals. The sensitivity function is Display Formula

SSF=1Q(s)P(s)eT1s1+C(s)P(s)eT0s+Q(s)P(s)(eT0seT1s).(1)

The improvement of the feedforward control is mainly subject to 1eT0s if this feedforward controller Q(s) is designed to be the inverse P(s). It is obvious that the term 1eT1s is close to T1s in low frequencies. In this situation, the smaller the time parameter is set, the more the feedforward control contributes to the closed-loop performance. However, it is impractical to implement Q(s)=P1(s), because the term P(s) includes not only the non-nominal part, but also the high-frequency characteristics.

P(s) is depicted in Fig. 3, where G(s) is the control plant and Cv(s) is the velocity controller designed according to the classical control theorem. A tachometer providing the angular velocity of the gimbals usually has a high BW, resulting only in a minor influence on the closed-loop control system.

Graphic Jump Location
Fig. 3
F3 :

The velocity closed-loop control.

We can easily get the transfer function from O to V in Fig. 3: Display Formula

P(s)=OV=Cv(s)G(s)1+Cv(s)G(s)1s.(2)

As a matter of fact, this term Cv(s)G(s)/1+Cv(s)G(s) is very close to constant 1 in low frequencies because the velocity closed loop has a much higher BW than that of the position closed loop. Thus, P(s)1/s is true to some extent. In this case, the feedforward controller can be described as Display Formula

Q(s)=s1+Tfs.(3)

The phase lag term 1/1+Tfs indicates the main feature of a filter. It is impossible to obtain the velocity by directly differentiating the synthesizing signal E+Y, because noise can pollute the differential signal, resulting in ineffectiveness.

The open-loop transfer function in Fig. 2 is given by Eq. (4): Display Formula

Sopen=Q(s)P(s)+C(s)P(s)1Q(s)P(s)eT1seT0s.(4)

From Eqs. (1) and (4), the difference between the two parameters and the filter Q(s) may affect the stability or even lead to control instability.

The sampling frequency of the CCD is 50 Hz. The time delay T0 is 0.06 s, which is about three times more than the sampling interval of 0.02 s. The position open-loop transfer function without a feedforward control is in Eq. (5): Display Formula

eT0sC(s)P(s)Kp(KIs+1)seT0ss.(5)

For the feedback system to be robust, a gain margin larger than 6 dB and a phase margin larger than 35 deg is usually specified.11 Given wc is the crossover frequency of the open-loop transfer function, according to the definition of the phase margin, we have Display Formula

{arctan(KIwc)T0wc=π4KpKI2wc2+1=wc2.(6)

Combining the two equations to remove the integral parameter KI yields Eq. (9): Display Formula

Kp=w2cos(T0wc+π4).(7)

Let Kp/w=0, then the maximum gain can be obtained and wcT00.5275 is resolved. Substituting this into Eqs. (6) and (7), the PI controller parameters can be resolved as Kp=0.0709/T02=19.71 and KI=7.188T0=0.43.

Approximating eT1s1/1+T1s and Q(s)P(s)1/1+Tfs, where 1/1+Tfs is referred to as the characteristics of the Kalman filter, Eq. (4) can be rewritten into Eq. (8): Display Formula

arg[Sopen(s)|s=jwc]=arg[[Kp(KIs+1)(Tfs+1)+1](T0s+1)TfT0s+Tf+T0eT0ss3|s=jwc]arctan(T0wc)+arctan(KIwc)+arctan(Tfwc)T0wcarctan(TfT0wcTf+T0)32π.(8)

The requirements of the closed-loop system with feedforward control need to meet a phase margin larger than 35 deg of the open-loop transfer function, we have Display Formula

arg[Sopen(s)|s=jwc]+180deg35.(9)

The feedforward control is expected to have little influence on the closed-loop BW, resulting in wcwc. Substituting the results of the PI controller parameters into Eq. (8), we get Tf1.893T0, from which it can be deduced that the BW of the filter must be limited to less than 1/(1.893T0)=1.221  Hz.

The Kalman filter is an optimal linear mean minimum square filter to suppress noise. The target velocity can be estimated by the Kalman filter and be fed into the control system. The standard Kalman equations are depicted as follows: Display Formula

{xk+1=Axk+Bu+wkyk+1=Cxk+vk.(10)

The observed signal is the target trajectory recovered by LOS error and the angular position. A simple mode called the Singer acceleration model, nicknamed the zero-mean first-order Markov model,9 is used to estimate the target speed. Then we can easily know the Kalman filter parameters as follows: Display Formula

A=[1T0.5T201T001],B=[16T312T2T]T,C=[100].(11)

The solution of the Kalman filter is given below: Display Formula

x^k+1=(AKk+1CA)x^k+Kk+1yk.(12)

The gain Kk+1 of the Kalman filter can be obtained from the Ricatti equation if the matrices A, B, and C are time invariant. Using the “dare” function can help to quickly solve the Ricatti equation. The reconstructive characteristics of the Kalman filter in the frequency domain can be described in Eq. (13): Display Formula

ϕ=(ZIA+Kk+1CA)1Kk+1.(13)

The bode plot indicates that the target speed is pretty efficient in low frequencies in Fig. 4, where the phase lags a little. The bigger the process variance is, the higher the BW of the Kalman filter. A high BW probably leads to control system instability, although it has enough margin to adapt to a maneuvering target.

Graphic Jump Location
Fig. 4
F4 :

Velocity response based Kalman filter.

The BW of the Kalman filter needs to be smaller than 1.2 Hz according to the analysis above. The green line in Fig. 4 illustrates the Kalman filter response for which the BW is about 0.99 Hz.

From the sensitivity transfer function, the misalignment of two kinds of time parameters and the BW of the Kalman filter jointly affect the closed loop stability. The open-loop transfer function responses in Fig. 5 correspond to the different BWs of the Kalman filter in Fig. 4. A high BW of the Kalman filter weakens the margin of the open-loop control system and even leads to instability. In this paper, the Kalamn filter with a BW of 0.99 Hz is preferable in this control system, while the phase margin (Pm) and the gain margin (Gm) of the open-loop transfer function [Eq. (4)] are 41.8 deg and 10.3 dB, respectively. The sensitivity function without a feedforward control is as follows: Display Formula

SSF=eT0s1+C(s)P(s)eT0s.(14)

Graphic Jump Location
Fig. 5
F5 :

The open-loop response while the process variance of the Kalman filter is varying.

Based on these considerations and design, the responses in the sensitivity function [Eqs. (1) and (14)] are shown in Fig. 6. A large attenuation, about one-tenth less than that with only a feedback loop control, is achieved in the low-frequency region with a feedforward controller in Fig. 6. However, the attenuation in the middle-frequency range is magnified from 0.2 Hz to about 0.4 Hz, a little larger than that with only the feedback control. This is due to the amplification by the Kalman filter.

Graphic Jump Location
Fig. 6
F6 :

The sensitivity function response.

It is natural that the misalignment error inevitably occurs, although the sample frequency of the encoder can reach several thousand hertz or more while the CCD has usually a few dozens of hertz. In comparison with the Kalman filter, we can see from Fig. 7 that the deviations of the misalignment between the two parameters have little effect on the stability.

Graphic Jump Location
Fig. 7
F7 :

The open-loop response while the time delay is varying.

The trajectory of a moving target is provided by dynamic moving target simulators. A two-axis gimbal with a 50-Hz CCD as a tracker is used to track the moving target, for which the angular speed differentiated by a position angular from encoder is about 11.8deg/s for the azimuth axis and 5deg/s for the elevation axis in Figs. 8 and 9. The target acceleration is obtained by further differentiating the speed, about 4deg/s2 for the azimuth axis and 2deg/s2 for the elevation axis in Fig. 10.

Graphic Jump Location
Fig. 8
F8 :

The target speed in the A axis.

Graphic Jump Location
Fig. 9
F9 :

The target speed in the E axis.

Graphic Jump Location
Fig. 10
F10 :

The target acceleration.

Comparing the target velocity estimated with the Kalman filter with the target velocity by the differentiating encoder, the noise is suppressed by the Kalman filter.

In Fig. 11, A stands for the azimuth axis, whereas E represents the elevation axis. The LOS error with the feedforward control is shown in Fig. 11, and is much less than that without a feedforward control whose error curve is not drawn because the field of view of this CCD is too narrow to accommodate the 0.2-deg range.

Graphic Jump Location
Fig. 11
F11 :

The line-of-sight (LOS) error.

This paper—focusing on the implementation of the feedforward control, the optimization of the control parameters, and the detailed stability analysis—proposes a feedforward control mechanism and configuration based on data fusion in a CCD-based tracking system. Experiments verify that this technique effectively enhances the closed-loop performance in comparison with the classical control mode. Related topics not covered in this paper are how to optimize the control parameters in an all-around manner rather than in an independent way and what kind of filters must be used to estimate target speeds and their effects on control stability. Furthermore, it is very challenging to investigate acceleration feedforward12 to improve performance further.

The authors thank anonymous reviewers for their valuable comments, and also give thanks to the Youth Innovation Promotion Association, Chinese Academy of Science, and the China scholarship council for their sponsorship.

Hutchinson  S., , Hager  G. D., and Corke  P. I., “A tutorial on visual servo control,” IEEE Trans. Rob. Autom.. 12, (5 ), 651 –670 (1996). 1042-296X CrossRef
Ulich  B. L., “Overview of acquisition, tracking, and pointing system technologies,” Proc. SPIE. 887, , 40 –63 (1988). 0277-786X CrossRef
Corke  P. I., Visual Control of Robots: High-Performance Visual Servoing. , Robotics and Mechatronics Series, 2 ,  Research Studies Press (John Wiley) ,  Taunton, England  (1996).
Ekstrand  B., “Tracking filters and models for seeker applications,” IEEE Trans. Aerosp. Electron. Syst.. 37, (3 ), 965 –976 (2001).CrossRef
Fitts  J. M., “Aided tracking as applied to high accuracy pointing systems,” IEEE Trans. Aerosp. Electron. Syst.. 9, (3 ), 350 –368 (1973).CrossRef
Tomizuka  M., “Control methodologies for manufacturing applications,” Manuf. Lett.. 1, (1 ), 46 –48 (2013).CrossRef
Chen  X., and Tomizuka  M., “New repetitive control with improved steady-state performance and accelerated transient,” IEEE Trans. Control Syst. Technol.. 22, (2 ), 664 –675 (2014). 1063-6536 CrossRef
Tenne  D., and Singh  T., “Tracking for maneuvering target trajectories via the 3D circular filter,” IEEE Trans. Aerosp. Electron. Syst.. 23, (3 ), 298 –310 (1987).CrossRef
Li  X. R., and Jilkov  V. P., “A survey of maneuvering target tracking: dynamic models,” Proc. SPIE. 4048, , 212 –236 (2000). 0277-786X CrossRef
Lee  B. J., , Joo  Y. H., and Park  J. B., “An intelligent tracking method for a maneuvering target,” Int. J. Control Autom. Syst.. 1, (1 ), 93 –100 (2003).
Horowitz  R.  et al., “Dual-stage servo systems and vibration compensation in computer hard disk drives,” Control Eng. Pract.. 15, (3 ), 291 –305 (2007). 0967-0661 CrossRef
Boerlage  M.  et al., “Model based feedforward for motion systems,” in  Proc. IEEE Conf. Control Applications , Vol. 2, pp. 1158 –1163,  Istanbul, Turkey  (2003).

Tao Tang received his PhD in 2009 from the Institute of Optics and Electronics, Chinese Academy of Sciences. He joined this institute after his graduation, where he is currently an associate professor. His research interests include control-based sensors, FSM control, adaptive control, and inertial stabilization.

Huaxiang Cai is a PhD candidate in the Institute of Optics and Electronics, Chinese Academy of Sciences. His research interests include control-based sensors, FSM control, adaptive control, and tracking control.

Yongmei Huang is with the Institute of Optics and Electronics, Chinese Academy of Sciences. She received her doctoral degree in 2004, and now is a professor and doctor director in the Institute of Optics and Electronics. Currently, her research interests include technology of beam control and optical engineering.

Ge Ren is with the Institute of Optics and Electronics. He is a professor and doctor director in the Institute of Optics and Electronics. Currently, his research interests include technology of beam control and optical engineering.

© The Authors. Published by SPIE under a Creative Commons Attribution 3.0 Unported License. Distribution or reproduction of this work in whole or in part requires full attribution of the original publication, including its DOI.

Citation

Tao Tang ; Huaxiang Cai ; Yongmei Huang and Ge Ren
"Combined line-of-sight error and angular position to generate feedforward control for a charge-coupled device–based tracking loop", Opt. Eng. 54(10), 105107 (Oct 15, 2015). ; http://dx.doi.org/10.1117/1.OE.54.10.105107


Figures

Graphic Jump Location
Fig. 3
F3 :

The velocity closed-loop control.

Graphic Jump Location
Fig. 2
F2 :

An equivalent feedforward control.

Graphic Jump Location
Fig. 1
F1 :

classical feedforward control.

Graphic Jump Location
Fig. 4
F4 :

Velocity response based Kalman filter.

Graphic Jump Location
Fig. 5
F5 :

The open-loop response while the process variance of the Kalman filter is varying.

Graphic Jump Location
Fig. 6
F6 :

The sensitivity function response.

Graphic Jump Location
Fig. 11
F11 :

The line-of-sight (LOS) error.

Graphic Jump Location
Fig. 7
F7 :

The open-loop response while the time delay is varying.

Graphic Jump Location
Fig. 8
F8 :

The target speed in the A axis.

Graphic Jump Location
Fig. 9
F9 :

The target speed in the E axis.

Graphic Jump Location
Fig. 10
F10 :

The target acceleration.

Tables

References

Hutchinson  S., , Hager  G. D., and Corke  P. I., “A tutorial on visual servo control,” IEEE Trans. Rob. Autom.. 12, (5 ), 651 –670 (1996). 1042-296X CrossRef
Ulich  B. L., “Overview of acquisition, tracking, and pointing system technologies,” Proc. SPIE. 887, , 40 –63 (1988). 0277-786X CrossRef
Corke  P. I., Visual Control of Robots: High-Performance Visual Servoing. , Robotics and Mechatronics Series, 2 ,  Research Studies Press (John Wiley) ,  Taunton, England  (1996).
Ekstrand  B., “Tracking filters and models for seeker applications,” IEEE Trans. Aerosp. Electron. Syst.. 37, (3 ), 965 –976 (2001).CrossRef
Fitts  J. M., “Aided tracking as applied to high accuracy pointing systems,” IEEE Trans. Aerosp. Electron. Syst.. 9, (3 ), 350 –368 (1973).CrossRef
Tomizuka  M., “Control methodologies for manufacturing applications,” Manuf. Lett.. 1, (1 ), 46 –48 (2013).CrossRef
Chen  X., and Tomizuka  M., “New repetitive control with improved steady-state performance and accelerated transient,” IEEE Trans. Control Syst. Technol.. 22, (2 ), 664 –675 (2014). 1063-6536 CrossRef
Tenne  D., and Singh  T., “Tracking for maneuvering target trajectories via the 3D circular filter,” IEEE Trans. Aerosp. Electron. Syst.. 23, (3 ), 298 –310 (1987).CrossRef
Li  X. R., and Jilkov  V. P., “A survey of maneuvering target tracking: dynamic models,” Proc. SPIE. 4048, , 212 –236 (2000). 0277-786X CrossRef
Lee  B. J., , Joo  Y. H., and Park  J. B., “An intelligent tracking method for a maneuvering target,” Int. J. Control Autom. Syst.. 1, (1 ), 93 –100 (2003).
Horowitz  R.  et al., “Dual-stage servo systems and vibration compensation in computer hard disk drives,” Control Eng. Pract.. 15, (3 ), 291 –305 (2007). 0967-0661 CrossRef
Boerlage  M.  et al., “Model based feedforward for motion systems,” in  Proc. IEEE Conf. Control Applications , Vol. 2, pp. 1158 –1163,  Istanbul, Turkey  (2003).

Some tools below are only available to our subscribers or users with an online account.

Related Content

Customize your page view by dragging & repositioning the boxes below.

Related Book Chapters

Topic Collections

Advertisement
  • Don't have an account?
  • Subscribe to the SPIE Digital Library
  • Create a FREE account to sign up for Digital Library content alerts and gain access to institutional subscriptions remotely.
Access This Article
Sign in or Create a personal account to Buy this article ($20 for members, $25 for non-members).
Access This Proceeding
Sign in or Create a personal account to Buy this article ($15 for members, $18 for non-members).
Access This Chapter

Access to SPIE eBooks is limited to subscribing institutions and is not available as part of a personal subscription. Print or electronic versions of individual SPIE books may be purchased via SPIE.org.