[PYTHON] Explanation and implementation of SocialFoceModel

I will explain the SocialFoceModel, which is a type of artificial potential method. This is a model of human crowd behavior based on the dynamics advocated by Helbing.

Click here for the original paper https://arxiv.org/pdf/cond-mat/9805244.pdf

It is basically expressed as the sum of attractive force and repulsive force as shown below.

F_{all} = F_{gall} + F_{others} + F_{obstacle}

Attraction

Here is the formula of the attractive force that works between the pedestrian and the goal.

F_{all} =\frac{V_0e_t-v_t}{\tau}

$ V_0: Maximum speed $ $ e_t: Unit vector in the direction of the goal $ $ \ tau: Time to reach V_0 (time constant) $ $ v_t: Current velocity vector $

repulsive force

Here is the formula for the repulsive force that acts between an obstacle or other pedestrian and the pedestrian. The formulas used are the same.

F_{others}  = F_{obstacle} =  v_rUe^{-\frac{d}{R}}

$ d: Distance to obstacles and other pedestrians (excluding radius) $ $ R, U: Constant $ $ v_r: Direction from obstacle (unit vector) $

The parameters set by the author Helbing are

V_0:1.34 [m/s] \tau:0.5 [s] R:10.0[m^2/s^2] U:0.2[m]

It has become.

Implementation

From this formula and parameters, I would like to display the trajectory when a pedestrian avoids an obstacle.

Normally, you have to consider obstacles and pedestrian radii, but I have omitted it for the sake of simplicity.

# coding: utf-8
import numpy as np
import matplotlib.pyplot as plt

V0  = 1.34 # [m/s] Speed of equilibrium (= max speed)
tau = 0.5  # [sec] Time to reach V0 ziteisuu
delta_sec = 0.01 #[sec]

U = 10.0 # [m^2/s^2]
R = 0.2  # [m]

user_mass_kg = 55 # [kg]
user_mass_N = user_mass_kg / 9.8 # [N]

user_pos = np.array([0.0, 0.0]) # 0=x, 1=y[m]
user_vel = np.array([0.3, 0.0]) # 0=x, 1=y[m/s]
map_size = np.array([10, 10]) # 0=x, 1=y[m]
goal_pos = np.array([10, 5]) # 0=x, 1=y[m]
obstacle_pos = np.array([6, 3]) # 0=x, 1=y[m]

user_pos_lines = user_pos.reshape((2,1))


while True:
	#Attraction
	dist_xy = goal_pos - user_pos
	dist_norm = np.linalg.norm(dist_xy, ord=1)
	e = dist_xy / dist_norm
	F_goal = (V0 * e - user_vel) / tau # [N]
	
	#repulsive force
	dist_xy = user_pos - obstacle_pos
	dist_norm = np.linalg.norm(dist_xy, ord=2)
	v_r = dist_xy / dist_norm
	F_obstacle = v_r * U * np.exp((-1) * dist_norm / R)
	
	#Pedestrian position calculation
	F_all = F_goal + F_obstacle # [N]
	#acceleration[m/s^2]
	user_acc = F_all / user_mass_N
	#speed[m/s]
	user_vel += user_acc * delta_sec
	#position
	user_pos += user_vel * delta_sec
	
	#The distance between the pedestrian and the goal is 0.Finish when it is less than 2m
	if np.linalg.norm(goal_pos - user_pos) < 0.2:
		break
	user_pos_lines = np.append(user_pos_lines, user_pos.reshape((2,1)), axis=1)


#drawing
#Obstacle
plt.scatter(obstacle_pos[0], obstacle_pos[1], color='b', s=100)
#Trajectory
plt.plot(user_pos_lines[0,:], user_pos_lines[1,:], color='r')
#goal
plt.scatter(goal_pos[0], goal_pos[1], color='g', s=200)
plt.show()

print("end")

Execution result

In this way, I was able to create a trajectory to avoid obstacles while heading toward the goal.