人工势场法脱离极小值点
生活随笔
收集整理的這篇文章主要介紹了
人工势场法脱离极小值点
小編覺得挺不錯(cuò)的,現(xiàn)在分享給大家,幫大家做個(gè)參考.
????????填個(gè)坑，之前的這篇文章基于人工勢(shì)場法的UR5機(jī)械臂避障算法（Python）提到了人工勢(shì)場法可能會(huì)陷入極小值，這里提一下怎么處理以及附上源碼。
????????我這里的大致思路是用RRT算法產(chǎn)生一個(gè)臨時(shí)目標(biāo)點(diǎn)來生成新的引力場。
????????首先是先判斷是否陷入極小值，我是通過連續(xù)兩個(gè)路徑點(diǎn)相同則陷入極小，隨后跳出人工勢(shì)場法。用RRT算法，隨機(jī)樹向之前的目標(biāo)位姿進(jìn)行擴(kuò)展，一直運(yùn)行，直到出現(xiàn)100個(gè)可行節(jié)點(diǎn)（這個(gè)數(shù)量可以自己定），對(duì)應(yīng)可能有多條路徑，找到距離當(dāng)前陷入極小值的位姿最遠(yuǎn)的那個(gè)節(jié)點(diǎn)（我這里的RRT算法在隨機(jī)數(shù)擴(kuò)張的時(shí)候每一步的擴(kuò)張方向都加入了到目標(biāo)點(diǎn)的單位向量，因此距離當(dāng)前節(jié)點(diǎn)最遠(yuǎn)的節(jié)點(diǎn)基本上算是距離目標(biāo)點(diǎn)最近的），以這個(gè)節(jié)點(diǎn)為臨時(shí)目標(biāo)點(diǎn)產(chǎn)生引力場讓機(jī)械臂重新進(jìn)行人工勢(shì)場法進(jìn)行路徑規(guī)劃，等到達(dá)臨時(shí)目標(biāo)點(diǎn)的時(shí)候，就將目標(biāo)點(diǎn)重新設(shè)置為之前的目標(biāo)點(diǎn)再進(jìn)行規(guī)劃。
附上代碼：（注意，代碼還存在一些問題，文末會(huì)提及，至于為什么沒有改，因?yàn)槲覒辛?/strong>）
????????首先是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然后人工勢(shì)場法的：
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算法源碼，把人工勢(shì)場法的源碼換成以上這個(gè)。
代碼存在的問題：
1.當(dāng)向臨時(shí)目標(biāo)點(diǎn)規(guī)劃路徑的時(shí)候，可能走到一半又陷入了新的極小值點(diǎn)，但這個(gè)問題比較好解決，思路是再次使用RRT算法再找一個(gè)臨時(shí)目標(biāo)點(diǎn)，切忌不要直接把目標(biāo)點(diǎn)再次改為最開始的目標(biāo)，因?yàn)橐耘R時(shí)目標(biāo)點(diǎn)為目標(biāo)已經(jīng)運(yùn)行了幾步了，這時(shí)候陷入了臨時(shí)目標(biāo)點(diǎn)的極小值，雖然已經(jīng)跳出了先前目標(biāo)點(diǎn)的極小值點(diǎn)，但也可能出現(xiàn)把目標(biāo)點(diǎn)改為之前的目標(biāo)點(diǎn)時(shí)會(huì)出現(xiàn)機(jī)械臂再次朝著極小值點(diǎn)運(yùn)動(dòng)的情況。
2.有讀者提到了動(dòng)態(tài)障礙物，實(shí)際我在寫代碼的時(shí)候考慮的是靜態(tài)障礙物，但是也留了一個(gè)接口，就是人工勢(shì)場法的py文件里面有個(gè)get_obstacle的方法，后續(xù)的算法都是調(diào)用這個(gè)方法來獲取障礙物信息，因此直接從這里實(shí)時(shí)將障礙物信息傳入即可。
總結(jié)
以上是生活随笔為你收集整理的人工势场法脱离极小值点的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 蓝桥杯省赛 2021 杨辉三角形 pyt
- 下一篇： wget下载github的release