(57)【要約】【課題】航法誤差が小さくなるように誘導軌道を最適化することができる誘導制御プログラムを提供すること。【解決手段】誘導制御プログラムは、複数のセンサを備える移動体に搭載されるコンピュータに、前記複数のセンサに含まれる第１センサと第２センサとのそれぞれの検出値に基づき、カルマンフィルタを用いて将来の前記移動体の状態量を予測させる際に、前記カルマンフィルタのパラメータに基づいて、前記移動体の状態量の誤差の程度を示す指標値を導出させ、前記指標値が要素として含まれる評価関数が最適となる前記移動体の状態量を、最適化問題を解くことで導出させるためのものである。【選択図】図２
2020041858.tif 000002
【特許請求の範囲】
【請求項１】
複数のセンサを備える移動体に搭載されるコンピュータに、
前記複数のセンサに含まれる第１センサと第２センサとのそれぞれの検出値に基づき、カルマンフィルタを用いて将来の前記移動体の状態量を予測させる際に、前記カルマンフィルタのパラメータに基づいて、前記移動体の状態量の誤差の程度を示す指標値を導出させ、
前記指標値が要素として含まれる評価関数が最適となる前記移動体の状態量を、最適化問題を解くことで導出させる、
誘導制御プログラム。
【請求項２】
前記コンピュータに、
前記最適化問題を解くことで導出させた前記移動体の状態量と、予め設定された参照状態量との差分のノルムが閾値を超える場合、前記移動体の状態量を基に前記パラメータを変更させ、
前記変更させたパラメータを含む前記カルマンフィルタに基づいて、前記指標値を導出させる、
請求項１に記載の誘導制御プログラム。
【請求項３】
前記コンピュータに、
前記ノルムが閾値以下となるまで、前記移動体の状態量を基に前記パラメータを変更させ、前記変更させたパラメータを含む前記カルマンフィルタに基づいて、前記指標値を導出させることを繰り返させる、
請求項２に記載の誘導制御プログラム。
【請求項４】
前記コンピュータに、
前記導出させた前記移動体の状態量を基に前記パラメータを決定させ、前記決定させたパラメータを含む前記カルマンフィルタに基づいて、第１共分散行列を前記指標値として導出させ、
予め設定された参照状態量を基に前記カルマンフィルタのパラメータを決定させ、前記決定させたパラメータを含む前記カルマンフィルタに基づいて、第２共分散行列を前記指標値として導出させ、
前記第２共分散行列のトレース値に対する前記第１共分散行列のトレース値の除算値を前記第２共分散行列に乗算した値に、前記第１共分散行列が近似すると仮定して、前記最適化問題を解かせる、
請求項１から３のうちいずれか一項に記載の誘導制御プログラム。
【請求項５】
前記第１センサは、地形を検出する地形検出センサであり、前記第２センサは、慣性計測装置であり、
前記コンピュータに、
前記地形検出センサによる検出結果に基づいて、前記移動体の状態量を導出させ、
前記慣性計測装置により計測された前記移動体の状態量と、前記画像に基づき導出させた前記移動体の状態量との差分を前記カルマンフィルタの観測値とすることで、前記移動体の将来の状態量を予測させる、
請求項１から４のうちいずれか一項に記載の誘導制御プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、誘導制御プログラムに関する。
【背景技術】
【０００２】
従来、月や惑星などの重力天体に対して、画像航法により自律精密着陸（Autonomous Precision Landing：ＡＰＬ）を行う宇宙機が知られている。重力天体に宇宙機を着陸させる際の動力降下着陸フェーズ（Powerd Descent Landing：ＰＤＬ）では、重力天体の地表面を撮像した画像と、事前に取得された重力天体の地形データとの照合によって画像航法を行うとともに、現在地に基づいて誘導軌道をリアルタイムに生成することが要求される。一方で、慣性センサと測位衛星による測位センサとを搭載する飛行体が、各センサの検出値をカルマンフィルタの観測値として利用することで、各センサの検出結果を複合的に利用した複合航法を行う技術が知られている（特許文献１参照）。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１８−１０９５３０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
しかしながら、従来の技術では、航法誤差が小さくなるように誘導軌道を十分に最適化することができていなかった。
【０００５】
本発明は、このような事情を考慮してなされたものであり、航法誤差が小さくなるように誘導軌道を最適化することができる誘導制御プログラムを提供することを目的の一つとする。
【課題を解決するための手段】
【０００６】
本発明の一態様は、複数のセンサを備える移動体に搭載されるコンピュータに、前記複数のセンサに含まれる第１センサと第２センサとのそれぞれの検出値に基づき、カルマンフィルタを用いて将来の前記移動体の状態量を予測させる際に、前記カルマンフィルタのパラメータに基づいて、前記移動体の状態量の誤差の程度を示す指標値を導出させ、前記指標値が要素として含まれる評価関数が最適となる前記移動体の状態量を、最適化問題を解くことで導出させるための誘導制御プログラムである。
【発明の効果】
【０００７】
本発明の一態様によれば、航法誤差が小さくなるように誘導軌道を最適化することができる。
【図面の簡単な説明】
【０００８】
【図１】実施形態の移動体Ｍの一例を示す図である。
【図２】実施形態の航法装置１００の構成の一例を示す図である。
【図３】実施形態の制御部１１０による一連の処理の一例を示すフローチャートである。
【図４】共分散行列Ｐ（→）ｋと処理周期ｋとの関係の一例を示す図である。
【図５】評価関数Ｊの勾配情報を用いた凸最適化問題を解く処理を模式的に示す概念図である。
【発明を実施するための形態】
【０００９】
以下、図面を参照し、本発明の誘導制御プログラムの実施形態について説明する。本実施形態における誘導制御プログラムは、例えば、移動体に搭載されたコンピュータによって実行される。誘導制御プログラムを実行するコンピュータは、種々の処理を行う。
【００１０】
図１は、実施形態の移動体Ｍの一例を示す図である。移動体Ｍは、例えば、月や惑星などの重力天体（以下、単に天体ＰＬと称する）に着陸し、その天体ＰＬの探査を行うような宇宙機（宇宙探査機）であってよい。宇宙機である移動体Ｍには、誘導制御プログラムを実行する航法装置（コンピュータ）１００が搭載される。また、宇宙機である移動体Ｍには、航法装置１００に加えて、カメラ１０と、慣性計測装置（Inertial Measurement Unit；ＩＭＵ）２０と、推進力出力装置３０とが搭載される。
【００１１】
カメラ１０は、例えば、移動体Ｍの下側に設けられ、天体ＰＬに移動体Ｍが着陸する際に、天体ＰＬの地表を撮像する。「下側」とは、例えば、天体ＰＬの地表と接する脚が設けられた筐体側である。言い換えれば、カメラ１０は、天体ＰＬの重力が移動体Ｍに作用する方向（鉛直方向下向き）側に設けられる。カメラ１０は、撮像した画像を航法装置１００に出力する。カメラ１０は、「第１センサ」の一例である。また、カメラ１０は、「地形検出センサ」の一例である。
【００１２】
慣性計測装置２０は、例えば、ＭＥＭＳ（Micro Electro Mechanical Systems）や光ファイバによって構成される三軸式加速度センサと、三軸式ジャイロセンサとを含む。慣性計測装置２０は、これらのセンサによって検出された検出値を航法装置１００に出力する。慣性計測装置２０による検出値には、例えば、水平方向、垂直方向、奥行き方向の各加速度及び／又は角速度や、ピッチ、ロール、ヨーの各軸の速度（レート）などが含まれる。慣性計測装置２０は、「第２センサ」の一例である。
【００１３】
推進力出力装置３０は、例えば、ジンバルアクチュエータやイオンエンジンなどを含む。例えば、推進力出力装置３０は、ジンバルアクチュエータを駆動させることによって、イオンエンジンが発生させた推進力の出力方向を、任意の方向に変更する。
【００１４】
航法装置１００は、誘導制御プログラムを実行することで、例えば、宇宙機である移動体Ｍを、カメラ１０によって撮像された画像と、慣性計測装置２０の検出結果とを利用して航行させる。以下、カメラ１０によって撮像された画像を利用した航法を「画像航法」と称し、慣性計測装置２０の検出結果を利用した航法を「慣性航法」と称して説明する。なお、移動体Ｍは、宇宙機に限られず、ＧＮＳＳ（Global Navigation Satellite System）を利用できない環境下において飛行するＵＡＶ（Unmanned Aerial Vehicle）や、海底の資源探査などを行うために水中を推進するＡＵＶ（Autonomous Underwater Vehicle）といった他の移動体であってもよい。以下、一例として、移動体Ｍが宇宙機であるものとして説明する。
【００１５】
図２は、実施形態の航法装置１００の構成の一例を示す図である。航法装置１００は、例えば、通信部１０２と、制御部１１０と、記憶部１３０とを備える。
【００１６】
通信部１０２は、例えば、テレメータ回線などで利用されるような周波数帯の電波を用いて地球上の監視装置と無線通信する。地球上の監視装置は、例えば、通信部１０２を介して航法装置１００を遠隔制御（遠隔誘導）することで、着陸すべき天体ＰＬと地球とを結ぶ直線（電波の直進方向）に対して直行する方向の移動体Ｍの位置を制御する。すなわち、地球上の監視装置は、天体ＰＬの重力を基準とした鉛直方向に対して直行する水平方向に関する移動体Ｍの位置を制御する。
【００１７】
制御部１１０は、例えば、取得部１１２と、画像航法演算処理部１１４と、慣性航法演算処理部１１６と、パラメータ決定部１１８、予測部１２０と、機体制御部１２２とを備える。
【００１８】
制御部１１０の構成要素は、例えば、ＣＰＵ（Central Processing Unit）やＧＰＵ（Graphics Processing Unit）などのプロセッサが記憶部１３０に格納された誘導制御プログラムを実行することにより実現される。また、制御部１１０の構成要素の一部または全部は、ＬＳＩ（Large Scale Integration）、ＡＳＩＣ（Application Specific Integrated Circuit）、またはＦＰＧＡ（Field-Programmable Gate Array）などのハードウェアにより実現されてもよいし、ソフトウェアとハードウェアの協働によって実現されてもよい。また、プロセッサにより参照される誘導制御プログラムは、予め記憶部１３０に格納されていてもよいし、ＤＶＤやＣＤ−ＲＯＭなどの着脱可能な記憶媒体に格納されており、その記憶媒体が航法装置１００のドライブ装置に装着されることで記憶媒体から記憶部１３０にインストールされてもよい。
【００１９】
記憶部１３０は、例えば、ＨＤＤ（Hard Disc Drive）、フラッシュメモリ、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）などの記憶装置により実現される。記憶部１３０には、ファームウェアやアプリケーションプログラム（誘導制御プログラムを含む）などの各種プログラムのほかに、地形データ１３２や参照軌道情報１３４、フィルタパラメータ情報１３６などが格納される。
【００２０】
地形データ１３２は、例えば、移動体Ｍに着陸させる天体ＰＬの地表面の三次元形状がモデル化されたデータである。天体ＰＬの地表面の三次元形状を示すモデルには、天体ＰＬの地表面に凹凸を形成するクレータや岩などの視覚的に目立つ特徴（特徴点）が含まれる。
【００２１】
参照軌道情報１３４は、参照軌道（ノミナル軌道）を定義した情報である。参照軌道とは、地上の監視装置から移動体Ｍを天体ＰＬに遠隔誘導する際に、移動体Ｍがとるべき位置や速度、加速度、姿勢などの状態量を、ある時間間隔ごと、または距離間隔ごとに指定した情報である。参照軌道情報１３４が示す状態量は、「参照状態量」の一例である。
【００２２】
フィルタパラメータ情報１３６は、後述するカルマンフィルタの方程式の各パラメータが定義された情報である。
【００２３】
取得部１１２は、カメラ１０から画像を取得するとともに、慣性計測装置２０から加速度や速度といった検出値を取得する。また、取得部１１２は、通信部１０２を介して、地上の監視装置から参照軌道情報１３４を定期的に取得し、これを記憶部１３０に記憶させることで、記憶部１３０に格納された参照軌道情報１３４を更新する。
【００２４】
画像航法演算処理部１１４は、取得部１１２によってカメラ１０から取得された画像と、記憶部１３０に格納された地形データ１３２とを照合（比較）して、移動体Ｍの状態量を導出する。例えば、画像航法演算処理部１１４は、カメラ１０の画像に対して画像処理を行って、岩やクレータなどの特徴点を抽出し、抽出した特徴点と、地形データ１３２が示す三次元モデルに含まれる特徴点とのパターンマッチングによって、天体ＰＬの重力を基準とした鉛直方向に対して直行する水平方向に関する移動体Ｍの位置を、状態量として導出する。
【００２５】
慣性航法演算処理部１１６は、例えば、天体ＰＬの地表面を基準とした（例えば地表面の高度をゼロとした）移動体Ｍの高度が所定高度以下となった場合、慣性計測装置２０の検出結果を利用した慣性航法によって、移動体Ｍの状態を定量的に示す状態量を導出する。「状態量」には、例えば、位置、速度、機体座標系から慣性座標系へのクォータニオンなどが含まれる。機体座標系は、移動体Ｍの重心を座標の原点とし、移動体Ｍの機軸方向をある一軸とした座標系である。慣性座標系は、天体ＰＬの重力を基準とした鉛直方向をある一軸とした座標系である。また、「状態量」には、慣性計測装置２０によって検出された加速度や角速度に対するセンサバイアス値が含まれていてもよい。
【００２６】
一般的に、地球上の監視装置との無線通信によって遠隔制御する場合、地球と天体ＰＬとの距離のオーダーが億キロメートルである場合があり、電波の往復伝搬時間に換算して数十分以上掛かる場合がある。そのため、天体ＰＬの地表面に対する移動体Ｍの高度を低下させ、移動体Ｍを天体ＰＬに着陸させていく際、地上からの指令に基づく遠隔制御では対応しきれないことがある。従って、慣性航法演算処理部１１６は、自律航法を行うために、慣性計測装置２０の検出値を利用した慣性航法によって移動体Ｍの状態量を導出する。
【００２７】
パラメータ決定部１１８は、参照軌道情報１３４を参照し、カルマンフィルタの各種方程式に含まれるパラメータの値を決定する。パラメータには、例えば、後述するカルマンフィルタの状態遷移行列や、カルマンフィルタのプロセスノイズの共分散行列、カルマンフィルタの観測ノイズの共分散行列などが含まれる。パラメータ決定部１１８は、パラメータの値を決定すると、そのパラメータ値をフィルタパラメータ情報１３６として記憶部１３０に記憶させる。
【００２８】
予測部１２０は、慣性航法演算処理部１１６によって移動体Ｍの状態量が導出されると、フィルタパラメータ情報１３６によってパラメータが定義されたカルマンフィルタを利用した第１処理（予測処理）において、移動体Ｍの状態量の誤差（航法誤差）の変動を予測し、カルマンフィルタを利用した第２処理（更新処理）において、移動体Ｍの状態量の誤差（航法誤差）を低減させる。
【００２９】
例えば、カルマンフィルタにおける今回の処理周期をｋとした場合、第１処理には、現周期ｋにおいて、次回の処理周期ｋ＋１の移動体Ｍの状態量を予測する処理と、現周期ｋにおいて、次回の処理周期ｋ＋１の移動体Ｍの状態量の誤差を予測する処理とが含まれる。より具体的には、第１処理には、現周期ｋにおいて得られると推定された移動体Ｍの状態量（後述する状態量ベクトル）と、現周期ｋにおける制御入力（後述する制御ベクトル）とに基づいて、次の周期ｋ＋１において得られることが推定される移動体Ｍの状態量（推定値）を予測する処理と、現周期ｋの推定値に含まれる誤差の程度、あるいは推定値の精度を示す共分散（事前推定共分散）と、現周期ｋにおけるカルマンフィルタのプロセスノイズの程度を示す共分散とに基づいて、現周期ｋにおいて導出される次の周期ｋ＋１の推定値に含まれる誤差の程度、あるいは推定値の精度を示す共分散（事後推定共分散）を導出する処理と、が含まれる。
【００３０】
第２処理は、第１処理において事後推定共分散として推定された移動体Ｍの状態量の共分散に基づいて、カルマンゲインを導出し、導出したカルマンゲインと、ある観測値とに基づいて、第１処理において推定された移動体Ｍの状態量を更新する処理と、導出したカルマンゲインと、第１処理において事後推定共分散として推定された移動体Ｍの状態量の共分散とに基づいて、移動体Ｍの状態量の共分散を更新する処理と、が含まれる。観測値は、例えば、慣性航法演算処理部１１６によって導出された移動体Ｍの状態量と、画像航法演算処理部１１４によって導出された移動体Ｍの状態量との差分であってよい。すなわち、観測値は、慣性航法によって求めた移動体Ｍの位置と、画像航法によって求めた移動体Ｍの位置との差分であってよい。第２処理では、位置の差分を観測値として利用することで、位置に加えて、位置の微分値である速度や加速度なども併せて状態量として推定される。
【００３１】
ここで、予測部１２０は、カルマンフィルタを用いて第１処理および第２処理を行う際に、数式（１）から（６）として例示する最適化問題（最適制御問題）を解くことで、将来の移動体Ｍの状態量の最適解を導出する。以下、アルファベットに（→）を付けたものは、ベクトルあるいは行列を表すものとする。
【００３２】
【数１】
2020041858.tif 000003
【００３３】
【数２】
2020041858.tif 000004
【００３４】
【数３】
2020041858.tif 000005
【００３５】
【数４】
2020041858.tif 000006
【００３６】
【数５】
2020041858.tif 000007
【００３７】
【数６】
2020041858.tif 000008
【００３８】
ｘ（→）は、移動体Ｍの状態量を示す状態量ベクトルを表し、ｕ（→）は、移動体Ｍの制御ベクトルを表し、ｆは、非線形ダイナミクス関数を表し、ｇは、状態制約式を表し、ｈは、制御制約式を表し、Ｆ（→）は、カルマンフィルタの状態遷移行列を表し、Ｈ（→）は、カルマンフィルタの観測行列を表し、Ｋ（→）は、上述した第２処理（更新処理）を行う処理周期（ステップ）の数を表し、Ｐ（→）は、カルマンフィルタの共分散行列を表し、Ｑ（→）は、カルマンフィルタのプロセスノイズの共分散行列を表し、Ｒ（→）は、カルマンフィルタの観測ノイズの共分散行列を表し、ｋは、カルマンフィルタの各処理周期（ステップ）を表している。
【００３９】
数式（１）は、ある評価関数Ｊの最小化問題を定式化した数式を表し、数式（２）から（６）は、数式（１）を解く上で満たすべき数式を表している。数式（１）に示す評価関数Ｊは、ある処理周期ｋｆの観測行列Ｇ（→）と、処理周期ｋｆの共分散行列Ｐ（→）ｋｆとの合成写像のトレース値を表している。処理周期ｋｆとは、地上の監視装置から指示された誘導軌道に基づいて計算されたカルマンフィルタの最終処理時刻（最終処理周期）である。言い換えれば、処理周期ｋｆは、移動体Ｍが天体ＰＬに着陸するときの最終処理周期である。観測行列Ｇ（→）は、非対角要素がゼロである行列であり、最終処理時刻ｋｆでの共分散行列Ｐ（→）ｋｆの特定の対角成分を抜き出すための専用の行列を表している。そのため、評価関数Ｊは、共分散行列Ｐ（→）ｋｆの特定の対角要素のみのトレース値となる。
【００４０】
数式（２）は、状態量ベクトルｘ（→）と制御ベクトルｕ（→）とを変数とする非線形ダイナミクス関数によって表される状態方程式であり、数式（３）は、等式の制約条件式を表し、数式（４）は、不等式の制約条件式を表している。
【００４１】
数式（５）は、上述した第１処理に含まれる２つの処理のうち、事後推定共分散を導出する処理を表している。すなわち、数式（５）は、前周期ｋ−１の推定値に含まれる誤差の共分散行列（事前推定共分散）Ｐ（→）ｋ−１｜ｋ−１と、現周期ｋにおけるカルマンフィルタのプロセスノイズの共分散行列Ｑ（→）ｋとに基づいて、現周期ｋの推定値に含まれる誤差の共分散行列（事後推定共分散）Ｐ（→）ｋ｜ｋ−１を導出することを表している。ｋ｜ｋ−１は、一つ前の周期ｋ−１において、一つ先の将来にあたる現周期ｋの共分散行列を推定することを意味する。
【００４２】
また、数式（６）は、上述した第２処理に含まれる２つの処理のうち、移動体Ｍの状態量の共分散を更新する処理を表している。すなわち、数式（６）は、数式（５）に示すように、前の周期ｋ−１で推定した現周期ｋの共分散行列Ｐ（→）ｋ｜ｋ−１に基づいて、現周期ｋの共分散行列Ｐ（→）ｋ｜ｋを再度導出することで、前の周期ｋ−１で推定した共分散行列Ｐ（→）ｋ｜ｋ−１を、より確からしい共分散行列Ｐ（→）ｋ｜ｋに更新することを表している。上述したカルマンゲインは、数式（６）の右辺項Ｐ（→）ｋ｜ｋ−１Ｈ（→）Ｔ（Ｈ（→）Ｐ（→）ｋ｜ｋ−１Ｈ（→）Ｔ＋Ｒ（→）ｋ）−１に相当する。共分散を更新する処理は、予め決められた処理周期Ｋ（→）で行われる。また、ｋ＞Ｋｆである。
【００４３】
予測部１２０は、数式（１）に示す評価関数Ｊが最小となる制御ベクトルｕ（→）の時間履歴を求めることで、最適化問題を解く。言い換えれば、予測部１２０は、カルマンフィルタの各処理をｋ１からｋｆまで繰り返す中で得られたｋｆ個の制御ベクトルｕ（→）の集合の中から、評価関数Ｊが最小となる制御ベクトルｕ（→）を探索することで、最適化問題を解く。
【００４４】
最適化問題を解くためには評価関数Ｊを、ある条件下で、制御ベクトルｕ（→）に対して離散線形化する計算手法が必要である。従って、本実施形態では、予測部１２０は、数式（７）に示す近似式を第１処理および第２処理に適用し、評価関数Ｊの一要素である共分散行列Ｐ（→）ｋの離散線形化を行う。数式（７）中のＰ（−）は、参照軌道として決定された移動体Ｍがとるべき移動量を基にしてカルマンフィルタによって導出した共分散行列を表している。移動体Ｍが参照軌道を移動した際の航法精度を表す分散行列Ｐ（−）は、実際に移動体Ｍが参照軌道を移動するときに利用されるカルマンフィルタを仮想的に動作させることで導出される。「仮想的に動作させる」とは、カルマンフィルタの計算結果を移動体Ｍの機体制御に利用しないことである。（−）は、オーバーライン（バー）を表している。
【００４５】
【数７】
2020041858.tif 000009
【００４６】
【数８】
2020041858.tif 000010
【００４７】
数式（７）に示す近似式は、参照軌道（ｘ（→），ｕ（→））の近傍で、各処理周期の共分散行列Ｐ（→）ｋが、Ｐ（→）ｋをｔｒ（Ｐ（→）ｋ）で微分した結果と一致するか、または極めて近くなるという仮定を表しており、数式（８）に示す条件式を満たす場合のみ成立する。言い換えれば、数式（７）に示す近似式は、共分散行列Ｐ（→）ｋ（−）のトレース値（ｔｒ（Ｐ（→）ｋ（−）））に対する共分散行列Ｐ（→）ｋのトレース値（ｔｒ（Ｐ（→）ｋ））の除算値を、共分散行列Ｐ（→）ｋ（−）に乗算した値に、共分散行列Ｐ（→）ｋが近似することを仮定している。数式（７）に示す共分散行列Ｐ（→）ｋは、「第１共分散行列」の一例であり、数式（７）に示す共分散行列Ｐ（→）ｋ（−）は、「第２共分散行列」の一例である。
【００４８】
数式（９）は、共分散行列Ｐ（→）ｋの線形式を表す。予測部１２０は、数式（９）と離散線形化された数式（２）から（４）とに従って、評価関数Ｊを最小にする制御ベクトルｕ（→）の時間履歴を導出する。数式（９）の［Ｋ−，Ｋ＋］は、式（６）の処理周期、すなわち第２処理（共分散行列の更新処理）の処理周期の間であることを表しており、各ｔｒ（Ｐ（→））は、線形化の際に基準とした参照軌道（ｘ（→），ｕ（→））の近傍において成り立つ線形式で表されている。
【００４９】
【数９】
2020041858.tif 000011
【００５０】
具体的には、予測部１２０は、数式（９）に従って評価関数Ｊを最小にする制御ベクトルｕ（→）を導出すると、その導出した制御ベクトルｕ（→）を数式（２）に代入することで状態量ベクトルｘ（→）を導出し、導出した状態量ベクトルｘ（→）および制御ベクトルｕ（→）を基に、再び数式（７）を用いた離散化および線形化を行うことを繰り返し行う。
【００５１】
このように、評価関数Ｊが最小となる制御ベクトルｕ（→）の時間履歴を探索するという最適化問題を、参照軌道（ｘ（→），ｕ（→））周りで離散化および線形化することで、線形化した最適化問題を、凸最適化問題として解くことができる。凸最適化問題は、凸集合上の凸関数の最小化問題であり、一般的な最適化問題よりも簡単に最適化が可能であり、局所的な最小値が大域的な最小値と一致する性質をもつ。このように最適化問題を線形化して凸最適化問題として扱うことで、共分散行列のトレース値（スカラー値）を基にした比較的簡易な計算を行った場合であっても、あるいは物理的に成り立たない初期解が与えられた場合であっても、その計算の解を収束させ、最適解（ｘ（→），ｕ（→））を求めることができる。
【００５２】
機体制御部１２２は、予測部１２０がカルマンフィルタを用いて導出した最適解（ｘ（→），ｕ（→））に基づいて推進力出力装置３０を制御することで、移動体Ｍの位置、速度、加速度、姿勢などの状態が、最適解（ｘ（→），ｕ（→））が示す所望の状態となるように制御する。
【００５３】
図３は、実施形態の制御部１１０による一連の処理の一例を示すフローチャートである。本フローチャートの処理は、例えば、天体ＰＬの地表面と移動体Ｍとの距離（高度）が所定距離（例えば４０［ｍ］程度）以下となる降下フェーズにおいて行われる。
【００５４】
まず、パラメータ決定部１１８は、参照軌道情報１３４を参照し、初期値となる参照軌道の（ｘ（→），ｕ（→））を取得する（ステップＳ１００）。次に、パラメータ決定部１１８は、初期値とする参照軌道の（ｘ（→），ｕ（→））を基に、カルマンフィルタのパラメータ（Ｆ（→）、Ｑ（→）、Ｒ（→））の値を決定する（ステップＳ１０２）。
【００５５】
次に、予測部１２０は、カルマンフィルタの処理周期ｋが、予め決められた共分散行列の更新処理を開始する処理周期Ｋ（→）となるまで、パラメータ決定部１１８によって決定されたパラメータを含むカルマンフィルタを用いて、共分散行列Ｐ（→）ｋを導出することを繰り返す（ステップＳ１０４）。
【００５６】
図４は、共分散行列Ｐ（→）ｋと処理周期ｋとの関係の一例を示す図である。図示のように、共分散行列Ｐ（→）ｋは、初期値となるＰ（→）０が与えられると、処理を繰り返すほど一定の値に収束していく。このとき、収束値となる共分散行列Ｐ（→）ｋは、カルマンフィルタのパラメータによって一意に決定される。
【００５７】
図３の説明に戻り、次に、予測部１２０は、参照軌道として与えられた参照軌道（ｘ（→），ｕ（→））周りで、勾配情報を取得（導出）する（ステップＳ１０６）。勾配情報とは、評価関数Ｊの勾配に関する情報である。評価関数Ｊの勾配は、例えば、数式（７）の左辺式によって表される、評価関数Ｊの一要素である共分散行列Ｐ（→）ｋのトレース値のｘ（→）やｕ（→）での偏微分量である。例えば、予測部１２０は、勾配情報を得るために、共分散行列Ｐ（→）ｋをｘ（→）またはｕ（→）で偏微分する。予測部１２０は、この偏微分結果を用いて、共分散行列Ｐ（→）ｋのトレース値（ｔｒ（Ｐ（→）ｋ））を離散化および線形化（すなわち凸化）させることができる。離散化および線形化とは、評価関数Ｊを近似的に離散化された各時刻ｋのｘ（→）やｕ（→）の線形式で書き表す際に、ｘ（→）やｕ（→）にかかる係数としてこの偏微分値を利用することである。
【００５８】
次に、予測部１２０は、最適化問題の一つである凸最適化問題を解くことで、トレース値（ｔｒ（Ｐ（→）ｋ））が最小値をとる最適解（ｘ（→），ｕ（→））を導出する（ステップＳ１０８）。
【００５９】
図５は、評価関数Ｊの勾配情報を用いた凸最適化問題を解く処理を模式的に示すの概念図である。図示の例は、最適化問題を、評価関数Ｊの勾配情報を用いた凸最適化問題に変形することを表している。図示の例のように、最適化問題の一つである凸最適化問題を、評価関数Ｊの勾配情報を利用して解く場合、予測部１２０は、共分散行列Ｐ（→）ｋのトレース値（ｔｒ（Ｐ（→）ｋ））を凸化させる。これによって、局所的に最適解を探索した場合であっても、トレース値（ｔｒ（Ｐ（→）ｋ））が最小値をとる最適解が局所解として導出されず、凸最適化によって、元の問題の局所解の一つが高速に求解出来る。
【００６０】
次に、予測部１２０は、参照軌道として与えられた（ｘ（→），ｕ（→））（図中ｘ（−））と、最適解として導出した（ｘ（→），ｕ（→））（図中ｘ）との差分のノルム（ユークリッドノルム）が、閾値Δ以下であるか否かを判定する（ステップＳ１１０）。
【００６１】
予測部１２０によってノルムが閾値Δ以下であると判定された場合、機体制御部１２２は、予測部１２０がカルマンフィルタを用いて導出した最適解（ｘ（→），ｕ（→））に基づいて推進力出力装置３０を制御する（ステップＳ１１２）。これによって本フローチャートの処理が終了する。
【００６２】
予測部１２０は、ノルムが閾値Δを超えると判定した場合、初期値として与えられた参照軌道（ｘ（→），ｕ（→））を、Ｓ１０８の処理で最適解として導出した軌道（ｘ（→），ｕ（→））に更新し（ステップＳ１１４）、Ｓ１０２の処理に戻る。なお、予測部１２０は、Ｓ１１４の処理において、最適解として得られる状態量ｘ（→）が、参照軌道が示す状態量ｘ（→）（−）から大きく乖離しないように、最適解として取り得る状態量ｘ（→）の上下限を変更してもよい。
【００６３】
これを受けて、パラメータ決定部１１８は、更新された軌道（ｘ（→），ｕ（→））を基に、カルマンフィルタのパラメータ（Ｆ（→）、Ｑ（→）、Ｒ（→））の値を再度決定する。このように、参照軌道、あるいは前回の処理で最適解として求めた軌道との乖離が閾値Δ以下となるまで凸最適化問題を解くこと繰り返すことで、移動体Ｍを天体ＰＬに着陸させる際に行わせる自律航法の航法誤差が小さくなる移動体Ｍの将来の状態量を逐次的に導出することができる。この結果、時系列に連続する移動体Ｍの将来の状態量によって表される誘導軌道を航法誤差が小さくなるように最適化することができる。
【００６４】
以上説明した実施形態によれば、カメラ１０や慣性計測装置２０といった複数のセンサを備える移動体Ｍに搭載されたコンピュータである航法装置１００に、カメラ１０と慣性計測装置２０とのそれぞれの検出値に基づき、カルマンフィルタを用いて移動体Ｍの将来の状態量を予測させる際に、カルマンフィルタのパラメータ（Ｆ（→）、Ｑ（→）、Ｒ（→））に基づいて、移動体Ｍの将来の状態量の誤差の程度を示す指標値である共分散行列Ｐ（→）ｋを導出させ、共分散行列Ｐ（→）ｋのトレース値（ｔｒ（Ｐ（→）ｋ））が要素として含まれる評価関数Ｊが最適となる移動体の状態量ｘ（→）を、最適化問題（例えば凸最適化問題）を解くことで導出させる。これによって、航法誤差が小さくなるように、天体ＰＬに移動体Ｍを着陸させる際の誘導軌道を最適化することができる。
【００６５】
［変形例］
以下、上述した実施形態の変形例について説明する。上述した実施形態では、移動体Ｍが宇宙機であり、その移動体Ｍに搭載されるセンサが、カメラ１０および慣性計測装置２０であるものとして説明したがこれに限られない。例えば、移動体Ｍが上述したＵＡＶである場合、その移動体Ｍに搭載されるセンサには、カメラ１０に代えて、あるいは加えて、レーダやＬＩＤＡＲ（Laser(Light) Imaging Detection and Ranging）が含まれてよい。また、移動体Ｍが上述したＡＵＶである場合、その移動体Ｍに搭載されるセンサには、カメラ１０に代えて、あるいは加えて、ソナーが含まれてよい。レーダ、ＬＩＤＡＲ、およびソナーは、「地形検出センサ」の他の例である。
【００６６】
また、上述した実施形態では、航法装置１００が、最適化問題の一つである凸最適化問題を解くことで、評価関数Ｊが最適となる移動体の状態量ｘ（→）を導出するものとして説明したがこれに限られない。例えば、航法装置１００の予測部１２０は、評価関数Ｊの勾配情報である共分散行列Ｐ（→）ｋのトレース値のｘ（→）やｕ（→）による偏微分量に基づいて、ニュートン法や最急降下法といった他の最適化問題を解くことで、評価関数Ｊが最適となる移動体の状態量ｘ（→）を導出してもよい。言い換えれば、予測部１２０は、共分散行列Ｐ（→）ｋ（−）のトレース値（ｔｒ（Ｐ（→）ｋ（−）））に対する共分散行列Ｐ（→）ｋのトレース値（ｔｒ（Ｐ（→）ｋ））の除算値を、共分散行列Ｐ（→）ｋ（−）に乗算した値に、共分散行列Ｐ（→）ｋが近似するという仮定の下で、ニュートン法や最急降下法といった他の最適化問題を解くことで、評価関数Ｊが最適となる移動体の状態量ｘ（→）を導出してよい。
【００６７】
以上、本発明を実施するための形態について実施形態を用いて説明したが、本発明はこうした実施形態に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。
【符号の説明】
【００６８】
１０…カメラ、２０…慣性計測装置、１００…航法装置、１０２…通信部、１１０…制御部、１１２…取得部、１１４…画像航法演算処理部、１１６…慣性航法演算処理部、１１８…パラメータ決定部、１２０…予測部、１２２…機体制御部、１３０…記憶部、Ｍ…移動体
【図１】
2020041858.tif 000012
【図２】
2020041858.tif 000013
【図３】
2020041858.tif 000014
【図４】
2020041858.tif 000015
【図５】
2020041858.tif 000016