Artificial Potential Field - python code

  본 포스팅에서는 Artificial Potential Field를 python으로 구현한 코드에 대해 말씀드리도록 하겠습니다. 본 포스팅에서 사용한 코드는 Reference [1] 코드를 참고하여 수정하였습니다. 

 

Program Start

  Python 코드를 실행시키면, __name__ 이란 변수에 __main__ 이 입력됩니다. 따라서 첫번째 코드인 if __name__ == '__main__' 은 python으로 작성된 프로그램이 실행되었는지 확인하는 것입니다. 프로그램이 실행되면, define된 main()으로 갑니다.

 

if __name__=='__main__':
	print('__file__'+" start!!")
	main()
	print('__file__'+" Done!!")

 

def main():

  변수를 초기화합니다. 로봇의 출발지점인 start_x, start_y를 초기화하고, goal_x, goal_y를 목표지점으로 설정합니다. Obstacle의 위치는 obs np.array에 저장합니다. 프로그램을 테스트할 때 obstacle의 수를 조절해가며 테스트해보세요. 모든 변수가 초기화되면, Artificial_Potential_Field 함수를 실행합니다.

 

def main():
	print("Artificial Potential Field Start!!")

	start_x, start_y = 0.0, 0.0
	goal_x, goal_y = 30.0, 30.0

	obs = np.array([[15.0,14.0],[10.0,11.0]])

	Artificial_Potention_Field(start_x,start_y,goal_x,goal_y,obs)

 

def Artificial_Potential_Field(start_x, start_y, goal_x, goal_y, obs):

  로봇의 위치 (x,y)를 start_x, start_y로 초기화합니다. 로봇의 이동궤적을 저장하기 위해 trace_x, trace_y를 [] 배열로 초기화합니다. 그리고 while 구문을 실행하기 전에 로봇의 초기 위치를 ".append()" API를 사용해서 trace_x와 trace_y에 채워넣습니다.

  while 구문은 error < 1이 되면 종료되도록 무한루프를 만듭니다. calc_attractive_force 함수와 calc_repulsive_force 함수를 사용하여 계산한 Attractive Force의 x, y축 성분과 Repulsive Force의 x,y축 성분을 각각 더해서 pot_x, pot_y 변수에 저장합니다.

 

로봇의 위치를 update 하기 위해 x = x +pot_x (y = y + pot_y) 를 계산합니다. 로봇의 위치 궤적을 저장하기 위해서 trace_x와 trace_y에 ".append()" API를 이용하여 update된 로봇의 위치 (x,y)를 채워넣습니다. 

  로봇이 goal (목적지)에 도달했는지 확인하기 위해서 로봇과 goal사이의 norm2 (피타고라스 정리를 이용한 두 점사이의 거리)를 계산합니다. error < 1 이면 obstacles(obs)와 로봇의 궤적(trace_x, trace_y), 그리고 목적지 (goal_x, goal_y)를 plt.show()를 이용하여 plot하고 무한루프를 빠져나옵니다. 만약 error >= 1 이면 error < 1이 될 때까지 무한루프가 실행됩니다.

 

def Artificial_Potention_Field(start_x,start_y,goal_x,goal_y,obs):

	x,y = start_x,start_y

	trace_x = []
	trace_y = []

	trace_x.append(x)
	trace_y.append(y)

	while(1):
		att_x, att_y = calc_attractive_force(x,y,goal_x,goal_y)
		rep_x, rep_y = calc_repulsive_force(x,y,obs)

		pot_x = att_x+rep_x
		pot_y = att_y+rep_y

		x = x + pot_x
		y = y + pot_y

		trace_x.append(x)
		trace_y.append(y)

		error = np.linalg.norm([goal_x-x,goal_y-y])

		if error < 1:
			plt.plot(obs[:,0],obs[:,1],'bo')
			plt.plot(trace_x,trace_y,'go',goal_x,goal_y,'ro')
			plt.show()
			break

 

def calc_attractive_force(x,y,gx,gy):

  로봇과 goal의 거리를 계산하기 위해서 "np.linalg.norm" API를 사용합니다. 그 후에, Attractive Force 수식에 따라 x, y축 각각에 대한 Attractive Force를 계산하고 return 합니다.

 

 

def calc_attractive_force(x,y,gx,gy):
	e_x, e_y = gx-x, gy-y
	distance = np.linalg.norm([e_x,e_y])

	att_x = Kp_att * e_x/distance
	att_y = Kp_att * e_y/distance

	return att_x, att_y

 

def calc_repulsive_force(x,y,obs):

  Obstacle의 수는 여러개가 존재할 수 있기 때문에 for 문을 활용합니다. 먼저, Repulsive Force의 크기(rep_x, rep_y)를 0으로 초기화합니다. obs.shape[0]은 obstacle의 수를 나타내고, obs_xy는 각 obstacle의 (x,y) 좌표를 나타냅니다. obstacle의 수만큼 for 문을 실행합니다. "np.linalg.norm" API를 활용하여 로봇과 obstacle의 거리를 각각 계산하고, 모든 obstacle의 벡터를 합하기 위해 for 문의 각 loop를 실행할 때마다 rep_x, rep_y를 update합니다. 모든 obstacles에 대한 Repulsive Force가 더해진 rep_x와 rep_y를 return 합니다.

 

 

def calc_repulsive_force(x,y,obs):

	rep_x,rep_y = 0,0

	for obs_xy in np.ndindex(obs.shape[0]):

		obs_dis_x, obs_dis_y = obs[obs_xy][0]-x, obs[obs_xy][1]-y 
		obs_dis = np.linalg.norm([obs_dis_x,obs_dis_y]) 

		if obs_dis < obstacle_bound:
			rep_x = rep_x - Kp_rel * (1/obs_dis - 1/obstacle_bound)*(1/(obs_dis*obs_dis))*obs_dis_x/obs_dis
			rep_y = rep_y - Kp_rel * (1/obs_dis - 1/obstacle_bound)*(1/(obs_dis*obs_dis))*obs_dis_y/obs_dis
		else:
			rep_x = rep_x
			rep_y = rep_y

	return rep_x, rep_y

 

Simulation Result

  다음 코드는 작성한 모든 코드입니다. 복사해서 실행하면 아래와 같은 결과를 얻을 수 있습니다. Parameters와 obstacle의 수, 그리고 goal의 위치를 바꿔보면서 실행해보면서 Potential Field 알고리즘이 어떻게 작동하는지 관찰해 보시길 바랍니다.

 

 

"""

Artificial Potential Field based path planner
author: Jeonggeun Lim
Reference:
https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/PotentialFieldPlanning/potential_field_planning.py

"""

import numpy as np
import matplotlib.pyplot as plt

# Parameters
# Parameters
Kp_att = 0.02
Kp_rel = 3.0
obstacle_bound = 2.0

def calc_attractive_force(x,y,gx,gy):
	e_x, e_y = gx-x, gy-y
	distance = np.linalg.norm([e_x,e_y])

	att_x = Kp_att * e_x/distance
	att_y = Kp_att * e_y/distance

	return att_x, att_y

def calc_repulsive_force(x,y,obs):

	rep_x,rep_y = 0,0

	for obs_xy in np.ndindex(obs.shape[0]):

		obs_dis_x, obs_dis_y = obs[obs_xy][0]-x, obs[obs_xy][1]-y 
		obs_dis = np.linalg.norm([obs_dis_x,obs_dis_y]) 

		if obs_dis < obstacle_bound:
			rep_x = rep_x - Kp_rel * (1/obs_dis - 1/obstacle_bound)*(1/(obs_dis*obs_dis))*obs_dis_x/obs_dis
			rep_y = rep_y - Kp_rel * (1/obs_dis - 1/obstacle_bound)*(1/(obs_dis*obs_dis))*obs_dis_y/obs_dis
		else:
			rep_x = rep_x
			rep_y = rep_y

	return rep_x, rep_y

def Artificial_Potention_Field(start_x,start_y,goal_x,goal_y,obs):

	x,y = start_x,start_y

	trace_x = []
	trace_y = []

	trace_x.append(x)
	trace_y.append(y)

	while(1):
		att_x, att_y = calc_attractive_force(x,y,goal_x,goal_y)
		rep_x, rep_y = calc_repulsive_force(x,y,obs)

		pot_x = att_x+rep_x
		pot_y = att_y+rep_y

		x = x + pot_x
		y = y + pot_y

		trace_x.append(x)
		trace_y.append(y)

		error = np.linalg.norm([goal_x-x,goal_y-y])

		if error < 1:
			plt.plot(obs[:,0],obs[:,1],'bo')
			plt.plot(trace_x,trace_y,'go',goal_x,goal_y,'ro')
			plt.show()
			break

def main():
	print("Artificial Potential Field Start!!")

	start_x, start_y = 0.0, 0.0
	goal_x, goal_y = 30.0, 30.0

	obs = np.array([[15.0,14.0],
					[10.0,11.0]])

	Artificial_Potention_Field(start_x,start_y,goal_x,goal_y,obs)

if __name__=='__main__':
	print('__file__'+" start!!")
	main()
	print('__file__'+" Done!!")

 

Reference code

[1] https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/PotentialFieldPlanning/potential_field_planning.py