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

Authors: Hongzhe Jin; Hui Zhang; Zhangxing Liu; Decai Yang; Dongyang Bie; He Zhang; Ge Li; Yanhe Zhu; Jie Zhao
Journal: Mathematical Problems in Engineering
 (2017)

---

## Abstract

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.


---

## Body

## 1. 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–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 real-time 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 multiple-dynamic 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 byA∗ 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–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 athttps://doi.org/10.1155/2017/7310105).This paper is organized as follows. Section2 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.


## 2. Obstacles Free Case

### 2.1. Based on the Common Method

As shown in Figure1(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 O5O6 indicates the end-effector. The Cartesian coordinate XOY is defined as the base coordinate system of the planar manipulator and the inverse kinematics of the 6-DOF planar manipulator is(1)Θ˙=J∗X˙,where X˙ is the velocity of end-effector in Cartesian space. X is the position and attitude of end-effector, X=x,y,Ψ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,θ6T. J∗ is the pseudoinverse of Jacobian J(Θ) based on the damped least squares (DLS) method, J∗=JTJJT+λI-1, and J∈R3×6. λ is the damping factor. I=diag(1,1,1). J(Θ)=JeΘ,JcΘT. Je(Θ)=(∂x/∂Θ,∂y/∂Θ)T; Jc(Θ)=(∂Ψ/∂Θ). 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 J^ be the Jacobian with some errors and the corresponding simulations are presented in Section 2.3.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.
(a)
(b)In order to track a moving object in real-time, the velocity of end-effector,X˙ in (1), is defined as (2)X˙=X˙r+X˙obj,where X˙obj is the velocity of the moving object. X˙r is the relative velocity and X˙r is expressed as [29] (3)X˙r=K·Xobj-X,where K is the gain coefficient of the position error. Xobj is the position and attitude of the object, Xobj=xobj,yobj,ΨobjT, 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 (4)ΔΘ≈J∗ΔΧ,where ΔX is the real-time path; ΔX=ΔXr+ΔXobj. ΔXobj, ΔXr are the displacement of object and the relative displacement in a sampling time interval ΔT, respectively. And (3) becomes (5)ΔΧr=μ·Χobj-Χ,where μ=K·ΔT. 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). (6)Θ≔Θ+ΔΘ.Figure 2
The control diagram tracking a moving object.Uin=Xobj and Uout=Θ. (a) The control diagram based on the common method. (b) The control diagram based on the proposed method.
(a)
(b)


### 2.2. 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ΔXr, is modified as ΔXV. (7)ΔΧV=kpξi+kI∑j=1iξj+ksγi,where kp, ks, kI are the ratio coefficients. Index i stands for the ith iterative process. ∑ξj, γi are used to compensate ΔXV according to the speed of object. ξi, γi are calculated by (8)γi=1-e-αξij1+e-αξij,ξi=ηΔXr+ΔX˙r,where ξi={ξij}, j=1,2,3. j=1 expresses x coordinate value. j=2 expresses y coordinate value. j=3 expresses the attitude Ψ. α, η are the adaptive coefficients defined as (9)α=α0+f1vobj,η=η0+f2vobj,where α0, η0 are the initial coefficients. The velocity correlation functions f1(vobj), f2(vobj) can adaptively increase with the increase of speed. When the object speed is low, the values of ξi and γi keep small. When the object speed becomes large, the values of ξi and γi will increase appropriately. Meanwhile, when the speed becomes too large, f1(vobj), f2(vobj) will reach saturation state and ξi, γi will remain maximum, which keeps ΔXr not excessive. f1(vobj), f2(vobj) are defined as (10)f1vobj=αp1-e-p1·vobj,f2vobj=ηp1-e-p2·vobj,where αp, ηp, p1, p2 are the ratio coefficients. vobj is the velocity magnitude of the moving object. The control diagram with the virtual controller is shown in Figure 2(b).


### 2.3. 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(t+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 kp×η0=0.048. And the ΔXr in the both cases will be coincident. Since the integral link ∑ξj is accumulated gradually and the adaptive adjusting part γi compensates ΔXV, the planned path ΔXV 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.Table 1
Parameters in the virtual controller.
Parameters
k
p
k
I
k
s
α
0
α
p
η
0
η
p
p
1
p
2
Value
0.016
10−7
2
0.05
0.02
300
100
0.2
0.6Table 2
D-H parameters of 6-DOF manipulator.
Parameters
(mm)
Parameters
(mm)
Parameters
(rad)
l
1
150
l
^
1
140
θ
1
0.3
l
2
150
l
^
2
150
θ
2
0.3
l
3
150
l
^
3
160
θ
3
0.6
l
4
150
l
^
4
141
θ
4
0.5
l
5
150
l
^
5
155
θ
5
0.5
l
6
150
l
^
6
149
θ
6
0.8Figure 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.
(a)
(b)Simulation 2.
This paper assumes that the lengthlp (p=1,2,…,6) of manipulator arms contains some errors, which results in Jacobian J being not desired. That is, l^p=lp+Δlp; Δlp is the length error of the pth arm. The simulations with the virtual controller based on J^ 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 J^ 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.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)
(b)


## 2.1. Based on the Common Method

As shown in Figure1(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 O5O6 indicates the end-effector. The Cartesian coordinate XOY is defined as the base coordinate system of the planar manipulator and the inverse kinematics of the 6-DOF planar manipulator is(1)Θ˙=J∗X˙,where X˙ is the velocity of end-effector in Cartesian space. X is the position and attitude of end-effector, X=x,y,Ψ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,θ6T. J∗ is the pseudoinverse of Jacobian J(Θ) based on the damped least squares (DLS) method, J∗=JTJJT+λI-1, and J∈R3×6. λ is the damping factor. I=diag(1,1,1). J(Θ)=JeΘ,JcΘT. Je(Θ)=(∂x/∂Θ,∂y/∂Θ)T; Jc(Θ)=(∂Ψ/∂Θ). 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 J^ be the Jacobian with some errors and the corresponding simulations are presented in Section 2.3.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.
(a)
(b)In order to track a moving object in real-time, the velocity of end-effector,X˙ in (1), is defined as (2)X˙=X˙r+X˙obj,where X˙obj is the velocity of the moving object. X˙r is the relative velocity and X˙r is expressed as [29] (3)X˙r=K·Xobj-X,where K is the gain coefficient of the position error. Xobj is the position and attitude of the object, Xobj=xobj,yobj,ΨobjT, 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 (4)ΔΘ≈J∗ΔΧ,where ΔX is the real-time path; ΔX=ΔXr+ΔXobj. ΔXobj, ΔXr are the displacement of object and the relative displacement in a sampling time interval ΔT, respectively. And (3) becomes (5)ΔΧr=μ·Χobj-Χ,where μ=K·ΔT. 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). (6)Θ≔Θ+ΔΘ.Figure 2
The control diagram tracking a moving object.Uin=Xobj and Uout=Θ. (a) The control diagram based on the common method. (b) The control diagram based on the proposed method.
(a)
(b)


## 2.2. 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ΔXr, is modified as ΔXV. (7)ΔΧV=kpξi+kI∑j=1iξj+ksγi,where kp, ks, kI are the ratio coefficients. Index i stands for the ith iterative process. ∑ξj, γi are used to compensate ΔXV according to the speed of object. ξi, γi are calculated by (8)γi=1-e-αξij1+e-αξij,ξi=ηΔXr+ΔX˙r,where ξi={ξij}, j=1,2,3. j=1 expresses x coordinate value. j=2 expresses y coordinate value. j=3 expresses the attitude Ψ. α, η are the adaptive coefficients defined as (9)α=α0+f1vobj,η=η0+f2vobj,where α0, η0 are the initial coefficients. The velocity correlation functions f1(vobj), f2(vobj) can adaptively increase with the increase of speed. When the object speed is low, the values of ξi and γi keep small. When the object speed becomes large, the values of ξi and γi will increase appropriately. Meanwhile, when the speed becomes too large, f1(vobj), f2(vobj) will reach saturation state and ξi, γi will remain maximum, which keeps ΔXr not excessive. f1(vobj), f2(vobj) are defined as (10)f1vobj=αp1-e-p1·vobj,f2vobj=ηp1-e-p2·vobj,where αp, ηp, p1, p2 are the ratio coefficients. vobj is the velocity magnitude of the moving object. The control diagram with the virtual controller is shown in Figure 2(b).


## 2.3. 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(t+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 kp×η0=0.048. And the ΔXr in the both cases will be coincident. Since the integral link ∑ξj is accumulated gradually and the adaptive adjusting part γi compensates ΔXV, the planned path ΔXV 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.Table 1
Parameters in the virtual controller.
Parameters
k
p
k
I
k
s
α
0
α
p
η
0
η
p
p
1
p
2
Value
0.016
10−7
2
0.05
0.02
300
100
0.2
0.6Table 2
D-H parameters of 6-DOF manipulator.
Parameters
(mm)
Parameters
(mm)
Parameters
(rad)
l
1
150
l
^
1
140
θ
1
0.3
l
2
150
l
^
2
150
θ
2
0.3
l
3
150
l
^
3
160
θ
3
0.6
l
4
150
l
^
4
141
θ
4
0.5
l
5
150
l
^
5
155
θ
5
0.5
l
6
150
l
^
6
149
θ
6
0.8Figure 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.
(a)
(b)Simulation 2.
This paper assumes that the lengthlp (p=1,2,…,6) of manipulator arms contains some errors, which results in Jacobian J being not desired. That is, l^p=lp+Δlp; Δlp is the length error of the pth arm. The simulations with the virtual controller based on J^ 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 J^ 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.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)
(b)


## 3. Multidynamic Obstacles Case

### 3.1. 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 Figure5(a), the dot lines stand for the initial moving paths and the ballsA, 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 S indicated by polygons OO1O2O3O4O5O6A. The other one is not inside S. The criterion for judging an obstacle insideS is based on the method proposed by Hormann and Agathos [32] and the criterion is expressed as (11)ini=1,ifSi∈S0,ifSi∉S,where Si is the ith obstacle, i=1,2,…,m. m is the number of obstacles within the workspace of manipulator.Figure 5
Observation, path prediction, and path planning.A is the object.B,C,D, andE 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 objectA′ and obstaclesB′,C′, andD′. (d) Planning feasible paths and the optimized path.
(a)
(b)
(c)
(d)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 x, y direction. Spline filter can be expressed as (12)I+β4Qωt=zt,where ω(t) is the filtered path containing n points. z(t) is the observed path with noises. Q and I are the n×n matrix. β is given by (13)β=2sin⁡πΔxsλc-1,where Δxs is the sample interval and λc is the cut-off length. More details about Spline filter are presented in [17].As shown in Figure5(b), the black curve is the real path. The orange points stand for the predicted points. The red points, smoothed via Spline filter, indicate initial motion of the object or obstacles, which are used to predict the following path. n-1 vectors (a1,a2,a3,…,an-1) can be formed by n points (Q1,Q2,…,Qn), and the angles of every two adjacent vectors are calculated by (14)θi∗=arccos⁡ai·ai+1ai·ai+1.The angleθi∗ (i=1,2,3,…,n-2) can be fitted with a second-order polynomial via least-square methods (15)θ∗i=aθ∗+bθ∗·i+cθ∗·i2,where aθ∗, bθ∗, cθ∗ are the coefficients. The future angular θ∗(i) (i≥n-1) is calculated by (15). The rotation matrix R, used to determine the direction of the next vector ai+1, is defined as (16)R=cos⁡θ∗i-sin⁡θ∗isin⁡θ∗icos⁡θ∗i.The weight Wi, introduced to calculate the size of the next vector ai+1, is expressed as(17)Wi=ai+1ai.Similarly, the weights are fitted by a first-order polynomial(18)Wi=aW+bW·i,where i is the ith point on the path and i=1,2,3,…,n-2. aW, bW are the scale coefficients.Based on (15)–(18), the following predicted vector, ai+1, can be calculated by (19)ai+1T=Wi·R·aiT,i>n-2.Then the following pointQi+1(xi+1,yi+1) can be obtained by (20). The predicted path is accurate and the simulation is presented in Section 4. (20)xi+1,yi+1=xi,yi+ai,i=n,n+1,…,n+k-1,where k 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′, andEE′, are the predicted paths of obstacles, respectively. The predicted pointA′ is the grasped position of the object. The pointsB′,C′,D′,E′ are the predicted positions of obstacles when the object A is grasped. According to the predicted positions of object and obstacles, we can plan the paths C1=path{O6-A}, C2=path{O6-B-A}, C3=path{O6-C-A}, and C4=path{O6-D-A} as the tracked paths as shown in Figure 5(d). However, the convex polygonOA′B′C′D′ will limit the joint rotation range, which may result in the object not being grasped along C1. Similarly, polygonsOA′C′D′,OA′D′ have the same influence on C2, C3 as shown in Figure 5(c).So, the convex polygons, which have influence on the paths, should be found out. Then, pointA′ is defined as the rotation center.A′O6 rotates clockwise around pointA′. LineA′O has set the finishing line for rotation. The obstacles withinS and the object can form polygonsOA′B′C′D′,OA′C′D′,OA′D′. For a convex polygon, each interior angle of the polygon is less than 180°. So, let Pj be the evaluation function for the convexity-concavity of an arbitrary polygon(21)Pj=∑k=1Kjmin⁡bk×bk+1,0,where j=1,2,…,N, N is the number of polygons. Kj is the number of angles for the jth polygon. bk, bk+1 are the adjacent edges of thejth polygon along clockwise or anticlockwise direction. If Pj is 0, thejth polygon is convex. Otherwise, it is not convex. As shown in Figure 5(c), polygonsOA′B′C′D′,OA′C′D′,OA′D′ are convex. Then, let Fj be the feasibility criterion for C1, C2, C3, C4. (22)Fj=Lj-OA′¯-∑p=16lp+δ,where Lj is the perimeter of thejth convex polygon which has an influence on the corresponding path. lp is the length of thepth link. δ is the estimated safe distance (δ = 220 mm), which ensures that no collision will happen when grasping the object. Andδ can be determined by simulation or experiment. If Fj is less than 0, the path is feasible. Otherwise, the path is infeasible.Based on (22), the feasible paths can be obtained (C2, C3, C4). 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), C2 is the optimized path due to C2<C3<C4. Based on the above analysis, thealgorithm of the optimized path can be briefly described in Figure 6.Figure 6
The algorithm diagram of the optimized path.


### 3.2. Obstacles Included in the Planned Path

Based onalgorithm of the optimized path, C2 is tracked by the end-effector of manipulator as illustrated in Figure 7(a). However, the end-effector 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 B is tracked at first in tracking the path C2. And then when the end-effector O6 moves on the edge of safe area, the object is tracked in real-time. Similarly, if any one of paths C3 and C4 is the optimized path, their tracked processes are the same to the path C2. Besides, when the end-effector O6 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=Δx,Δy,ΔΨT. A local coordinate based on the point O6 is established and the XB-axis is along lineO6A as shown in Figure 7(b). In the local coordinate, the planned path Δx,ΔyT can be expressed as ΔxB,0T, ΔxB=Δx2+Δy21/2. To avoid the obstacle, ΔyB is defined along YB-axis in (23). (23)ΔxB=Δx2+Δy2,ΔyB=-1τ·KB·ΔxB·cos⁡π·dB-dminRsafe-dmin+12,where KB is the scale factor. Rsafe is the radius of safe area. dB is the distance between point O6 and obstacle B in the safe area. dmin is the minimum distance. τ decides the direction around the obstacle (along clockwise or counter clockwise direction). τ is calculated as (24)τ=1,ifxA-xO6>00,Otherwise,where xA, xO6 are the positions of object A and end-effector along x-axis in Cartesian space. Considering that the object is tracked all the time when path C1 is the optimized path as shown in Figure 7(c). To reduce the length of obstacle avoidance path in safe area, τ is calculated by(25)τ=1,ifd·u×w≥00,Otherwise,where vector d=(0,0,1). Vectors u and w are shown in Figure 7(c). Then the planned path ΔxB,ΔyBT in local coordinateXBOBYB is mapped to Cartesian coordinateXOY by the rotation matrix Ro.(26)Ro=cos⁡φo-sin⁡φosin⁡φocos⁡φo,where φo is the rotation angle calculated by (27)φo=arccos⁡ΔxΔx2+Δy2.Figure 7
The principle of obstacles avoidance on the path. (a) The overall layout. (b) The end-effector moves around the obstacleB in real-time. (c) In the case that path C1 is the optimized path. (d) The LRCM diagram. U1in is ΔX; U1out is the updated path ΔX.
(a)
(b)
(c)
(d)Then the planned pathΔx,ΔyT will be updated by (28)ΔxΔy≔Ro·ΔxBΔyB.The real-time planned pathΔX is thus modified. And the LRCM can be described by a diagram as shown in Figure 7(d).


### 3.3. 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 are five critical points on each arm as shown in Figure7(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: (29)ΔΘ=J∗ΔΧ+kI-J∗J∑j=1Mkj∇Hj,where M is the number of obstacles. k(I-J∗J)kj∇Hj is the null space, which is used to avoid thejth obstacle. kj is a scale coefficient. k is defined as(30)k=2,ford≤dmincos⁡πd-dminRsafe-dmin+1,fordmin<d<Rsafe0,ford≥Rsafe,where Rsafe is the radius of safe area. d is the distance between critical point and obstacles in the safe area and dmin is defined as the minimum distance between the obstacle and the critical points.


### 3.4. 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 Figure8. 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.Figure 8
The synthetic algorithm diagram of tracking a moving object in multiple-dynamic obstacles environment.Uin=Xobj; Uout=Θ.


## 3.1. 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 Figure5(a), the dot lines stand for the initial moving paths and the ballsA, 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 S indicated by polygons OO1O2O3O4O5O6A. The other one is not inside S. The criterion for judging an obstacle insideS is based on the method proposed by Hormann and Agathos [32] and the criterion is expressed as (11)ini=1,ifSi∈S0,ifSi∉S,where Si is the ith obstacle, i=1,2,…,m. m is the number of obstacles within the workspace of manipulator.Figure 5
Observation, path prediction, and path planning.A is the object.B,C,D, andE 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 objectA′ and obstaclesB′,C′, andD′. (d) Planning feasible paths and the optimized path.
(a)
(b)
(c)
(d)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 x, y direction. Spline filter can be expressed as (12)I+β4Qωt=zt,where ω(t) is the filtered path containing n points. z(t) is the observed path with noises. Q and I are the n×n matrix. β is given by (13)β=2sin⁡πΔxsλc-1,where Δxs is the sample interval and λc is the cut-off length. More details about Spline filter are presented in [17].As shown in Figure5(b), the black curve is the real path. The orange points stand for the predicted points. The red points, smoothed via Spline filter, indicate initial motion of the object or obstacles, which are used to predict the following path. n-1 vectors (a1,a2,a3,…,an-1) can be formed by n points (Q1,Q2,…,Qn), and the angles of every two adjacent vectors are calculated by (14)θi∗=arccos⁡ai·ai+1ai·ai+1.The angleθi∗ (i=1,2,3,…,n-2) can be fitted with a second-order polynomial via least-square methods (15)θ∗i=aθ∗+bθ∗·i+cθ∗·i2,where aθ∗, bθ∗, cθ∗ are the coefficients. The future angular θ∗(i) (i≥n-1) is calculated by (15). The rotation matrix R, used to determine the direction of the next vector ai+1, is defined as (16)R=cos⁡θ∗i-sin⁡θ∗isin⁡θ∗icos⁡θ∗i.The weight Wi, introduced to calculate the size of the next vector ai+1, is expressed as(17)Wi=ai+1ai.Similarly, the weights are fitted by a first-order polynomial(18)Wi=aW+bW·i,where i is the ith point on the path and i=1,2,3,…,n-2. aW, bW are the scale coefficients.Based on (15)–(18), the following predicted vector, ai+1, can be calculated by (19)ai+1T=Wi·R·aiT,i>n-2.Then the following pointQi+1(xi+1,yi+1) can be obtained by (20). The predicted path is accurate and the simulation is presented in Section 4. (20)xi+1,yi+1=xi,yi+ai,i=n,n+1,…,n+k-1,where k 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′, andEE′, are the predicted paths of obstacles, respectively. The predicted pointA′ is the grasped position of the object. The pointsB′,C′,D′,E′ are the predicted positions of obstacles when the object A is grasped. According to the predicted positions of object and obstacles, we can plan the paths C1=path{O6-A}, C2=path{O6-B-A}, C3=path{O6-C-A}, and C4=path{O6-D-A} as the tracked paths as shown in Figure 5(d). However, the convex polygonOA′B′C′D′ will limit the joint rotation range, which may result in the object not being grasped along C1. Similarly, polygonsOA′C′D′,OA′D′ have the same influence on C2, C3 as shown in Figure 5(c).So, the convex polygons, which have influence on the paths, should be found out. Then, pointA′ is defined as the rotation center.A′O6 rotates clockwise around pointA′. LineA′O has set the finishing line for rotation. The obstacles withinS and the object can form polygonsOA′B′C′D′,OA′C′D′,OA′D′. For a convex polygon, each interior angle of the polygon is less than 180°. So, let Pj be the evaluation function for the convexity-concavity of an arbitrary polygon(21)Pj=∑k=1Kjmin⁡bk×bk+1,0,where j=1,2,…,N, N is the number of polygons. Kj is the number of angles for the jth polygon. bk, bk+1 are the adjacent edges of thejth polygon along clockwise or anticlockwise direction. If Pj is 0, thejth polygon is convex. Otherwise, it is not convex. As shown in Figure 5(c), polygonsOA′B′C′D′,OA′C′D′,OA′D′ are convex. Then, let Fj be the feasibility criterion for C1, C2, C3, C4. (22)Fj=Lj-OA′¯-∑p=16lp+δ,where Lj is the perimeter of thejth convex polygon which has an influence on the corresponding path. lp is the length of thepth link. δ is the estimated safe distance (δ = 220 mm), which ensures that no collision will happen when grasping the object. Andδ can be determined by simulation or experiment. If Fj is less than 0, the path is feasible. Otherwise, the path is infeasible.Based on (22), the feasible paths can be obtained (C2, C3, C4). 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), C2 is the optimized path due to C2<C3<C4. Based on the above analysis, thealgorithm of the optimized path can be briefly described in Figure 6.Figure 6
The algorithm diagram of the optimized path.


## 3.2. Obstacles Included in the Planned Path

Based onalgorithm of the optimized path, C2 is tracked by the end-effector of manipulator as illustrated in Figure 7(a). However, the end-effector 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 B is tracked at first in tracking the path C2. And then when the end-effector O6 moves on the edge of safe area, the object is tracked in real-time. Similarly, if any one of paths C3 and C4 is the optimized path, their tracked processes are the same to the path C2. Besides, when the end-effector O6 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=Δx,Δy,ΔΨT. A local coordinate based on the point O6 is established and the XB-axis is along lineO6A as shown in Figure 7(b). In the local coordinate, the planned path Δx,ΔyT can be expressed as ΔxB,0T, ΔxB=Δx2+Δy21/2. To avoid the obstacle, ΔyB is defined along YB-axis in (23). (23)ΔxB=Δx2+Δy2,ΔyB=-1τ·KB·ΔxB·cos⁡π·dB-dminRsafe-dmin+12,where KB is the scale factor. Rsafe is the radius of safe area. dB is the distance between point O6 and obstacle B in the safe area. dmin is the minimum distance. τ decides the direction around the obstacle (along clockwise or counter clockwise direction). τ is calculated as (24)τ=1,ifxA-xO6>00,Otherwise,where xA, xO6 are the positions of object A and end-effector along x-axis in Cartesian space. Considering that the object is tracked all the time when path C1 is the optimized path as shown in Figure 7(c). To reduce the length of obstacle avoidance path in safe area, τ is calculated by(25)τ=1,ifd·u×w≥00,Otherwise,where vector d=(0,0,1). Vectors u and w are shown in Figure 7(c). Then the planned path ΔxB,ΔyBT in local coordinateXBOBYB is mapped to Cartesian coordinateXOY by the rotation matrix Ro.(26)Ro=cos⁡φo-sin⁡φosin⁡φocos⁡φo,where φo is the rotation angle calculated by (27)φo=arccos⁡ΔxΔx2+Δy2.Figure 7
The principle of obstacles avoidance on the path. (a) The overall layout. (b) The end-effector moves around the obstacleB in real-time. (c) In the case that path C1 is the optimized path. (d) The LRCM diagram. U1in is ΔX; U1out is the updated path ΔX.
(a)
(b)
(c)
(d)Then the planned pathΔx,ΔyT will be updated by (28)ΔxΔy≔Ro·ΔxBΔyB.The real-time planned pathΔX is thus modified. And the LRCM can be described by a diagram as shown in Figure 7(d).


## 3.3. 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 are five critical points on each arm as shown in Figure7(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: (29)ΔΘ=J∗ΔΧ+kI-J∗J∑j=1Mkj∇Hj,where M is the number of obstacles. k(I-J∗J)kj∇Hj is the null space, which is used to avoid thejth obstacle. kj is a scale coefficient. k is defined as(30)k=2,ford≤dmincos⁡πd-dminRsafe-dmin+1,fordmin<d<Rsafe0,ford≥Rsafe,where Rsafe is the radius of safe area. d is the distance between critical point and obstacles in the safe area and dmin is defined as the minimum distance between the obstacle and the critical points.


## 3.4. 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 Figure8. 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.Figure 8
The synthetic algorithm diagram of tracking a moving object in multiple-dynamic obstacles environment.Uin=Xobj; Uout=Θ.


## 4. Simulations

### 4.1. 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 Figure9. The sample interval, Δxs, is 1 mm and cut-off length, λc, is 40 mm for Spline filter. The similarity degree between the real moving path and predicted path can be calculated as (31)SSI=1kSSI∑j=1kSSISSIj=1kSSI∑j=1kSSI1-dsjrs2,where SSI ranges from 0 to 1. If the value of SSI is closer to 1, the predicted path is more accurate. SSIj is similarity degree of thejth predicted point. kSSI is the number of predicted points. dsj is the distance between the predicting point and the real point at each instant t or position. rs (rs = 20 mm) is the maximum radius of allowable deviation.Figure 9
Comparison of the predicted path and the real moving path with noises for 20 points in the future. (a) Straight line:x=5j and y=5j. (b) Circle curve: x=R·sin⁡(0.03πj/16), y=R·cos⁡(0.03πj/16), and R=400. (c) Combined curve: x=0.05j2+2j and y=5j, j=0,1,2,…,39. Subfigures (d), (e), (f), (g), (h), and (i) show the similarity degrees of 20 experiments for each prediction. In subfigures, the value of SSIkSSI is the similarity degree of the last predicted point. SSI¯ and SSIkSSI¯ are the average values of SSI and SSIkSSI, respectively.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)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(g) are corresponding to Figure 9(a). Figures 9(e) and 9(h) are corresponding to Figure 9(b). Figures 9(f) 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.Table 3
The similarity degrees of the different motions for Figure9.
Similarity degree
Type
Straight line
Circle curve
Combined curve
S
S
I
¯
0.9969
0.9891
0.8043
S
S
I
k
S
S
I
¯
0.9906
0.9814
0.8890


### 4.2. 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 Figures10(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 tracking path. Figures 10(c), 10(d), 10(g), and 10(h) show that the artificial potential field cannot work well in this case. The reason is that the repulsive force Fref is along the radial direction of the obstacle so that the end-effector cannot turn to the right. The force Fref 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. (32)Fref=kr1d-1Rsafe1d2∂d∂xifd≤Rsafe0ifd>Rsafe,where x is the position vector of the end-effector, x=(x,y). d is the distance between critical point and obstacles in safe area.Table 4
Parameters for Figures9 and 10.
Parameters
R
s
a
f
e
V
o
b
s
t
a
c
l
e
V
o
b
j
e
c
t
K
B
d
m
i
n
k
r
Value
80 mm
8 mm/s
63 mm/s
5
30 mm
1.0 × 107Table 5
Parameters in the virtual controller.
Parameters
k
p
k
I
k
s
α
0
α
p
η
0
η
p
p
1
p
2
Value
0.005
10−7
1
10−3
10−2
300
100
0.02
0.02Figure 10
Avoiding the obstacle close to the end-effector by using LRCM in real-time. The initial joint anglesΘ=0.9,0.2,0.8,0.5,0.5,0.5T. (a), (e) Static obstacle by using LRCM. (b), (f) Moving obstacle by using LRCM. (c), (g) Static obstacle by using artificial potential field. (d), (h) Moving obstacle by using artificial potential field. Subfigures (a)–(d) show the movement of 3D model. Subfigures (e)–(h) show the movement of the simplified model.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)


### 4.3. 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 Figure11, 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 ∑ξj and the adaptive adjusting part γi. Besides, the time-consuming becomes less with the increasing of the speed of the object, due to the adaptive adjusting part γi. 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.Figure 11
Tracking the object with different moving speed.Ψ is the attitude of end-effector; Ψobj is the attitude of the object. (a), (b) In the case without the visual controller. (c), (d) In the case with the visual controller without the visual controller.
(a)
(b)
(c)
(d)


### 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 C1 is tracked in real-time as shown in Figure 5(d). In Figures 12(a) and 12(b), although the end-effector 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 Figures 12(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 C2 is planned. Figure 12(e) shows that the moving object is tracked in real-time along the optimized path C2. Figures 12(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 Figures 12(g) and 12(h). 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].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.8T. (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.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)


## 4.1. 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 Figure9. The sample interval, Δxs, is 1 mm and cut-off length, λc, is 40 mm for Spline filter. The similarity degree between the real moving path and predicted path can be calculated as (31)SSI=1kSSI∑j=1kSSISSIj=1kSSI∑j=1kSSI1-dsjrs2,where SSI ranges from 0 to 1. If the value of SSI is closer to 1, the predicted path is more accurate. SSIj is similarity degree of thejth predicted point. kSSI is the number of predicted points. dsj is the distance between the predicting point and the real point at each instant t or position. rs (rs = 20 mm) is the maximum radius of allowable deviation.Figure 9
Comparison of the predicted path and the real moving path with noises for 20 points in the future. (a) Straight line:x=5j and y=5j. (b) Circle curve: x=R·sin⁡(0.03πj/16), y=R·cos⁡(0.03πj/16), and R=400. (c) Combined curve: x=0.05j2+2j and y=5j, j=0,1,2,…,39. Subfigures (d), (e), (f), (g), (h), and (i) show the similarity degrees of 20 experiments for each prediction. In subfigures, the value of SSIkSSI is the similarity degree of the last predicted point. SSI¯ and SSIkSSI¯ are the average values of SSI and SSIkSSI, respectively.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)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(g) are corresponding to Figure 9(a). Figures 9(e) and 9(h) are corresponding to Figure 9(b). Figures 9(f) 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.Table 3
The similarity degrees of the different motions for Figure9.
Similarity degree
Type
Straight line
Circle curve
Combined curve
S
S
I
¯
0.9969
0.9891
0.8043
S
S
I
k
S
S
I
¯
0.9906
0.9814
0.8890


## 4.2. 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 Figures10(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 tracking path. Figures 10(c), 10(d), 10(g), and 10(h) show that the artificial potential field cannot work well in this case. The reason is that the repulsive force Fref is along the radial direction of the obstacle so that the end-effector cannot turn to the right. The force Fref 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. (32)Fref=kr1d-1Rsafe1d2∂d∂xifd≤Rsafe0ifd>Rsafe,where x is the position vector of the end-effector, x=(x,y). d is the distance between critical point and obstacles in safe area.Table 4
Parameters for Figures9 and 10.
Parameters
R
s
a
f
e
V
o
b
s
t
a
c
l
e
V
o
b
j
e
c
t
K
B
d
m
i
n
k
r
Value
80 mm
8 mm/s
63 mm/s
5
30 mm
1.0 × 107Table 5
Parameters in the virtual controller.
Parameters
k
p
k
I
k
s
α
0
α
p
η
0
η
p
p
1
p
2
Value
0.005
10−7
1
10−3
10−2
300
100
0.02
0.02Figure 10
Avoiding the obstacle close to the end-effector by using LRCM in real-time. The initial joint anglesΘ=0.9,0.2,0.8,0.5,0.5,0.5T. (a), (e) Static obstacle by using LRCM. (b), (f) Moving obstacle by using LRCM. (c), (g) Static obstacle by using artificial potential field. (d), (h) Moving obstacle by using artificial potential field. Subfigures (a)–(d) show the movement of 3D model. Subfigures (e)–(h) show the movement of the simplified model.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)


## 4.3. 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 Figure11, 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 ∑ξj and the adaptive adjusting part γi. Besides, the time-consuming becomes less with the increasing of the speed of the object, due to the adaptive adjusting part γi. 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.Figure 11
Tracking the object with different moving speed.Ψ is the attitude of end-effector; Ψobj is the attitude of the object. (a), (b) In the case without the visual controller. (c), (d) In the case with the visual controller without the visual controller.
(a)
(b)
(c)
(d)


## 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 C1 is tracked in real-time as shown in Figure 5(d). In Figures 12(a) and 12(b), although the end-effector 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 Figures 12(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 C2 is planned. Figure 12(e) shows that the moving object is tracked in real-time along the optimized path C2. Figures 12(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 Figures 12(g) and 12(h). 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].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.8T. (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.
(a)
(b)
(c)
(d)
(e)
(f)
(g)
(h)
(i)


## 5. 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.



---

*Source: 7310105-2017-04-30.xml*