特許 6754518 位置推定システム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】6754518

(24)【登録日】2020年8月26日

(45)【発行日】2020年9月16日

(54)【発明の名称】位置推定システム

(51)【国際特許分類】

   G01S 5/16 20060101AFI20200907BHJP

   G01S 3/783 20060101ALI20200907BHJP

   G05D 1/10 20060101ALI20200907BHJP

【ＦＩ】

   G01S5/16

   G01S3/783

   G05D1/10

【請求項の数】6

【全頁数】18

(21)【出願番号】特願2015-239674(P2015-239674)

(22)【出願日】2015年12月8日

(65)【公開番号】特開2016-109698(P2016-109698A)

(43)【公開日】2016年6月20日

【審査請求日】2018年11月7日

(31)【優先権主張番号】特願2014-248336(P2014-248336)

(32)【優先日】2014年12月8日

(33)【優先権主張国】JP

(73)【特許権者】

【識別番号】504171134

【氏名又は名称】国立大学法人 筑波大学

(74)【代理人】

【識別番号】100106909

【弁理士】

【氏名又は名称】棚井 澄雄

(74)【代理人】

【識別番号】100188558

【弁理士】

【氏名又は名称】飯田 雅人

(72)【発明者】

【氏名】延原 肇

(72)【発明者】

【氏名】鋤先 星汰

【審査官】 田中 純

(56)【参考文献】

【文献】 特表２００４−５３６７１５（ＪＰ，Ａ）

【文献】 特開昭６１−０９７７１５（ＪＰ，Ａ）

【文献】 特開２０１０−０６６０７２（ＪＰ，Ａ）

【文献】 米国特許出願公開第２０１４／０１６６８５４（ＵＳ，Ａ１）

【文献】 米国特許出願公開第２０１３／０１４１２８３（ＵＳ，Ａ１）

【文献】 特開２００５−１６７５１７（ＪＰ，Ａ）

【文献】 特開平０９−１７１５９９（ＪＰ，Ａ）

【文献】 特開昭６２−１６５１７１（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｇ０１Ｓ ３／００ − Ｇ０１Ｓ ３／７８９

Ｇ０１Ｓ ５／００ − Ｇ０１Ｓ ５／１６

Ｇ０５Ｄ １／１２

Ｇ０１Ｊ １／４２

(57)【特許請求の範囲】

【請求項1】

光を発する発光部と、
第１の方向を指向する第１の受光素子と、
前記第１の方向とは異なる第２の方向を指向する第２の受光素子と、
前記第１の方向および前記第２の方向とは異なる第３の方向を指向する第３の受光素子とを含み、前記第１の受光素子、前記第２の受光素子、および前記第３の受光素子が互いに離間しないように設けられた受光部と、
前記第１の受光素子、前記第２の受光素子および前記第３の受光素子の受光結果を用いて前記受光部に対する前記発光部の位置を推定する位置推定部と、を備え、
前記位置推定部は、
前記第１の受光素子、前記第２の受光素子および前記第３の受光素子のそれぞれに対して幾何領域を設定し、
前記設定した幾何領域と、前記設定した幾何領域に対して設定された幾何学的な拘束条件と、前記受光結果である光の強さを示す情報に基づいて得られた前記発光部が発光した光の減衰度と、に基づいて前記受光部に対する前記発光部の位置を推定し、
前記幾何領域は、受光素子との距離が遠くなるのに応じて前記受光素子の指向方向を中心とした半径が小さくなり、前記半径の変化率は前記距離が遠くなるのに従って小さくなる三次元曲面であり、且つ前記発光部の存在する可能性がある領域である、
位置推定システム。

【請求項2】

前記第１の受光素子が指向する前記第１の方向と、前記第２の受光素子が指向する前記第２の方向と、前記第３の受光素子が指向する前記第３の方向とは、互いに直交し、且つ前記発光部から、前記第１の受光素子までの距離と、前記第２の受光素子までの距離と、前記第３の受光素子までの距離とが同じとみなせるように配置されている、
請求項１に記載の位置推定システム。

【請求項3】

前記発光部が発光する光は、振幅変調光である、
請求項１または２に記載の位置推定システム。

【請求項4】

前記受光部および前記位置推定部は、飛行体に搭載され、
前記発光部は固定局である、
請求項１から３のうちいずれか１項に記載の位置推定システム。

【請求項5】

前記発光部は、第１の飛行体に搭載され、
前記受光素子および前記位置推定部は、第２の飛行体に搭載され、
前記第２の飛行体は、前記第１の飛行体に追従して飛行する、
請求項１から３のうちいずれか１項に記載の位置推定システム。

【請求項6】

前記第１の飛行体は、
更に、自己位置を認識する設備と、前記自己位置と予め設定された飛行ルートとに基づいて、自機の飛行を制御する飛行制御部と、前記自機が飛行している際に、前記発光部に光を放射させる発光制御部と、を備え、
前記第２の飛行体は、
更に、自己位置を認識する設備の認識結果に依らずに、前記位置推定部の推定部に基づいて、前記第１の飛行体に追従させるように自機を制御する飛行制御部と、
を備える、
請求項５に記載の位置推定システム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、位置推定システムに関する。
本願は、２０１４年１２月８日に、日本に出願された特願２０１４−２４８３３６号に基づき優先権を主張し、その内容をここに援用する。

【背景技術】

【0002】

対象物体の位置を推定するために、モーションセンサやＧＰＳ（Global Positioning System）等を含んだ位置推定装置が用いられている。この位置推定装置は、例えば、ＵＡＶ（Unmanned Aerial Vehicle）などの移動体の制御に用いられている。

【先行技術文献】

【非特許文献】

【0003】

【非特許文献1】阿部剛、水島晃、野口伸、レーザスキャナを用いた農用車両の直進追従制御に関する研究、農業機械学会誌、２００５年、６７（３）、６５−７１

【非特許文献2】神原誠之、横矢直和、複数の赤外線受光体を利用した位置姿勢センサの試作 電子情報通信学会総合大会講演論文集 ２００４年

【非特許文献3】土方俊介、梅田和昇、室内における赤外ＬＥＤを用いた移動ロボットの位置・姿勢推定手法、第１１回 ロボティクスシンポジア予稿集、２００６．３ ｐｐ．１４１−１４６

【非特許文献4】岩倉ほか、赤外線距離センサを搭載した飛行ロボットの屋内自己位置推定（ＯＳ３ 自律知能無人ビークルの運動と制御、「振動と運動の制御」シンポジウム講演論文集 ２０１１（１２）、２０１１−０６−２８、２５３−２５８

【非特許文献5】藤永仁、得竹浩、砂田茂 小型無人航空機の誘導制御と自律飛行試験、日本航空宇宙学会論文集、２００８、Ｖｏｌ．５６、Ｎｏ．６４９、ｐｐ．５７−６４

【非特許文献6】安達宏幸、中山栄純、菅生誠、水澤純一、ＫＩＮＥＣＴを用いた大関節三次元リアルタイム測定実験の報告、電子情報通信学会技術研究報告＝ＩＥＩＣＥ ｔｅｃｈｎｉｃａｌ ｒｅｐｏｒｔ：信学技報 ２０１４−０７−２６、１１４（１５３）、２５−２９

【発明の概要】

【発明が解決しようとする課題】

【0004】

従来の位置推定装置の推定精度は十分でない場合があった。
本発明は、上記事情を考慮してなされたものであり、より高精度に位置を推定することができる位置推定システムを提供することを目的とする。

【課題を解決するための手段】

【0005】

本発明は、光を発する発光部と、第１の方向を指向する第１の受光素子と、前記第１の方向とは異なる第２の方向を指向する第２の受光素子と、前記第１の方向および前記第２の方向とは異なる第３の方向を指向する第３の受光素子とを含み、前記第１の受光素子、前記第２の受光素子、および前記第３の受光素子が互いに離間しないように設けられた受光部と、前記第１の受光素子、前記第２の受光素子および前記第３の受光素子の受光結果を用いて前記受光部に対する前記発光部の位置を推定する位置推定部と、を備え、前記位置推定部は、前記第１の受光素子、前記第２の受光素子および前記第３の受光素子のそれぞれに対して幾何領域を設定し、前記設定した幾何領域と、前記設定した幾何領域に対して設定された幾何学的な拘束条件と、前記受光結果である光の強さを示す情報に基づいて得られた前記発光部が発光した光の減衰度と、に基づいて前記受光部に対する前記発光部の位置を推定し、前記幾何領域は、前記受光素子との距離が遠くなるのに応じて前記受光素子の指向方向を中心とした半径が小さくなり、前記半径の変化率は前記距離が遠くなるのに従って小さくなる三次元曲面であり、且つ前記発光部の存在する可能性がある領域である位置推定システムである。

【発明の効果】

【0006】

本発明は、より高精度に位置を推定することができる位置推定システムを提供することである。

【図面の簡単な説明】

【0007】

【図1】第１の実施形態の位置推定システム１の概念図である。

【図2】位置推定システム１の機能構成を示す図である。

【図3】位置推定装置５０により実行される処理の流れを示すフローチャートである。

【図4】幾何領域の概念図である。

【図5】位置推定マップ６０の一例を示す図である。

【図6】位置推定システム１Ａを搭載したＵＡＶ群（飛行体システム）の構成概要図である。

【図7】第２の実施形態の位置推定システム１Ａの目標飛行体１００の機能構成を示す図である。

【図8】第３の実施形態の幾何領域の概念図である。

【図9】第４の実施形態の幾何領域の概念図である。

【図10】位置推定システムが位置を推定する場合における手法の概要図である。

【図11】出力信号と信号伝搬の概念図である。

【図12】自己位置発信装置の回路図である。

【図13】マイコンを用いて発信信号１セットごとに送受信機間の距離変化による赤外線信号の受信時間を計測した結果を示す図である。

【図14】受信機の回路図である。

【図15】実験１−２の結果を示す図である。

【図16】送受信機間の距離と受光カウント数の相関を示す図である。

【図17】サンプリング回数の違いによる推定距離と真値の差を示す図である。

【図18】角度変化による受信時間を計測した結果を示す図である。

【図19】各距離におけるカウント回数と角度の相関関係のグラフを示す図である。

【図20】センサ配置の概念図を示す。

【図21】カウント回数と距離・角度との関係を示す図。

【発明を実施するための形態】

【0008】

以下、図面を参照し、本発明の位置推定システムの実施形態について説明する。

【0009】

＜第１の実施形態＞
図１は、第１の実施形態の位置推定システム１の概念図である。位置推定システム１は、発光装置１０と、位置推定装置５０を搭載した飛行体２０とを含んでもよいが、これに限定されない。位置推定装置５０は、発光装置１０から発光された光を互いに独立した方向を指向する複数の受光素子によって受光し、複数の受光素子によって受光された光の強さを示す情報に基づいて、発光装置１０の位置を導出する。

【0010】

図２は、位置推定システム１の機能構成を示す図である。発光装置１０は、発光部１２−１から１２−ｎ、および発光制御部１４が搭載される。以下、発光部１２−１から１２−ｎを、区別しない場合は、単に発光部１２という。

【0011】

発光部１２は、例えば固定局または移動局として設けられる。発光部１２は、指向性の低い光を発光する。発光部１２−１から１２−ｎは、それぞれ異なる方向に向けて光を発光する。発光部１２は、例えば時間ごとに振幅変調された赤外光である。

【0012】

例えば、発光部１２は、発光部１２の光源を覆うカバー部を有する。カバー部は、発光部１２の光源から発光された光を散乱させることで、発光された光の指向性を低下させる。なお、振幅変調に代えて、周波数変調や、パルス変調など、その他の変調方式を用いてもよい。発光制御部１４は、自装置の記憶領域に記憶された発光部１２の発光パターンを示す情報を参照し、発光部１２に所定の光を発光させる。

【0013】

飛行体２０は、蓄電池２２と、アンテナ２４と、通信制御部２６と、センサ２８と、ロータ３０−１から３０‐４と、ロータ駆動部３２と、飛行制御部３４と、位置推定装置５０とを含んでもよいが、これに限定されない。

【0014】

蓄電池２２は、例えばリチウムポリマー電池や、リチウムイオン電池である。蓄電池２２は、飛行体２０に着脱可能に搭載される。蓄電池２２は、飛行体２０の各部に電力を供給する。

【0015】

アンテナ２４は、コントローラ（不図示）の送信部から送信された電波を受信する。コントローラとは、ユーザが飛行体を遠隔操作するための装置である。コントローラの送信部から送信された電波には、コントローラに対して入力された操作量が含まれる。通信制御部２６は、コントローラと、飛行体２０との無線通信を制御する。

【0016】

センサ２８は、例えば高度センサや、測距センサ、ジャイロセンサ等である。高度センサは、飛行体２０の機体の鉛直下方にレーザを投光し、投光したレーザの反射光を受光する。高度センサは、例えば受光した光の位相の遅れに基づいて、飛行体２０の高度を算出する。測距センサは、飛行体２０の機体の水平方向にレーザを投光し、投光したレーザの反射光を受光する。測距センサは、例えば受光した光の位相の遅れに基づいて、飛行体２０の周辺に存在する物体を検出する。ジャイロセンサは、飛行体２０の機体の姿勢を検出する。

【0017】

ロータ３０−１から３０‐４は回転体（回転翼）である。以下、ロータ３０−１から３０−４を、区別しない場合は単にロータ３０と称する。ロータ駆動部３２は、各ロータ３０を駆動させる。なお、本実施形態では、一例としてロータ３０−１から３０−４を備えるものとして説明するが、これに限られない。例えば、ロータは、１つであってもよい。

【0018】

飛行制御部３４は、位置推定装置５０の処理結果や、センサ２８により算出された高度、検出された機体の周辺状況、機体の姿勢等を統合して、ロータ駆動部３２を制御する。飛行制御部３４は、例えばロータ駆動部３２を制御して、各ロータ３０の回転数を増減させる。これにより、飛行制御部３４は、飛行体２０を上昇、下降、方向転換、前進等させる。飛行制御部３４は、例えば予め設定された飛行ルートに基づいて自機を制御する。

【0019】

位置推定装置５０は、記憶部５２と、受光部５６（受光素子５７−１から５７−３）と、位置推定部５８を備える。記憶部５２は、例えば、ＲＡＭ（Random Access Memory）やＲＯＭ（Read Only Memory）、ＨＤＤ（Hard Disk Drive）、フラッシュメモリ等の記憶装置により実現される。位置推定部５８は、ＣＰＵ（Central Processing Unit）などのプロセッサがプログラムを実行することで実現される。また、これらの位置推定部５８は、ＬＳＩ（Large Scale Integration）やＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）などのハードウェアで実現されてもよい。

【0020】

記憶部５２には、受光素子５７−１から５７−３が取り付けられた位置、受光素子５７−１から５７−３が光を受光する受光方向の中心軸の向き、発光部１２が発光する光に関する情報（振幅等）が記憶される。また、記憶部５２には、予め設定された飛行ルートや、飛行計画が記憶されている。

【0021】

受光素子５７−１から５７−３は、例えばフォトダイオードである。受光素子５７−１から５７−３は、それぞれ異なる方向に受光面が正対するように飛行体２０に配置される。受光素子５７−１から５７−３は、例えば機体の下側の側面（地上と対向する面）に配置される。また、受光素子５７−１から５７−３は、例えば、ほぼ１箇所に集められて設けられる。なお、受光素子５７−１から５７−３は、互いに異なる箇所に離間して設けられてもよい。以下、受光部５６の受光素子５７−１から５７−３を、区別しない場合は単に受光素子５７と称する。

【0022】

位置推定部５８は、各受光素子５７により受光された光に含まれる光の強さを示す情報に基づいて、発光部１２の存在する可能性がある幾何領域を設定し（限定し）、設定した幾何領域の交わりに基づいて発光部１２の位置を推定する。位置推定部５８の処理の詳細については後述する。

【0023】

図３は、位置推定装置５０により実行される処理の流れを示すフローチャートである。本処理は、繰り返し実行される処理である。まず、位置推定部５８が、受光部５６の各受信素子５７により受光された光に含まれる光の強さを示す情報（受光結果）を取得する（ステップＳ１００）。

【0024】

次に、位置推定部５８は、ステップＳ１００で取得した受光結果に基づいて、幾何領域を設定する（ステップＳ１０２）。次に、位置推定部５８は、ステップＳ１０２で設定した幾何領域の交わり位置を導出する（ステップＳ１０４）。幾何領域の交わり位置は、発光部１２の位置である。

【0025】

図４は、幾何領域の概念図である。幾何領域は、曲面、平面、空間領域、曲線、直線などを含む。図中、Ｘ軸は受光素子５７の指向方向と一致している。指向方向とは、受光素子５７の受光感度が高い部分の中心軸方向である。位置推定部５８は、受光素子５７の受光結果に基づいて、幾何領域を設定する。位置推定部５８は、受光結果（図中、出力Ａ＞Ｂ＞Ｃ）に対応する受光素子５７からの距離と角度との関係（幾何領域）を複数導出する。受光結果とは、受光素子５７の出力結果である。この三次元曲面の幾何領域は、距離が遠くなるのに応じて受光素子５７に正対する方向を軸とした半径が小さくなり、半径の変化率は距離が遠くなるのに従って小さくなる曲面（円錐を曲成したような曲面）である。なぜなら、光の強さは、受光素子５７と発光部１２との距離が遠くなる程弱くなり、受光素子５７と発光部１２との入射角度が正対方向から離れている程弱くなるからである。この傾向を反映させて、発光部１２が存在する位置の集合を求めたものが幾何領域である。

【0026】

また、幾何領域の交わり位置は、位置推定部５８により３つの受光素子５７のそれぞれに対応して設定された３つの幾何領域の交わりである。位置推定部５８は、導出された幾何領域の交わり位置、および受光素子５７の設置条件に基づいて、発光部１２の位置を導出する。位置の導出手法の詳細は、後述する「実施例」で説明する。

【0027】

次に、位置推定部５８は、発光部１２の位置に対する自機の位置を推定する（ステップＳ１０６）。次に、飛行制御部３４が、位置推定部５８により推定された自機の位置と、予め設定された飛行ルートに基づいて、自機を制御する（ステップＳ１０８）。これにより本フローチャートの１ルーチンの処理は終了し、ステップＳ１００の処理に戻る。

【0028】

例えば、発光部１２が飛行体２０の着陸位置付近に設けられている場合、飛行体２０は、位置推定部５８の推定結果に基づいて自機を制御することにより、より精度よく自機の着陸目標位置を特定することができる。この結果、飛行体２０は、より精度よく自機を着陸目標位置に着陸させることができる。

【0029】

なお、本フローチャートのステップＳ１０２およびステップＳ１０４の処理に代えて、位置推定部５８は、位置推定マップに基づいて、自機に対する発光部１２の位置を導出してもよい。この場合、記憶部５２には、位置推定マップが格納される。図５は、位置推定マップ６０の一例を示す図である。位置推定マップ６０は、３つの受光素子５７の受光結果に、自機に対する発光部１２の位置が対応付けられたマップである。位置推定マップ６０は、幾何領域の重なりに基づいて位置を推定する原理に基づいて作成されたものである。位置推定マップ６０は、例えば上述した原理を用いた実験またはシミュレーションによって求められた結果に基づいて設定されたものである。

【0030】

図中、α１からα３は、受光素子５７−１から５７−３のそれぞれに対応する出力結果である。位置推定部５８は、位置推定マップ６０を参照し、３つの受光素子５７の受光結果の交点に基づいて、自機に対する発光部１２の位置（図中、Ｐ１またはＰ２）を導出する。図中、位置Ｐ１およびＰ２は、例えば、三次元座標により定義される。また、位置推定部５８は、四則演算、近似式（多項式近似、直線近似、折れ線近似）を用いて発光部１２の位置を導出してもよい。

【0031】

以上説明した第１の実施形態の位置推定システム１は、光を発する発光部１２により発光された光を受光する互いに独立した方向に向く複数の受光素子５７の受光結果に基づき、幾何領域の交わり位置を発光部１２の位置と推定することにより、より高精度に発光部１２の位置を推定することができる。この結果、飛行体２０は、位置推定部５８により導出された発光部１２の位置に基づいて、より精度よく自機を制御することができる。

【0032】

（第２の実施形態）
以下、第２の実施形態について説明する。ここでは、第１の実施形態との相違点を中心に説明し、第１の実施形態と共通する機能等についての説明は省略する。第２の実施形態では、発光部１２は、飛行体２０が追従する目標飛行体１００に設けられる。

【0033】

図６は、位置推定システム１Ａを搭載したＵＡＶ群（飛行体システム）の構成概要図である。本実施形態において取り扱うＵＡＶ群に関しては、まず目標飛行体（親機）１００と飛行体（子機）２０に役割を分担させる。親機には事前情報を与えるなどにより絶対的な自己位置を認識できるような設備を搭載する。そのような設備は高価で重いため子機には搭載せず、親機を追従させる。親機に発光装置１０を搭載し、これを子機に搭載した受光素子５７によって発光装置１０から発光された光を検出し追従するような形で群体制御を実現する。

【0034】

第２の実施形態の位置推定システム１Ａは、飛行体２０と、目標飛行体１００とを備える。図７は、第２の実施形態の位置推定システム１Ａの目標飛行体１００の機能構成を示す図である。

【0035】

目標飛行体１００が備える発光部１２−１から１２−ｎと、発光制御部１４とは、発光装置１０が備える発光部１２−１から１２−ｎと、発光制御部１４と同様の機能構成であるため説明を省略する。また、目標飛行体１００が備える蓄電池２２と、アンテナ２４と、通信制御部２６と、センサ２８と、ロータ３０−１から３０−４と、ロータ駆動部３２と、記憶部５２とは、飛行体２０が備える蓄電池２２と、・・・記憶部５２と同様の機能構成であるため説明を省略する。

【0036】

目標飛行体１００の飛行制御部３４は、センサ２８の検出結果や、記憶部５２に記憶された飛行ルートに基づいて、ロータ３０を制御する。これにより、目標飛行体１００は、目標地点に向けて、または飛行ルートに従って飛行する。また、目標飛行体１００は、コントローラから発信された電波に基づいて、制御されてもよい。

【0037】

第２の実施形態の飛行体２０の動作について説明する。まず、飛行体２０の受光素子５７が、目標飛行体１００の発光部１２から発光された光を受光する。飛行体２０の位置推定部５８が、位置推定マップ６０を参照し、受信素子５７の受光結果に基づいて、目標飛行体１００の位置を導出する。位置推定部５８が、目標飛行体１００に対する自機の位置を推定する。飛行制御部３４が、位置推定部５８により導出された目標飛行体１００の位置と、自機の位置とに基づいて、目標飛行体１００に追従するように自機を制御する。

【0038】

なお、目標飛行体１００には、位置推定装置５０が搭載されてもよい。この場合、目標飛行体１００の位置推定装置５０は、例えば地上等に設けられた発光装置１０の発光部１２から発光される光の受光結果に基づいて、発光部１２の位置を導出する。目標飛行体１００の位置推定装置５０は、導出した発光部１２の位置に基づいて、自機を制御する。更に目標飛行体１００に追従する飛行体２０は、目標飛行体１００の発光部１２から発光される光の受光結果に基づいて、目標飛行体１００の位置を導出する。飛行体２０の位置推定装置５０は、導出した目標飛行体１００の位置に基づいて、自機を制御する。この結果、より精度よく飛行体２０および目標飛行体１００の群体制御を行うことができる。

【0039】

以上説明した第２の実施形態によれば、位置推定システム１Ａは、目標飛行体１００の発光部１２により発光された光の受光結果に基づいて、目標飛行体１００の位置をより高精度に推定することができる。この結果、飛行体２０は、位置推定部５８により導出された目標飛行体１００および自機の位置に基づいて、より精度よく目標飛行体１００を追従することができる。

【0040】

（第３の実施形態）
以下、第３の実施形態について説明する。ここでは、第１の実施形態との相違点を中心に説明し、第１の実施形態と共通する機能等についての説明は省略する。第１の実施形態では、三次元空間における発光部１２の位置を導出したが、第２の実施形態では、二次元空間における発光部１２の位置を導出する。位置推定装置５０は、例えば自動車における駐車制御や、自走式掃除機などに適用可能である。

【0041】

第３の実施形態の位置推定装置５０は、２つの受光素子５７の受光結果に基づいて、二次元曲線の交点に基づいて、発光部１２の二次元の位置を導出する。図８は、第３の実施形態の幾何領域の概念図である。図中、Ｘ軸は受光素子５７−１の指向方向と一致している。図中、Ｙ軸は受光素子５７−２の指向方向と一致している。指向方向とは、受光素子５７の受光感度が高い部分の中心軸方向である。位置推定部５８は、受光素子５７の受光結果に基づいて、二次元曲線の幾何領域を設定する。位置推定部５８は、受光結果（図中、出力Ａ＞Ｂ＞Ｃ＞Ｄ）に対応する自機からの距離と角度との関係を複数導出する。位置推定部５８は、設定した２つの幾何領域の交わりを導出し、導出した交わり位置を発光部１２の位置として導出する。例えば、受光素子５７−１の受光結果が出力Ｄであり、受光素子５７−２の受光結果が出力Ｂである場合、位置推定部５８は、出力Ｄと出力Ｂの交わる位置に発光部１２が存在すると推定する。

【0042】

第３の実施形態の位置推定装置５０は、自走型の移動体に搭載されてもよい。自動車における駐車制御に適用される場合、自動車の駐車目標位置付近に発光部１２が設けられる。位置推定装置５０が搭載された自動車は、発光部１２から発光された光の受光結果に基づいて自車の位置を推定する。自動車は、自車の位置の推定結果に基づいて自車を精度よく駐車することができる。

【0043】

例えば位置推定装置５０は、床面に散乱するごみ等を吸引する自走型床掃除装置に搭載されてもよい。例えば自走型床掃除装置は、複数の走行モータと、複数の車輪と、ごみ吸引部と、走行制御部とを含む。車輪は、走行モータにより出力された動力により回転する。これにより自走型床掃除装置は、所定の方向に向けて走行する。走行制御部は、位置推定装置５０により導出された発光部１２の位置に基づいて、走行モータを制御する。この結果、自走型床掃除装置は、位置推定装置５０の位置の推定結果に基づいて、より精度よく自装置を制御することができる。

【0044】

以上説明した第３の実施形態によれば、位置推定装置５０は、発光部１２により発光された光の受光結果に基づいて、発光部１２の二次元位置を、より高精度に位置を推定することができる。

【0045】

（第４の実施形態）
以下、第４の実施形態について説明する。ここでは、第１から第３の実施形態との相違点を中心に説明し、第１から第３の実施形態と共通する機能等についての説明は省略する。第１から第３の実施形態では、受光素子５７−１から５７−３は、ほぼ１箇所に集められて設けられているものとした。これに対して、第４の実施形態では、互いに異なる箇所に離間して設けられ、位置推定部５８は、複数の受光素子５７により受光された光の強さを示す情報に基づいて、発光部１２の位置を推定する。なお、本実施形態では、位置推定部５８は、平面上における発光部１２の位置を推定する。

【0046】

図９は、第４の実施形態の幾何領域の概念図である。第４の実施形態では、受光素子５７−１および５７−２は互いに異なる箇所に離間して設けられる。図示する例では、受光素子５７−１の指向方向（Ｙ軸方向）と受光素子５７−２の指向方向（Ｙ軸方向）とは一致している。更に受光素子５７−１と受光素子５７−２との距離Ｌａｂは、記憶部５２に予め記憶されている。位置推定部５８は、受光結果に対応する幾何領域を、受光素子５７ごとに導出する。図中、出力Ａ（５７−１）から出力Ｃ（５７−１）は、受光素子５７−１の受光結果に基づく幾何領域、すなわち発光部１２が存在する可能性がある領域を示し、出力Ａ（５７−２）から出力Ｅ（５７−２）は、受光素子５７−２の受光結果に基づく幾何領域を示している。

【0047】

位置推定部５８は、設定した２つの曲線（幾何領域）の交わりを導出する。そして、位置推定部５８は、導出した曲線の交わりに基づいて、発光部１２の位置を導出する。例えば、受光素子５７−１の受光結果が出力Ｂ（５７−１）であり、受光素子５７−２の受光結果が出力Ｄ（５７−２）である場合、位置推定部５８は、出力Ｂ（５７−１）に対応する幾何領域と出力Ｄ（５７−２）に対応する幾何領域との交わる位置と、受光素子５７間の距離Ｌａｂとに基づいて、受光素子５７に対する発光部１２が存在する位置（例えば角度αおよび距離Ｌａと、角度βおよび距離Ｌｂ）を推定することができる。

【0048】

なお、位置推定部５８は、位置推定マップに基づいて、自機に対する発光部１２の位置を導出してもよい。位置推定マップ６０は、受光素子５７の受光結果に、自機に対する発光部１２の位置が対応付けられたマップである。位置推定マップ６０は、幾何領域の重なりに基づいて位置を推定する原理および受光素子５７間の距離に基づいて作成されたものである。また、本実施形態では、位置推定部５８は、２つの受光素子５７の受光結果に基づいて、発光部１２の二次元位置を推定するものとしてが、これに限られない。例えば、位置推定部５８は、それぞれが互いに離間した３つの受光素子５７の受光結果に基づいて、発光部１２の三次元位置を推定してもよい。また、複数の受光素子５７は、互いに異なる箇所に離間して設けられ、互いに独立した方向を指向してもよいし、同一の方向を指向してもよい。

【0049】

以上説明した第４の実施形態によれば、複数の受光素子５７は互いに異なる箇所に離間して設けられ、位置推定装置５０は、複数の受光素子５７により受光された光の強さを示す情報に基づいて、発光部１２の存在する可能性がある幾何領域を複数の受光素子５７のそれぞれに対応させて設定し、設定した幾何領域の交わり位置を発光部１２の位置と推定する。この結果、より高精度に発光部１２の位置を推定することができる。

【0050】

また、第１から第４の実施形態の位置推定システム１または１Ａは、両眼視差映像を液晶ディスプレイに表示する三次元表示手法に適用してもよい。三次元映像をユーザ（看者）に提供する場合、ユーザの頭部の位置を推定する必要がある。従って、位置推定システム１の発光部１２をユーザの頭部における任意の位置等に配置し、受光部５６を発光部１２が発する光を受光可能な任意の位置に配置する。これにより、位置推定システム１または１Ａは、ユーザの頭部の位置（視点）を推定し、推定した頭部の位置に対応した両眼視差映像を液晶ディスプレイに表示させることができる。なお、受光部５６をユーザの頭部における任意の位置等に配置し、発光部１２は受光部５６に光を届けることができる任意の位置に配置してもよい。なお、ユーザの頭部の位置がある程度、一律である場合、位置推定システム１または１Ａは、ユーザの頭部の位置を２次元で推定してもよい。

【0051】

（実施例）
出願人は、以下の実験を行い位置推定システムにより、振幅変調光による自己位置発信とその３次元位置の推定について確認した。
本実験では、提案手法を実機に実装する前の予備実験として、２つの予備実験を示す。１つめの予備実験は、発光素子と受光素子をそれぞれ１つずつ利用した、距離推定のための予備実験である。２つめの予備実験は、１つの発光素子と３つの受光素子を用いた、３次元位置推定のための予備実験である。

【0052】

図１０は、位置推定システムが位置を推定する場合における手法の概要図である。位置推定システムは、例えば発信装置、受信装置および周辺回路を組み合わせたシステムである。位置推定システムは、無段階に赤外光の輝度を変化させたものを、３８［ｋＨｚ］のパルスによるキャリア波を用いて変調し、その減衰の様子から距離を推定することにより、数ミリ秒〜数十ミリ秒の短時間で外光等の環境による影響の少ない高精度の自己位置発信を行うことができる。また、３次元位置推定においては、あらかじめ発信機側センサの赤外光の受信強度の分布の対応を取っておくことや位置推定を行う際の判定時間を調整することで精度の向上を図ることができる。また、今回、位置推定システムで行う３次元位置推定実験においては３個の受光素子を用いているが、個数を増加させることで推定範囲及び精度を向上させることができる。

【0053】

ここで、自己位置発信装置として中心波長９４０［ｎｍ］の赤外線を用い、距離に相関してエネルギーの減衰した光をＩＲセンサで検出することで自己位置発信源までの距離を推定する。さらにＩＲセンサを任意の方向に多数配置することで自己位置発信源までの角度も推定可能になるため３次元位置推定を行うことができる。

【0054】

図１１は、出力信号と信号伝搬の概念図である。自己位置発信装置は、ＡＭ変調された単調減少の信号を３８［ｋＨｚ］のキャリア波を用いて変調し、（図１０及び図１１）に示す信号を赤外線発光する。キャリア波を用いることで、出力を大きくすることができるため、機体間の距離を長くすることが可能となる。また、周波数フィルターを用いて外乱の影響を軽減する。

【0055】

受信装置はキャリア波に合わせたフィルター回路を搭載したＩＲセンサ（デジタル出力の赤外線リモコン受光モジュール：ＰＬ−ＩＲＭ１２６１−Ｃ４３８）を使用する。赤外線信号が空間を伝搬する際、空間中でそのエネルギーが減衰する。そのため図１１に示すように、距離に応じて受信機が信号を受信する時間が変化する。

【0056】

この時間と発信−受信機間の距離の相関関係を用いて距離を推定することができる。さらに、任意の方向に多数センサを配置し、３次元的空間の距離及び角度と、各センサの反応する時間を統合的に処理し相関関係を解析することで３次元空間上の位置推定が可能になる。

【0057】

（距離推定のための予備実験）
発光素子と受光素子をそれぞれ１つずつ利用した予備実験に関しては、４段階に分けて性能向上を行った。それぞれの過程を、以下に説明する。図１２は、自己位置発信装置の回路図である。ｓｉｇ１を一定時間ＯＮにしてＶｃｃの電圧でコンデンサを充電する。ｓｉｇ２を３０［ｋＨｚ］で２００回スイッチングすることでコンデンサのエネルギーを用いて図１１に示す赤外線信号を出力する。Ｖｃｃ、約３０［ｋＨｚ］のｄｕｔｙ比、コンデンサ容量、充電時間、ＬＥＤの電流制限抵抗、放電時間（パルス回数）を変えることで任意の距離分解能と測定周期（充電時間＋発光時間）を得ることができる。本実験では、Ｖｃｃ５［Ｖ］，ｄｕｔｙ１／３，コンデンサ４．７［μＦ］，充電時間５００［μｓ］，抵抗３３０［Ω］，２００パルスで実験を行った。

【0058】

（実験１−１）
図１３は、マイコン（ＡＶＲ：１６［ＭＨｚ］）を用いて発信信号１セットごとに送受信機間の距離変化による赤外線信号の受信時間を計測した結果を示す図である。図１３に示すように、送受信機間の距離が小さくなるに従って赤外線の受信時間が増加していることがわかる。一方、距離が小さくなるに従って、センサ感度が非常に悪くなり出力波形が安定するまで約１秒を要し応答性が悪くなる、といった結果が得られた。また、送信機側から赤外線を７ミリ秒ほど出力しているが受信側では２５０［ｍｍ］の距離でも１ミリ秒ほどしか受信できていないという結果が得られた。この原因は、信号を受信し続けると内部回路で電荷が飽和しセンサが正常に動作しなくなる特性であると考え、この問題を解決するため図１４に示す回路を用いて受信信号１セットごとに受信機（センサ）の電源をグランドに接地することでセンサ内部の電荷のリセットを行った。

【0059】

（実験１−２）
実験１−１の問題を解決するため、図１４に示す回路を用いて受信信号１セットごとに受信機（センサ）の電源をグランドに接地することでセンサ内部の電荷のリセットを行った。図１４は、受信機の回路図である。赤外線信号の受光時間計測直後に３００マイクロ秒電源を落とす。再度電源を供給し、安定するまで１００マイクロ秒待つ。再び赤外線信号の受光時間計測する。以上の流れで計測を行った。実験１−２の結果を図１５に示す。

【0060】

図１４に示す回路により、毎回初期状態で計測できるため応答性は非常に良く約２００マイクロ秒程度の遅延である。しかし、図１５より送受信機間の距離が１５００［ｍｍ］〜５００［ｍｍ］の間で距離の減少に従って受信時間が増加するという結果が得られなかった。原因として、ノイズ等の影響によりセンサへ電源を供給するタイミングが不安定になったと考えられる。

【0061】

（実験１−３）
実験１−２で発生した問題を解決するため、マイコンのメインプログラムのループ回数で赤外線信号受光時間をカウントするプログラムを作成した。送信機側の電源や環境等の影響でカウント回数にばらつきがあるため、５００カウントを１０００回計測した平均回数を結果のグラフを図１６に示す。１０００回の計測に要する時間は約７秒であることから５００カウントは送信信号１セットの約７．２ミリ秒で行われていることが確認できた。測定結果を図１６に示す。

【0062】

図１６は、送受信機間の距離と受光カウント数の相関を示す図である。また、図１１の回路を用いて一定回数ごとに初期化することで応答性に関しては約２００マイクロ秒の遅延という、ＵＡＶに実装し位置推定を行うのに十分な結果を得ることができた。このデータを元に、距離３００［ｍｍ］〜７００［ｍｍ］（式（１））と７００［ｍｍ］〜２５００［ｍｍ］（式（２））に分けてカウント（ｘ）から距離（ｌ）を推定する４次の近似式を作成した。近似式を式（１）および式（２）に示す。

【数1】

【数2】

【0063】

図１７は、サンプリング回数の違いによる推定距離と真値の差を示す図である。サンプリング回数が多いほど誤差が小さくなる。サンプリング回数が１００回（約０．７秒）を超えると誤差は２０［ｍｍ］以内に収束する。しかし、近似式による誤差や環境等の影響によりサンプリング回数を一定以上増やしても誤差は０に収束しなかった。

【0064】

（実験１−４）
図１８は、同様にして、光軸を９０［ｄｅｇ］として０［ｄｅｇ］〜１８０［ｄｅｇ］の角度変化による受信時間を計測した結果を示す図である。図１８より、センサの感度は中心から±１５［ｄｅｇ］では変化しないこと、左右に対称性があることなどの特性がわかった。

【0065】

（３次元位置推定のための予備実験）
上述した「距離推定のための予備実験」と同様の条件でセンサの素子を３個に拡張し、３次元位置推定の予備実験、及び評価実験を行う。

【0066】

（実験２−１）
図１９は、各距離におけるカウント回数と角度の相関関係のグラフを示す図である。送受信機間の距離を５０［ｍｍ］間隔で変えて計測した。グラフの曲線は左上から順に距離３００［ｍｍ］のライン、３５０［ｍｍ］のライン、４００［ｍｍ］のラインを示している。図１９より、センサが受光したカウント回数（ｎ）に対応する距離（ｒ）と角度（θ）のセットが複数推定できる（図４参照）。

【0067】

（３次元位置推定のための評価実験）
３次元空間上に直交座標系ｘ−ｙ−ｚ軸と受信機の光軸が一致するように３つ配置した。図２０は、センサ配置の概念図を示す。センサの中心から発信機までの距離ｒを３５０［ｍｍ］とした。位置推定手法を以下に示す。３つのセンサから取得したカウント回数を図１９のグラフ（近似式）に当てはめ、各距離の場合における推定角度を算出する。例として、距離３００［ｍｍ］の場合のカウント回数と角度の関係の近似式を式（３）に示す。

【数3】

【0068】

近似式のｘにセンサから得られた値（センサＡ＝２８９，Ｂ＝３０２，Ｃ＝２９３）を代入することで推定角度を求める。同様に、他の距離の場合における角度推定の近似式にも代入し、距離３００［ｍｍ］，３５０［ｍｍ］，４００［ｍｍ］の場合の推定角度を求める。結果を図２１に示す。図２１は、カウント回数と距離・角度との関係を示す図である。

【0069】

３つセンサは１点に存在すると見なすと、送信機までの距離はすべてのセンサで同じである。また、３つのセンサの光軸を直交させているので図２０に示すように幾何学的な束縛条件が存在する。図２０で使用している文字を用いて式（４）を示す。

【数4】

【0070】

式（４）より、発信機の光軸と３つのそれぞれのセンサのなす角度の余弦の平方和が１なる。図２１より、各距離における推定角度を代入すると、３００［ｍｍ］における角度の余弦の平方和は０．８３９６、３５０［ｍｍ］では０．９５７４、４００［ｍｍ］では数値１．０６４となり、３５０［ｍｍ］の場合が最も１に近いので解となる。また、その時の角度が推定される送信機の光軸と各センサの光軸とのなす推定角度である。

【0071】

このアルゴリズムにより３次元位置推定を行ったところ、約０．２秒のサンプリング時間により、送受信機関の距離精度±２５［ｍｍ］、各センサの光軸に対する角度精度±３°以内の３次元の位置推定のデータ取得が行えたことを確認した。センサの特性データを５０［ｍｍ］間隔でサンプリングしたため誤差が最大±２５［ｍｍ］となったが、さらに細かくデータを収集しておくことで精度の向上が可能である。

【0072】

このように位置推定システムは、受信部として３つの受光素子を直交するように配置し、その光軸に沿って３次元直行座標軸を定義し（原点を受光素子）、振幅変調赤外光の発信源の位置（距離、角度）を推定する。３つの受光素子が反応する時間を３つの入力として、角度に関する条件、距離に関する条件により、発信源の位置（距離１つ、角度２つ）を推定する。

【0073】

また、位置推定システムは、ＵＡＶを含む移動体の群体制御に利用することができる。例えば、あるマルチロータのＵＡＶに発信機および周辺回路を搭載し、その他のマルチロータＵＡＶに受信機および周辺回路を搭載する。発信機を搭載しているＵＡＶの飛行に合わせて受信機を搭載しているその他のＵＡＶが追従することにより編隊飛行を実現することができる。

【0074】

上述したように、屋内外問わず使用できる安価で小型軽量の３次元位置推定システムの実現に向けて、振幅変調赤外光の有効性を検証した。様々な光量の下、実験を行い同様の性能を得ることができたため外乱による影響は非常に小さいと言える。屋外など様々な環境で運用されているＵＡＶへの応用に期待できる。発信機及び３次元位置推定アルゴリズムを作り、短時間で高精度の位置推定が可能なデータを取得するシステムを実現した。位置推定システムによる位置推定手法は安定して高精度な位置推定が可能であり、ＧＰＳと併用して自律的な機体間の距離の短い編隊飛行を行うことができる。さらに、ＧＰＳの使えない建物内や瓦礫の中などの狭い場所でも要救助者の探索を含めた情報収集が体系的に実現できる。災害時の初期情報収集が迅速に行えるため、被害の抑制や人命救助に大きく貢献できる。また、自律制御の際の着陸誘導や、通過ポイントの誘導など様々な面で使用可能であると考えられる。

【0075】

以上、本発明を実施するための形態について実施形態または実験結果を用いて説明したが、本発明はこうした実施形態または実験結果に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。

【符号の説明】

【0076】

１，１Ａ…位置推定システム、１０…発光装置、１２…発光部、２０…飛行体、５６…受光部、５７…受光素子、５８…位置推定部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】

【図12】

【図13】

【図14】

【図15】

【図16】

【図17】

【図18】

【図19】

【図20】

【図21】