モノを追うということ・ぶつかるということ
※こちらは「みす51代 Advent Calendar 2018」の12月24日分の記事となります。
いっけなーい 遅刻遅刻ー(cv.江原正士)
絶起して焦って走ると曲がり角でぶつかるよね
というわけで2018年年始にやったクソアニメの中から一幕です。
閑話休題、今回の話題は「モノを追うということ・ぶつかるということ」についてです。
ある移動するTargetを追跡してぶつけるというTrackerを考えるとき、どのようにTrackerを操作すればTargetにぶつけられるかというアルゴリズムについて考えたいと思います。
早速ですが、物体を追うアルゴリズムの代表的なものには以下の二つあります。
- 純粋追尾航法 (PPN: Pure Pursuit Navigation, 別名:猟犬と兎のコース)
- 比例航法 (PN:Proportional Navigation)
順番に見ていきましょう
- 純粋追尾航法
別名を猟犬と兎のコースとも呼ばれる純粋追尾航法は、Trackerの速度ベクトルをTargetへのベクトルに一致させるように追跡する方法です。つまり、相手のいる位置めがけて追跡をかけます。
Pythonで等速直線運動を行うTargetに対して純粋追尾航法に従って移動をするTracerを実装してみると以下のようになります。
import math import numpy as np import matplotlib.pyplot as plt import matplotlib.animation as animation #set target pos and vel t_x0 = 1000.0 #[m] t_y0 = 1000.0 #[m] t_vx = -10.0 #[m/s] t_vy = 0 #[m/s] #set chaser pos and vel c_x0 = 0.0 #[m] c_y0 = 0.0 #[m] c_v = 15.0 #[m/s] #set simulation time parameter max_t = 300.0 #[s] dlt_t = 0.1 #[s] t = [0] #[s] end_phase = (int)(max_t/dlt_t) #initialize array of target & chaser pos t_x = [t_x0] #[array of m] t_y = [t_y0] #[array of m] c_x = [c_x0] #[array of m] c_y = [c_y0] #[array of m] diff_x = t_x[-1] - c_x[-1] #[m] diff_y = t_y[-1] - c_y[-1] #[m] dis = [math.sqrt(diff_x ** 2 + diff_y ** 2)] #[m] n_vecx = diff_x / dis[-1] n_vecy = diff_y / dis[-1] c_vx = [n_vecx * c_v] #[array of m/s] c_vy = [n_vecy * c_v] #[array of m/s] c_ax = [0] #[array of m/s^2] c_ay = [0] #[array of m/s^2] acc = [0.0] #[array of m/s^2] #initialize flight time ft = [0.0] def pure_pursuit(): ft_flag = True for i in range(end_phase): t.append(t[-1] + dlt_t) #cal normal argument vector diff_x = t_x[-1] - c_x[-1] diff_y = t_y[-1] - c_y[-1] dis.append(math.sqrt(diff_x ** 2 + diff_y ** 2)) n_vecx = diff_x / dis[-1] n_vecy = diff_y / dis[-1] c_vx.append(c_v * n_vecx) c_vy.append(c_v * n_vecy) #cal acc c_ax.append((c_vx[-1]-c_vx[-2])/dlt_t) c_ay.append((c_vy[-1]-c_vy[-2])/dlt_t) #cal loss enegy acc.append(math.sqrt(c_ax[-1] ** 2+c_ay[-1] ** 2)) t_x.append(t_x[-1] + t_vx * dlt_t) t_y.append(t_y[-1] + t_vy * dlt_t) c_x.append(c_x[-1] + c_vx[-1] * dlt_t) c_y.append(c_y[-1] + c_vy[-1] * dlt_t) if ft_flag: ft.append(ft[-1] + dlt_t) if(ft_flag and dis[-2] < dis[-1]): flighttime = i*dlt_t ft_flag = False print(flighttime) pure_pursuit() fig, (graph1, graph2,graph3,graph4) = plt.subplots(ncols=4, figsize=(10,4)) graph1.plot(t_x,t_y) graph1.plot(c_x,c_y) graph2.plot(ft,c_vx[:len(ft)]) graph2.plot(ft,c_vy[:len(ft)]) graph3.plot(ft[:len(ft)-1],c_ax[:len(ft)-1]) graph3.plot(ft[:len(ft)-1],c_ay[:len(ft)-1]) graph4.plot(ft[:len(ft)-1],acc[:len(ft)-1])
実行してみるとこんな感じ
左から
1.Target(青線),Tracker(橙線)の軌跡
2.Targetの速度 青線:x成分 橙線:y成分
3.Targetの加速度 青線:x成分 橙線:y成分
4.Targetの総加速度(値が小さいほうがエネルギー損失が低い)
純粋比例航法はその計算式の容易さから実装が簡単ですが、追尾の最終段階では必ずTargetの後ろに回り込むという特徴があります(証明は省きます)
そのため出合い頭で追尾しようとすると下の図のように追尾の最終段階で急激に回り込む軌道を取るようになってしまいます。
- 比例航法
比例航法の概要図 [1]
比例航法は、TrackerからみたTargetの角度(見越し角)をあらかじめ決めておいた値を推移するように制御することで追尾を行うものです。
あらかじめ決めておく値は、一般的には定数:0におきます。(つまり、ずっとTargetが同じ角度に見えるように運動を行うようにします。)
例えば上図の概要図ではΦ<γ であるため、Trackerは、より左側に舵を切りながら移動をすればよいということになります。
(余談:見越し角を一定にしているとやがて衝突するというのは日常生活においても発生しており、見通しのいい田舎道で車が正面衝突する現象はまさしくこれです。コリジョンコース現象と呼ばれています。)
比例航法の速度ベクトルの角度は以下のように計算されます
Nは比例定数と呼ばれています
このNをいじってやることでどのぐらいの速さで運動を収束に向かわせるかを決めてやることができます。
ただ、実際の比例航法においてはTrackerの運動性能を考慮して緩やかに真正面に見えるように調整するなど様々な最適化法があるようです。 [2]
今回はその比例航法をガチガチに最適化したモデルを使ってその運動を見てみましょう。
MATLABが提供している「aero_guidance」コマンドから提供される自動操縦ミサイルについての運動を見てみます。[3]
図:ミサイルシミュレーション
図:Target,Missleの軌跡
図:見越し角推移
こんどは後ろに回り込まないことが軌跡からわかり、見越し角推移からは二回目の×印(比例航法モードに入ったことを表す)からおおよそ見越し角を一定に保たれていることがわかります。
- まとめ
・追尾アルゴリズムは純粋追尾航法と比例航法がある
・純粋追尾航法は実装簡単
・比例航法は性能がえぐい
・MATLABたのちい
というわけで標準機能でミサイルシミュレーションが入っているMATLABをクリスマスに買いましょう(こちらから買えます→
https://jp.mathworks.com/store/link/products/home/ML
)ということで今日は失礼いたします。
[1] 航法
[2]
www.youtube.com
[3]
jp.mathworks.com