概述
填个坑，之前的这篇文章基于人工势场法的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然后人工势场法的：
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的方法，后续的算法都是调用这个方法来获取障碍物信息，因此直接从这里实时将障碍物信息传入即可。
最后
以上就是眯眯眼世界为你收集整理的人工势场法脱离极小值点的全部内容，希望文章能够帮你解决人工势场法脱离极小值点所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复