Estimation and Tracking of a Moving Target by Unmanned Aerial Vehicles 111Department of Mechanical Engineering, National Chiao Tung University, Hsinchu, Taiwan 30010 Email: email@example.com, firstname.lastname@example.org, email@example.comThis research is supported by the Ministry of Science and Technology, Taiwan (Grant Number 107-2628-E-009-005-MY3, 106-2622-8-009-017), and partially supported by Pervasive Artificial Intelligence Research (PAIR) Labs, Taiwan (Grant Number MOST 108-2634-F-009-006-).
An image-based control strategy along with estimation of target motion is developed to track dynamic targets without motion constraints. To the best of our knowledge, this is the first work that utilizes a bounding box as image features for tracking control and estimation of dynamic target without motion constraint. The features generated from a You-Only-Look-Once (YOLO) deep neural network can relax the assumption of continuous availability of the feature points in most literature and minimize the gap for applications. The challenges are that the motion pattern of the target is unknown and modeling its dynamics is infeasible. To resolve these issues, the dynamics of the target is modeled by a constant-velocity model and is employed as a process model in the Unscented Kalman Filter (UKF), but process noise is uncertain and sensitive to system instability. To ensure convergence of the estimate error, the noise covariance matrix is estimated according to history data within a moving window. The estimated motion from the UKF is implemented as a feedforward term in the developed controller, so that tracking performance is enhanced. Simulations are demonstrated to verify the efficacy of the developed estimator and controller.
Estimation and Tracking of a Moving Target by Unmanned Aerial Vehicles
Jun-Ming Li Ching Wen Chen and Teng-Hu Cheng
22footnotetext: Department of Mechanical Engineering, National Chiao Tung University, Hsinchu, Taiwan 30010 Email: firstname.lastname@example.org, email@example.com, firstname.lastname@example.org: This research is supported by the Ministry of Science and Technology, Taiwan (Grant Number 107-2628-E-009-005-MY3, 106-2622-8-009-017), and partially supported by Pervasive Artificial Intelligence Research (PAIR) Labs, Taiwan (Grant Number MOST 108-2634-F-009-006-).
Unscented Kalman Filter, Estimation, Tracking of moving targets, UAV
Knowledge about the position and velocity of surrounding objects is important to the booming fields such as self-driving cars, target tracking and monitoring. In case of performing an object tracking task, position and velocity of the tracking target are typically assumed to be available to achieve better control performance  and  using visual servo controllers. When the target is not static, its velocity needs be considered in the system dynamics as to eliminate the tracking error and to calculate the accurate motion command for the camera. However, obtaining the knowledge online is challenging since the dynamics of the target might be complicated and unknown. Moreover, there are instances that the measurement can be unexpected. For example, the target can exceed the field of view (FOV) of the camera, or cannot be detected due to the unexpected occlusion. Several approaches have been proposed for estimating position or velocity of the target such as by using a fixed camera, sensor networks [4, 5, 6], radar , and some known reference information in the image scene. In order to integrate with applications based on vision system such as target tracking, exploration, visual servo control and navigation  and [9, 10, 11], an algorithm for a monocular camera to estimate position and velocity of a moving target is developed in this work.
To continuously estimate the position or the velocity of a target, it needs to remain in the field of view of the camera, and therefore, motion of the target should be considered. Structure from motion (SfM), Structure and Motion (SaM) methods are usually used to reconstruct the relative position and motion between the vision system and objects in many applications  and . With the knowledge of length between two feature points,  proposed methods to estimate position of the stationary features. In , 3D Euclidean structure of a static object is estimated based on the linear and angular velocities of a single camera mounted on a mobile platform, where the assumption is relaxed in . However, SfM can only estimate the position of the object and usually the object is assumed to be stationary. In order to address the problem to estimate motion of moving objects, SaM is applied for estimation by using the knowledge of camera motion. Nonlinear observers are proposed in  and  to estimate the structure of a moving object with time-varying velocities. The velocity of the object in  is assumed to be constant, and  relaxes the constant-velocity assumption to time-varying velocities for targets moving in a straight line or on a ground plane. In practice, measurement can be intermittent when the object is occluded, outside the camera FOV, etc. [17, 18, 19] present the development of dwell time conditions to guarantee that the state estimate error converges to an ultimate bound under intermittent measurement. In [17, 18, 19], the estimation is based on the knowledge about the velocity of the moving object and the camera. However, in practice the velocity of the target is usually unknown, and modeling its dynamics is complicated and challenging.
In fact, the relationship between target motion estimator and vision-based controller is inseparable. Specifically, output from a high performance target motion estimator can be used as a feedforward term for the controller to keep the target in the field of view longer, which, in return, results in a longer period for the estimate error to converge. In this work, a dynamic monocular camera is employed to estimate the position and velocity of a moving target. Compared to the multi-camera system, using a monocular camera has the advantage of reducing power consumption and the quantity of image data. A You-Only-Look-Once (YOLO) deep neural network is applied in this work for target detection, which relaxes the assumption of continuous availability of the feature point and minimizes the gap for applications, but it also introduces some challenges. That is, the detected box enclosing the target can lead to intermittent measurement, and the probability distribution function of the noise from inaccurate motion model may not follow the normal distribution. An Unscented Kalman Filter (UKF) based algorithm is developed in this work to deal with problems of intermittent measurement and to obtain continuous estimate the target motion even when it leaves the FOV. To deal with the uncertain noise during the estimation, method in  is applied to update the process noise covariance matrix online to guarantee the convergence and the accuracy of the estimation.
Ii Preliminaries and Problem Statement
Ii-a Kinematics Model
Based on the model in , Fig. 1 depicts the relationship between a moving target, a camera, and an image plane. The camera is mounted on the multirotor without relative motion. The subscript denotes the inertial frame with its origin set arbitrarily on the ground, and the subscript represents the body-fixed camera frame with its origin fixed at the principle point of the camera, where and are axes with denoted direction. The vectors denotes the position of the feature point of the target, which is unknown and to be estimated, denotes the position of the camera, which can be measured by the embedded GPS/Motion Capture Systems, and denotes the relative position between the feature point and the camera, all expressed in the camera frame. Their relation can be written as
Taking the time derivative on the both sides of (II-A) yields the relative velocity as
where is the linear velocity of the camera, is the angular velocity of the camera, both are the control command to be designed. In (II-A), is the linear velocity of the dynamic target, which is unknown and needs to be estimated. To relax the limitation of existing results, following assumption is made throughout this work.
The trajectory of the target is unknown but bounded.
Since the dynamics of the camera and the target are coupled, the states of the overall system are defined as
To estimate the position and velocity of the target, the state is defined to facilitate the subsequent analysis. Taking the time derivative on the both sides of (II-A) and using (II-A) obtain a nonlinear function that represents the dynamics of the overall system as
where , , , are defined as
Since the trajectory and motion pattern of the target is unknown, it is modeled by a zero acceleration (i.e., constant velocity) dynamics as formulated in (II-A), which is reasonable during a short sampling time with the unneglectable mass of the moving target. The mismatch between the true and modeled dynamics can be considered as a process noise in an UKF developed in the subsequent section.
Ii-B Image Model
By projecting the feature point into the image frame using the pinhole model yields the projection point as
where denotes the position of the feature point in the image frame, and are the focal length of pixel unit, and represents the position of the center of the image. The area of the bounding box is defined as , and based on the pinhole model the relation between and can be expressed as
where is the area of the target on the side, observed from the camera111Given a sedan as the target, the is about m 1.5m..
The optical axis of the camera remains perpendicular to to ensure better detection accuracy from YOLO.
To estimate precisely from (7), needs to be accurate. Since is a fixed value, the optical axis of the camera needs to remain at fixed angle relative to the plane of
Ii-C Measurement Model
To correct the unobserved system states, the measurement is defined as
where , and can be obtained directly from the detected bounding box, and is measurable as described in Section II-A. By using (II-A), (6), and (7), the estimate measurement for the UKF can be obtained as
where is a signum function, and is the estimate of the denoted argument obtained from the process step in the UKF developed in the next section. In (7), the area of bounding box remains positive despite of the sign of which is positive since the depth is nonnegative. Therefore, to ensure converge to a positive value, the term is added to (8).
Despite the aforementioned advantages, bounding boxes can lose unexpectedly, or the Intersection over Union (IoU) may sometime decrease, leading to intermittent or inaccurate measurements. These inherited defects from the data-driven-based detection motivate the need of Kalman filter for estimation. As the target velocity changes, state predicted by the constant-velocity dynamics model can be inaccurate, and the prediction error can considered as process noise.
Iii Position and Velocity Estimation
Iii-a Unscented Kalman Filter
To estimate state of dynamic systems with noisy measurement or intermittent measurement, Unscented Kalman Filter has been applied in this work. Based on (II-A) and (8), the UKF for nonlinear dynamic system can be expressed as
Based on Remark 3, the YOLO detection might fail incidentally, which makes the measurement correction step in (10) unavailable. When it happens, the state is only predicted by the dynamics model using (9), which is used as a feedforward term to keep the target in the field of view, which is reliable in a short period of time before the detection is recovered.
Iii-B Estimation of Noise Covariance Matrices
When applying Kalman filter, the process and measurement noise covariance matrices are usually provided in prior. As mentioned in Remark 3, the unmodeled dynamics model can be considered as process noises, and the covariance matrix is sensitive to the convergence of estimation. It has been confirmed in our simulations that inaccurate constant covariance matrices can lead to large estimate error or converge failure. To dynamically estimate the process noise covariance matrices, a method developed in  is applied in this work to estimate and update the covariance matrices online, so that a faster and reliable convergence performance can be obtained. That is, the process noise is assumed to be uncorrelated, time-varying, and nonzero means Gaussian white noises that satisfies
where is the Kronecker function. By selecting a window of size the estimate of the process noise covariance matrix can be expressed as
Since might not be a diagonal matrix and positive definite, it is further converted to a diagonal, positive definite matrix as
where is the -th diagonal element of the matrix . On the other hand, the measurement noise can be measured in advance.
Iv Tracking Control
In this section, a motion controller for the multirotor is designed using vision feedback. Compared to the existing Image-based Visual Servo (IBVS) control methods, the controller developed in this work not only uses feedback but also includes a feedforward term to compensate the target motion and to ensure better tracking performance, where the feedforward term is obtained from the UKF developed in Section III-A. Most existing approaches either focus on the estimate of target position/velocity or camera position/velocity, but yet the controllers designed for the cameras are rarely discussed, and vice versa. Additionally, the relation between estimating the target motion and controlling the camera are highly coupled. That is, a high performance motion controller can minimizes the estimate error (i.e., the camera is controlled to keep the target in the field-of-view longer), which, in return, yields a precise feedforward term to facilitate the tracking performance, and vice versa. Finally, since YOLO deep neural network is employed to enclose the target in the image, the envelop area is defined as a new reference signal for the controller to track.
Iv-a Target Recognition
YOLO is a real-time object detection system with reasonable accuracy after training. Our YOLO network is trained by using a large number of dataset and the performance is verified before implementation in this work.
The IBVS controller based on  is employed in this work for achieving tracking control of dynamic targets. To this end, a vector denoted a feature vector is defined as the control state which is defined in (II-A). The visual error to be controlled is defined as
where is a desired constant vector of the feature vector predefined by the user (i.e., typically is selected as the center of the image and is a function of the expected distance to the target). Taking the time derivative of (14) and using (II-A) yield the open-loop error system as
where and are considered as the control inputs, is the feedforward term estimated by the UKF, and is the interaction matrix defined as
Note that as the error signal converges to zero, the position of the camera relative to the target is not unique, due to the fact that the camera control input and have a higher dimension compared to To keep the camera staying on the left-hand-side of the target as to maintain high detection accuracy from YOLO222Pose estimate of the target at this angle can be achieved by a well-trained YOLO, and the extension to multiple angles of view will be trained in the future., is controlled to track the moving target as to stay on the specified angle facing toward the target as
where the design is inspired from . In (16), is the width of the image in pixel, denotes the expected distance to the target, is the horizontal field of view of the camera, and and are the current and the expected angle of view with respect to the target, respectively. Since is specified in (16), the corresponding column in the interaction matrix defined in (15) can be removed, which gives the resultant matrix as
Using the Moore-Penrose pseudo-inverse of as well as adding a feedforward term, the tracking controller for the camera can be designed as
The block diagram of the controller is shown in Fig. 3.
V-a Environment Setup
In the simulation333https://goo.gl/93EDnd, a car is considered as a moving target and is tracked by a quadrotor, where the developed controller as well as the UKF are implemented. A camera is implemented on the quadrotor to provide visual feedback. Specifically, bounding boxes are generated in the image to enclose detected cars as shown in Fig. 2, which is achieved by a pretrained YOLO deep neural network. The simulation is conducted in the ROS framework (16.04, kinetic) with Gazebo simulator. In the simulation environment, the value of and the resolution of the image is x with 50 fps. The intrinsic parameters matrix of the camera is
which is obtained by calibration.
A time moving window of width is set to be 150 with sampling rate of . The initial process and measurement noise covariance matrices are selected as and respectively, and the process noise covariance matrix is estimated online using (13).
The initial location of the car and the drone are and along with the initial orientations and in radians, respectively, all expressed in the global frame. The car is free to move on the plane, and its velocity is specified based on the real-time user command.
V-B Simulation Results
Fig. 4 depicts the position estimate errors of the moving vehicle with simulation period of 223 seconds. The position estimate errors are reduced from 14% to 7% as the target moves from time-varying velocity to constant velocity, despite some noises. Note that in practice the drone may not react fast enough to the rapid change of velocity, in which the optical axis cannot remain facing right to the target, leading to a slight deviation in the depth estimate (i.e., ). Note that the position estimate error in the -axis increases as the velocity in the -axis increases. This can be attributed to the fact that the increasing velocity of the vehicle in the -axis causes the quadrotor to tilt forward for tracking, which breaks the assumption 2 that the optical axis is facing toward the side of the vehicle and leads to a large estimate error.
In Gazebo environment, the velocity of the car is set below 4 m/s due to a large drag. As the speed of the car increases, the quadrotor accelerates with a tilt angle, which increases the chance of object detection failure. However, the problem can be resolved by expanding the training dataset with images from different angles of view, which will be part of the future work.
Fig. 5 depicts the velocity estimate errors of the moving vehicle. The estimate performance is slightly compromised when the vehicle is accelerated (i.e., due to the constant-velocity model utilized in the UKF), but better estimate performance can be expected by increasing the sensing rate for the UKF measurement. The increase of the velocity estimate error between 103-223 seconds can be attributed to the acceleration of the target in the -direction.
Fig. 6 depicts the estimate error without process noise estimation, where the constant covariance matrix is same as the initial value. Compared to Fig. 5, using the estimated noise covariance matrix developed in (13) yields a better velocity estimate performance.
In this work, a motion controller for a camera on an UAV is developed to track a dynamic target with unknown motion and without motion constraint. The unknown target motion is estimated in the developed UKF with process noise covariance matrix estimated based on the past data within a moving window, and the intermittent measurement caused by YOLO detection is addressed. The estimated target velocity is then included as a feedforward term in the developed controller to improve tracking performance. Compared to the case without noise estimation, the developed approach is proven to obtain better tracking performance. Although Assumption 2 is rigorous in practice, this work is the first one to prove the feasibility of the overall control architecture, and future work will be relaxing the assumption by training a YOLO network to detect the target from any angles. Additionally, eliminating the knowledge of the ground truth will be another future works.
-  N. R. Gans, A. Dani, and W. E. Dixon, “Visual servoing to an arbitrary pose with respect to an object given a single known length,” in Proc. Am. Control Conf., Seattle, WA, USA, Jun. 2008, pp. 1261–1267.
-  A. Dani, N. Gans, and W. E. Dixon, “Position-based visual servo control of leader-follower formation using image-based relative pose and relative velocity estimation,” in Proc. Am. Control Conf., St. Louis, Missouri, Jun. 2009, pp. 5271–5276.
-  V. Chitrakaran, D. M. Dawson, W. E. Dixon, and J. Chen, “Identification of a moving object’s velocity with a fixed camera,” Automatica, vol. 41, pp. 553–562, 2005. http://ncr.mae.ufl.edu/papers/auto05.pdf
-  A. Stroupe, M. Martin, and T. Balch, “Distributed sensor fusion for object position estimation by multi-robot systems,” in Proc. IEEE Int. Conf. Robotics and Automation, Seoul, South Korea, May 2001, pp. 1092–1098.
-  A. Kamthe, L. Jiang, M. Dudys, and A. Cerpa, “Scopes: Smart cameras object position estimation system,” in EWSN ’09, Berlin, Heidelberg, Feb. 2009, pp. 279–295.
-  H. S. Ahn and K. H. Ko, “Simple pedestrian localization algorithms based on distributed wireless sensor networks,” IEEE Trans. Ind. Electron., vol. 56, no. 10, pp. 4296–4302, Mar. 2009.
-  J. Schlichenmaier, N. Selvaraj, M. Stolz, and C. Waldschmidt, “Template matching for radar-based orientation and position estimation in automotive scenarios,” in Proc. IEEE MTT-S Int. Conf. Microw. Intell. Mobility (ICMIM), Nagoya, Japan, Mar. 2017, pp. 95–98.
-  Z. Huang, W. Liu, and J. Zhong, “Estimating the real positions of objects in images by using evolutionary algorithm,” in Proc. Int. Conf. Mach. Vis. Inf. Technol., Singapore, Feb. 2017, pp. 34–39.
-  J. Thomas, J. Welde, G. Loianno, K. Daniilidis, and V. Kumar, “Autonomous flight for detection, localization, and tracking of moving targets with a small quadrotor,” IEEE Robot. Autom. Lett., vol. 2, no. 3, pp. 1762–1769, Jul. 2017.
-  E. Palazzolo and C. Stachniss, “Information-driven autonomous exploration for a vision-based mav,” in Proc. of the ISPRS Int. Conf. on Unmanned Aerial Vehicles in Geomatics (UAV-g), Bonn, Germany, Sep. 2017, pp. 59–66.
-  D. Scaramuzza, M. Achtelik, L. Doitsidis, F. Fraundorfer, E. Kosmatopoulos, A. Martinelli, M. Achtelik, M. Chli, S. Chatzichristofis, L. Kneip, D. Gurdan, L. Heng, G. Lee, S. Lynen, L. Meier, M. Pollefeys, A. Renzaglia, R. Siegwart, J. Stumpf, P. Tanskanen, C. Troiani, and S. Weiss, “Vision-controlled micro flying robots: From system design to autonomous navigation and mapping in gps-denied environments,” IEEE Robot. Autom. Mag., vol. 21, no. 3, pp. 26–40, Aug. 2014.
-  V. K. Chitrakaran and D. M. Dawson, “A lyapunov-based method for estimation of euclidean position of static features using a single camera,” in Proc. Am. Control Conf., New York, NY, USA, Jul. 2007, pp. 1988–1993.
-  D. Braganza, D. M. Dawson, and T. Hughes, “Euclidean position estimation of static features using a moving camera with known velocities,” in Proc. IEEE Conf. Decis. Control, New Orleans, LA, USA, Dec 2007, pp. 2695–2700.
-  A. P. Dani, N. R. Fischer, and W. E. Dixon, “Single camera structure and motion,” IEEE Transactions on Automatic Control, vol. 57, no. 1, pp. 238–243, Jan. 2012.
-  A. Dani, Z. Kan, N. Fischer, and W. E. Dixon, “Structure and motion estimation of a moving object using a moving camera,” in Proc. Am. Control Conf., Baltimore, MD, 2010, pp. 6962–6967.
-  A. Dani, Z. Kan, N. Fischer, and W. E. Dixon, “Structure estimation of a moving object using a moving camera: An unknown input observer approach,” in Proc. IEEE Conf. Decis. Control, Orlando, FL, 2011, pp. 5005–5012.
-  A. Parikh, T.-H. Cheng, and W. E. Dixon, “A switched systems approach to image-based localization of targets that temporarily leave the field of view,” in Proc. IEEE Conf. Decis. Control, 2014, pp. 2185–2190.
-  A. Parikh, T.-H. Cheng, and W. E. Dixon, “A switched systems approach to vision-based localization of a target with intermittent measurements,” in Proc. Am. Control Conf., Jul. 2015, pp. 4443–4448.
-  A. Parikh, T.-H. Cheng, H.-Y. Chen, and W. E. Dixon, “A switched systems framework for guaranteed convergence of image-based observers with intermittent measurements,” IEEE Trans. Robot., vol. 33, no. 2, pp. 266–280, April 2017.
-  K. Zhang, J. Chen, B. Jia, and Y. Gao, “Velocity and range identification of a moving object using a static-moving camera system,” in IEEE Conf. Decis. Control, Las Vegas, NV, USA, Dec. 2016, pp. 7135–7140.
-  J. Redmon and A. Farhadi, “Yolov3: An incremental improvement,” arXiv preprint arXiv:1804.02767, Tech. Rep., 2018.
-  S. Gao, G. Hu, and Y. Zhong, “Windowing and random weighting-based adaptive unscented kalman filter,” Int. J. Adapt. Control Signal Process., vol. 29, no. 2, pp. 201–223, Feb. 2015.
-  E. A. Wan and R. van der Menve, “The unscented kalman filter for nonlinear estimation,” in Proc. IEEE 2000 Adaptive Syst. Signal Process., Commun., Control Symp., Lake Louise, Alberta, Canada, Oct. 2000, pp. 153–158.
-  F. Chaumette and S. Hutchinson, “Visual servo control part I: Basic approaches,” IEEE Rob. Autom. Mag., vol. 13, no. 4, pp. 82–90, 2006.
-  N. Shahriari, S. Fantasian, F. Flacco, and G. Oriolo, “Robotic visual servoing of moving targets,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Tokyo, Japan, Nov. 2013, pp. 77–82.
-  J. Pestana, J. L. Sanchez-Lopez, S. Saripall, and P. Campoy, “Computer vision based general object following for gps-denied multirotor unmanned vehicles,” in Am. Control Conf., Portland, OR, USA, Jun. 2014, pp. 1886–1891.