人工ポテンシャル法の一種であるSocialFoceModelの解説を行います。
こちらはHelbingが提唱した力学に基づく人間の群衆行動のモデルです。

元論文はこちら
https://arxiv.org/pdf/cond-mat/9805244.pdf

基本的には以下のように引力と斥力の総和で表されます。


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

引力

歩行者とゴールとの間に働く引力の式がこちらになります。


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

$V_0:速度の最大値$
$e_t:ゴール方向への単位ベクトル$
$\tau:V_0に到達するまでの時間（時定数）$
$v_t:現在の速度ベクトル$

###斥力
障害物や他の歩行者と、歩行者との間に働く斥力の式がこちらになります。
用いられる式は同じです。


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

$d:障害物や他の歩行者との距離（半径を除く）$
$R,U:定数$
$v_r:障害物からの向き（単位ベクトル）$

著者のHelbingが設定しているパラメータは

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

となっています。

実装

この式とパラメータから、歩行者が障害物回避したときの軌跡を表示してみたいと思います。

本来であれば障害物や歩行者の半径を考慮させなければいけませんが、簡単のためにあえて抜かしてあります。

# 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:
	# 引力
	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]
	
	# 斥力
	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)
	
	# 歩行者の位置計算
	F_all = F_goal + F_obstacle # [N]
	# 加速度 [m/s^2]
	user_acc = F_all / user_mass_N
	# 速度[m/s]
	user_vel += user_acc * delta_sec
	# 位置
	user_pos += user_vel * delta_sec
	
	# 歩行者とゴールの距離が0.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)


# 描画
# 障害物
plt.scatter(obstacle_pos[0], obstacle_pos[1], color='b', s=100)
# 軌跡
plt.plot(user_pos_lines[0,:], user_pos_lines[1,:], color='r')
# ゴール
plt.scatter(goal_pos[0], goal_pos[1], color='g', s=200)
plt.show()

print("end")

実行結果

このように、ゴールに向かいながら障害物を回避する軌跡をつくることができました。