Leader-Follower Tracking System for Agricultural Vehicles: Fusion of Laser and Odometry Positioning Using Extended Kalman Filter

Received Jul 10, 2014 Revised Sep 11, 2014 Accepted Oct 5, 2014 The aim of this research was to develop a safe human-driven and autonomous leader-follower tracking system for an autonomous tractor. To enable the tracking system, a laser range finder (LRF)-based landmark detection system was designed to observe the relative position between a leader and a follower used in agricultural operations. The virtual followerbased formation-tracking algorithm was developed to minimize tracking errors and ensure safety. An extended Kalman filter (EKF) was implemented for fusing LRF and odometry position to ensure stability of tracking in noisy farmland conditions. Simulations were conducted for tracking the leader in small and large sinusoidal curved paths. Simulated results verified high accuracy of formation tracking, stable velocity, and regulated steering angle of the follower. The tracking method confirmed the follower could follow the leader with a required formation safely and steadily in noisy conditions. The EKF helped to improve observation accuracy, velocity, and steering angle stability of the follower. As a result of the improved accuracy of observation and motion action, the tracking performance for lateral, longitudinal, and heading were also improved after the EKF was implemented in the tracking system. Keyword:


INTRODUCTION
The challenges of high-quality agricultural production and lack of farming workforce demand changes in the traditional agricultural production system. The development of autonomous agricultural machinery creates the opportunity to shift conventional agriculture to an intelligent agricultural system. Autonomous agricultural machinery could ensure precise operation, increase productivity, minimize the size of the required workforce, and improve production.
In the past few decades, numerous studies have been performed on navigation of autonomous tractors, including positioning, driving, and steering control functions. Advanced sensing technologies, control theories, and high accuracy control of autonomous tractors have been developed [1]- [4]. However, most of the previous research focused on the navigation of a single tractor. There is in fact a strong need for cooperation between multiple machines in agricultural operation. One of the typical applications of multiple machines can be observed in harvest operations. During harvest operations, a follower tractor is required to keep formation with a leader combine (Fig.1a) or operation of multiple harvesters (Fig.1b). This is an arduous and dangerous task for drivers who have to focus their attention for a long time. An autonomous tracking follower is a good choice for not only solving the safety problem but also improving working performance [5]. Compared to the navigation of a single tractor robot, a multi-tractor robot system is much more difficult and involves a much higher order of complexity [6]. Considering tracking stability and safety, the primary problem is designing a suitable sensing system that can provide constant and precise observation of the relative position of two vehicles. The sensing system should be competent under changing movement and posture of the two vehicles. A master-slave tractor-based auto tracking system was developed using a RTK-GPS and Gyroscope which provided positional information [7]. The system was tested using Fendt936 model tractors, and the tracking error was less than 20 cm on a curved path. The lack of signal correction during interruptions in the GPS and the additional cost of a GPS and Gyroscope make this system suboptimal for solving the tracking problem. Additionally, GPS provides only absolute position information. While tracking, we need to continually update the relative distance between the leader and the follower. If the GPS signal is interrupted, there is a possibility of collision in tracking, or the development of a large offset due to the loss of updated positional information. To overcome the limitations of relative distance measurement using GPS and safety concerns, a low cost and precise ultrasonic sensor-based system has been successfully applied on an auto tracking system for multiple combines [8]. However, the short detection distance and limited detection angle of the ultrasonic sensor often resulted in loss of the target. In other research, position detection of a leader vehicle was conducted using a LRF, which was installed on a follower vehicle [9]. The leader body recognize-based algorithm was observed that a notable measurement error would occur in the curved path. However, using an LRF was considered as a potential method for determining multiple tractors' relative position. As the LRF could not only provide the distance to an object but also position, motion, and direction quickly with high accuracy over a wide detection angle [10].
In the tracking of a leader-follower, there are limitations of the control system due to nonlinear and nonholonomic constraints [11]. In the multi-robot navigation system, the leader always controls its own way and motion strategy, and the follower decides its action based on the relative position and current action of the leader. On the other hand, in a dynamic tracking system, there is an unavoidable time delay for the follower to respond. A control law that can quickly and correctly respond to tracking error is required. Simulation of sliding mode control and simple PD control were proposed to solve the tracking problem for master-slave tractors [12]. These two methods were compared, and it was observed that the sliding mode control had better performance in improving lateral offset and spacing controls. Strict feedback control using Lyapunov's second method, based on the chained form, was successfully designed for solving multiple nonholonomic mobile robot problems [13], [14]. Model predictive control [15] and receding horizon control [16] were also successfully ultilized for addressing the formation control problem of multiple nonholonomic mobile robots.
In an adverse environment such as agricultural operations, a proper sensing and control system alone are insufficient to ensure the stability and safety tracking of the leader and the follower. Because there are sources of noise from sensors, for example, farmland surface conditions cause a large odometry error [17], laser detection is affected by swinging of the vehicle body [18], and dust and strong sunshine make laser detection difficult [19]. Thus, obtaining correct observation information from noisy signals is another 3 key issue for stability and safety tracking. Unfortunately, not much research has been performed to solve noise reduction problems during the formation tracking of multiple tractors in agricultural application. As an effective method, EKF was most often applied for data fusing and noise reduction [20]. A LRF-based EKF position estimation system in tree fruit orchards was designed, and field tests showed that the position estimation system works with sufficient accuracy [21]. To overcome the problems of noisy conditions described above, LRF-landmark and Odometry-based fusing can be used in the curve-path formation tracking.
One potential means is a LRF-landmark-based system, where a virtual follower-based feedback control system and EKF fusing system could be implemented together. The LRF-landmark-based method could be used to detect the relative position between the leader and the follower. The LRF could be implemented on the follower, and reflectors mounted on the leader can be used as landmarks. Utilizing the geometric relationship between the LRF and the landmarks, the relative position between the leader and the follower could easily be calculated. Landmark detection has already been utilized and has proven to have high precision and stability in our previous research [22]. The virtual follower-based feedback-tracking algorithm has the potential to ensure safe tracking, where the virtual follower can maintain the required position with the leader. The EKF can be used to fuse odometry data with LRF position. In the odometers, the rotary encoders installed on rear wheels and the linear encoder installed on steering part provide the position and posture of the vehicles. Encoders can support rapid and accurate data collection, but their drawbacks come from accumulated errors and they are sensitive to slope and uneven surface [23]. By fusing odometry data and laser-based positioning data with an EKF, the bounded noisy laser signal can overcome the unbounded accumulated error of odometry [24]. A short-duration smooth signal of odometry can also be used to support the laser position, and then a real-value near-estimated relative position between the leader and the follower can be determined. Thus, the objectives of this research were as follows: 1) To develop a tracking system for multiple agricultural machinery combinations, with a leader and a follower, including virtual follower-based feedback control. 2) To develop a laser-landmark based tracking system to identify the relative position between the leader and the follower. 3) To introduce an EKF fusing system to improve accuracy of the leader-follower relative position in the virtual follower-based feedback control system.

LEADER-FOLLOWER FORMATION SYSTEM
In this research, a leader-follower-based formation system was proposed. The point and the arrow in (Fig.2a) represent the required position of the follower and a virtual follower was imaged there (Fig.2b). The relative position between the leader and virtual follower was set to a constant. By tracking the position of virtual follower, safety and required formation between the leader and the follower could be obtained. In addition, the information about the velocity and steering angular velocity of the leader could be sent from the leader to the follower.

FORMATION TRACKING ALGORITHM
This section describes a solution of the formation-tracking problem for the leader and follower tractors. The location of the leader, the virtual follower, and the follower are defined as locations 0 P , 1 P ,and 2 P . These three points are located on the middle of their rear axles (Fig.3). The state of the leader, the virtual follower, and the follower can be expressed as: [ ] x y ,and    Considering the operational mode and working style of agricultural operation, the formationkeeping problem in this research can be stated as follows: the leader is driving on a given path and asks the follower to keep a relative distance 01 d and a relative angle 01  ; this required state is the virtual follower. If x y  between the follower and the virtual follower can always converge to zero, then the required formation between the leader and the follower could be maintained. By changing the relative distance of 01 d and a relative angle 01  , formation can be varied.

Kinematic Model
According to the car-like kinematic model, the rear-wheel drive kinematic equation for both the leader and the follower is given by the following expression: Where v is the velocity and w is the steering angular velocity of the vehicle and L is the length of the wheelbase. The state of the virtual follower depends on the global coordinates and relative position (Fig.3). The state of the virtual follower can be expressed as: As mentioned above, the formation-tracking problem can be thought of as the tracking problem of the follower and the virtual follower, and if the state error   , , e e e x y  asymptotically approaches zero, the desired formation can be kept. The formation-tracking error between the follower and the virtual follower can be expressed as: A feedback control law can be obtained according to the formation-tracking error and control input of the virtual follower. The expression of feedback control can be referred to as [25]: Where  

LRF-based Follower Formation Tracking Error Observation
The formation tracking error   , , e e e x y  was calculated under global states. If both the leader and the follower were equipped with a GPS, the global state of both was available; however, the formation-tracking problems discussed in this research are based on some assumptions: 1) In the case of an auto-driven leader, only the leader is equipped with GPS and the follower is GPS-free but equipped with the LRF for obtaining relative position information of the leader. This means that only the global state of leader is available, and the follower can only obtain its position relative to the leader.
2) Furthermore, in the case of the human-driven leader, both the leader and the follower are not equipped with GPS, and in this condition, it is impossible for the leader and the follower to get their global states. Thus in both the GPS equipped auto-driven leader and the human-driven leader cases, it is impossible for the GPS-free follower to obtain its global position. To solve this problem, the LRF-landmark observation system could detect the relative distance and the relative angle between the leader and the follower. Furthermore, the laser-detected relative position can be used to estimate the formation tracking error.
Three landmarks were considered on the leader (shown in the red dotted circle) (Fig.4), and an LRF was on the follower. To facilitate the calculation, we mounted the first landmark on the middle point of front axles and the third landmark on the middle point of the leader rear axles 0 P . The LRF was placed at the middle point of the follower rear axles 1 P . The distance from the first landmark to the third landmark is equal to the length of the leader ( 3 l L  ). The location of the third landmark can be used to represent the location of the leader, and the location of the LRF can be used to represent the location of the follower. It is clear that the laser detection of 3 d and 3  represents the relative distance and the relative angle between the leader and the follower. Thus, the local coordinate system was established based on the follower and the position of the leader and was obtained using x y means the leader location under the follower-based local coordinate (Fig.5).
Using the geometrical relationship between the LRF and the landmarks, the relative heading angle  between the leader and the follower could also be calculated as: Obviously, the follower-based local heading angle of the leader is equal to the relative heading angle and is given by The formation-tracking error ( , , ) e e e x y  could be easily calculated under the leader-based local coordinate system, following the relationship of coordinates between the leader, the follower, and the virtual follower (Fig.5).

Extended Kalman Filter
The feedback control law was established under the ideal kinematic assumption of no sensor noise and disturbance. In real conditions, the formation-tracking error detected by the LRF in Eq. 12, odometer data of the leader ( , ) l l v w , and odometer data of the follower ( , ) f f v w are corrupted with errors and noise.
Additionally, the LRF data are low update frequency, and they are also noisy in an adverse environment. To improve the stability of the formation controller, an EKF was introduced to reduce the model error and fuse the LRF observation and odometer data. The nonlinear leader-follower model described the state transition under a control input, and observation model described the observation under current state, can be expressed respectively as follows: Where k X is the current state vector representing the leader-follower relative state at time instant k ; V and k W are noises from odometer data and LRF observation, and their covariance matrices were defined as k Q and k R . The EKF include two steps which are prediction step and correction step, and data fusion was practiced through recursive the two steps. The prediction step predicts the current leader-follower relative state k X  based on the nonlinear system model ( ) f  ;estimated the observation k Z  from the current estimate state k X  based on the observation model ( ) h  ; and prediction the state error covariance matrix k P  : The correction step updates the Kalman gain k K . The estimateˆk X and state error covariance matrix k P were corrected by integrating the observation function ( ) h  when the LRF observation is available: where 1k X  represent the corrected state and 1k P  represent the corrected state error covariance matrix at previous time. , h  with respect to input k U and observation k Z , and I is defined as an identity matrix.

Accomplishment of the EKF
Based on this research, the leader-follower relative state vector and state observation vector assuming no noise is defined as:

P P
and heading position of the follower (Fig.6). By analyzing the relation between the leader and the follower, we get the following equations: 12 12 12 12 cos( ) sin( ) l l The above equations show the leader-follower relative state based on odometer. Note that the heading angles of the leader and the follower l  and f  in Eq.24 are in global coordinates. As discussed above, it is impossible to obtain the global coordinates in this research due to the absence of GPS and Gyroscope. ( t a n ) where  The state evolution from time instant k-1 to k+1, in which left-side vehicles represent the corrected state 1k X  at the previous time instant and right-side vehicles represent the prettiest state k X  at current (Fig.7). As an important step, the leader-based local coordinates were updated at each prediction step, which meant that the heading angle of the leader under the leader-based local coordinates was a constant equal to zero: During the time interval s T , both the leader and the follower changed their state. Equation (25) expressed updated information of leader-based local coordinates, and estimation of the leader-follower relative state in the leader-based local coordinate system from time k-1 to k. Establishing and updating the leader-based coordinates in a timely manner made tracking possible even though there were no GPS and Gyroscope in the follower and helped to eliminate the effect of incremental error of encoders.
Following the odometry-based state estimate Eq.25, the system and input Jacobeans , Based on the above observation function Eq.27 and the LRF-landmark observation calculation in Eqs.9 and 10, the observation Jacobean k H and , w k J can be given (see Appendix): The system and observation functions was defined and Jacobeans functions were calculated. Once the Jacobeans functions are known, Kalman gain, the leader-follower relative state observation, and state error covariance matirx can be found using Eqs.15 to 20. In this research, the input of system function ( ) f  is the encoder information of steering angle and velocity. Under farmland conditions, the noise covariance matrix Q for the encoders can be defined as: Besides, the observation was assumed to be provided by the LRF (SICK LMS 211), and data were relative distance and angle of each landmark from the LRF. The observation noise covariance matrix R for the LRF can be expressed as: As noted above, the LRF observation information is not always available because of its low update frequency (compared with odometry system) and propagation delay of LRF signals. Thus, the correction step is only processed when the LRF observation is available. This means that If LRF observation is available: The posteriori estimateˆk X and state error covariance matrixˆk P could be calculated by fusing the odometry based priori estimate state k X  and LRF-based observation results k Z using Eqs.19 and 20. If LRF observation gets delayed: The posteriori estimateˆk X and state error covariance matrixˆk P approximately adopt the priori estimate state k X  and state error covariance matrix k P  for calculating next time instant k+1:

RESULTS AND DISCUSSION
Simulation were executed to evaluate the EKF relative position estimate algorithm based on the leader-follower feedback control system. The simulator was designed using C++ builder XE3. The simulation was conducted for a human-driven tractor equipped with rotary encoders on the rear and front wheels to record velocity and steering angle of the tractor. A wireless LAN module was included for transmitting data to the follower. A follower might also be equipped with the encoders and the wireless LAN and can be implemented same as leader. To measure the relative position of the leader, a LRF was considered on the follower to measure the leader's position using artificial landmarks.
As discussed above, the human-driven leader and the autonomous follower can be used for harvesting operations. In our simulation, trajectory of the leader was given as a small and a large curvatures sinusoidal path. The speed of the leader was set at 1. The time for data transmission using wireless LAN and LRF scan interval was simulated as 20 ms and 200 ms, respectively. Note that the laser detection frequency was set lower than the usual frequency of LMS 211. And the time interval Ts was 100 ms.
In the simulation, sensor noise was selected based on the farmland condition and previous studies. The noise of odometry (encoders) was generated by random functions, and added to the velocity and steering angle of the leader and the follower were around 0.032 m/s and 0.0524 rad/s [26,27]. The noise of LRF was also generated by random functions, and added to the distances and angles between the LRF and each landmarks were around 5 cm and 0.035 rad under a distance around 4 m [28] (Fig.4). The initial states of the leader and the follower are expressed as ( , , , ) x y   . At the initial states, there were position errors for lateral, longitudinal, and heading of 1.68 m, 0.25 m, and 1.26˚, respectively, in the small curved path. The position error in the large curved path was 0.82 m, 0.47 m, and 10.37˚ for lateral, longitudinal, and headring, respectively. Meanwhile, the required relative distance and angle were set as 3.5 m and 40º. In the simulation, the global position of the follower was unavailable and the only information transmitted from the leader to the follower was the velocity and steering angular velocity of the leader (simulating LRF-based and GPS-free autonomous driving). The relative distance and the relative angle between the leader and the follower could be relayed using the wireless LAN during farmland operations. While we used the feedback control, the leader velocity and steering angular velocity could also be transmitted using the wireless LAN to the follower. Simulations on small and large sinusoidal curvature paths were performed ( Fig.8 and 9).  The results of formation tracking were compared for three cases: excluding sensor noise, including sensor noise, and fusion of sensor data using the EKF. In the second case, the odometry positioning was used while the LRF positioning was not available. In the third case, the fusion was performed with odometry and LRF-observed position data using the EKF algorithm. In the first case, the ideal case was assumed, where both the LRF and odometry were noise-free. In the simulated experiment results, it was clearly observed that the small and large curvature paths were affected by sensor noise. Furthermore, the trajectory of the follower was not smooth and deviated from the ideal trajectory. After fusing sensor data using EKF algorithm, the trajectory of the follower was smooth. The formation tracking and observation accuracies were also improved.
In the case of leader-follower relative positioning, error was observed while there was sensor noise ( Fig.10 and 11). Consequently, instability in the velocity and steering angle of the follower was observed. To improve the relative positioning error, the EKF algorithm was implemented for motion stability ( Fig.12 and 13). After adding the EKF, the observation error was decreased. As a result, the velocity and steering angle of the follower was stable and close to the value similar to the case with no noise.
After adding the EKF for the small sinusoidal curved path, the RMSE of lateral, longitudinal, and heading observation error was reduced from 0.181 to 0.173 m, 0.166 to 0.053 m, and 4.373 to 1.807˚, respectively. The RMSEs of the velocity and steering angle of the follower were reduced from 0.167 to 0.053 m/s and 6.029 to 2.406˚, respectively. In the case of large curvature path, the RMSEs for lateral, longitudinal, and heading error were reduced from 0.191 to 0.126 m, 0.175 to 0.045 m, and 4.672 to 1.718˚, respectively. The RMSEs of velocity and the steering angle of the follower were also reduced from 0.176 to 0.039 m/s and 7.659 to 3.157˚, respectively.   Because observation accuracy was improved significantly along with stable velocity and steering angle, the tracking accuracy was improved for small and larger curved paths ( Fig.14 and 15). On the small curved path after adding the EKF, the RMSE of lateral, longitudinal, and heading tracking error was reduced from 0.295 to 0.251 m, 0.135 to 0.11 m, and 4.856 to 3.938˚, respectively. On the large curved path, the RMSEs of lateral, longitudinal, and heading tracking error were reduced from 0.303 to 0.227 m, 0.259 to 0.228 m, and 13.416 to 13.198˚, respectively.

CONCLUSIONS
In this study, a human-driven leader and automatic follower formation-tracking system was proposed. A simple and effective LRF-landmark detection method and a safe virtual follower-based feedback control law were introduced. An EKF estimation algorithm was also developed and proven to have good performance for reducing noise. After integrating the EKF, the LRF noise was decreased and updated relative positional information between the leader and the follower quickly and with high accuracy. As a result, stable velocity and steering angle of the follower and high accuracy of formation tracking was established. By estimating the relative position between the leader and the follower in the local coordinate system, the follower could update positional information independently, particularly without GPS and Gyroscope sensors to define its position for tracking the leader. Therefore, a low-cost, reliable navigation system for the leader and the follower could be available.

APPENDIX
The system and input Jacobeans ,