Object Position and Orientation Tracking for Manipulators Under Sensing Dynamics Yongxiang Fan 1 , Hsien-Chung Lin 1 , Yu Zhao 1 , Chung-Yen Lin 1 , Te Tang 1 , Masayoshi Tomizuka 1 and Wenjie Chen 2 Abstract— Real-time object tracking for manipulators has been applied to industrial automation such as assembly and human robot collaboration. During the tracking process, the vision sensor is used as a feedback to the robot controller. It means that the imperfect sensing and stability issues must be carefully considered. This paper deals with the sensing dynam- ics (e.g. limited sampling rate, irregular sampling time, packet loss, noise and latency), and realizes globally asymptotically stable tracking. First, an irregular time Kalman filter is used to estimate the markers’ positions and the corresponding error covariances. Then, a maximum likelihood estimation problem is solved to estimate the transformation between the target frame and the world frame. In addition, a dynamic tracking controller is implemented to realize object tracking. The stability of the tracking controller under model uncertainties is also discussed. The proposed tracking algorithm is experimentally verified on an industrial robot. I. INTRODUCTION Machine vision techniques have broad applications in industry, including material handling, assembly and human- robot collaboration. Traditionally, a manipulator follows the look-then-move algorithm [1], which involves 1) take a photo and locate the static workpieces, 2) run the motion planning algorithm and generate a feasible trajectory, and 3) execute the trajectory to reach the workpiece. However, with increasingly complex tasks and strictly precise requirement, the traditional look-then-move method will not be sufficient. Instead, real-time visual tracking is able to detect moving workpieces and feedback the sensor signal into the robot controller in an on-line manner. Therefore, it explores the possibilities to control a manipulator to execute more com- plicated tasks with higher accuracy. However, in order to serve as feedback for manipulator, the vision signal has to be updated in each controller servo cycle. This requires high-speed vision sensors, which are not practical due to the cost issue. On the other hand, low-cost vision systems may have sensing dynamics, such as a limited sampling rate, irregular sampling time, packet loss, noise, and latency. Without considering these undesired properties, the tracking accuracy and stability cannot be guaranteed. Apart from the sensing dynamics, there exist several challenges for tracking controller design. First, a global pa- rameterization of attitude has to be built to avoid singularities 1 Yongxiang Fan, Hsien-Chung Lin, Yu Zhao, Chung-Yen Lin, Te Tang, and Masayoshi Tomizuka are with Department of Mechanical Engineering, University of California at Berkeley, Berkeley,94720, CA, USA {yongxiang fan, hclin, yzhao334, chung yen, tetang, tomizuka}@berkeley.edu 2 Wenjie Chen is with FANUC Corporation, Oshino-mura, Yamanashi Prefecture, 401-0597, Japan and parameterize arbitrary attitude. Secondly, a Cartesian space controller needs to be properly designed to achieve globally asymptotically stable tracking. Thirdly, the closed system should have a desired speed of convergence to track a moving target. Lastly, the system needs to resist a certain amount of model uncertainties. Some methods are proposed in order to deal with sensing dynamics and estimate the pose of the target. Considering the irregular sampling time and noise, [2] applies a Kalman filter to a kinematic model to estimate target position. To increase the estimation accuracy, [3] identifies the model parameters for Kalman filter by expectation-maximization in an off-line manner. To overcome the slow sampling rate and the latency effects, [4] utilizes sensor fusion and a kinematic Kalman filter to estimate position. These methods only partially deal with sensing issues, instead all of them mentioned above. With regard to tracking controller design, [5] applies a local parameterization of orientation and use a PID controller to track the position and orientation of the object. However, the object will lose tracking if the orientation error is larger than a threshold. To realize global parameterization and asymptotically stable tracking, [6] provides a quaternion based tracking controller from kinematic level. [7] studies the stabilities of a kinematic and a dynamic controller without considering parameter uncertainties. This paper proposes a visual tracking control framework in presence of sensing dynamics. In Section II, an irregular time Kalman filter and the maximum likelihood techniques are combined to estimate the pose of the target. In Section III, a tracking controller is designed considering both robot kinematics and robot dynamics. Also, the stability analysis under parameter uncertainties is conducted. Experimental results on an industrial robot are presented in Section IV. Section V concludes the work. II. TARGET POSE ESTIMATION Given the positions of feature points measured by the vision sensor, we can calculate the pose of the target by least squares, as is shown in [8]. However, it assumes the data sets are complete and reliable. In reality, the sensor might have aforementioned sensing dynamics properties, and the tracking performance will be downgraded if these properties are not well considered. A reasonable solution to reduce the effect of the sensing dynamics is to adjust the weights of different feature points based on their relative measurement uncertainties. Therefore, in this section, a Kalman filter and a maximum likelihood are combined to estimate the target