# Collision-Free and Energy-Saving Trajectory Planning for Large-Scale Redundant Manipulator Using Improved PSO

Authors: Min Jin; Dan Wu
Journal: Mathematical Problems in Engineering
 (2013)

---

## Abstract

The large-scale boom system, such as the five-arm concrete pump truck with the arm length of 36–65 meters, usually operates in an unknown dynamic outdoor environment. The motion safety and the energy consumption are thus the two vital measurements to the effectiveness of the trajectory planning for the large-scale boom system. Due to the redundancy of the large-scale boom system and some drawbacks of the original particle swarm optimization (PSO) algorithm, an improved PSO algorithm is presented to solve the inverse kinematic problem of the redundant large-scale boom system. By the improved PSO algorithm, the energy-saving trajectory planning of the large-scale boom system that operates in a workspace without obstacles and with obstacles is optimized, which considers different important degrees of the subgoals, respectively. The optimal results from the simulation study and the practical application verify the effectiveness of the proposed planning strategy. At the same time, the performance of the improved strategy is compared with that of the traditional, and the superiority is further demonstrated.


---

## Body

## 1. Introduction

Owing to the agility of action, capability of obstacle avoidance, and excellent dynamic performance, redundant manipulators have attracted more and more attention and have been used widely in lots of fields, such as manufacturing and surgery. As they possess more degrees of freedom than those required to execute a given task, the inverse kinematics admit a number of solutions. Furthermore, when manipulator tasks are demanded in precision and diversity, these tasks’ execution might make the design of a trajectory planning of the redundant manipulator difficult because of the nonlinearity of the dynamics and the coupling between axes. How to obtain an optimal solution for the inverse kinematics problem of redundant manipulators has become a research focus.At present, there are mainly three methods, algebra method, geometrical method, and numerical iteration method, which are applied to the inverse kinematics of redundant manipulator [1–4]. Although these methods can solve the problem, they have their own disadvantage, respectively. Algebra method and geometrical method are only used to solve the simple redundant manipulators such as two-joint SCARA mechanical arm. The numerical iteration method can be utilized to obtain the solution for the intricate redundant manipulators, but the solution is unique and unreliable. Moreover, these methods only considered one object, namely, the end-effector of manipulator move from the initial position to the final desired precisely. The workspace obstacle avoidance is not discussed.Aiming at this problem, many researchers made a lot of research. Han and Piu presented the Tank-Hopfield network and J function to plan the trajectory of the redundant robot, so that the desired end-effector trajectory is tracked closely, the workspace obstacles are avoided simultaneously [5]. McAvoy et al. utilized a genetic algorithm (GA) to optimize the point-to-point trajectory planning of kinematically redundant manipulators [6]. Peng and Wei proposed the ASAGA trajectory planning method of redundant manipulators by incorporating the GA and simulated annealing algorithm [7]. Although these methods can obtain a superior solution for the kinematic of redundant manipulator, when number of the parameters to be optimized is large and the parameters are highly correlated, they can use more computing time and degrade efficiency to search the global optimum solution. Furthermore, some vital characteristics of the redundant manipulator such as safety, energy consumption, and stability of the manipulator may not be considered.The large-scale boom system of the five-arm concrete bump truck with the arm length of 36–65 meters is a representative 6-degree-of-freedom redundant manipulator. They usually operate in an unknown dynamic outdoor environment with overhead power transmission lines and field construction workers. Thus, the motion safety and the energy consumption are the two vital measurements to the effectiveness of trajectory planning for the large-scale boom system. In order to ensure the motion safety as well as reduce the energy consumption, an improved particle swarm optimization (PSO) algorithm which can overcome the drawbacks mentioned above is adopted to optimize the trajectory of the boom system in this paper.The paper is organized as follows. The kinematic description of boom system of concrete pump truck is given in Section2. Section 3 introduces the improved PSO algorithm. The PSO method is used to solve the inverse kinematic problem of the boom system in Section 4. Section 5 gives some cases study. The practical application is demonstrated in Section 6. The conclusion is drawn in Section 7.


## 2. Kinematic Description of Boom System of Five-Arm Concrete Pump Truck

Figure1 shows the boom system of five-arm concrete pump truck with 6 degrees of freedom. It is composed of five arms and a revolving table. The revolving table is fixed to the frame foundation with the screws and revolves on the z axis. The five arms and the revolving table are connected in a series with five revolute joints and each arm is driven by the arm cylinder.Figure 1
The boom system of concrete pump truck.According to the characteristic of the framework of the boom system, the mechanism of the boom system and the coordinate system of each arm are displayed in Figure2. It can be seen that the revolving table coordinate system and the first arm coordinate system are coincided with the reference coordinate system and the material outlet of boom system is set as the end coordinate system origin of the fifth arm. li(i=1,2,…,6) denote the length of each arm, θi(i=1,2,…,6) signifies the angle between the ith arm and the (i-1)th arm. Thereinto, l1 and θ1 are the length of revolving table and the angle between the first arm and the axis X1, respectively. For convenience, they are all set as 0; that is, the boom system becomes a planar five-bar linkages. Table 1 shows the kinematic parameters of each arm.Table 1
The kinematic parameters of each arm.
J
i
θ
i (°)
Interval (°)
α
i
-
1 (°)
a
i
-
1 (mm)
d
i (mm)
1
θ
1
0
−90
l
1
0
2
θ
2
[0, 90]
90
l
2
0
3
θ
3
[0, 180]
0
l
3
0
4
θ
4
[0, 180]
0
l
4
0
5
θ
5
[0, 200]
0
l
5
0
6
θ
6
[0, 240]
0
l
6
0Figure 2
Geometry of a six-degrees-of-freedom of joint anglesθi, i=1,2,…,6.Utilizing these parameters the transformation matrixes [8] which describe the relative position and gesture between two adjacent arms can be obtained and described as follows. 						(1)A0=[100000100-1000001],A1=[c10s10s10-c1001000001],A2=[c2-s20l1c2s2c20l1s200100001],A3=[c3-s30-l2c3s3c30-l2s300100001],A4=[c4-s40l3c4s4c40l3s400100001],A5=[c5-s50-l4c5s5c50-l4s500100001],A6=[c6-s60-l5c6s6c60-l5s600100001],
where Ai(i=0,1,…,6) denotes the relative position and gesture pose of the ith arm to the (i-1)th arm, ci(i=1,2,…,6) signifies cos(θi), and si(i=1,2,…,6) represents sin(θi). Thus, the position and gesture of the end of the fifth arm can be expressed as
(2)T7=A0A1A2A3A4A5A6=(nxoxaxpxnyoyaypynzozazpz0001),
where
(3)nx=c1c23456,ny=s23456,nz=-s1c23456ox=-c1s23456,oy=c23456,oz=s1s23456ax=s1,ay=0,az=c1,px=c1(l5c23456-l4c2345+l3c234-l2c23+l1c2)py=l5s23456-l4s2345+l3s234-l2s23+l1s2pz=s1(-l5c23456+l4c2345-l3c234+l2c23-l1c2)
in which c1=cos(θ1), s1=sin(θ1), c12=cos(θ1+θ2) and so on.


## 3. Review of Particle Swarm Optimization Algorithm and Improved PSO

### 3.1. Review of Particle Swarm Optimization Algorithm (PSO)

Particle swarm optimization algorithm is a new entrant to the family of evolutionary algorithms, which is capable of mimicking the social behavior of birds flocking and fish schooling in search for food and proposed by Kennedy and Eberhart [9]. Its basic concept is that the potential solutions are flown through hyperspace and are accelerated towards better or more optimum solutions. The algorithm is described simply as follows.Given a swarm ofN particles in a D-dimension space, the ith particle which is a feasible solution in an optimal problem is represented by the position vector xi=(x1i,x2i,…,xDi). The best previous solution which has been found is denoted by the particle pi=(p1i,p2i,…,pDi), and the current group’s best solution that is found by the neighborhood particles of the ith particle is signified by gi=(g1i,g2i,…,gDi). The velocity of the ith particle is represented as Vi=(v1i,v2i,…,vDi). At the iteration step t+1, the position of the ith particle is updated by the following equations:
(4)vdi(t+1)=w0vdi(t)+c1r1[pdi(t)-xdi(t)]+c2r2[gdi(t)-xdi(t)],xdi(t+1)=xdi(t)+vdi(t+1),
where i=1,2,…,N, d=1,2,…,D. w0, c1 and c2 are the positive constants, and r1 and r2 are two random parameters which belong to the interval [0,1]. Thus, according to (4), the iterative process will continue warm by warm until the fittest solution is obtained.


### 3.2. Improved PSO

The original PSO has been applied to all kinds of engineering fields widely because of its robustness and fastness in solving the optimization problems, but it still has some difficulties in searching the overall optimal solution and improving the computational efficiency. Jiang et al. have analyzed the relationship between the convergence and parameter selection of the standard particle algorithm [10]. And many scholars have proposed some methods to improve the performance of the standard PSO algorithm [11–13]. Although these modified PSO algorithms have improved the global search ability of the particles, the computation burden is increased. In order to ensure the real-time control and intelligent work of the boom system, an improved PSO algorithm is introduced.It is well known that the convergence property and performance of the standard PSO algorithm are influenced by many factors such as the selection of the nonnegative real parameterw, c1 and c2 [10]. Thereinto, the parameter w is called the inertia weight, which can adjust the velocity of the particle and enable the particle to search the new solution space. To enhance the particle’s global search ability, the positive constant w is modified using the following equation:
(5)w=w0×1+cos(t/Tmax)2,
where w0 is the initial value and is set as 0.729, t is the tth iteration step, and Tmax is the maximal iteration step number. And the maximal and minimal velocity of particles are set as
(6)Vmax=Xmax-Xmin2Vmin=-Vmax,
where Xmax and Xmin are the maximal and minimal boundaries of the parameter space.For the parametersc1 and c2 which is included in the second term and the third term on the right hand of (4), they represent the cognitive part of PSO which can enable the particle to change its velocity based on its own thinking and memory and the social part as the particle change its velocity based on the social-psychological adaptation of knowledge, respectively, and reflect the personal experience and the social experience of its own. When the personal and social experiences accumulated so far are overused the particle can be driven away from the local optimum. However, if they are not fully used, the convergence performance of the PSO algorithm is undermined. In addition, considering the working characteristic and environment of the of the boom system of the five-arm concrete pump truck, there can be a small number of local optimal solutions to the inverse kinematic problem. To make use of the personal experience and social experience of the particle and obtain the global optimal configuration of the boom system, the velocity of particle is updated by the following formula:
(7)vdi(t+1)=wvdi(t)+(1-|pdi(t)-xdi(t)|xdmax-xdmin)2×r1[pdi(t)-xdi(t)]+(1-|gdi(t)-xdi(t)|xdmax-xdmin)2×r2[gdi(t)-xdi(t)].


## 3.1. Review of Particle Swarm Optimization Algorithm (PSO)

Particle swarm optimization algorithm is a new entrant to the family of evolutionary algorithms, which is capable of mimicking the social behavior of birds flocking and fish schooling in search for food and proposed by Kennedy and Eberhart [9]. Its basic concept is that the potential solutions are flown through hyperspace and are accelerated towards better or more optimum solutions. The algorithm is described simply as follows.Given a swarm ofN particles in a D-dimension space, the ith particle which is a feasible solution in an optimal problem is represented by the position vector xi=(x1i,x2i,…,xDi). The best previous solution which has been found is denoted by the particle pi=(p1i,p2i,…,pDi), and the current group’s best solution that is found by the neighborhood particles of the ith particle is signified by gi=(g1i,g2i,…,gDi). The velocity of the ith particle is represented as Vi=(v1i,v2i,…,vDi). At the iteration step t+1, the position of the ith particle is updated by the following equations:
(4)vdi(t+1)=w0vdi(t)+c1r1[pdi(t)-xdi(t)]+c2r2[gdi(t)-xdi(t)],xdi(t+1)=xdi(t)+vdi(t+1),
where i=1,2,…,N, d=1,2,…,D. w0, c1 and c2 are the positive constants, and r1 and r2 are two random parameters which belong to the interval [0,1]. Thus, according to (4), the iterative process will continue warm by warm until the fittest solution is obtained.


## 3.2. Improved PSO

The original PSO has been applied to all kinds of engineering fields widely because of its robustness and fastness in solving the optimization problems, but it still has some difficulties in searching the overall optimal solution and improving the computational efficiency. Jiang et al. have analyzed the relationship between the convergence and parameter selection of the standard particle algorithm [10]. And many scholars have proposed some methods to improve the performance of the standard PSO algorithm [11–13]. Although these modified PSO algorithms have improved the global search ability of the particles, the computation burden is increased. In order to ensure the real-time control and intelligent work of the boom system, an improved PSO algorithm is introduced.It is well known that the convergence property and performance of the standard PSO algorithm are influenced by many factors such as the selection of the nonnegative real parameterw, c1 and c2 [10]. Thereinto, the parameter w is called the inertia weight, which can adjust the velocity of the particle and enable the particle to search the new solution space. To enhance the particle’s global search ability, the positive constant w is modified using the following equation:
(5)w=w0×1+cos(t/Tmax)2,
where w0 is the initial value and is set as 0.729, t is the tth iteration step, and Tmax is the maximal iteration step number. And the maximal and minimal velocity of particles are set as
(6)Vmax=Xmax-Xmin2Vmin=-Vmax,
where Xmax and Xmin are the maximal and minimal boundaries of the parameter space.For the parametersc1 and c2 which is included in the second term and the third term on the right hand of (4), they represent the cognitive part of PSO which can enable the particle to change its velocity based on its own thinking and memory and the social part as the particle change its velocity based on the social-psychological adaptation of knowledge, respectively, and reflect the personal experience and the social experience of its own. When the personal and social experiences accumulated so far are overused the particle can be driven away from the local optimum. However, if they are not fully used, the convergence performance of the PSO algorithm is undermined. In addition, considering the working characteristic and environment of the of the boom system of the five-arm concrete pump truck, there can be a small number of local optimal solutions to the inverse kinematic problem. To make use of the personal experience and social experience of the particle and obtain the global optimal configuration of the boom system, the velocity of particle is updated by the following formula:
(7)vdi(t+1)=wvdi(t)+(1-|pdi(t)-xdi(t)|xdmax-xdmin)2×r1[pdi(t)-xdi(t)]+(1-|gdi(t)-xdi(t)|xdmax-xdmin)2×r2[gdi(t)-xdi(t)].


## 4. Trajectory Planning of Boom System Based on Improved PSO

Because of the redundancy and difference of working requirement and environment, the improved PSO algorithm is adopted to solve the inverse kinematic problem of the boom system when the concrete pump truck work. The calculation procedure is shown in Figure3.Figure 3
The flowchart of the PSO.Assumed that the initial position of the material outlet of the boom system is represented by the vectorPini=[pxini,pyini,pzini], the initial joint angle is denoted by the vector θini=[θ0ini,θ1ini,…,θ5ini], the desired position and desired joint angle are represented by two vectors Pgoal=[pxgoal,pygoal,pzgoal], and θgoal=[θ0goal,θ1goal,…,θ5goal], respectively. The fitness function is designed according to the desired goal. All constraints and criteria are translated into penalty functions to be minimized and that are defined in the sequel. Consider the following:(1)
In order to reduce the loss of energy, when material outlet move from the initial positionPini to the desired position Pgoal, according to the characteristic of the boom system the variation of joint angle should be minimized. 									(8)f1(θ)=∑i=051(i+1)×[abs(θigoal-θiini)].(2)
For working convenience of boom system, the variation orientation of angle between two adjacent links should be concurrent; namely, the configuration of the boom system is flexible, the objective function can be described by the following the function:(9)f2(θ)=∑i=15λi[θi(t+1)-θi(t)]2,wheret and t+1 are two consecutive sampling instants, and
(10)λi={t1/2if(θi+1-θi)(θi-θi-1)<01otherwise,i=2,…,4.(3)
In order to avoid tilt of the concrete pump truck because of self-weight, the gravity center of the boom system should be close to the revolving table; namely,(11)f3(θ)=max(θ1).(4)
In order to avoid obstacle, the displacements between obstacle and each end points of all links should be maximized, and there is no interference between each link and the obstacle. The function can be represented as follows:(12)f4(θ)=max(di)i=1,…,6,wheredi represents the displacement between obstacle and the ith end point.(5)
In order to ensure that the material outlet of boom system can get to the desired final positionPgoal, the positional error Perror is defined as follows:
(13)Perror=(pxgoal-pxc)2+(pygoal-pyc)2+(pzgoal-pzc)2,
where Pc=(pxc,pyc,pzc) is the current position of material outlet of boom system.Thus, the fitness functionFfitness is defined as follows:
(14)Ffitness=Perror+αifi(θ),
where0<αi<1 denotes a weight factor which is used to represent the importance degree of different subgoals.


## 5. Simulation Study

Given that the length of the arm of the boom system isl0=0, l1=10, l2=9, l3=9, l4=9, l5=9, respectively. The initial joint angle is θ0ini=[0,60°,150°,150°,162°,150°]. The desired position is set as Pgoal=[30,20,0]. Utilizing the PSO method mentioned above, the inverse kinematic problem of the redundant boom system in a workspace with obstacles and without obstacles is solved. In addition, the number of particles is set as N=40, the maximal number of iteration step is set as Tmax=150, and the positive constants are set as w0=0.729, c1=1.49 and c2=1.49, respectively.


### 5.1. Improved PSO Performance in a Workspace without Obstacles

Generally speaking, the boom system of concrete pump truck usually works in a workspace without obstacles. Under different work conditions, when the performance of concrete pump truck considered is different, the attitude of the boom system can be different. In order to enable the boom system to work better, three different configurations of the boom system are optimized in this paper.When the outlet of the boom system move to the desired position and the different subgoals are also considered preferentially, the configuration of the boom system is different. Figure4 shows the optimal configuration with the minimal loss of energy. Figure 5 displays the optimal configuration with the tilt-avoidance. The optimal configuration which has considered all the subgoals is shown in Figure 6.Figure 4
The configuration with the minimal loss of energy.Figure 5
The configuration with the tilt-avoidance.Figure 6
The optimal configuration of boom system.From Figure4 it can be seen that when the loss of energy of the concreter pump truck is considered precedently the flexibility of the boom system is not optimal. The solution corresponding to the configuration in Figure 5 can avoid the tilt effectively, and the flexibility is superior, but the loss of energy is the maximal. The solution corresponding to the configuration in Figure 6 is the best, which consider, all the performance such as the loss of the energy, tilt-avoidance, and flexibility.In addition, from these figures it can be drawn that the improved PSO can optimize the multigoal trajectory planning problem. In other words, according to the different performance requirements of the boom system considered, the optimal configuration of the boom system, which considers different degree of different subgoal, can be obtained by the improved PSO algorithm effectively.


### 5.2. Improved PSO Performance in a Workspace with Obstacles

Considering two obstacles in the workspace, for a given initial configuration some arms of the boom system locate between two obstacles, the final configuration result of the boom system obtained by the improved PSO algorithm is shown in Figure7. It can be seen that the boom system can work safely and the material outlet can move to the desired position accurately. But the flexibility of boom system is bad. At the same time, it is also demonstrated that the improved PSO can solve the inverse kinematic problem with obstacle-avoidance.Figure 7
The configuration in a workspace with two obstacles.


### 5.3. Comparison Analysis of Improved PSO and PSO

In order to further illustrate the superiority of the improved PSO, the improved PSO algorithm and original PSO are used to solve the same inverse kinematic problem of the boom system in the workspace with the obstacle mentioned above. The optimal process obtained by these two algorithms is shown in Figure8. From the figure, it can be seen that the convergence velocity of the improved PSO is quicker than that of the PSO and the solution is also better. All these have demonstrated that the performance of the improved PSO is superior to that of the PSO.Figure 8
The optimization process obtained by the improved PSO and PSO.


## 5.1. Improved PSO Performance in a Workspace without Obstacles

Generally speaking, the boom system of concrete pump truck usually works in a workspace without obstacles. Under different work conditions, when the performance of concrete pump truck considered is different, the attitude of the boom system can be different. In order to enable the boom system to work better, three different configurations of the boom system are optimized in this paper.When the outlet of the boom system move to the desired position and the different subgoals are also considered preferentially, the configuration of the boom system is different. Figure4 shows the optimal configuration with the minimal loss of energy. Figure 5 displays the optimal configuration with the tilt-avoidance. The optimal configuration which has considered all the subgoals is shown in Figure 6.Figure 4
The configuration with the minimal loss of energy.Figure 5
The configuration with the tilt-avoidance.Figure 6
The optimal configuration of boom system.From Figure4 it can be seen that when the loss of energy of the concreter pump truck is considered precedently the flexibility of the boom system is not optimal. The solution corresponding to the configuration in Figure 5 can avoid the tilt effectively, and the flexibility is superior, but the loss of energy is the maximal. The solution corresponding to the configuration in Figure 6 is the best, which consider, all the performance such as the loss of the energy, tilt-avoidance, and flexibility.In addition, from these figures it can be drawn that the improved PSO can optimize the multigoal trajectory planning problem. In other words, according to the different performance requirements of the boom system considered, the optimal configuration of the boom system, which considers different degree of different subgoal, can be obtained by the improved PSO algorithm effectively.


## 5.2. Improved PSO Performance in a Workspace with Obstacles

Considering two obstacles in the workspace, for a given initial configuration some arms of the boom system locate between two obstacles, the final configuration result of the boom system obtained by the improved PSO algorithm is shown in Figure7. It can be seen that the boom system can work safely and the material outlet can move to the desired position accurately. But the flexibility of boom system is bad. At the same time, it is also demonstrated that the improved PSO can solve the inverse kinematic problem with obstacle-avoidance.Figure 7
The configuration in a workspace with two obstacles.


## 5.3. Comparison Analysis of Improved PSO and PSO

In order to further illustrate the superiority of the improved PSO, the improved PSO algorithm and original PSO are used to solve the same inverse kinematic problem of the boom system in the workspace with the obstacle mentioned above. The optimal process obtained by these two algorithms is shown in Figure8. From the figure, it can be seen that the convergence velocity of the improved PSO is quicker than that of the PSO and the solution is also better. All these have demonstrated that the performance of the improved PSO is superior to that of the PSO.Figure 8
The optimization process obtained by the improved PSO and PSO.


## 6. Practical Application

Figure9 shows the flowchart of control system that makes the boom system of the concrete pump truck works intelligently. Utilizing the data which describes the position of the outlet of the boom system and is received by the remote receiver and emitted by the remote controller along with the signals that the angle sensors acquire, the motion controller uses the improved PSO algorithm to plan the configuration of the boom system. When the angles between two adjacent arms are obtained, the displacement increment of each cylinder is acquired by the geometrical relationship between the cylinder and two adjacent arms. Accordingly, the variation of hydraulic oil volume of each arm cylinder is obtained. And the driver current value is calculated by the proportion of variation of the oil volume to the current increment. Thus, the arm cylinder of the boom system is driven by the actuator and the outlet of the boom system can get to the desired position.Figure 9
The flowchart of control system of the boom system.In order to verify the practicability of the control system that the improved PSO algorithm has been embedded in, an application test that plans the trajectory of the 46- meters five-arm boom system of concrete pump truck is done. In this test, the length of each arm isl1=9, l2=9, l3=8, l4=10, l5=10, respectively. There is no obstacle in the workspace and all subgoals are considered. The initial position of the outlet is (10, 0, 0), namely, the boom system, is not opened up and the desired position is (45.5, 1.5, 0).After running the control system, the schematic diagram obtained theoretically is shown in Figure10. The joint angle obtained is θgoal=[00°,141°,1752°,1722°,1752°,1752°] and the obtained position of the outlet is (45.47, 1.46, 0). In practice, the final configuration of the boom system is displayed in Figure 10. The position of the outlet is (45.12, 1.25, 0). It is deviated from the desired goal in a sort. This is mainly attributed to the gravity of boom system and the calculation error, but it is still satisfied with the working requirements.Figure 10
The schematic diagram of the boom system obtained theoretically.


## 7. Conclusion

In this paper, an improved PSO algorithm is proposed to solve the multigoal trajectory planning problem of the redundant large-scale boom system of concrete pump truck. Because of the drawback of the original PSO such as the low convergence velocity and the inexact solution, an improved PSO is introduced.Utilizing the improved PSO algorithm, the simulation result to the inverse kinematic problem which considers different performance requirements of the redundant boom system of concrete pump truck that works in a workspace without obstacles and with obstacles have been obtained. At the same time, the performance of the improved PSO is compared with that of the PSO. Furthermore, the improved PSO algorithm have been embedded in the controller and applied to the trajectory planning of the boom system of concrete pump truck. All results illustrate that the proposed strategy can plan a collision-free and energy-saving trajectory for the redundant large-scale boom system, and thus it has the practicability.



---

*Source: 208628-2013-12-10.xml*