JP2018200732A - 移動体制御装置、移動体制御方法、移動体制御プログラム及び記録媒体 - Google Patents

Abstract

【課題】移動体の移動速度が高速であっても、適切な移動体の移動制御を行う。【解決手段】無人飛行体５００に搭載された飛行体搭載装置３００が備える周囲情報取得部３３０により取得された周囲情報に基づいて、無人飛行体５００の外部に設置され、飛行体搭載装置３００との間で無線通信が可能な処理制御装置が、第２周期で移動体の自己位置に関する精度の高い自己位置情報を第２周期で推定する。また、飛行体搭載装置３００が備える移動量取得部３４０により、第２周期よりも短い第１周期で、第１周期の期間ごとの移動量に関する移動量情報を自己位置情報の精度よりも低い精度で取得する。そして、飛行体搭載装置３００が備える制御情報出力部３５０が、自己位置情報及び移動量情報に基づいて、飛行制御情報を算出して出力することにより、無人飛行体５００の移動を制御する。【選択図】 図３

Description

本発明は、移動体制御装置、移動体制御方法及び移動体制御プログラム、並びに、当該移動体制御プログラムが記録された記録媒体に関する。

従来から、ロボット等の移動体の自律移動に際して適用可能な技術として、移動体の自己位置推定と、移動体の周囲の環境地図作成とを同時に行うＳＬＡＭ（Simultaneous Localization And Mapping）技術が注目されている。かかるＳＬＡＭ技術では、作成された環境地図内における移動***置が高精度で推定できるようになっている。

なお、ＳＬＡＭ技術の実装に際しては、一般的に、移動体の周囲情報を取得するためのＬｉＤＡＲ（Light Detection and Ranging）装置等の周囲情報取得用センサと、所定周期期間ごとの移動体の移動量を取得するための加速度センサ等の移動量取得用センサとを用意するようになっている。ここで、周囲情報取得用センサによる検出結果のデータ量は多く、データ処理にも時間がかかるようになっている。

こうしたＳＬＡＭ機能を利用しつつ、現在位置から目標位置までの移動のための経路に沿った移動体の移動を制御する技術が提案されている（特許文献１参照：以下、「従来例」と呼ぶ）。この従来例の技術では、周囲情報取得用センサによる検出結果データの処理が終了するたびに、移動体の移動を制御するための制御情報を導出するようになっている。

特開２０１０−０８６４１６号公報

上述した従来例の技術では、周囲情報取得用センサによる検出結果データに関する処理が終了するたびに、移動体の移動を制御するための制御情報を導出するので、制御情報を導出周期が長くなっている。このため、移動体の移動速度が遅い場合には、移動体の現在位置に基づいた適切な移動制御を実質的に行うことができるといえるが、移動体の移動速度が速くなると、移動体の現在位置に基づいた適切な移動制御を実質的に行うことができるとはいい難い。

ところで、従来例の技術は、路面上又は床面上を走行する移動体に適用される技術となっており、ＳＬＡＭ処理を行う演算資源が移動体に搭載されている。しかしながら、ＳＬＡＭ処理を行う演算資源は、大きさ、重さ又は電力消費の点から、小型の移動体（特に、小型の飛行体）に搭載するには不向きである。

そこで、移動体の外部にＳＬＡＭ処理を行う演算資源を設置することが考えられるが、上述した周囲情報取得用センサ及び移動量取得用センサは移動体に搭載される必要がある。したがって、これらのセンサの検出データは、無線通信によりＳＬＡＭ処理を行う演算資源へ送信することになる。しかしながら、上述したように、周囲情報取得用センサによる検出結果データの量が多いことから、周囲情報取得用センサによる検出結果データの送信に時間を要してしまう。

したがって、移動体の外部にＳＬＡＭ処理を行う演算資源を設置したうえで、従来例の技術を適用して移動体の移動制御を行うようにすると、上述した無線通信によるデータ転送時間が、更なる制御遅延要素となってしまう。この結果、適切な移動制御を行いつつ、移動体の移動速度を速くすることが益々困難となる。

このため、移動体の移動速度が高速であっても、適切な移動体制御を行うことができる技術が望まれている。かかる要請に応えることが、本発明が解決すべき課題の一つとして挙げられる。

請求項１に記載の発明は、移動体に搭載され、前記移動体の周囲情報を取得する周囲情報取得部と；前記移動体に搭載され、第１周期で前記移動体の前記第１周期の期間ごとの移動量に関する移動量情報を取得する移動量取得部と；前記周囲情報に基づき、前記第１周期より長い第２周期で前記移動体の自己位置に関する自己位置情報を推定する自己位置推定部と；前記自己位置情報と、前記移動量情報とに基づいて、前記移動体の移動制御に関する制御情報を出力する出力部と；を備えることを特徴とする移動体制御装置である。

請求項１０に記載の発明は、移動体に搭載され、前記移動体の周囲情報を取得する周囲情報取得部と；前記移動体に搭載され、第１周期で前記移動体の前記第１周期の期間ごとの移動量に関する移動量情報を取得する移動量取得部と；を備え、前記移動体の移動を制御する移動体制御装置において使用される移動体制御方法であって、前記周囲情報に基づき、前記第１周期より長い第２周期で前記移動体の自己位置に関する自己位置情報を推定する自己位置推定工程と；前記自己位置情報と、前記移動量情報とに基づいて、前記移動体の移動制御に関する制御情報を出力する出力工程と；を備えることを特徴とする移動体制御方法である。

請求項１１に記載の発明は、移動体の移動を制御する移動体制御装置が有するコンピュータに、請求項１０に記載の移動体制御方法を実行させる、ことを特徴とする移動体制御プログラムである。

請求項１２に記載の発明は、移動体の移動を制御する移動体制御装置が有するコンピュータにより読み取り可能に、請求項１１に記載の移動体制御プログラムが記録されている、ことを特徴とする記録媒体である。

本発明の一実施形態に係る移動体制御装置を構成する要素の配置、及び、当該要素の一つである処理制御部の構成を説明するための図である。 図１の目標移動ベクトル決定部の構成を説明するための図である。 図１の飛行体搭載装置の構成を説明するための図である。 図３の制御情報出力部の構成を説明するための図である。 図２の目標移動ベクトル決定部による目標移動ベクトル決定処理を説明するためのフローチャートである。 目標移動ベクトル決定処理により順次決定される目標移動ベクトルの例を説明するための図である。 図３の制御情報出力部による制御情報出力処理を説明するためのフローチャートである。

以下、本発明の一実施形態を、図１〜図７を参照して説明する。なお、以下の説明及び図面においては、同一又は同等の要素には同一の符号を付し、重複する説明を省略する。

［構成］
図１に示されるように、一実施形態に係る移動体制御装置１００は、処理制御装置２００と、飛行体搭載装置３００とを備えている。ここで、飛行体搭載装置３００は、移動体である無人飛行体５００に搭載され、無人飛行体５００とともに移動する。一方、処理制御装置２００は、無人飛行体５００の外部の固定位置に配設される。そして、処理制御装置２００と飛行体搭載装置３００との間では、無線通信による情報の授受が行われるようになっている。

なお、本実施形態では、無人飛行体５００は、複数のロータを備えており、これらのロータの回転を制御することにより、飛行制御（移動方向及び移動速度の制御）が可能となっている。

＜処理制御装置２００の構成＞
上記の処理制御装置２００は、図１に示されるように、無線送受信部２２０と、記憶部２３０と、ＳＬＡＭ処理部２４０と、目標移動ベクトル決定部２５０とを備えている。また、処理制御装置２００は、入力部２６０と、表示部２７０とを備えている。

上記の無線送受信部２２０は、アンテナ２１０を備えて構成されている。この無線送受信部２２０は、アンテナ２１０を利用して、飛行体搭載装置３００との間で情報の送受信を行う。

無線送受信部２２０は、アンテナ２１０を介して、飛行体搭載装置３００から送信された情報を受信する。そして、無線送受信部２２０は、当該受信した情報をＳＬＡＭ処理部２４０へ送る。なお、飛行体搭載装置３００から送信される情報には、後述する周囲情報及び移動量情報が含まれている。

また、無線送受信部２２０は、目標移動ベクトル決定部２５０から送られた後述する目標移動ベクトル情報を受ける。そして、無線送受信部２２０は、当該目標移動ベクトル情報を、アンテナ２１０を介して、飛行体搭載装置３００へ送信する。

上記の記憶部２３０は、処理制御装置２００で利用される様々な情報を記憶する。こうして記憶部２３０に記憶される情報としては、ＳＬＡＭ処理部２４０により生成された地図情報が含まれている。この記憶部２３０に対しては、ＳＬＡＭ処理部２４０がアクセス可能となっている。

上記のＳＬＡＭ処理部２４０は、飛行体搭載装置３００から送信された周囲情報及び移動量情報を、無線送受信部２２０を介して受ける。ＳＬＡＭ処理部２４０は、当該周囲情報及び移動量情報に基づいて、無人飛行体５００の自己位置推定処理と、無人飛行体５００の周囲の環境地図作成処理とを同時に行うＳＬＡＭ処理を実行する。そして、ＳＬＡＭ処理部２４０は、自己位置推定処理により推定された自己位置情報を目標移動ベクトル決定部２５０へ送るとともに、環境地図作成処理により作成された環境地図情報を記憶部２３０に格納する。

なお、ＳＬＡＭ処理部２４０の自己位置情報の出力周期は周期時間長ＴＰ₂（例えば、２００ｍｓ）の第２周期であり、出力される自己位置情報は、出力時点から時間長ＴＰ₂だけ前の時刻の無人飛行体５００の位置を高精度で反映した情報となっている。

また、ＳＬＡＭ処理部２４０は、入力部２６０から送られた地図表示態様指定に応答して、作成された最新の環境地図、及び、最新に推定された環境地図中における無人飛行体５００の自己位置を表示部２７０に表示させるための表示データを生成する。そして、ＳＬＡＭ処理部２４０は、生成された表示データを表示部２７０へ送る。この結果、地図表示態様指定に応じた環境地図、及び、無人飛行体５００の環境地図中における自己位置が、表示部２７０に表示される。

上記の目標移動ベクトル決定部２５０は、入力部２６０から送られた目標位置指定に含まれる環境地図上における目標位置及び当該目標位置までの目標移動時間を受けるとともに、第２周期でＳＬＡＭ処理部２４０から送られてくる自己位置情報を受ける。ここで、目標移動時間は、時間長ＴＰ₂の整数倍が指定されるようになっている。

引き続き、目標移動ベクトル決定部２５０は、当該目標位置、当該目標移動時間及び当該自己位置情報に基づいて、現在位置を推定するとともに、目標移動ベクトル及び目標移動速度を決定する。そして、目標移動ベクトル決定部２５０は、決定された目標移動ベクトル及び目標移動速度を新たな目標移動ベクトル情報として、無線送受信部２２０を介して、飛行体搭載装置３００へ送信する。

なお、目標移動ベクトル決定部２５０の構成の詳細については、後述する。

上記の入力部２６０は、本実施形態では、キーボード等のストロークデバイスと、マウス等のポインティングデバイスを備えて構成されている。この入力部２６０に対する入力操作を行うことにより、利用者は、上述した地図表示態様指定及び目標位置指定を入力できるようになっている。

なお、入力部２６０は、地図表示態様指定が入力されると、入力結果をＳＬＡＭ処理部２４０へ送る。また、入力部２６０は、目標位置指定が入力されると、入力結果を目標移動ベクトル決定部２５０へ送る。

上記の表示部２７０は、例えば、液晶パネル、有機ＥＬ（Electro Luminescence）パネル、ＰＤＰ（Plasma Display Panel）等の表示デバイスを備えて構成されている。この表示部２７０は、ＳＬＡＭ処理部２４０から送られた表示データを受けると、当該表示データに対応する画像を表示する。

《目標移動ベクトル決定部２５０の構成》
ここで、上述した目標移動ベクトル決定部２５０の構成について説明する。

目標移動ベクトル決定部２５０は、図２に示されるように、目標位置保持部２５１と、現在位置推定部２５２とを備えている。また、目標移動ベクトル決定部２５０は、目標移動ベクトル算出部２５３を備えている。

上記の目標位置保持部２５１は、入力部２６０から送られた目標位置指定を受ける。そして、目標位置保持部２５１は、当該目標位置指定に含まれる目標位置及び目標移動時間を保持する。こうして入力部２６０から送られた目標位置指定の内容を保持すると、目標位置保持部２５１は、新たな目標位置及び目標移動時間を保持した旨を現在位置推定部２５２へ送る。

また、目標位置保持部２５１は、目標移動ベクトル算出部２５３から送られた新たな目標移動時間を受けた場合には、保持する目標移動時間を当該新たな目標移動時間に更新する。こうして保持された目標位置及び目標移動時間は、目標移動ベクトル算出部２５３により読み取り可能となっている。

上記の現在位置推定部２５２は、ＳＬＡＭ処理部２４０から送られた自己位置情報、並びに、目標移動ベクトル算出部２５３から送られた目標移動ベクトル及び目標移動速度を受ける。そして、現在位置推定部２５２は、当該自己位置情報、当該目標移動ベクトル及び当該目標移動速度に基づいて、新たな現在位置を推定する。こうして推定された新たな現在位置は、目標移動ベクトル算出部２５３へ送られる。

なお、現在位置推定部２５２が実行する処理については、後述する。

上記の目標移動ベクトル算出部２５３は、目標位置保持部２５１から読み取られた目標位置及び目標移動時間、並びに、現在位置推定部２５２から送られた現在位置を受ける。引き続き、目標移動ベクトル算出部２５３は、当該目標位置、当該目標移動時間及び当該現在位置に基づいて、新たな目標移動ベクトル及び新たな目標移動速度を算出する。そして、目標移動ベクトル算出部２５３は、算出された新たな目標移動ベクトル及び新たな目標移動速度を新たな目標移動ベクトル情報に決定する。こうして決定された目標移動ベクトル情報は、無線送受信部２２０を介して飛行体搭載装置３００へ送信されるとともに、現在位置推定部２５２へ送られる。

なお、目標移動ベクトル算出部２５３が実行する処理については、後述する。

＜飛行体搭載装置３００の構成＞
次に、上記の飛行体搭載装置３００の構成について説明する。

飛行体搭載装置３００は、図３に示されるように、無線送受信部３２０と、周囲情報取得部３３０と、移動量取得部３４０とを備えている。また、飛行体搭載装置３００は、制御情報出力部３５０と、ロータ駆動部３６０とを備えている。

上記の無線送受信部３２０は、アンテナ３１０を備えて構成されている。この無線送受信部３２０は、アンテナ３１０を利用して、処理制御装置２００との間で情報の送受信を行う。

無線送受信部３２０は、アンテナ３１０を介して、処理制御装置２００から送信された目標移動ベクトル情報を受信する。そして、無線送受信部３２０は、当該受信した目標移動ベクトル情報を制御情報出力部３５０へ送る。

また、無線送受信部３２０は、周囲情報取得部３３０から送られた周囲情報、及び、移動量取得部３４０から送られた移動量情報を受ける。そして、無線送受信部３２０は、当該周囲情報及び移動量情報を、アンテナ３１０を介して、処理制御装置２００へ送信する。

上記の周囲情報取得部３３０は、本実施形態では、ＬｉＤＡＲ機能を有している。この周囲情報取得部３３０は、無人飛行体５００の現在位置から全方位に関し、無人飛行体５００の現在位置から移動の障害物（例えば、無人飛行体５００が屋内を飛行する場合には、壁、柱、梁、天井、床等）までの距離を取得する。そして、周囲情報取得部３３０は、取得結果を、周囲情報として、無線送受信部３２０を介して処理制御装置２００（より詳細には、ＳＬＡＭ処理部２４０）へ送信する。

なお、周囲情報取得部３３０は、上述した第２周期で、新たな周囲情報を、処理制御装置２００へ向けて順次出力するようになっている。

上記の移動量取得部３４０は、本実施形態では、オプティカルフローセンサを備えて構成されている。この移動量取得部３４０は、オプティカルフローセンサによる検出結果に基づいて、周期時間長ＴＰ₁（例えば、１０ｍｓ）の第１周期で、周期期間中における移動量情報を取得する。そして、移動量取得部３４０は、取得された移動量情報を、制御情報出力部３５０へ送るとともに、無線送受信部３２０を介して処理制御装置２００（より詳細には、ＳＬＡＭ処理部２４０）へ送信する。

なお、移動量取得部３４０は、制御情報出力部３５０から送られた、地図座標系におけるベクトル成分表現が行われている補正目標移動ベクトルを受ける。そして、移動量取得部３４０は、補正目標移動ベクトルの方向となっている無人飛行体５００の飛行方向の状態で得られるオプティカルフローセンサによる検出結果から得られる移動量を、補正目標移動ベクトルに基づいて、地図座標系におけるベクトル成分表現に変換する。かかる変換の結果が、移動量情報として、制御情報出力部３５０へ送られるとともに、無線送受信部３２０を介して処理制御装置２００へ送信されるようになっている。

また、上述した時間長ＴＰ₂は、ほぼ正確に、時間長ＴＰ₁の整数Ｍ（例えば、「２０」）倍となっている。さらに、移動量取得部３４０により取得される移動量情報には、いわゆるオフセット誤差、ランダム誤差等の誤差が含まれており、上述したＳＬＡＭ処理部２４０により取得される自己位置情報よりも精度が低い情報となっている。

上記の制御情報出力部３５０は、処理制御装置２００（より詳細には、目標移動ベクトル決定部２５０）から送られた目標移動ベクトル情報を、無線送受信部３２０を介して受信するとともに、移動量取得部３４０から送られた移動量情報を受ける。そして、制御情報出力部３５０は、当該目標移動ベクトル情報及び当該移動量情報に基づいて飛行制御情報を算出し、算出された飛行制御情報を出力する。こうして出力された飛行制御情報は、ロータ駆動部３６０へ送られる。

本実施形態では、制御情報出力部３５０が新たな目標移動ベクトル情報を受信するタイミングと、制御情報出力部３５０が新たな移動量情報を受けるタイミングとが同期するようになっている。すなわち、制御情報出力部３５０は、上述したように第２周期の周期時間長ＰＴ₂（＝Ｍ・ＰＴ₁：Ｍは２以上の整数）ごとに、Ｍ個の移動量情報を受ける。そして、当該Ｍ個の移動量情報のうちの一つの移動量情報を受けた後、時間長ＰＴ₁よりも十分に短い時間の経過後に目標移動ベクトル情報を受信するように、タイミング同期が図られるようになっている。

なお、制御情報出力部３５０の構成の詳細については、後述する。

上記のロータ駆動部３６０は、制御情報出力部３５０から送られた飛行制御情報を受ける。そして、ロータ駆動部３６０は、当該飛行制御情報に従って、無人飛行体５００が備える複数のロータを回転駆動する。このため、無人飛行体５００は、制御情報出力部３５０で算出された飛行制御情報に従った飛行を行うようになっている。

《制御情報出力部３５０の構成》
ここで、上述した制御情報出力部３５０の構成について説明する。

制御情報出力部３５０は、図４に示されるように、累積移動量推定部３５１を備えている。また、制御情報出力部３５０は、目標移動ベクトル保持部３５６と、制御情報算出部３５７とを備えている。

上記の累積移動量推定部３５１は、処理制御装置２００から送信された目標移動ベクトル情報を、無線送受信部３２０を介して受信するとともに、移動量取得部３４０から送られた移動量情報を受ける。そして、累積移動量推定部３５１は、目標移動ベクトル情報の受信後、次の目標移動ベクトル情報を受信する期間において、移動量情報を受けるたびに、移動量情報により示された移動量を、順次、累積加算する。この累積加算により算出された累積移動量は、累積移動ベクトルとして、制御情報算出部３５７へ送られる。かかる機能を有する累積移動量推定部３５１は、累積移動量保持部３５２と、加算部３５３とを備えている。

上記の累積移動量保持部３５２は、目標移動ベクトル情報を受信する。そして、累積移動量保持部３５２は、目標移動ベクトル情報を受信すると、保持内容を「０」にリセットする。

また、累積移動量保持部３５２は、加算部３５３から送られた新たな加算結果を受けるたびに、保持内容を当該新たな加算結果に更新する。こうして更新された保持内容が、累積移動ベクトルとして、加算部３５３及び制御情報算出部３５７へ送られる。

上記の加算部３５３は、累積移動量保持部３５２から送られた保持内容、及び、移動量取得部３４０から送られた移動量情報を受ける。そして、加算部３５３は、移動量情報を受けるたびに、当該保持内容（その時点での累積移動量）と、当該移動量情報として示されている移動量とを加算する。かかる加算の結果は、累積移動量保持部３５２へ送られる。

上記の目標移動ベクトル保持部３５６は、処理制御装置２００から送られた目標移動ベクトル情報を、無線送受信部３２０を介して受信する。そして、目標移動ベクトル保持部３５６は、当該目標移動ベクトル情報、すなわち、目標移動ベクトル及び目標移動速度を保持する。こうして目標移動ベクトル保持部３５６に保持された目標移動ベクトル及び目標移動速度は、制御情報算出部３５７へ送られる。

上記の制御情報算出部３５７は、目標移動ベクトル保持部３５６に保持された目標移動ベクトル及び目標移動速度を読み取るとともに、累積移動量推定部３５１から送られた累積移動ベクトルを受ける。そして、制御情報算出部３５７は、当該目標移動ベクトルから累積移動ベクトルを差し引いて補正目標移動ベクトルを算出し、当該算出された補正目標移動ベクトルの方向へ、当該目標移動速度で移動するためのロータ駆動の制御情報を飛行制御情報として算出する。こうして算出された飛行制御情報は、ロータ駆動部３６０へ送られる。また、算出された補正目標移動ベクトルは、移動量取得部３４０へ送られる。

［動作］
次に、上記のように構成された移動体制御装置１００の動作について、上述した目標移動ベクトル決定部２５０による目標移動ベクトル決定処理、及び、上述した制御情報出力部３５０による制御情報出力処理に主に着目して説明する。

なお、以下の説明においては、目標移動ベクトル決定部２５０が新たに目標位置指定を受けた後に最初に自己位置情報を受けた時刻を、時刻Ｔ₀と記す。さらに、時刻（Ｔ₀＋ｊ×ＰＴ₀）を、時刻Ｔ_jと記す。

また、ＳＬＡＭ処理部２４０から時刻Ｔ_jに出力される自己位置情報で示される時刻Ｔ_j-1における自己位置を、自己位置Ｑ_j-1と記す。さらに、時刻Ｔ_jにおける予定位置を、予定位置Ｒ_jと記す。ここで、「予定位置」とは、移動量情報に誤差がない状態で、制御情報出力部３５０から出力された制御情報に従って目標移動ベクトルに沿って移動した場合における無人飛行体５００の位置をいう。

また、時刻Ｔ_jにおける推定現在位置を、推定現在位置ＣＰ_jと記す。さらに、推定現在位置ＣＰ_jから自己位置Ｑ_jまでの誤差を、誤差Ｅ_jと記す。

また、推定現在位置ＣＰ_jを始点とし、時刻Ｔ₀において指定された目標位置Ｐ_Nを終点とするベクトルを目標移動ベクトルＶＣＴ_jと記す。また、目標移動時間（ＴＭ₀−ｊ×ＰＴ₂）を、目標移動時間ＴＭ_jと記す。さらに、目標移動ベクトルＶＣＴ_jに沿った移動制御が行われている期間における目標移動速度を目標移動速度Ｖ_jと記す。

前提として、無人飛行体５００は、時刻Ｔ₀において位置Ｐ₀に位置しており、時刻Ｔ₀より前の時間ＰＴ₂の２倍以上の期間にわたって位置Ｐ₀から移動していないものとする。この結果、自己位置Ｑ_-2，Ｑ_-1，Ｑ₀は、いずれも位置Ｐ₀に精度良く一致することになるとともに、目標移動速度Ｖ_-2，Ｖ_-1が「０」となるようになっている。このため、後述する予定位置Ｒ_-2，Ｒ_-1，Ｒ₀及び推定現在位置ＣＰ_-2，ＣＰ_-1，ＣＰ₀、いずれも位置Ｐ₀に精度良く一致することになる。

また、時刻Ｔ₀において目標移動ベクトル決定部２５０が新たな目標位置指定を受ける前に、目標位置保持部２５１に保持されている目標位置は位置Ｐ₀であるものとする。この結果、目標移動ベクトルＶＣＴ_-2，ＶＣＴ_-1は実質的な「０」ベクトルとなる。

また、移動量取得部３４０は、動作を開始しており、移動量情報を、第１周期で目標移動ベクトル決定部２５０へ送信するとともに、制御情報算出部３５７へ送っているものとする。また、周囲情報取得部３３０は、動作を開始しており、周囲情報を、第２周期でＳＬＡＭ処理部２４０へ送信しているものとする。さらに、ＳＬＡＭ処理部２４０は、動作を開始しており、自己位置情報を、第２周期で目標移動ベクトル決定部２５０へ送っているものとする。

＜目標移動ベクトル決定処理＞
まず、目標移動ベクトル決定部２５０により実行される目標移動ベクトル決定処理について説明する。

目標移動ベクトル決定処理に際しては、図５に示されるように、まず、ステップＳ１１において、目標位置保持部２５１が、新たに目標位置情報を受けたか否かを判定する。ステップＳ１１における判定の結果が否定的であった場合（ステップＳ１１：Ｎ）には、ステップＳ１１の処理が繰り返される。

新たに目標位置情報を受け、ステップＳ１１における判定の結果が肯定的となると（ステップＳ１１：Ｙ）、目標位置保持部２５１は、新たに受けた目標位置情報に含まれる新たな目標位置Ｐ_N及び新たな目標移動時間（Ｎ×ＰＴ₂）を内部に保持する。こうして目標位置Ｐ_N及び目標移動時間（Ｎ×ＰＴ₂）が内部に保持されると、目標位置保持部２５１は、その旨を現在位置推定部２５２へ送る。そして、処理はステップＳ１２へ進む。

ステップＳ１２では、現在位置推定部２５２が、ＳＬＡＭ処理部２４０から自己位置Ｒ_-1を示す自己位置情報を受けたか否かを判定する。ステップＳ１２における判定の結果が否定的であった場合（ステップＳ１２：Ｎ）には、ステップＳ１２の処理が繰り返される。

時刻Ｔ₀となり、ＳＬＡＭ処理部２４０から新たな自己位置情報を受け、ステップＳ１２における判定の結果が肯定的となると（ステップＳ１２：Ｙ）、パラメータｊを「０」に設定する。そして、処理はステップＳ１３へ進む。このステップＳ１３では、現在位置推定部２５２が、無人飛行体５００の現在位置の推定を、推定現在位置ＣＰ_jを算出することにより行う。

かかる推定現在位置ＣＰ_jの算出に際して、現在位置推定部２５２は、まず、予定位置Ｒ_j-1から自己位置Ｒ_j-1までの誤差Ｅ_j-1を算出する。ここで、予定位置Ｒ_j-1は、予定位置Ｒ_j-2から目標移動ベクトルＶＣＴ_j-2に沿って目標移動速度Ｖ_j-2（＝０）で移動した場合における時刻Ｔ_j-1での位置として算出される。

なお、「ｊ＝０」の場合には、予定位置Ｒ_-1（＝位置Ｐ₀）は、推定現在位置ＣＰ_-2（＝位置Ｐ₀）から目標移動ベクトルＶＣＴ_-2（＝「０」ベクトル）に沿って目標移動速度Ｖ_-2（＝０）で移動した場合における時刻Ｔ_-1での位置として算出される。この結果、誤差Ｅ_-1は「０」となる。

また、「ｊ＝１」の場合には、予定位置Ｒ₀（＝位置Ｐ₀）は、推定現在位置ＣＰ_-1（＝位置Ｐ₀）から目標移動ベクトルＶＣＴ_-1（＝「０」ベクトル）に沿って目標移動速度Ｖ_-1（＝０）で移動した場合における時刻Ｔ_０での位置として算出される。この結果、誤差Ｅ₀も「０」となる。

引き続き、ステップＳ１４において、現在位置推定部２５２が、時刻Ｔ_jにおける推定現在位置ＣＰ_jを算出する。かかる推定現在位置ＣＰ_jの推定に際して、現在位置推定部２５２は、まず、推定現在位置ＣＰ_j-1から目標移動ベクトルＶＣＴ_j-1に沿って目標移動速度Ｖ_j-1で移動した場合における時刻Ｔ_jでの予定位置Ｒ_jを算出する。引き続き、現在位置推定部２５２は、予定位置Ｒ_jに誤差Ｅ_j-1を加算して推定現在位置ＣＰ_jとを算出する。そして、現在位置推定部２５２は、推定現在位置ＣＰ_jを現在位置情報として目標移動ベクトル算出部２５３へ送る。

次に、ステップＳ１５において、現在位置情報を受けた目標移動ベクトル算出部２５３が、目標移動ベクトル情報を算出する。かかる目標移動ベクトル情報の算出に際して、目標移動ベクトル算出部２５３は、まず、目標位置保持部２５１から目標位置Ｐ_N及び目標移動時間ＴＭ_jを読み取る。引き続き、目標移動ベクトル算出部２５３は、推定現在位置ＣＰ_jを始点とし、目標位置Ｐ_Nを終点とする目標移動ベクトルＶＣＴ_jを算出する。

次に、目標移動ベクトル算出部２５３は、目標移動ベクトルＶＣＴ_jの長さを算出する。引き続き、算出された長さから推定現在位置ＣＰ_jが目標位置Ｐ_Nの近傍であるいえるか否かを判定する。この判定の結果が肯定的であった場合には、目標移動ベクトル算出部２５３は、目標移動速度を「０」とする。そして、目標移動ベクトル算出部２５３は、目標移動ベクトル及び目標移動速度を、目標移動ベクトルとして、飛行体搭載装置３００の制御情報出力部３５０へ送信するとともに、現在位置推定部２５２へ送る。なお、本実施形態では、時刻Ｔ_Nにおいて、推定現在位置ＣＰ_Nが目標位置Ｐ_Nの近傍となるようになっている。

この後、ステップＳ１５の処理が終了し、処理はステップＳ１１へ戻る。

一方、当該判定が否定的であった場合には、目標移動ベクトル算出部２５３は、算出された新たな目標移動ベクトルの長さを、読み取られた目標移動時間ＴＭ_jで除算することにより、目標移動速度Ｖ_jを算出する。引き続き、目標移動ベクトル算出部２５３は、目標移動ベクトルＶＣＴ_j及び目標移動速度Ｖ_jを、目標移動ベクトルとして、飛行体搭載装置３００の制御情報出力部３５０へ送信するとともに、現在位置推定部２５２へ送る。

次に、目標移動ベクトル算出部２５３は、新たな目標移動時間ＴＭ_j+1（＝ＴＭ_j−ＰＴ₂）を算出する。引き続き、目標移動ベクトル算出部２５３は、目標移動時間ＴＭ_j+1を目標位置保持部２５１へ送る。この結果、目標位置保持部２５１に保持される目標移動時間が、目標移動時間ＴＭ_j+1に更新される。

次いで、目標移動ベクトル算出部２５３は、パラメータｊをインクリメントする。
この後、ステップＳ１５の処理が終了し、処理はステップＳ１２へ戻る。

以後、ステップＳ１１〜Ｓ１５の処理が繰り返される。この結果、目標移動ベクトル情報が逐次決定され、決定された目標移動ベクトル情報が、制御情報出力部３５０へ送信される。

上記の目標移動ベクトル決定処理により決定される目標移動ベクトルの例が、図６に示されている。なお、図６においては、目標位置Ｐ_N（Ｎ＝４）の例が示されている。

＜制御情報出力処理＞
次いで、制御情報出力部３５０により実行される目標移動ベクトル決定処理について説明する。

制御情報出力部３５０では、目標移動ベクトル保持部３５６が、処理制御装置２００から送られた目標移動ベクトル情報を受信すると、当該目標移動ベクトル情報、すなわち、目標移動ベクトル及び目標移動速度を保持する。また、制御情報出力部３５０では、累積移動量推定部３５１における累積移動量保持部３５２と加算部３５３とが、上述したようにして協働して、最新に目標移動ベクトル情報を受信した後における無人飛行体５００の累積移動量を推定する。そして、累積移動量推定部３５１は、累積移動量の推定結果を、累積移動ベクトルの表現態様で、第１周期で制御情報算出部３５７へ送る。この制御情報算出部３５７では、制御情報算出処理が実行される。

かかる制御情報算出処理では、図７に示されるように、ステップＳ２１において、制御情報算出部３５７が、累積移動量推定部３５１から送られた新たな累積移動量ベクトルを受けたか否かを判定する。ステップＳ２１における判定の結果が否定的であった場合（ステップＳ２１：Ｎ）には、ステップＳ２１の処理が繰り返される。

新たな累積移動量ベクトルを受け、ステップＳ２１における判定の結果が肯定的となると（ステップＳ２１：Ｎ）、処理はステップＳ２２へ進む。このステップＳ２２では、制御情報算出部３５７が、まず、目標移動ベクトル保持部３５６に保持されている目標移動ベクトル及び目標移動速度を読み取る。引き続き、制御情報算出部３５７は、読み取られた目標移動ベクトルから当該新たな累積移動量ベクトルを減算することにより、新たな補正目標移動ベクトルを算出する。

次に、ステップＳ２３において、制御情報算出部３５７が、ＰＩＤ制御演算等を行って、新たな補正目標移動ベクトルの方向に沿って、読み取られた目標移動速度で移動させるための飛行制御情報を算出する。そして、制御情報算出部３５７は、算出された飛行制御情報をロータ駆動部３６０へ出力する。

こうしてステップＳ２３の処理が終了すると、処理はステップＳ２１へ戻る。以後、ステップＳ２１〜Ｓ２３の処理が繰り返される。この結果、自己位置情報の推定周期である第２周期の周期時間長ＰＴ₂よりも短い周期時間長ＰＴ₁の第１周期で、飛行制御情報が算出されて、無人飛行体５００の移動が制御される。

以上説明したように、本実施形態では、無人飛行体５００に搭載された周囲情報取得部３３０により取得された周囲情報に基づいて、ＳＬＡＭ処理部２４０が、第２周期で移動体の自己位置に関する精度の高い自己位置情報を第２周期で推定する。また、無人飛行体５００に搭載された移動量取得部３４０により、第２周期よりも短い第１周期で、第１周期の期間ごとの移動量に関する移動量情報を自己位置情報の精度よりも低い精度で取得する。そして、目標移動ベクトル決定部２５０が、第２周期の周期時間前の自己位置情報に基づいて現時点での目標移動ベクトルを第２周期で修正する。また、目標移動ベクトルについての次の修正が行われるまでの間は、制御情報出力部３５０が、移動量情報に基づく第１周期での目標移動ベクトルの補正を行いつつ、補正された目標移動ベクトルに基づいて、飛行制御情報を算出して出力することにより、無人飛行体５００の移動を制御する。

したがって、本実施形態によれば、無人飛行体５００の移動速度が高速であっても、適切な飛行制御を行うことができる。

また、本実施形態では、制御情報出力部３５０から出力された飛行制御情報に従った移動における第２周期の周期時間前の前回予定位置から、第２周期の周期時間前の自己位置までの誤差を、今回予定位置に加算することにより、現在位置を推定する。このため、移動量情報における誤差を適切に評価して、現時点における目標移動ベクトルを第２周期で決定でき、ひいては、無人飛行体５００の移動速度が高速であっても、適切な飛行制御を行うことができる。

［実施形態の変形］
本発明は、上記の実施形態に限定されるものではなく、様々な変形が可能である。

例えば、上記の実施形態では、第２周期の周期時間前の前回予定位置から第２周期の周期時間前の自己位置までの誤差を今回予定位置に加算して、現在位置を推定するようにした。これに対し、前回予定位置から第２周期の周期時間前の自己位置までの誤差分の２倍を今回予定位置に加算することにより、現在位置情報を推定するようにしてもよい。この場合には、移動量情報における誤差においてオフセット誤差が支配的である場合には、上記の実施形態の場合よりも、高精度で現在位置を推定することができ、より適切な目標移動ベクトルを決定することができる。

また、上記の実施形態では、飛行体搭載装置が制御情報出力部を備えるようにしたが、制御情報出力部の機能を処理制御装置に実装するようにしてもよい。この場合には、飛行体搭載装置の軽量化及び省電力化を更に図ることができる。

また、上記の実施形態では、移動量取得部がオプティカルフローセンサを備える構成とした。これに対し、オプティカルフローセンサに代えて、ジャイロセンサを備え、飛行体のピッチ角とロール角とから飛行体の速度を推定し、推定された速度の時間積分に基づいて移動量情報を取得するようにしてもよい。また、オプティカルフローセンサに代えて、ジャイロセンサ及び加速度センサを備え、ジャイロセンサによる検出結果から移動方向を推定するとともに、加速度センサによる検出結果から速度を推定し、推定された移動方向及び推定された速度の時間積分に基づいて移動量情報を取得するようにしてもよい。

また、上記の実施形態では、移動体が無人飛行体である場合に本発明を適用したが、無人飛行体以外の移動体（例えば、路面上又は床面上を走行する車両等）に本発明を適用してもよい。この場合、移動体搭載装置の重量や消費電力に余裕がある場合には、上記の実施形態における処理制御装置の機能の一部又は全部を、移動体搭載装置に実装するようにすることができる。

また、上記の実施形態では、無人飛行体の移動が２次元移動であるか、又は、３次元移動であるかを明示しなかったが、本発明は、２次元移動及び３次元移動のいずれであっても適用することができる。

なお、上記の実施形態の処理制御装置を、中央処理装置（ＣＰＵ：Central Processing Unit）等の演算手段としてのコンピュータを備えた構成とし、予め用意されたプログラムを当該コンピュータで実行することにより、上記の実施形態における処理制御装置の機能を実現するようにしてもよい。また、上記の実施形態の飛行体搭載装置を、中央処理装置等の演算手段としてのコンピュータを備えた構成とし、予め用意されたプログラムを当該コンピュータで実行することにより、上記の実施形態における飛行体搭載装置の機能を実現するようにしてもよい。これらのプログラムはハードディスク、ＣＤ−ＲＯＭ、ＤＶＤ等のコンピュータで読み取り可能な記録媒体に記録され、当該コンピュータによって記録媒体から読み出されて実行される。また、このプログラムは、ＣＤ−ＲＯＭ、ＤＶＤ等の可搬型記録媒体に記録された形態で取得されるようにしてもよいし、インターネットなどのネットワークを介した配信の形態で取得されるようにしてもよい。

１００ … 移動体制御装置
２５２ … 現在位置推定部
２５３ … 目標移動ベクトル算出部
３３０ … 周囲情報取得部
３４０ … 移動量取得部
３５０ … 制御情報出力部（出力部）
３５１ … 累積移動量推定部
３５６ … 目標移動ベクトル保持部（保持部）
３５７ … 制御情報算出部

Claims (1)

1. 移動体に搭載され、前記移動体の周囲情報を取得する周囲情報取得部と；
   前記移動体に搭載され、第１周期で前記移動体の前記第１周期の期間ごとの移動量に関する移動量情報を取得する移動量取得部と；
   前記周囲情報に基づき、前記第１周期より長い第２周期で前記移動体の自己位置に関する自己位置情報を推定する自己位置推定部と；
   前記自己位置情報と、前記移動量情報とに基づいて、前記移動体の移動制御に関する制御情報を出力する出力部と；
   を備えることを特徴とする移動体制御装置。

Images