A Synthetic Algorithm for Tracking a Moving Object in a Multiple-Dynamic Obstacles Environment Based on Kinematically Planar Redundant Manipulators

This paper presents a synthetic algorithm for tracking a moving object in a multiple-dynamic obstacles environment based on kinematically planar manipulators. By observing the motions of the object and obstacles, Spline filter associated with polynomial fitting is utilized to predict their moving paths for a period of time in the future. Several feasible paths for the manipulator in Cartesian space can be planned according to the predicted moving paths and the defined feasibility criterion. The shortest one among these feasible paths is selected as the optimized path. Then the real-time path along the optimized path is planned for the manipulator to track the moving object in real-time. To improve the convergence rate of tracking, a virtual controller based on PD controller is designed to adaptively adjust the real-time path. In the process of tracking, the null space of inverse kinematic and the local rotation coordinate method (LRCM) are utilized for the arms and the end-effector to avoid obstacles, respectively. Finally, the moving object in a multiple-dynamic obstacles environment is thus tracked via real-time updating the joint angles of manipulator according to the iterative method. Simulation results show that the proposed algorithm is feasible to track a moving object in a multiple-dynamic obstacles environment.


Introduction
Planar manipulators can accomplish many tasks with high efficiency for a long time in specific/dangerous environments.Many researches have integrated vision technologies into manipulators system to further improve their intelligence, involving object perception, path prediction, action decision, path planning and execution, and so on [1][2][3].The planar redundant manipulators with vision technologies can be used to inspect the facilities in complex environments or narrow spaces, such as ITER relevant inspection robots [4,5] and SAFIRE robots [6].The control systems of the redundant manipulators can be designed based on dynamics or kinematics [7].The system based on dynamics has better realtime performance, but it heavily depends on the hardware system and the level of the designer.Besides, the hardware system is closed, which limits the further development.The system based on kinematics is open and easier to achieve a complicated motion of the manipulator via the upper planning.And such system plays key roles in realizing intelligent control of robot manipulator.So the focus of this paper will be devoted to the study of kinematically planar redundant manipulator.
Typical researches on the manipulators with vision system aim at real-time tracking moving objects in the varying environment.Allen et al. introduce a control algorithm to predict ahead the trajectory of the object in real-time, and the tracking can be performed by kinematic transformation computation [8].Husain et al. present an automated system with a Microsoft Kinect sensor to track the moving object via reducing the distance between the object and the tripper mounted on the 7 degrees of freedom (DOF) robotic arm in real-time [9].A simulation research, developed by Wei et al. [10], predicts the pose of the object in the next period by using the observed feature points at first; then the manipulator is controlled to achieve the real-time tracking.In [11], the authors propose an adaptive neuropredictive control method for redundant robot manipulators, which includes the prediction and obstacles avoidance, but the tracked path is planned offline and defined to be not affected by the dynamic obstacles.
Nevertheless, to track a moving object in a multipledynamic obstacles environment, several technologies must be considered at the same time, containing moving path prediction, path planning, and real-time obstacles avoidance.Moreover, these technologies must be synthesized reasonably to ensure that the moving object can be grasped successfully.In the literatures, however, although the existing methods have considered the prediction, path planning, and the obstacles avoidance, the failure of tracking always occurs in a multiple-dynamic obstacles environment.The main reason, on the one hand, is mainly due to the insufficient synthesis of these technologies.On the other hand, the impacts of the dynamic obstacles on the moving path of the end-effector and the behavioral constraints of the manipulator are not taken into account fully, since, for the spatial manipulators, they can bypass obstacles by its own joint rotation in 3D space.But for the planar manipulators, the dynamic obstacles cannot be bypassed because the joints of planar manipulators only rotate in a plane, which may result in the moving object not being grasped.Hence, considering these deficiencies of the planar manipulators in practice, a synthetic algorithm involving path prediction, path planning, and obstacles avoidance need to be further improved to track a moving object in a multiple-dynamic obstacles environment.Furthermore, in order to ensure the effectiveness of the synthetic algorithm, some developments on path prediction, path planning, and obstacles avoidance are also required, respectively.
In path prediction, the future positions of the object are always predicted first to achieve a moving object tracking.The common prediction methods contain Kalman filter [12,13], learning algorithm [14], biomimetic approach [15], and polynomial fitting method [16].Kalman filter can predict the current path based on the past information.Learning approach and biomimetic approach need a lot of offline training for many moving curves before prediction.Polynomial fitting method can predict many moving curves in the future by the polynomial calculation, and the error is small, but the motion paths are always considered as regular curves.Spline filter is an advanced method to smooth a large curve and it can ride the curve trend well [17], so Spline filter is raised to ride the trend of the moving paths for the first time in this paper.Then, polynomial fitting is introduced to predict the future moving paths of object and obstacles along their path trends.
On the basis of the predicted moving paths, the tracking path of the manipulator can be planned by A * search method [18], the artificial potential field method [19], neural networks [20,21], ant colony algorithm [22], or C-space method [23].But they all have some limitations.Since the computational amount of A * search method will increase sharply with the increase of space dimension, A * search method is difficult applied to plan paths in real-time.The artificial potential field method is suitable for dealing with dynamic obstacles, but it is easy to fall into local minimum point.Neural network method needs to be trained again when the environment changes.Ant colony algorithm has slow convergence speed and easily falls into local optimum.C-space method needs massive computation, but it is more intuitive and easier to plan a tracking path in a dynamic obstacles environment.So, C-space method is applied to plan a path for the end-effector to track the moving object in this paper.Besides, most of these methods are based on the hypothesis that the positions on the planned path could be reached by manipulators.However, in a multiobstacles environment, some positions on the planned path cannot be reached for the planar manipulator due to obstacles limiting the joint angles, which may result in the moving object not being grasped.That is, no criterion is set to judge whether the future position of the object along the planned path can be reached.Consequently, a feasibility criterion is defined to ensure that the path planned by C-space method is feasible for manipulator to grasp the object.
Although the tracking path is planned appropriately, the real-time obstacle avoidance for the end-effector and the manipulator arms, which is based on local obstacle avoidance approaches [24], still needs to be considered in the varying environment.Most of the local approaches aim at producing motions away from the obstacle for every point or each critical point on the manipulator when they are close to the obstacle in Cartesian space [25][26][27][28].In the varying environment, the obstacles avoidance will be more complex.In [7], switching primary and secondary task and setting priorities are applied to avoid multiple obstacles.This paper will draw on this strategy and set the same priority for these obstacles to avoid the multidynamic obstacles in tracking the moving object at the same time.
Considering that the errors of predicting paths are small in a short time, the tracking path should be as short as possible and the tracking speed must be accelerated.In generally, the gain coefficient of the position error is constant [29].To achieve fast tracking, a large gain coefficient is usually set, which results in large system gain.Besides, the joint driver overshoot and jitter will occur.Thus an adaptively adjusting method, in which gain coefficient varies with the change of the tracked object speed, is proposed to ease the process.
As previously described, some further studies still need to be done in moving path prediction, path planning, and the real-time obstacles avoidance.Meanwhile, these aspects must be synthesized effectively to track a moving object in a multiple-dynamic obstacles environment.Therefore, based on kinematically 6-DOF planar manipulator, this paper presents a synthetic algorithm to achieve a moving object tracking in a multiple-dynamic obstacles environment, which involves the moving path prediction, path planning, and the real-time obstacles avoidance.The algorithm applies the predicted moving paths of the object and obstacles to plan feasible paths, and an optimized path is selected among these feasible paths based on the shortest principle.A virtual controller is designed for the manipulator to adjust the tracking speed adaptively.Moreover, the local rotation coordinate method (LRCM), which is proposed by the authors, is used to avoid the obstacles that get close to the end-effector, and the null space of inverse kinematics is adapted to avoid obstacles in the vicinity of the manipulator arms.The supplementary material, which can demonstrate the proposed method, is available online (see Supplementary Material available online at https://doi.org/10.1155/2017/7310105).
This paper is organized as follows.Section 2 describes the real-time path tracking approaches by utilizing iterative method in the absence of obstacles.The strategies of tracking a moving object in the multiple-dynamic obstacles environment are developed in Section 3, involving moving path prediction, the optimized path planning, obstacles avoidance, and the synthetic object tracking algorithm.And the corresponding simulations are presented in Section 4. The conclusion is drawn in Section 5.

Obstacles Free Case
2.1.Based on the Common Method.As shown in Figure 1(a), the 3D model of 6-DOF planar manipulator is presented.The manipulator can only work in a plane with six rotary joints and the axes of joints are parallel to each other.And it is obvious to see that each joint is driven by an actuator directly.For a given task, the motion path of the end-effector is planned in Cartesian space at first.Then the rotation of each joint can be obtained by the inverse kinematic.Finally, according to the planned path, the movement of the manipulator is realized by the position control of actuators.In this paper, to facilitate the modeling and analysis of kinematics in Cartesian space, a simplified model is established based on the 3D model of 6-DOF planar manipulator as shown in Figure 1(b).Each thin solid line expresses an arm of the manipulator.Also, the line  5  6 indicates the end-effector.The Cartesian coordinate  is defined as the base coordinate system of the planar manipulator and the inverse kinematics of the 6-DOF planar manipulator is where Ẋ is the velocity of end-effector in Cartesian space.X is the position and attitude of end-effector, X = (, , Ψ) T , Ψ is the attitude of end-effector, and Ψ =  1 +  2 +  3 +  4 +  5 +  6 .Θ is the joint velocity of manipulator in joint space.Θ is the joint angle; Θ = ( 1 ,  2 ,  3 ,  4 ,  5 ,  6 ) T .J * is the pseudoinverse of Jacobian J(Θ) based on the damped least squares (DLS) method, J * = J T (JJ T + I) −1 , and J ∈  3×6 . is the damping factor.I = diag(1, 1, 1).J(Θ) = [J  (Θ), J  (Θ)] T .J  (Θ) = (/Θ, /Θ) T ; J  (Θ) = (Ψ/Θ).Jacobian J is a desired matrix; that is, all parameters in J are accurately identified.In application, J always contains some errors, but the tasks can still be carried out normally.To prove this view, let Ĵ be the Jacobian with some errors and the corresponding simulations are presented in Section 2.3.
In order to track a moving object in real-time, the velocity of end-effector, Ẋ in (1), is defined as where Ẋobj is the velocity of the moving object.Ẋr is the relative velocity and Ẋr is expressed as [29] Ẋr =  ⋅ (X obj − X) , where  is the gain coefficient of the position error.X obj is the position and attitude of the object, X obj = ( obj ,  obj , Ψ obj ) T , and Ψ obj is the object attitude.
In this paper, the iterative method is used to achieve the manipulator movement [30,31].Then (1) is modified as where ΔX is the real-time path; ΔX = ΔX r + ΔX obj .ΔX obj , ΔX r are the displacement of object and the relative displacement in a sampling time interval Δ, respectively.And (3) becomes where  =  ⋅ Δ.Then the moving object can be tracked by updating the joint angles Θ in real-time by (6).The control diagram of the tracking process is described in Figure 2(a).

Based on the Virtual Controller.
Based on the common method, the moving object can be tracked, but in order to increase the convergence rate, a large proportional coefficient is usually set and it is not flexible.So, a virtual controller, similar to PD controller, is designed to improve the convergence rate adaptively.Then ΔX r , is modified as ΔX V .
where   ,   ,   are the ratio coefficients.Index  stands for the th iterative process.∑   ,   are used to compensate ΔX V according to the speed of object.  ,   are calculated by , where   = {   },  = 1, 2, 3.  = 1 expresses  coordinate value. = 2 expresses  coordinate value. = 3 expresses the attitude Ψ. ,  are the adaptive coefficients defined as where  0 ,  0 are the initial coefficients.The velocity correlation functions  1 (V obj ),  2 (V obj ) can adaptively increase with the increase of speed.When the object speed is low, the values of   and   keep small.When the object speed becomes large, the values of   and   will increase appropriately.Meanwhile, when the speed becomes too large,  1 (V obj ),  2 (V obj ) will reach saturation state and   ,   will remain maximum, which keeps ΔX r not excessive. 1 (V obj ),  2 (V obj ) are defined as where   ,   ,  1 ,  2 are the ratio coefficients.V obj is the velocity magnitude of the moving object.The control diagram with the virtual controller is shown in Figure 2(b).

Simulation Verification
Simulation 1.This simulation is to compare the common method with the proposed method.The object moves back and forth along a curve (Ψ obj = 0.7 × sin(2( + 0.95)) + 2.07) as shown in Figure 1.In the simulation,  is 0.048 for common method and  is 0.01 for proposed method, which will make   ×  0 = 0.048.And the ΔX r in the both cases will be coincident.Since the integral link ∑   is accumulated gradually and the adaptive adjusting part   compensates ΔX V , the planned path ΔX V will become large and then the convergence rate is increased.In Figure 3, the convergence rate of the proposed method is much quicker than that of the common method in both positions and attitudes, and the error with the virtual controller is smaller.The simulation parameters are shown in Tables 1 and 2.
Simulation 2. This paper assumes that the length   ( = 1, 2, . . ., 6) of manipulator arms contains some errors, which results in Jacobian J being not desired.That is, l =   + Δ  ; Δ  is the length error of the th arm.The simulations with the virtual controller based on Ĵ and J are shown in Figure 4 and the corresponding parameters are shown in Tables 1 and  2. In Figure 4, although there are some errors in Jacobian, the moving object is still tracked.The stable error based on Ĵ is slightly larger in positions as shown in Figure 4(a).The attitude error is the same as that based on J as shown in Figure 4(b).Based on the simulations, we can conclude that some errors in Jacobian have less impact on the primary tasks and the system has good robustness.

Observation, Moving Path Prediction, and Path Planning.
In a multidynamic obstacles environment, avoiding the obstacles will limit the rotation range of the joint angles, which results in some positions within the workspace not being reached.So, before tracking the moving object, the grasped position of object should be predicted, and a reachable path should be planned for the manipulator.To complete the prediction and path planning, the initial motions of the object and obstacles are observed by camera or sensor at first.As shown in Figure 5(a), the dot lines stand for the initial moving paths and the balls A, B, C, D, E stand for their current positions.Based on their current positions, the obstacles are divided into two kinds of cases.One is inside a closed polygon  indicated by polygons  1  2  3  4  5  6 .The other one is not inside .The criterion for judging an obstacle inside S is based on the method proposed by Hormann and Agathos [32] and the criterion is expressed as where   is the th obstacle,  = 1, 2, . . ., .  is the number of obstacles within the workspace of manipulator.
Remark 1.This paper assumes that the object and the obstacles keep motions within the workspace of the manipulator, and their motions can be observed.Besides, the moving curves of the object and obstacles are smooth.Consider that the moving paths observed by camera or sensor contain noises.Gaussian filter is always used to suppress the noises, but it suffers from edge effects due to the local weight average [33].Spline filter is advanced without edge effects, whose amplitude transmission characteristic is similar to that of Gaussian filter.And it can ride the curve trend well.So, Spline filter is attempted to suppress the noises and ride the trend of the moving paths in ,  direction.Spline filter can be expressed as where () is the filtered path containing  points.() is the observed path with noises.Q and I are the  ×  matrix. is given by where Δ  is the sample interval and   is the cut-off length.More details about Spline filter are presented in [17].
As shown in Figure 5 The angle  *  ( = 1, 2, 3, . . .,  − 2) can be fitted with a second-order polynomial via least-square methods where  *  ,  *  ,  *  are the coefficients.The future angular  * () ( ≥  − 1) is calculated by (15).The rotation matrix R, used to determine the direction of the next vector a +1 , is defined as The weight   , introduced to calculate the size of the next vector a +1 , is expressed as Similarly, the weights are fitted by a first-order polynomial where  is the th point on the path and  = 1, 2, 3, . . .,  − 2.   ,   are the scale coefficients.
Based on ( 15)-( 18), the following predicted vector, a +1 , can be calculated by Then the following point  +1 ( +1 ,  +1 ) can be obtained by (20).The predicted path is accurate and the simulation is presented in Section 4.
where  is the number of the predicted points.As shown in Figure 5(c), the black dash curve, AA  , is the predicted path of the moving object.The dash lines, BB  , CC  , DD  , and EE  , are the predicted paths of obstacles, respectively.The predicted point A  is the grasped position of the object.The points B  , C  , D  , E  are the predicted positions of obstacles when the object  is grasped.According to the predicted positions of object and obstacles, we can plan the paths  1 = path{ 6 -},  2 = path{ 6 --},  3 = path{ 6 --}, and  4 = path{ 6 --} as the tracked paths as shown in Figure 5(d).
However, the convex polygon OA  B  C  D  will limit the joint rotation range, which may result in the object not being grasped along  1 .Similarly, polygons OA  C  D  , OA  D  have the same influence on  2 ,  3 as shown in Figure 5(c).So, the convex polygons, which have influence on the paths, should be found out.Then, point A  is defined as the rotation center.

Real path
Predicted path of the polygon is less than 180 ∘ .So, let   be the evaluation function for the convexity-concavity of an arbitrary polygon where  = 1, 2, . . ., ,  is the number of polygons.  is the number of angles for the th polygon.b  , b +1 are the adjacent edges of the jth polygon along clockwise or anticlockwise direction.If   is 0, the jth polygon is convex.Otherwise, it is not convex.As shown in Figure 5(c), polygons OA  B  C  D  , OA  C  D  , OA  D  are convex.Then, let   be the feasibility criterion for  1 ,  2 ,  3 ,  4 .
where   is the perimeter of the jth convex polygon which has an influence on the corresponding path.  is the length of the pth link. is the estimated safe distance ( = 220 mm),

Observation Region division
Predict the paths of obstacles and object by Eqs. ( 12), ( 19), ( 20 which ensures that no collision will happen when grasping the object.And  can be determined by simulation or experiment.If   is less than 0, the path is feasible.Otherwise, the path is infeasible. Based on (22), the feasible paths can be obtained ( 2 ,  3 ,  4 ).And among these paths, the shortest path is selected as the optimized path for the manipulator to track the moving object in real-time.As shown in Figure 5(d),  2 is the optimized path due to  2 <  3 <  4 .Based on the above analysis, the algorithm of the optimized path can be briefly described in Figure 6.

Obstacles Included in the Planned
Path.Based on algorithm of the optimized path,  2 is tracked by the end-effector of manipulator as illustrated in Figure 7(a).However, the endeffector cannot move along the path completely due to the obstacle being on the planned path.As shown in Figures 7(a) and 7(b), the obstacle  is tracked at first in tracking the path  2 .And then when the end-effector  6 moves on the edge of safe area, the object is tracked in real-time.Similarly, if any one of paths  3 and  4 is the optimized path, their tracked processes are the same to the path  2 .Besides, when the endeffector  6 moves on the edge of safe area, the motion of obstacle avoidance for end-effector should be produced; that is, the moving direction of end-effector needs to be changed as shown in Figure 7(b).For this purpose, the local rotation coordinate method (LRCM) is proposed for end-effector to avoid the obstacles that are located in the moving path and LRCM can be described as follows.Firstly, according to the iterative method presented in Section 2, the real-time path is planned ΔX = (Δ, Δ, ΔΨ) T .A local coordinate based on the point  6 is established and the   -axis is along line O 6 A as shown in Figure 7(b).In the local coordinate, the planned path (Δ, Δ) T can be expressed as (Δ  , 0) T , Δ  = (Δ 2 + Δ 2 ) 1/2 .To avoid the obstacle, Δ  is defined along   -axis in (23).
where   is the scale factor. safe is the radius of safe area.  is the distance between point  6 and obstacle  in the safe area. min is the minimum distance. decides the direction around the obstacle (along clockwise or counter clockwise direction). is calculated as where   ,   6 are the positions of object  and end-effector along -axis in Cartesian space.Considering that the object is tracked all the time when path  1 is the optimized path as shown in Figure 7(c).To reduce the length of obstacle avoidance path in safe area,  is calculated by where vector d = (0, 0, 1).Vectors u and w are shown in Figure 7(c).Then the planned path (Δ  , Δ  ) T in local coordinate X B O B Y B is mapped to Cartesian coordinate XOY by the rotation matrix R  .
where   is the rotation angle calculated by Then the planned path (Δ, Δ) T will be updated by The real-time planned path ΔX is thus modified.And the LRCM can be described by a diagram as shown in Figure 7(d).

Obstacles Close to Manipulator Arms.
In the case that the obstacles are close to manipulator arms, the principle of avoiding obstacles is based on the critical points and there

Critical points
Obstacle avoidance path End-e ector in safe area Yes No U1 in [Δx, Δy] T by Eqs. ( 23) and ( 26) by Eq. ( 28) are five critical points on each arm as shown in Figure 7(a).In this paper, there are four obstacles in the workspace of manipulator.To avoid multiobstacles, the method by setting task priority is proposed by Petrič et al. [7].When these obstacles move to the manipulator at the same time, the priorities of tasks are the same.So the obstacles avoiding formula are modified as follows: End  23) and ( 26) where  is the number of obstacles.(I − J * J)  ∇H  is the null space, which is used to avoid the jth obstacle.  is a scale coefficient. is defined as where  safe is the radius of safe area. is the distance between critical point and obstacles in the safe area and  min is defined as the minimum distance between the obstacle and the critical points.

The Synthetic Object Tracking Algorithm in Multiple-
Dynamic Obstacle Environment.On the basis of above analysis, an algorithm can be synthesized to track the moving object in a multidynamic obstacle environment, and the algorithm diagram is shown in Figure 8.Before tracking, a reachable path for real-time tracking is planned via the motions observation, moving paths prediction, and path planning, which ensures that the object can be grasped if the end-effector moves along the planned path.Then the tracking can be performed in real-time.

Simulation of the Predicted Path.
The first part of the simulation is to prove the exactness and the stability of the predicted path for the paths of the moving body in the future.
Based on the initial 40 points, the future 20 points of three kind curves are predicted as shown in Figure 9.The sample interval, Δ  , is 1 mm and cut-off length,   , is 40 mm for Spline filter.The similarity degree between the real moving path and predicted path can be calculated as where SSI ranges from 0 to 1.If the value of SSI is closer to 1, the predicted path is more accurate.SSI  is similarity degree of the jth predicted point. SSI is the number of predicted points.  is the distance between the predicting point and the real point at each instant  or position.  (  = 20 mm) is the maximum radius of allowable deviation.
According to (31), the similarity degrees of the different motions are calculated and 20 experiments of each curve are shown in Figure 9. Figures 9(d), 9(e), and 9(f) show the average similarity degrees of 20 experiments, and Figures 9(g), 9(h), and 9(i) show the similarity degree of the last predicted point.Figures 9(d  and 9(i) are corresponding to Figure 9(c).In Figures 9(a)-9(c), it is obvious to see that the moving path, via Spline filter, is smoothing and can ride the desired path well.Besides, the deviation of predicted path related to the desired path is small.As shown in Table 3, the average value of the similarity degrees of 20 experiments is calculated.In this paper, the similarity degrees, more than 0.7, are considered to be effective.And based on Table 3, it shows that the proposed method for predicting path is stable and exact.So the proposed method is suitable to predict the moving paths in this paper.

Simulation of LRCM.
The second part of the simulation is to verify the feasibility of LRCM.LRCM is used to avoid the obstacles that are close to the end-effector of manipulator.In Figures 10(a) and 10(e), the movement of avoidance obstacles will be produced when the end-effector enters the safe area, and the static obstacle is avoided based on LRCM.The motion of obstacle avoidance will not work when the end-effector moves out of the safe area.As shown in Figures 10(b) and 10(f), the LRCM also shows the feasibility for the moving obstacle to track the moving object.When end-effector moves over the obstacle, the null space of inverse kinematics is used to avoid the obstacle for other arms of manipulator.Besides, based on LRCM, the end-effector of the manipulator can achieve a large turn to avoid the obstacle that is located in The reason is that the repulsive force F ref is along the radial direction of the obstacle so that the end-effector cannot turn to the right.The force F ref of artificial potential field can be calculated by (32) [16].Tables 4 and 5 show the simulation parameters of LRCM and the artificial potential field.where x is the position vector of the end-effector, x = (, ). is the distance between critical point and obstacles in safe area.

Simulation of the Virtual Controller.
The third part of the simulation is to test the applicability of tracking the moving object with different speeds.As shown in Figure 11, the objects at the speed of 90 mm/s, 63 mm/s, and 36 mm/s are tracked, respectively, and the moving process is similar to Figures 10(b) and 10(f).In Figure 11, it is obvious to see that the convergence rate of the tracking with the virtual controller is more fast than that without the virtual controller, which is caused by the integral link ∑   and the adaptive adjusting part   .Besides, the time-consuming becomes less with the Mathematical Problems in Engineering increasing of the speed of the object, due to the adaptive adjusting part   .What is more, based on the slope of the position error curve, the convergence rate will decrease when the distance between the object and the end-effector becomes small.So it is more safe to track the moving object within the predicted time.The simulation parameters are shown in Table 5.In Figure 12(i), we can see that the changes of the joint angles are continuous and smooth.And during the first six seconds, the manipulator remains static, and the object and obstacles keep moving.At 9th second, the moving object is almost grasped.Although there still is some position error, it is relatively small comparing with the sizes of the object or the obstacles as shown in Figure 12(h).The simulation results show that the reachability of the object positon can be guaranteed by introducing the observation and moving path prediction, and the proposed algorithm is suitable to tracking a moving object in the multidynamic obstacle environment.The parameters of simulation are shown in Table 5 and the simulation process of the proposed algorithm can be downloaded from [34].

Conclusion
A synthetic algorithm, involving observation, moving path prediction, path planning, and real-time tracking, is proposed to track a moving object in a multidynamic obstacles environment for kinematically planar redundant manipulators.Spline filter combining with polynomial fitting is developed to predict the moving paths of the object and obstacles.The predicted moving paths and the defined feasibility criterion are used to plan paths of the end-effector in Cartesian space.Then the optimized path algorithm is raised for the manipulator to plan a tracking path.In tracking, the LRCM method is proposed for end-effector to avoid obstacles, and the null space of inverse kinematic is adopted for the manipulator arms to avoid obstacles.A virtual controller based on PD controller is designed to achieve the adaptive fast tracking.The moving object is thus tracked by updating the joint angles based on iterative method in real-time.Simulation results show that the predicting method of path is stable and accurate, the LRCM method is feasible for the end-effector to avoid obstacles, and the convergence rate of tracking can increase adaptively by using the virtual controller.Besides, the proposed synthetic algorithm is feasible to track a moving object in a multidynamic obstacle environment.Therefore, the synthetic algorithm is recommended by the authors.

Figure 1 :
Figure 1: Real-time tracking a moving object in absence of obstacles.(a) 3D model of 6-DOF planar manipulator.(b) The simplified model of 6-DOF planar manipulator.

Figure 2 :
Figure 2: The control diagram tracking a moving object. in = X obj and  out = Θ.(a) The control diagram based on the common method.(b) The control diagram based on the proposed method.

Figure 3 :
Figure 3: The comparison of the common method and the proposed method.(a) The position error between end-effector and object.(b)The attitudes Ψ and Ψ obj ; the attitude error between Ψ and Ψ obj .

Figure 4 :
Figure 4: The changes of positions and attitude based on the proposed method in tracking a moving object.(a) The position error between end-effector and object.(b) The attitudes Ψ and Ψ obj ; the attitude error between Ψ and Ψ obj .
A  O 6 rotates clockwise around point A  .Line A  O has set the finishing line for rotation.The obstacles within S and the object can form polygons OA  B  C  D  , OA  C  D  , OA  D  .For a convex polygon, each interior angle

Figure 5 :
Figure 5: Observation, path prediction, and path planning.A is the object.B, C, D, and E are the obstacles.(a) Motions observation and region division of the obstacles.(b) Path prediction based on observed points.(c) Path judgment and selection based on convex polygons formed by the object A  and obstacles B  , C  , and D  .(d) Planning feasible paths and the optimized path.

Figure 6 :
Figure 6: The algorithm diagram of the optimized path.

Figure 7 :
Figure 7: The principle of obstacles avoidance on the path.(a) The overall layout.(b) The end-effector moves around the obstacle  in real-time.(c) In the case that path  1 is the optimized path.(d) The LRCM diagram.1 in is ΔX; 1 out is the updated path ΔX.

Figure 8 :
Figure 8: The synthetic algorithm diagram of tracking a moving object in multiple-dynamic obstacles environment. in = X obj ;  out = Θ.

Figure 12 :
Figure 12: The simulation of tracking the moving object in multiple-dynamic obstacles environment.Ψ is the attitude of end-effector; Ψ obj is the attitude of the object.The initial joint angles Θ = (0.8, −0.3, 0.6, 0.5, 0.5, 0.8) T .(a), (b) Tracking the moving object without observation and moving path prediction.(d) Observation and prediction.(e) Real-time tracking the object and avoiding obstacles.(c), (f) The whole process of tracking the object.(g) The attitudes Ψ, Ψ obj and the error between Ψ and Ψ obj .(h) The position error between end-effector and object.(i) The joint angular displacement.Subfigures (a), (c) show the movement of 3D model.Subfigures (b), (d)-(f) show the movement of the simplified model.

4. 4 .
Simulation of the Synthetic Object Tracking Algorithm.The last part of the simulation is to validate the feasibility of the synthetic tracking algorithm proposed by the authors.Firstly, the simulation of tracking the moving object without observation and moving path prediction is presented as shown in Figures12(a) and 12(b).Based on the shortest principle, path  1 is tracked in real-time as shown in Figure5(d).In Figures12(a) and 12(b), although the endeffector can avoid the obstacle, it cannot reach the position of object due to the obstacles limiting the joint angles of manipulator.So, the observation and moving path prediction are introduced into the proposed algorithm in this paper as shown in Figures12(c)-12(f).
Figure 12(d)  presents that the motions of the object and the obstacles are observed, and the future position is predicted at first before tracking.Meanwhile, the optimized path  2 is planned.Figure12(e)shows that the moving object is tracked in real-time along the optimized path  2 .Figures12(c) and 12(f) display the whole process of tracking in the dynamic environment based on the proposed tracking algorithm.The tracking pose of manipulator and the pose error are shown in Figures12(g) and 12(h).

Table 3 :
The similarity degrees of the different motions for Figure9.

Table 5 :
Parameters in the virtual controller.