Research on Path Planning of Substation X-Robot

: In this paper, according to the environment of the robot in the substation, firstly, the D-H mathematical modeling of the UR10 manipulator is built and the forward and inverse kinematics of the UR10 manipulator are solved by studying the basic knowledge of the manipulator. Secondly, aiming at the problems existing in the traditional artificial potential field method, the improved artificial potential field method is studied and the principle that the link of the manipulator can not collide with obstacles is studied. The collision detection algorithm is added and the collision detection of cylindrical bounding box is designed. Then according to the starting position of the end of the manipulator and the position of the obstacle, the moving path of the end of the manipulator is determined. Finally, the improved artificial potential field method is used in MATLAB to simulate the moving path of the UR10 manipulator


Introduction
In recent years, the mobile robot technology has been widely applied in industry, agriculture, military, space and marine exploration and many other fields. The path planning is an important aspect of robotic research. Meanwhile, it is one of the most important task of robot navigation [1]. In the non-destructive testing of GIS equipment in substations, the path planning of manipulators of mobile robot plays a crucial role. Path planning is the safety guarantee for mobile robots to complete tasks, and it is also an important indicator of the degree of intelligence of mobile robots. Especially in the case that the accuracy of the robot hardware system can not be solved in a short period of time, the research on the path planning algorithm is particularly important [2].
At present, there are many methods to solve path planning, such as A* algorithm, D* algorithm, artificial potential field method, genetic algorithm, particle swarm optimization algorithm [3][4][5]. In this paper, the improved artificial potential field method is used to solve the obstacle avoidance problem of mobile robot in substation. Artificial potential field method is a more mature and efficient real-time route planning method, in the mid-1980s by Khatib first proposed [6]. As the artificial potential field method is simple and realtime, it is widely used in local planning and real-time planning of robot [7]. However, the traditional artificial potential field method also has its own limitations [8]: easy to fall into the local minimum point; in the vicinity of obstacles there is jitter phenomenon; frequent fluctuations in the narrow channel and so on [9]. In view of these problems, many scholars have proposed the improved method that the traditional artificial potential method is combined with other optimization algorithms [10][11]. In order to solve this problem, a new repulsion potential field function is established and the distance between the robot and the target point is taken into account in this paper. The experimental results show that this method can effectively solve the obstacle avoidance problem of mobile robot in substation and greatly improve the disadvantages of traditional artificial potential field method.

Kinematics Model of Manipulator
Because of the special working environment of the manipulator in the substation, UR10 manipulator is selected as the manipulator of the mobile robot. Compared with the traditional manipulator, UR10 has the advantages of large dead weight ratio, easy operation, high safety, simple programming and so on. In order to facilitate the research and Simulation of manipulator, mathematical modeling of UR10 manipulator is firstly established in this paper.

Position Description
In a certain coordinate system {A}, the position of any point in the space can be described by a 3 × 1 column vector A P .
In the formula, x p , y p and z p represent the coordinates of the point in the coordinate system {A}. The superscript A of A p represents the coordinate system {A} and A p is called the position vector [12].

Pose Description
In order to better describe the points in the space, it is also necessary to describe the pose. B is considered a rigid body. In order to describe the orientation of B, another coordinate system {B} can be fixed to the rigid body and then the coordinate system {B} with three unit principal vectors B x , B y , B z relative to the direction cosine of the reference coordinate system {A} composed of 3 × 3 matrix to describe [13]. 11 12 13 21 22 23 31 32 33 A B R is called the rotation matrix [14], the superscript A represents the reference coordinate system {A} and the subscript B represents the the coordinate system{B}.

Position and Pose Description
The three vector n , o , a describes the coordinate system F at the origin of the fixed coordinate system. n , o and a represent normal, directions and proximity vectors respectively [15]. The expression of coordinate system in another coordinate system is shown in figure 1. The coordinate system F is expressed by three vectors in the matrix: Figure 1. The expression of one coordinate system in another coordinate system [15].
In order to study the motion of a manipulator, it involves not only the posture relationship between its links, but also the relationship between it and the environment objects (obstacles and operating objects). We regard every link, object and obstacle of the manipulator as a rigid body. To fully describe the position and pose of rigid body B in space [13], we can attach a coordinate system {B} to rigid body B. Therefore, in order to determine the position and pose of a rigid body in a fixed coordinate system, only the rigid body in the coordinate system is expressed in space. [ ]

D-H Modeling
In 1955, Denavit and Hartenberg proposed a representation and modeling method for robots [16]. The D-H model can be used in any robot configuration. The joints of the robot can be sliding or rotating, the joints can be in any plane and the length of the joints is arbitrary.
Because the manipulator is UR10 manipulator and all the joints are rotating joints, so only the rotating joints are analyzed here. i θ is a joint variable, 0 i a = and the remaining two are related parameters for this joint, so if the values of the four parameters are known, the D-H matrix of the whole link would be determined.
For a six-degree-of-freedom manipulator UR10, 1 6 i T − can show the relationship between the coordinate system at the end of the manipulator UR10 and the i-1 coordinate system of the link rod. The expression is shown in formula (6): The linkage transformation formula can be calculated by the formula (7).
The expression of the end coordinate system of the UR10 manipulator relative to the reference base coordinate is shown in formula (8): 6 1 2 3 4 5 6

T A A A A A A
Therefore, for a manipulator with n degrees of freedom, in addition to establishing the standard orthogonal Cartesian coordinate system of each joint, it is necessary to establish a reference coordinate system on the base of the manipulator, so the manipulator has n+1 coordinate systems [17].
If the position of each sub-coordinate system is selected at the end of each rod, then the coordinate system can move with the link rod. Therefore, the position and pose of the coordinate system at the end of the manipulator relative to the basic coordinate system can be described by the transformation between n coordinate systems. The robot parameters of UR10 is shown in figure 2 and the D-H coordinate system of the UR10 robot is shown in figure 3. The D-H coordinate system parameters of the manipulator is shown in table 1.

Forward Kinematics of UR10 Manipulator
According to the formula of linkage transformation, we figured out the homogeneous transformation matrix. The results is shown in formula (9): The relationship between the coordinate {6} of the end of the manipulator and the reference datum {0} of the manipulator is calculated from above formulas.

T T T T T n o a p
The parameters in the matrix are shown in formula (11).

Inverse Kinematics of UR10 Manipulator
For a 6-dof robot, the kinematic inverse solution only has a closed solution in some special cases and the following two conditions must be met: (1) Three adjacent joint axes intersect at one point.
(2) Three adjacent joint axes are parallel to each other. The UR10 robot satisfies second conditions, so the Pieper method can be used to solve its closed solution. When these joints 2, 3 and 4 are parallel to each other, the kinematic equation of the robot can be expressed as follows: The inverse kinematics solution is divided into three steps: firstly, 1 θ , then 2 θ and 3 θ , and finally 4 θ , 5 θ and 6 θ .
The UR10 robot is solved by algebraic method and the joint variable is calculated by using the formula (13).
If the pose of the end link is given, the column vectors n , o , a and p are known. Then the joint variables, 1 The solution formula (14)

Traditional Artificial Potential Field Method
The traditional artificial potential field method is mainly applied in two-dimensional space. Firstly, the initial position of the robot when it moves is assumed to be [ , ] T X x y = and the final target position is assumed to be The gravity and repulsion functions corresponding to the whole moving environment are as follows: The gravitational potential field function of the target point on the manipulator is shown in equation (15): In the formula, att k refers to the scaling factor, whose value is always positive; g ρ is the Euclidean measure from position X to position G X . The gravity ( ) att F X of the corresponding target point on the manipulator is the negative gradient of the gravitational potential field function, as shown in equation (16): The formula (16) shows that: as the robot gets closer and closer to the position of the target point, its gravity is getting smaller and smaller. When the robot moves to the position of the target point, its gravity converges to 0.
The gravitational potential field function of the target point on the manipulator is shown in equation (17): In the formula, rep k refers to the scaling factor, whose value is always positive; ρ is the minimum distance from the position X to the position of the obstacle and 0 ρ is the influence distance of the obstacle. The repulsive force of the corresponding obstacle on the manipulator is the negative gradient of the repulsive force potential field function, as shown in equation (18): The formula shows that the repulsive force on the robot is greater and greater as the robot gets closer to the target point. When the robot gets closer to the obstacle, the repulsive force on the robot is infinite.
In the process of robot movement, according to the gravity and repulsive force at a certain moment, the resultant force at this moment can be calculated, as shown in equation (19):

Based on Improved Artificial Potential Field Method
In the traditional artificial potential field method, the repulsion potential field function increases faster than the gravitational potential field function, which finally leads to the robot moving away from the target point under the action of resultant force. In order to solve the local minimum problem in the traditional artificial potential field method, a new repulsion potential field function needs to be established and the growth rate of this function needs to be slower than that of gravity. It is found that this problem can be solved by taking the distance between the robot and the target point into consideration.
The above method is the study of two-dimensional space. Because the movement of the manipulator is generally not in a plane but in a three-dimensional space, so the following is an improved artificial potential field method in threedimensional space: (1) completely encircling obstacles with the smallest cube; (2) When the length of the long side of the cube is not more than twice the radius of the manipulator, the diagonal of the cube is used as the outer ball of the cube.
(3) When the long side of the cube is more than twice the radius of the manipulator, the cube is divided into two smaller cubes by using the plane passing the midpoint of the four long sides.
(4) Release the information of each small cube which does not contain obstacles, repeat the process of (2), (3) for each remaining cube, and get the obstacles surrounded by the outside ball.
(5) When there is an overlap between the receivers, the two receivers are still treated as two separate receivers.
The above handling method can effectively solve the shortcomings of the traditional artificial potential field method, which also solve the planning problem of end of the manipulator in the whole avoidance obstacle path planning.

Collision Detection
Collision detection technology is an important part of robot in the path planning simulation system. It is the main task to judge whether two objects have collided or shared the same space area. The rapidity and effectiveness of collision detection will be the key to the success of path planning simulation system. Common collision detection methods (bounding box method) include bounding sphere [19], bounding box AABB [19] (cube bounding box), directional bounding box (OBB) [20][21], K-DOP bounding box [14].
In this paper, because the mobile robot works in a substation, most of the equipments in the substation are cylindrical GIS equipments and the manipulator is also cylindrical shape. Therefore, due to the working environment of the manipulator, this paper selects cylindrical bounding box.
The manipulator and the GIS equipment is surrounded by a cylindrical bounding box, so the collision detection between the manipulator and the GIS equipment can be transformed into the collision detection between two cylindrical bounding boxes. Then the collision detection between two cylindrical bounding boxes can be transformed into the collision detection of two line segments with two cylindrical bounding box axes. The collision detection of two line segments can be divided into two line coplanar and two line different surface. First, this paper assume that the two line segments are line segment AB and CD. ( ) , , x y z are the end coordinates of two line segments. When two line segments are not on a plane, the distance between two straight lines is first calculated, which is shown in Figure 4. Two the distance formula between AB and CD of different faces is shown in formula (21): PQ is perpendicular to line AB and CD, where P and Q are vertical feet. PQ is the distance between line AB and CD.
, the shortest distance between two lines can be calculated, as shown in table 2: When two line segments AB and CD are coplanar, they can be divided into two cases: line segments AB and CD intersect or disintersect. When they intersect, the shortest distance is 0. It indicates that the two cylinder bounding boxes collide; when they do not intersect, the two line segments are parallel. According to the formula (22), the vertical distance T L of two line segments can be calculated.

Kinematics Simulation Analysis of Manipulator
In this paper, MATLAB robot toolbox is used to verify effectiveness of the algorithm and compare the advantages between the new algorithm and the traditional algorithm. Then, according to the actual working environment, the improved artificial potential field method and the cylinder bounding box are used to simulate the avoidance obstacle path planning of the manipulator.

Obstacle Avoidance Simulation Based on Improved Artificial Potential Field Method
The improved artificial potential field method is used to simulate the avoidance obstacle path planning in Matlab. The result is shown in figure 5: In the figure 5, the initial position coordinates are (0, 0, 0), the target position coordinates are (3,3,3), the obstacle position coordinates are (1, 1.5, 1.5), and the obstacle radius is 1. The Fig. 5 shows that end of the manipulator can be used to avoid the state of the local minimum when using the improved artificial potential field method simulates the avoidance obstacle path planning of end of the manipulator.

Matlab Simulation Based on UR10 Manipulator
The improved artificial potential field method can accomplish the task of avoidance obstacle path planning by the above Matlab simulation. The robot toolbox is used to simulate the manipulator. Firstly, according to the geometry and working environment of the manipulator, the manipulator is used to build model in Matlab. Then using the improved artificial potential field method completes the avoidance obstacle path planning of the end of the manipulator. In this paper, according to the special working environment of the manipulator, the typical situation is analyzed as follows: Let the starting position of the manipulator be (0.05, 0.10, 0.16) and the coordinates of the target position is (-0.35, -0.35, 0.6). The results of the improved artificial potential field method are as follows:  The cylinder represents the cylindrical GIS equipment in the substation and the curve represents the motion path at the end of the manipulator.

Results and Discussions
The simulation results show that the end of the manipulator is able to avoid obstacles and reach the target location. According to changing value of each joint angle of the manipulator, the sixth joint angle of manipulator has not changed, which keeps the position of end of the manipulator constant and can guarantee that testing equipment of end of manipulator remains inactive in the process of mobile. The effectiveness of the algorithm is verified by the fact that the manipulator can reach the target position accurately.
However, from the planning of the end path of the manipulator to the realization of the manipulator's movement according to the planned path, there are many steps, such as: path planning of the manipulator, discretization of the planned path and calculation of the joint angle when the end of the manipulator is at each point, input the joint angle value into UR simulator to generate motion files. The parts are imported into the control system of the manipulator and the manipulator moves along the planned path. These steps are not automatically and coherently executed. In this paper, only one obstacle is studied. Therefore, we need to focus on the control system of the manipulator so that these steps can be fully automated and coherently implemented in the follow-up study. At the same time, we need to further study and analyze the effectiveness of this obstacle avoidance algorithm in the case of multiple obstacles.

Field Test
The manipulator of the master X-robot (imaging board) adopts a structure of six degree of freedom and multi-joint. A camera is arranged at the end of manipulator. The camera takes real-time photos and identifies them. During the manipulator movement, the height of the imaging board rises. When a certain height is reached, the two-dimensional code of the detection part enters the camera's photographic field of view. After the robot recognizes the two-dimensional code, it shows that the imaging plate has reached the shooting height and the manipulator stops moving.
The manipulator of slave robot adopts the structure of three degrees of freedom, which can realize the height, pitch and rotation of the X-ray machine. The height of The manipulator of slave robot is mainly achieved by tracking the height of the manipulator of main robots. The mission planning of manipulator is shown in

Conclusions
The simulation of MATLAB and field test show that the improved artificial potential field method, which combines with cylindrical bounding box path planning, can make the manipulator avoid obstacles well and eventually reach the detection target and make the X-ray detection equipment installed on the manipulator complete the detection of substation GIS power equipment. This not only improves the efficiency and security of substation detection, but also makes X-ray nondestructive testing technology more widely used.