Article

A TSR Visual Servoing System Based on a Novel Dynamic Template Matching Method † Jia Cai 1,2 , Panfeng Huang 1,2, *, Bin Zhang 1,2 and Dongke Wang 1,2 Received: 28 October 2015; Accepted: 10 December 2015; Published: 21 December 2015 Academic Editors: Lianqing Liu, Ning Xi, Wen Jung Li, Xin Zhao and Yajing Shen 1

2

* †

National Key Laboratory of Aerospace Flight Dynamics, Northwestern Polytechnical University, 710072 Xi’an, China; [email protected] (J.C.); zhangbindt@mail. nwpu.edu.cn (B.Z.); [email protected] (D.W.) Research Center for Intelligent Robotics, School of Astronautics, Northwestern Polytechnical University, 710072 Xi’an, China Correspondence: [email protected]; Tel.: +86-29-88460366 (ext. 801) This paper is an extended version of the paper entitled “Novel Dynamic Template Matching of Visual Servoing for Tethered Space Robot” presented at ICIST, 2014, Shenzhen, China, 26–28 April 2014.

Abstract: The so-called Tethered Space Robot (TSR) is a novel active space debris removal system. To solve its problem of non-cooperative target recognition during short-distance rendezvous events, this paper presents a framework for a real-time visual servoing system using non-calibrated monocular-CMOS (Complementary Metal Oxide Semiconductor). When a small template is used for matching with a large scene, it always leads to mismatches, so a novel template matching algorithm to solve the problem is presented. Firstly, the novel matching algorithm uses a hollow annulus structure according to a FAST (Features from Accelerated Segment) algorithm and makes the method be rotation-invariant. Furthermore, the accumulative deviation can be decreased by the hollow structure. The matching function is composed of grey and gradient differences between template and object image, which help it reduce the effects of illumination and noises. Then, a dynamic template update strategy is designed to avoid tracking failures brought about by wrong matching or occlusion. Finally, the system synthesizes the least square integrated predictor, realizing tracking online in complex circumstances. The results of ground experiments show that the proposed algorithm can decrease the need for sophisticated computation and improves matching accuracy. Keywords: Tethered Space Robot; object recognition; template matching; visual servoing; target tracking

1. Introducation 1.1. Motivation Nowadays, on-orbit service technologies are drawing more and more attention, since they have many potential values for novel applications, such as space debris cleaning, on-orbit assembly, and maintenance, as shown in [1–4]. In the last few decades, almost all robotics used in space have the same mechanical elements. For on-orbit service tasks, they are usually modelled as a manipulator mounted on a rigid body structure. Devices such as the Orbital Express, the Shuttle Remote Manipulator System (Canadarm), and the Japanese ETS-VII are examples. However, limited by the manipulator length their flexibility seems to not be enough for some novel emerging space tasks [5,6]. Aiming to overcome these rigid manipulator arms’ shortcomings, our proposed Tethered Space Robot (TSR) system is designed to be different from traditional robotics for space manipulation, as shown in [7]. As a novel type of space robot, it has a creative architecture, whereby the TSR comprises Sensors 2015, 15, 32152–32167; doi:10.3390/s151229884

www.mdpi.com/journal/sensors

Sensors 2015, 15, page–page Sensors 2015, 15, 32152–32167

shown in [7]. As a novel type of space robot, it has a creative architecture, whereby the TSR comprises an Operational a 100 Space meter-long and a Robot Platform, as shown in an Operational Robot, a 100Robot, meter-long TetherSpace and a Tether Robot Platform, as shown in Figure 1. TSR Figure 1. TSR has a large operational radiusfrom that benefits from the 100 m tether, which also enhances has a large operational radius that benefits the 100 m tether, which also enhances its flexibility, its flexibility, making it promising an especially promising for non-cooperative making it an especially technology fortechnology non-cooperative object capture.object capture.

Figure 1. Description of the tethered space robot system.

In autonomous robot tracking and capture of aa non-cooperative non-cooperative target, computer vision is exclusively used as the primary feedback sensor in these tasks to control the pose of the end-effector with respect respect totothe thetarget, target, described in [8–10]. concentrates on calculating the as as described in [8–10]. This This paperpaper concentrates on calculating the target’s target’s on tracking the region of interest (ROI), whichisisselected selected for for capture azimuthazimuth angles angles based based on tracking the region of interest (ROI), which manipulation. areare common parts mounted on non-cooperative targets, such manipulation. According Accordingtoto[11,12], [11,12],there there common parts mounted on non-cooperative targets, as theas apogee rocket motor the docking ring, the separator bolts, the span solar panels and such the apogee rocket injector, motor injector, the docking ring, the separator bolts,ofthe span of solar so on. Since they are in common use, we also select them as the target for the Operational Robot’s panels and so on. Since they are in common use, we also select them as the target for the capture manipulation and wemanipulation choose the span ofwe solar panels as span our research in this Operational Robot’s capture and choose the of solar interest panels as our paper. research interest in this paper. 1.2. Challenge Descriptions

1.2. Challenge Descriptions Many hypothetic methods from the literature can be applied to optimize the association process. Target observations can methods be acquired using methods, such as template-based Many hypothetic from therecognition literature and can tracking be applied to optimize the association or feature-based ones. Unfortunately, the span of solar panels is mostly made by with a such smooth process. Target observations can be acquired using recognition and trackingmetal methods, as surface in our scenario. Sometimes they are even covered with a burnished membrane for thermal template-based or feature-based ones. Unfortunately, the span of solar panels is mostly made by control. As described great detail in scenario. [13], it is difficult to extract andeven trackcovered feature points such a metal with a smoothinsurface in our Sometimes they are with afrom burnished smooth region, and even using FAST, ORB (Oriented FAST and Rotated BRIEF) can only detect few membrane for thermal control. As described in great detail in [13], it is difficult to extract and track corners. Furthermore, theaextracted featuresand areeven confused disordered, and cannot selected as feature points from such smooth region, usingand FAST, ORB (Oriented FASTbeand Rotated ROI for capture manipulation. In addition, tracking algorithms such as Meanshift, CAMSHIFT, based BRIEF) can only detect few corners. Furthermore, the extracted features are confused and on color histograms will not well since therefor are capture insufficient colors and the panels tracking are very disordered, and cannot bework selected as ROI manipulation. Insolar addition, similar to the satellite’s main body. algorithms such as Meanshift, CAMSHIFT, based on color histograms will not work well since In this paper, we focus on template template matching approach is there are insufficient colors and the solar matching panels aretechniques. very similarThe to the satellite’s main body. derived from the idea of searching for assigned image sections or given features within an interrelated In this paper, we focus on template matching techniques. The template matching approach is image [14]. The template can occupy only a limited image area, or it can have the same size as derived from the idea of searching for assigned image sections or given features within an the search image. function is accomplished by exploiting a correlation relationship. interrelated image The [14].matching The template can occupy only a limited image area, or it can have the same Researchers proposed of correlation among whichby Sum of Hamming Distances size as the have search image.many The kinds matching function laws, is accomplished exploiting a correlation (SHD), Normalized Cross have Correlation (NCC), Sum of Absolute Difference of Squared relationship. Researchers proposed many kinds of correlation laws,(SAD), amongSum which Sum of Difference (SSD), and distance transformation ones are the most commonly used. Hamming Distances (SHD), Normalized Cross Correlation (NCC), Sum of Absolute Difference In view of Squared the aboveDifference analysis, a(SSD), template algorithm is worth deal with (SAD), Sum of and matching distance transformation onesconsidering are the mosttocommonly this troublesome problem. In general, standard template matching has four advantages: simplicity used. and accuracy, high applicability and good anti-noise performance.

2 32153

Sensors 2015, 15, 32152–32167

When computing azimuth angles for a far-distance approach, one crucial task is how to recognize the target’s ROI and track it accurately and stably. Once given a small image of the non-cooperative object, as small as 35 pixels ˆ 35 pixels, the problem becomes more difficult. A detailed description of this problem is that the algorithm is requested to recognize a preset ROI online in dynamic scenes precisely and robustly. Between consequent frames, there are several challenges, such as motion blur, noise level, target’s whirl movement and so on. For our target, which has a monotonous texture and simple structure, this becomes even more troublesome. 1.3. Related Works Given that ROI extraction is outside the scope of this paper, we assume that the initial template object to be recognized and tracked can be extracted by some popular recognition algorithm or selected manually. Recently, video cameras and related methods for image processing have become a standard perception system and technique for robotics [15]. Based on visual perception and feedback, the robot control is commonly known as visual servoing. Visual servoing deals with the control problem and robot motion using visual data [16]. It is a complex problem since there many techniques are involved such as kinematics and dynamics, the control theory, image processing, and real-time computational operations [17]. According to [18], visual servoing systems can be divided into three categories: (a) Position-based visual servoing (PBVS). Such a system is required to retrieve three-dimensional (3D) information about the scene. Researchers usually use a known camera model to measure relative six-dimensional (6D) information of the target, including its 3D position by coordinate transformation and 3D velocity by filters; (b) Image-based visual servoing (IBVS). Such a system is required to calculate the displacement of the robot in axis x and y based on a two-dimensional (2D) image measurement technique; (c) “2½D” visual servoing. Such a system is a method that combines the above two categories. PBVS can acquire accurate positioning information, but at high computational cost, while IBVS is faster, but may lead to undesired trajectories. In this paper, 3D information about the scene are not required, but 2D measurements in the image are necessary. A monocular camera is used to acquire the target’s ROI for azimuth computation. The camera does not need to be calibrated, so the IBVS scheme based on template matching is worth considering for solving the problems before our eyes, similar to the work of Herzog et al. [19]. In the past the template matching technique has been widely used for object recognition and tracking problems. For example, aiming to solve special technical problems in different application scenarios, Li et al. [20], and Tiirkan et al. [21] presented different template matching improvements. However, in our view, it should be able to deal with drastic illumination changes and shape deformations. Moreover, the size of template image must be selected as 30%–50% of the scene image. Although many researchers have addressed it for many years, the task is far from trivial. You et al. [22] improved the rotation invariance of the template matching algorithm by using a mean absolute difference method. Since assigned regions in reference and query images are arranged into a circular structure, the accumulated error may be brought out when confronting complex environments. Furthermore, the authors did not use a prediction filter to decrease the computational complexity and improve the accuracy. Lu et al. [23] studied dynamic template matching. Their method was designed for recognition and tracking air moving targets. In that paper, the authors used standard template matching, which is not a rotation-invariant method, and their experimental results showed some mismatching. Paravati et al. [24] improved the performance of the template matching-based tracking method. They used a relevance-based technique to reduce the domain space complexity. Furthermore, they studied a tracking filter and they used a dynamic threshold to reduce the number of points to be processed. Then they carried out some tests on image sequences from different public datasets. Their results showed that the proposed scheme could reduce the time consumption of the reference method while it can ensure the robustness. Roberto et al. [25] presented a 3D template matching method used

32154

threshold to reduce the number of points to be processed. Then they carried out some tests on image sequences from different public datasets. Their results showed that the proposed scheme could reduce the time consumption of the reference method while it can ensure the robustness. Roberto et al. [25] presented a 3D template matching method used for autonomous pose measurement. They restrained the pose search space to a three degree of freedom (3-DOF) database Sensors 2015, 15, 32152–32167 concerning only the attitude parameters. The templates are built on-line before correlating them with the acquired dataset. This method can reduce the time consumption and cut down on data storage, but the pose computational cost They is stillrestrained a big problem while ensuring same for autonomous measurement. the pose search space constant to a threeresults degreeat ofthe freedom time. (3-DOF) database concerning only the attitude parameters. The templates are built on-line before In order to circumvent the above limitations of existing wetime propose a novel dynamic correlating them with the acquired dataset. This method canmethods, reduce the consumption and cut template matching strategy. we define similarity Normalized SAD.results Then down on data storage, but theFirstly, computational costa is still a bigfunction problemnamed while ensuring constant wethe improve the matching criterion by constructing a series of hollow annulus structures from at same time. square regions and counting and gradient values. Thirdly, least square In order to circumvent theboth abovegray limitations of existing methods, we apropose a novelintegrated dynamic predictor matching is adopted in eachFirstly, frame.we Simultaneously, we function design a named fast matching process and an template strategy. define a similarity Normalized SAD. Then updating ofthe dynamic template to ensure robustness. use it for TSR’s we improve matching criterionstrategy by constructing a series of hollowFinally, annuluswe structures froma square monocular servoing scheme perform the necessary tests integrated to verify the method’s regions andvisual counting both gray and and gradient values. Thirdly, a ground least square predictor is performance andframe. efficiency. adopted in each Simultaneously, we design a fast matching process and an updating of dynamic A preliminary versionrobustness. of this work waswe reported [26]. The currentvisual workservoing is an extended template strategy to ensure Finally, use it forina TSR’s monocular scheme version. The the remainder of this paper is organized asmethod’s follows: Section 2 describes the novel dynamic and perform necessary ground tests to verify the performance and efficiency. template matching version algorithm in detail. Section 3 designs a The visual servoing In Section 4, A preliminary of this work was reported in [26]. current workcontroller. is an extended version. we describe experiments performed on ground and analyze results. Concluding drawn The remainder of this paper is organized as follows: Sectionthe 2 describes the novel remarks dynamicare template in Section 5. matching algorithm in detail. Section 3 designs a visual servoing controller. In Section 4, we describe experiments performed on ground and analyze the results. Concluding remarks are drawn in Section 5. 2. Methodology 2. Methodology 2.1. Improved Template Matching 2.1. Improved Template Matching We have introduced some common correlation-based similarity functions in the section above, We SSD, haveNCC, introduced some common correlation-based functions themore section above, namely SAD and SHD. In contrast with others, similarity SAD is less complexinand accurate. namely SSD, NCC, SAD and SHD. In contrast with others, SAD is less complex and more accurate. However, its performance is limited when reference and query images have almost the same grey However, its To performance is limited when query images have almost the same SAD grey distribution. improve its resolution, wereference define aand similarity function called Normalized distribution. To improve its resolution, we define a similarity function called Normalized SAD (NSAD), (NSAD), shown in Equation (1): shown in Equation (1): ( A( xyq , y)´AAmq) ´  (pBpx, B( x, yq y) ´  BBm )q| ÿ ÿ | pApx, m m (1) d ( ABq , B“)  dpA, (1) maxpApx, yq, Bpx, yqq max( A ( x , y ), B ( x , y )) x xy y



where, A(x, A(x, y), y), B(x, B(x, y) y) are are the the gray grayvalue valueof ofposition position(x, (x,y) y)in inthe thereference referenceand andquery queryimages. images.AAmm,,BBmm are the mean values values of of the the same same region region in in the the reference reference and andquery queryimages. images. After a searching searching process, process, when when the the value value d(A, d(A, B) B) is is minimum, minimum, the the best best matching matching position position is acquired. It can be found that NSAD has translation invariance by analyzing the matching found that NSAD has translation invariance by analyzing the matching criterion function. rotation invariance, and it and may bring about mismatching when confronting function. However, However,it lacks it lacks rotation invariance, it may bring about mismatching when complex circumstances. confronting complex circumstances.

Figure 2. Schematic diagram of distributed pixels assembled into a hollow annulus. Figure 2. Schematic diagram of distributed pixels assembled into a hollow annulus.

4 Considering the FAST [27] algorithm adopts a hollow annulus for detection corners and obtains a good result, we similarly create a series of concentric rings from a square region for reference, as shown in Figure 2. Given a circular structure has the quality of central symmetry, hence we propose a ring-shape criterion that can ensure the matching method is rotation-invariant. It is worth mentioning 32155

Sensors 2015, 15, 32152–32167

that the matching criterion based on a solid circular structure has the same performance, however, it is apt to generate accumulated errors when confronting a varied environment. Furthermore, it may lose some detailed information caused by the sum of difference values. For these reasons, some mismatching results may be obtained, as seen in [28]. Given the above factors, we construct a series of concentric complete and incomplete rings, whose radius is r = 1, 2, . . . , 18. A 35 pixels ˆ 35 pixels square region can be regarded as composed of these concentric rings. Similarly, we can deal with an N pixels ˆ N pixels square region in the same way. According to this idea, we define a matching criterion based on gray value. For eoi , i means the i-th sub concentric ring and it is the sum of the grey difference values on this ring: ˇ ˇ ˇř ˇ Ni ř ˇ Ni ˇ pA j px, yq ´ Aim q ´ pBj px, yq ´ Bim qˇ ˇ ˇ j “1 ˇ j “1 eoi “ (2) maxpA j px, yq, Bj px, yqq Then we define the whole matching function by Equation (3): Mo “

18 ÿ

eoi

(3)

i “1

where A j (x, y) and B j (x, y) are the grey value of the j-th coordinate on the i-th ring of reference and query images. Aim and Bim represent the mean grey value of the i-th ring. Ni is the total number of all the pixels contained on the i-th ring, while Mo is defined as the matching function of 35 pixels ˆ 35 pixels square image based on grey value. From the results obtained with Equation (3), we find that it also loses some information about the image’s details caused by accumulated difference values. Hence, it is necessary to add some more features to describe the region more accurately and robustly. This draws our attention to the fact that the gradient feature is usually used for extracting edges. The explanation is that it represents the grey change between adjacent pixels. Hence, we try to combine gradient information with gray value for a detailed description in the matching function benefitting from the increasing pixel’s uniqueness. For pixel (x, y) of image I, we can calculate its gradient g(x, y) by Equation (4): gpx, yq “ rIpx ` 1, yq ´ Ipx ´ 1, yqs ˚ i ` rIpx, y ` 1q ´ Ipx, y ´ 1qs ˚ j

(4)

where, I(x, y) represents the grey value of pixel (x, y), i, j is its coordinate. Meanwhile, the gradient module mg (x, y) can be calculated by Equation (5): b m g px, yq “ gpx, y, 1q2 ` gpx, y, 2q2 (5) where, g(x, y, 1) and g(x, y, 2) represent the gradient value of pixel (x, y) in direction i and j. Then we define a function using the gradient module with Equation (6): ˇ ˇ ˇ Ni ˇˇ pm px, yq ´ m q ´ pm px, yq ´ m q ÿ jga igam jgb igbm ˇ e gi “ (6) maxpm jga px, yq, pm jgb px, yqq j “1

Mg “

18 ÿ

e gi

(7)

i “1

where, egi is the sum of the gradient difference values of the i-th sub concentric ring, m jga (x, y) and m jgb (x, y) represent the gradient modules of the j-th coordinate in the i-th ring of reference and query images. migam and migbm represent the mean gradient module of the i-th ring. Mg represents the matching function of 35 pixels ˆ 35 pixels square region based on gradient module. Thus, Mo and Mg are combined together and we construct a matching criterion Mh named novel NSAD function:

32156

Sensors 2015, 15, 32152–32167

Mh “ W1 ˆ Mo ` W2 ˆ Mg

(8)

where W 1 and W 2 are the weight coefficients and their values are decided according to the different circumstances. From the above analysis, it can be concluded that our synthesized criterion Mh is rotation-invariant like criteria Mo and Mg . Meanwhile, matching accuracy can be improved since it contains more details. Hence, it can provide support to recognize targets even when confronting an inconstant environment. 2.2. Coarse to Fine Matching Strategy The process of matching using template images can be described as a solid window sliding in a flat surface to find a similar region. For image I, its size is defined as W ˆ H. The size of the square template image is defined as M ˆ M. The searching step increment is usually set as one by one, hence, the sliding count is (W ´ M) ˆ (H ´ M). Assuming W = 640 pixels, H = 480 pixels and M = 50 pixels, we can calculate that the original count is 253,700. To decrease the computational complexity, we design a two-steps (coarse to fine) matching strategy. Firstly, the searching step is set at N by N (N = 5–50). A coarse matching result can thus be acquired, and the matching position (xc , yc ) is recorded. Secondly, we define a square region, whose center is (xc , yc ) and side length is N. Let the template image slide in the square region one step at a time. When the similarity is smallest, the matching position is best. Through the coarse to fine matching strategy, we can reduce calculation counts and save time immediately. For example, given N = 20, the improved counts becomes 1168 (768 + 400), which is only about 4.6% of the original counts. 2.3. Least Square Integrated Predictor When our target to be tracked is a moving one in the scene, it is essential to predict its position in the next frame using the past information since the use of predicted positions can improve occlusion invariance and decrease time consumption. Some algorithms, such as the Kalman filter, particle filter, etc., have been developed to predict variance. In general, for these filter algorithms, the higher the accuracy is, the more inefficient it is. In this paper, the method is expected to perform in an online application, so we adopt a least squares integrated predictor to acquire the position in the next frame. We define (x(t), y(t)) as the central coordinate of the target to be tracked. The relative movement between the target and our Operational Robot is assumed to be a uniform straight-line motion. Then we can represent the best linear estimation of x(t) using the following equation: Xl ptq “ a0 ` a1 t « ” which can be rewritten as Xl “

ı 1

t

a0 a1

(9)

ff ,

Define Xl pti qpi “ 1, 2, ¨ ¨ ¨ , Nq as the measured value in different moments. Then we can acquire the difference between the estimated value and the measured information value by Equation (10): ε i “ pa0 ` a1 ti q ´ Xl pti q

(10)

We then calculate its N moments residual sum of squares: N ÿ i “1

ε2i “

N ÿ

rpa0 ` a1 ti q ´ Xl pti qs2

(11)

i “1

Through the least squares method, we can obtain the best approximation. The solution is acquired only when Equation (12) achieves the minimum value:

32157

Sensors 2015, 15, 32152–32167

» «

a0 a1

ff

fi Xl pti qti ffi ffi i “1 i “1 i “1 i “1 ffi D ffi ffi N N N ř ř ř Xl pti qti ffi Xl pti q ` N ´ ti fl N ř

— — — “— — — –

t2i

i “1

N ř

Xl pti q ´

N ř

ti

N ř

i “1

i“1

D ¸2 ˜ N N ÿ ÿ 2 t D “ N ti ´ i“1

(12)

(13)

i “1

Given the condition of least error, Equation (13) shows a general solution of the N moments optimum linear prediction. Similarly, if the relative movement between the target and our Operational Robot is assumed as a uniform acceleration curvilinear motion, we can give the best square estimation of x(t) using Equation (14): Xq “ a0 ` a1 t ` a2 t2 (14) Equally, the general solution of a0 , a1 , a2 can also be calculated by the least squares method. To consider this more deeply, we can find that the relative motion can be always divided into linear motion and curvilinear motion together in an actual scenario. On the one hand, a linear approximation can reflect the target’s state quickly. On the other hand, a square approximation can smooth the target’s trajectory. Hence, we consider constructing a least squares integrated predictor using them together. The function X(t) is shown in Equation (15): Xptq “ W4 ˆ Xl pt ` 1{tq ` p1 ´ W4 q ˆ Xq pt ` 1{tq

(15)

Selection of weight coefficient W 4 for the linear and square predictor can be decided by the prediction errors. Our proposed integrated predictor possesses the property of automatic corrective measures. Furthermore, it can fit non-uniform growing curves. Through many tests, we find that W 4 = 0.8 makes the results fine in our scenario. Tests also indicate that its prediction accuracy declines when N grows under a threshold. When we choose Xpk ` 1q “ p4xpkq ` xpk ´ 1q ´ 2xpk ´ 2qq{3 (a three moments linear predictor) combined with Xpk ` 1q “ p9xpkq ´ 4xpk ´ 2q ´ 3xpk ´ 3q ` 3xpk ´ 4qq{5 (a five moments square predictor), the best result is acquired. Once the estimation coordinate is acquired, a rectangular candidate window Rc is defined for searching the template image. The width and the height are both twice those of the template image. Hence, it does not need to search the template in the whole image, which decreases the computation complexity and the time consumption. 2.4. Dynamic Template Updating Strategy 2.4.1. Updating Strategy When approaching from 100 m to 20 m, due to the pose variation in a complex and sometimes changing environment, a non-cooperative satellite’s shape and size change quickly. This phenomenon gradually makes template images very different from the target images. After several frames, the difference will be accumulated to a threshold value, and the template image used from the start may become invalid. To overcome the accumulated deviation and ensure stable matching, the template image has to be updated dynamically in time. The matched position is defined as the geometrical center of a rectangular matching region in our method. First of all, we define two updating criteria according to the deviation of distance and similarity: (a) The coordinate distance between a target’s predicted position (x’, y’) and practically matched b position (x, y) is defined as DIS “

px ´ x1 q2 +py ´ y1 q2 .

(b) The similarity function between reference and query images is defined as S = 1/Mh .

32158

Sensors 2015, 15, page–page

(b) The similarity function between reference and query images is defined as S = 1/Mh. Sensors 2015, 15, 32152–32167

Based on the above two definitions, we then design an adaptive dynamic template updating strategy. The following criteria are judged in each frame: Based onDIS the >above two definitions, design an dynamic template (a) Once Dt, it means there maywe be then an occlusion or adaptive sudden change. The positionupdating tracking strategy. The following criteria are judged in each frame: does not work well any more. In order to keep tracking stably, our strategy is to use the template (a)continually. Once DIS >Meanwhile, Dt , it meansthe there may beposition an occlusion change. The position tracking image predicted (x’, y’)orissudden temporarily adopted as the matched does not work well any more. In order to keep tracking stably, our strategy is to use the template coordinate (x, y) until the tracking sequence becomes fine. image predicted position (x’, y’)isisclose temporarily adopted as theHowever, matched (b)continually. Once DIS Td, the searched sub-image is viewed as a mismatched one. Then the search range tracking frames. The difference d M can be used for a passive updating strategy. Once the difference changes from the rectangular candidate window Rc to the whole image. Once dM < Td, the least d M > Td , the searched sub-image is viewed as a mismatched one. Then the search range changes from squares integrated predictor works again and then the method narrows down the search range to the rectangular candidate window Rc to the whole image. Once d M < Td , the least squares integrated the rectangular candidate window Rc again. predictor works again and then the method narrows down the search range to the rectangular candidate window Rc again. 2.5. Theory of Calculating Azimuth Angles 2.5. Theory of to Calculating Azimuth In order analyze the resultsAngles more easily, we draw the pinhole camera model shown in Figure 3, where is tothe Retinal (RCS), is the Pixel Coordinate System (PCS), In oxy order analyze theCoordinate results moreSystem easily, we drawo’uv the pinhole camera model shown in Figure 3, OcXcYcZc is the Camera Coordinate System (CCS), and OwXwYwZw is the World Coordinate where oxy is the Retinal Coordinate System (RCS), o’uv is the Pixel Coordinate System (PCS), OcXcYcZc System (WCS).Coordinate System (CCS), and OwXwYwZw is the World Coordinate System (WCS). is the Camera

Figure 3. Schematic Schematic diagram of image sensor’s sensor’s imaging imaging model. model.

Here o is the origin of the RCS and o’ is the origin of the PCS. o is referred to be the principal Here o is the origin of the RCS and o’ is the origin of the PCS. o is referred to be the principal point, point, which is at the intersection of the image plane and the optical axis. which is at the intersection of the image plane and the optical axis. 8

32159

Sensors 2015, 15, 32152–32167

Usually, the defined principle point is not equivalent to the center of the imager because of machining errors. Moreover, the center of the chip is usually not on the optical axis as a result of installation errors. Hence two new parameters, u0 and v0 , are introduced to model a possible displacement (away from the optical axis) of the center of coordinates on the projection screen. We define the physical size of each pixel in axis x and y as dx , dy . Then we use a relatively simple model to represent the transformation between coordinate systems. When a coordinate point Q(x, y) in RCS is projected onto the screen at some pixel location given by (u, v), we can describe the transformation using the following equation: $ x ’ ’ & u “ d x ` u0 (17) y ’ ’ v “ ` v % 0 dy Given a point P(xw , yw , zw ) of the non-cooperative satellite in WCS, when projected onto the screen, we represent its 2D location by P(x, y) in RCS. Define line of sight Oc P, and Oc px is projection vector in Oc Xc Zc plane of the line of sight Oc P. Define α as the position angle, which is the angle between Oc px and Oc Zc . Oc py is projection vector in Oc Yc Zc plane of the line of sight Oc P. Define β is the angle of site, which is the angle between Oc py and Oc P: $ pu ´ u0 qd x ’ ’ ’ & α “ arctan f (18) ’ pv ´ v qd y 0 ’ ’ β “ arctan % f Given the relation that maps the point P(xw , yw , zw ) in WCS on the projection screen represented as coordinates P(u, v), we can calculate α,β by Equation (18) according to the research described in [29], where dx , dy can be cato the physical size of the CCD and picture resolution. They can also be acquired from the calibration results.lculated according 3. Visual Servoing Controller Define α as the non-cooperative satellite’s angle of position in the Operational Robot’s field of view (FOV), β is the angle of the site. We design a visual servoing controller using a PD controller with a preset dead zone. Some similar works can be seen in [30,31]. Then we design the control law as follows: $ & Fyr “ ´pK pα ` Kdα sqα While |α| ě α0 (19) % Fz “ ´pK ` K sqβ While |β| ě β 0 pβ dβ r where, Fyr ,Fzr are the control forces in direction y and z. K pα , K pβ are the proportion coefficients. Kdα , Kdβ are the damping coefficients α0 , β 0 are the dead zone of our designed controller. As described in Figure 4, α, β actually reflect the position deviation in the directions yr and zr . As a result, we can control the position deviations by controlling the azimuth angles under a certain threshold range. The axes of the target’s orbit coordinate Ot Xt Yt Zt are defined as follows: x-axis is in the orbital plane directed along the local horizontal, y-axis directs along the orbital normal and z-axis directs from the center of the Earth to the centroid of the space platform and completes a right-handed triad with the x-axis and y-axis. During the whole approach process, when the deviations of α, β are restricted under a certain threshold, the position deviations will gradually decrease since the distance to the Operational Robot shortens. Three factors should be considered when selecting a proper α0 , β0 First, they should satisfy the requirement of position control. Second, the need for frequent control should be avoided in order to save mass carrier. Furthermore, the monocular camera’s FOV is worth being considering.

32160

Sensors Sensors2015, 2015,15, 15,32152–32167 page–page Sensors 2015, 15, page–page

Figure 4. 4. Schematic Schematic diagram diagram of of the the TSR’s TSR’s approach approach using using azimuth azimuthangles. angles. using azimuth angles. Figure

4. Experimental Experimental Validation 4. 4. ExperimentalValidation Validation 4.1. Experimental Experimental Set-up Set-up 4.1. We set set up up aaaservoing servoingcontrol controlsystem systembased basedon onmonocular monocular vision visionto totest testour ouralgorithm. algorithm. It It is is We We set up servoing control system based on monocular vision to test our algorithm. composed of a control subsystem, visual perception subsystem, intelligent motion platform and composed of a control subsystem, subsystem, visual visual perception perceptionsubsystem, subsystem,intelligent intelligentmotion motionplatform platformand andaa target simulator. The visual perception subsystem uses a HaiLong M200A camera (Camerasw, atarget targetsimulator. simulator.The Thevisual visualperception perceptionsubsystem subsystemuses usesaa HaiLong HaiLong M200A M200A camera camera (Camerasw, Changsha, China) China) mounted mounted on on aa 1/3.2 1/3.2 inch inch (4:3) CMOS sensor (Micron Technology, Boise, ID, ID, USA) USA) Changsha, inch(4:3) (4:3)CMOS CMOSsensor sensor(Micron (MicronTechnology, Technology, Boise, China) 1/3.2 ˝ and a Computar M1614 fixed focus lense with 23.4° FOV (Computar, Tokyo, Japan), 16 mm focal and a Computar Computar M1614 fixed focus lense with 23.4 23.4° FOV (Computar, Tokyo, Japan), Japan), 16 16 mm mm focal focal length. The The image image resolution isisset set asas1280 1280 pixel 960960 pixel. A HP HP workstation (the(the CPUCPU is aa Xeon Xeon 5130 length. imageresolution resolutionis setas 1280pixel pixel ˆ pixel. A HP workstation is a Xeon ×× 960 pixel. A workstation (the CPU is 5130 2.0 GHz GHz whilewhile its RAM RAM is 2G) 2G) (Hewlett-Packard Development Company, Palo Alto, CA, USA) is 5130 2.0 GHz its RAM is 2G) (Hewlett-Packard Development Company, Palo Alto, CA, USA) 2.0 while its is (Hewlett-Packard Development Company, Palo Alto, CA, USA) is used to obtain the experimental results. We then implemented our algorithm in the Windows XP is used obtainthe theexperimental experimentalresults. results.We Wethen thenimplemented implementedour our algorithm algorithm in in the the Windows XP used totoobtain XP system using using VS2010 VS2010IDE IDEinstalled installedOpenCV OpenCV2.4.3 2.4.3libraries. libraries. system system using VS2010 IDE installed OpenCV 2.4.3 libraries. 4.2. 4.2. Design Design of of Experiments Experiments 4.2. Design of Experiments In In our our laboratory, laboratory, aaa series series of of semi-physical semi-physical experiments experiments were were designed designed to to test test our our method’s method’s In our laboratory, series of semi-physical experiments were designed to test our method’s performance. The experimental system used in our lab is shown in Figure 5. performance. The experimental system used in our lab is shown in Figure 5. performance. The experimental system used in our lab is shown in Figure 5.

Figure 5. 5. Six Six DOF DOF motion motion platform platform and and target target model. model. target model. Figure

32161 10 10

Sensors 2015, 15, 32152–32167 Sensors 2015, 15, page–page

To simulate a non-cooperative target, we use a Chang’e-II satellite model, whose main body is a target, we use a Chang'e-Ⅱsatellite model, whose body is 70 ˆTo 70 simulate ˆ 60 mma3 non-cooperative cube. The span of its solar panel is about 300 mm long. A six DOFmain motion platform 3 cube. The span of its solar panel is about 300 mm long. A six DOF motion ais 70 × 70 × 60 mm used to simulate their relative movement, which consists of a two DOF slipway, two DOF (pitch platform used to simulate theirtwo relative consists ofThe a two two and yaw)isrevolving table, and DOF movement, lifting and which roll platforms. six DOF axes slipway, can be controlled DOF (pitchor and yaw) revolving table, and two DOF lifting andprogram, roll platforms. Theacceleration, six axes can velocity, be separately together by the PEWIN32RO software or a C++ including controlled separately or together by the PEWIN32RO software or a C++ program, including and amplitude parameters. The system performance is summarized in Table 1. acceleration, velocity, and amplitude parameters. The system performance is summarized in Table 1. Table 1. Performance of the 6 DOF moving platform. Table 1. Performance of the 6 DOF moving platform. 2

Route (mm) Velocity(m/s) (m/s) Acceleration(m/s (m/s2) ) AxisAxis Route (mm) Velocity Acceleration X axis 0–1000 ±0.01–1 ±0.01–1 X axis 0–1000 ˘0.01–1 ˘0.01–1 Y axis 0–1500 ˘0.01–1 ˘0.01–1 Y axis 0–1500 ±0.01–1 ±0.01–1 Z axis 0–1200 ˘0.01–1 ˘0.01–1 Z axis 0–1200 ±0.01–1 ±0.01–1 2) Axis Amplitude (˝ ) Angle Velocity (˝ /s) Angle Acceleration (˝ /s Axis Amplitude (°) Angle Velocity (°/s) Angle Acceleration (°/s2) pitch axis ˘45 ˘0.01~10 ˘0.01~10 pitch yaw axis axis ±45˘90 ±0.01~10 ±0.01~10 ˘0.01~10 ˘0.01~10 yaw axis ±90 ±0.01~10 ±0.01~10 roll axis ˘90 ˘0.01~10 ˘0.01~10 roll axis ±90 ±0.01~10 ±0.01~10

Thelights lightssystem, system,background background simulation other support system canseparately be separately The simulation andand anyany other support system can be or or jointly controlled, too. We then use the system for simulating several curved approach paths jointly controlled, too. We then use the system for simulating several curved approach paths to testto test our method methodinindetail. detail.InInthe theexperiments, experiments, also design some challenge scenarios. our wewe also design some challenge scenarios. 4.3. Results Resultsand andDiscussion Discussion 4.3. 4.3.1. Qualitative Analysis 4.3.1. Qualitative Analysis Thesize sizeofoftemplate templateimage image selected this paper is pixels 60 pixels 60 pixels, as shown in Figure The selected in in this paper is 60 × 60ˆpixels, as shown in Figure 6a. 6a.

(a)

(b)

(c) Figure 6. Qualitative tests of the proposed template matching algorithm. Figure 6. Qualitative tests of the proposed template matching algorithm.

Figure 6b marks marksthe the matching region under different illumination conditions. this Figure 6b matching region under different illumination conditions. In thisIn experiment, experiment, the lights system is controlled to simulate different illumination conditions, ranging the lights system is controlled to simulate different illumination conditions, ranging from bright to from bright to dark. Generally, the illumination characterizes nonlinear changes. In Figure 6b, we dark. Generally, the illumination characterizes nonlinear changes. In Figure 6b, we can see our method can see our method is robust to brightness changes. is robust to brightness changes. 11 32162

Sensors 2015, 15, 32152–32167

Sensors 2015, page–page Figure 6c 15, shows the matching results with different rotation angles. Satellites mounted on the two DOF lifting and roll platforms are controlled to rotate with different arbitrary angles. For these Sensors 2015, 15, page–page Figure 6c shows the matching results with different rotation angles. Satellites mounted on the conditions, the template image is used as shown in Figure 6a. From the matching results drawn in the two DOF lifting rollthe platforms are controlled to rotate with different arbitrary angles. For these Figure 6c and shows matching with different rotation angles. Satellites mounted on the red rectangle, we can conclude that ourresults method is robust under a certain illumination and rotation. conditions, the template image is used as shown in Figure 6a. From the matching results drawn in

two DOF lifting and roll platforms are controlled to rotate with different arbitrary angles. For these the red rectangle, we can conclude that ouras method robust under a certain and rotation. conditions, the template image is used shownisin Figure 6a. From the illumination matching results drawn in 4.3.2. Quantitative Comparisons the red rectangle, we can conclude that our method is robust under a certain illumination and rotation. 4.3.2. Quantitative Comparisons To test the proposed method deeply and practically, we merged the method into the TSR’s

4.3.2. Quantitative Comparisons monocular servoing system anddeeply validated with semi-physical Thenthe weTSR’s recorded Tovisual test the proposed method and it practically, we mergedexperiments. the method into monocular visual servoing system and validated it with semi-physical experiments. Then we all the matched regions that aremethod used for obtaining azimuth angles. To test the proposed deeply and practically, we merged the method into the TSR’s recorded all the matched regions that are used for obtaining azimuth angles. Figure 7 shows the matching results and by our methoditinwith the first 717 frames. In this experiment, monocular visual servoing system validated semi-physical experiments. Then we the Figure all 7platform shows theismatching byused our method in the first 717angles. frames. In this experiment, six DOF motion controlled to are pick up for the camera towards the satellite model. During the recorded the matched regionsresults that obtaining azimuth the six DOF motion platform is controlled to pick up the camera towards the satellite model. During Figure 7there shows thechanges matching byof our in the first 717roll frames. thislights experiment, approach process, are inresults the axis x, method y, z, pitch, yaw and whileInthe system is the approach process, there areischanges in to thepick axisup ofthe x, y, z, pitch, yaw and roll while the lights the six motion platform controlled the the satellite model. Duringwas controlled to DOF simulate different illumination conditions. Itcamera can betowards seen that template image system is controlled to simulate conditions. It can be and seenroll thatwhile the template therelatively approach process, there aredifferent changesillumination in the axis of x, y, z, pitch, yaw the lights matched well. image was matched relatively well. system is controlled to simulate different illumination conditions. It can be seen that the template image was matched relatively well.

Frame 1

Frame 80

Frame 1

Frame 400

Frame 80

Frame 480

Frame 160 Frame 160

Frame 560

Frame 240 Frame 240

Frame 640

Frame 400 Figure 7.Frame 480 560during the first Frame Tracking results of our Frame algorithm 717 640 frames. Figure 7. Tracking results of our algorithm during the first 717 frames.

Frame 320 Frame 320

Frame 717 Frame 717

Figure 7. Tracking results of our algorithm during the first 717 frames.

Figure 8 shows the measurements of a matched target’s centroid (x, y) in the first 717 frames.

Figure 8 shows the measurements of a matched target’s centroid (x, y) in the first 717 frames. DuringFigure the approach our method the template as shown Figure 6a. frames. From 8 shows process, the measurements of uses a matched target’simage centroid y) ininthe 717 During the 8, approach process, our method uses the target template image as(x,shown infirst Figure 6a. From Figure it can found that although the original rotates arbitrarily angles and endures During the approach process, our method uses the template image as shown in Figure 6a. From Figure 8, it can found that although the original target rotates arbitrarily angles and endures different different the curve of the tracking or detection is smooth. Figure illumination, 8, it can found that although the original targetresults rotates arbitrarily angles and endures illumination, the curve of the tracking or detection results is smooth. different illumination, the curve of the tracking or detection results is smooth.

(a)

(b)

(a) of coordinate x and y in the first 717 frames. (a). (b) Measurement of x Figure 8. Measurements coordinate; (b) Measurement of y coordinate. Figure 8. Measurements of coordinate x and y in the first 717 frames. (a). Measurement of x Figure 8. Measurements of coordinate x and y in the first 717 frames. (a). Measurement of x coordinate; coordinate; (b) Measurement of y coordinate.

As the CMOS inch (4:3) and the image resolution used is 1280 pixel × 960 pixel, it can be (b) Measurement ofisy 1/3.2 coordinate. calculated the the physical the longer is 6.35 mm, and theisshorter one ×is960 4.76pixel, mm.itThus, As the CMOS is 1/3.2size inchof(4:3) and theside image resolution used 1280 pixel can be theisthe physical of and the longer side isresolution 6.35 mm, and theisshorter one isˆ4.76 Thus, Ascalculated the CMOS 1/3.2 inchsize (4:3) the image used 1280 pixel 960mm. pixel, it can be calculated the the physical size of the longer12side is 6.35 mm, and the shorter one is 4.76 mm. 12

32163

Sensors 2015, 15, 32152–32167 Sensors 2015, 15, page–page Sensors 2015, 15, page–page

Thus, we can acquire dx = 6.35/1280, dy = 4.76/960. According to the description in Section 2.4, we can acquire dx = 6.35/1280, dy = 4.76/960. According to the description in Section 2.4, the the corresponding azimuth siteangles anglescan canbebe calculated as shown in Figure 9. Sensors 2015, 15, page–page corresponding azimuth and and site calculated as shown in Figure 9.

site AngleAngle of siteof(° ) (°) Angle of site (°)

of azimuth AngleAngle of azimuth (°) (°) Angle of azimuth (°)

we can acquire dx = 6.35/1280, dy = 4.76/960. According to the description in Section 2.4, the corresponding azimuth and site angles can be calculated as shown in Figure 9. of angle of site 2.4, the Measurement of angle ofdy azimuth we can acquire dx = 6.35/1280, = 4.76/960. According to Measurement the description in Section 2 0 corresponding azimuthofand siteofangles can be calculated as shown in Figureof9.angle of site Measurement Measurement angle azimuth 2 0 0 Measurement of angle of site Measurement of angle of azimuth 0-2 02 -2 -2 -20 -4 -2-4 -2 -4 -4 -6 -4 -6 -4-6 -8 0 200 400 600 800 0 200 400 600 800 -6 -8 -6 Frame number (frame) Frame number (frame) 0 200 400 600 800 0 200 400 600 800 (a) (frame) (b) (frame) Frame number Frame number -8 -6 0 200 400 600 800 0 200 400 600 800 (a) (b) Figure 9. Measurements of angles of site and azimuth in the first Frame 717 frames. (a) Measurement of the Frame number (frame) number Figure 9. Measurements of angles of site and azimuth in the first 717 frames. (a)(frame) Measurement of the azimuth angle; (b) Measurement of site the site Figure 9. Measurements of and angle. azimuth in the first 717 frames. (a) of angles (b)(a) Measurement of the azimuth angle; (b) Measurement of the site angle. azimuth angle; (b) Measurement of the site angle. Figure 9. Measurements of angles of site and azimuth in the first 717 frames. (a) Measurement of the azimuth angle; (b) Measurement of the site angle.

Figure 10. Comparison of prediction error per frame of the least square synthesis predictor. Figure 10. Comparison of prediction error per frame of the least square synthesis predictor.

Figure 10.analyze Comparison of prediction per framepredictor. of the least square synthesis predictor. We also the performance oferror the synthesis From Figure 10, accurate error data Figure 10. Comparison of prediction error per frame of the least square synthesis predictor. are We drawn in detail. The max absolute error of the x coordinate is 9.5 pixel and the mean absolute also analyze the performance of the synthesis predictor. From Figure 10, accurate error data We also analyze the performance of theof synthesis predictor. From Figure 10, accurate errorisdata error is 0.75 pixel. The max absolute error the y coordinate is 13.5 pixel and mean absolute error are drawn in detail. The max absolute error of the x coordinate is 9.5 pixel and the mean absolute also analyze the performance of the synthesis predictor. From Figure 10, accurate error data are drawn in detail. The max absolute error of the xdistance coordinate ispixel 9.5 pixel and the mean absolute 0.84We pixel. The max error of the Euclidean is 13.5 14.57 pixel andmean mean absolute error error is 0.75 pixel. Theabsolute max absolute error of the y coordinate and absolute error isis are drawn in detail. The max absolute error of the x coordinate is 9.5 pixel and the mean absolute error 0.84 is1.28 0.75 pixel. The max absolute error of the y coordinate is 13.5 pixel and mean absolute error is pixel. It can be concluded that our designed predictor has good accuracy performance. pixel. The max absolute error of the Euclidean distance is 14.57 pixel and mean absolute error is error is 0.75 pixel. The max absolute error of the y coordinate is 13.5 pixel and mean absolute error is 0.84 pixel. The max absolute error of the Euclidean distance is 14.57 pixel and mean absolute error is 1.28 pixel. It can be concluded that our designed predictor has good accuracy performance. 0.84 pixel. The absolutethat errorour of the Euclidean distancehas is 14.57 pixel and mean absolute error is 1.28 pixel. It can bemax concluded designed predictor good accuracy performance. 1.28 pixel. It can be concluded that our designed predictor has good accuracy performance.

Figure 11. Comparison of path generated by our algorithm and real path. Figure 11. Comparison of path generated 13 by our algorithm and real path.

13 Figure Comparisonofofpath pathgenerated generated by and realreal path. Figure 11. 11. Comparison byour ouralgorithm algorithm and path. 13

32164

Sensors 2015, 15, 32152–32167

Sensorson 2015, page–page Based the15,data, we can also draw the real motion path and the predicted one of the Operational 2015, 15, approach page–page process. In Figure 11, the red line marked with a triangle is the real motion RobotSensors during the Based on the data, we can also draw the real motion path and the predicted one of the path while the blue one is the predicted one. We can intuitively find that the predicted one highly Operational during approach process. In Figure 11, path the red linethe marked with one a triangle Based onRobot the data, wethecan also draw the real motion and predicted of theis coincides with the realpath one.while Oncethe theblue predictor givespredicted the possible position, the small template image the real motion one is the Weline canmarked intuitively that the Operational Robot during the approach process. In Figure 11, one. the red with find a triangle is only has to search for a matching scene within a 50 pixels ˆ 50 pixels window centered on this point. predicted one highly real Once the predictor the possiblefind position, the the real motion path coincides while thewith bluethe one is one. the predicted one. We gives can intuitively that the As a result, can sophisticated computation and improve the In contrast, small ittemplate imagecoincides only haswith to search for one. a matching scene within amatching 50 the pixels ×accuracy. 50 pixels window predicted onedecrease highly the real Once the predictor gives possible position, the standard template matching used to arecognize the same target. The time consumed by both centered on this point. As result, it for can decrease improve the small template image onlywas hasaalso to search matchingsophisticated scene within acomputation 50 pixels × 50and pixels window matching accuracy. In contrast, standard template matching was also used to recognize the same centered on frame this point. a result,Figure it can 12a decrease sophisticated computation and improve processes in each wereAs recorded. describes the time consumption in each framethe of our target. The time consumed by both processes each frame were recorded. describes the matching accuracy. In contrast, standard template matching was also frame used Figure to the same proposed method (M1). Figure 12b describes timeinconsumption in each ofrecognize the12a standard template time consumption in each frame of that our M2 proposed method (M1). frame Figure 12b time target. TheFrom time the consumed by both processes in each frame were recorded. Figure 12a describes theonly method (M2). data, we can find failed to work from 204, sodescribes Figure 12b consumption in each frame of the standard template method (M2). From the data, we can find that time consumption in each frame of our proposed method (M1). Figure 12b describes time draws the curve of time consumption for the first 203 frames. We can see that our method worked well M2 failed to in work from frame 204, so Figure 12b only draws(M2). the curve time consumption the consumption each frame of the standard template method Fromofthe data, we can findfor that during the whole process. We can also calculate that the average time consumptions of M1 and M2 are firstfailed 203 frames. can see that method worked well during theofwhole process. We can also M2 to workWe from frame 204, our so Figure 12b only draws the curve time consumption for the 78.42 first ms and 648.16 ms, respectively. calculate that the average time consumptions M1 andwell M2 are 78.42the mswhole and 648.16 ms, We respectively. 203 frames. We can see that our methodof worked during process. can also

calculate that the average time consumptions of M1 and M2 are 78.42 ms and 648.16 ms, respectively.

(a) (b) (a) (b) Figure 12. Comparison of time consumption per frame between the two algorithms. Figure 12. Comparison of time consumption per frame between the two algorithms. Figure 12. Comparison of time consumption per frame between the two algorithms.

To validate our novel dynamic template matching method in a natural scene, we applied the To validate our novel dynamic template matching method in a natural scene, we applied the method to deal with a video of a tracking dataset as shown in Figure 13. In this video, a boy rode To validate our novel dynamic template matching method in a natural scene, we applied thea method to deal with a video of a tracking dataset as shown in Figure 13. In this video, a boy rode a bike from the left side to theofright while there was tree ininthe scene. thevideo, 13th to 41throde frame, method to deal with a video a tracking dataset as ashown Figure 13.From In this a boy a bike from theon left side towas the right while in the thescene. scene.From From 13th to 41th frame, the boy the bike by thethere tree. bike from the left side to blocked the right while therewas was aa tree tree in thethe 13th to 41th frame, the boy on the bike was blocked by the tree. the boy on the bike was blocked by the tree.

Frame 1 Frame 1

Frame 13 Frame 13

Frame 22 Frame 22

Frame 41 Frame 41

Frame 42 Frame 42

Frame 58 Frame 58

Frame 75 Frame 75

Frame 92 Frame 92

Frame 109 Frame 109

Frame 126 Frame 126

Figure 13. Tracking results of object with occlusion in natural scene. Figure 13. Tracking results of object with occlusion in natural scene.

Tracking results object in natural When theFigure target 13. crossed behind the of tree, thewith true occlusion target could not bescene. found, so the matched sub-image in 13th to 41st frame is notthe the tree, real target. In target these frames, the be similarity is much When the target crossed behind the true could not found, function so the matched When the target crossed behind the tree, the true target could not be found, so the matched larger than frames, such as thethe 42nd to 126th. The difference sub-image in those 13th tocomputed 41st frameinisnormal not the tracking real target. In these frames, similarity function is much sub-image 13ththe to searched 41st frame is not the target. these the similarity function is much dM > Tin d, andthose sub-image arereal viewed as a In mismatched Then the126th. searchThe range changes larger than computed in normal tracking frames, suchframes, as one. the 42nd to difference largerdfrom than those computed in normal tracking frames, such as the 42nd to 126th. The difference rectangular candidate window Rc to the image. Then from 42 frame, the occlusion M > Tdthe , and the searched sub-image are viewed as whole a mismatched one. Then thend search range changes d M > from Td , and the searched sub-image are viewed as a mismatched one. Then the search range the rectangular candidate window Rc to the whole image. Then from 42 nd frame, the occlusion 14

changes from the rectangular candidate window R 14c to the whole image. Then from 42 nd frame, the 32165

Sensors 2015, 15, 32152–32167

occlusion disappears and d M < Td . The object is tracked successfully, and least square integrated predictor works again. From the 42 nd frame on, the method narrows down the search range to the rectangular candidate window Rc again. From this frame sequence, it can be found that our method has anti-occlusio ability. 5. Conclusions In this paper, we present a novel dynamic template matching that may be used for a TSR’s monocular visual servoing scheme. To meet the robot’s performance requirements in robustness, adaptability and tracking system effectiveness, our main contributions are fourfold: (1) A prototype framework of a real-time servoing control system using monocular-CMOS is proposed; (2) A similarity function named NSAD is designed; (3) An improved matching criterion by constructing a series of hollow annulus structures from square regions and counting both gray and gradient values is defined; (4) A least squares integrated predictor and an updating strategy are proposed. A large volume of experimental results of virtual, actual and robotic tests show that our proposed matching method can quickly detect assigned small template regions with excellent accuracy under various environmental conditions. In this paper, we present only a preliminary prototype of the robotic visual servoing system. Limited to our experimental conditions, it is hard to verify the method in space. Although we achieved success in our ground experiments, our goal in future works is to combine IBVS and PBVS controllers in a switched system controller and we expect to deal with the problems of keeping the observed ROI in the camera field of view. How to profoundly improve the template matching and make it be scale invariant will be addressed in our future work as well. Acknowledgments: This research is sponsored by the National Natural Science Foundation of China (Grant No. 11272256 and 60805034) and the Doctorate Foundation of Northwestern Polytechnical University (Grant No. CX201304). Author Contributions: All authors are responsible for the concept of the paper, the results presented and the writing. The authors have read and approved the final published manuscript. This work was mainly conducted by Jia Cai under the supervision of Panfeng Huang. Jia Cai has developed the algorithm, conducted the experiments and drafted the paper. Bin Zhang helped perform the experiments. Dongke Wang designed the visual servoing controller. Conflicts of Interest: The authors declare no conflict of interest.

References 1. 2. 3. 4. 5. 6. 7. 8. 9.

Xu, W.; Liang, B.; Li, B.; Xu, Y. A universal on-orbit servicing system used in the geostationary orbit. Adv. Sp. Res. 2011, 48, 95–119. [CrossRef] Coleshill, E.; Oshinowo, L.; Rembala, R.; Bina, B.; Rey, D.; Sindelar, S. Dextre: Improving maintenance operations on the international space station. Acta Astronaut. 2009, 64, 869–874. [CrossRef] Friend, R.B. Orbital express program summary and mission overview. In Sensors and Systems for Space Applications II; SPIE: San Francisco, CA, USA, 2008; pp. 1–11. Nock, K.T.; Aaron, K.M.; McKnight, D. Removing orbital debris with less risk. J. Spacecr. Rocket. 2013, 50, 365–379. [CrossRef] Liu, J.; Cui, N.; Shen, F.; Rong, S. Dynamics of Robotic GEostationary orbit Restorer system during deorbiting. IEEE Aerosp. Electron. Syst. Mag. 2014, 29, 36–42. Flores-Abad, A.; Ma, O.; Pham, K.; Ulrich, S. A review of space robotics technologies for on-orbit servicing. Progress Aerosp. Sci. 2014, 68, 1–26. [CrossRef] Huang, P.; Cai, J.; Meng, Z.J.; Hu, Z.H.; Wang, D.K. Novel Method of Monocular Real-Time Feature Point Tracking for Tethered Space Robots. J. Aerosp. Eng. 2013. [CrossRef] Crétual, A.; Chaumette, F. Application of motion-based visual servoing to target tracking. Int. J. Robot. Res. 2001, 20, 878–890. [CrossRef] Petit, A.; Marchand, E.; Kanani, K. Vision-based space autonomous rendezvous: A case study. In Proceedings of the 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2011), San Francisco, CA, USA, 25–30 September 2011; pp. 619–624.

32166

Sensors 2015, 15, 32152–32167

10. 11. 12. 13. 14. 15.

16. 17. 18. 19. 20. 21.

22.

23. 24. 25. 26.

27. 28. 29. 30. 31.

Larouche, B.P.; Zhu, Z.H. Autonomous robotic capture of non-cooperative target using visual servoing and motion predictive control. Auton. Robot. 2014, 37, 157–167. [CrossRef] Wang, C.; Dong, Z.; Yin, H.; Gao, Y. Research Summarizing of On-orbit Capture Technology for Space Target. J. Acad. Equip. 2013, 24, 63–66. Du, X.; Liang, B.; Xu, W.; Qiu, Y. Pose measurement of large non-cooperative satellite based on collaborative cameras. Acta Astronaut. 2011, 68, 2047–2065. [CrossRef] Yilmaz, A.; Javed, O.; Shah, M. Object tracking: A survey. Acm Comput. Surv. (CSUR) 2006, 38, 1–45. [CrossRef] Gonzalez, R.C.; Woods, R.E. Digital Image Processing, 2nd ed.; Prentice Hall: Upper Saddle River, NJ, USA, 2002. Ivan, I.A.; Ardeleanu, M.; Laurent, G.J. High dynamics and precision optical measurement using a position sensitive detector (PSD) in reflection-mode: Application to 2D object tracking over a smart surface. Sensors 2012, 12, 16771–16784. [CrossRef] [PubMed] Máthé, K.; Bu¸soniu, L. Vision and control for UAVs: A survey of general methods and of inexpensive platforms for infrastructure inspection. Sensors 2015, 15, 14887–14916. [CrossRef] [PubMed] Hutchinson, S.; Hager, G.D.; Corke, P. A tutorial on visual servo control. IEEE Trans. Robot. Autom. 1996, 12, 651–670. [CrossRef] Kragic, D.; Christensen, H.I. Survey on visual servoing for manipulation. Comput. Vis. Act. Percept. Lab. Fiskartorpsv 2002, 15, 6–9. Herzog, A.; Pastor, P.; Kalakrishnan, M.; Righetti, L.; Bohg, J.; Asfour, T.; Schaal, S. Learning of grasp selection based on shape-templates. Auton. Robot. 2014, 36, 51–65. [CrossRef] Li, H.; Duan, H.B.; Zhang, X.Y. A novel image template matching based on particle filtering optimization. Pattern Recognit. Lett. 2010, 31, 1825–1832. [CrossRef] Türkan, M.; Guillemot, C. Image prediction: Template matching vs. Sparse approximation. In Proceedings of the 2010 17th IEEE International Conference on Image Processing (ICIP 2010), Hong Kong, China, 26–29 September 2010; pp. 789–792. You, L.; Xiu, C.B. Template Matching Algorithm Based on Edge Detection. In Proceedings of the 2011 International Symposium on Computer Science and Society (ISCCS 2011), Kota Kinabalu, Malaysia, 16–17 July 2011; pp. 7–9. Lu, X.H.; Shi, Z.K. Detection and tracking control for air moving target based on dynamic template matching. J. Electron. Meas. Instrum. 2010, 24, 935–941. [CrossRef] Paravati, G.; Esposito, S. Relevance-based template matching for tracking targets in FLIR imagery. Sensor 2014, 14, 14106–14130. [CrossRef] [PubMed] Opromolla, R.; Fasano, G.; Rufino, G.; Grassi, M. A Model-Based 3D Template Matching Technique for Pose Acquisition of an Uncooperative Space Object. Sensors 2015, 15, 6360–6382. [CrossRef] [PubMed] Cai, J.; Huang, P.F.; Wang, D.K. Novel Dynamic Template Matching of Visual Servoing for Tethered Space Robot. In Proceedings of the Fourth IEEE International Conference on Information Science and Technology (ICIST2014), Shen Zhen, China, 26–28 April 2014; pp. 388–391. Rosten, E.; Porter, R.; Drummond, T. Faster and better: A machine learning approach to corner detection. IEEE Trans. Pattern Anal. Mach. Intell. 2010, 32, 105–119. [CrossRef] [PubMed] Xu, Y.; Yang, S.Q.; Sun, M. Application of least square filtering for target tracking. Command Control Simul. 2007, 29, 41–42. Zeng, D.X.; Dong, X.R.; Li, R. Optical measurement for spacecraft relative angle. J. Cent. South Univ. 2007, 38, 1155–1158. Pounds, P.E.; Bersak, D.R.; Dollar, A.M. Stability of small-scale UAV helicopters and quadrotors with added payload mass under PID control. Auton. Robot. 2012, 33, 129–142. [CrossRef] Wang, D.K.; Huang, P.F.; Cai, J.; Meng, Z.J. Coordinated control of tethered space robot using mobile tether attachment point in approaching phase. Adv. Sp. Res. 2014, 54, 1077–1091. [CrossRef] © 2015 by the authors; licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons by Attribution (CC-BY) license (http://creativecommons.org/licenses/by/4.0/).

32167