JP5528362B2 - 目標追尾装置及び目標追尾方法 - Google Patents
目標追尾装置及び目標追尾方法 Download PDFInfo
- Publication number
- JP5528362B2 JP5528362B2 JP2011008740A JP2011008740A JP5528362B2 JP 5528362 B2 JP5528362 B2 JP 5528362B2 JP 2011008740 A JP2011008740 A JP 2011008740A JP 2011008740 A JP2011008740 A JP 2011008740A JP 5528362 B2 JP5528362 B2 JP 5528362B2
- Authority
- JP
- Japan
- Prior art keywords
- route
- acceleration
- avoidance
- entry
- waypoint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Radar Systems Or Details Thereof (AREA)
Description
例えば、航空管制では、従来の3次元位置による航法に対して、3次元位置と時間を含む4次元軌道(4DT:4 Dimensional Trajectory)を航法に取り入れる検討がなされている。
この4DTを管理することで、航空交通量や空域容量の予測が可能となることから、運航に対する安全性の向上が期待される。
航空機が等速直進運動によって、飛行ルート上の経由点へ向かう場合には、航空機が経由点に至るまでの飛行ルート上の将来位置や将来到達時刻などを容易に算出することができる。
しかし、実際には、気象状況の悪化(雷雲などの発生)に伴って、飛行ルートの変更を余儀なくされる場合がある。
雷雲などの進入禁止域への進入を回避するために飛行ルートが変更された場合、航空機の将来位置や将来到達時刻が変更されるため、その将来位置や将来到達時刻を再度推定する必要がある。
この目標追尾装置では、移動体が等速直進運動によって移動すると仮定して、移動体の現在の速度と機首方向から将来位置を推定するようにしているが、移動体が雷雲などの進入禁止域を回避するために経路を変更すると、等速直進運動による移動ではなくなるため、将来位置の推定精度が悪化することがある。
この目標追尾装置では、センサにより観測された位置と追尾予測位置との差(残差)を用いて、複数の運動モデルの中から適切な運動モデルを選択するようにしている。
しかし、運動モデルを選択する際、雷雲などの進入禁止域の位置を考慮していないため、移動体が雷雲などの進入禁止域を回避する必要がある場合などには、将来位置の推定精度が悪化することがある。
A*アルゴリズムは、進入禁止域(障害物)を含む経路候補の移動空間をメッシュ上に細分化して、出発地点からゴール地点(または、経由点)までのノードを決定するものであり、経路を決定する際、移動体の運動を考慮せずに、格子点によって決定される。
このため、自然な経路を得るには、格子点の間隔を細かく設定する必要があり、多くの処理時間を要することがある。
このファジィ推論では、地形を回避するのに必要な操舵量と、経由点に到達するのに必要な操舵量とを入力して、低空飛行を行う飛翔体の操舵量を決定している。
この技術の適用先として、目標がレーダ覆域外となり、観測値が得られない場合の長時間予測技術などが考えられる。
ただし、ファジィ推論におけるルールテーブルやメンバーシップ関数の設定は、試行錯誤による経験的な手法により行われることが多い。
このため、ルール数が増大するにしたがって設定作業が困難になることが想定される。また、将来位置の推定精度はファジィ推論のルールテーブルに依存する問題があった。
図1はこの発明の実施の形態1による目標追尾装置を示す構成図である。
図1の目標追尾装置は、航空機、船舶、車両などの移動体を追尾する装置であるが、この実施の形態1では、移動体が航空機である例を説明する。
図1において、追尾処理部1は例えばカルマンフィルタなどの追尾フィルタを用いて、追尾対象の移動体の観測値(例えば、レーダやGPSなどのセンサから得られる移動体の位置などを示す観測情報)から、現時刻tkにおける移動体の位置及び速度を示す状態ベクトルxkを算出する処理を実施する。なお、追尾処理部1は位置速度算出手段を構成している。
経由点位置情報格納部3は例えばRAMやハードディスクなどの記憶装置から構成されており、移動体の経由点の位置を示す経由点位置情報を格納している。
進入禁止域位置情報格納部4は例えばRAMやハードディスクなどの記憶装置から構成されており、雷雲などの進入禁止域(障害物)の位置を示す進入禁止域位置情報を格納している。
経由点指向経路特定部12は追尾処理部1により算出された状態ベクトルxkと指向用加速度算出部11により算出された経由点指向用の加速度ベクトルukを上記運動モデルに適用することで、移動体が経由点に至るまでの経路である経由点指向経路を特定する処理を実施する。なお、経由点指向経路特定部12は経由点指向経路特定手段を構成している。
回避用加速度算出部14は進入禁止域衝突判定部13により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度(水平方向の進入回避用加速度v’g,鉛直方向の進入回避用加速度w’g)を算出する処理を実施する。なお、回避用加速度算出部14は回避用加速度算出手段を構成している。
移動経路予測部16は進入禁止域衝突判定部13により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部15により特定された回避経路が移動体の移動経路であると予測する一方、進入禁止域衝突判定部13により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定部12により特定された経由点指向経路が移動体の移動経路であると予測する処理を実施する。
また、移動経路予測部16は移動体が予測した移動経路上を移動して経由点に到達する時刻を算出する処理を実施する。
なお、移動経路予測部16は移動経路予測手段及び到達時刻算出手段を構成している。
図2はこの発明の実施の形態1による目標追尾装置の処理内容を示すフローチャートである。
追尾処理部1は、例えばカルマンフィルタなどの追尾フィルタを実装しており、レーダやGPSなどのセンサから追尾中の移動体の観測値(移動体の位置などを示す観測情報)を受けると、その観測値に基づいて現時刻tkにおける移動体の位置及び速度を示す状態ベクトルxkを算出する(図2のステップST1)。
ここでは、追尾処理部1がカルマンフィルタを実装している例を示しているが、追尾フィルタはカルマンフィルタに限るものではなく、他の公知のフィルタを実装するようにしてもよい。
式(2)において、xkは時刻tkにおける状態ベクトル(位置・速度)、Φkは時刻tkから時刻tk+1に推移する際の既知の状態ベクトル推移行列、ukは運動座標における加速度ベクトル、Γkは加速度ベクトルukの運動座標から北基準直交座標に変換する際の既知の変換行列である。
移動体は加速度ベクトルukが設定されることで、図3に示すように、直線的に移動している状態から、左旋回/右旋回を行う状態、上昇/下降を行う状態又は加速/減速を行う状態に変化する。
加速度ベクトルukは、レーダやGPSなどのセンサからは観測不可能な値であるので、この実施の形態1では、後述する指向用加速度算出部11及び回避用加速度算出部14が算出するようにしている。
以下、経路予測部2の処理内容を具体的に説明する。
指向用加速度算出部11は、追尾処理部1により算出された状態ベクトルxkを用いて、現時刻tkからTa秒後(指定時間後)に移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度ベクトルukを算出する(ステップST2)。
即ち、指向用加速度算出部11は、下記の式(3)〜(5)を計算することで、経由点指向用の加速度ベクトルukを算出する。
即ち、経由点指向経路特定部12は、式(2)に基づいて、現時刻tkからTa秒後における指向終了位置を計算し、Ta秒以降は移動体が等速直線運動で移動するものとして、経由点に至るまでの経由点指向経路を特定する(図6を参照)。
以下、回避用加速度算出部14による進入回避用の加速度の算出処理を具体的に説明する。ただし、ここでは、説明の簡単化のため、移動体が加速及び減速を行わないものとする。
まず、回避用加速度算出部14は、移動体の性能から想定される水平方向の最大加速度を上限として、Δuステップで水平方向の加速度の候補を設定する。
回避用加速度算出部14は、水平方向の加速度の候補を複数設定すると、各加速度の候補で経由点指向経路を変更した場合の経路を仮想して、複数の仮想経路を生成する(図7のステップST21)。
図8は水平方向の仮想経路の設定例を示す説明図であり、図8では、3個の仮想経路を設定している例を示している。
仮想経路が進入禁止域内に入る経路であるか否かの判定は、進入禁止域衝突判定部13と同様に、仮想経路上での各時刻における移動体の位置と進入禁止域位置情報格納部4により格納されている進入禁止域位置情報が示す進入禁止域の位置を比較することで判定することができる。
即ち、回避用加速度算出部14は、移動体が進入禁止域内に進入する進入時刻Tc(図8の例では、2ΔTの時刻)を算出し、その進入時刻Tcにおいて、例えば、Δuステップで、鉛直方向の加速度を大きくすることで、当該仮想経路を鉛直方向に段階的に変更する。
回避用加速度算出部14は、当該仮想経路を鉛直方向に段階的に変更することで、当該仮想経路が進入禁止域内に入る経路から、進入禁止域内に入らない経路に変化する際の鉛直方向の加速度を探索する。
ただし、鉛直方向の最大加速度を設定しても、当該仮想経路が進入禁止域内に入る経路である場合、必要最小限の鉛直方向の加速度として、最大加速度を設定する。
ただし、Hは進入禁止域を回避するために必要な高度差、Tcは進入時刻(ΔTステップで設定した時刻の中で、進入禁止域内に入る最小の時刻)、wg,Hは高度差Hを回避するために必要な鉛直方向の加速度である。
図10は回避用加速度算出部14により生成される回避加速度マップの一例を示す説明図である。
図10の例では、ABCDで囲まれている領域が進入禁止域であり、上記領域の外側の領域が進入禁止域外である。
具体的には、以下のようにして、進入回避用の加速度に用いる加速度の組み合わせを選択する。
図において、vgは経由点指向用の加速度ベクトルukにおける水平方向の加速度、wgは経由点指向用の加速度ベクトルukにおける鉛直方向の加速度、v’gは水平方向の進入回避用加速度、w’gは鉛直方向の進入回避用加速度である。
この実施の形態1では、移動体が民間の航空機である等の場合、鉛直方向の加速度を変化させるよりも、水平方向の加速度を変化させることが、より自然な運航であるため(対地高度を一定とする運航)、水平方向を変化させる組み合わせを優先的に選択するものとする(鉛直方向の加速度が小さい組み合わせを優先的に選択するものとする)。
特に、経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号を持つ水平方向の回避加速度を変化させる組み合わせを優先的に選択するものとする。
「経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号である条件下で、水平方向の回避加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度v’gが経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号を持つ加速度であり、鉛直方向の進入回避用加速度w’gがゼロ(対地高度が一定)である組み合わせを選択する。
このケースは図11に示すケース1に該当し、水平方向の進入回避用加速度v’gは、下記の式(7)に示すように、回避加速度マップの進入禁止域内に進入しない最小限の値vg,LIMに対して、回避量Δvg(Δvgは0以上のオフセット値であって、事前に設定される固定値である)を加えることで算出する(ステップST23,ST24)。
このケースでは、対地高度を一定とするため、鉛直方向の進入回避用加速度w’gはゼロとする。
w’g=0
「経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号である条件下で、水平方向の回避加速度を最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度v’gが経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号を持つ最大加速度であり、鉛直方向の進入回避用加速度w’gが経由点指向用の加速度ベクトルukにおける鉛直方向の加速度wgと同じ符号を持つ加速度の組み合わせを選択する。
このケースは図12に示すケース2に該当し、水平方向の進入回避用加速度v’gは、経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号を持つ最大加速度vg,maxに設定する(ステップST23,ST24)。
v’g=vg,max
鉛直方向の進入回避用加速度w’gは、下記の式(8)に示すように、回避加速度マップの進入禁止域内に進入しない最小限の値wg,LIMに対して、回避量Δwg(Δwgは0以上のオフセット値であって、事前に設定される固定値である)を加えることで算出する(ステップST25,ST26)。
「経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号である条件下で、水平方向の回避加速度を最大値にしても進入禁止域内となるが、その加速度vgと逆符号である条件下で、水平方向の回避加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度v’gが経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと逆の符号を持つ加速度であり、鉛直方向の進入回避用加速度w’gがゼロ(対地高度が一定)である組み合わせを選択する。
このケースは図13に示すケース3に該当し、水平方向の進入回避用加速度v’gは、下記の式(9)に示すように、回避加速度マップの進入禁止域内に進入しない最小限の値vg,LIMに対して、回避量Δvgを加えることで算出する(ステップST23,ST24)。
このケースでは、対地高度を一定とするため、鉛直方向の進入回避用加速度w’gはゼロとする。
w’g=0
「いずれの進入回避用の加速度を選択しても、進入禁止域内となる場合」
水平方向の進入回避用加速度v’gが経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号を持つ最大加速度であり、鉛直方向の進入回避用加速度w’gが経由点指向用の加速度ベクトルukにおける鉛直方向の加速度wgと同じ符号を持つ最大加速度である組み合わせを選択する。
このケースは図14に示すケース4に該当し、水平方向の進入回避用加速度v’gは、経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号を持つ最大加速度vg,maxに設定する(ステップST23,ST27)。
v’g=vg,max
また、鉛直方向の進入回避用加速度w’gは、経由点指向用の加速度ベクトルukにおける鉛直方向の加速度wgと同じ符号を持つ最大加速度wg,maxに設定する(ステップST23,ST27)。
w’g=wg,max
あるいは、上記の経由点と異なる他の経由点について、進入回避用の加速度を設定する処理を実施するようにしてもよい。
即ち、経由点指向用の加速度ベクトルukにおける水平方向の加速度vgの符号と逆の符号を持つ最小限の値vg,LIMが、経由点指向用の加速度ベクトルukにおける水平方向の加速度vgの符号と同じ符号を持つ最小限の値vg,LIMより小さい場合、逆の符号を持つ最小限の値vg,LIMから水平方向の進入回避用加速度v’gを算出するようにしてもよい。
また、ここでは、説明の簡単化のために、移動体が加速及び減速を行わないものとして説明したが、加速及び減速を含めて仮想経路を生成するようにしてもよいことは言うまでもない。
回避経路特定部15は、その回避地点と移動体の速度を追尾航跡の位置・速度とみなすことで、移動体の出発地点(または、前回の回避地点)から回避地点までの経路を回避経路として特定する。
移動経路予測部16は、経由点指向経路特定部12により特定された経由点指向経路が移動体の移動経路であると予測すると、式(2)に基づいて、現時刻tkからTa秒後(指定時間後)における指向終了位置を計算し、Ta秒以降は移動体が等速直線運動で経由点に向かうものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する(図6を参照)。
移動経路予測部16は、回避経路特定部15により特定された回避経路が移動体の移動経路であると予測すると、回避時刻Tvにおける回避地点から経由点までは等速直線運動で移動するものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する。
式(12)において、右辺第2項は式(11)の右辺と同じ意味であり、Tv,iはi−1番目からi番目の回避地点に到達するまでに要する時間である。ただし、i=1である場合に限り、現在位置から1番目の回避地点に到達するまでに要する時間である。
このため、対地速度を状態ベクトルxkの速度成分として用いるようにしてもよい。
予め、目標進行予定の空間における風速データが既知の場合には、風速による状態ベクトルの速度変化を仮想経路の生成や、移動経路の予測に反映することが可能である。
そこで、隣り合う回避地点で、進入回避用加速度が急激に変化する状況の発生を防止するために、予め加速度変化率パラメータΔαを設定し、時間方向に対して、Δα刻みで進入回避用加速度を変化させるようにしてもよい。
上記実施の形態1では、進入禁止域衝突判定部13により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避用加速度算出部14が、その進入禁止域内への進入を回避するために必要な進入回避用の加速度(水平方向の進入回避用加速度v’g,鉛直方向の進入回避用加速度w’g)を算出し、回避経路特定部15が回避用加速度算出部14により算出された進入回避用の加速度を用いて、回避経路を特定するものを示したが、図19に示すように、進入禁止域が進行方向に対して奥行きがある形を成している場合、回避経路特定部15により特定される可能性がある回避経路(図中、実線の経路)が、他の回避経路(図中、点線の経路)よりも経路長が長くなる場合がある。
この実施の形態2では、進入禁止域が進行方向に対して奥行きがある形を成している場合でも、経路長が短い回避経路を特定することができるようにしている。
進入禁止域容積算出部21は進入禁止域衝突判定部13により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域における経由点指向経路の進行方向左側の容積を算出するとともに、その進入禁止域における経由点指向経路の進行方向右側の容積を算出する処理を実施する。なお、進入禁止域容積算出部21は容積算出手段を構成している。
進入禁止域容積算出部21は、進入禁止域衝突判定部13が上記実施の形態1と同様にして、経由点指向経路が進入禁止域内に入る経路であるか否かの判定処理を行うと、その判定結果が、経由点指向経路が進入禁止域内に入る経路である旨を示していれば、進入禁止域位置情報格納部4により格納されている進入禁止域位置情報を参照して、進入禁止域の形状を確認する。
進入禁止域容積算出部21は、進入禁止域の形状を確認すると、その進入禁止域と経由点指向経路特定部12により特定された経由点指向経路の位置関係を把握し、その経由点指向経路に対して、進入禁止域を進行方向左側のエリアと右側のエリアに分断する。
なお、正確な容積を算出することができない場合には、容積に相当する量(例えば、奥行きの長さ×幅)を算出し、容積に相当する量をエリアの容積として回避用加速度算出部22に出力する。
ただし、容積が小さいエリア側の方向に移動体が移動することでは、進入禁止域内への進入を回避することができない場合、容積が大きいエリア側の方向に移動体が移動することで、進入禁止域内への進入を回避するために必要な進入回避用の加速度(水平方向の進入回避用加速度v’g,鉛直方向の進入回避用加速度w’g)を算出する。
図19の例では、進行方向左側のエリアの容積が、進行方向右側のエリアの容積より小さいので、下記に示す優先順位にしたがって、進入回避用の加速度(水平方向の進入回避用加速度v’g,鉛直方向の進入回避用加速度w’g)を算出する。
「水平方向の回避加速度を進行方向左側に変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度v’gは、下記の式(13)に示すように、進行方向左側において、回避加速度マップの進入禁止域内に進入しない最小限の値vg,LIMに対して、回避量Δvg(Δvgは0以上のオフセット値であって、事前に設定される固定値である)を加えることで算出する。
この場合、鉛直方向の進入回避加速度を変化させる必要がないので、鉛直方向の進入回避用加速度w’gはゼロとする。
w’g=0
「水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度v’gは、進行方向左側の向きに最大加速度vg,maxを設定する。
v’g=vg,max
鉛直方向の進入回避用加速度w’gは、下記の式(14)に示すように、進行方向左側において、回避加速度マップの進入禁止域内に進入しない最小限の値wg,LIMに対して、回避量Δwg(Δwgは0以上のオフセット値であって、事前に設定される固定値である)を加えることで算出する。
「水平方向の回避加速度を進行方向左側に最大値にしても進入禁止域内となるが、水平方向の回避加速度を進行方向右側に変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度v’gは、下記の式(15)に示すように、進行方向右側において、回避加速度マップの進入禁止域内に進入しない最小限の値vg,LIMに対して、回避量Δvgを加えることで算出する。
この場合、鉛直方向の進入回避加速度を変化させる必要がないので、鉛直方向の進入回避用加速度w’gはゼロとする。
w’g=0
「水平方向の回避加速度を進行方向右側に最大値にしても進入禁止域内となるが、鉛直方向の進入回避用加速度を変化させることで進入禁止域外となる場合」
水平方向の進入回避用加速度v’gは、進行方向右側の向きに最大加速度vg,maxを設定する。
v’g=vg,max
鉛直方向の進入回避用加速度w’gは、下記の式(16)に示すように、進行方向右側において、回避加速度マップの進入禁止域内に進入しない最小限の値wg,LIMに対して、回避量Δwgを加えることで算出する。
「いずれの進入回避用の加速度を選択しても、進入禁止域内となる場合」
水平方向の進入回避用加速度v’gは、経由点指向用の加速度ベクトルukにおける水平方向の加速度vgと同じ符号を持つ最大加速度vg,maxに設定する。
v’g=vg,max
鉛直方向の進入回避用加速度w’gは、経由点指向用の加速度ベクトルukにおける鉛直方向の加速度wgと同じ符号を持つ最大加速度wg,maxに設定する。
w’g=wg,max
あるいは、上記の経由点と異なる他の経由点について、進入回避用の加速度を設定する処理を実施するようにしてもよい。
これにより、回避用加速度算出部22により高い優先順位1,2で進入回避用加速度が算出されている場合、進入禁止域が進行方向に対して奥行きがある形を成していても、経路長が短い経路(図中、点線の経路)が回避経路となる。
移動経路予測部16の処理内容は、上記実施の形態1と同様であるため説明を省略する。
上記実施の形態1,2では、回避経路特定部15が1つの回避経路を特定し、経由点指向経路が進入禁止域内に入る経路であれば、移動経路予測部16が回避経路特定部15により特定された回避経路が移動体の移動経路であると予測するものを示したが、回避経路特定部15が複数の回避経路を特定し、経由点指向経路が進入禁止域内に入る経路であれば、移動経路予測部16が回避経路特定部15により特定された複数の回避経路が移動体の移動経路であると予測し、複数の移動経路の中で、経由点の到達時刻Tp又は進入回避用の加速度が最小となる移動経路を選択するようにしてもよい。
回避用加速度算出部31は進入禁止域衝突判定部13により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度(水平方向の進入回避用加速度v’g,鉛直方向の進入回避用加速度w’g)を算出する処理を実施する。
回避用加速度算出部31における進入回避用の加速度の算出方法は、図1の回避用加速度算出部14の算出方法と同一であってもよいし、図18の回避用加速度算出部22の算出方法と同一であってもよいが、回避用加速度算出部31では、進入回避用の加速度を複数算出する。
なお、回避用加速度算出部31は回避用加速度算出手段を構成している。
移動経路予測部33は進入禁止域衝突判定部13により経由点指向経路が進入禁止域内に入る経路であると判定された場合、回避経路特定部32により特定された複数の回避経路が移動体の移動経路であると予測する一方、進入禁止域衝突判定部13により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、経由点指向経路特定部12により特定された経由点指向経路が移動体の移動経路であると予測する処理を実施する。
また、移動経路予測部33は移動体が予測した移動経路上を移動して経由点に到達する時刻を算出する処理を実施する。
なお、移動経路予測部33は移動経路予測手段及び到達時刻算出手段を構成している。
図21はこの発明の実施の形態3による目標追尾装置の移動経路選択部34の処理内容を示すフローチャートである。
回避用加速度算出部31は、進入禁止域衝突判定部13により経由点指向経路が進入禁止域内に入る経路であると判定された場合、その進入禁止域内への進入を回避するために必要な進入回避用の加速度(水平方向の進入回避用加速度v’g,鉛直方向の進入回避用加速度w’g)を複数算出する。
回避用加速度算出部31における進入回避用の加速度の算出方法は、特に問わないが、図1の回避用加速度算出部14と同様にして、例えば、図22に示すように、第1の進入回避用加速度(水平方向の進入回避用加速度v’g1,鉛直方向の進入回避用加速度w’g1)を算出し、図18の回避用加速度算出部22と同様にして、第2の進入回避用加速度(水平方向の進入回避用加速度v’g2,鉛直方向の進入回避用加速度w’g2)を算出する。
以降の説明では、説明の簡単化のため、図22に示すように、回避用加速度算出部31が、2つの進入回避用加速度(第1の進入回避用加速度、第2の進入回避用加速度)を算出するものとするが、3以上の進入回避用加速度を算出するようにしてもよい。
また、第2の進入回避用加速度を加速度ベクトルukとして、その加速度ベクトルukと状態ベクトルxkを式(2)の運動モデルに適用することで、移動体が進入禁止域内に入らずに経由点に至る第2の回避経路を特定する。
言うまでもないが、回避用加速度算出部31がN個の進入回避用加速度を算出していれば、回避経路特定部32がN個の回避経路を特定する。
移動経路予測部33は、経由点指向経路特定部12により特定された経由点指向経路が移動体の移動経路であると予測すると、図1の移動経路予測部16と同様に、現時刻tkからTa秒後における指向終了位置を計算し、Ta秒以降は移動体が等速直線運動で経由点に向かうものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する。
また、移動経路予測部33は、図1の移動経路予測部16と同様に、その指向終了位置と経由点の間の距離Raと、その指向終了位置における移動体の速度Vaとから、移動体が経由点に到達する時刻Tpを算出する。
移動経路予測部33は、回避経路特定部32により特定された第1及び第2の回避経路が移動体の移動経路であると予測すると、回避時刻Tvにおける回避地点から経由点までは等速直線運動で移動するものとして、任意の時刻における移動体の位置を算出することで、移動体の現在位置から経由点まで移動経路を最終的に特定する。
また、移動経路予測部33は、図1の移動経路予測部16と同様にして、移動体が第1の回避経路上を移動する場合の経由点到達時刻Tp,1を算出し、また、移動体が第2の回避経路上を移動する場合の経由点到達時刻Tp,2を算出する。
一方、移動経路予測部33により第1及び第2の回避経路が移動体の移動経路であると予測された場合、移動体が第1の回避経路上を移動する場合の経由点到達時刻Tp,1と、移動体が第2の回避経路上を移動する場合の経由点到達時刻Tp,2との時刻差ΔTpを算出する(図21のステップST31)。
例えば、経由点到達時刻Tp,1が経由点到達時刻Tp,2より早ければ、第1の移動経路(第1の回避経路)と経由点到達時刻Tp,1を出力する。一方、経由点到達時刻Tp,2が経由点到達時刻Tp,1より早ければ、第2の移動経路(第2の回避経路)と経由点到達時刻Tp,2を出力する。
例えば、進入回避用加速度w’g1が進入回避用加速度w’g2より小さければ、第1の移動経路(第1の回避経路)と経由点到達時刻Tp,1を出力する。一方、進入回避用加速度w’g2が進入回避用加速度w’g1より小さければ、第2の移動経路(第2の回避経路)と経由点到達時刻Tp,2を出力する。
例えば、進入回避用加速度v’g1が進入回避用加速度v’g2より小さければ、第1の移動経路(第1の回避経路)と経由点到達時刻Tp,1を出力する。一方、進入回避用加速度v’g2が進入回避用加速度v’g1より小さければ、第2の移動経路(第2の回避経路)と経由点到達時刻Tp,2を出力する。
Claims (10)
- 追尾対象の移動体の観測値を用いて、現時刻における上記移動体の位置及び速度を算出する位置速度算出手段と、上記位置速度算出手段により算出された位置及び速度を用いて、指定時間後に上記移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出手段と、上記位置速度算出手段により算出された位置及び速度と上記指向用加速度算出手段により算出された経由点指向用の加速度を用いて、上記移動体が上記経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定手段と、上記経由点指向経路特定手段により特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定手段と、上記判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出手段と、上記回避用加速度算出手段により算出された進入回避用の加速度と上記位置速度算出手段により算出された位置及び速度を用いて、上記移動体が上記進入禁止域内に入らずに上記経由点に至る回避経路を特定する回避経路特定手段と、上記判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記回避経路特定手段により特定された回避経路が上記移動体の移動経路であると予測する一方、上記判定手段により経由点指向経路が進入禁止域内に入らない経路であると判定された場合、上記経由点指向経路特定手段により特定された経由点指向経路が上記移動体の移動経路であると予測する移動経路予測手段とを備えた目標追尾装置。
- 移動体が移動経路予測手段により予測された移動経路上を移動して経由点に到達する時刻を算出する到達時刻算出手段を設けたことを特徴とする請求項1記載の目標追尾装置。
- 回避用加速度算出手段は、現時刻における移動体の進行方向を基準とする複数種類の運動モデルを考慮して、進入禁止域内への進入を回避するために必要な最小限の進入回避用の加速度を算出することを特徴とする請求項1または請求項2記載の目標追尾装置。
- 回避用加速度算出手段は、移動体に対する水平方向の加速度の候補を複数用意し、各加速度の候補で経由点指向経路を変更した場合の経路を仮想して、当該仮想の経路が進入禁止域内に入る経路であれば、上記進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を算出する一方、当該仮想の経路が進入禁止域内に入らない経路であれば、上記鉛直方向の加速度をゼロに設定し、
上記移動体に対する水平方向及び鉛直方向の複数の加速度の組み合わせの中から、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を選択することを特徴とする請求項1から請求項3のうちのいずれか1項記載の目標追尾装置。 - 回避用加速度算出手段は、仮想の経路が進入禁止域内に入る経路であれば、移動体が上記進入禁止域内に進入する進入時刻を算出し、その進入時刻と当該進入時刻における進入禁止域の高さを用いて、上記進入禁止域内への進入を回避するために必要な最小限の鉛直方向の加速度を算出することを特徴とする請求項4記載の目標追尾装置。
- 回避用加速度算出手段は、水平方向及び鉛直方向の複数の加速度の組み合わせのうち、鉛直方向の加速度が小さい組み合わせを優先的に選択することを特徴とする請求項4または請求項5記載の目標追尾装置。
- 回避用加速度算出手段は、水平方向及び鉛直方向の複数の加速度の組み合わせの中に、鉛直方向の加速度がゼロの組み合わせが1以上ある場合、鉛直方向の加速度がゼロの組み合わせの中から、水平方向の加速度が経由点指向用の加速度と同じ符号である組み合わせを優先的に選択することを特徴とする請求項4または請求項5記載の目標追尾装置。
- 判定手段により経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記進入禁止域における上記経由点指向経路の進行方向左側の容積及び進行方向右側の容積を算出する容積算出手段を設け、
回避用加速度算出手段は、上記容積算出手段により算出された進行方向左側の容積及び進行方向右側の容積のうち、容積が小さい側の方向に移動体が移動することで、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する
ことを特徴とする請求項1または請求項2記載の目標追尾装置。 - 回避用加速度算出手段が進入禁止域内への進入を回避するために必要な進入回避用の加速度を複数算出し、回避経路特定手段が上記回避用加速度算出手段により算出された複数の加速度を用いて、複数の回避経路を特定し、移動経路予測手段が上記回避経路特定手段により特定された複数の回避経路が移動体の移動経路であると予測する場合、
上記移動経路予測手段により予測された複数の移動経路の中で、到達時刻算出手段により算出された到達時刻又は上記回避用加速度算出手段により算出された進入回避用の加速度が最小となる移動経路を選択する移動経路選択手段を設けた
ことを特徴とする請求項2記載の目標追尾装置。 - 位置速度算出手段が追尾対象の移動体の観測値を用いて、現時刻における上記移動体の位置及び速度を算出する位置速度算出処理ステップと、指向用加速度算出手段が上記位置速度算出処理ステップで算出された位置及び速度を用いて、指定時間後に上記移動体の向きを経由点に指向させる際に必要な経由点指向用の加速度を算出する指向用加速度算出処理ステップと、経由点指向経路特定手段が上記位置速度算出処理ステップで算出された位置及び速度と上記指向用加速度算出処理ステップで算出された経由点指向用の加速度を用いて、上記移動体が上記経由点に至るまでの経路である経由点指向経路を特定する経由点指向経路特定処理ステップと、判定手段が上記経由点指向経路特定処理ステップで特定された経由点指向経路が進入禁止域内に入る経路であるか否かを判定する判定処理ステップと、回避用加速度算出手段が上記判定処理ステップで経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記進入禁止域内への進入を回避するために必要な進入回避用の加速度を算出する回避用加速度算出処理ステップと、回避経路特定手段が上記回避用加速度算出処理ステップで算出された進入回避用の加速度と上記位置速度算出処理ステップで算出された位置及び速度を用いて、上記移動体が上記進入禁止域内に入らずに上記経由点に至る回避経路を特定する回避経路特定処理ステップと、移動経路予測手段が上記判定処理ステップで経由点指向経路が進入禁止域内に入る経路であると判定された場合、上記回避経路特定処理ステップで特定された回避経路が上記移動体の移動経路であると予測する一方、上記判定処理ステップで経由点指向経路が進入禁止域内に入らない経路であると判定された場合、上記経由点指向経路特定処理ステップで特定された経由点指向経路が上記移動体の移動経路であると予測する移動経路予測処理ステップとを備えた目標追尾方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011008740A JP5528362B2 (ja) | 2011-01-19 | 2011-01-19 | 目標追尾装置及び目標追尾方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011008740A JP5528362B2 (ja) | 2011-01-19 | 2011-01-19 | 目標追尾装置及び目標追尾方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2012149997A JP2012149997A (ja) | 2012-08-09 |
JP5528362B2 true JP5528362B2 (ja) | 2014-06-25 |
Family
ID=46792374
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2011008740A Active JP5528362B2 (ja) | 2011-01-19 | 2011-01-19 | 目標追尾装置及び目標追尾方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5528362B2 (ja) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP6715899B2 (ja) * | 2018-09-05 | 2020-07-01 | 三菱電機株式会社 | 衝突回避装置 |
-
2011
- 2011-01-19 JP JP2011008740A patent/JP5528362B2/ja active Active
Also Published As
Publication number | Publication date |
---|---|
JP2012149997A (ja) | 2012-08-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10802494B2 (en) | Method for motion planning for autonomous moving objects | |
US10102761B2 (en) | Route prediction device | |
EP4014098B1 (en) | System and method for predictive path planning in autonomous vehicles | |
Kunz et al. | Autonomous driving at Ulm University: A modular, robust, and sensor-independent fusion approach | |
WO2020243162A1 (en) | Methods and systems for trajectory forecasting with recurrent neural networks using inertial behavioral rollout | |
JP6207723B2 (ja) | 衝突防止装置 | |
CN108698595A (zh) | 用于控制车辆运动的方法和车辆的控制系统 | |
US11055540B2 (en) | Method for determining anchor boxes for training neural network object detection models for autonomous driving | |
JP6316146B2 (ja) | 移動物体位置予測装置、移動物体位置予測方法及び移動物体位置予測用コンピュータプログラム | |
WO2018235159A1 (ja) | 経路予測装置および経路予測方法 | |
CN113625762B (zh) | 无人机避障方法及系统、无人机集群避障方法及系统 | |
CN111752294B (zh) | 一种飞行控制方法及相关装置 | |
CN116448115B (zh) | 基于导航雷达和光电的无人艇概率距离地图构建方法 | |
CN115060263A (zh) | 一种考虑低空风和无人机能耗的航迹规划方法 | |
Farmani et al. | Optimal UAV sensor management and path planning for tracking multiple mobile targets | |
JP5528362B2 (ja) | 目標追尾装置及び目標追尾方法 | |
JP7200970B2 (ja) | 車両制御装置 | |
Lu et al. | An information potential approach for tracking and surveilling multiple moving targets using mobile sensor agents | |
Frew et al. | Trajectory generation for constant velocity target motion estimation using monocular vision | |
WO2023129890A1 (en) | Integrated trajectory forecasting, error estimation, and vehicle handling when detecting an observed scenario | |
CN114740882A (zh) | 一种无人机保证可视性的弹性目标跟踪的轨迹生成方法 | |
US20210262819A1 (en) | A mixed regular and open-space trajectory planning method for autonomous driving vehicle | |
KR101745506B1 (ko) | 표적 추적을 위한 센서 유도 방법, 및 이를 이용한 센서 유도 시스템과 공중 비행체 | |
CN114003027A (zh) | 移动体控制装置、移动体、移动体控制方法及存储介质 | |
Barz et al. | Near-Term Flight Path Adaption for Tilt-Wing Aircraft Obstacle Avoidance |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20130918 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20140226 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20140318 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20140415 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5528362 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |