Obstacle Avoidance Path Planning of a 4-DOF Weapon Arm Based on Improved RRT (RRT-H) Algorithm

To improve soldiers ’ combat capability, weapon arms have a good development prospect. However, due to special work scenarios and tasks, new requirements are exerted on. Based on the fast-expanding random tree algorithm (RRT), path algorithm optimization (RRT-H) is proposed for the path planning of weapon arms. Overall path optimization is achieved by reducing the local path length with a closer path point planning to the obstacle. In a complex environment, the RRT-H algorithm can avoid local traps by guiding the new path extension direction and exploring multiple different paths in the map. The superiority of this algorithm is veri ﬁ ed with 2D plane obstacle avoidance and path ﬁ nding simulation experiments. Compared to RRT ∗ , RRT ∗ smart, and information RRT ∗ , the RRT-H can obtain high-quality calculation results in a shorter time. After setting degrees of freedom (DOF) as that of variables, the algorithm is applied to the 4-DOF weapon arm, which con ﬁ rms an effective reduction to the 4-DOF weapon arm ’ s motion costs.


Introduction
As a kind of individual intelligent equipment, wearable weapon arms gain much academic attention since they can provide wearers with accurate striking ability and a fire cover, while reducing soldiers' training periods and training costs.Compared with traditional manipulators (mechanical arms) mainly used in manufacturing processes, wearable weapon arms are equipped with lethal weapons and coordinate with human beings in battlefield environments.A good pathplanning method can effectively improve man-manipulator cooperation and, therefore, soldiers' target tracking capacity and self-protection.For a weapon arm, it requires the good path planning to accurately avoid the human body, while efficiently aiming and tracking the target.Weapon arm researches are still at the initial stage, and there is no comprehensive theoretical support, so it is necessary to carry out relevant researches in this field.Current algorithms for path planning include artificial potential field method, ant colony optimization, genetic algorithm, A-Star (A * ), probabilistic roadmaps, and fast expanding random tree algorithm (usually referred to as RRT) [1][2][3][4][5][6][7][8].Not requiring a detailed map to find a path, the RRT algorithm has computing advantages over other algorithms in the case of a multidimensional path solution of a manipulator.Yet, due to its random space exploration, there may not be a unique final solution, and most probably, they are not the optimal solutions [9].In order to overcome these path planning limitations, many researches have improved RRT by optimizing various aspects such as the path search processes, path results, and path smoothness.The RRT * further repairs the path obtained by RRT, making the final path closer to the optimal solution [10].Informed RRT * limits the new expansion points in the space where new expansion points can optimize the existing path [11].Islam et al. [12] presented RRT * Smart, which limits new expansion points near the obstacles around the path, resulting in a further optimized path than those by those obtained by the former two algorithms.Burget et al. [13] introduced a bidirectional extended random tree search algorithm, which can better deal with the complex environment and improve the search efficiency.Zhou et al. [14] used the Dubins curve strategy to process the informed RRT * path and improved the path generated by the RRT algorithm.All of these improved RRT algorithms have been applied to many fields, manipulator path planning being one prominent area.Cai et al. [15] proposed an improved rapidly exploring RRT for manipulator path planning, taking the target point as the sampling point with a certain probability.The algorithm improved the directivity of path planning.To improve obstacle avoidance, the step size can be changed according to the number of obstacles in the surrounding environment [15].Similarly, Ma et al. [16] improved the advantage of RRT * in time and path, optimizing RRT * for the manipulator.You et al. [17] preserved the initial tree, took it as the basis of pathfinding, and applied it to 9-degrees of freedom (DOF) manipulator pathfinding, with a much improved the obstacle avoidance efficiency.Yang et al. [18] combined RRT with an improved artificial potential field, provided avoid-rapidly exploring random tree.This algorithm was applied to the path planning of the forestry harvesting manipulator, which improved the smoothness of the path and the calculation time [14].
As a peculiar subcategory in manipulators, path planning of weapon arms requires a shorter calculation time and a shorter path to reduce energy loss.To better adapting to weapon arms, RRT algorithm efficiency needs tailor-made improvements, with a further reduced calculation time and optimized final path results.This paper provides an algorithm optimization called RRT-H, which can be close to the optimal solution in a shorter time.RRT * , RRT * smart, and informed RRT * are selected for comparison.The path planning in 2D space is verified to prove that the RRT-H can obtain a more efficient path in less time and less points.In this algorithm verification operation, a 4-DOF weapon arm ensures that it does not affect soldiers in space, so it can be used as the main weapon arm structure.The 4-DOF weapon arm's path planning can effectively avoid obstacles along its movement with a shorter Euclidean distance of the end of the 4-DOF weapon arm simulation model.
In this paper, Section 2 introduces RRT and its improved algorithms, with RRT-H is introduced in more detail.Section 3 illustrates the performance of RRT * , RRT * smart, in-formed RRT * , and RRT-H in three maps, and the respective advantages and disadvantages.Section 4 proposes a tailor-made escape from local traps method is proposed, making up for a weakness in RRT-H.Section 5 conducts a simulation model of a 4-DOF weapon arm.Based on the 4-DOF weapon arm's characteristics.Section 6 first designs a collision detection model, and applies this 4-DOF weapon arm for the path planning simulation.Concluding remarks are made in Section 7.

RRT-Based Obstacle Avoidance Algorithms
This section introduces and analyzes RRT algorithms, with RRT * , informed RRT * , and RRT * smart being follow-ups, and the latter of which will be compared with optimized RRT-H algorithm.
2.1.RRT Algorithm.RRT algorithm is a sampling processbased search algorithm.Its random expansion process is shown in Figure 1.
The expansion principle is as follows: P start and P goal are the starting and ending points of the path.The point set T is used to store points of the tree, and T's first point is P start .The algorithm randomly generates P random in the map.The point closest to P random in T is marked as P near , which is P new is generated between P random and P near .If there is no collision between P new and P near , this P new will be added to T, and this P near will be marked as P parent to P near .When a generated P new is close enough to P goal , the new path will take the P new and P goal as the last two points, and P goal 's P parent is the P new .Starting from the P goal , the path is generated by finding each point's parent until the P start is found.
Despite the great advantages of higher dimension obstacle avoidance, the path generated by the basic RRT algorithm is usually tortuous, with the possible failure of an optimal solution in the environment, a lengthened time, and unstable results.

Improved RRT Algorithms.
To overcome the abovementioned weaknesses, many studies [9][10][11][12]13] are being conducted to improve the RRT algorithm.RRT * is to add paths and points to optimize the original RRT-generated path (Figure 2).
After generating a new P new , P parent , and P next will be selected to reduce the path cost.The points around P new in T are marked as potential P parents .The path length of each potential P parent as parent to P new are calculated and used to select the shortest P parent as the best P parent .Again, points around P new in T are marked as potential P next s.By calculating the path length of each potential P next and comparing the path of the potential P next with the original parent point, the shortest path will be selected, and this P new will be updated as the parent point to P next .

Mathematical Problems in Engineering
Informed RRT * and RRT * smart further optimize the RRT * algorithm, both intelligently selecting the P random (Figure 3).Simulational result from informed RRT * is shown in Figure 3(a).Informed RRT * algorithm immediately calculates the path cost as soon as it obtains the first path.Path cost sum from subsequent P random -P start and from the subsequent P random -P goal should not be higher than that of the existing path.In this way, a space is obtained, and the generated points in the space can optimize the path.For RRT * smart, after obtaining the first path RRT * smart algorithm simplifies the path.With reference to simplified results, it then analyzes the obstacle position according to the simplification results and generates a new P random near the obstacle.RRT * smart simulation is shown in Figure 3(b).Both of them further optimize RRTRRT * algorithm but increase the calculation cost and slow down the pathfinding speed.
To maintain the previous advantages and overcome disadvantages, once the first is found, it will be directly and immediately optimized in RRT-H.It functions as if placing a curve between obstacles; stretching the two ends of a line will shorten the path within (Figure 4).
It takes three steps of optimization processes, and step one is illustrated in Figure 5.
As is shown in Figure 5, the first step is to optimize the existing path.Step 1 takes three consecutive points: P i , P i + 1 , and P i + 2 in the existing path.Multiple points distributed along the lines of P i + 1 and P i + 2 , respectively, are marked as P Ts .P Ts are connected with P i to generate connection lines L Ts .According to the lengths of L Ts , step one evenly distributes point P Cs on each L T .If any of P Cs has no collision with the obstacles, delete P i + 1 and the path between P i ,P i + 1 , and P i + 2 , connect P i and P i + 2 as the new path, mark P i as a new If there is a collision in P T (n), mark P T (n − 1) as P i + 1 , i = i + 1.The optimization operation goes on.Reaching the P goal , the first step of optimization is over.
Then, starting from the P goal , the second round of path optimization is carried out in the same way.The first two steps lay a solid foundation for the third step, optimization.The third step is shown in Figure 6.
As is shown in Figure 6, step three selects three consecutive points P i , P i + 1 , and P i + 2 in the path, similar to that of the first two optimization steps.The path of P i + 1 and P i + 2 generates multiple points P T , which are, respectively, connected with P i , and then the points P Cs are evenly distributed on L T .Collision detection is carried out for each P C .Assuming that the first collision position is the P C (m) on L T (n), and L T (n) is the path between P T (n) and P i , enter P C (m) of L T (n − 2) into the path set, and mark P C (m of L T (n − 2) as P i , P T (n) as P i + 1 , then optimize P i , P i + 1 , and P i + 2 again.Distribution gaps between detection points on the path make it liable for sharp obstacles permeate in between, as is shown in Figure 7, which gives misjudgment of "no collision."In this case, the optimization will fall into an endless loop, and the distance between P i and P i + 1 will be reduced close to 0.

Mathematical Problems in Engineering
In order to avoid this problem, the judgment points distribution should be set as dense as possible.In order to avoid the occurrence of extreme conditions when the density of judgment points distribution is small enough, k is set as the minimum distance permitted between P i and P i + 1 .If the calculated distance between P i and P i + 1 is less than k, then i = i + 1.Meanwhile, the path passes through the sharp parts of the obstacle.An endless loop is thus avoided, and a new path optimization starts, until getting the final optimal solution.The calculation time of the algorithm will increase with the increase of P C density and the decrease of the k value.A balanced distribution density of P C and a value of k are helpful for the algorithm to achieve satisfactory results (Figure 7).According to the obstacle avoidance strategy of weapon arms in this paper (Section 5), there will be a certain fault tolerance space between the edge of obstacles and the human body.The method for jumping out of the endless loop will not affect the obstacle avoidance effect if the distribution density of points used for collision detection is reasonably planned.

Comparison of Path Planning Effects
With referring to the maps used in other papers [19][20][21][22], three kinds of maps are designed in this paper, which are C-shaped, spiral-shaped, and fence-shaped, respectively.These maps in this paper are only used to prove the algorithm's superiority, ignoring map complexity influences (Figure 8).

Mathematical Problems in Engineering
In this paper, the algorithms are compared in terms of calculation time, number of P random s, and the generated path length.Both RRT and RRT-H algorithm in this paper stop the calculation after finding the path, while RRT * , RRT * smart, and informed RRT * stop the calculation after reaching 2,000 steps.Figure 9 displays the results of the five algorithms in the above three different environments.
As is shown in Figure 9, all algorithms can obtain a basic path in a short time; the paths of RRT * smart and RRT-H are shorter, that of RRT * and informed RRT * algorithms being approximately longer.In the above three types of maps, the first path obtained by informed RRT * is large.According to the principle of informed RRT * algorithm, the optimization space is planned according to the first path's length.In the above three maps, the optimization space covers almost the entire map.Therefore, RRT * and informed RRT * would show similar performances within 2,000 steps.Thus, the informed RRT * algorithm does not have any obvious advantage in the tortuous path planning.
The path cost of RRT * , RRT-H, RRT * smart, and informed RRT * vary with the number of P random s (Figure 10(a)).In order to highlight the advantages of the informed RRT * , new P start and P goal are defined, and the path planning effect is shown in Figure 10(b).
RRT-H algorithm does not need a new P random once it finds a path.In the above simulation experiment, the RRT-H algorithm can reach the last P random within 200 points without any new P random .Also, the RRT-H algorithm has the smallest P random number in the four algorithms.RRT-H has a longer path than the other three before optimization, while a shorter one after optimization.
Because the asymptotic optimality of the RRT * , RRT * smart, and informed RRT * , the longer the time consumed, the better the result they have.The time consumption of RRT * smart, informed RRT * , and RRT-H here is compared with that of informed RRT * algorithm, to show the relationship between their respective path cost and time.Based on RRT algo, the RRT-H algorithm has a high cost at the beginning and gradually decreases with the three-step optimization.After the optimization, it can quickly exceed the other three ones and reaches a smaller path cost (Figure 11).
Adopting the common practice, each algorithm takes ten groups of data for analysis.Of all four algorithms, the average path cost and time consumption are obtained from 10 experimental data groups and compared within the same map.Then, RRT-H result histograms are compared with the 2,000 steps results of the other three algorithms results.RRT-H algorithm can optimize the path to a better result in a shorter time compared with other algorithms (Figure 12).

A Tailor-Made Escape from Local Traps by RRT-H
RRT * , RRT * smart, and informed RRT * have the ability to avoid local traps.In particular, the informed RRT * algorithm can effectively jump out of local traps and obtain globally optimal solutions.RRT * algorithm is not limited by local traps due to Mathematical Problems in Engineering P random s, but it needs a large number of P random to complete path comparison.RRT * smart is based on the paths found by RRT * .It is easier to fall into local traps, but there is still a certain probability to jump out of local traps due to the probability distribution of P random s out of optimization space.Because the RRT-H algorithm directly optimizes the existing path, it is easy to fall into the local traps, so it optimizes the P random s generating method here (Figure 13).
As is shown in Figure 13, the points on the existing path (before optimization) are entered into the path club.When a new P random is generated, this P random will be filtered according to the probability.The method searches a range near the    Mathematical Problems in Engineering P new .When the number of points in the path club is large, the new P random is invalid, and it generates a new P random again.The optimization effect of this method is shown in Figure 14.The algorithm will plan scattered multiple paths, which improves the map exploration efficiency and avoids falling into local traps to a certain extent.
Because RRT-H is easy to fall into local traps, the problem can be effectively avoided by this escape method in a complex environment, which can provide a better path planning effect when applied to the weapon arm.

Path Planning Modeling of a 4-DOF Weapon Arm
This 4-DOF weapon arm model is based on preliminary researches in the Lab for Individual Equipment Technology, Nanjing University of Science and Technology.According to weapon arm requirements, 4-DOF is defined to provide the weapon arm with an omnidirectional strike capability (Figure 15).As long as a target point is on the outside ballistic track, the 4-DOF weapon arm can hit the target, regardless of the position and posture of the weapon itself to which a weapon arm is attached.Therefore, the weapon system at the end of the 4-DOF weapon arm is regarded as a translational DOF, and the 4-DOF weapon arm's joints are regarded as rotational DOF.When the end of a translational DOF coincides with the target position, the position and posture of the weapon arm are regarded as the achievable strike posture.The arm can thus be regarded as a 5-DOF manipulator for kinematics modeling.The 5-DOF manipulator model is only used for the path planning algorithm here, and for other situations, such as actual prototypes, it is a 4-DOF model.
In this paper, the weapon arm space posture is described with the method of modified Denavit-Hartenberg (D-H) parameters.The D-H coordinate modeling is shown in Figure 16, and the D-H parameters are shown in Table 1.Then, the kinematics simulation model of the 4-DOF weapon arm is constructed and adopted for obstacle avoidance path planning, together with the obstacle position information.

RRT-H-based 4-DOF Weapon Arm Path Planning Algorithm
In order to apply the RRT-H algorithm to the 4-DOF weapon arm, algorithm variables are increased from 2 in the 2D space to 5 in the weapon arm simulation model.A collision detection model is set, aiming at the working conditions of the 4-DOF weapon arm.Simulation results confirm that the algorithm can be applied to an efficient path planning of the 4-DOF weapon arm.Mathematical Problems in Engineering human body, it is unnecessary to build a detailed human body model as avoidance obstacles.The weapon arm is simplified as a lever structure without volume, and the human body shape is simplified as a bounding tree composed of multiple regular cuboids.The whole obstacle avoidance issue thus is simplified as a lever-cuboid collision detection.Considering the safety and its volume ignorance in the weapon arm simplification, the cuboid representing the human body is moderately extended a certain distance larger than the actual human body.The collision simplification method is shown in Figure 17. Figure 18 is the schematic diagram of the collision detection principle.The midpoint P O of each cuboid face was taken as the starting point.Each face generates a vector perpendicular to the face and pointing to the outside of the cuboid.P Cs on the manipulator are used as collision detection points and are taken as the vectors D Ss' starting points, with the direction P O-P C .The dot products P S of S S and D S are used to judge whether there is a collision as follows: where m is the number of points in P C .Only when col = 6m, there is no collision between the weapon arm and the human body in this posture.Given that the weapon arm is equipped with lethal weapons at its end and that it coordinates with the human wearer, the human body must be avoided, and its gun muzzle should never point at the human body in its operation process.Therefore, it is necessary to consider extending a certain distance forward at the gun muzzle.Since the length of the weapon arm in the model is short, 1 m is taken in front of the muzzle to add 10 points as P C to participate in the collision detection in the path planning process (Figure 19).The algorithm transforms the path from the folded state to the target-directed state by setting the same target position.The algorithm is applied to the path planning of a wearable weapon arm system, and eight path plannings are performed.The paths are then subjected to interpolation and smoothing.The resulting path is shown in Figure 20.
As shown in Figure 20, the RRT algorithm has some randomness in its computation results, while RRT-H is optimized and has relatively more stable results.The average cumulative angle changes of the 4-DOF in the path for RRT are 181.28,172.57, 212.57, and 140.38, respectively, while for RRT-H, they are 88.38, 69.16, 92.00, and 93.89, respectively.RRT-H exhibits significantly higher path quality compared to the traditional RRT algorithm.The average optimization degree for the 4-DOF is 51.24%, 59.92%, 56.72%, and 33.11%, respectively, resulting in an overall optimization effect of 51.4%.The optimization effect is evident.In the workspace, the planning effects of the two path planning algorithms for multi-DOF manipulators are shown in Figure 21.It can be concluded that the RRT-H algorithm is meaningful for obstacle avoidance planning in multi-DOF robotic arms.

Conclusion
In this paper, a new RRT optimization algorithm (RRT-H) is proposed.It has obvious advantages for the environment with obstacles.RRT-H can effectively improve the pathfinding efficiency and obstacle avoidance in the given three maps.It can find the shortest obstacle avoidance path in a short time, and can avoid the local traps by tailor-made method in a complex environment.The algorithm is applied to a 4-DOF weapon arm system, which can effectively reduce the path search time and path length.The RRT-H algorithm in this paper caters to the rapid reaction needs of weapon arms with their better integration.Yet, due attention must be paid to the simplicity of the maps in this paper.RRT-H algorithm may process meaningless computations within complex maps, owing to numerous judgment points generated on the path.Increased time cost becomes inevitable.In future studies, more complex environmental information will be considered when generating collision detection points.Improvements should be directed at    Mathematical Problems in Engineering RRT-H's adaptability and computational efficiency for different maps.Besides, RRT-H only considers the muzzle safety, so further studies are called for to deal with issues like more complex weapon arm characteristics and the ejecting process.
In further research, the algorithm will optimize the Prandoms generation position and collision detection point distribution, further improve the algorithm's speed and the complex environment adaptability.

FIGURE 6 :
FIGURE 6: The schematic diagram of the third step optimization.

FIGURE 7 :
FIGURE 7: Sharp obstacles permeation between two detection points.

FIGURE 10 :
FIGURE 10: (a) Path costs against the number of P random s and (b) path planning effect with new P start and P goal .

FIGURE 11 :
FIGURE 11: Relationships between path cost and time consumption.

FIGURE 12 :
FIGURE 12: Path cost and time cost histograms of different algorithms.

6. 1 .FIGURE 13 :FIGURE 14 :
FIGURE 13: The escaping method from local traps.* The P random in (a) is invalid, and the P random in (b) is added to T.

FIGURE 16 :
FIGURE 16: D-H coordinate modeling of the 4-DOF weapon arm.

FIGURE 18 :
FIGURE 18: Schematic diagram of the collision detection principle.

FIGURE 21 :
FIGURE 21: Simulation effects of RRT and RRT-H.