人工势场法脱离极小值点

101 阅读 0 评论 67 点赞

我是靠谱客的博主眯眯眼世界，这篇文章主要介绍人工势场法脱离极小值点，现在分享给大家，希望可以做个参考。

填个坑，之前的这篇文章基于人工势场法的UR5机械臂避障算法（Python）提到了人工势场法可能会陷入极小值，这里提一下怎么处理以及附上源码。

我这里的大致思路是用RRT算法产生一个临时目标点来生成新的引力场。

首先是先判断是否陷入极小值，我是通过连续两个路径点相同则陷入极小，随后跳出人工势场法。用RRT算法，随机树向之前的目标位姿进行扩展，一直运行，直到出现100个可行节点（这个数量可以自己定），对应可能有多条路径，找到距离当前陷入极小值的位姿最远的那个节点（我这里的RRT算法在随机数扩张的时候每一步的扩张方向都加入了到目标点的单位向量，因此距离当前节点最远的节点基本上算是距离目标点最近的），以这个节点为临时目标点产生引力场让机械臂重新进行人工势场法进行路径规划，等到达临时目标点的时候，就将目标点重新设置为之前的目标点再进行规划。

附上代码：（注意，代码还存在一些问题，文末会提及，至于为什么没有改，因为我懒了）

首先是RRT算法的。

复制代码

from typing import List
from self_defined_modules.robot_class import Cube_obstacle, Joint_point
from math import pi
import random
import numpy.linalg as lg
from self_defined_modules.ur5_collision_check import ur5_collision_check

class RRT():
          def __init__(self, start:Joint_point,goal:Joint_point, cube_obstacle:Cube_obstacle, joint_limit_angle:List[Joint_point], num_iterations:int, plan_area = [Joint_point([-pi]*6), Joint_point([pi]*6)]) -> None:
                    self.start = start
                    self.goal = goal
                    self.cube_obstacle = cube_obstacle
                    self.joint_limit_angle = joint_limit_angle
                    self.plan_area = plan_area

#Set the step length
                    self.step_length = 5*pi/180

#Set the possibility of set the goal as new joint point
                    self.set_goal = 0.1

#Store the joint_point
                    self.joint_point_list = [self.start]

#Set the number of iterations
                    self.num_iterations = num_iterations
          
          def generate_new_joint_point(self) -> Joint_point:
                    theta1 = random.uniform(self.plan_area[0].theta[0], self.plan_area[1].theta[0])
                    theta2 = random.uniform(self.plan_area[0].theta[1], self.plan_area[1].theta[1])
                    theta3 = random.uniform(self.plan_area[0].theta[2], self.plan_area[1].theta[2])
                    theta4 = random.uniform(self.plan_area[0].theta[3], self.plan_area[1].theta[3])
                    theta5 = random.uniform(self.plan_area[0].theta[4], self.plan_area[1].theta[4])
                    theta6 = 0
                    return Joint_point([theta1, theta2, theta3, theta4, theta5, theta6])

def get_the_nearest_joint_point(self, new_random_joint_point:Joint_point) -> int:
                    """
                    Get the nearest joint point in self.joint_point_list
                    """
                    d = list()
                    for joint_point in self.joint_point_list:
                              d.append(lg.norm(joint_point.vector-new_random_joint_point.vector))
                    return d.index(min(d))
          
          def expand_new_joint_point(self, nearest_joint_point:Joint_point, new_random_joint_point:Joint_point) ->Joint_point:
                    new_point = self.step_length*((new_random_joint_point.vector-nearest_joint_point.vector)+(self.goal.vector-nearest_joint_point.vector))+nearest_joint_point.vector
                    return Joint_point(new_point.tolist())

def collision_check(self, new_joint_point:Joint_point) -> bool:
                    return ur5_collision_check(new_joint_point, self.cube_obstacle)

def get_farest_joint_point_from_start(self) -> Joint_point:
                    d = list()
                    for joint_point in self.joint_point_list:
                              d.append(lg.norm(joint_point.vector-self.start.vector))
                    return self.joint_point_list[d.index(max(d))]

def check_joint_point_exceed_limit(self, joint_point:Joint_point) -> bool:
                    """
                    Check the joint_point whether exceed the joint limit angle
                    """
                    exceed_limit = False
                    for i in range(len(joint_point.theta)):
                              if joint_point.theta[i] < self.joint_limit_angle[0].theta[i] or joint_point.theta[i] > self.joint_limit_angle[1].theta[i]:
                                        exceed_limit = True
                                        break
                    return exceed_limit

def path_planning(self) -> List[Joint_point]:
                    count = 0           #Record the number of iterations
                    while count < self.num_iterations:
                              #Generate a new_radom_joint_point
                              if random.random() > self.set_goal:
                                        new_random_joint_point = self.generate_new_joint_point()
                              else:
                                        new_random_joint_point = self.goal
                              
                              #Get the nearest joint_point in self.joint_point_list
                              index = self.get_the_nearest_joint_point(new_random_joint_point)

#Expand the tree
                              nearest_joint_point = self.joint_point_list[index]
                              new_joint_point = self.expand_new_joint_point(nearest_joint_point, new_random_joint_point)

#Collision check
                              if self.collision_check(new_joint_point) or self.check_joint_point_exceed_limit(new_joint_point):
                                        continue

#Add the new joint_point into self.joint_point_list
                              new_joint_point.signal = index
                              self.joint_point_list.append(new_joint_point)
                              count = count + 1
    
                    path = [self.get_farest_joint_point_from_start()]
                    index = path[-1].signal
                    while self.joint_point_list[index].signal is not None:
                              joint_point = self.joint_point_list[index]
                              path.append(joint_point)
                              index = joint_point.signal
                    path.append(self.start)
                    path.reverse()
                    return path

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
from typing import List
from self_defined_modules.robot_class import Cube_obstacle, Joint_point
from math import pi
import random
import numpy.linalg as lg
from self_defined_modules.ur5_collision_check import ur5_collision_check

class RRT():
          def __init__(self, start:Joint_point,goal:Joint_point, cube_obstacle:Cube_obstacle, joint_limit_angle:List[Joint_point], num_iterations:int, plan_area = [Joint_point([-pi]*6), Joint_point([pi]*6)]) -> None:
                    self.start = start
                    self.goal = goal
                    self.cube_obstacle = cube_obstacle
                    self.joint_limit_angle = joint_limit_angle
                    self.plan_area = plan_area

                    #Set the step length
                    self.step_length = 5*pi/180

                    #Set the possibility of set the goal as new joint point
                    self.set_goal = 0.1

                    #Store the joint_point
                    self.joint_point_list = [self.start]

                    #Set the number of iterations
                    self.num_iterations = num_iterations
          
          def generate_new_joint_point(self) -> Joint_point:
                    theta1 = random.uniform(self.plan_area[0].theta[0], self.plan_area[1].theta[0])
                    theta2 = random.uniform(self.plan_area[0].theta[1], self.plan_area[1].theta[1])
                    theta3 = random.uniform(self.plan_area[0].theta[2], self.plan_area[1].theta[2])
                    theta4 = random.uniform(self.plan_area[0].theta[3], self.plan_area[1].theta[3])
                    theta5 = random.uniform(self.plan_area[0].theta[4], self.plan_area[1].theta[4])
                    theta6 = 0
                    return Joint_point([theta1, theta2, theta3, theta4, theta5, theta6])

          def get_the_nearest_joint_point(self, new_random_joint_point:Joint_point) -> int:
                    """
                    Get the nearest joint point in self.joint_point_list
                    """
                    d = list()
                    for joint_point in self.joint_point_list:
                              d.append(lg.norm(joint_point.vector-new_random_joint_point.vector))
                    return d.index(min(d))
          
          def expand_new_joint_point(self, nearest_joint_point:Joint_point, new_random_joint_point:Joint_point) ->Joint_point:
                    new_point = self.step_length*((new_random_joint_point.vector-nearest_joint_point.vector)+(self.goal.vector-nearest_joint_point.vector))+nearest_joint_point.vector
                    return Joint_point(new_point.tolist())

          def collision_check(self, new_joint_point:Joint_point) -> bool:
                    return ur5_collision_check(new_joint_point, self.cube_obstacle)

          def get_farest_joint_point_from_start(self) -> Joint_point:
                    d = list()
                    for joint_point in self.joint_point_list:
                              d.append(lg.norm(joint_point.vector-self.start.vector))
                    return self.joint_point_list[d.index(max(d))]

          def check_joint_point_exceed_limit(self, joint_point:Joint_point) -> bool:
                    """
                    Check the joint_point whether exceed the joint limit angle
                    """
                    exceed_limit = False
                    for i in range(len(joint_point.theta)):
                              if joint_point.theta[i] < self.joint_limit_angle[0].theta[i] or joint_point.theta[i] > self.joint_limit_angle[1].theta[i]:
                                        exceed_limit = True
                                        break
                    return exceed_limit

          def path_planning(self) -> List[Joint_point]:
                    count = 0           #Record the number of iterations
                    while count < self.num_iterations:
                              #Generate a new_radom_joint_point
                              if random.random() > self.set_goal:
                                        new_random_joint_point = self.generate_new_joint_point()
                              else:
                                        new_random_joint_point = self.goal
                              
                              #Get the nearest joint_point in self.joint_point_list
                              index = self.get_the_nearest_joint_point(new_random_joint_point)

                              #Expand the tree
                              nearest_joint_point = self.joint_point_list[index]
                              new_joint_point = self.expand_new_joint_point(nearest_joint_point, new_random_joint_point)

                              #Collision check
                              if self.collision_check(new_joint_point) or self.check_joint_point_exceed_limit(new_joint_point):
                                        continue

                              #Add the new joint_point into self.joint_point_list
                              new_joint_point.signal = index
                              self.joint_point_list.append(new_joint_point)
                              count = count + 1
    
                    path = [self.get_farest_joint_point_from_start()]
                    index = path[-1].signal
                    while self.joint_point_list[index].signal is not None:
                              joint_point = self.joint_point_list[index]
                              path.append(joint_point)
                              index = joint_point.signal
                    path.append(self.start)
                    path.reverse()
                    return path

然后人工势场法的：

复制代码

from typing import List
from self_defined_modules.robot_class import Cube_obstacle, Point, Joint_point
from ur_kinematics.foward_kinematics import FK
from self_defined_modules.get_distance import *
from math import pi, sqrt
from self_defined_modules.ur5_collision_check import ur5_collision_check
import numpy.linalg as lg
from APF_RRT.RRT_local_minimum import RRT

class APF_RRT():
          """
          Artificial Poential Field and RRT
          """
          def __init__(self, start:Joint_point, goal:Joint_point, cube_obstacle:Cube_obstacle, joint_limit_angle:List[Joint_point]) -> None:
                    self.start = start
                    self.goal = goal
                    self.cube_obstacle = cube_obstacle
                    self.joint_limit_angle = joint_limit_angle

#Get the goal end point in Cartesian space
                    T = FK(self.goal)
                    self.goal_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])

#Set the Katt and Krep
                    self.Katt_end = 0.5
                    self.Katt_joint = 0.1
                    self.Krep = 0.1

#Set the range of repulsive potential field
                    self.d0 = 0.3

#Set the step length
                    self.step_length = 2*pi/180

#Set a signal to record whether stuck in local minimum
                    self.stuck_in_local_minimum = False

def get_attractive_energy(self, joint_point:Joint_point) -> float:
                    """
                    Get the attractive energy in joint_point
                    """
                    if not self.stuck_in_local_minimum:
                              #Set the self.goal as the attractive joint_point
                              #Get the endlink position of theta
                              T = FK(joint_point)
                              endlink_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])

#Get the distance between endlink and goal
                              d = distance_point_to_point(endlink_point, self.goal_point)

#Return the attractive energy
                              return 0.5*self.Katt_end*(d**2)+0.5*self.Katt_joint*lg.norm(self.goal.vector-joint_point.vector)**2
                    else:
                              #Set the self.new_goal as the attractive joint_point
                              #Get the endlink position of theta
                              T = FK(joint_point)
                              endlink_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])

#Get the distance between endlink and new_goal
                              d = distance_point_to_point(endlink_point, self.new_goal_point)

#Return the attractive energy
                              return 0.5*self.Katt_end*(d**2)+0.5*self.Katt_joint*lg.norm(self.new_goal.vector-joint_point.vector)**2

def get_repulsive_energy(self, joint_point:Joint_point) -> float:
                    """
                    Get the repulsive energy in joint_point
                    """
                    #Get the repulsive energy of each DH joints
                    Ui = list()         #Stores the repulsive energy of each DH joints
                    T = FK(joint_point)
                    for A in T:
                              DH_joint_point = Point([A[0,3], A[1,3], A[2,3]])
                              d = distance_point_to_cube_obstacle(DH_joint_point, self.get_obstacle())
                              if d < self.d0:
                                        Ui.append(0.5*self.Krep*(1/d-1/self.d0))
                              else:
                                        Ui.append(0)
                    
                    return sum(Ui)

def get_obstacle(self) -> Cube_obstacle:
                    """
                    This is a interface to get the information of obstacle
                    """
                    return self.cube_obstacle

def deal_with_local_minimum(self, joint_point:Joint_point) -> None:
                    """
                    Method to deal with algorithms getting stuck in local minimum
                    """
                    """
                    Use RRT to generate a new joint_point as a attractive joint point
                    """
                    #Get a new attractive joint_point
                    rrt = RRT(start = joint_point, goal = self.goal, cube_obstacle = self.get_obstacle(), joint_limit_angle = self.joint_limit_angle, num_iterations = 100)
                    self.new_goal = rrt.path_planning()[-1]

#Get the new goal end point in Cartesian space
                    T = FK(self.new_goal)
                    self.new_goal_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])

#Change the self.stuck_in_local_minimum
                    self.stuck_in_local_minimum = True
                    print("I get the new goal : ",self.new_goal.vector)

def path_planning(self) -> List[Joint_point]:
                    """
                    Start the path planning
                    """
                    #Initial theta
                    theta = self.start.theta
                    #Initial the path
                    path = [self.start]
                    while True:
                              """
                              Initial the U and theta_list
                              U : Stores the total potential energy of once search
                              joint_point_list : Stores the joint_point of once search
                              """
                              U = []
                              joint_point_list = []
                              for theta1 in [theta[0]-self.step_length, theta[0], theta[0]+self.step_length]:
                                        for theta2 in [theta[1]-self.step_length, theta[1], theta[1]+self.step_length]:
                                                  for theta3 in [theta[2]-self.step_length, theta[2], theta[2]+self.step_length]:
                                                            for theta4 in [theta[3]-self.step_length, theta[3], theta[3]+self.step_length]:
                                                                      for theta5 in [theta[4]-self.step_length, theta[4], theta[4]+self.step_length]:
                                                                                theta6 = self.start.theta[5]
                                                                                joint_point = Joint_point([theta1, theta2, theta3, theta4, theta5, theta6])
                                                                                #Check collision
                                                                                if ur5_collision_check(joint_point, self.get_obstacle()) or self.check_joint_point_exceed_limit(joint_point):
                                                                                          pass
                                                                                else:
                                                                                          #Get the atrractive energy
                                                                                          Uatt = self.get_attractive_energy(joint_point)
                                                                                          #Get the repulsive energy
                                                                                          Urep = self.get_repulsive_energy(joint_point)
                                                                                          U.append(Uatt+Urep)           #The total potential energy
                                                                                          joint_point_list.append(joint_point)
                              #FInd the index of minimum U
                              index = U.index(min(U))
                              #Add the theta_list[index] into path
                              path.append(joint_point_list[index])
                              print(path[-1].theta)
                              #Update the theta
                              theta = joint_point_list[index].theta
                              #Judge whether getting stuck in local minimum
                              if path[-1].theta == path[-2].theta and (not self.stuck_in_local_minimum):
                                        if lg.norm(path[-1].vector-self.goal.vector) < sqrt((self.step_length/2)**2*6):
                                                  #If path[-1] approximately equal to self.goal, path planning is finished
                                                  break
                                        else:
                                                  #Set a signal when getting stuck in local minimum
                                                  self.deal_with_local_minimum(path[-1])
                              
                              #When stuck in local minimum, judge whether meet the self.new_goal
                              if  self.stuck_in_local_minimum:
                                        if lg.norm(self.new_goal.vector-path[-1].vector)<sqrt((self.step_length/2)**2*6):
                                                  print("I have met the new goal!!!!!!!")
                                                  self.stuck_in_local_minimum = False

return path

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
from typing import List
from self_defined_modules.robot_class import Cube_obstacle, Point, Joint_point
from ur_kinematics.foward_kinematics import FK
from self_defined_modules.get_distance import *
from math import pi, sqrt
from self_defined_modules.ur5_collision_check import ur5_collision_check
import numpy.linalg as lg
from APF_RRT.RRT_local_minimum import RRT

class APF_RRT():
          """
          Artificial Poential Field and RRT
          """
          def __init__(self, start:Joint_point, goal:Joint_point, cube_obstacle:Cube_obstacle, joint_limit_angle:List[Joint_point]) -> None:
                    self.start = start
                    self.goal = goal
                    self.cube_obstacle = cube_obstacle
                    self.joint_limit_angle = joint_limit_angle

                    #Get the goal end point in Cartesian space
                    T = FK(self.goal)
                    self.goal_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])

                    #Set the Katt and Krep
                    self.Katt_end = 0.5
                    self.Katt_joint = 0.1
                    self.Krep = 0.1

                    #Set the range of repulsive potential field
                    self.d0 = 0.3

                    #Set the step length
                    self.step_length = 2*pi/180

                    #Set a signal to record whether stuck in local minimum
                    self.stuck_in_local_minimum = False

          def get_attractive_energy(self, joint_point:Joint_point) -> float:
                    """
                    Get the attractive energy in joint_point
                    """
                    if not self.stuck_in_local_minimum:
                              #Set the self.goal as the attractive joint_point
                              #Get the endlink position of theta
                              T = FK(joint_point)
                              endlink_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])

                              #Get the distance between endlink and goal
                              d = distance_point_to_point(endlink_point, self.goal_point)

                              #Return the attractive energy
                              return 0.5*self.Katt_end*(d**2)+0.5*self.Katt_joint*lg.norm(self.goal.vector-joint_point.vector)**2
                    else:
                              #Set the self.new_goal as the attractive joint_point
                              #Get the endlink position of theta
                              T = FK(joint_point)
                              endlink_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])

                              #Get the distance between endlink and new_goal
                              d = distance_point_to_point(endlink_point, self.new_goal_point)

                              #Return the attractive energy
                              return 0.5*self.Katt_end*(d**2)+0.5*self.Katt_joint*lg.norm(self.new_goal.vector-joint_point.vector)**2

          def get_repulsive_energy(self, joint_point:Joint_point) -> float:
                    """
                    Get the repulsive energy in joint_point
                    """
                    #Get the repulsive energy of each DH joints
                    Ui = list()         #Stores the repulsive energy of each DH joints
                    T = FK(joint_point)
                    for A in T:
                              DH_joint_point = Point([A[0,3], A[1,3], A[2,3]])
                              d = distance_point_to_cube_obstacle(DH_joint_point, self.get_obstacle())
                              if d < self.d0:
                                        Ui.append(0.5*self.Krep*(1/d-1/self.d0))
                              else:
                                        Ui.append(0)
                    
                    return sum(Ui)

          def get_obstacle(self) -> Cube_obstacle:
                    """
                    This is a interface to get the information of obstacle
                    """
                    return self.cube_obstacle

          def deal_with_local_minimum(self, joint_point:Joint_point) -> None:
                    """
                    Method to deal with algorithms getting stuck in local minimum
                    """
                    """
                    Use RRT to generate a new joint_point as a attractive joint point
                    """
                    #Get a new attractive joint_point
                    rrt = RRT(start = joint_point, goal = self.goal, cube_obstacle = self.get_obstacle(), joint_limit_angle = self.joint_limit_angle, num_iterations = 100)
                    self.new_goal = rrt.path_planning()[-1]

                    #Get the new goal end point in Cartesian space
                    T = FK(self.new_goal)
                    self.new_goal_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])

                    #Change the self.stuck_in_local_minimum
                    self.stuck_in_local_minimum = True
                    print("I get the new goal : ",self.new_goal.vector)

          def check_joint_point_exceed_limit(self, joint_point:Joint_point) -> bool:
                    """
                    Check the joint_point whether exceed the joint limit angle
                    """
                    exceed_limit = False
                    for i in range(len(joint_point.theta)):
                              if joint_point.theta[i] < self.joint_limit_angle[0].theta[i] or joint_point.theta[i] > self.joint_limit_angle[1].theta[i]:
                                        exceed_limit = True
                                        break
                    return exceed_limit

          def path_planning(self) -> List[Joint_point]:
                    """
                    Start the path planning
                    """
                    #Initial theta
                    theta = self.start.theta
                    #Initial the path
                    path = [self.start]
                    while True:
                              """
                              Initial the U and theta_list
                              U : Stores the total potential energy of once search
                              joint_point_list : Stores the joint_point of once search
                              """
                              U = []
                              joint_point_list = []
                              for theta1 in [theta[0]-self.step_length, theta[0], theta[0]+self.step_length]:
                                        for theta2 in [theta[1]-self.step_length, theta[1], theta[1]+self.step_length]:
                                                  for theta3 in [theta[2]-self.step_length, theta[2], theta[2]+self.step_length]:
                                                            for theta4 in [theta[3]-self.step_length, theta[3], theta[3]+self.step_length]:
                                                                      for theta5 in [theta[4]-self.step_length, theta[4], theta[4]+self.step_length]:
                                                                                theta6 = self.start.theta[5]
                                                                                joint_point = Joint_point([theta1, theta2, theta3, theta4, theta5, theta6])
                                                                                #Check collision
                                                                                if ur5_collision_check(joint_point, self.get_obstacle()) or self.check_joint_point_exceed_limit(joint_point):
                                                                                          pass
                                                                                else:
                                                                                          #Get the atrractive energy
                                                                                          Uatt = self.get_attractive_energy(joint_point)
                                                                                          #Get the repulsive energy
                                                                                          Urep = self.get_repulsive_energy(joint_point)
                                                                                          U.append(Uatt+Urep)           #The total potential energy
                                                                                          joint_point_list.append(joint_point)
                              #FInd the index of minimum U
                              index = U.index(min(U))
                              #Add the theta_list[index] into path
                              path.append(joint_point_list[index])
                              print(path[-1].theta)
                              #Update the theta
                              theta = joint_point_list[index].theta
                              #Judge whether getting stuck in local minimum
                              if path[-1].theta == path[-2].theta and (not self.stuck_in_local_minimum):
                                        if lg.norm(path[-1].vector-self.goal.vector) < sqrt((self.step_length/2)**2*6):
                                                  #If path[-1] approximately equal to self.goal, path planning is finished
                                                  break
                                        else:
                                                  #Set a signal when getting stuck in local minimum
                                                  self.deal_with_local_minimum(path[-1])
                              
                              #When stuck in local minimum, judge whether meet the self.new_goal
                              if  self.stuck_in_local_minimum:
                                        if lg.norm(self.new_goal.vector-path[-1].vector)<sqrt((self.step_length/2)**2*6):
                                                  print("I have met the new goal!!!!!!!")
                                                  self.stuck_in_local_minimum = False

                    return path

注意，你得先去下载文章开头那篇链接文章的源码，然后加上RRT算法源码，把人工势场法的源码换成以上这个。

代码存在的问题：

1.当向临时目标点规划路径的时候，可能走到一半又陷入了新的极小值点，但这个问题比较好解决，思路是再次使用RRT算法再找一个临时目标点，切忌不要直接把目标点再次改为最开始的目标，因为以临时目标点为目标已经运行了几步了，这时候陷入了临时目标点的极小值，虽然已经跳出了先前目标点的极小值点，但也可能出现把目标点改为之前的目标点时会出现机械臂再次朝着极小值点运动的情况。

2.有读者提到了动态障碍物，实际我在写代码的时候考虑的是静态障碍物，但是也留了一个接口，就是人工势场法的py文件里面有个get_obstacle的方法，后续的算法都是调用这个方法来获取障碍物信息，因此直接从这里实时将障碍物信息传入即可。