(57)【要約】【課題】異種複数センサの測位精度が算出でき、かつ、センサ位置と目標位置を変更する場合やセンサ数増加に伴う測位精度の計算量を削減することのできる測位精度評価装置を得る。【解決手段】観測効果行列算出部２は観測効果行列を算出して、観測効果行列和算出部３で観測効果行列和を算出する。測位誤差指標算出部４は測位誤差指標を算出すると共に、総合測位誤差指標算出部５で、測位誤差指標から算出した総合測位誤差指標を算出する。総合測位誤差指標評価部６は、総合測位誤差指標算出部５で算出された総合測位誤差指標が総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する。【選択図】図１
2016217974.tif 000002
【特許請求の範囲】
【請求項１】
複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定するセンサ観測誤差分散設定部と、
センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、
前記センサ観測誤差分散と前記観測効果行列変換行列を用いて、前記複数のセンサ毎の観測効果を表す観測効果行列を算出する観測効果行列算出部と、
前記複数のセンサの観測効果行列の和を算出する観測効果行列和算出部と、
測位精度の初期値である測位誤差初期値の逆行列と前記観測効果行列の和とを加算した値の逆行列から測位誤差指標を算出する測位誤差指標算出部と、
複数地点の前記測位誤差指標の演算結果を示す総合測位誤差指標を算出する総合測位誤差指標算出部と、
前記総合測位誤差指標が、設定された総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合測位誤差指標評価部とを備えたことを特徴とする測位精度評価装置。
【請求項２】
複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定するセンサ観測誤差分散設定部と、
センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、
前記センサ観測誤差分散から観測効果行列算出前行列係数を算出すると共に、前記観測効果行列変換行列を用いて、前記複数のセンサ毎の観測効果行列算出前行列を算出する観測効果行列算出前行列算出部と、
前記観測効果行列算出前行列と、センサの種類によって決定される観測効果行列算出前行列係数とを用いて観測効果行列を求め、さらに複数のセンサの観測効果行列和を算出する第２の観測効果行列和算出部と、
前記センサの種類を変更する場合、当該センサの変更を示す信号を前記第２の観測効果行列和算出部に送出するセンサ変更設定部と、
測位精度の初期値である測位誤差初期値の逆行列と前記観測効果行列の和とを加算した値の逆行列から測位誤差指標を算出する測位誤差指標算出部と、
複数地点の前記測位誤差指標の演算結果を示す総合測位誤差指標を算出する総合測位誤差指標算出部と、
前記総合測位誤差指標が、設定された総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合測位誤差指標評価部とを備え、
前記第２の観測効果行列和算出部は、前記センサ変更設定部より前記センサの変更を示す信号を受けた場合、当該変更されるセンサの観測効果行列算出前行列係数を用いて観測効果行列を求めることを特徴とする測位精度評価装置。
【請求項３】
複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定すると共に、複数の観測成分を観測するセンサを仮想的に同一のセンサ位置で複数のセンサ観測誤差分散を有するセンサとして設定する第２のセンサ観測誤差分散設定部と、
センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、
前記センサ観測誤差分散と前記観測効果行列変換行列を用いて、前記複数のセンサ毎の観測効果を表す観測効果行列を算出する観測効果行列算出部と、
前記複数のセンサの観測効果行列の和を算出する観測効果行列和算出部と、
測位精度の初期値である測位誤差初期値の逆行列と前記観測効果行列の和とを加算した値の逆行列から測位誤差指標を算出する測位誤差指標算出部と、
複数地点の前記測位誤差指標の演算結果を示す総合測位誤差指標を算出する総合測位誤差指標算出部と、
前記総合測位誤差指標が、設定された総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合測位誤差指標評価部とを備えたことを特徴とする測位精度評価装置。
【請求項４】
複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定すると共に、複数の観測成分を観測するセンサを仮想的に同一のセンサ位置で複数のセンサ観測誤差分散を有するセンサとして設定する第２のセンサ観測誤差分散設定部と、
センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、
前記センサ観測誤差分散から観測効果行列算出前行列係数を算出すると共に、前記観測効果行列変換行列を用いて、前記複数のセンサ毎の観測効果行列算出前行列を算出する観測効果行列算出前行列算出部と、
前記観測効果行列算出前行列と、センサの種類によって決定される観測効果行列算出前行列係数とを用いて観測効果行列を求め、さらに複数のセンサの観測効果行列和を算出する第２の観測効果行列和算出部と、
前記センサの種類を変更する場合、当該センサの変更を示す信号を前記第２の観測効果行列和算出部に送出するセンサ変更設定部と、
測位精度の初期値である測位誤差初期値の逆行列と前記観測効果行列の和とを加算した値の逆行列から測位誤差指標を算出する測位誤差指標算出部と、
複数地点の前記測位誤差指標の演算結果を示す総合測位誤差指標を算出する総合測位誤差指標算出部と、
前記総合測位誤差指標が、設定された総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合測位誤差指標評価部とを備え、
前記第２の観測効果行列和算出部は、前記センサ変更設定部より、前記センサの変更を示す信号を受けた場合、当該変更されるセンサの観測効果行列算出前行列係数を用いて観測効果行列を求めることを特徴とする測位精度評価装置。
【請求項５】
目標初期位置から目標最終位置までの目標経路を設定する目標経路設定部と、
前記目標経路に沿った複数地点の予測誤差共分散行列を設定する予測誤差共分散行列設定部と、
前記予測誤差共分散行列により、前記目標経路に沿った複数地点の測位誤差初期値を設定する測位誤差初期値設定部とを備え、
前記測位誤差指標算出部は、前記測位誤差初期値設定部から出力された前記複数地点の測位誤差初期値を用いて測位誤差指標を算出することを特徴とする請求項１から請求項４のうちのいずれか１項記載の測位精度評価装置。
【請求項６】
センサ位置と目標位置とを設定するための格子点を設定する格子点設定部と、
前記総合測位誤差指標の値により前記格子点の間隔を設定する格子点数設定部とを備えたことを特徴とする請求項１から請求項５のうちのいずれか１項記載の測位精度評価装置。
【請求項７】
複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定するセンサ観測誤差分散設定部と、
センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、
前記センサ観測誤差分散と前記観測効果行列変換行列を用いて、前記複数のセンサ毎の観測効果を表す観測効果行列を算出する観測効果行列算出部と、
前記複数のセンサの観測効果行列の和を算出する観測効果行列和算出部と、
複数地点の観測効果行列和の演算結果を示す総合観測効果行列和を算出し、設定された総合観測効果行列和しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合観測効果行列和評価部を備えたことを特徴とする測位精度評価装置。
【請求項８】
総合測位誤差指標算出部で算出される複数の総合測位誤差指標が、総合測位誤差指標しきい値より大きく、かつ、設定範囲内の同一値であった場合、事前に設定されたセンサ位置を選択し当該センサ位置を設定させる指定目標設定部を備えたことを特徴とする請求項１から請求項７のうちのいずれか１項記載の測位精度評価装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数の異種複数センサによる測位精度を算出する測位精度評価装置に関するものである。
【背景技術】
【０００２】
従来、複数のセンサを用いた目標位置の測位誤差を求める方法としては例えば非特許文献１に示すようなものがあった。この方法では、ＤＯＰ（Ｄｉｌｕｔｉｏｎ  Ｏｆ  Ｐｒｅｃｉｓｉｏｎ，誤差倍増率）により測位精度を求めている。例えば、目標である航空機に搭載されたトランスポンダが送信する応答を、複数の受信局で受信した時刻の受信局差の到達時刻差、つまり、航空機と各受信局間の距離が観測された場合の水平方向のＤＯＰを求めている。受信局アンテナが航空機を取り囲む配置となった場合にＤＯＰが良好になる。そして、ＤＯＰの大小により、目標に対する受信局の配置を決定する。
【０００３】
従来技術であるＤＯＰを説明するための図を図２１に示す。非特許文献１では、距離差の観測であったが、説明を簡略化するため、以降の説明では、距離を観測し、２次元位置を測位する場合の例で説明する。
図２１は、距離を観測するセンサ３台により目標位置を測位する図を表す。図２１において、センサ１、センサ２、センサ３の位置をｓ１、ｓ２、ｓ３、目標位置をｘとした場合の各成分は、式（１）となり、センサ１と目標間の距離をＲ１、センサ２と目標間の距離をＲ２、センサ３と目標間の距離をＲ３とすると、式（２）の関係が成り立つ。式（１）において、「′」はベクトル、行列の転置を表し、式（２）において、「＾２」は２乗、「＾（１／２）」は１／２乗を表す。なお、以降の数式においても「′」「＾２」「＾（１／２）」は同様の関係を表すとする。
ｓ１＝［ｘ１  ｙ１］′
ｓ２＝［ｘ２  ｙ２］′
ｓ３＝［ｘ３  ｙ３］′
ｘ＝［ｘ  ｙ］′                                              （１）
Ｒ１＝（（ｘ−ｘ１）＾２＋（ｙ−ｙ１）＾２）＾（１／２）
Ｒ２＝（（ｘ−ｘ２）＾２＋（ｙ−ｙ２）＾２）＾（１／２）
Ｒ３＝（（ｘ−ｘ３）＾２＋（ｙ−ｙ３）＾２）＾（１／２）      （２）
【０００４】
これにより、ＤＯＰは式（３）で計算できる。式（３）において、Ｄ（１，１）は行列Ｄの１行１列成分、Ｄ（２，２）は行列Ｄの２行２列成分である。行列Ｄは、式（４）から算出する。式（４）において、ｉｎｖ（）は、行列の逆行列を表す。式（４）における行列Ｇは式（５）から算出する。式（５）において、Ｇ（１，１）は行列Ｇの１行１列成分、Ｇ（１，２）は行列Ｇの１行２列成分であり、Ｇ（２，１）、Ｇ（２，２）、Ｇ（３，１）、Ｇ（３，２）も同様である。式（５）における∂Ｒ１／∂ｘは、式（２）におけるＲ１をｘで偏微分した値であり、∂Ｒ１／∂ｙ、∂Ｒ２／∂ｘ、∂Ｒ２／∂ｙ、∂Ｒ３／∂ｘ、∂Ｒ３／∂ｙも同様である。
ＤＯＰ＝（Ｄ（１，１）＋Ｄ（２，２））＾（１／２）      （３）
Ｄ＝ｉｎｖ（ＧＧ′）            （４）
Ｇ（１，１）＝∂Ｒ１／∂ｘ
Ｇ（１，２）＝∂Ｒ１／∂ｙ
Ｇ（２，１）＝∂Ｒ２／∂ｘ
Ｇ（２，２）＝∂Ｒ２／∂ｙ
Ｇ（３，１）＝∂Ｒ３／∂ｘ
Ｇ（３，２）＝∂Ｒ３／∂ｙ      （５）
【０００５】
式（３）〜式（５）の計算を、センサ位置と目標位置を変更して、センサ位置と目標位置の組合せからなる、非特許文献１の図６に示すＤＯＰを求めることができる。
【先行技術文献】
【非特許文献】
【０００６】
【非特許文献１】宮崎，上田，角張，二瓶，古賀，“広域マルチラテレーションの基礎実験結果，”電子航法研究所研究発表会，第１０回平成２２年６月．
【発明の概要】
【発明が解決しようとする課題】
【０００７】
上記のように、従来技術では、全センサが距離のような同じ観測値を得る同種のセンサに対するＤＯＰを求めている。ところで、センサは、距離と方位角を観測する２次元のレーダ、距離と仰角及び方位角を観測する３次元のレーダ、仰角と方位角を観測するＩＲ（Ｉｎｆｒａｒｅｄ、赤外線）センサ、非特許文献１に示すような距離差を観測するＭＬＡＴ（Ｍｕｌｔｉｌａｔｅｒａｔｉｏｎ：マルチラテレーション）やＷＡＭ（Ｗｉｄｅ  Ａｒｅａ  Ｍｕｌｔｉｌａｔｅｒａｒａｔｉｏｎ：広域マルチラテレーション）のように、観測する次元や観測成分が異なる異種センサが多数存在する。しかしながら、ＤＯＰは、異種センサを複数台組み合わせた場合、いわゆる異種複数センサの測位精度の算出には適用できないという問題があった。
また、ＤＯＰでは、センサ位置と目標位置を変更するたびに、式（３）〜式（５）の測位精度の計算を毎回行うため、それらの計算量が膨大となる問題があった。特に、センサ数が増えると測位精度の計算量の問題は深刻な問題である。
【０００８】
この発明は上記のような課題を解決するためになされたもので、異種複数センサの測位精度が算出でき、かつ、センサ位置と目標位置を変更する場合やセンサ数増加に伴う測位精度の計算量を削減することのできる測位精度評価装置を得ることを目的とする。
【課題を解決するための手段】
【０００９】
この発明に係る測位精度評価装置は、複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定するセンサ観測誤差分散設定部と、センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、センサ観測誤差分散と観測効果行列変換行列を用いて、複数のセンサ毎の観測効果を表す観測効果行列を算出する観測効果行列算出部と、複数のセンサの観測効果行列の和を算出する観測効果行列和算出部と、測位精度の初期値である測位誤差初期値の逆行列と観測効果行列の和とを加算した値の逆行列から測位誤差指標を算出する測位誤差指標算出部と、複数地点の測位誤差指標の演算結果を示す総合測位誤差指標を算出する総合測位誤差指標算出部と、総合測位誤差指標が、設定された総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合測位誤差指標評価部とを備えたものである。
【発明の効果】
【００１０】
この発明の測位精度評価装置は、
観測効果行列を算出して観測効果行列和を算出し、測位誤差指標を算出すると共に、測位誤差指標から算出した総合測位誤差指標を算出し、総合測位誤差指標を評価するようにしたので、異種複数センサの測位精度を算出することができ、かつ、センサ位置と目標位置を変更する場合やセンサ数増加に伴う測位精度の計算量を削減することができる。
【図面の簡単な説明】
【００１１】
【図１】この発明の実施の形態１による測位精度評価装置を示す構成図である。
【図２】この発明の実施の形態１による測位精度評価装置におけるある領域を格子上に分割した格子点の例を示す説明図である。
【図３】この発明の実施の形態１による測位精度評価装置における格子点毎に測位誤差指標を算出することを示す説明図である。
【図４】この発明の実施の形態１による測位精度評価装置におけるセンサが１台の場合のセンサ配置パターンを示す説明図である。
【図５】この発明の実施の形態１による測位精度評価装置におけるセンサが２台の場合のセンサ配置パターンを示す説明図である。
【図６】この発明の実施の形態１による測位精度評価装置における構造物と障害物からなるレイアウトを示す説明図である。
【図７】図６のレイアウトで格子点を設定した場合を示す説明図である。
【図８】図６のレイアウトで目標位置を設定するための計算領域とセンサ位置を設定するための計算領域の格子点が一致する例を示す説明図である。
【図９】図６のレイアウトで目標位置を設定するための計算領域とセンサ位置を設定するための計算領域の格子点が一致しない例を示す説明図である。
【図１０】図６のレイアウトで目標位置を設定するための計算領域とセンサ位置を設定するための計算領域が異なる領域を設定する場合の例を示す説明図である。
【図１１】この発明の実施の形態２による測位精度評価装置を示す構成図である。
【図１２】この発明の実施の形態３による測位精度評価装置を示す構成図である。
【図１３】この発明の実施の形態４による測位精度評価装置を示す構成図である。
【図１４】この発明の実施の形態５による測位精度評価装置を示す構成図である。
【図１５】この発明の実施の形態５による測位精度評価装置における目標経路と格子点との関係を示す説明図である。
【図１６】この発明の実施の形態６による測位精度評価装置を示す構成図である。
【図１７】この発明の実施の形態７による測位精度評価装置を示す構成図である。
【図１８】この発明の実施の形態８による測位精度評価装置を示す構成図である。
【図１９】この発明の実施の形態８による測位精度評価装置において、指定目標に対し等距離のセンサ配置となるセンサ配置候補を示す説明図である。
【図２０】この発明の実施の形態８による測位精度評価装置において、指定目標に対し等距離のセンサ配置となる場合に選択するセンサ配置候補を示す説明図である。
【図２１】距離を観測するセンサ３台により目標位置を測位する場合の説明図である。
【発明を実施するための形態】
【００１２】
実施の形態１．
図１は、この発明の実施の形態１による測位精度評価装置を示す構成図である。
図１に示す測位精度評価装置は、センサ観測誤差分散設定部１、観測効果行列算出部２、観測効果行列和算出部３、測位誤差指標算出部４、総合測位誤差指標算出部５、総合測位誤差指標評価部６、センサ位置設定部７、観測効果行列変換行列算出部８、観測効果行列蓄積部９、測位誤差初期値設定部１０、レイアウト設定部１１、格子点設定部１２、計算領域設定部１３、目標位置設定部１４を備えている。
【００１３】
センサ観測誤差分散設定部１では、ユーザが測位のために設置したいセンサの台数を決め、そのセンサ毎に、センサの観測精度であるセンサ観測誤差分散を設定する。センサ観測誤差分散の例は、距離を観測するセンサのセンサ観測誤差分散は式（６）、角度を観測するセンサのセンサ観測誤差分散は式（７）、距離と角度を観測するセンサのセンサ観測誤差分散は式（６）と式（７）となる。センサ観測誤差分散は、距離や角度を観測する成分の数とセンサの数とを設定する。ここで、式（６）のσＲは距離観測精度、式（７）のσθは角度観測精度であり、これらの値は、センサ毎に異なる場合も同じ場合もある。また、式（６）及び式（７）は距離及び角度を観測するセンサのセンサ観測誤差分散の例であるが、距離変化率を観測するセンサの場合、加速度を観測するセンサの場合、オイラー角を観測するセンサの場合等、異なる観測成分が得られるセンサに適用可能である。
センサ観測誤差分散＝（σＲ）＾２      （６）
センサ観測誤差分散＝（σθ）＾２      （７）
【００１４】
観測効果行列算出部２では、観測効果行列変換行列算出部８から得る観測効果行列変換行列と、センサ観測誤差分散設定部１から得るセンサ観測誤差から観測効果行列を式（８）により算出する。式（８）において、ｉはセンサｉを意味し、１台以上のセンサの中のある１つのセンサを意味する。
式（８）において、Ｅｉはセンサｉの観測効果行列、Ｈｉはセンサｉの観測効果行列変換行列、Ｂｉは観測誤差共分散行列である。観測誤差共分散行列Ｂｉは、センサｉが距離のみ観測の場合は式（９）、角度のみ観測の場合は式（１０）、距離と角度を観測する場合は式（１１）における１行１列成分Ｂｉ（１，１）、１行２列成分Ｂｉ（１，２）、２行１列成分Ｂｉ（２，１）、２行２列成分Ｂｉ（２，２）を成分に持つ行列となる。観測誤差共分散行列は、センサｉの観測成分や次元により異なっても良い。なお、ここで、センサの観測効果とは、ある座標での観測誤差を小さくする度合いを示している。
式（９）〜式（１１）において、（σＲｉ）＾２はセンサｉが距離を観測する場合のセンサ観測誤差分散、（σθｉ）＾２はセンサｉが角度を観測する場合のセンサ観測誤差分散である。
Ｅｉ＝Ｈｉ′ｉｎｖ（Ｂｉ）Ｈｉ      （８）
Ｂｉ＝（σＲｉ）＾２                （９）
Ｂｉ＝（σθｉ）＾２                （１０）
Ｂｉ（１，１）＝（σＲｉ）＾２
Ｂｉ（１，２）＝０
Ｂｉ（２，１）＝０
Ｂｉ（２，２）＝（σθｉ）＾２      （１１）
【００１５】
観測効果行列和算出部３では、観測効果行列算出部２から得る観測効果行列を１台以上のセンサ分足して、観測効果行列和を算出する。Ｎ台のセンサの場合、観測効果行列和は式（１２）により算出できる。ΣＥｉ＿（ｉ＝１＿Ｎ）はＮ台のセンサの観測効果行列和である。
ΣＥｉ＿（ｉ＝１＿Ｎ）＝Ｅ１＋Ｅ２＋…＋Ｅｉ＋…＋ＥＮ      （１２）
【００１６】
測位誤差指標算出部４では、観測効果行列和算出部３から得る観測効果行列和と、測位誤差初期値設定部１０から得る測位誤差共分散行列初期値の逆行列より、測位誤差指標の逆数を式（１３）により算出する。式（１３）において、Ｐｐｏｓは測位誤差共分散行列、Ｐｐｏｓｉｎｉｔは測位誤差共分散行列初期値、ｉｎｖ（Ａ）は行列Ａの逆行列を意味する。つまり、ｉｎｖ（Ｐｐｏｓｉｎｉｔ）は測位誤差共分散行列初期値の逆行列である。さらに、式（１４）から測位誤差共分散行列を算出する。
ｉｎｖ（Ｐｐｏｓ）
＝ΣＥｉ＿（ｉ＝１＿Ｎ）＋ｉｎｖ（Ｐｐｏｓｉｎｉｔ）    （１３）
Ｐｐｏｓ
＝ｉｎｖ（ΣＥｉ＿（ｉ＝１＿Ｎ）＋ｉｎｖ（Ｐｐｏｓｉｎｉｔ））    （１４）
【００１７】
ここで、測位誤差共分散行列Ｐｐｏｓから測位誤差指標ＰＥＩ（Ｐｏｓｉｔｉｏｎｉｎｇ  Ｅｒｒｏｒ  Ｉｎｄｅｘ）を求める方法を示す。測位位置が３次元で求まり、測位誤差共分散行列Ｐｐｏｓとして３行３列の行列が算出された場合、測位誤差共分散行列Ｐｐｏｓの１行１列の成分をＰｐｏｓ（１，１）とし、以下同様に３行３列の成分をＰｐｏｓ（３，３）まで定義する。１行または１列は３次元位置のｘ成分、２行または２列は３次元位置のｙ成分、３行または３列は３次元位置のｚ成分である。
【００１８】
測位誤差共分散行列Ｐｐｏｓの最大固有値の平方根を測位誤差指標ＰＥＩと定義した場合は式（１５）となる。式（１５）において、ｍａｘｅｉｇ（）は行列の最大固有値を算出する関数である。また、式（１６）は測位誤差共分散行列Ｐｐｏｓの最大固有値をＰＥＩ、式（１７）は測位誤差共分散行列Ｐｐｏｓのｘ、ｙ、ｚ成分の和の平方根をＰＥＩ、式（１８）は測位誤差共分散行列Ｐｐｏｓのｘ、ｙ、ｚ成分の和をＰＥＩ、式（１９）は測位誤差共分散行列Ｐｐｏｓのｘ、ｙ成分の和の平方根、式（２０）は測位誤差共分散行列Ｐｐｏｓのｘ、ｙ成分の和、式（２１）は測位誤差共分散行列Ｐｐｏｓのｚ成分の平方根、式（２２）は測位誤差共分散行列Ｐｐｏｓのｚ成分である。
【００１９】
式（１５）〜式（２２）では、測位位置誤差共分散行列が３行３列の場合の測位誤差指標の例であるが、３次元位置と距離変化率を測位する場合には、４行４列の行列となったり、２次元位置を測位する場合には、２行２列の行列となったり、センサの観測成分の数により、測位誤差共分散行列の大きさは変化し、また、測位誤差共分散行列のどの成分を抽出して、測位誤差指標ＰＥＩを求めてもよい。例えば、３次元位置と距離変化率を測位する場合や３次元位置とバイアス時刻等の４行４列の測位誤差共分散行列を持つ場合の測位誤差指標ＰＥＩは、式（２３）や式（２４）とすることもできる。式（２３）、式（２４）におけるｔｒａｃｅ（）は行列の対角成分の和を意味する。
【００２０】
ＰＥＩ＝（ｍａｘｅｉｇ（Ｐｐｏｓ））＾（１／２）      （１５）
ＰＥＩ＝ｍａｘｅｉｇ（Ｐｐｏｓ）                      （１６）
ＰＥＩ
＝（Ｐｐｏｓ（１，１）＋Ｐｐｏｓ（２，２）＋Ｐｐｏｓ（３，３））＾（１／２）
（１７）
ＰＥＩ＝Ｐｐｏｓ（１，１）＋Ｐｐｏｓ（２，２）＋Ｐｐｏｓ（３，３）    （１８）
ＰＥＩ＝（Ｐｐｏｓ（１，１）＋Ｐｐｏｓ（２，２））＾（１／２）      （１９）
ＰＥＩ＝Ｐｐｏｓ（１，１）＋Ｐｐｏｓ（２，２）    （２０）
ＰＥＩ＝（Ｐｐｏｓ（３，３））＾（１／２）        （２１）
ＰＥＩ＝Ｐｐｏｓ（３，３）                        （２２）
ＰＥＩ＝（ｔｒａｃｅ（Ｐｐｏｓ））＾（１／２）    （２３）
ＰＥＩ＝ｔｒａｃｅ（Ｐｐｏｓ）                    （２４）
【００２１】
測位誤差指標算出部４では、式（１５）〜式（２４）の測位誤差指標ＰＥＩの内、事前にユーザが決めた算出式により測位誤差指標ＰＥＩを算出する。
【００２２】
総合測位誤差指標算出部５を図２を用いて説明する。図２は、ある領域を格子上に分割した格子点の例である。図２において、(1)〜(9)は格子点である。測位誤差指標算出部４は、図２の例では、図３のように、格子点(1)でＰＥＩを算出し、同様に格子点(2)〜(9)でＰＥＩを算出する。格子点(1)〜(9)毎に算出したＰＥＩをＰＥＩ(1)、…、ＰＥＩ(9)とする。
総合測位誤差指標をＣＰＥＩ（Ｃｏｍｐｒｅｈｅｎｓｉｖｅ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｅｒｒｏｒ  Ｉｎｄｅｘ）と呼ぶことにする。ＣＰＥＩを、格子点毎に算出する測位誤差指標ＰＥＩの平均と定義した場合、図３の例では、ＣＰＥＩは式（２５）により算出できる。式（２５）におけるｍｅａｎ（）は平均を取る関数を意味する。
【００２３】
同様に、和を取る関数ｓｕｍ（）、最大値を取る関数ｍａｘ（）、最小値を取る関数ｍｉｎ（）、最頻値を取る関数ｍｏｄｅ（）、メジアンを取る関数ｍｅｄｉａｎ（）でＣＰＥＩを定義した場合、図３の例では、ＣＰＥＩはそれぞれ式（２６）〜式（３０）により算出できる。式（２５）〜式（３０）は格子点数が９の場合のＣＰＥＩであるが、格子点数Ｎ個の場合にも容易に拡張できる。また、ＣＰＥＩを算出する関数はｍｅａｎ（）、ｓｕｍ（）、ｍａｘ（）、ｍｉｎ（）、ｍｏｄｅ（）、ｍｅｄｉａｎ（）の例を挙げたが、他の関数への拡張も容易にできる。
【００２４】
総合測位誤差指標算出部５では、式（２５）〜式（３０）の内、事前に決めた指標により、ＣＰＥＩを計算する。そして、センサ配置パターン毎の総合測位誤差指標を蓄積する。センサ配置パターンを図４及び図５により説明する。
図４はセンサが１台の場合（第１のセンサ）のセンサ配置パターンの例、図５はセンサ数が２台の場合（第１、第２のセンサ）のセンサ配置パターンの例である。図４及び図５の点線の交点が格子点である。まず、図４の（ａ）または（ｂ）に示すセンサ配置パターンのように、第１のセンサが配置された場合に、全格子点の総合測位誤差指標ＣＰＥＩを算出する。そして、図５の（ａ）または（ｂ）に示すセンサ配置パターンのように、全格子点の総合測位誤差指標ＣＰＥＩを算出する。センサ配置パターンは説明を簡略化するため２次元平面で表しているが、３次元空間におけるセンサ配置パターンへと拡張可能である。
ＣＰＥＩ＝ｍｅａｎ（ＰＥＩ(1)，ＰＥＩ(2)，…，ＰＥＩ(9)）      （２５）
ＣＰＥＩ＝ｓｕｍ（ＰＥＩ(1)，ＰＥＩ(2)，…，ＰＥＩ(9)）        （２６）
ＣＰＥＩ＝ｍａｘ（ＰＥＩ(1)，ＰＥＩ(2)，…，ＰＥＩ(9)）        （２７）
ＣＰＥＩ＝ｍｉｎ（ＰＥＩ(1)，ＰＥＩ(2)，…，ＰＥＩ(9)）        （２８）
ＣＰＥＩ＝ｍｏｄｅ（ＰＥＩ(1)，ＰＥＩ(2)，…，ＰＥＩ(9)）      （２９）
ＣＰＥＩ＝ｍｅｄｉａｎ（ＰＥＩ(1)，ＰＥＩ(2)，…，ＰＥＩ(9)）  （３０）
【００２５】
総合測位誤差指標評価部６では、総合測位誤差指標算出部５から得たセンサ配置パターン毎の総合測位誤差指標ＣＰＥＩが、ユーザが事前に設定した総合測位誤差指標しきい値よりも大きい場合は、センサ位置設定部７に、センサの配置場所を変更する制御信号を出力する。
【００２６】
センサ位置設定部７では、計算領域設定部１３から得るセンサ位置を設定するための計算領域に基づき、計算領域内の格子点上に順にセンサ位置を設定する。
センサ位置を設定するための計算領域とは、図２のような格子点の集合である。
例えば、センサ１台の場合の例として、図４（ａ）のセンサ配置パターンのように設定する。また、センサ２台の場合の例として、図５（ａ）のセンサ配置パターンのように設定する。図４及び図５は２次元平面におけるセンサ位置の設定の例であるが、３次元平面にも拡張可能である。
【００２７】
観測効果行列変換行列算出部８では、センサ位置設定部７から得るセンサ位置と、目標位置設定部１４から得る目標位置から、センサｉの観測効果行列変換行列Ｈｉを算出する。
観測効果行列変換行列Ｈｉを式（３１）により算出する。
Ｈｉ＝∂ｈｉ（ｘ）／∂ｘ            （３１）
センサｉが観測成分として距離Ｒｉを観測する場合、２次元平面を仮定すると、式（３２）となる。また、センサｉが観測成分として距離Ｒｉを観測する場合、３次元平面を仮定すると、式（３３）となる。ここで、式（３２）において、目標位置ｘは（ｘ，ｙ）、センサｉの位置ｓｉは（ｘｉ，ｙｉ）、式（３３）において、目標位置ｘは（ｘ，ｙ，ｚ）、センサｉの位置ｓｉは（ｘｉ，ｙｉ，ｚｉ）である。また、ｈｉ（ｘ）は目標位置ｘを観測成分に変換する関数である。
また、センサｉが観測成分として角度θｉを観測する場合は式（３４）となる。式（３４）のａｔａｎ（）はアークタンジェントを意味する。そして、センサｉが観測成分として、距離と角度の２次元の観測成分を得る場合や、距離と仰角と方位角の３次元の観測成分を得る場合や、距離変化率を得る場合や、目標の３次元の加速度を得る場合や、目標のｘ、ｙ成分の２次元位置を得る場合や、目標のｘ、ｙ、ｚ成分の３次元位置を得る場合にも拡張可能である。
【００２８】
ｈｉ（ｘ）＝Ｒｉ
＝（（ｘ−ｘｉ）＾２＋（ｙ−ｙｉ）＾２）＾（１／２）    （３２）
ｈｉ（ｘ）＝Ｒｉ
＝（（ｘ−ｘｉ）＾２＋（ｙ−ｙｉ）＾２＋（ｚ−ｚｉ）＾２）＾（１／２）  （３３）
ｈｉ（ｘ）＝θｉ＝ａｔａｎ（（ｘ−ｘｉ）／（ｙ−ｙｉ））      （３４）
【００２９】
観測効果行列蓄積部９では、観測効果行列算出部２から得るセンサ１、…センサｉ、…センサＮ（ただし、１≦ｉ≦Ｎ）の観測効果行列を蓄積する。そして、蓄積した観測効果行列を観測効果行列和算出部３に出力する。
【００３０】
測位誤差初期値設定部１０では、２次元位置を測位する例では、測位誤差初期値のＰｉｎｉｔを式（３５）、３次元位置を測位する例では、測位誤差初期値Ｐｉｎｉｔを式（３６）のように設定する。式（３５）及び式（３６）において、（σｘｐｏｓｉｎｉｔ）＾２、（σｙｐｏｓｉｎｉｔ）＾２、（σｚｐｏｓｉｎｉｔ）＾２は、それぞれｘ、ｙ、ｚ成分の測位誤差初期値の分散である。
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（１，１）＝１／（σｘｐｏｓｉｎｉｔ）＾２
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（２，２）＝１／（σｙｐｏｓｉｎｉｔ）＾２
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（１，２）＝０
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（２，１）＝０                          （３５）
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（１，１）＝（σｘｐｏｓｉｎｉｔ）＾２
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（２，２）＝（σｙｐｏｓｉｎｉｔ）＾２
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（３，３）＝（σｚｐｏｓｉｎｉｔ）＾２
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（１，２）＝０
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（１，３）＝０
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（２，１）＝０
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（２，３）＝０
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（３，１）＝０
ｉｎｖ（Ｐｐｏｓｉｎｉｔ）（３，２）＝０                          （３６）
【００３１】
レイアウト設定部１１を図６により説明する。図６は、構造物と障害物からなるレイアウトを示している。構造物や障害物は図６中の塗りつぶした箇所１１０ａであり、塗りつぶしていない箇所１１０ｂは例えば道である。ここで、構造物の例としては、店舗やビル等の建物、自然の崖や山、家が考えられる。そして、障害物の例としては、構造物からはみ出したものであり、店舗等では、ファサードやパラペット等が考えられる。図６のレイアウトは説明を簡略化するために２次元平面で表しているが、３次元空間で表しても良い。また、レイアウトは、屋内でも屋外でも適用可能である。そして、構造物や障害物は、建物が実際になくても、ユーザが仮想的にレイアウト上で設定できるものとする。
そして、レイアウト図では、構造物や障害物と道との相対位置座標がわかるものとする。そして、レイアウト図上の絶対位置が１点分かれば、構造物や障害物や道の絶対位置が分かる。レイアウト設定部１１では、構造物や障害物や道の相対位置座標を含むレイアウト図を出力する。
【００３２】
格子点設定部１２を、図７を用いて説明する。図７は、図６のレイアウトにおいて、格子点を設定した場合の例を表した図である。図７において、格子として、指定する間隔で縦方向と横方向に分割し、格子点を設定する。格子点は、図７の点線同士の交点である。図７では２次元平面での格子点設定の例であるが、３次元空間の縦方向、横方向、高さ方向でも指定する間隔で分割し、格子点を設定する事が可能である。
各格子点はレイアウト図の構造物や障害物や道の相対位置座標と同様、相対位置座標を持ち、レイアウト図上の絶対位置が１点分かれば、各格子点の位置座標が分かるものとする。
【００３３】
計算領域設定部１３では、図７のように、構造物や障害物を除いた格子点を計算領域とするように設定する。実際には構造物があっても、構造物を含む箇所を計算領域に設定することが可能である。計算領域には、構造物や障害物や道の位置座標、格子点の位置座標が含まれる。
計算領域設定部１３では、目標位置を設定するための計算領域、センサ位置を設定するための計算領域を出力する。ここで、目標位置を設定するための計算領域と、センサ位置を設定するための計算領域は、同じ領域を設定しても良いし、異なる領域を設定しても良い。
計算領域設定部１３では、目標位置を設定するための計算領域を、目標位置設定部１４に出力し、センサ位置を設定するための計算領域をセンサ位置設定部７に出力する。
【００３４】
目標位置設定部１４では、計算領域設定部１３から出力される目標位置を設定するための計算領域上の格子点に目標を設定する。図８は、目標位置を設定するための計算領域とセンサ位置を設定するための計算領域の格子点が一致する例、図９は、目標位置を設定するための計算領域とセンサ位置を設定するための計算領域の格子点が一致しない例である。図８及び図９において、○は目標位置１１０ｃ、△はセンサ位置１１０ｄである。
図１０は、目標位置を設定するための計算領域とセンサ位置を設定するための計算領域が異なる領域を設定する場合の例である。
【００３５】
以上説明したように、実施の形態１の測位精度評価装置によれば、複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定するセンサ観測誤差分散設定部と、センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、センサ観測誤差分散と観測効果行列変換行列を用いて、複数のセンサ毎の観測効果を表す観測効果行列を算出する観測効果行列算出部と、複数のセンサの観測効果行列の和を算出する観測効果行列和算出部と、測位精度の初期値である測位誤差初期値の逆行列と観測効果行列の和とを加算した値の逆行列から測位誤差指標を算出する測位誤差指標算出部と、複数地点の測位誤差指標の演算結果を示す総合測位誤差指標を算出する総合測位誤差指標算出部と、総合測位誤差指標が、設定された総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合測位誤差指標評価部とを備えたので、次のような効果がある。
すなわち、異種複数センサの観測効果行列を算出して観測効果行列和を算出し、測位誤差指標を算出することで、異種複数センサの測位精度が算出できる。また、測位誤差指標から算出した総合測位誤差指標を算出し、総合測位誤差指標を評価することで、総合測位誤差指標に基づく、測位誤差を低減するような効率的なセンサ配置が可能となり、結果として、センサ台数を削減することが可能である。さらに、各センサの観測効果行列を算出して観測効果行列を蓄積し、蓄積した観測効果行列を用いて、観測効果行列を算出し、測位誤差指標を算出することで、従来技術と比べて、センサ位置と目標位置を変更する場合やセンサ数増加に伴う測位精度の計算量を削減することが可能となる。
【００３６】
実施の形態２．
図１１はこの発明の実施の形態２の測位精度評価装置を示す構成図である。
実施の形態２の測位精度評価装置は、センサ観測誤差分散設定部１、観測効果行列算出前行列算出部２０１、第２の観測効果行列和算出部３０１、測位誤差指標算出部４、総合測位誤差指標算出部５、総合測位誤差指標評価部６、センサ位置設定部７、観測効果行列変換行列算出部８、観測効果行列算出前行列蓄積部９０１、測位誤差初期値設定部１０、レイアウト設定部１１、格子点設定部１２、計算領域設定部１３、目標位置設定部１４、センサ変更設定部１５からなる。ここで、観測効果行列算出前行列算出部２０１、第２の観測効果行列和算出部３０１、観測効果行列算出前行列蓄積部９０１及びセンサ変更設定部１５以外の構成及び動作は図１に示した実施の形態１と同様であるため、対応する部分に同一符号を付してその説明を省略する。
【００３７】
観測効果行列算出前行列算出部２０１では、式（３７）により、センサｉの観測効果行列変換行列Ｈｉから、観測効果行列算出前行列Ｆｉを算出する。
Ｆｉ＝Ｈｉ′Ｈｉ                （３７）
観測効果行列算出前行列算出部２０１では、センサｉが距離のみの１次元の観測成分を観測する例の場合は、式（３８）に示すように観測効果行列算出前行列係数εｉを算出する。ここで、（σＲｉ）＾２はセンサｉの観測誤差分散である。また、センサｉが角度のみの１次元の観測成分を観測する例の場合は、式（３９）に示すように観測効果行列算出前行列係数εｉを算出する。ここで、距離や角度の場合の観測効果行列算出前行列係数εｉの計算の例を示したが、距離差や距離変化率等の別の観測成分を観測する場合にも容易に拡張できる。
εｉ＝１／（σＲｉ）＾２        （３８）
εｉ＝１／（σθｉ）＾２        （３９）
観測効果行列算出前行列算出部２０１は、センサｉの観測効果行列算出前行列係数εｉと観測効果行列算出前行列Ｆｉを出力する。
【００３８】
観測効果行列算出前行列蓄積部９０１は、センサ毎に得られる観測効果行列算出前行列係数と観測効果行列算出前行列を蓄積する。
【００３９】
センサ変更設定部１５は、センサの種類を変更して観測効果行列を算出するための制御信号を、第２の観測効果行列和算出部３０１に出力する。
センサの種類を変更の場合の例を説明するために、センサの種類を変更前のセンサをセンサｉ−ｂｅｆｏｒｅ、センサの種類を変更後のセンサをｉ−ａｆｔｅｒと呼ぶこととする。
例えば、センサｉ−ｂｅｆｏｒｅが距離のみの１次元の観測成分を観測するセンサ、センサｉ−ａｆｔｅｒが角度のみの１次元の観測成分を観測するセンサとなるようなセンサ、つまり、異種センサへの変更が考えられ、その逆も同様である。また、センサｉ−ｂｅｆｏｒｅが距離のみの１次元の観測成分を観測するセンサであり、センサの距離観測誤差分散（σＲｉ−ｂｅｆｏｒｅ）＾２を持つ場合から、センサｉ−ａｆｔｅｒが距離のみの１次元の観測成分を観測するセンサであり、センサの距離観測誤差分散（σＲｉ−ａｆｔｅｒ）＾２、となるようなセンサ、つまり同種センサであるが観測誤差分散が異なるセンサへの変更が考えられ、１次元の観測成分として角度や距離差、距離変化率等を観測する場合も同様である。
また、センサ変更設定部１５では、センサの種類が変更前と変更後の観測効果行列算出前行列係数を蓄積する制御信号を、観測効果行列算出前行列蓄積部９０１に出力する。
【００４０】
観測効果行列算出前行列蓄積部９０１では、観測効果行列算出前行列Ｆｉとセンサの種類が変更前と変更後の観測効果行列算出前行列係数εｉをセンサ１〜センサＮ分蓄積する。
【００４１】
第２の観測効果行列和算出部３０１では、センサ変更設定部１５から得る、センサの種類を変更して観測効果行列を算出するための制御信号により、あるセンサｉのセンサ種類が変わる場合、センサｉの観測誤差分散から観測効果行列算出前行列係数εｉを算出し、その算出した観測効果行列算出前行列係数εｉと、観測効果行列算出前行列Ｆｉを式（４０）に代入し、センサｉの観測効果行列Ｅｉを算出する。
そして、第２の観測効果行列和算出部３０１は、観測効果行列算出前行列算出部２０１と観測効果行列算出前行列蓄積部９０１から得るセンサ１〜Ｎのセンサの観測効果行列算出前行列Ｆｉ（１≦ｉ≦Ｎ）と観測効果行列算出前行列係数εｉ（１≦ｉ≦Ｎ）から、式（４０）により、センサｉの観測効果行列Ｅｉ（１≦ｉ≦Ｎ）を算出し、式（１２）により、Ｎ台のセンサの観測効果行列和ΣＥｉ＿（ｉ＝１＿Ｎ）を算出する。
Ｅｉ＝（εｉ）Ｆｉ                （４０）
【００４２】
測位誤差指標算出部４以降の動作は実施の形態１と同様であるため、ここでの説明は省略する。
【００４３】
以上説明したように、実施の形態２の測位精度評価装置によれば、複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定するセンサ観測誤差分散設定部と、センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、センサ観測誤差分散から観測効果行列算出前行列係数を算出すると共に、観測効果行列変換行列を用いて、複数のセンサ毎の観測効果行列算出前行列を算出する観測効果行列算出前行列算出部と、観測効果行列算出前行列と、センサの種類によって決定される観測効果行列算出前行列係数とを用いて観測効果行列を求め、さらに複数のセンサの観測効果行列和を算出する第２の観測効果行列和算出部と、センサの種類を変更する場合、センサの変更を示す信号を前記第２の観測効果行列和算出部に送出するセンサ変更設定部と、測位精度の初期値である測位誤差初期値の逆行列と前記観測効果行列の和とを加算した値の逆行列から測位誤差指標を算出する測位誤差指標算出部と、複数地点の測位誤差指標の演算結果を示す総合測位誤差指標を算出する総合測位誤差指標算出部と、総合測位誤差指標が、設定された総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合測位誤差指標評価部とを備え、第２の観測効果行列和算出部は、センサ変更設定部よりセンサの変更を示す信号を受けた場合、変更されるセンサの観測効果行列算出前行列係数を用いて観測効果行列を求めるようにしたので、次のような効果がある。
すなわち、観測効果行列算出前行列と観測効果行列算出前行列係数から観測効果行列を算出することにより、観測効果行列の行列演算回数が少なくなる。また、センサ変更設定部で、センサの種類を変更して、観測効果行列を計算する場合、観測効果行列算出前行列係数のみを変更し、観測効果行列算出前行列の再計算は不要となる。その結果、実施の形態１の場合、センサの種類を変更した場合に、観測効果行列算出部における観測効果行列計算量が大きく増加するのに対し、本実施の形態の場合は、観測効果行列の計算量は少なく済む効果がある。
【００４４】
実施の形態３．
図１２はこの発明の実施の形態３の測位精度評価装置を示す構成図である。
実施の形態３の測位精度評価装置は、第２のセンサ観測誤差分散設定部１０１、観測効果行列算出部２、観測効果行列和算出部３、測位誤差指標算出部４、総合測位誤差指標算出部５、総合測位誤差指標評価部６、センサ位置設定部７、観測効果行列変換行列算出部８、観測効果行列蓄積部９、測位誤差初期値設定部１０、レイアウト設定部１１、格子点設定部１２、計算領域設定部１３、目標位置設定部１４からなる。ここで、第２のセンサ観測誤差分散設定部１０１以外の構成及び動作は図１に示した実施の形態１と同様であるため、対応する部分に同一符号を付してその説明を省略する。
【００４５】
第２のセンサ観測誤差分散設定部１０１では、例えば、センサｉ（１≦ｉ≦Ｎ）が距離と角度を観測するセンサと仮定する。センサｉが距離と角度を観測するセンサの場合、観測誤差共分散行列Ｂｉの各成分は、式（１１）で与えられる。以下に式（１１）を再度示す。
Ｂｉ（１，１）＝（σＲｉ）＾２
Ｂｉ（１，２）＝０
Ｂｉ（２，１）＝０
Ｂｉ（２，２）＝（σθｉ）＾２      （１１）
【００４６】
第２のセンサ観測誤差分散設定部１０１では、距離と角度を観測するセンサｉが仮想的に、距離の１成分を観測するセンサｉ−１、と、角度の１成分を観測するセンサｉ−２に分けて観測できるとする。距離を観測するセンサｉ−１の観測誤差分散Ｂｉ−１、角度を観測するセンサｉ−２の観測誤差分散Ｂｉ−２は、それぞれ式（４１）及び式（４２）となる。
ここで、観測成分が２つのセンサを例にしたが、観測成分が３つ、また、観測成分が多くなっても、同様に、１成分ずつ分けて、観測誤差分散を設定することは容易である。
Ｂｉ−１＝（σＲｉ）＾２            （４１）
Ｂｉ−２＝（σθｉ）＾２            （４２）
【００４７】
第２のセンサ観測誤差分散設定部１０１では、距離と角度を観測するセンサｉを仮想的に、距離の１成分を観測するセンサｉ−１と、角度の１成分を観測するセンサｉ−２の別のセンサであるが、同一のセンサ位置を設定する制御信号をセンサ位置設定部７に出力する。一方、第２のセンサ観測誤差分散設定部１０１では、距離を観測するセンサｉ−１の観測誤差分散Ｂｉ−１、角度を観測するセンサｉ−２の観測誤差分散Ｂｉ−２のような別センサとして、観測誤差分散を観測効果行列算出部２に出力する。以降の動作は実施の形態１と同様である。
【００４８】
以上説明したように、実施の形態３の測位精度評価装置によれば、複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定すると共に、複数の観測成分を観測するセンサを仮想的に同一のセンサ位置で複数のセンサ観測誤差分散を有するセンサとして設定する第２のセンサ観測誤差分散設定部と、センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、センサ観測誤差分散と観測効果行列変換行列を用いて、複数のセンサ毎の観測効果を表す観測効果行列を算出する観測効果行列算出部と、複数のセンサの観測効果行列の和を算出する観測効果行列和算出部と、測位精度の初期値である測位誤差初期値の逆行列と前記観測効果行列の和とを加算した値の逆行列から測位誤差指標を算出する測位誤差指標算出部と、複数地点の測位誤差指標の演算結果を示す総合測位誤差指標を算出する総合測位誤差指標算出部と、総合測位誤差指標が、設定された総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合測位誤差指標評価部とを備えたので、複数の観測成分を観測するセンサを、仮想的に１成分を観測するセンサと見なし、観測効果行列を算出させることで、観測効果行列の計算の際の行列次数が小さくなるため、行列計算の負荷を軽減することができる。
【００４９】
実施の形態４
図１３はこの発明の実施の形態４の測位精度評価装置を示す構成図である。実施の形態３は、第２のセンサ観測誤差分散設定部１０１を実施の形態１の構成に適用したものであるが、実施の形態４は、第２のセンサ観測誤差分散設定部１０１を実施の形態２に適用したものである。すなわち、実施の形態４の測位精度評価装置は、第２のセンサ観測誤差分散設定部１０１、観測効果行列算出前行列算出部２０１、第２の観測効果行列和算出部３０１、測位誤差指標算出部４、総合測位誤差指標算出部５、総合測位誤差指標評価部６、センサ位置設定部７、観測効果行列変換行列算出部８、観測効果行列算出前行列蓄積部９０１、測位誤差初期値設定部１０、レイアウト設定部１１、格子点設定部１２、計算領域設定部１３、目標位置設定部１４、センサ変更設定部１５からなる。ここで、第２のセンサ観測誤差分散設定部１０１以外の構成及び動作は図１１に示した実施の形態２と同様であるため、対応する部分に同一符号を付してその説明を省略する。
【００５０】
第２のセンサ観測誤差分散設定部１０１から、距離、角度を観測するセンサｉを仮想的に、距離の１成分を観測するセンサｉ−１と、角度の１成分を観測するセンサｉ−２の別のセンサであるが、同一のセンサ位置を設定する制御信号を、センサ位置設定部７に出力する。一方、第２のセンサ観測誤差分散設定部１０１は、距離を観測するセンサｉ−１の観測誤差分散Ｂｉ−１、角度を観測するセンサｉ−２の観測誤差分散Ｂｉ−２のような別センサとして、観測誤差分散を観測効果行列算出前行列算出部２０１に出力する。これ以外の動作は実施の形態２と同様である。
【００５１】
以上説明したように、実施の形態４の測位精度評価装置によれば、複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定すると共に、複数の観測成分を観測するセンサを仮想的に同一のセンサ位置で複数のセンサ観測誤差分散を有するセンサとして設定する第２のセンサ観測誤差分散設定部と、センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、センサ観測誤差分散から観測効果行列算出前行列係数を算出すると共に、観測効果行列変換行列を用いて、複数のセンサ毎の観測効果行列算出前行列を算出する観測効果行列算出前行列算出部と、観測効果行列算出前行列と、センサの種類によって決定される観測効果行列算出前行列係数とを用いて観測効果行列を求め、さらに複数のセンサの観測効果行列和を算出する第２の観測効果行列和算出部と、センサの種類を変更する場合、センサの変更を示す信号を第２の観測効果行列和算出部に送出するセンサ変更設定部と、測位精度の初期値である測位誤差初期値の逆行列と前記観測効果行列の和とを加算した値の逆行列から測位誤差指標を算出する測位誤差指標算出部と、複数地点の前記測位誤差指標の演算結果を示す総合測位誤差指標を算出する総合測位誤差指標算出部と、総合測位誤差指標が、設定された総合測位誤差指標しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合測位誤差指標評価部とを備え、第２の観測効果行列和算出部は、センサ変更設定部より、センサの変更を示す信号を受けた場合、変更されるセンサの観測効果行列算出前行列係数を用いて観測効果行列を求めるようにしたので、複数の観測成分を観測するセンサを、仮想的に１成分を観測するセンサと見なし、観測効果行列算出前行列の計算を行い、観測効果行列の行列演算回数を減らすことができる。
【００５２】
実施の形態５．
図１４はこの発明の実施の形態５の測位精度評価装置を示す構成図である。図示の測位精度評価装置は、センサ観測誤差分散設定部１、観測効果行列算出部２、観測効果行列和算出部３、測位誤差指標算出部４、総合測位誤差指標算出部５、総合測位誤差指標評価部６、センサ位置設定部７、観測効果行列変換行列算出部８、観測効果行列蓄積部９、測位誤差初期値設定部１０、レイアウト設定部１１、格子点設定部１２、計算領域設定部１３、目標位置設定部１４、目標経路設定部１６、予測誤差共分散行列設定部１７からなる。ここで、目標経路設定部１６及び予測誤差共分散行列設定部１７以外の構成及び動作は図１に示した実施の形態１と同様であるため、対応する部分に同一符号を付してその説明を省略する。
【００５３】
目標経路設定部１６及び予測誤差共分散行列設定部１７を図１５を用いて説明する。
目標経路設定部１６では、図１５のように、目標位置設定部１４から得る目標位置設定のための計算領域から、目標初期位置と目標最終位置の格子点を設定し、目標初期位置と目標最終位置を含む目標経路を決定する。目標初期位置と目標最終位置を含む目標経路上の格子点はｋ＋１個あるものとする。
そして、目標速度Ｖｔｇｔと格子点間隔Ｄｉｎｔからサンプリング間隔Ｔｔｇｔを式（４３）により算出する。
Ｔｔｇｔ＝Ｄｉｎｔ／Ｖｔｇｔ        （４３）
【００５４】
予測誤差共分散行列設定部１７の説明のため、目標の２次元位置・速度を推定する前提で、Ｐｉｎｉｔ（−）は４行４列の行列とする。３次元位置・速度を推定する場合のＰｉｎｉｔ（−）は６行６列の行列、２次元位置・速度・加速度を推定する場合のＰｉｎｉｔ（−）は６行６列の行列、３次元位置・速度・加速度を推定する場合のＰｉｎｉｔ（−）は９行９列の行列のように拡張可能である。
【００５５】
まず、時刻ｔ＝ｋの場合の予測誤差共分散行列をＰ（−）（ｔ＝ｋ）とし、時刻ｔ＝ｋの場合の測位誤差共分散行列をＰ（＋）（ｔ＝ｋ）とすると、カルマンフィルタの予測誤差共分散行列算出式から式（４４）が成り立つ。ただし、Φ（ｔ＝ｋ−１）は、時刻ｔ＝ｋ−１の状態遷移行列、Ｑ（ｔ＝ｋ−１）は、時刻ｋ−１の事前に設定する目標の運動の曖昧さを定義する駆動雑音共分散行列である。式（４５）に示すように、時刻ｔ＝ｋの状態遷移行列Φ（ｔ＝ｋ）の１行１列成分をΦ（ｔ＝ｋ）（１，１）、…、４行４列成分をΦ（ｔ＝ｋ）（４，４）としている。
予測誤差共分散行列設定部１７では、式（４４）及び式（４５）により時刻ｔ＝ｋの場合の予測誤差共分散行列をＰ（−）（ｔ＝ｋ）を算出する。
なお、予測誤差とは（予測値−真値）の値であり、予測誤差共分散行列とは、予測値ベクトル［ｘｐ  ｙｐ  ｚｐ］と真値ベクトル［ｘｔ  ｙｔ  ｚｔ］の誤差共分散行列（３行３列の行列）を示している。
【００５６】
Ｐ（−）（ｔ＝ｋ）
＝Φ（ｔ＝ｋ−１）・Ｐ（＋）（ｔ＝ｋ−１）・Φ（ｔ＝ｋ−１）′＋Ｑ（ｔ＝ｋ−１）
（４４）
Φ（ｔ＝ｋ−１）（１，１）＝１
Φ（ｔ＝ｋ−１）（１，２）＝０
Φ（ｔ＝ｋ−１）（１，３）＝Ｔｔｇｔ
Φ（ｔ＝ｋ−１）（１，４）＝０
Φ（ｔ＝ｋ−１）（２，１）＝０
Φ（ｔ＝ｋ−１）（２，２）＝１
Φ（ｔ＝ｋ−１）（２，３）＝０
Φ（ｔ＝ｋ−１）（２，４）＝Ｔｔｇｔ
Φ（ｔ＝ｋ−１）（３，１）＝０
Φ（ｔ＝ｋ−１）（３，２）＝０
Φ（ｔ＝ｋ−１）（３，３）＝１
Φ（ｔ＝ｋ−１）（３，４）＝０
Φ（ｔ＝ｋ−１）（４，１）＝０
Φ（ｔ＝ｋ−１）（４，２）＝０
Φ（ｔ＝ｋ−１）（４，３）＝０
Φ（ｔ＝ｋ−１）（４，４）＝１                （４５）
Ｔｇｔは、時刻ｋの格子点から時刻ｋ−１の格子点の間のサンプリング間隔
【００５７】
予測誤差共分散行列設定部１７では、観測効果行列変換行列算出部８において、観測効果行列変換行列Ｈｉを式（４６）で算出する制御信号を出力する。そして、観測効果行列変換行列算出部８では、式（４６）により、観測効果行列変換行列を設定する。ここで、目標の２次元の位置・速度を測位する場合は式（４６）により観測効果行列変換行列を設定するが、目標の３次元の位置・速度を測位する場合は式（４７）により観測効果行列変換行列を設定し、目標の２次元の位置・速度加速度を測位する場合は式（４８）により観測効果行列変換行列を設定する。このように、目標の位置、速度、加速度、それ以上の次元についても同様に拡張することは可能である。
Ｈｉ＝［∂ｈｉ（ｘ）／∂ｘ  ０  ０］          （４６）
Ｈｉ＝［∂ｈｉ（ｘ）／∂ｘ  ０  ０  ０］      （４７）
Ｈｉ＝［∂ｈｉ（ｘ）／∂ｘ  ０  ０  ０  ０］  （４８）
【００５８】
予測誤差共分散行列設定部１７では、時刻ｔ＝０において、観測効果行列変換行列算出部８、観測効果行列算出部２、観測効果行列蓄積部９、観測効果行列和算出部３、測位誤差指標算出部４の計算を行い、時刻ｔ＝１、…時刻ｔ＝ｋにおいて、同様の計算を行い、時刻ｔ＝０、…、時刻ｔ＝ｋの場合の予測誤差共分散行列Ｐ（−）（ｔ＝０）、…、Ｐ（−）（ｔ＝ｋ）を計算していく。
ここで、時刻ｔ＝０、…、時刻ｔ＝ｋの予測誤差共分散行列Ｐ（−）（ｔ＝０）、…、Ｐ（−）（ｔ＝ｋ）は、目標経路設定部１６で得る目標初期位置と目標最終位置を含む目標経路上の格子点における予測誤差共分散行列により計算する。
【００５９】
予測誤差共分散行列設定部１７では、測位誤差初期値設定部１０におけるＰｐｏｓｉｎｉｔに、時刻ｔ＝０から順に、時刻ｔ＝ｋの予測誤差共分散行列を設定する制御信号を出力する（式（４９））。そして、測位誤差初期値設定部１０において、式（５０）のように測位誤差共分散行列を算出する。
Ｐｐｏｓｉｎｉｔ＝Ｐ（−）（ｔ＝０）
Ｐｐｏｓｉｎｉｔ＝Ｐ（−）（ｔ＝１）
…
Ｐｐｏｓｉｎｉｔ＝Ｐ（−）（ｔ＝ｋ）        （４９）
【００６０】
Ｐ（＋）（ｔ＝０）＝ｉｎｖ（ΣＥｉ＿（ｉ＝１＿Ｎ）＋ｉｎｖ（Ｐ（−）（ｔ＝０））
Ｐ（＋）（ｔ＝１）＝ｉｎｖ（ΣＥｉ＿（ｉ＝１＿Ｎ）＋ｉｎｖ（Ｐ（−）（ｔ＝１））
…
Ｐ（＋）（ｔ＝ｋ）＝ｉｎｖ（ΣＥｉ＿（ｉ＝１＿Ｎ）＋ｉｎｖ（Ｐ（−）（ｔ＝ｋ））
（５０）
【００６１】
測位誤差指標算出部４は、前記測位誤差初期値設定部１０から出力された複数地点の測位誤差初期値を用いて測位誤差指標を算出する。以降の動作は実施の形態１と同様である。
【００６２】
以上説明したように、実施の形態５の測位精度評価装置によれば、目標初期位置から目標最終位置までの目標経路を設定する目標経路設定部と、目標経路に沿った複数地点の予測誤差共分散行列を設定する予測誤差共分散行列設定部と、予測誤差共分散行列により、目標経路に沿った複数地点の測位誤差初期値を設定する測位誤差初期値設定部とを備え、測位誤差指標算出部は、前記測位誤差初期値設定部から出力された複数地点の測位誤差初期値を用いて測位誤差指標を算出するようにしたので、センサの欠落やセンサ数が少ない場合でも、目標速度と目標経路を設定して目標経路上の格子点の予測誤差共分散行列を算出した上で測位誤差共分散行列を算出することができ、また、測位誤差を低減するセンサ配置が可能となる。
【００６３】
実施の形態６．
図１６はこの発明の実施の形態６の測位精度評価装置を示す構成図である。図示の測位精度評価装置は、センサ観測誤差分散設定部１、観測効果行列算出部２、観測効果行列和算出部３、測位誤差指標算出部４、総合測位誤差指標算出部５、総合測位誤差指標評価部６、センサ位置設定部７、観測効果行列変換行列算出部８、観測効果行列蓄積部９、測位誤差初期値設定部１０、レイアウト設定部１１、格子点設定部１２、計算領域設定部１３、目標位置設定部１４、格子点数設定部１８を備えている。ここで、格子点数設定部１８以外の構成及び動作は図１に示した実施の形態１と同様であるため、対応する部分に同一符号を付してその説明を省略する。
【００６４】
格子点数設定部１８では、総合測位誤差指標算出部５における総合測位誤差指標の値により格子点設定部１２における格子点の間隔を設定する。すなわち、最初に粗い指定する間隔で縦方向と横方向に分割し、少ない格子点を設定し、総合測位誤差指標算出部５から得る総合測位誤差指標が事前に指定したしきい値よりも大きい場合、細かい間隔で縦方向と横方向を分割し、多くの格子点を設定するよう格子点設定部１２に指示を行う。格子点の設定は説明を簡略化するため、縦方向と横方向の設定を行ったが、高さ方向でも指定する間隔で分割し、格子点を設定する事が可能である。
【００６５】
以上説明したように、実施の形態６の測位精度評価装置によれば、センサ位置と目標位置とを設定するための格子点を設定する格子点設定部と、総合測位誤差指標の値により格子点の間隔を設定する格子点数設定部とを備えたので、格子点を粗い設定から徐々に細かい設定とすることで、少ない格子点数から多い格子点数とし、測位誤差指標の計算量と計算時間の短縮化を図ることができる。
【００６６】
実施の形態７．
図１７はこの発明の実施の形態７の測位精度評価装置を示す構成図である。図示の測位精度評価装置は、センサ観測誤差分散設定部１、観測効果行列算出部２、観測効果行列和算出部３、測位誤差指標算出部４、総合測位誤差指標算出部５、総合測位誤差指標評価部６、センサ位置設定部７、観測効果行列変換行列算出部８、観測効果行列蓄積部９、測位誤差初期値設定部１０、レイアウト設定部１１、格子点設定部１２、計算領域設定部１３、目標位置設定部１４、総合観測効果行列和評価部１９を備えている。ここで、総合観測効果行列和評価部１９以外の構成及び動作は図１に示した実施の形態１と同様であるため、対応する部分に同一符号を付してその説明を省略する。
【００６７】
総合観測効果行列和評価部１９では、観測効果行列和算出部３から得る観測効果行列和ΣＥｉ＿（ｉ＝１＿Ｎ）を格子点毎に得て、式（２５）〜式（３０）のＣＰＥＩを総合観測効果行列和と置き、ＰＥＩを観測効果行列和ΣＥｉ＿（ｉ＝１＿Ｎ）と置きなおし、総合観測効果行列和を算出する。総合観測効果行列和評価部１９では、総合観測効果行列和が、ユーザが事前に設定した総合観測効果行列和のしきい値よりも小さい場合は、センサ位置設定部７に、センサの配置場所を変更する制御信号を出力する。
総合観測効果行列和評価部１９を使用する際は、総合測位誤差指標評価部６によるセンサの配置場所を変更する制御信号は出力しない。すなわち、総合観測効果行列和評価部１９を使用する場合は、測位誤差指標算出部４〜総合測位誤差指標評価部６の動作は行わない。
【００６８】
なお、上記例では実施の形態１の構成に総合観測効果行列和評価部１９を適用したが、実施の形態２〜実施の形態６の構成に適用しても良い。
【００６９】
以上説明したように、実施の形態７の測位精度評価装置によれば、複数のセンサ毎に各センサの観測精度であるセンサ観測誤差分散を設定するセンサ観測誤差分散設定部と、センサと目標との位置関係から決定される観測効果行列変換行列を算出する観測効果行列変換行列算出部と、センサ観測誤差分散と観測効果行列変換行列を用いて、複数のセンサ毎の観測効果を表す観測効果行列を算出する観測効果行列算出部と、複数のセンサの観測効果行列の和を算出する観測効果行列和算出部と、複数地点の観測効果行列和の演算結果を示す総合観測効果行列和を算出し、設定された総合観測効果行列和しきい値よりも大きい場合にセンサの配置場所を変更する制御信号を出力する総合観測効果行列和評価部を備えたので、総合観測効果行列和による判定により、測位誤差指標や総合測位誤差指標を算出せずとも、センサの配置場所を変更することが可能であり、演算量の低減を図ることができる。
【００７０】
実施の形態８．
図１８はこの発明の実施の形態８の測位精度評価装置を示す構成図である。図示の測位精度評価装置は、センサ観測誤差分散設定部１、観測効果行列算出部２、観測効果行列和算出部３、測位誤差指標算出部４、総合測位誤差指標算出部５、総合測位誤差指標評価部６、センサ位置設定部７、観測効果行列変換行列算出部８、観測効果行列蓄積部９、測位誤差初期値設定部１０、レイアウト設定部１１、格子点設定部１２、計算領域設定部１３、目標位置設定部１４、指定目標設定部２０を備えている。ここで、指定目標設定部２０以外の構成及び動作は図１に示した実施の形態１と同様であるため、対応する部分に同一符号を付してその説明を省略する。
【００７１】
指定目標設定部２０について、図１９及び図２０を用いて説明する。
総合測位誤差指標算出部５では、算出した複数の総合測位誤差指標で同じ値となり、かつ、これらの値が総合測位誤差指標しきい値より大きい場合が考えられる。例えば、図１９のように、指定目標に対し、等距離のセンサ配置となるセンサ配置候補１、センサ配置候補２、センサ配置候補３、センサ配置候補４である。そこで、指定目標設定部２０では、総合測位誤差指標算出部５において、総合測位誤差指標が同じ値を取る場合、ユーザが事前に設定したセンサ配置の格子点を指定する。例えば、ユーザが事前に設定したセンサ配置の格子点指定の例としては、指定目標の左下のものを選択するといったことが考えられる。この例の場合、センサ配置候補１である。図２０の例の場合、２次元平面上の左下のセンサ配置候補を選択するといった例であるが、３次元平面にも拡張でき、また、等距離や等角度にある配置場所を事前に決めた規則に基づき１つ選択や、上下左右のいずれかの選択も考えられる。
そして、指定目標設定部２０は、ユーザが事前に設定したセンサ配置の格子点以外の、総合測位誤差指標が同じ値を取るセンサ配置場所を変更しない制御信号を総合測位誤差指標評価部６とセンサ位置設定部７に対して出力する。これにより、総合測位誤差指標評価部６は、センサ位置設定部７に対してセンサ位置をユーザが自然に設定したセンサ位置とする制御信号を出力し、センサ位置設定部７は、このセンサ位置を設定する。
【００７２】
なお、複数の総合測位誤差指標が完全一致するのではなく、ある程度の幅を持った設定範囲内の同一値である場合に事前に設定されたセンサ位置を選択するようにしてもよい。
【００７３】
以上説明したように、実施の形態８の測位精度評価装置によれば、総合測位誤差指標算出部で算出される複数の総合測位誤差指標が、総合測位誤差指標しきい値より大きく、かつ、設定範囲内の同一値であった場合、事前に設定されたセンサ位置を選択しセンサ位置を設定させる指定目標設定部を備えたので、総合測位誤差指標の値が同じセンサ配置候補の格子点を一意に決めることが可能となり、センサ配置変更を無限に行うことを防ぐことができる。
【００７４】
なお、本願発明はその発明の範囲内において、各実施の形態の自由な組み合わせ、あるいは各実施の形態の任意の構成要素の変形、もしくは各実施の形態において任意の構成要素の省略が可能である。
【符号の説明】
【００７５】
１  センサ観測誤差分散設定部、２  観測効果行列算出部、３  観測効果行列和算出部、４  測位誤差指標算出部、５  総合測位誤差指標算出部、６  総合測位誤差指標評価部、７  センサ位置設定部、８  観測効果行列変換行列算出部、９  観測効果行列蓄積部、１０  測位誤差初期値設定部、１１  レイアウト設定部、１２  格子点設定部、１３  計算領域設定部、１４  目標位置設定部、１５  センサ変更設定部、１６  目標経路設定部、１７  予測誤差共分散行列設定部、１８  格子点数設定部、１９  総合観測効果行列和評価部、２０  指定目標設定部、１０１  第２のセンサ観測誤差分散設定部、２０１  観測効果行列算出前行列算出部、３０１  第２の観測効果行列和算出部、９０１  観測効果行列算出前行列蓄積部。
【図１】
2016217974.tif 000003
【図２】
2016217974.tif 000004
【図３】
2016217974.tif 000005
【図４】
2016217974.tif 000006
【図５】
2016217974.tif 000007
【図６】
2016217974.tif 000008
【図７】
2016217974.tif 000009
【図８】
2016217974.tif 000010
【図９】
2016217974.tif 000011
【図１０】
2016217974.tif 000012
【図１１】
2016217974.tif 000013
【図１２】
2016217974.tif 000014
【図１３】
2016217974.tif 000015
【図１４】
2016217974.tif 000016
【図１５】
2016217974.tif 000017
【図１６】
2016217974.tif 000018
【図１７】
2016217974.tif 000019
【図１８】
2016217974.tif 000020
【図１９】
2016217974.tif 000021
【図２０】
2016217974.tif 000022
【図２１】
2016217974.tif 000023