Path Planning for Obstacle Avoidance of Robot Arm Based on Improved Potential Field Method

In medical and surgical scenarios, the trajectory planning of a collaborative robot arm is a difficult problem. The artificial potential field (APF) algorithm is a classic method for robot trajectory planning, which has the characteristics of good real-time performance and low computing consumption. There are many variants of the APF algorithm, among which the most widely used variants is the velocity potential field (VPF) algorithm. However, the traditional VPF algorithm has inherent defects and problems, such as easily falling into local minimum, being unable to reach the target, poor dynamic obstacle avoidance ability, and safety and efficiency problems. Therefore, this work presents the improved velocity potential field (IVPF) algorithm, which considers direction factors, obstacle velocity factor, and tangential velocity. When encountering dynamic obstacles, the IVPF algorithm can avoid obstacles better to ensure the safety of both the human and robot arm. The IVPF algorithm also does not easily fall into a local problem when encountering different obstacles. The experiments informed the RRT* algorithm, VPF algorithm, and IVPF algorithm for comparison. Compared with the informed RRT* and VPF algorithm, the result of experiments indicate that the performances of the IVPF algorithm have significant improvements when dealing with different obstacles. The main aim of this paper is to provide a safe and efficient path planning algorithm for the robot arm in the medical field. The proposed algorithm can ensure the safety of both the human and the robot arm when the medical and surgical robot arm is working, and enables the robot arm to cope with emergencies and perform tasks better. The application of the proposed algorithm could make the collaborative robots work in a flexible and safe condition, which could open up new opportunities for the future development of medical and surgical scenarios.


Introduction
With the rapid development of the robot industry, the robot arm is no longer operated by an operator or a fixed program, but it rather behaves more like an human staff in the foreseeable future. In the medical field, surgical robots have begun to be used and are developed rapidly. The Da Vinci System is already in service and can operate on patients in the medical field [1]. Although, at present, robotic surgical arms are mainly operated by doctors to perform operations indirectly, autonomous robots have been developed. The autonomous robots can operate autonomously on patients without having to be operated by a doctor or operator [2]. This can be seen as a developing trend for human doctors and robot doctors to cooperate in surgery. For future surgical robots, the preoperative and intraoperative path planing of a surgical robot arm will become an important research and application content [3]. avoidance strategy for a dual-arm robot based on speed field with improved VPF. They introduced a new repulsion function and added virtual constraint points to eliminate the existing limitations and improve the VPF.
However, all these methods cannot handle neither dynamic or static obstacles problems well in surgery tasks. Therefore, these methods are not suitable for human-robot interaction scenarios with high real-time and safety requirements, as in this paper. In this work, an improved VPF method is proposed to solve the above mentioned problems, which can make the robot arm cope better with both dynamic and static obstacles. This work is structured as follows. Section 1 introduces the relevant information, advantages, and disadvantages of several trajectory generation methods. The related works of the APF and VPF algorithm are summarized. Section 2 describes the improvement strategies of the VPF algorithm in detail in this work. In Section 3, simulation and actual experiments are conducted to verify the proposed algorithm. In Section 4, the work of the whole article is summarized.

Traditional Potential Field
The VPF algorithm is a type of virtual velocity method that builds the environment into potential fields of attraction and repulsion. The target is built as a attractive potential field and generates attractive velocity. The obstacle is built as a repulsive potential field and generates repulsive velocity.
The attractive potential function is defined as where q is the current joint positions of the robot arm, ζ is the attractive potential gain, and ρ g is the relative Cartesian distance between the end effector of robot arm and the target. The construction of the repulsive potential field is similar to that of the attractive potential field, which can be defined as where k is the repulsive potential gain, ρ b is the minimum distance between the robot's body and the obstacle, and ρ 0 is the max range of the repulsive potential field. When the minimum distance between the robot's body and the obstacle is beyond ρ 0 , then the magnitude of the potential field is 0, and the robot will not be affected by the obstacle. The velocity is the gradient of the potential field. The functions of velocities can be expressed as where v att is the attraction velocity caused by the target, which is acting on the end effector. v rep is the repulsive velocity caused by the obstacle, which is acting on the robot links or the end effector. The velocity in Cartesian space cannot recognized by the robot arm directly, and needs to be converted into joint space. The 6 × 1 velocity vector can be calculated as where ω att is the attractive angular velocity.
The traditional VPF algorithm does not calculate the part of the angular velocity of attraction, so ω att is a 3 × 1 zero vector. According to Equations (5) and (6), the Cartesian velocity can be converted to the joint speed as follows: where J is the Jacobian matrix of the robot.q att andq rep are the attractive joint speed and repulsive joint speed, respectively, which can drive the robot arm to move toward the target or move away from obstacles. This work makes improvements based on the traditional VPF algorithm, and changes are inflicted on the attractive field, repulsive field, and singularity avoidance strategy.

Improved Attractive Field
The tradition VPF produces first-order linear attraction velocity, which is not flexible enough. When the effector is far away from the target, the velocity becomes high, which can make the robot arm move at a high joint speed. Therefore, the robot arm may not have enough time to decelerate, which will greatly increase the possibility of collisions with obstacles. This work adopts the segmented attractive potential field method to build an attractive potential field. The attractive potential field is defined as where, ζ is the attraction potential field coefficient, ρ g is the distance between the end effector and target, ρ g0 is the distance constant decided by the environment, and s is the constant coefficient. The attraction linear velocity can be calculated by When ρ g exceeds ρ g0 , the attractive velocity is a constant value. It ensures that the attractive joint speed will not be too high. The traditional VPF algorithm does not consider the calculation of the angular velocity, but this work defines how to calculate the angular velocity. Firstly, the direction vector of the end effector A and the direction vector of the actuator pointing to the target B should be determined. The direction vector B is considered to be obtained by rotating the direction vector A around a space vector k by angle θ. The quaternion rotation matrix can be obtained from k and θ. Then, the Euler angle [ α β γ ] T can be calculated by the quaternion rotation matrix. The angular velocity is processed in the same way as the linear velocity, it can be calculated by where ζ a is the coefficient of the attraction angular velocity.

Improved Repulsive Field
The construction of the improved repulsive potential field is more complicated than that of the attractive potential field. As the Figure 1 shows, O j and P j are the two closest points on the robot arm and the obstacle. T is the Target point. The vector O j T is the direction vector from O j to the target point. The vector O j P j is the direction vector from O j to P j . V obs is obstacle's velocity. θ rob_tar is the angle between the vector O j P j and the vector O j T, and θ rob_Vobs is the angle between P j O j and the direction of V obs .  The function of the improved repulsive field is defined in Equation (12).
where σ s and σ d are the static and dynamic obstacle velocity-direction factor, respectively. k is the repulsive potential gain, ρ b is the minimum distance between the robot arm and the obstacle, and ρ 0 is the maximum range of repulsive potential filed. a and b are constants. m is the order. r is the reciprocal of V re f , which is the reference velocity of obstacles and can be regarded as a constant. V re f is set to be very small and used to determine whether the obstacle is a dynamic obstacle or a static obstacle. When V obs is much greater than V re f , the obstacle can be considered a dynamic obstacle. When the V obs is much less than V re f and close to 0, the obstacle will be considered as a static obstacle. The traditional potential field method does not considered the positional relationship among obstacles, robotic arms, and target points. When the target point and obstacles are very close, the attractive velocity and the repulsive velocity may balance, and the robot arm will fall into local minimum problem and cannot reach the target. θ rob_tar is introduced to improve this situation. The effect of θ rob_tar is to deform the repulsive potential field. When the robot arm is on the side that is closed to the target point around the obstacle, θ rob_tar is smaller. When other parameters remain unchanged, the repulsive potential field U rep will be small. The robot arm can reach the target point more easily. When the robot arm is on the side that is far away from the target point around the obstacle, θ rob_tar increases. The repulsive potential field U rep will be larger, which makes the robot arm stay away from obstacles.
The traditional potential field method does not take measures against dynamic obstacles. When the obstacle moves fast, collisions may occur. In order to enhance the potential field's ability to react to dynamic obstacles, the velocity of the obstacle V obs is introduced. When the V obs is much less than V re f and close to 0, σ d is approximately equal to 0 and σ s is approximately equal to aθ rob_tar . This means that the repulsive velocity potential field U rep is mainly affected by σ s . When V obs is much greater than V re f , the obstacle can be considered a dynamic obstacle, and σ s is approximately equal to 1. At this time, the repulsive velocity potential field U rep is mainly affected by σ d . When an obstacle moves towards the robot arm, θ rob_V obs increase and U rep increase accordingly. The larger repulsive velocity reduces the possibility of the robot arm colliding with obstacles greatly. When the obstacle moves away from the robot arm, θ rob_V obs decreases and U rep decreases, reducing the obstacle's impact on the robot arm. The effect of θ rob_tar is to deform the repulsive potential field. When the robot arm is on the side which is closed to the target point around the obstacle, θ rob_tar is smaller. When other parameters remain unchanged, the repulsive potential field U rep will be smaller. When the robot arm is on the side that is far away from the target point around the obstacle, θ rob_tar increases, and the repulsive potential field U rep will be larger.
The velocity-direction factors σ s and σ d explain the effect of different obstacles on the potential field well. In addition, the introduction of θ rob_tar and V obs can make the U rep deal with dynamic and static obstacles better at the same time.
The repulsive velocity V rep can be expressed as follows: where the repulsive angle velocity ω rep is [0 0 0] T . The repulsive linear velocity is the gradient of the repulsive potential field and it can be obtained from Equation (12) v The w 1 , w 2 , w 3 are expressed as where, In order to deal with fast and dynamic obstacles, the strategy of adjusting the coefficient of the obstacle's range of action has been added. The function of ρ 0 is expressed as follows: where ρ 02 and ρ 01 are the maximum and the minimum of obstacle's range of action, respectively, and V obs0 is the obstacle velocity that maximizes the range of action. ρ 0 , which changes according to the transformation of V obs , is more flexible and enables the robot arm to better respond to static obstacles and dynamic obstacles at the same time. According to the above, safety and efficiency are all taken into account in this work.

Tangential Velocity
The traditional potential field does not have a good processing method for large cubic obstacles such as walls. Both the mobile robot and robot arm are prone to to fall into local minimum in front of large cube obstacles. To solve this problem, this work uses the virtual tangential velocity to make the robot arm get rid of the trap. When the boundary of the obstacle is determined, the side boundary of the obstacle is expanded to prevent the robot arm from getting too close to obstacles. On the side closer to the robot arm, the point which moves a certain distance from the apex of the expanded obstacle boundary to the back side of the obstacle is the tangential point.
As Figure 2 shows, EE is the end effector, P tan is the tangent point and the direction of tangential linear velocity is from EE to P tan . The function of the linear velocity v tan is v tan = µρ t + δ (22) where ρ t is the distance between P tan and the end effector, µ is the coefficient factor and δ is the constant velocity. At the same time, the use of tangential velocity may conflict with the original attraction velocity. When using tangential velocity, the weight of attraction velocity should be greatly reduced. When the end effector moves to the vicinity of the tangential point, the use of tangential velocity ends. The calculation of tangential angle velocity is done in the same way as that of the attractive angle velocity.

Singularity Avoidance Strategy Based on Velocity Space
Although the VPF algorithm calculates the path in Cartesian space, it also needs to map velocity to joint space. The mapping relationship between Cartesian space velocity and joint speed of the VPF can be expressed aṡ whereq is the joints speed andẋ is the Cartesian velocity. An important problem encountered in planning path in Cartesian space is singularity. When the robot arm encounters singularity, it will lose one or more degrees of freedom and the joint speeds will increase a lot. For the singularity problem, this work adopts the damped least squares solution with dynamic damping factor [31]. This method sacrifices the accuracy of the end trajectory, but can avoid singularities effectively. The function of the damped least square solution is defined aṡ where I is the identity matrix and λ is dynamic damping factor. The function of λ can be expressed as where k const is a constant and d thershold is the bias of the tangent trigonometric function.
The main role of d thershold is to amplify the small changes in c. c is the number of conditions to judge whether the robot arm has fallen into singularity and c can be described as When the robot arm approaches the singularity, the determinant of the Jacobian matrix approaches 0, which leads c close to 0, too. According to Equation (25), the value of λ will increase. Although the accuracy is sacrificed slightly, the singularity can be avoided. When the robot arm is far away from the singular point, c is much greater than 0, which leads λ close to 0. Then, Equation (24) is equal to Equation (23).
The repulsive joint speed cannot be calculated directly, similar to the attractive joint speed, because obstacles do not act on the end effector necessarily. In most cases, they act on other links. When an obstacle acts on the link i (usually not an end effector), this link is regarded as an end effector, and the Jacobian matrix with link i as the end of the robot arm is used to solve the joint speed, that is, only the repulsive speed of joint i and all previous joints are calculated. Therefore, J is replaced by J i in Equations (24) and (26), and J −1 is replaced by J i + (the pseudo-inverse of J i ) in Equation (23).

Experiments
In this section, a series of simulations are designed to compare the informed RRT*, VPF, and IVPF. The experiments are carried out on the KINOVA GEN2 collaborative robot arm with 6 DOF (J2N6S200). This lightweight robot arm has a unique structure without a bulky control cabinet and teaching pendant. It is small in size and weight, and the maximum working radius is 0.985 m. The simulation experiments of the informed RRT*, VPF, and IVPF are compared in MATLAB2021a. Besides, the performances of IVPF are also proved on the Kinova robot.
In the experiment results, the purple and orange circles are the waypoints of the end effector and dynamic obstacle, respectively. The green and magenta cross markers are the final position of the end effector and the target point, respectively.
The parameters of the experiments are defined as follows: d 0 is the distance between the end effector and the target point. d 1 is the distance between the robot arm and obstacle A. d 2 is the distance between the robot arm and obstacle B.
The key parameters of the IVPF algorithm are defined in Table 1.

Static Obstacles Avoidance Experiment I
In this group of comparative experiments, the robot arm base is located at the origin of the coordinate system. The unit of the coordinate axis is m. Two static spherical obstacles  Table 2. Figures 3a and 4a show the motion trajectory and state of the robot arm when it is using the informed RRT* algorithm. It can be seen that the robot arm can reach the target point and does not collide with the obstacles.
Due to the short distance between the target point and the obstacle, the robot arm falls into the local minimum problem and cannot reach the target point when using the VPF algorithm (Figure 3b), and the distance between the target point and stop point is 7.4 cm (d 0 in Figure 4b).
The motion trajectory and state of the robot arm when it is using IVPF are shown in Figures 3c and 4c. The smoothness of the trajectory is much higher than that of informed RRT*, and the movement running time of informed IVPF (2.9 s) is much shorter than that of informed RRT* (7.78 s). Compared with the VPF algorithm, IVPF also does not easily fall into the local minimum problem. In this case, the robot arm can reach the target point quickly and smoothly when using the IVPF algorithm, and the robot arm always maintains a safe distance from the two obstacles in the movement.

Static Obstacles Avoidance Experiment II
In this group of experiments, a wall obstacle is set to verify the ability of three different algorithms to deal with wall obstacles. The static wall obstacle is located at (0.36, 0, 0.  Table 3. Figure 5a exhibits the motion trajectory of the robot arm when using informed RRT* algorithm. The motion trajectory indicates that the robot arm can cross the wall obstacle smoothly, and the whole process only took 6.22 s.
However, the VPF algorithm cannot deal with this wall obstacle well. The motion trajectory in Figure 5b shows that the robot arm falls into the local minimum problem and cannot reach the target. The robot arm cannot cross the wall obstacle when it is using the VPF algorithm. In addition, the robot arm cannot keep a safe distance from the obstacle. Figure 5c shows the motion trajectory of IVPF and that the robot arm finally reached the target point. The whole process took 6.38 s and the running time is almost the same as that of the informed RRT*. Compared with the VPF algorithm, the IVPF algorithm falls less easily into a local minimum in front of wall obstacles.

Dynamic Obstacles Avoidance Experiment I
In this group of experiments, the obstacle A becomes a dynamic one. The velocity vector of obstacle A is (−0.05, 0.2, 0.05) and its unit is m/s. The running processes and distance curves of the different algorithms are shown in Figures 7 and 8. The result of different algorithms are compared in Table 4. Figure 7a shows the motion trajectory of the robot arm when using the informed RRT* algorithm. In Figure 8a, the robot arm collides with the obstacle A in 1.5 s. The informed RRT* algorithm does not have real-time obstacle avoidance capability in this experiment.
The VPF algorithm is used in the same experimental conditions. Figure 7b exhibits the whole motion trajectory of the end effector of the robot arm. From Figure 8b, it can be seen that the minimum value of d 1 is 5.4 cm (0.48 s). However, if the obstacle suddenly accelerates, this distance may not guarantee that the robot arm has enough reaction time to avoid obstacles. It can be seen from the trajectory in Figure 7b that the end effector of the robot arm made a small obstacle avoidance action. The VPF algorithm takes 7.5 s for the robot arm to reach the target point.Two seconds after starting to move, the robot arm moves very slowly. This is because the target point and obstacle B are very close, and the robot arm is almost at a standstill state. This experiment explains how the VPF algorithm cannot deal with both dynamic and static obstacles well at the same time. Although the robot arm reaches the target point in final, it takes long and does not achieve satisfactory results for dynamic obstacles.
Finally, the performances of the IVPF are verified. Figure 7c shows the whole motion trajectory of the end effector of the robot arm. The motion trajectory of the big obstacle avoidance action of the end effector can be seen clearly from this figure. This evasive action can make the robot arm stay away from obstacle and is verified by the curve of d 1 in Figure 8c. The curve of d 1 shows the distance between the robot arm and dynamic obstacle A, and the shortest distance is 11.8 cm. This distance increased 118.5% by that of VPF. Furthermore, it only took 2.75 s for the robot arm to reach the target point, which is much faster than that of VPF. Although the target point is very close to obstacle B, it has little effect on the robot arm and the robot arm can still reach the target point quickly.

Dynamic Obstacles Avoidance Experiment II
In this group of comparative experiments, the target point is located at (0.7, 0, 0.7). The initial location of a dynamic spherical obstacle is set at (0.4, 0.1, 0.4), and the radius of the obstacle is 8 cm. The obstacle will move towards joint 3 of the robot arm with the velocity vector of (−0.1, 0.1, 0) (m/s). The result of different algorithms are compared in Table 5.
The motion process of the informed RRT* algorithm is exhibited in Figure 9a. In Figure 10a, the robot arm collides with the obstacle A in 1.02 s. The informed RRT* algorithm does not have real-time obstacle avoidance capability in this experiment.
The VPF algorithm is used in the same experimental conditions. The motion process of the VPF algorithm is exhibited in Figure 9b. The trajectory in the figure shows that the robot arm made an evasive action when the obstacle moved towards joint 3. However, it can be seen that the robot arm is very close to the obstacle at 0.68 s, which can be verified by curve d 1 in Figure 10b. The minimum value of d 1 is 2 cm. Although the robot arm can react to the dynamic obstacle when it is using VPF, it is difficult for the robot arm to maintain a sufficient safe distance from the dynamic obstacle. Therefore, the ability handling of the dynamic obstacle using the VPF algorithm is not ideal.
The moving performances of the IVPF algorithm are exhibited in Figures 9c and 10c. The curve d 1 shows that the robot arm reaches the closest position to the obstacle twice. The two minimum distances are 9.5 cm and 6.9 cm. These distances can ensure that the robot arm and obstacles will not collide temporarily. However, these distances are the minimum between the robot arm and the obstacle at a whole motion. The robot arm has avoided the front of the obstacle's movement at this time. The minimum distance in this processing is still longer than that of the VPF algorithm, and increases by 245%.

The Actual Experiments
In the actual experiment, a simple robot obstacle avoidance scenario is designed to verify the feasibility of the IVPF algorithm. The initial state of the Kinova robot arm is shown in Figure 11a. There is a wall obstacle (iron plate) set near the robot arm. The obstacle is 54 cm high and 42 cm wide. The coordinates of the obstacle's center is (0.2, 0, 0.27). The purpose of this experiment is to verify that the IVPF algorithm can make the robot arm safely cross the obstacle and reach the other side of the obstacle. In order to ensure the safety during the experiment, the maximum joint speed of the robot arm is set as 15°/s.  Figure 11 exhibits the process of the movement of the robot arm. Figure 11a shows the initial state of the robot arm, and (b) to (d) show the state of the robot arm in the experiment. It can be seen from the figures that the robot arm can cross the obstacle and maintain a distance with the obstacle. The minimum distance between the robot arm and the obstacle is 2.5 cm, which can be seen in Figure 8c. The curves in Figure 11e can also confirm this situation.

Discussion
It can be seen from several comparative experiments that compared with VPF and the informed-RRT* algorithms, the IVPF algorithm has better dynamic obstacle avoidance ability and a fast response speed. When the medical and surgical robot arm uses the IVPF algorithm, the robot arm can always keep a safe distance from the dynamic obstacles, which can ensure the safety of both the human and the robot arm. The fast response to obstacles can help the robot arm address unexpected situations better. In addition, the IVPF algorithm reduces the influence of static obstacles, which can make the robot reach the target point to perform a task easily. However, IVPF also has shortcomings. Although the tangential velocity is used to get rid of the local minimum problem, the path quality planned by theIVPF algorithm is not good enough compared with the informed-RRT* algorithm. Therefore, the IVPF algorithm needs to be improved in how to execute tasks more efficiently in some scenarios.

Conclusions
This work proposed an improved VPF algorithm to deal with the inherent disadvantages of the traditional algorithm, such as easily falling into local minimum, being unable to reach the target and also showcasing poor dynamic obstacle avoidance ability. In the proposed IVPF algorithm, dynamic and static obstacle velocity-direction factors are introduced into the attractive and repulsive field, respectively, and the singularity problem is also discussed. Experiments including static and dynamic obstacles avoidance showed that the proposed algorithm has good performances in the obstacle avoidance effect and trajectory smoothness. The actual experiment also elucidates the algorithm's manifestation on the wall obstacle. The results of the study could be applied to relevant human-robot interaction conditions, which could greatly promote the scientific development of robot safety control. Compared with other algorithms, the proposed IVPF algorithm can make the medical and surgical robot cooperate with humans without collision, while ensuring the safety of the humans at the same time to complete the task. With its ability to provide a faster response time, enhanced safety, and improved task execution, the IVPF algorithm is likely to have a positive impact on medical and surgical scenarios. The IVPF algorithm can ensure the safety of humans when the robot arm is performing a task, and it enables the robot arm to address unexpected situations and perform tasks better. The IVPF algorithm has the potential to significantly improve the performance of medical and surgical robots, making them safer and more effective tools for doctors and surgeons. The application of this algorithm could make the collaborative robots work in a flexible and safe condition, which could open up new opportunities for the future development of medical and surgical scenarios.