Improved line of sight robot tracking toward a moving target

ABSTRACT In this paper, the line of sight (LOS) guidance law is improved to implement tracking toward a moving target. In the presence of sensor noise, an optimal information fusion Kalman filter weighted by scalars is utilized for two-sensor information fusing, improving the trajectory tracking precision. Under the communication delay, n-step ahead Kalman predictor compensates for communication delay and provides LOS guidance law with more accurate target estimates. The results of the simulation demonstrate the feasibility and effectiveness of the proposed control strategy.


Introduction
The line of sight (LOS) guidance law is extensively applied in the field of navigation control, and it is known as three-point guidance (Lee & Lee, 1995;Shneydor, 1998;Zarchan, 1997). The paper of Belkhouche, Belkhouche, and Rastgoufard (2004) presents a linearization method for the kinematics equations of the LOS guidance law, and the method consists of a trajectory-based linearization. In Belkhouche, Belkhouche, and Rastgoufard (2006), the aim is to contribute to the solution of the problem of tracking-interception of a moving object by a wheeled mobile robot and navigation toward amoving goal. In Belkhouche, Wu, Lin, and Jin (2008), the authors suggest a three-point path planning law with multiple observers for wheeled mobile robot navigation to intercept/track a moving goal. Based on Belkhouche et al. (2006), we have designed the three-dimensional LOS guidance law of Feng, Wang, and Zhang (2013). In the study of Jalali- Naini and Esfahanian (2004), the pursuer is always on the line between the target tracker and the target without any error, and the present solution can be used in both surface-to-air and air-to-surface. Different from the control strategy proposed in Jalali-Naini and Esfahanian (2004), it is shown in Belkhouche et al. (2006) that this strategy is based on the integration of the kinematics equations with geometric rules. In Jalali-Naini and Esfahanian (2004), the LOS guidance law is defined in terms of the acceleration of the pursuer.
CONTACT G. L. Zhang zhanggueilin0531@163.com Even though the algorithm of Belkhouche et al. (2006) seems to be quite efficient, it suffers from the following problems.
(1) The value range for the control input of the robot steering angle can be further extended.
(2) Kalman filter cannot be combined with the LOS control strategy to enhance the navigation process. (3) The LOS guidance law was derived assuming no or little communication delay.
In a complicated environment, a filter method is widely used in dynamic state estimation and tracking applications to enhance the navigation process. As studied in Bai, Wang, Zou, and Cheng (2018), the recursive filtering algorithm is proposed in order to track the plant states as accurately as possible. In Li, Karimi, Ahn, et al. (2018), a novel fault detection filter design scheme is formulated as two bi-objective optimization problems. To further improve the accuracy of the individual sensor-based estimates, various multi-sensor data fusion approaches have been studied to resolve this problem. In Jo, Chu, and Sunwoo (2012), numerous Bayesian filters based on sensor fusion algorithms have been studied to mitigate GPS errors. Basing on a simple convex combination track fusion algorithm and the Bar Shalom-Campo track fusion algorithm, the paper of Chen, Gu, and Peng (2015) designed an optimal distributed track fusion algorithm without feedback. In Sun (2004), a unified multi-sensor optimal information fusion criterion weighted by scalars is presented in the linear minimum variance sense. In Sun and Deng (2004), a multi-sensor optimal information fusion decentralized Kalman filter with a two-layer fusion structure is given for discrete time-varying linear stochastic control systems with multiple sensors and correlated noises. The two-sensor information fusion nstep-ahead steady-state optimal Kalman predictor and Wiener predictor are presented in Gao, Wang, Mao, Liang, and Deng (2005), where the optimal weighting matrices and minimum fused error variance matrix are given.
Conventional LOS guidance law was derived assuming no or little delay. In Alkharabsheh, Xiao, Lewis, and Manry (2007), a multi-step predictor is used before the conventional pure proportional navigation (PPN) is applied in the guidance loop, and the predictor compensates for communication delay and provides PPN with more accurate target estimates. Integrated missile guidance and control systems using time-delay control (TDC) are developed in Park, Kim, and Tahk (2011), where TDC can estimate the unknown dynamics and the unexpected disturbance using a one-step time delay. Li, Karimi, Zhang, Zhao, and Li (2018) mainly study the fault detection problem for linear discrete time-varying systems subject to random sensor delay.
In this paper, the work is mainly motivated by the study in Belkhouche et al. (2006). In this paper, the work mainly improved the method in Belkhouche et al. (2006). Different from the control strategy proposed in Jalali-Naini and Esfahanian (2004), this paper defines the LOS guidance law in a simple way in terms of matching the rate of turn of the LOS angle with the rate of turn of the LOS angle for target measured from the stationary observer. Combining the notion of an observer with polar kinematics models, the LOS guidance law is derived to implement navigating and tracking the target. Different from the method in Belkhouche et al. (2006), this paper extends the range of values for the orientation angle of the robot, and improves the LOS guidance law. Moreover, the LOS guidance law combined with information fusion EKF weighted by scalars is applied in this paper. Based on Gao et al. (2005) and Alkharabsheh et al. (2007), the n-step ahead Kalman predictor compensates for communication delay and provides the LOS guidance law with more accurate target estimates.
The contribution of this paper is mainly to improve upon and perfect the LOS guidance law given in Belkhouche et al. (2006). The value range for the control input of the robot steering angle has been further extended. The information fusion Kalman filter weighted by scalars is utilized for two-sensor information fusing, improving the trajectory tracking precision. The n-step ahead Kalman predictor compensates for communication delay and provides the LOS guidance law with more accurate target estimates.
The remainder of this paper is organized as follows. In Section 2, the kinematics models of the robot and the target are derived. Section 3 proposes the improved LOS guidance law. Two-sensor information fusion enhancement is given in Section 4. Section 5 proposes the LOS guidance law combined with the n-step ahead Kalman predictor. The simulation results for different scenarios are given in Section 6. Finally, Section 7 is devoted to the conclusion.

Kinematics models
The robot and the target move in the Cartesian frame of reference. As shown in Figure 1 The distance observer-robot and the distance observer-target are denoted by r b R and r b T , respectively. At the initial time t 0 , the target, the robot and the observer are with three collinear points. The robot is between the observer and the target, and the exception is that the observer coincides with the initial position of the robot. In this paper, the following properties are satisfied by r b Belkhouche et al. (2006), the kinematics equations are written bẏ ( 2 )

Improved LOS guidance law
To simplify the problem, the following assumptions are usually adopted.

Assumption 3.1:
The robot is faster than the moving target, i.e. |v R | > v T > 0. v R may be either a negative value or a positive one.
Assumption 3.2: The observer has a sensory system that allows to measure pose states information of the robot and the target. The robot receives the commands of the observer to track the target.
Based on the LOS guidance law proposed in Belkhouche et al. (2006), the robot can reach the moving target by matching thatσ As a result, the robot lies always on the LOS joining the target and the observer. As shown in Figure 1 Applying (1) then The LOS guidance law matches the positions between the target and the robot, which means that r b R → r b T . Then, r b R /r b T ≤ 1. By using Assumption 1, one can further have Theorem 3.1: Based on Assumptions 3.1 and 3.2 and the control strategy given in (6), the robot can reach the moving target.
Proof: This paper will prove that the relative range between the robot and the target is a decreasing function. The proof is based on the following remarks. Depending on the value of (θ R − σ ), the following cases will be discussed. ( First case: Based on (4), (7) and (8), it is clear thatṙ d < 0.
In the Cartesian frame of reference, the robot moves according to the following discrete kinematics equations: Based on (27) and (28), the robot can reach the moving target under the improved LOS guidance law.
In this paper, the observer is a stationary point. This may cause tracking error when the robot is distant from the observer, and communication between the robot and the observer becomes difficult. In the future work, the closed-form solution of LOS guidance law with a moving observer will be designed, where the robot is always on the instantaneous line between the target and the observer.
In this section, the robot can reach the moving target with the improved LOS guidance law in the absence of obstacles. In the sequel, one briefly imagines the obstacle avoidance algorithm in the future work. According to Kim and Kim (2003), a limit-cycle navigation method is proposed for the robot to avoid obstacles using the limitcycle characteristics of a second-order nonlinear function. This method will be designed for the robot to avoid obstacles using the limit-cycle characteristics. In the future work, the robot can avoid obstacles to reach the moving target under the improved LOS guidance law combined with limit-cycle characteristics.

Two-sensor information fusion enhancement
The state of the target is described by its position and velocity in the X-Y plane, where x(k), y(k) are the position coordinates of the target along X-and Y -axes, respectively, and v x (k), v y (k) are the velocities of the target along X-and Y -directions at the kth time step, respectively. Consider a two-sensor discrete-time system defined as where w(k) and v i (k) are assumed to be mutually independent white Gaussian noise with zero mean and covariance Q, R i , respectively. Initial state X(0) is independent from w(k) and v i (k). Let h i (X(k)) be the distance between the target and the observer. According to Bar-Shalom, Li, and Kirubarajan (2001), an extended Kalman filter (EKF) is expressed as follows: where H i (k) = (∂h i (X(k))/∂X(k))| X(k)=X(k|k−1) , P i (k | k) and P i (k | k − 1) are filter and prediction error covariance matrix, and K i (k) is a gain vector. The error covariance matrix is As designed in Sun (2004), the optimal fusion coefficients weighted by scalars can be calculated as . (35) Thus, the optimal Kalman filter is denoted bŷ The error covariance matrix of optimal information fusion is where trP 0 (k | k) ≤ trP i (k | k), i = 1, 2. The trace of the matrix is denoted by 'tr'.
In the presence of sensor noise, the Kalman estimator in the -target tracking algorithm is described as follows. Firstly, one can obtain the single-sensor filtered position of the target (x T Fi , y T Fi ) and the robot ( In the sequel, the two-sensor fusion position of the target (x T Fu , y T Fu , z T Fu ) and the robot (x R Fu , y R Fu , z R Fu ) can be calculated. r b R Fu and r b T Fu denote the distances from the observer to the robot and the target. The angle of LOS RT Under the discrete model, the robot moves according to the following equations: Based on (38) and (39), the robot can reach the moving target under the improved LOS guidance law combined with two-sensor information fusion enhancement.

LOS guidance law combined with n-step ahead Kalman predictor
In this section, the n-step ahead Kalman predictor is proposed to compensate for delay. The LOS guidance law runs on the observer. Once the observation with the estimation stamped with t k = T s k arrive at t k+n = T s (k + n), where n is the integer number of delay steps, the discrete n-step ahead Kalman predictor is applied to predict the target states at t k+n . Assuming the communication system is time synchronized, when the robot receives the commands from the observer, it compares the time stamp in the packet with its clock to obtain the communication delay and predict target stateX T . By using Gao et al. (2005) and Alkharabsheh et al. (2007), one has and the n-step prediction error covariance matrix is In the Cartesian frame of reference, the robot moves according to the following discrete kinematics equations: Based on (42) and (43), the robot can reach the moving target under the improved LOS guidance law combined with the EKF-based n-step ahead Kalman predictor.

Simulation results
A series of simulations are conducted in this section, where the robot can reach the moving target. For simplicity, it is assumed that the velocity, the distance and the time are without units.
Example 6.1: In the absence of sensor noise, the target is moving clockwise and counterclockwise. In Figure 2, (50, 50) and (0, 0) are the initial positions of the target and the robot, respectively. The observer is located at (−5, −5). Different from the Case 2 given in Belkhouche et al. (2006), the velocity of the robot is v R = −2.55, and θ R is provided by the control strategy given in (26). From Figure 2, the robot can reach the moving target.  Example 6.2: In the absence of sensor noise, the target is moving with the nonlinear time-varying θ T . In Figure 3, the initial positions of the target and the robot are (160, 160) and (0, 0), respectively. The observer is located at (−10, −10). The velocities of the robot and the target are v R = 8 and v T = 4. According to (12), the robot can reach the moving target. In Figure 4, the initial positions of the target and the robot are (310, −189) and (0, 0), respectively. The observer is located at (0, 0). The velocities of the robot and the target are v R = −9 and v T = 6. According to (26), the robot can reach the moving target.

Example 6.3:
In the presence of sensor noise, two-sensor information fusion EKF weighted by scalars combined with the LOS guidance law is proposed to enhance the precision of trajectory tracking. In Figure 5, v R = 10.31 and v T = 1.56, the control strategy is given in (12). In   Figure 6, v R = −10.3 and v T = 1.52, the control strategy is given in (26). The filtered trajectory for the LOS guidance law is illustrated in Figures 5 and 6, where twosensor information fusion is better than the individual sensor-based estimate.

Example 6.4:
In the presence of communication delay, the LOS guidance law combined with n-step ahead Kalman predictor is proposed to enhance the accuracy of trajectory tracking. In this case, the number of delay steps is n = 2. As illustrated in Figure 7, the LOS guidance law combined with the n-step ahead Kalman predictor can achieve more reasonable tracking performance.

Conclusion
In the absence of sensor noise, the robot can successfully reach the moving target with the improved LOS guidance  law. Under the sensor noise, the LOS guidance law combined with information fusion EKF weighted by scalars are proposed to enhance the precision of tracking. In consideration of the negative influence of the communication delay, the n-step ahead Kalman predictor compensates for communication delay and provides LOS guidance law with more accurate estimates.