ikarosの作業場

飛行機の設計もできる系のプログラマー。なおこの記事は個人的見解であり、所属する組織の意見とは一切関係がありません

モノを追うということ・ぶつかるということ

※こちらは「みす51代 Advent Calendar 2018」の12月24日分の記事となります。

f:id:ikarostech:20181223223250p:plain
いっけなーい 遅刻遅刻ー(cv.江原正士)

f:id:ikarostech:20181223223428p:plain

絶起して焦って走ると曲がり角でぶつかるよね

というわけで2018年年始にやったクソアニメの中から一幕です。


閑話休題、今回の話題は「モノを追うということ・ぶつかるということ」についてです。

ある移動するTargetを追跡してぶつけるというTrackerを考えるとき、どのようにTrackerを操作すればTargetにぶつけられるかというアルゴリズムについて考えたいと思います。


早速ですが、物体を追うアルゴリズムの代表的なものには以下の二つあります。

  • 純粋追尾航法 (PPN: Pure Pursuit Navigation, 別名:猟犬と兎のコース)
  • 比例航法 (PN:Proportional Navigation)

順番に見ていきましょう

  • 純粋追尾航法

別名を猟犬と兎のコースとも呼ばれる純粋追尾航法は、Trackerの速度ベクトルをTargetへのベクトルに一致させるように追跡する方法です。つまり、相手のいる位置めがけて追跡をかけます。

f:id:ikarostech:20181223225923p:plain

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])

実行してみるとこんな感じ
f:id:ikarostech:20181223225945p:plain

左から
1.Target(青線),Tracker(橙線)の軌跡
2.Targetの速度 青線:x成分 橙線:y成分
3.Targetの加速度 青線:x成分 橙線:y成分
4.Targetの総加速度(値が小さいほうがエネルギー損失が低い)

純粋比例航法はその計算式の容易さから実装が簡単ですが、追尾の最終段階では必ずTargetの後ろに回り込むという特徴があります(証明は省きます)
そのため出合い頭で追尾しようとすると下の図のように追尾の最終段階で急激に回り込む軌道を取るようになってしまいます。
f:id:ikarostech:20181223230956p:plain

  • 比例航法

f:id:ikarostech:20181223224522p:plain
比例航法の概要図 [1]
比例航法は、TrackerからみたTargetの角度(見越し角)をあらかじめ決めておいた値を推移するように制御することで追尾を行うものです。
あらかじめ決めておく値は、一般的には定数:0におきます。(つまり、ずっとTargetが同じ角度に見えるように運動を行うようにします。)
例えば上図の概要図ではΦ<γ であるため、Trackerは、より左側に舵を切りながら移動をすればよいということになります。

(余談:見越し角を一定にしているとやがて衝突するというのは日常生活においても発生しており、見通しのいい田舎道で車が正面衝突する現象はまさしくこれです。コリジョンコース現象と呼ばれています。)

比例航法の速度ベクトルの角度は以下のように計算されます
f:id:ikarostech:20181223232639p:plain

Nは比例定数と呼ばれています
このNをいじってやることでどのぐらいの速さで運動を収束に向かわせるかを決めてやることができます。


ただ、実際の比例航法においてはTrackerの運動性能を考慮して緩やかに真正面に見えるように調整するなど様々な最適化法があるようです。 [2]

今回はその比例航法をガチガチに最適化したモデルを使ってその運動を見てみましょう。

MATLABが提供している「aero_guidance」コマンドから提供される自動操縦ミサイルについての運動を見てみます。[3]

f:id:ikarostech:20181223233010p:plain
図:ミサイルシミュレーション

f:id:ikarostech:20181223233328p:plain
図:Target,Missleの軌跡

f:id:ikarostech:20181223233254p:plain
図:見越し角推移

こんどは後ろに回り込まないことが軌跡からわかり、見越し角推移からは二回目の×印(比例航法モードに入ったことを表す)からおおよそ見越し角を一定に保たれていることがわかります。

  • まとめ

・追尾アルゴリズムは純粋追尾航法と比例航法がある
・純粋追尾航法は実装簡単
・比例航法は性能がえぐい
MATLABたのちい

というわけで標準機能でミサイルシミュレーションが入っているMATLABをクリスマスに買いましょう(こちらから買えます→
https://jp.mathworks.com/store/link/products/home/ML
)ということで今日は失礼いたします。

[1] 航法

[2]
www.youtube.com

[3]
jp.mathworks.com

当サイトのソースコード及びその他の情報は個人・商用問わず自由に使っていただいてかかまいませんが、当サイトの情報が元で発生したいかなる結果・不利益については責任を負いかねますのでご了承ください