????????填個坑，之前的這篇文章基于人工勢場法的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_checkclass 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 = startself.goal = goalself.cube_obstacle = cube_obstacleself.joint_limit_angle = joint_limit_angleself.plan_area = plan_area#Set the step lengthself.step_length = 5*pi/180#Set the possibility of set the goal as new joint pointself.set_goal = 0.1#Store the joint_pointself.joint_point_list = [self.start]#Set the number of iterationsself.num_iterations = num_iterationsdef 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 = 0return 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.vectorreturn 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 = Falsefor 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 = Truebreakreturn exceed_limitdef path_planning(self) -> List[Joint_point]:count = 0 #Record the number of iterationswhile count < self.num_iterations:#Generate a new_radom_joint_pointif 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_listindex = self.get_the_nearest_joint_point(new_random_joint_point)#Expand the treenearest_joint_point = self.joint_point_list[index]new_joint_point = self.expand_new_joint_point(nearest_joint_point, new_random_joint_point)#Collision checkif 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_listnew_joint_point.signal = indexself.joint_point_list.append(new_joint_point)count = count + 1path = [self.get_farest_joint_point_from_start()]index = path[-1].signalwhile self.joint_point_list[index].signal is not None:joint_point = self.joint_point_list[index]path.append(joint_point)index = joint_point.signalpath.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 RRTclass 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 = startself.goal = goalself.cube_obstacle = cube_obstacleself.joint_limit_angle = joint_limit_angle#Get the goal end point in Cartesian spaceT = FK(self.goal)self.goal_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])#Set the Katt and Krepself.Katt_end = 0.5self.Katt_joint = 0.1self.Krep = 0.1#Set the range of repulsive potential fieldself.d0 = 0.3#Set the step lengthself.step_length = 2*pi/180#Set a signal to record whether stuck in local minimumself.stuck_in_local_minimum = Falsedef 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 thetaT = FK(joint_point)endlink_point = Point([T[5][0,3], T[5][1,3], T[5][2,3]])#Get the distance between endlink and goald = distance_point_to_point(endlink_point, self.goal_point)#Return the attractive energyreturn 0.5*self.Katt_end*(d**2)+0.5*self.Katt_joint*lg.norm(self.goal.vector-joint_point.vector)**2else:#Set the self.new_goal as the attractive joint_point#Get the endlink position of thetaT = 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_goald = distance_point_to_point(endlink_point, self.new_goal_point)#Return the attractive energyreturn 0.5*self.Katt_end*(d**2)+0.5*self.Katt_joint*lg.norm(self.new_goal.vector-joint_point.vector)**2def get_repulsive_energy(self, joint_point:Joint_point) -> float:"""Get the repulsive energy in joint_point"""#Get the repulsive energy of each DH jointsUi = list() #Stores the repulsive energy of each DH jointsT = 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_obstacledef 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_pointrrt = 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 spaceT = 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_minimumself.stuck_in_local_minimum = Trueprint("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 = Falsefor 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 = Truebreakreturn exceed_limitdef path_planning(self) -> List[Joint_point]:"""Start the path planning"""#Initial thetatheta = self.start.theta#Initial the pathpath = [self.start]while True:"""Initial the U and theta_listU : Stores the total potential energy of once searchjoint_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 collisionif ur5_collision_check(joint_point, self.get_obstacle()) or self.check_joint_point_exceed_limit(joint_point):passelse:#Get the atrractive energyUatt = self.get_attractive_energy(joint_point)#Get the repulsive energyUrep = self.get_repulsive_energy(joint_point)U.append(Uatt+Urep) #The total potential energyjoint_point_list.append(joint_point)#FInd the index of minimum Uindex = U.index(min(U))#Add the theta_list[index] into pathpath.append(joint_point_list[index])print(path[-1].theta)#Update the thetatheta = joint_point_list[index].theta#Judge whether getting stuck in local minimumif 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 finishedbreakelse:#Set a signal when getting stuck in local minimumself.deal_with_local_minimum(path[-1])#When stuck in local minimum, judge whether meet the self.new_goalif 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 = Falsereturn path

????????注意，你得先去下載文章開頭那篇鏈接文章的源碼，然后加上RRT算法源碼，把人工勢場法的源碼換成以上這個。

代碼存在的問題：

1.當向臨時目標點規劃路徑的時候，可能走到一半又陷入了新的極小值點，但這個問題比較好解決，思路是再次使用RRT算法再找一個臨時目標點，切忌不要直接把目標點再次改為最開始的目標，因為以臨時目標點為目標已經運行了幾步了，這時候陷入了臨時目標點的極小值，雖然已經跳出了先前目標點的極小值點，但也可能出現把目標點改為之前的目標點時會出現機械臂再次朝著極小值點運動的情況。

2.有讀者提到了動態障礙物，實際我在寫代碼的時候考慮的是靜態障礙物，但是也留了一個接口，就是人工勢場法的py文件里面有個get_obstacle的方法，后續的算法都是調用這個方法來獲取障礙物信息，因此直接從這里實時將障礙物信息傳入即可。

總結

以上是生活随笔為你收集整理的人工势场法脱离极小值点的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。