JPWO2019111701A1 - 制御装置、および制御方法、プログラム、並びに移動体 - Google Patents

制御装置、および制御方法、プログラム、並びに移動体 Download PDF

Info

Publication number
JPWO2019111701A1
JPWO2019111701A1 JP2019558116A JP2019558116A JPWO2019111701A1 JP WO2019111701 A1 JPWO2019111701 A1 JP WO2019111701A1 JP 2019558116 A JP2019558116 A JP 2019558116A JP 2019558116 A JP2019558116 A JP 2019558116A JP WO2019111701 A1 JPWO2019111701 A1 JP WO2019111701A1
Authority
JP
Japan
Prior art keywords
self
posture
time
unit
series information
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.)
Granted
Application number
JP2019558116A
Other languages
English (en)
Other versions
JP7180612B2 (ja
Inventor
小林　大
大 小林
諒 渡辺
諒 渡辺
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Sony Corp
Original Assignee
Sony Corp
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Sony Corp filed Critical Sony Corp
Publication of JPWO2019111701A1 publication Critical patent/JPWO2019111701A1/ja
Application granted granted Critical
Publication of JP7180612B2 publication Critical patent/JP7180612B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0248—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means in combination with a laser
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0272—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising means for registering the travel distance, e.g. revolutions of wheels
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Theoretical Computer Science (AREA)
  • Electromagnetism (AREA)
  • Software Systems (AREA)
  • Artificial Intelligence (AREA)
  • Molecular Biology (AREA)
  • Computing Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Biomedical Technology (AREA)
  • Mathematical Physics (AREA)
  • Biophysics (AREA)
  • Evolutionary Computation (AREA)
  • General Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Data Mining & Analysis (AREA)
  • Health & Medical Sciences (AREA)
  • Computational Linguistics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Optics & Photonics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

本開示は、自己位置が不明でも、再度、迅速に自己位置を推定することができるようにする制御装置、および制御方法、プログラム、並びに移動体に関する。LIDARや車輪エンコーダにより検出される時系列に供給される情報を格納し、格納された時系列の情報を用いて自己位置を推定する場合、キッドナップ状態などの予め予測できない姿勢変化が検出されたときには、格納された時系列の情報をリセットしてから、再度、自己位置を推定する。搭載された計算機に従い自律的に移動する多足ロボット、飛翔体、車載システムに適用することができる。

Description

本開示は、制御装置、および制御方法、プログラム、並びに移動体に関し、特に、自己位置が不明な状態でも、計算機により行動計画を生成し、自律的な移動を実現できるようにした制御装置、および制御方法、プログラム、並びに移動体に関する。
ロボット等の移動体の自律的な移動を実現するためには、行動を計画する起点となる自己の位置を認識・推定する必要がある。そこで、自己の周囲の状況をセンサ等の手段で認識して、自己の位置を推定し、自律的な移動を計画する技術が提案されている。
例えば、連続的に過去に検出してきた自己位置の情報を利用して自己位置を順次推定する場合、何らかの外力により、それまでの自己位置の傾向とは全く異なる自己位置に移動してしまった場合、過去の自己位置の情報との連続性を失うことにより、自己位置を認識することができない状態となる。
そこで、自己位置が認識できない状態である、スリップやキッドナップ（自己位置が認識できない状態）などの異常移動状態が発生するときに、多重状態情報を補正して移動装置の最終位置情報を算出し、キッドナップ中の移動距離を反映する技術が提案されている（特許文献１参照）。
また、走行に係る制御入力量と実際の移動量の差による変動を監視するとともに、変動が外乱によって制御可能な範囲を逸脱し、自己位置を認識できない状態になったときに走行を停止する技術が提案されている（特許文献２参照）。
特許第４７１８５１８号公報 特開２０１３−４５２９８号公報
特許文献１においては、キッドナップ後の補正にキッドナップ中の移動距離を反映することを明示しており、再度、自己位置を求めるものではないため、正確に自己位置を検出することができない恐れがある。
また、特許文献２においては、制御入力量と実際の移動量の差による変動監視に限定されており、複数のセンサを用いて、再度、自己位置を推定することはできない恐れがある。
本開示は、このような状況に鑑みてなされたものであり、特に、自己位置が不明な状態から、再度、自己位置を推定できるようにするものである。
本開示の一側面の制御装置は、センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出部と、前記自己姿勢検出部による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出部と、センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定部とを含む制御装置である。
前記自己位置推定部には、前記センサ情報を時系列情報として蓄積する時系列情報蓄積部と、前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部を含ませるようにすることができ、前記時系列情報蓄積部は、前記予め予測できない姿勢の変化が検出される場合、蓄積された過去の前記時系列情報がリセットされるようにすることができ、前記時系列情報自己位置推定部には、前記第１の推定モデルとして、前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定させ、前記第２の推定モデルとして、前記時系列情報蓄積部に蓄積された過去の時系列情報がリセットされた後の、前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定させるようにすることができる。
前記時系列情報蓄積部に蓄積された過去の時系列情報は、前記リセットにより、所定の時間以上長く蓄積された時系列情報から削除されるようにすることができる。
前記時系列情報自己位置推定部には、前記時系列情報蓄積部に蓄積された過去の時系列情報が前記リセットされた後、前記時系列情報蓄積部に蓄積された時間が長い前記時系列情報ほど、前記時系列情報により推定される自己位置の重みを軽くして、前記時系列情報自己位置を推定させるようにすることができる。
前記自己位置推定部には、事前確率密度分布の観測値を最新の時系列情報により更新するステップを繰り返すカルマンフィルタを用いて時系列情報自己位置を推定する時系列情報自己位置推定部を含ませるようにすることができ、前記時系列情報自己位置推定部には、前記第１の推定モデルとして、前記時系列情報自己位置を、事前確率密度分布の観測値を最新の時系列情報により更新するステップを繰り返すカルマンフィルタを用いて推定させ、前記第２の推定モデルとして、前記予め予測できない姿勢の変化が検出される場合、最新の前記時系列情報の重みを最大にした後、前記カルマンフィルタを用いて、前記時系列情報自己位置を推定させるようにすることができる。
前記自己位置推定部には、前記センサ情報によりセンシングされる時系列情報として蓄積する時系列情報蓄積部と、前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部と、前記センサ情報によりセンシングされる現在の情報である現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部とを含ませるようにすることができ、前記自己位置推定部には、前記予め予測できない姿勢の変化が検出されない場合、前記時系列情報自己位置を自己位置推定結果として採用し、前記予め予測できない姿勢の変化が検出された場合、前記現在情報自己位置を自己位置推定結果として採用するようにさせることができる。
前記自己位置推定部には、前記姿勢変化検出部の検出結果に応じて、動作モードを、少なくとも第１のモード、第２のモード、および第３のモードに遷移させるようにすることができ、動作開始時において、前記動作モードは、第１のモードとされ、前記第１の推定モデルにより前記自己位置を推定させるようにすることができ、前記第１のモードにおいて、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記動作モードを第２のモードに遷移させ、前記予め予測できない姿勢の変化が検出されたことを通知させるようにすることができ、前記第２のモードにおいて、所定時間が経過した後、前記動作モードを第３のモードに遷移させ、前記第２の推定モデルにより、前記自己位置を推定させるようにすることができ、前記第３のモードにおいて、所定時間が経過した後、前記動作モードを前記第１のモードに遷移させるようにすることができる。
前記自己位置推定部には、前記センサ情報を時系列情報として蓄積する時系列情報蓄積部と、前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部と、前記センサ情報によりセンシングされる現在の情報である現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部とを含ませるようにすることができ、前記第１のモードにおいて、前記第１の推定モデルである、前記時系列情報蓄積部に蓄積された時系列情報を用いた、前記時系列情報自己位置推定部により推定された前記時系列情報自己位置を自己位置推定結果として採用させ、前記第３のモードにおいて、前記第２の推定モデルである、前記現在情報を用いた、前記現在情報自己位置推定部により推定された前記現在情報自己位置を自己位置推定結果として採用させるようにすることができる。
前記予め予測できない姿勢の変化は、前記自己位置および姿勢の変化に連続性がなくなる状態への変化であり、前記第１のモードは通常モードであり、前記第２のモードは自己位置不定モードであり、前記第３のモードは現在情報自己位置推定モードであるようにすることができる。
前記動作モードが、前記第３のモードである、前記現在情報自己位置推定モードの場合、前記予め予測できない姿勢の変化が検出されたとき、前記自己位置推定部には、前記動作モードを、前記第２のモードである、前記自己位置不定モードに遷移させるようにすることができる。
前記自己位置および姿勢の変化に連続性がなくなる状態には、キッドナップ状態、足裏接地面及び車輪のスリップ状態、非固定物体の乗り降りおよびセンサ異常状態を含ませるようにすることができる。
前記予め予測できない姿勢の変化は、事故発生状態への変化であり、前記第１のモードは通常モードであり、前記第２のモードは事故直後モードであり、前記第３のモードは制御回復モードであるようにすることができる。
前記事故発生状態には、人、物体、および他機・他車との衝突、パンク、部品欠け、部品および貨物落下状態を含ませるようにすることができる。
前記時系列情報は、LIDARにより検出される３次元点群情報、および車輪エンコーダにより検出される位置情報、姿勢、速度、加速度、および角速度とすることができ、時系列情報自己位置推定部には、前記時系列情報に基づいて、カルマンフィルタまたは粒子フィルタを用いて、前記自己位置を推定させ、推定結果を時系列情報自己位置として出力させ、前記現在情報は、ステレオカメラにより撮像される視差画像とすることができ、現在情報自己位置推定部には、前記視差画像よりデプス画像を生成させ、前記デプス画像より画像特徴量を抽出させて、前記画像特徴量に基づいて自己位置を推定させ、推定結果を現在情報自己位置として出力させる。
前記自己姿勢検出部が前記自己の姿勢を検出する際に使用する前記センサ情報には、鉛直方向の加速度を含ませるようにすることができ、前記自己姿勢検出部には、前記鉛直方向の加速度の変化の周波数成分を抽出する周波数成分抽出部と、前記周波数成分抽出部により抽出された周波数成分より特徴量を抽出する特徴量抽出部と、前記特徴量に基づいて、前記自己の姿勢を検出する姿勢検出部とを含ませるようにすることができる。
既知の姿勢に対するセンサ情報に基づいて、学習により、前記姿勢と、前記周波数成分の特徴量とが対応付けられた状態で登録されたデータベースをさらに含ませるようにすることができ、前記姿勢検出部には、前記特徴量に基づいて、前記データベースより、対応する姿勢を検索することにより、前記姿勢を検出させるようにすることができる。
前記データベースには、前記既知の姿勢に対するセンサ情報に基づいて、ニューラルネットワークを用いた学習により、前記姿勢と、前記周波数成分の特徴量とが対応付けられた状態で登録されるようにすることができる。
本開示の一側面の制御方法は、センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出処理と、前記自己姿勢検出処理による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出処理と、センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出処理により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定処理とを含む制御方法である。
本開示の一側面のプログラムは、センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出部と、前記自己姿勢検出部による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出部と、センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定部としてコンピュータを機能させるプログラムである。
本開示の一側面の移動体は、センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出部と、前記自己姿勢検出部による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出部と、センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定部と、前記自己位置推定部により推定された自己位置の周辺の状況に基づいて、行動計画を生成する行動計画生成部と、前記行動計画生成部により決定された行動計画に基づいて移動体の動作を制御する制御部とを含む移動体。
本開示の一側面においては、センサ情報に基づいて、自己の姿勢が検出され、検出結果に基づいて、予め予測できない姿勢の変化が検出され、センサ情報に基づいて、第１の推定モデルにより自己位置が推定されると共に、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置が推定される。
本開示の一側面によれば、特に、自己位置が不明な状態になっても、再度、自己位置を推定することが可能となる。
本開示の概要を説明する移動体の構成例を示す図である。 本開示の概要を説明する図である。 本開示の移動体を制御する移動体制御システムの構成例を説明するブロック図である。 本開示の好適な実施の形態に係る認識処理部とその周辺の構成例の詳細なブロック図である。 姿勢検出を説明する図である。 図４の特徴量姿勢DBを生成する特徴量姿勢DB生成部の構成例である。 時系列情報自己位置推定処理を説明するフローチャートである。 現在情報自己位置推定処理を説明するフローチャートである。 姿勢変化検出処理を説明するフローチャートである。 自律移動制御処理を説明するフローチャートである。 特徴量姿勢DB学習処理を説明するフローチャートである。 第１の変形例の自律移動制御処理を説明するフローチャートである。 第２の変形例における動作モードの状態変化を説明する図である。 第２の変形例の自律移動制御処理を説明するフローチャートである。 図１４の通常モード処理を説明するフローチャートである。 図１４の位置不定モード処理を説明するフローチャートである。 図１４の現在情報自己位置推定モード処理を説明するフローチャートである。 第３の変形例における動作モードの状態変化を説明する図である。 第３の変形例の自律移動制御処理を説明するフローチャートである。 図１９の事故直後モード処理を説明するフローチャートである。 図１９の制御回復モード処理を説明するフローチャートである。 汎用のコンピュータの構成例を説明する図である。
以下に添付図面を参照しながら、本開示の好適な実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。
以下、本技術を実施するための形態について説明する。説明は以下の順序で行う。
１．本開示の概要
２．好適な実施の形態
３．第１の変形例
４．第２の変形例
５．第３の変形例
６．ソフトウェアにより実行させる例
＜＜１．本開示の概要＞＞
本開示の移動体は、連続的に推定してきた過去の自己位置の推定結果を用いて、新たな自己位置推定を繰り返すとき、外力により自己位置が失われた後にも、再度、新たに自己位置を推定する移動体である。
図１は、本開示の移動体１１の概要となる構成例を示している。
移動体１１は、例えば、ロボットなどであり、センサ群２１、自律移動制御部２２、およびアクチュエータ群２３を備えている。
センサ群２１は、移動体１１の内部、および移動体１１の周囲の状況の認識に必要な各種の情報を検出するセンサ２１ａ−１乃至２１ａ−ｎを備えており、検出結果を自律移動制御部２２に出力する。また、センサ２１ａ−１乃至２１ａ−ｎについて特に区別する必要がない場合、単に、センサ２１ａと称するものとし、その他の構成についても同様に称する。
より具体的には、センサ２１ａ−１乃至２１ａ−ｎは、例えば、移動体１１の周囲を撮像するカメラ、移動体１１の動きを検出する加速度センサ、移動体１１の周囲に存在する物体までの距離を測定するLIDAR、ToF（Time of Flight）センサ、方向を検出する地磁気センサ、ジャイロセンサ、加速度センサ、周囲の気圧の変化を検出する気圧センサ、接触の有無等を検出する接触センサ、温度を検出する温度センサ、湿度を検出する湿度センサ、PSD（Position Sensitive Detector）測距センサおよび、地球上の位置を検出するGNSS（Global Navigation Satellite System）などを含む。
自律移動制御部２２は、センサ群２１の各種の検出結果より、周囲の状況を認識し、認識結果に基づいて行動計画を生成し、行動計画に応じて、ロボットを駆動させるアクチュエータ群２３の各種アクチュエータ２３ａ−１乃至２３ａ−ｎを動作させる。また、アクチュエータ２３ａ−１乃至２３ａ−ｎについて特に区別する必要がない場合、単に、アクチュエータ２３ａと称するものとし、その他の構成についても同様に称する。
より詳細には、自律移動制御部２２は、認識処理部３１、行動計画処理部３２、および行動制御処理部３３を備えている。
認識処理部３１は、センサ群２１より供給される検出結果に基づいて、認識処理を実行し、例えば、画像、人、物体、表情の種類、位置、属性、および自らや障害物の位置等を認識し、認識結果として行動計画処理部３２に出力する。また、認識処理部３１は、センサ群２１より供給される検出結果に基づいて、自己位置を推定する。この際、認識処理部３１は、所定のモデルを用いて自己位置を推定する。さらに、認識処理部３１は、センサ群２１により供給される検出結果に基づいて、外力の影響により、所定のモデルを用いた自己位置の推定ができない状態になると、所定のモデルとは異なるモデルで自己位置を推定する。
行動計画処理部３２は、認識結果に基づいて、移動体１１の全体の行動である、移動体１１の移動に係る機器の移動の軌跡、状態変化、および速度または加速度などの行動計画を生成し、行動制御処理部３３に供給する。
行動制御処理部３３は、行動計画処理部３２より供給される行動計画に基づいて、アクチュエータ群２３のアクチュエータ２３ａ−１乃至２３ａ−ｎのそれぞれの具体的な動きを制御するための制御信号を生成し、アクチュエータ群２３を動作させる。
アクチュエータ群２３は、行動制御処理部３３より供給される制御信号に基づいて、移動体１１を具体的に動作させるアクチュエータ２３ａ−１乃至２３ａ−ｎを動作させる。より詳細には、アクチュエータ２３ａ−１乃至２３ａ−ｎは、移動体１１の具体的な動き実現するモータ、サーボモータ、ブレーキ等の動作を制御信号に基づいて動作させる。
また、アクチュエータ２３ａ−１乃至２３ａ−ｎは、伸縮運動、屈伸運動、または旋回運動などを実現させる構成を含むと共に、情報を表示するLED（Light Emission Diode）やLCD（Liquid Crystal Display）などからなる表示部、および、音声を出力するスピーカなどの構成をさらに含むこともできる。したがって、アクチュエータ群２３が制御信号に基づいて、制御されることにより、移動体１１を駆動させる各種の装置の動作が実現されると共に、情報が表示される、および音声が出力される。
すなわち、アクチュエータ群２３のアクチュエータ２３ａ−１乃至２３ａ−ｎが制御されることにより、移動体１１の移動に係る動作が制御されると共に、情報の表示や音声の出力などの各種の情報の提示も制御される。
＜本開示の自己位置推定の概要＞
認識処理部３１は、センサ群２１より供給される検出結果に基づいて、自己位置を推定する。この際、認識処理部３１は、所定のモデルとして、過去の自己位置の推定結果を利用して、現在の自己位置を推定する。そして、認識処理部３１は、センサ群２１より供給される検出結果に基づいて、外力により、それまでとは異なる大きな挙動が検出されるなどして、過去の自己位置の推定結果を利用できない状態を検出すると、所定のモデルとは異なるモデルを利用して、例えば、自らの周辺の現在の情報に基づいて自己位置を推定する。
より詳細には、認識処理部３１は、例えば、図２で示されるように、時刻（ｔ−１）において、推定モデルＭＡにより、時刻（ｔ−１）におけるセンサ群２１により供給される検出結果と、過去の時刻（ｔ−２）における情報である、自己位置Ｄ（ｔ−２）とを利用して、現在の自己位置Ｄ（ｔ−１）を推定する。
同様に、時刻ｔにおいて、認識処理部３１は、推定モデルＭＡにより、時刻ｔにおけるセンサ群２１により供給される検出結果と、過去の時刻（ｔ−１）における情報である、自己位置Ｄ（ｔ−１）（さらに、必要に応じて、過去の時刻（ｔ−２）における情報である、自己位置Ｄ（ｔ−２））とを利用して、現在の自己位置Ｄ（ｔ）を推定する。
次に、時刻（ｔ＋１）において、認識処理部３１は、外力により、自己位置が大きく変化し、過去の自己位置推定結果との連続例が失われて、利用することができない状態になった場合、推定モデルを推定モデルＭＡと異なる推定モデルＭＢに切り替えて、例えば、現在の時刻（ｔ＋１）におけるセンサ群２１により供給される検出結果のみを用いて、自己位置Ｄ（ｔ＋１）を推定する。
すなわち、認識処理部３１は、例えば、自己位置を、連続的に推定できている場合には、推定モデルＭＡにより現在の検出結果と過去の自己位置推定結果とを利用して自己位置を推定する。一方、何らかの外力により自己位置が大きく変化し、過去の自己位置推定結果がりようできないときには、認識処理部３１は、推定モデルＭＡと異なる、現在のセンサ群２１の検出結果のみから自己位置を推定することができる推定モデルＭＢにより自己位置を推定する。ただし、一般に、推定モデルＭＢは、推定モデルＭＡよりも自己位置の検出精度が低いため、通常は、推定モデルＭＡが利用される。
このようにセンサ群２１の検出結果に応じて、自己位置が喪失された状態になっても、推定モデルを切り替えることで、自己位置の推定を継続することができるようになる。
結果として、自己位置が不明な状態になっても、再度、自己位置の推定を継続させることが可能となり、継続的に自律行動を実現することが可能となる。
＜＜２．好適な実施の形態＞＞
＜本開示の移動体を制御する移動体制御システムの構成例＞
上述した機能を実現させるための移動体１１を制御する移動体制御システムについて説明する。
図３は、本開示の移動体１１を制御する移動体制御システム１００の概略的な機能の構成例を示すブロック図である。尚、図３の移動体制御システム１００は、本技術が適用され得るロボットからなる移動体１１を制御する移動体制御システムの一例であるが、他の移動体、例えば、航空機、船舶、およびマルチローターコプター（ドローン）などを制御するシステムとして適用することもできる。また、ロボットについても、車輪型のロボットや搭乗可能な自動運転車でもよいし、多足歩行型のロボットでもよい。
移動体制御システム１００は、入力部１０１、データ取得部１０２、通信部１０３、移動体内部機器１０４、出力制御部１０５、出力部１０６、駆動系制御部１０７、駆動系システム１０８、記憶部１０９、及び、自律移動制御部１１０を備える。入力部１０１、データ取得部１０２、通信部１０３、出力制御部１０５、駆動系制御部１０７、記憶部１０９、及び、自律移動制御部１１０は、通信ネットワーク１１１を介して、相互に接続されている。通信ネットワーク１１１は、例えば、ＣＡＮ（Controller Area Network）、ＬＩＮ（Local Interconnect Network）、IEEE802.3 等のＬＡＮ（Local Area Network）、又は、ＦｌｅｘＲａｙ（登録商標）等の任意の規格に準拠した通信ネットワークやバス、あるいは規格化されていない独自の通信方式等からなる。なお、移動体制御システム１００の各部は、通信ネットワーク１１１を介さずに、直接接続される場合もある。
なお、以下、移動体制御システム１００の各部が、通信ネットワーク１１１を介して通信を行う場合、通信ネットワーク１１１の記載を省略するものとする。例えば、入力部１０１と自律移動制御部１１０が、通信ネットワーク１１１を介して通信を行う場合、単に入力部１０１と自律移動制御部１１０が通信を行うと記載する。
入力部１０１は、搭乗者が各種のデータや指示等の入力に用いる装置を備える。例えば、入力部１０１は、タッチパネル、ボタン、マイクロフォン、スイッチ、及び、レバー等の操作デバイス、並びに、音声やジェスチャ等により手動操作以外の方法で入力可能な操作デバイス等を備える。また、例えば、入力部１０１は、赤外線若しくはその他の電波を利用したリモートコントロール装置、又は、移動体制御システム１００の操作に対応したモバイル機器若しくはウェアラブル機器等の外部接続機器であってもよい。入力部１０１は、搭乗者により入力されたデータや指示等に基づいて入力信号を生成し、移動体制御システム１００の各部に供給する。
データ取得部１０２は、移動体制御システム１００の処理に用いるデータを取得する各種のセンサ等を備え、取得したデータを、移動体制御システム１００の各部に供給する。
例えば、データ取得部１０２は、移動体の状態等を検出するための各種のセンサを備えることでセンサ群１１２を構成し、図１のセンサ２１ａ−１乃至２１ａ−ｎより構成されるセンサ群２１に対応する。具体的には、例えば、データ取得部１０２は、ジャイロセンサ、加速度センサ、慣性計測装置（ＩＭＵ）、及び、アクセル等の加速入力の操作量、減速入力の操作量、方向指示入力の操作量、エンジンやモータ等の駆動装置の回転数や入出力エネルギー・燃料量、エンジンやモータ等のトルク量、若しくは、車輪や関節の回転速度やトルク等を検出するためのセンサ等を備える。
また、例えば、データ取得部１０２は、移動体の外部の情報を検出するための各種のセンサを備える。具体的には、例えば、データ取得部１０２は、ＴｏＦ（Time Of Flight）カメラ、ステレオカメラ、単眼カメラ、赤外線カメラ、偏光カメラ、及び、その他のカメラ等の撮像装置を備える。また、例えば、データ取得部１０２は、天候又は気象等を検出するための環境センサ、及び、移動体の周囲の物体を検出するための周囲情報検出センサを備える。環境センサは、例えば、雨滴センサ、霧センサ、日照センサ、雪センサ等からなる。周囲情報検出センサは、例えば、レーザ測距センサ、超音波センサ、レーダ、ＬｉＤＡＲ（Light Detection and Ranging、Laser Imaging Detection and Ranging）、ソナー等からなる。
さらに、例えば、データ取得部１０２は、移動体の現在位置を検出するための各種のセンサを備える。具体的には、例えば、データ取得部１０２は、ＧＮＳＳ（Global Navigation Satellite System）衛星からのＧＮＳＳ信号を受信するＧＮＳＳ受信機等を備える。
通信部１０３は、移動体内部機器１０４、並びに、移動体外部の様々な機器、サーバ、基地局等と通信を行い、移動体制御システム１００の各部から供給されるデータを送信したり、受信したデータを移動体制御システム１００の各部に供給したりする。なお、通信部１０３がサポートする通信プロトコルは、特に限定されるものではなく、また、通信部１０３が、複数の種類の通信プロトコルをサポートすることも可能である。
例えば、通信部１０３は、無線ＬＡＮ、Ｂｌｕｅｔｏｏｔｈ（登録商標）、ＮＦＣ（Near Field Communication）、又は、ＷＵＳＢ（Wireless USB）等により、移動体内部機器１０４と無線通信を行う。また、例えば、通信部１０３は、図示しない接続端子（及び、必要であればケーブル）を介して、ＵＳＢ（Universal Serial Bus）、ＨＤＭＩ（登録商標）（High-Definition Multimedia Interface）、又は、ＭＨＬ（Mobile High-definition Link）等により、移動体内部機器１０４と有線通信を行う。
さらに、例えば、通信部１０３は、基地局又はアクセスポイントを介して、外部ネットワーク（例えば、インターネット、クラウドネットワーク又は事業者固有のネットワーク）上に存在する機器（例えば、アプリケーションサーバ又は制御サーバ）との通信を行う。また、例えば、通信部１０３は、Ｐ２Ｐ（Peer To Peer）技術を用いて、移動体の近傍に存在する端末（例えば、歩行者若しくは店舗の端末、又は、ＭＴＣ（Machine Type Communication）端末）との通信を行う。さらに、例えば、移動体１１が車両の場合、通信部１０３は、車車間（Vehicle to Vehicle）通信、路車間（Vehicle to Infrastructure）通信、移動体と家との間（Vehicle to Home）の通信、及び、歩車間（Vehicle to Pedestrian）通信等のＶ２Ｘ通信を行う。また、例えば、通信部１０３は、ビーコン受信部を備え、道路上に設置された無線局等から発信される電波あるいは電磁波を受信し、現在位置、渋滞、通行規制又は所要時間等の情報を取得する。
移動体内部機器１０４は、例えば、搭乗者が有するモバイル機器若しくはウェアラブル機器、移動体に搬入され若しくは取り付けられる情報機器、及び、任意の目的地までの経路探索を行うナビゲーション装置等を含む。
出力制御部１０５は、移動体の搭乗者又は移動体外部に対する各種の情報の出力を制御する。例えば、出力制御部１０５は、視覚情報（例えば、画像データ）及び聴覚情報（例えば、音声データ）のうちの少なくとも１つを含む出力信号を生成し、出力部１０６に供給することにより、出力部１０６からの視覚情報及び聴覚情報の出力を制御する。具体的には、例えば、出力制御部１０５は、データ取得部１０２の異なる撮像装置により撮像された画像データを合成して、俯瞰画像又はパノラマ画像等を生成し、生成した画像を含む出力信号を出力部１０６に供給する。また、例えば、出力制御部１０５は、衝突、接触、危険地帯への進入等の危険に対する警告音又は警告メッセージ等を含む音声データを生成し、生成した音声データを含む出力信号を出力部１０６に供給する。
出力部１０６は、移動体の搭乗者又は移動体外部に対して、視覚情報又は聴覚情報を出力することが可能な装置を備える。例えば、出力部１０６は、表示装置、インストルメントパネル、オーディオスピーカ、ヘッドホン、搭乗者が装着する眼鏡型ディスプレイ等のウェアラブルデバイス、プロジェクタ、ランプ等を備える。出力部１０６が備える表示装置は、通常のディスプレイを有する装置以外にも、例えば、ヘッドアップディスプレイ、透過型ディスプレイ、ＡＲ（Augmented Reality）表示機能を有する装置等の運転者の視野内に視覚情報を表示する装置であってもよい。尚、出力制御部１０５および出力部１０６は、自律移動の処理には必須の構成ではないため、必要に応じて省略するようにしてもよい。
駆動系制御部１０７は、各種の制御信号を生成し、駆動系システム１０８に供給することにより、駆動系システム１０８の制御を行う。また、駆動系制御部１０７は、必要に応じて、駆動系システム１０８以外の各部に制御信号を供給し、駆動系システム１０８の制御状態の通知等を行う。
駆動系システム１０８は、移動体の駆動系に関わる各種の装置を備える。例えば、駆動系システム１０８は、4本の脚の各関節に備わった角度やトルクを指定可能なサーボモータ、ロボット自体の移動の動きを4本の足の動きに分解・置換するモーションコントローラ並びに、各モータ内のセンサや足裏面のセンサによるフィードバック制御装置を備える。
別の例では、駆動系システム１０８は、4基ないし6基の機体上向きのプロペラを持つモータ、ロボット自体の移動の動きを各モータの回転量に分解・置換するモーションコントローラを備える。
さらに、別の例では、駆動系システム１０８は、内燃機関又は駆動用モータ等の駆動力を発生させるための駆動力発生装置、駆動力を車輪に伝達するための駆動力伝達機構、舵角を調節するステアリング機構、制動力を発生させる制動装置、ＡＢＳ（Antilock Brake System）、ＥＳＣ（Electronic Stability Control）、並びに、電動パワーステアリング装置等を備える。尚、出力制御部１０５、出力部１０６、駆動系制御部１０７、および駆動系システム１０８は、アクチュエータ群１１３を構成し、図１のアクチュエータ２３ａ−１乃至２３ａ−ｎからなるアクチュエータ群２３に対応する。
記憶部１０９は、例えば、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＨＤＤ（Hard Disc Drive）等の磁気記憶デバイス、半導体記憶デバイス、光記憶デバイス、及び、光磁気記憶デバイス等を備える。記憶部１０９は、移動体制御システム１００の各部が用いる各種プログラムやデータ等を記憶する。例えば、記憶部１０９は、ダイナミックマップ等の３次元の高精度地図、高精度地図より精度が低く、広いエリアをカバーするグローバルマップ、及び、移動体の周囲の情報を含むローカルマップ等の地図データを記憶する。
自律移動制御部１１０は、自動運転又は運転支援等の自律移動に関する制御を行う。具体的には、例えば、自律移動制御部１１０は、移動体の衝突回避あるいは衝撃緩和、移動体間距離に基づく追従移動、移動体速度維持移動、または、移動体の衝突警告の機能実現を目的とした協調制御を行う。また、例えば、自律移動制御部１１０は、操作者・ユーザの操作に拠らずに自律的に移動する自律移動等を目的とした協調制御を行う。自律移動制御部１１０は、検出部１３１、自己位置推定部１３２、状況分析部１３３、計画部１３４、及び、動作制御部１３５を備える。このうち、検出部１３１、自己位置推定部１３２、および状況分析部１３３は、認識処理部１２１を構成し、図１の認識処理部３１に対応する。また、計画部１３４は、行動計画処理部１２２を構成し、図１の行動計画処理部３２に対応する。さらに、動作制御部１３５は、行動制御処理部１２３を構成し、図１の行動制御処理部３３に対応する。
検出部１３１は、自律移動の制御に必要な各種の情報の検出を行う。検出部１３１は、移動体外部情報検出部１４１、移動体内部情報検出部１４２、及び、移動体状態検出部１４３を備える。
移動体外部情報検出部１４１は、移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体の外部の情報の検出処理を行う。例えば、移動体外部情報検出部１４１は、移動体の周囲の物体の検出処理、認識処理、及び、追跡処理、並びに、物体までの距離の検出処理を行う。検出対象となる物体には、例えば、移動体、人、障害物、構造物、道路、信号機、交通標識、道路標示等が含まれる。また、例えば、移動体外部情報検出部１４１は、移動体の周囲の環境の検出処理を行う。検出対象となる周囲の環境には、例えば、天候、気温、湿度、明るさ、及び、路面の状態等が含まれる。移動体外部情報検出部１４１は、検出処理の結果を示すデータを自己位置推定部１３２、状況分析部１３３のマップ解析部１５１、及び、状況認識部１５２、並びに、動作制御部１３５等に供給する。
移動体内部情報検出部１４２は、移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体内部の情報の検出処理を行う。例えば、移動体内部情報検出部１４２は、運転者の認証処理及び認識処理、運転者の状態の検出処理、搭乗者の検出処理、及び、移動体内部の環境の検出処理等を行う。検出対象となる運転者の状態には、例えば、体調、覚醒度、集中度、疲労度、視線方向等が含まれる。検出対象となる移動体内部の環境には、例えば、気温、湿度、明るさ、臭い等が含まれる。移動体内部情報検出部１４２は、検出処理の結果を示すデータを状況分析部１３３の状況認識部１５２、及び、動作制御部１３５等に供給する。
移動体状態検出部１４３は、移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体の状態の検出処理を行う。検出対象となる移動体の状態には、例えば、速度、加速度、舵角、異常の有無及び内容、運転操作の状態、パワーシートの位置及び傾き、ドアロックの状態、並びに、その他の移動体搭載機器の状態等が含まれる。移動体状態検出部１４３は、検出処理の結果を示すデータを状況分析部１３３の状況認識部１５２、及び、動作制御部１３５等に供給する。
自己位置推定部１３２は、移動体外部情報検出部１４１、及び、状況分析部１３３の状況認識部１５２等の移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体の位置及び姿勢等の推定処理を行う。また、自己位置推定部１３２は、必要に応じて、自己位置の推定に用いるローカルマップ（以下、自己位置推定用マップと称する）を生成する。自己位置推定用マップは、例えば、ＳＬＡＭ（Simultaneous Localization and Mapping）等の技術を用いた高精度なマップとされる。自己位置推定部１３２は、推定処理の結果を示すデータを状況分析部１３３のマップ解析部１５１、及び、状況認識部１５２等に供給する。また、自己位置推定部１３２は、自己位置推定用マップを記憶部１０９に記憶させる。
さらに、自己位置推定部１３２は、センサ群１１２より供給される検出結果に基づいて、時系列に供給される時系列情報をデータベースに蓄積すると共に、蓄積した時系列の情報に基づいて、自己位置を推定し、時系列情報自己位置として出力する。また、自己位置推定部１３２は、センサ群１１２より供給される現在の検出結果に基づいて、自己位置を推定し、現在情報自己位置として出力する。そして、自己位置推定部１３２は、時系列情報自己位置と、現在情報自己位置とを統合する、または、切り替えることにより自己位置推定結果として出力する。さらに、自己位置推定部１３２は、センサ群１１２より供給される検出結果に基づいて、移動体１１の姿勢を検出し、姿勢の変化が検出されて、自己位置が大きく変化し、時系列情報自己位置の推定精度が低下するとみなされるとき、現在情報自己位置のみから自己位置を推定する。また、例えば、移動体１１が別の移動体に搭載されて移動するような場合、自己位置推定部１３２は、センサ群１１２より供給される検出結果に基づいて、移動体１１の姿勢の変化が検出されなくても、自己位置が大きく変化するので、時系列情報自己位置の推定精度が低下するとみなし、現在情報自己位置のみから自己位置を推定する。これは、例えば、移動体１１が車両であって、カーフェリーボートに搭載されて移動するような場合が考えられる。このようにすることで、外力の影響の有無に関わらず、予め予測できない姿勢の変化があって、自己位置が大きく変化するようなときにでも、現在情報自己位置のみから自己位置が推定されるので、自己位置を所定の精度で推定することができる。尚、自己位置推定部１３２の詳細な構成については、図４を参照して後述する。
状況分析部１３３は、移動体及び周囲の状況の分析処理を行う。状況分析部１３３は、マップ解析部１５１、状況認識部１５２、及び、状況予測部１５３を備える。
マップ解析部１５１は、自己位置推定部１３２及び移動体外部情報検出部１４１等の移動体制御システム１００の各部からのデータ又は信号を必要に応じて用いながら、記憶部１０９に記憶されている各種のマップの解析処理を行い、自律移動の処理に必要な情報を含むマップを構築する。マップ解析部１５１は、構築したマップを、状況認識部１５２、状況予測部１５３、並びに、計画部１３４のルート計画部１６１、行動計画部１６２、及び、動作計画部１６３等に供給する。
状況認識部１５２は、自己位置推定部１３２、移動体外部情報検出部１４１、移動体内部情報検出部１４２、移動体状態検出部１４３、及び、マップ解析部１５１等の移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体に関する状況の認識処理を行う。例えば、状況認識部１５２は、移動体の状況、移動体の周囲の状況、及び、移動体の運転者の状況等の認識処理を行う。また、状況認識部１５２は、必要に応じて、移動体の周囲の状況の認識に用いるローカルマップ（以下、状況認識用マップと称する）を生成する。状況認識用マップは、例えば、占有格子地図（Occupancy Grid Map）、道路地図（Lane Map）、または、点群地図（Point Cloud Map）とされる。
認識対象となる移動体の状況には、例えば、移動体の位置、姿勢、動き（例えば、速度、加速度、移動方向等）、並びに、異常の有無及び内容等が含まれる。認識対象となる移動体の周囲の状況には、例えば、周囲の静止物体の種類及び位置、周囲の動物体の種類、位置及び動き（例えば、速度、加速度、移動方向等）、周囲の道路の構成及び路面の状態、並びに、周囲の天候、気温、湿度、及び、明るさ等が含まれる。認識対象となる運転者の状態には、例えば、体調、覚醒度、集中度、疲労度、視線の動き、並びに、運転操作等が含まれる。
状況認識部１５２は、認識処理の結果を示すデータ（必要に応じて、状況認識用マップを含む）を自己位置推定部１３２及び状況予測部１５３等に供給する。また、状況認識部１５２は、状況認識用マップを記憶部１０９に記憶させる。
状況予測部１５３は、マップ解析部１５１、及び状況認識部１５２等の移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体に関する状況の予測処理を行う。例えば、状況予測部１５３は、移動体の状況、移動体の周囲の状況、及び、運転者の状況等の予測処理を行う。
予測対象となる移動体の状況には、例えば、移動体の挙動、異常の発生、及び、移動可能距離等が含まれる。予測対象となる移動体の周囲の状況には、例えば、移動体の周囲の動物体の挙動、信号の状態の変化、及び、天候等の環境の変化等が含まれる。予測対象となる運転者の状況には、例えば、運転者の挙動及び体調等が含まれる。
状況予測部１５３は、予測処理の結果を示すデータを、及び状況認識部１５２からのデータとともに、計画部１３４のルート計画部１６１、行動計画部１６２、及び、動作計画部１６３等に供給する。
ルート計画部１６１は、マップ解析部１５１及び状況予測部１５３等の移動体制御システム１００の各部からのデータ又は信号に基づいて、目的地までのルートを計画する。例えば、ルート計画部１６１は、グローバルマップに基づいて、現在位置から指定された目的地までのルートを設定する。また、例えば、ルート計画部１６１は、渋滞、事故、通行規制、工事等の状況、及び、運転者の体調等に基づいて、適宜ルートを変更する。ルート計画部１６１は、計画したルートを示すデータを行動計画部１６２等に供給する。
行動計画部１６２は、マップ解析部１５１及び状況予測部１５３等の移動体制御システム１００の各部からのデータ又は信号に基づいて、ルート計画部１６１により計画されたルートを計画された時間内で安全に移動するための移動体の行動を計画する。例えば、行動計画部１６２は、発進、停止、進行方向（例えば、前進、後退、左折、右折、方向転換等）、移動速度、及び、追い越し等の計画を行う。行動計画部１６２は、計画した移動体の行動を示すデータを動作計画部１６３等に供給する。
より詳細には、行動計画部１６２は、それぞれルート計画部１６１により計画されたルートのそれぞれについて、計画された時間内で安全に移動するための移動体の行動計画の候補を行動計画候補として生成する。より具体的には、行動計画部１６２は、例えば、環境を格子状に区切って、到達判定および経路の重みを最適化して最良のパスを生成するA*algorithm（A star探索アルゴリズム）、道路中心線に従って経路を設定するLane algorithm、および、自己位置からインクリメンタルに到達可能な場所へのパスを適切に枝刈りしながら伸ばしていくRRT(Rapidly-exploring Random Tree) algorithmなどにより行動計画候補を生成する。
動作計画部１６３は、マップ解析部１５１及び状況予測部１５３等の移動体制御システム１００の各部からのデータ又は信号に基づいて、行動計画部１６２により計画された行動を実現するための移動体の動作を計画する。例えば、動作計画部１６３は、加速、減速、及び、移動軌道等の計画を行う。動作計画部１６３は、計画した移動体の動作を示すデータを、動作制御部１３５等に供給する。
動作制御部１３５は、移動体の動作の制御を行う。
より詳細には、動作制御部１３５は、移動体外部情報検出部１４１、移動体内部情報検出部１４２、及び、移動体状態検出部１４３の検出結果に基づいて、衝突、接触、危険地帯への進入、運転者の異常、移動体の異常等の緊急事態の検出処理を行う。動作制御部１３５は、緊急事態の発生を検出した場合、急停止や急旋回等の緊急事態を回避するための移動体の動作を計画する。
また、動作制御部１３５は、動作計画部１６３により計画された移動体の動作を実現するための加減速制御を行う。例えば、動作制御部１３５は、計画された加速、減速、又は、急停止を実現するための駆動力発生装置又は制動装置の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部１０７に供給する。
動作制御部１３５は、動作計画部１６３により計画された移動体の動作を実現するための方向制御を行う。例えば、動作制御部１３５は、動作計画部１６３により計画された移動軌道又は急旋回を実現するためのステアリング機構の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部１０７に供給する。
＜認識処理部の詳細な構成例＞
次に、図４を参照して、図３の移動体制御システム１００のうち、認識処理部３１に対応する自律移動制御部１１０の認識処理部１２１と、その周辺の具体的な構成例について説明する。
尚、ここでは、移動体が車輪型移動ロボットからなる移動体１１である例について説明するが、その他のロボットや車両などの移動体であってもよい。
センサ群１１２は、LIDAR２０１、車輪エンコーダ２０２、ステレオカメラ２０３、および内界センサ２０４を備えているものとする。当然のことながら、４足歩行ロボットや車両等であれば、その他のセンサを使用するようにしてもよい。
LIDAR２０１は、移動体１１の周囲の物体までの距離を３次元点群データとして取得し、検出部１３１を介して、自己位置推定部１３２の時系列情報自己位置推定部２２１に時系列情報として出力する。
車輪エンコーダ２０２は、移動体１１の位置情報（Ｘ，Ｙ，Ｚ）、姿勢（クォータニオン）、速度（ｄｘ，ｄｙ，ｄｚ）、加速度（ａｘ，ａｙ，ａｚ）、および角速度（ｗｘ，ｗｙ，ｗｚ）を検出し、検出部１３１を介して、時系列情報自己位置推定部２２１に時系列情報として出力する。
ステレオカメラ２０３は、移動体１１の周囲の視差画像を撮像して、検出部１３１を介して、現在情報自己位置推定部２２３に現在情報として出力する。
内界センサ２０４は、加速度センサ、ジャイロセンサ、および地磁気センサなどの総称であり、移動体１１の加速度、角度、角速度、および地磁気方向等を検出し、検出部１３１を介して、自己位置推定部１３２に出力する。
自己位置推定部１３２は、時系列情報自己位置推定部２２１、時系列情報DB２２２、現在情報自己位置推定部２２３、位置画像特徴量DB２２４、自己位置推定結果切替部２２５、自己姿勢検出部２２６、特徴量姿勢DB２２７、および姿勢変化検出部２２８を備えている。
時系列情報自己位置推定部２２１は、LIDAR２０１および車輪エンコーダ２０２より供給される、自己位置、および周囲の障害物の位置を表現する３次元点群データ等の時系列情報を時系列情報DB２２２に格納する。また、時系列情報自己位置推定部２２１は、時系列情報DB２２２より、過去から現在までの時系列情報を必要に応じて読み出し、読み出した過去から現在までの時系列情報に基づいて自己位置を推定し、自己位置推定結果切替部２２５に供給する。
尚、時系列情報自己位置推定部２２１による、SLAM（Simultaneous Localisation and Mapping）を用いた具体的な自己位置推定方法については、「拡張カルマンフィルタを用いた移動ロボットの自己位置推定と環境認識」森本祐介，正滑川徹、「Simultaneous Localisation and Mapping (SLAM): Part I The Essential Algorithms Hugh Durrant-Whyte, Fellow, IEEE, and Tim Bailey」、および「Simultaneous Localisation and Mapping (SLAM): Part II State of the Art Tim Bailey and Hugh Durrant-Whyte」を参照されたい。尚、時系列情報自己位置推定部２２１により時系列情報に基づいて推定される自己位置を時系列情報自己位置と称する。
現在情報自己位置推定部２２３は、ステレオカメラ２０３より供給される視差画像に基づいて、デプス画像（距離画像）を生成し、デプス画像より画像特徴量を抽出する。そして、現在情報自己位置推定部２２３は、位置と画像特徴量とが対応付けて記憶されている位置画像特徴量DB２２４より、抽出した特徴量に対応する位置の情報に基づいて、自己位置を推定し、自己位置推定結果切替部２２５に供給する。尚、現在情報自己位置推定部２２３により現在情報に基づいて推定される自己位置を現在情報自己位置と称する。
自己姿勢検出部２２６は、内界センサ２０４より供給される検出結果に基づいて、移動体１１の自己の姿勢を検出して姿勢変化検出部２２８に出力する。
より詳細には、自己姿勢検出部２２６は、周波数成分抽出部２７１、特徴量抽出部２７２、および姿勢検出部２７３を備えている。
周波数成分抽出部２７１は、内界センサ２０４の検出結果に基づいて、移動体１１の振動の周波数成分を、例えば、FFT（Fast Fourier Transform）により抽出し、特徴量抽出部２７２に出力する。
特徴量抽出部２７２は、周波数成分抽出部２７１より供給された移動体１１の振動の周波数成分より特徴量を抽出して姿勢検出部２７３に出力する。
姿勢検出部２７３は、予め学習により特徴量と姿勢とが対応付けて登録されている特徴量姿勢DB２２７にアクセスし、抽出された特徴量に基づいて、対応付けて登録されている姿勢を抽出し、現在の移動体１１の姿勢の情報として、姿勢変化検出部２２８に出力する。姿勢検出部２７３が特徴量に基づいて検出する姿勢の情報は、少なくとも通常歩行状態、抱き上げ、および静止状態の３種類の姿勢を含むものである。
より具体的には、移動体１１が走行している状態と、突然持ち上げられてしまう、いわゆる、抱き上げ状態（kidnap状態）とは、例えば、鉛直方向の加速度と、その周波数解析結果であるパワースペクトルとに基づいて、識別することができる。
図５の上段は、鉛直方向の加速度を示しており、内界センサ２０４の出力が20Hzであるときの1.6秒分、すなわち、３２要素のついての変位を示しており、横軸が時間であり、縦軸が加速度である。
また、図５の下段は、左からｘ方向（ｘ）、ｙ方向（ｙ）、ｚ（ｚ）方向の加速度であり、ｘ方向（ｒｘ）、ｙ方向（ｒｙ）、ｚ方向（ｚｙ）のそれぞれの回転速度のパワースペクトルであり、横軸が周波数（Hz：10Hz分ずつ）であり、縦軸がそれぞれのパワースペクトルである。ここでは、ｘ，ｙ方向が水平方向の座標系であり、ｚ方向が垂直（鉛直）方向の座標系である。
さらに、図５の左部の上段および下段は、いずれも抱き上げ状態（kidnap状態）における波形を示しており、図５の右部の上段および下段は、いずれも走行状態における波形を示している。
図５の上段で示されるような波形に、FFTを用いた周波数解析が掛けられることにより、図５の下段で示されるような波形となる。
走行状態であるときには、図５の右上段で示されるように、鉛直方向の加速度には変化がないことが示されている。これに対して、抱き上げ状態ときには、図５の左上段で示されるように、0.4秒付近において小さくなった後、1.1秒乃至1.4秒の範囲で大きく変化している。
この結果、パワースペクトルにおいては、特に、鉛直方向であるｚ方向大きな変化が現れて、走行状態においては、全範囲において平均してほぼ1.0以下程度であるが、抱き上げ状態においては、2.5Hzまでの範囲では平均してほぼ2.5を超える大きな値となる。また、抱き上げ状態においては、ｘ，ｙ方向の2.5Hzまでの範囲において、平均してほぼ1.0以上となり、5Hz乃至10Hzまでの範囲において、平均がほぼ2.0以下となる。これに対して、走行状態においては、ｘ，ｙ方向の2.5Hzまでの範囲において、平均してほぼ2.0以上となり、5Hz乃至10Hzまでの範囲において、平均して1.0以下となる。
これにより、抱き上げ状態については、ｚ方向に対して、2.5Hzまでの範囲でパワースペクトルが2.5以上で、ｘ，ｙ方向の2.5Hzまでの範囲において、平均してほぼ1.0以上となり、5Hz乃至10Hzまでの範囲において、平均がほぼ2.0以下となる。
すなわち、このような、ｘ，ｙ，ｚ方向のそれぞれの周波数帯域ごとのパワースペクトルの分布が、抱き上げ状態の特徴量となる。
尚、姿勢の検出方法は、これ以外の方法でもよく、例えば、「加速度センサとジャイロセンサを用いたジェスチャ認識、板口貴司 金森務 片寄晴弘 佐藤宏介 井口征士著」および「ユーザの身体的特徴情報を用いた行動認識モデルの学習手法、前川卓也 渡部 晋治著」を参照されたい。
特徴量姿勢DB２２７には、このように特徴量と、姿勢とが対応付けて登録されている。そこで、姿勢検出部２７３は、特徴量姿勢DB２２７にアクセスし、周波数解析結果より得られる特徴量に対応付けて登録されている姿勢を検出する。
尚、特徴量姿勢DB２２７を生成する特徴量姿勢DB生成部３０１（図６）については、図６を参照して詳細を後述する。
姿勢変化検出部２２８は、自己姿勢検出部２２６より出力される姿勢情報を取得して、姿勢の変化の有無を検出し、検出結果を自己位置推定結果切替部２２５に出力する。より詳細には、姿勢変化検出部２２８は、移動体１１の少なくとも通常歩行状態、抱き上げ、および静止状態の３種類の姿勢の変化を検出し、検出結果を自己位置推定結果切替部２２５に出力する。
自己位置推定結果切替部２２５は、時系列情報自己位置推定部２２１より供給される時系列情報自己位置と、現在情報自己位置推定部２２３より供給される現在情報自己位置とを切り替える、または、統合して自己位置推定結果として、状況分析部１３３に出力する。
また、自己位置推定結果切替部２２５は、姿勢変化判定部２５１を備えており、姿勢変化検出部２２８より姿勢の変化が検出されたことが通知されるか否かを判定し、姿勢の変化が検出されたことを通知されると、時系列情報DB２２２に格納されている時系列情報をリセットして全て消去する。
＜特徴量姿勢DB生成部の構成例＞
次に、図６を参照して、図４の特徴量姿勢DB２２７を生成する特徴量姿勢DB生成部３０１の構成例について説明する。
特徴量姿勢DB生成部３０１は、過去の姿勢がわかっている内界センサ２０４の検出結果が登録された姿勢DB３０２の情報に基づいて、姿勢毎に検出結果より抽出される特徴量との関係を学習して、特徴量姿勢DB２２７に登録する。
より詳細には、特徴量姿勢DB生成部３０１は、周波数成分抽出部３１１、特徴量抽出部３１２、および特徴量姿勢学習部３１３を備えている。
周波数成分抽出部３１１は、周波数成分抽出部２７１に対応する構成であり、過去の姿勢がわかっている内界センサ２０４の検出結果を姿勢DB３０２より読み出す。そして、周波数成分抽出部３１１は、読み出した検出結果である振動の周波数成分を、例えば、FFT（Fast Fourier Transform）を掛けることにより解析して抽出し、姿勢の情報と共に特徴量抽出部３１２に出力する。
特徴量抽出部３１２は、特徴量抽出部２７２に対応する構成であり、周波数成分抽出部３１１より供給される周波数成分より特徴量を抽出して、姿勢の情報と共に特徴量姿勢学習部３１３に出力する。
特徴量姿勢学習部３１３は、予めわかっている姿勢と対応する特徴量との関係を学習し、学習結果として得られた、姿勢と特徴量との関係を対応付けて、特徴量姿勢DB２２７に登録する。
＜時系列情報自己位置推定処理＞
次に、図７のフローチャートを参照して、時系列情報自己位置推定処理について説明する。
ステップＳ１１において、LIDAR２０１は、移動体１１の周囲の物体までの距離を３次元点群データとして取得し、検出部１３１を介して、自己位置推定部１３２の時系列情報自己位置推定部２２１に時系列情報として出力する。
ステップＳ１２において、車輪エンコーダ２０２は、移動体１１の位置情報、姿勢、速度、加速度、および角速度を検出し、検出部１３１を介して、時系列情報自己位置推定部２２１に時系列情報として出力する。
ステップＳ１３において、時系列情報自己位置推定部２２１は、最新の時系列情報を取得し、時系列情報DB２２２に登録する。
ステップＳ１４において、時系列情報自己位置推定部２２１は、時系列情報DB２２２に登録されている最新の時系列情報と、過去の時系列情報とから、例えば、SLAMにより自己位置を推定し、推定結果を時系列情報自己位置情報として自己位置推定結果切替部２２５に出力する。
ステップＳ１５において、時系列情報自己位置推定部２２１は、処理の終了が指示されたか否かを判定し、終了ではない場合、処理は、ステップＳ１１に戻る。
そして、ステップＳ１５において、処理の終了が指示された場合、処理は、終了する。
以上の処理により、LIDAR２０１および車輪エンコーダ２０２により検出される時系列情報が順次検出されて、時系列情報DB２０６に順次蓄積されて、時系列に蓄積された過去から現在までの時系列に蓄積された時系列情報に基づいて、時系列情報自己位置が推定されて、繰り返し自己位置推定結果切替部２２５に出力される。
＜現在情報自己位置推定処理＞
次に、図８のフローチャートを参照して、現在情報自己位置推定処理について説明する。
ステップＳ３１において、ステレオカメラ２０３は、移動体１１の周囲の視差画像を撮像して、検出部１３１を介して、現在情報自己位置推定部２２３に現在情報として出力する。
ステップＳ３２において、現在情報自己位置推定部２２３は、ステレオカメラ２０３より供給される視差画像に基づいて、デプス画像（距離画像）を生成する。
ステップＳ３３において、現在情報自己位置推定部２２３は、生成したデプス画像より、画像特徴量を抽出する。
ステップＳ３４において、現在情報自己位置推定部２２３は、位置と画像特徴量とが対応付けて記憶されている位置画像特徴量DB２２４より、抽出した画像特徴量に対応する位置の情報を検出することで自己位置を推定し、推定結果を現在情報自己位置として自己位置推定結果切替部２２５に供給する。
ステップＳ３５において、現在情報自己位置推定部２２３は、処理の終了が指示されたか否かを判定し、終了ではない場合、処理は、ステップＳ３１に戻る。
そして、ステップＳ３５において、処理の終了が指示された場合、処理は、終了する。
以上の処理により、ステレオカメラ２０３により視差画像が現在情報として撮像され、現在情報としての視差画像よりデプス画像が生成され、生成されたデプス画像の画像特徴量が抽出されて、画像特徴量に対応付けて登録されている位置情報に基づいて、現在情報自己位置が推定され、繰り返し自己位置推定結果切替部２２５に出力される。
＜姿勢変化検出処理＞
次に、図９のフローチャートを参照して、姿勢変化検出処理について説明する。
ステップＳ５１において、内界センサ２０４が、移動体１１の加速度、角度、角速度、および地磁気方向等を検出し、検出部１３１を介して、自己位置推定部１３２の自己姿勢検出部２２６に出力する。
ステップＳ５２において、自己姿勢検出部２２６の周波数成分抽出部２７１は、内界センサ２０４により検出された加速度、角度、角速度、および地磁気方向の検出結果に、FFTを掛けて周波数解析し、解析結果を特徴量抽出部２７２に出力する。
ステップＳ５３において、特徴量抽出部２７２は、周波数解析結果より特徴量を抽出して、姿勢検出部２７３に出力する。
ステップＳ５４において、姿勢検出部２７３は、図５を参照して説明したように、特徴量姿勢DB２２７にアクセスし、特徴量に基づいて、対応する姿勢の情報を検索して、検索結果を現在の姿勢として検出し、姿勢変化検出部２２８に出力する。
ステップＳ５５において、姿勢変化検出部２２８は、自己姿勢検出部２２６より供給される、新たな現在の姿勢の情報を記憶すると共に、自己位置推定結果切替部２２５に通知する。
ステップＳ５６において、姿勢変化検出部２２８は、自己姿勢検出部２２６より供給された、新たな現在の姿勢の情報と、直前までに記憶されていた姿勢の情報とを比較し、姿勢に変化が生じたか否かを判定する。
ステップＳ５６において、直前までの姿勢と、現在の姿勢とが異なり変化が生じているとみなされた場合、処理は、ステップＳ５７に進む。
ステップＳ５７において、姿勢変化検出部２２８は、姿勢が変化したことを自己位置推定結果切替部２２５に通知する。
一方、ステップＳ５６において、姿勢の変化が検出されない場合、ステップＳ５７の処理がスキップされる。
ステップＳ５８において、自己姿勢検出部２２６は、処理の終了が指示されたか否かを判定し、終了ではない場合、処理は、ステップＳ５１に戻る。すなわち、終了が指示されるまで、ステップＳ５１乃至Ｓ５８の処理が繰り返されて、自己姿勢が検出されると共に、変化の有無が検出され続ける。
そして、ステップＳ５８において、処理の終了が指示された場合、処理は、終了する。
以上の処理により、移動体１１の姿勢が検出されると共に、検出された姿勢の情報が自己位置推定結果切替部２２５に通知される。また、検出された姿勢の情報が順次記憶され、検出された現在の姿勢の情報と直前までに記憶されている姿勢との比較により、姿勢が変化しているか否かが判定されて、姿勢変化が検出された場合、姿勢変化が検出されたことが、自己位置推定結果切替部２２５に通知される。
＜自律移動制御処理＞
次に、図１０のフローチャートを参照して、図４の移動体制御システム１００における自律移動制御処理について説明する。
ステップＳ７１において、自己位置推定結果切替部２２５の姿勢変化判定部２５１は、姿勢変化検出処理により検出される、抱き上げ状態（kidnap状態）であることを示す姿勢に変化する姿勢変化が検出されたことが通知されたか否かを判定する。
ステップＳ７１において、抱き上げ状態（kidnap状態）であることを示す姿勢に変化する姿勢変化が検出された場合、処理は、ステップＳ７２に進む。
ステップＳ７２において、自己位置推定結果切替部２２５は、移動体１１を所定時間だけ停止するように行動制御処理部１２３に対して指示を出力する。これにより、行動制御処理部１２３は、アクチュエータ群１１３を制御して、所定時間だけ移動体１１の動作を停止させる。
ステップＳ７３において、自己位置推定結果切替部２２５は、所定時間が経過し、動作の停止が解除された後、時系列情報DB２２２に登録されている過去の時系列情報をリセットする。
尚、ステップＳ７１において、姿勢変化が検出されていないとみなされた場合、ステップＳ７２，Ｓ７３の処理はスキップされる。すなわち、この場合、時系列情報DB２２２に登録されている過去の時系列情報は、リセットされることなく、登録された状態が維持される。また、抱き上げ状態が検出されると、過去の時系列情報を用いて自己位置を推定することができないので、時系列情報DB２２２に登録されている時系列情報の情報が全て消去されてリセットされる。
ステップＳ７４において、自己位置推定結果切替部２２５は、時系列情報自己位置推定部２２１の推定結果である時系列情報自己位置を、自己位置推定結果として状況分析部１３３に出力する。
ステップＳ７５において、状況分析部１３３は、自己位置の推定結果に基づいて、自己位置の周囲の状況を分析し、分析結果を行動計画処理部１２２に出力する。
ステップＳ７６において、行動計画処理部１２２は、自己位置の推定結果に基づいた周囲の分析結果により、目的地までのルートを計画し、計画されたルートに基づいて、行動計画を決定し、アクチュエータ群１１３を制御するための動作計画を生成し、行動制御処理部１２３に出力する。
ステップＳ７７において、行動制御処理部１２３は、動作計画部１６３より供給される動作計画に基づいて、アクチュエータ群１１３を制御する。
ステップＳ７８において、自律移動制御部１１０は、終了が指示されたか否かを判定し、終了が指示されていない場合、処理は、ステップＳ７１に戻る。すなわち、ステップＳ７１乃至Ｓ７８の処理が繰り返される。そして、ステップＳ７８において、終了が指示された場合、処理は、終了する。
以上の処理により、外力の影響などにより抱き上げ状態になる姿勢変化が検出されていない場合は、時系列情報DB２２２に登録されている過去の時系列情報はリセットされることがないので、信頼性の高い現在から過去に至る全ての時系列情報を利用して推定される時系列情報自己位置が自己位置推定結果として出力される。
これに対して、外力の影響などにより抱き上げ状態になるという予め予測できない姿勢変化が検出された場合は、時系列情報DB２２２に登録されている、比較的信頼性の低い、過去の時系列情報がリセットされて消去され、抱き上げ状態に姿勢変化が検出されたタイミングの後のタイミングからの、比較的新しい時系列情報を利用して推定された時系列情報自己位置が自己位置推定結果として出力される。
すなわち、外力の影響などにより抱き上げ状態になるという予め予測できない姿勢変化の検出の有無により、過去から現在に至るまでの全ての時系列情報を用いて時系列情報自己位置を推定する第１の推定モデルと、抱き上げ状態になる姿勢変化が検出されたタイミングで時系列情報DB２２２がリセットされた後の比較的新しい時系列情報を用いて時系列情報自己位置を推定する第２の推定モデルとが切り替えられる。
このため、抱き上げ状態などの外力による姿勢の変化が検出されない場合には、時系列情報DB２２２に、過去から現在に至るまでの、時系列に連続性が高い時系列情報過去から現在に至るまでの時系列情報を用いて、高精度に時系列情報自己位置を推定することができる。
一方、抱き上げ状態などの予め予測できない姿勢の変化が検出された場合には、時系列情報DB２２２に登録される時系列情報がリセットされるので、抱き上げなどの外力などにより、直前までの自己位置が変化することで、使用することができない過去の時系列情報を除いた、比較的新しい、抱き上げ状態以降の信頼性の高い時系列情報に基づいて、時系列情報自己位置を推定することが可能となる。また、時系列情報DB２２２に登録される時系列情報は、リセット後も時間の経過に従って、徐々に増えることにより、信頼性が向上していく。
結果として、予め予測できない姿勢変化の有無によらず、信頼性の高い時系列情報自己位置を推定させることが可能となる。
尚、以上においては、姿勢変化として、いずれかの姿勢から抱き上げ状態が検出されるときに、時系列情報DB２２２がリセットされる例について説明してきたが、時系列情報DB２２２の時系列情報がリセットされるタイミングは、抱き上げ状態以外であってもよい。例えば、舗装道路、砂利道、雪道、雨でぬれている路面、階段、平面、線路不整備走行路などの路面状態を検出して、例えば、雪道、および砂利道などであるときの４足歩行ロボットにおける足裏接地面のスリップや、車輪型ロボットにおける車輪のスリップが検出されたタイミング、または、非固定物体の乗り降りが検出されたタイミングでもよい。また、センサの検出結果を統計処理することにより、検出結果の変化に基づいて、センサの異常を検出し、環境変化（電磁ノイズ、モノの影）により視野が狭くなる、または、無くなるなどが検出されるタイミングでもよい。さらに、移動体１１が車両などの場合、カーフェリーボートに搭載されて移動することにより、乗船前後の自己位置や姿勢が所定値よりも大きく変化していることが検出されるタイミングでもよい。また、移動体１１が車両などの場合であって、タイヤがパンクすることにより、エンジンが停止し、パンクの修理前後などで、自己位置や姿勢が所定値よりも大きく変化していることが検出されるタイミングでもよい。すなわち、自己位置や姿勢の、時系列の変化における連続性が低下する状況（連続性がなくなる状態）等の、予め事前に予測できない姿勢変化が検出されるタイミングであれば、他のタイミングであってもよい。
また、ステップＳ７４においては、時系列情報自己位置推定部２２１により推定される時系列情報自己位置が自己位置として採用される例について説明してきたが、時系列情報自己位置と、現在情報自己位置との両方をカルマンフィルタや粒子フィルタなどにより統合した結果を、自己位置推定結果として出力するようにしてもよい。
さらに、以上においては、周波数成分の特徴量を利用して姿勢を検出する例について説明してきたが、姿勢を確率または確度にとして求めるようにしてもよい。この場合、複数の姿勢変化に対して、それぞれの確率あるいは確度に基づいた重みを利用して、それぞれの姿勢であるときの結果を、融合するようにしてもよい。
また、以上においては、移動体１１が車輪型移動ロボットである場合に、時系列情報として、LIDAR２０１と車輪エンコーダ２０２により検出される情報を用い、現在情報としてステレオカメラ２０３により撮像される視差画像を用いる例について説明してきた。
しかしながら、時系列情報および現在情報については、これに限らず、移動体１１の形態に合わせたものとしてもよい。すなわち、移動体１１がドローンである場合には、時系列情報を、LIDARとIMU（慣性計測装置）や加速度センサにより検出される情報にして、現在情報をGNSSにより取得される情報にしてもよい。
また、移動体１１が車両である場合には、時系列情報を、LIDARにより検出される情報にして、現在情報をGNSSにより取得される情報にしてもよい。
さらに、移動体１１が多足歩行ロボットである場合には、時系列情報を、LIDAR、IMU（慣性計測装置）、およびアクチュエータに付属のセンサにより検出される情報にして、現在情報をステレオカメラ、IMU（慣性計測装置）、およびアクチュエータに付属のセンサにより取得される情報にしてもよい。
また、ステレオカメラに代えて、LIDAR、およびToFセンサを用いるようにしてもよい。
さらに、以上においては、自己姿勢検出部２２６による自己姿勢の検出には、内界センサ２０４（加速度センサとジャイロセンサとの組み合わせ）の検出結果が用いられ、現在情報自己位置推定部２２３における現在情報自己位置推定には、ステレオカメラ２０３による視差画像が用いられ、時系列情報自己位置推定部２２１における時系列情報自己位置推定には、LIDAR２０１と車輪エンコーダ２０２センシング結果が用いられてきた。
しかしながら、自己姿勢検出部２２６において、自己姿勢が検出でき、現在情報自己位置推定部２２３において現在情報自己位置推定ができ、時系列情報自己位置推定部２２１において時系列情報自己位置推定ができれば、それぞれに供給されるセンシング結果は他のセンサによる検出結果でもよい。
また、時系列情報自己位置推定部２２１、現在情報自己位置推定部２２３、および自己姿勢検出部２２６のそれぞれにLIDAR２０１のセンシング結果である、３次元点群情報が供給されるようにして、それぞれ３次元点群情報に基づいて、自己姿勢検出、現在情報自己位置推定、および時系列情報自己位置推定が実現されるようにしてもよい。さらに、時系列情報自己位置推定部２２１、現在情報自己位置推定部２２３、および自己姿勢検出部２２６にステレオカメラ２０３のセンシング結果である、視差画像が供給されるようにして、視差画像に基づいて、それぞれ自己姿勢検出、現在情報自己位置推定、および時系列情報自己位置推定が実現されるようにしてもよい。
＜特徴量姿勢DB学習処理＞
次に、図１１のフローチャートを参照して、特徴量姿勢DB学習処理について説明する。
ステップＳ９１において、周波数成分抽出部３１１は、過去の姿勢がわかっている内界センサ２０４の検出結果を姿勢DB３０２より読み出して、振動の周波数成分を、例えば、FFT（Fast Fourier Transform）により解析して特徴量抽出部３１２に出力する。
ステップＳ９２において、特徴量抽出部３１２は、周波数成分抽出部３１１より供給される周波数成分より特徴量を抽出して特徴量姿勢学習部３１３に出力する。
ステップＳ９３において、特徴量姿勢学習部３１３は、予めわかっている姿勢と対応する特徴量との関係を、例えば、ニューラルネットワークなどを利用して学習する。
ステップＳ９４において、特徴量姿勢学習部３１３は、学習結果として得られた、姿勢と周波数成分の特徴量との関係を対応付けて、特徴量姿勢DB２２７に登録する。
以上の処理により、周波数成分の特徴量と姿勢とが対応付けられて特徴量姿勢DB２２７に登録される。結果として、周波数成分の特徴量から姿勢を検出することが可能となる。
＜＜３．第１の変形例＞＞
以上においては、外力の影響により自己位置が不定である場合においては、時系列情報DB２２２に登録されている時系列情報をリセットする例について説明してきたが、自己位置が不定となった直後においては時系列情報の信頼度が低いことが予想される。そこで、自己位置が不定となり、時系列情報をリセットした後は、時系列情報の信頼度が所定値より高くなるまでは、現在情報自己位置を自己位置推定結果として採用するようにしてもよい。
＜第１の変形例の自律移動制御処理＞
次に、図１２のフローチャートを参照して、図４の移動体制御システム１００における時系列情報の信頼度が所定値より高くなるまでは、現在情報自己位置を自己位置推定結果として採用するようにするようにした自律移動制御処理について説明する。
時系列情報の信頼度は、例えば、時系列情報として蓄積されている情報数に基づいて設定される値としてもよい。すなわち、時系列情報自己位置は、過去の情報から現在の情報までの複数の情報を用いることにより推定されるものであるので、自己位置推定の利用可能な情報数が信頼度の指標となる。
時系列情報の信頼度の別の例は、たとえば、時系列情報に基づいて算出した過去ｘ秒間の自己位置推定の結果と、実際の過去ｘ秒間の自己位置の距離差の時間平均の逆数としていもよい。
尚、図１２のフローチャートにおけるステップＳ１０１乃至Ｓ１０８の処理は、図１０のフローチャートにおけるステップＳ７１乃至Ｓ７８の処理と同様であるので、その説明は省略する。
ステップＳ１０１において、外力の影響により抱き上げ状態を示す姿勢に姿勢が変化したと判定された場合、換言すれば、予め予測できない姿勢変化が検出されたと判定された場合、処理は、ステップＳ１０９に進む。
ステップＳ１０９において、自己位置推定結果切替部２２５は、例えば、リセットがなされた後に、新たに時系列情報DB２２２に登録された時系列情報の情報数が所定数よりも多いか否かにより、時系列情報の信頼度が所定の閾値よりも高いか否かを判定する。
ステップＳ１０９において、時系列情報DB２２２に登録されている時系列情報の情報数が所定数よりも多く、信頼度が所定値よりも高いとみなされた場合、処理は、ステップＳ１０４に進む。
すなわち、この場合、時系列情報DB２２２に登録されている時系列情報に信頼度が所定値よりも高いので、ステップＳ１０４において、時系列情報自己位置が自己位置推定結果として採用される。
一方、ステップＳ１０９において、リセットが掛けられた直後に近いタイミングなどで、時系列情報DB２２２に登録されている時系列情報の情報数が所定数よりも少なく、信頼度が所定値よりも低いとみなされた場合、処理は、ステップＳ１１０に進む。
ステップＳ１１０において、自己位置推定結果切替部２２５は、現在情報自己位置推定部２２３の推定結果である現在情報自己位置を、自己位置推定結果として状況分析部１３３に出力する。
以上の処理により、外力の影響などにより抱き上げ状態になる姿勢変化が検出されていない場合は、時系列情報DB２２２に登録されている過去の時系列情報はリセットされることがないので、信頼性の高い現在から過去に至る全ての時系列情報を利用して推定される時系列情報自己位置が自己位置推定結果として出力される。
これに対して、外力の影響などにより抱き上げ状態になる姿勢変化が検出された場合は、時系列情報DB２２２に登録されている、比較的信頼性の低い、過去の時系列情報がリセットされて消去されると同時に、現在情報を利用して推定された現在情報自己位置が自己位置推定結果として出力される。
すなわち、外力の影響などにより抱き上げ状態になる姿勢変化の検出の有無により、過去から現在に至るまでの全ての時系列情報を用いて時系列情報自己位置を推定する第１の推定モデルと、抱き上げ状態になる姿勢変化が検出されたタイミング以降の現在情報を用いて現在情報自己位置を推定する第２の推定モデルとが切り替えられる。
このため、抱き上げ状態などの外力による姿勢の変化が検出されない場合には、時系列情報DB２２２に、過去から現在に至るまでの、時系列に連続性が高い時系列情報過去から現在に至るまでの時系列情報を用いて、高精度に時系列情報自己位置を推定することができる。
一方、抱き上げ状態などの外力による姿勢の変化が検出された場合には、信頼性の高い現在情報に基づいて、現在情報自己位置を推定することが可能となる。
さらに、時系列情報DB２２２に登録される時系列情報がリセットされた後、所定時間が経過して時系列情報が所定数よりも多く登録されて、信頼性が復帰すると、推定モデルは、過去から現在に至るまでの全ての時系列情報を用いて時系列情報自己位置を推定する第１の推定モデルに復帰する。
いずれにおいても、結果として、予め予測できない姿勢変化の有無によらず、信頼性の高い時系列情報自己位置を推定させることが可能となる。
すなわち、時系列情報DB２２２に登録された時系列情報がリセットされた後においても、信頼度が所定値よりも高い状態になるまでは、現在情報自己位置が自己位置推定結果として採用することが可能となり、信頼度の低い時系列情報に基づいた、自己位置推定結果による推定精度の極端な低下を防止することが可能となる。
尚、以上においては、時系列情報がリセットされた後は、時系列情報の信頼度が所定値よりも高い状態になるまで、現在情報自己位置が自己位置推定結果として採用する例について説明してきた。
しかしながら、時系列情報自己位置が、事前確率密度分布の観測値を最新の時系列情報により更新するステップを繰り返すカルマンフィルタが用いられて推定されるような場合、姿勢の変化が検出されたときに、最新の時系列情報の重みを最大にした後、通常のカルマンフィルタを用いて、時系列情報自己位置を推定するようにしてもよい。
また、時系列情報がリセットされた後は、時系列情報DB２２２に登録された時系列情報のうち、最新の時系列情報について推定される自己位置の重みを大きくし、古い時系列情報により推定される自己位置ほど重みを軽く小さくするようにして求めるようにしてもよい。この場合、重みが０になってしまうような時系列情報については、時系列情報DB２２２より削除するようにしてもよい。このようにすることで、人間などの記憶における忘却と類似した処理を実現することが可能となり、時系列情報DB２２２に登録される時系列情報を古いものから順に削除することが可能となり、自己位置の推定精度の低下を防止しつつ、時系列情報DB２２２に蓄積される時系列情報の情報量を適切に圧縮することが可能となる。
＜＜４．第２の変形例＞＞
以上においては、自己姿勢の変化が検出されたときに自己位置推定結果切替部２２５における動作を切り替える例について説明してきたが、動作モードを切り替えるようにしてもよい。
図１３は、自律移動制御部１１０における動作モードの状態遷移を説明する図である。
自律移動制御部１１０における動作モードは、通常モードＳｔ１、位置不定モードＳｔ２、および現在情報自己位置推定モードＳｔ３の３種類の動作モードからなる。
通常モードＳｔ１は、時系列情報推定情報が、自己位置推定結果として採用される動作モードである。通常モードＳｔ１においては、外力による姿勢の変化が検出されないときには、動作モードが、矢印Ｃ０で示されるように、通常モードＳｔ１に戻る状態が継続される。また、通常モードＳｔ１においては、外力による姿勢の変化が検出されるときには、矢印Ｃ１で示されるように、動作モードが、位置不定モードＳｔ２に遷移する。
位置不定モードＳｔ２は、自己位置が不定状態であることが通知され続ける動作モードである。位置不定モードＳｔ２においては、所定の時間が経過すると、矢印Ｃ２で示されるように、動作モードが、現在情報自己位置推定モードＳｔ３に遷移する。
現在情報自己位置推定モードＳｔ３は、現在情報自己位置を自己位置推定結果として採用する動作モードである。現在情報自己位置推定モードＳｔ３においては、所定時間が経過するまでは、矢印Ｃ４で示されるように、動作モードが、現在情報自己位置推定モードＳｔ３に戻る状態が継続される。また、現在情報自己位置推定モードＳｔ３においては、所定時間が経過すると、矢印Ｃ３で示されるように、動作モードが、通常モードに遷移し、元の状態に復帰する。また、現在情報自己位置推定モードにおいては、外力による姿勢の変化が検出されると、すなわち、予め予測できない姿勢変化が検出されると、矢印Ｃ４で示されるように、動作モードが、位置不定モードＳｔ２に遷移する。
＜第２の変形例の自律移動制御処理＞
次に、図１４のフローチャートを参照して、通常モード、位置不定モード、および現在情報自己位置推定モードの３種類の動作モードからなる自律移動制御部１１０における自律移動制御処理について説明する。
ステップＳ１２１において、自己位置推定結果切替部２２５は、通常モード処理を実行して、通常モードで動作する。尚、通常モード処理については、図１５を参照して詳細を後述する。
ステップＳ１２２において、自己位置推定結果切替部２２５の姿勢変化判定部２５１は、姿勢変化検出処理により、例えば、抱き上げ状態（kidnap状態）であることを示す姿勢に変化する姿勢変化（予め予測できない姿勢変化）が検出されたことが通知されたか否かを判定する。
ステップＳ１２２において、姿勢変化（予め予測できない姿勢変化）が検出されていないとみなされた場合、処理は、ステップＳ１２５に進む。
ステップＳ１２５において、処理の終了が指示されたか否かが判定されて、終了が指示されない場合、処理は、ステップＳ１２１に戻る。また、ステップＳ１２２において、姿勢変化が検出されない場合、ステップＳ１２１，Ｓ１２２，Ｓ１２５の処理が繰り返されて、通常モードの動作状態が維持されて、通常モード処理が継続される。尚、ステップＳ１２５において、処理の終了が指示された場合、処理は、終了する。
＜通常モード処理＞
ここで、図１５のフローチャートを参照して、通常モード処理について説明する。尚、図１５のフローチャートにおけるステップＳ１４１乃至Ｓ１４４の処理は、図１０のフローチャートにおけるステップＳ７４乃至Ｓ７８の処理と同様であるので、その説明は省略する。
すなわち、通常モード処理においては、時系列情報自己位置が、自己位置推定結果として出力される。
ここで、図１４のフローチャートの説明に戻る。
ステップＳ１２２において、姿勢変化（予め予測できない姿勢変化）が検出されているとみなされた場合、動作モードは、通常モードから位置不定モードに変化して、処理は、ステップＳ１２３に進む。
ステップＳ１２３において、自律移動制御部１１０は、位置不定モード処理を実行して、位置不定モードで動作し、自己位置が不定であることが通知され続けることにより、動作が停止する。尚、位置不定モード処理については、図１６のフローチャートを参照して詳細を後述する。
ステップＳ１２４において、動作モードが現在情報自己位置推定モードに遷移し、現在情報自己位置推定モード処理が実行されて、その後、ステップＳ１２５に進む。
ステップＳ１２５において、動作の終了が指示されたか否かが判定され、終了が指示されていない場合、処理は、ステップＳ１２１に戻る。すなわち、終了が指示されるまで、ステップＳ１２１乃至Ｓ１２５の処理が繰り返される。そして、ステップＳ１２５において、処理の終了が指示されると、処理が終了する。尚、現在情報自己位置推定モード処理については、図１７を参照して詳細を後述する。
すなわち、動作モードは、通常モードＳｔ１から開始され、姿勢の変化が検出されない限り、ステップＳ１２１，Ｓ１２２，Ｓ１２５の処理が繰り返されて、通常モード処理が実行され続ける。そして、ステップＳ１２２において、姿勢の変化（予め予測できない姿勢変化）が検出されると、動作モードが位置不定モードＳｔ２に遷移して、ステップＳ１２３において、位置不定モード処理が実行される。そして、その後、動作モードが、現在情報自己位置推定モードＳｔ３に遷移して、ステップＳ１２４において、現在情報自己位置推定モード処理が実行される。そして、終了が指示されない限り、動作モードは、再び、通常モードに遷移する。
＜位置不定モード処理＞
次に、図１６のフローチャートを参照して、位置不定モード処理について説明する。
ステップＳ１６１において、自己位置推定結果切替部２２５は、姿勢変化に伴って自己位置が不定であることを行動制御処理部１２３に通知する。この通知に伴って、行動制御処理部１２３は、アクチュエータ群１１３を制御して、移動体１１の動作を停止させる。
ステップＳ１６２において、自己位置推定結果切替部２２５は、所定時間が経過して、現在情報自己位置推定モードに遷移できる状態になったか否かを判定し、遷移できない場合、処理は、ステップＳ１６１に戻る。すなわち、所定時間が経過して、現在情報自己位置推定モードに遷移できる状態になるまで、ステップＳ１６１，Ｓ１６２の処理が繰り返される。
そして、ステップＳ１６２において、所定時間が経過して、現在情報自己位置推定モードに遷移できる状態になると、処理は、終了し、動作モードが、位置不定モードから現在情報自己位置推定モードに遷移する。
また、別の実施形態として、自己位置推定結果切替部２２５は、過去x秒間の姿勢の変化量を、センサ値等をもとに算出し、その姿勢変化量の時間平均が所定量を下回った時に、位置不定モードから現在情報自己位置推定モードに遷移できる状態とすることもできる。
さらに、別の実施形態として、自己位置推定結果切替部２２５は、過去X秒間の自己機体に対する重力方向の変化量を、センサ値等をもとに算出し、その変化量の時間平均が所定量を下回った時に、位置不定モードから現在情報自己位置推定モードに遷移できる状態とすることもできる。
以上の処理により、位置不定モードにおいては、所定の時間が経過するまで、自己位置推定結果が位置不定であることを通知する処理が繰り返される。そして、所定時間が経過して、現在情報自己位置推定モードに遷移できる状態になると動作モードが、現在情報自己位置推定モードに遷移する。
＜現在情報自己位置推定モード処理＞
次に、図１７のフローチャートを参照して、現在情報自己位置推定モード処理について説明する。
ステップＳ１８１において、自己位置推定結果切替部２２５は、所定時間だけ動作の停止を行動制御処理部１２３に指示する。この指示に伴って、行動制御処理部１２３は、アクチュエータ群１１３を制御して、移動体１１の動作を所定時間だけ停止させる。
ステップＳ１８２において、自己位置推定結果切替部２２５の姿勢変化判定部２５１は、姿勢変化検出処理により検出される、抱き上げ状態（kidnap状態）であることを示す姿勢への変化（予め予測できない姿勢変化）が検出されたか否かを判定する。
ステップＳ１８２において、抱き上げ状態（kidnap状態）であることを示す姿勢への姿勢変化（予め予測できない姿勢変化）が検出された場合、処理は、図１４のステップＳ１２３に戻る。すなわち、動作モードが、現在情報自己位置推定モードから位置不定モードに戻り、処理は、ステップＳ１２３の位置不定モード処理に進む。
一方、ステップＳ１８２において、抱き上げ状態（kidnap状態）であることを示す姿勢に変化する姿勢変化（予め予測できない姿勢変化）が検出されない場合、処理は、ステップＳ１８３に進む。
ステップＳ１８３において、自己位置推定結果切替部２２５は、現在情報自己位置推定部２２３の推定結果である現在情報自己位置を、自己位置推定結果として状況分析部１３３に出力する。
ステップＳ１８４において、状況分析部１３３は、自己位置の推定結果に基づいて、自己位置の周囲の状況を分析し、分析結果を行動計画処理部１２２に出力する。
ステップＳ１８５において、行動計画処理部１２２は、自己位置の推定結果の周囲の分析結果により、目的地までのルートを計画し、計画されたルートに基づいて、行動計画を決定し、アクチュエータ群１１３を制御するための動作計画を生成し、アクチュエータ群１１３を制御するための動作計画を生成し、行動制御処理部１２３に出力する。
ステップＳ１８６において、行動制御処理部１２３は、動作計画部１６３より供給される動作計画に基づいて、アクチュエータ群１１３を制御する。
ステップＳ１８７において、自律移動制御部１１０は、所定時間が経過したか否かを判定し、所定時間が経過していない場合、処理は、ステップＳ１８２に戻る。すなわち、所定時間が経過するまで、ステップＳ１８２乃至Ｓ１８７の処理が繰り返される。そして、ステップＳ１８７において、所定時間が経過した場合、処理は、ステップＳ１８８に進む。
ステップＳ１８８において、自己位置推定結果切替部２２５は、時系列情報DB２２２に登録されている過去の時系列情報をリセットし、現在情報自己位置推定モード処理は、終了し、処理は、図１４のステップＳ１２５に戻る。
以上の処理により、抱き上げ状態などの姿勢に変化する自己姿勢の変化（予め予測できない姿勢変化）が検出されたとき、動作モードを通常モードから、位置不定モードに切り替えることで、自己位置推定結果が位置不定であることが通知され、所定時間だけ動作を停止させた後、現在情報自己位置推定モードにして、所定時間だけ現在情報自己位置を自己位置推定結果として採用する。そして、所定時間が経過した後、時系列情報DB２２２に登録されている時系列情報をリセットさせることで、抱き上げ状態などの後にでも、適切に自己位置を推定しながら自律移動制御処理を継続することが可能となる。
＜＜５．第３の変形例＞＞
以上においては、抱き上げ状態、走行路が雪道や、走行路が砂利道であることによるスリップ、センサ異常を生じさせる環境変化（電磁ノイズ、モノの影）により視野が狭くなる、または、無くなるような自己姿勢の変化、すなわち、予め予測できない姿勢変化が検出されたときに自己位置推定結果切替部２２５における動作モードを、通常モードＳｔ１、位置不定モードＳｔ２、および現在情報自己位置推定モードＳｔ３の３種類の動作モードに切り替えるようにする例について説明してきた。
しかしながら、予め予測できない自己姿勢の変化の種別は、さらに、人、物体、および他車などの衝突、パンクや部品欠け、部品や貨物落下などの不慮の故障などであってもよい。
図１８は、予め予測できない自己姿勢の変化の種別が、さらに、人や物体や他車との衝突、パンクや部品欠けや部品や貨物落下などの不慮の故障などである場合の自律移動制御部１１０における動作モードの状態遷移を説明する図である。
自律移動制御部１１０における動作モードは、通常モードＳｔ１１、事故直後モードＳｔ１２、および制御回復モードＳｔ１３の３種類の動作モードからなる。
通常モードＳｔ１１は、図１３の通常モードＳｔ１と同様であり、時系列情報推定情報が、自己位置推定結果として採用される動作モードである。通常モードＳｔ１１においては、外力による姿勢の変化が検出されないときには、動作モードは、矢印Ｃ１０で示されるように、通常モードＳｔ１に戻る状態が継続される。また、通常モードＳｔ１１においては、予め予測できない姿勢の変化として、事故の発生が検出されると、矢印Ｃ１１で示されるように、動作モードが、事故直後モードＳｔ１２に遷移する。
事故直後モードＳｔ１２は、事故直後の状態であり、時系列情報DB２２２の時系列情報がリセットされ、動作を停止させる動作モードである。事故直後モードＳｔ１２においては、所定の時間が経過すると、矢印Ｃ１２で示されるように、動作モードが、制御回復モードＳｔ１３に遷移する。
制御回復モードＳｔ１３は、自己位置として現在情報自己位置を自己位置推定結果として採用する動作モードである。制御回復モードＳｔ１３においては、所定時間が経過するまでは、矢印Ｃ１４で示されるように、動作モードが、制御回復モードＳｔ１３に戻る状態が継続される。また、制御回復モードＳｔ１３においては、所定時間が経過すると、矢印Ｃ１３で示されるように、動作モードが、通常モードＳｔ１１に遷移し、元の状態に復帰する。
＜第３の変形例の自律移動制御処理＞
次に、図１９のフローチャートを参照して、通常モード、事故直後モード、および制御回復モードの３種類の動作モードからなる自律移動制御部１１０における自律移動制御処理について説明する。
ステップＳ２０１において、自律移動制御部１１０は、通常モード処理を実行して、通常モードで動作する。尚、通常モード処理については、図１５を参照して説明した処理と同様であるので、その説明は省略する。
ステップＳ２０２において、自己位置推定結果切替部２２５の姿勢変化判定部２５１は、姿勢変化検出処理により検出される、事故が発生しているような異常状態を示す姿勢に変化する姿勢変化（予め予測できない姿勢変化）が検出されたことが通知されたか否かを判定する。
ステップＳ２０２において、姿勢変化（予め予測できない姿勢変化）が検出されていないとみなされた場合、処理は、ステップＳ２０５に進む。
ステップＳ２０５において、処理の終了が指示されたか否かが判定されて、終了が指示されない場合、処理は、ステップＳ２０１に戻る。すなわち、事故が発生しているような異常状態を示す姿勢に変化する姿勢変化（予め予測できない姿勢変化）が検出されない限り、ステップＳ２０１，Ｓ２０２，Ｓ２０５の処理が繰り返されて、通常モードの動作状態が維持されて、通常モード処理が継続される。尚、ステップＳ２０５において、処理の終了が指示された場合、処理は、終了する。
ステップＳ２０２において、姿勢変化（予め予測できない姿勢変化）が検出されているとみなされた場合、動作モードは、通常モードから事故直後モードに変化して、処理は、ステップＳ２０３に進む。
ステップＳ２０３において、自律移動制御部１１０は、事故直後モード処理を実行して、事故直後モードで動作し、動作が停止されて、時系列情報DB２２２に登録された時系列情報がリセットされる。尚、事故直後モード処理については、図２０を参照して詳細を後述する。
ステップＳ２０４において、動作モードが制御回復モード処理に遷移し、制御回復モードで動作し、その後、ステップＳ２０５に進む。制御回復モード処理においては、現在情報自己位置が自己位置推定結果として採用され、所定時間が経過した後、終了が指示されない限り、処理が、ステップＳ２０１に戻り、動作モードが、通常モードに復帰する。尚、制御回復モード処理については、図２１を参照して詳細を後述する。
すなわち、動作モードは、通常モードＳｔ１１から開始され、予め予測できない、事故が発生しているような異常状態を示す姿勢の変化が検出されない限り、ステップＳ２０１，Ｓ２０２，Ｓ２０５の処理が繰り返されて、通常モード処理が実行され続ける。そして、ステップＳ２０２において、事故が発生しているような異常状態を示す姿勢の変化（予め予測できない姿勢変化）が検出されると、動作モードが事故直後モードＳｔ１２に遷移して、ステップＳ２０３において、事故直後モード処理が実行される。そして、その後、動作モードが、制御回復モードＳｔ１３に遷移して、ステップＳ２０４において、事故回復モード処理が実行される。そして、終了が指示されない限り、動作モードは、再び、通常モードに遷移する。
＜事故直後モード処理＞
次に、図２０のフローチャートを参照して、事故直後モード処理について説明する。
ステップＳ２１１において、自己位置推定結果切替部２２５は、動作の停止を行動制御処理部１２３に指示する。この指示に伴って、行動制御処理部１２３は、アクチュエータ群１１３を制御して、移動体１１の動作を停止させる。
ステップＳ２１２において、自己位置推定結果切替部２２５は、時系列情報DB２２２に登録されている時系列情報をリセットさせる。
ステップＳ２１３において、自己位置推定結果切替部２２５は、所定時間が経過して、制御回復モードに遷移できる状態になったか否かを判定し、所定時間が経過して、遷移できるまで、同様の処理を繰り返す。
そして、ステップＳ２１３において、所定時間が経過して、制御回復モードに遷移できる状態になると、処理は、終了し、動作モードが、事故直後モードから制御回復モードに遷移する。
また、別の実施形態として、自己位置推定結果切替部２２５は、過去x秒間の姿勢の変化量を、センサ値等をもとに算出し、その姿勢変化量の時間平均が所定量を下回った時に、事故直後モードから制御回復モードに遷移できる状態とすることもできる。
さらに、別の実施形態として、自己位置推定結果切替部２２５は、過去X秒間の自己機体に対する重力方向の変化量を、センサ値等をもとに算出し、その変化量の時間平均が所定量を下回った時に、事故直後モードから制御回復モードに遷移できる状態とすることもできる。
以上の処理により、事故直後モードにおいて、事故が発生した直後であるので、動作を停止させることが指示されることにより動作が停止され、時系列情報DB２２２の時系列情報がリセットされる。そして、所定時間が経過して、制御回復モードに遷移できる状態になると動作モードが、制御回復モードに遷移する。
＜制御回復モード処理＞
次に、図２１のフローチャートを参照して、制御回復モード処理について説明する。尚、図２１のフローチャートにおけるステップＳ２２１乃至Ｓ２２６の処理は、図１７のステップＳ１８１，Ｓ１８３乃至Ｓ１８７の処理と同様であるので、その説明は省略する。
すなわち、図２１の制御回復モードにおいては、所定時間が経過するまで、現在情報自己位置が自己位置推定結果として採用されて、動作モードが通常モードに復帰する。
結果として、予め予測できない、事故が発生するような姿勢の変化が検出されても、安全に動作を停止させた後、自己位置についても、現在情報自己位置が自己位置推定結果として採用されるので、事故の直後で、自律移動制御部１１０が復帰したタイミングにおいても、自己位置推定に用いるには精度が低いことが予想される時系列情報がリセットされて、現在情報自己位置が自己位置推定結果として採用されることにより、自己位置推定結果の精度の大きな低下を防止することが可能となる。
尚、姿勢の変化に基づいて検出される予め予測できない姿勢変化であれば、事故以外が検出される場合であってもよく、例えば、強風、地震、津波、第三者からの攻撃、車両が搭載されうる乗り物（例えば、カーフェリーボードなど）の加速度の変化、線路や道路のがたつきが検出される場合であってもよい。
この場合、動作モードを通常モードから事故直後モード、または、位置不定モードに遷移させ、以降において、制御回復モード、または、現在情報自己位置推定モードに遷移させた後、通常モードに遷移させるようにしてもよい。
＜＜６．ソフトウェアにより実行させる例＞＞
ところで、上述した一連の処理は、ハードウェアにより実行させることもできるが、ソフトウェアにより実行させることもできる。一連の処理をソフトウェアにより実行させる場合には、そのソフトウェアを構成するプログラムが、専用のハードウェアに組み込まれているコンピュータ、または、各種のプログラムをインストールすることで、各種の機能を実行することが可能な、例えば汎用のコンピュータなどに、記録媒体からインストールされる。
図２２は、汎用のコンピュータの構成例を示している。このパーソナルコンピュータは、CPU(Central Processing Unit)１００１を内蔵している。CPU１００１にはバス１００４を介して、入出力インタフェース１００５が接続されている。バス１００４には、ROM(Read Only Memory)１００２およびRAM(Random Access Memory)１００３が接続されている。
入出力インタフェース１００５には、ユーザが操作コマンドを入力するキーボード、マウスなどの入力デバイスよりなる入力部１００６、処理操作画面や処理結果の画像を表示デバイスに出力する出力部１００７、プログラムや各種データを格納するハードディスクドライブなどよりなる記憶部１００８、LAN（Local Area Network）アダプタなどよりなり、インターネットに代表されるネットワークを介した通信処理を実行する通信部１００９が接続されている。また、磁気ディスク（フレキシブルディスクを含む）、光ディスク（CD-ROM(Compact Disc-Read Only Memory)、DVD(Digital Versatile Disc)を含む）、光磁気ディスク（ＭＤ(Mini Disc)を含む）、もしくは半導体メモリなどのリムーバブルメディア１０１１に対してデータを読み書きするドライブ１０１０が接続されている。
CPU１００１は、ROM１００２に記憶されているプログラム、または磁気ディスク、光ディスク、光磁気ディスク、もしくは半導体メモリ等のリムーバブルメディア１０１１ら読み出されて記憶部１００８にインストールされ、記憶部１００８からRAM１００３にロードされたプログラムに従って各種の処理を実行する。RAM１００３にはまた、CPU１００１が各種の処理を実行する上において必要なデータなども適宜記憶される。
以上のように構成されるコンピュータでは、CPU１００１が、例えば、記憶部１００８に記憶されているプログラムを、入出力インタフェース１００５及びバス１００４を介して、RAM１００３にロードして実行することにより、上述した一連の処理が行われる。
コンピュータ（CPU１００１）が実行するプログラムは、例えば、パッケージメディア等としてのリムーバブルメディア１０１１に記録して提供することができる。また、プログラムは、ローカルエリアネットワーク、インターネット、デジタル衛星放送といった、有線または無線の伝送媒体を介して提供することができる。
コンピュータでは、プログラムは、リムーバブルメディア１０１１をドライブ１０１０に装着することにより、入出力インタフェース１００５を介して、記憶部１００８にインストールすることができる。また、プログラムは、有線または無線の伝送媒体を介して、通信部１００９で受信し、記憶部１００８にインストールすることができる。その他、プログラムは、ROM１００２や記憶部１００８に、あらかじめインストールしておくことができる。
なお、コンピュータが実行するプログラムは、本明細書で説明する順序に沿って時系列に処理が行われるプログラムであっても良いし、並列に、あるいは呼び出しが行われたとき等の必要なタイミングで処理が行われるプログラムであっても良い。
尚、図２２におけるCPU１００１が、図３における自律移動制御部１１０の機能を実現させる。また、図２２における記憶部１００８が、図３における記憶部１０９を実現する。
また、本明細書において、システムとは、複数の構成要素（装置、モジュール（部品）等）の集合を意味し、すべての構成要素が同一筐体中にあるか否かは問わない。したがって、別個の筐体に収納され、ネットワークを介して接続されている複数の装置、及び、１つの筐体の中に複数のモジュールが収納されている１つの装置は、いずれも、システムである。
なお、本開示の実施の形態は、上述した実施の形態に限定されるものではなく、本開示の要旨を逸脱しない範囲において種々の変更が可能である。
例えば、本開示は、１つの機能をネットワークを介して複数の装置で分担、共同して処理するクラウドコンピューティングの構成をとることができる。
また、上述のフローチャートで説明した各ステップは、１つの装置で実行する他、複数の装置で分担して実行することができる。
さらに、１つのステップに複数の処理が含まれる場合には、その１つのステップに含まれる複数の処理は、１つの装置で実行する他、複数の装置で分担して実行することができる。
尚、本開示は、以下のような構成も取ることができる。
＜１＞ センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出部と、
前記自己姿勢検出部による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出部と、
センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定部とを含む
制御装置。
＜２＞ 前記自己位置推定部は、
前記センサ情報を時系列情報として蓄積する時系列情報蓄積部と、
前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部を含み、
前記時系列情報蓄積部は、前記予め予測できない姿勢の変化が検出される場合、蓄積された過去の前記時系列情報がリセットされ、
前記時系列情報自己位置推定部は、
前記第１の推定モデルとして、前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、
前記第２の推定モデルとして、前記時系列情報蓄積部に蓄積された過去の時系列情報がリセットされた後の、前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定する
＜１＞に記載の制御装置。
＜３＞ 前記時系列情報蓄積部に蓄積された過去の時系列情報は、前記リセットにより、所定の時間以上長く蓄積された時系列情報から削除される
＜２＞に記載の制御装置。
＜４＞ 前記時系列情報自己位置推定部は、前記時系列情報蓄積部に蓄積された過去の時系列情報が前記リセットされた後、前記時系列情報蓄積部に蓄積された時間が長い前記時系列情報ほど、前記時系列情報により推定される自己位置の重みを軽くして、前記時系列情報自己位置を推定する
＜２＞に記載の制御装置。
＜５＞ 前記自己位置推定部は、
事前確率密度分布の観測値を最新の時系列情報により更新するステップを繰り返すカルマンフィルタを用いて時系列情報自己位置を推定する時系列情報自己位置推定部を含み、
前記時系列情報自己位置推定部は、
前記第１の推定モデルとして、前記時系列情報自己位置を、事前確率密度分布の観測値を最新の時系列情報により更新するステップを繰り返すカルマンフィルタを用いて推定し、
前記第２の推定モデルとして、前記予め予測できない姿勢の変化が検出される場合、最新の前記時系列情報の重みを最大にした後、前記カルマンフィルタを用いて、前記時系列情報自己位置を推定する
＜１＞に記載の制御装置。
＜６＞ 前記自己位置推定部は、
前記センサ情報によりセンシングされる時系列情報として蓄積する時系列情報蓄積部と、
前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部と、
前記センサ情報によりセンシングされる現在の情報である現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部とを含み、
前記自己位置推定部は、
前記予め予測できない姿勢の変化が検出されない場合、前記時系列情報自己位置を自己位置推定結果として採用し、
前記予め予測できない姿勢の変化が検出された場合、前記現在情報自己位置を自己位置推定結果として採用する
＜１＞に記載の制御装置。
＜７＞ 前記自己位置推定部は、
前記姿勢変化検出部の検出結果に応じて、動作モードを、少なくとも第１のモード、第２のモード、および第３のモードに遷移させ、
動作開始時において、前記動作モードは、第１のモードとされ、前記第１の推定モデルにより前記自己位置を推定し、
前記第１のモードにおいて、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記動作モードを第２のモードに遷移させ、前記予め予測できない姿勢の変化が検出されたことを通知し、
前記第２のモードにおいて、所定時間が経過した後、前記動作モードを第３のモードに遷移させ、前記第２の推定モデルにより、前記自己位置を推定し、
前記第３のモードにおいて、所定時間が経過した後、前記動作モードを前記第１のモードに遷移させる
＜１＞に記載の制御装置。
＜８＞ 前記自己位置推定部は、
前記センサ情報を時系列情報として蓄積する時系列情報蓄積部と、
前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部と、
前記センサ情報によりセンシングされる現在の情報である現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部とを含み、
前記第１のモードにおいて、前記第１の推定モデルである、前記時系列情報蓄積部に蓄積された時系列情報を用いた、前記時系列情報自己位置推定部により推定された前記時系列情報自己位置を自己位置推定結果として採用し、
前記第３のモードにおいて、前記第２の推定モデルである、前記現在情報を用いた、前記現在情報自己位置推定部により推定された前記現在情報自己位置を自己位置推定結果として採用する
＜７＞に記載の制御装置。
＜９＞ 前記予め予測できない姿勢の変化は、前記自己位置および姿勢の変化に連続性がなくなる状態への変化であり、
前記第１のモードは通常モードであり、前記第２のモードは自己位置不定モードであり、前記第３のモードは現在情報自己位置推定モードである
＜８＞に記載の制御装置。
＜１０＞ 前記動作モードが、前記第３のモードである、前記現在情報自己位置推定モードの場合、前記予め予測できない姿勢の変化が検出されたとき、前記自己位置推定部は、前記動作モードを、前記第２のモードである、前記自己位置不定モードに遷移させる
＜９＞に記載の制御装置。
＜１１＞ 前記自己位置および姿勢の変化に連続性がなくなる状態は、キッドナップ状態、足裏接地面及び車輪のスリップ状態、非固定物体の乗り降りおよびセンサ異常状態を含む
＜９＞に記載の制御装置。
＜１２＞ 前記予め予測できない姿勢の変化は、事故発生状態への変化であり、
前記第１のモードは通常モードであり、前記第２のモードは事故直後モードであり、前記第３のモードは制御回復モードである
＜８＞に記載の制御装置。
＜１３＞ 前記事故発生状態は、人、物体、および他機・他車との衝突、パンク、部品欠け、部品および貨物落下状態を含む
＜１２＞に記載の制御装置。
＜１４＞ 前記時系列情報は、LIDARにより検出される３次元点群情報、および車輪エンコーダにより検出される位置情報、姿勢、速度、加速度、および角速度であり、
時系列情報自己位置推定部は、前記時系列情報に基づいて、カルマンフィルタまたは粒子フィルタを用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力し、
前記現在情報は、ステレオカメラにより撮像される視差画像であり、
現在情報自己位置推定部は、前記視差画像よりデプス画像を生成し、前記デプス画像より画像特徴量を抽出して、前記画像特徴量に基づいて自己位置を推定し、推定結果を現在情報自己位置として出力する
＜８＞に記載の制御装置。
＜１５＞ 前記自己姿勢検出部が前記自己の姿勢を検出する際に使用する前記センサ情報は、鉛直方向の加速度を含み、
前記自己姿勢検出部は、
前記鉛直方向の加速度の変化の周波数成分を抽出する周波数成分抽出部と、
前記周波数成分抽出部により抽出された周波数成分より特徴量を抽出する特徴量抽出部と、
前記特徴量に基づいて、前記自己の姿勢を検出する姿勢検出部とを含む
＜１＞乃至＜１４＞の少なくともいずれかに記載の制御装置。
＜１６＞ 既知の姿勢に対するセンサ情報に基づいて、学習により、前記姿勢と、前記周波数成分の特徴量とが対応付けられた状態で登録されたデータベースをさらに含み、
前記姿勢検出部は、前記特徴量に基づいて、前記データベースより、対応する姿勢を検索することにより、前記姿勢を検出する
＜１５＞に記載の制御装置。
＜１７＞ 前記データベースは、前記既知の姿勢に対するセンサ情報に基づいて、ニューラルネットワークを用いた学習により、前記姿勢と、前記周波数成分の特徴量とが対応付けられた状態で登録される
＜１６＞に記載の制御装置。
＜１８＞ センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出処理と、
前記自己姿勢検出処理による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出処理と、
センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出処理により、前記予め予測できない姿勢への変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定処理とを含む
制御方法。
＜１９＞ センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出部と、
前記自己姿勢検出部による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出部と、
センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定部と
してコンピュータを機能させるプログラム。
＜２０＞ センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出部と、
前記自己姿勢検出部による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出部と、
センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定部と、
前記自己位置推定部により推定された自己位置の周辺の状況に基づいて、行動計画を生成する行動計画生成部と、
前記行動計画生成部により決定された行動計画に基づいて移動体の動作を制御する制御部と
を含む移動体。
１１ 移動体， ２１ センサ群， ２１ａ，２１ａ−１乃至２１ａ−ｎ センサ， ２２ 自律移動制御部， ２３ アクチュエータ群， ２３ａ，２３ａ−１乃至２３ａ−ｎ アクチュエータ， ３１ 認識処理部， ３２ 行動計画処理部， ３３ 行動制御処理部， ４１ ルート計画部， ４２ 行動計画部， ４３ 動作計画部， １０２ データ取得部， １０５ 出力制御部， １０６ 出力部， １０７ 駆動系制御部， １０８ 駆動系システム， １１０ 自律移動制御部， １１２ センサ群， １１３ アクチュエータ群， １２１ 認識処理部， １２２ 行動計画処理部， １２３ 行動制御処理部， １３４ 計画部， １６１ ルート計画部， １６２ 行動計画部， １６３ 動作計画部， ２０１ LIDAR， ２０２ 車輪エンコーダ， ２０３ ステレオカメラ， ２０４ 内界センサ， ２２１ 時系列情報自己位置推定部， ２２２ 時系列情報DB， ２２３ 現在情報自己位置推定部， ２２４ 位置画像特徴量DB， ２２５ 自己位置推定結果切替部， ２２６ 自己姿勢検出部， ２２７ 特徴量姿勢DB， ２２８ 姿勢変化検出部， ２５１ 姿勢変化判定部， ２７１ 周波数成分抽出部， ２７２ 特徴量抽出部， ２７３ 姿勢検出部

Claims (20)

  1. センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出部と、
    前記自己姿勢検出部による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出部と、
    センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定部とを含む
    制御装置。
  2. 前記自己位置推定部は、
    前記センサ情報を時系列情報として蓄積する時系列情報蓄積部と、
    前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部を含み、
    前記時系列情報蓄積部は、前記予め予測できない姿勢の変化が検出される場合、蓄積された過去の前記時系列情報がリセットされ、
    前記時系列情報自己位置推定部は、
    前記第１の推定モデルとして、前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、
    前記第２の推定モデルとして、前記時系列情報蓄積部に蓄積された過去の時系列情報がリセットされた後の、前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定する
    請求項１に記載の制御装置。
  3. 前記時系列情報蓄積部に蓄積された過去の時系列情報は、前記リセットにより、所定の時間以上長く蓄積された時系列情報から削除される
    請求項２に記載の制御装置。
  4. 前記時系列情報自己位置推定部は、前記時系列情報蓄積部に蓄積された過去の時系列情報が前記リセットされた後、前記時系列情報蓄積部に蓄積された時間が長い前記時系列情報ほど、前記時系列情報により推定される自己位置の重みを軽くして、前記時系列情報自己位置を推定する
    請求項２に記載の制御装置。
  5. 前記自己位置推定部は、
    事前確率密度分布の観測値を最新の時系列情報により更新するステップを繰り返すカルマンフィルタを用いて時系列情報自己位置を推定する時系列情報自己位置推定部を含み、
    前記時系列情報自己位置推定部は、
    前記第１の推定モデルとして、前記時系列情報自己位置を、事前確率密度分布の観測値を最新の時系列情報により更新するステップを繰り返すカルマンフィルタを用いて推定し、
    前記第２の推定モデルとして、前記予め予測できない姿勢の変化が検出される場合、最新の前記時系列情報の重みを最大にした後、前記カルマンフィルタを用いて、前記時系列情報自己位置を推定する
    請求項１に記載の制御装置。
  6. 前記自己位置推定部は、
    前記センサ情報によりセンシングされる時系列情報として蓄積する時系列情報蓄積部と、
    前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部と、
    前記センサ情報によりセンシングされる現在の情報である現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部とを含み、
    前記自己位置推定部は、
    前記予め予測できない姿勢の変化が検出されない場合、前記時系列情報自己位置を自己位置推定結果として採用し、
    前記予め予測できない姿勢の変化が検出された場合、前記現在情報自己位置を自己位置推定結果として採用する
    請求項１に記載の制御装置。
  7. 前記自己位置推定部は、
    前記姿勢変化検出部の検出結果に応じて、動作モードを、少なくとも第１のモード、第２のモード、および第３のモードに遷移させ、
    動作開始時において、前記動作モードは、前記第１のモードとされ、前記第１の推定モデルにより前記自己位置を推定し、
    前記第１のモードにおいて、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記動作モードを第２のモードに遷移させ、前記予め予測できない姿勢の変化が検出されたことを通知し、
    前記第２のモードにおいて、所定時間が経過した後、前記動作モードを第３のモードに遷移させ、前記第２の推定モデルにより、前記自己位置を推定し、
    前記第３のモードにおいて、所定時間が経過した後、前記動作モードを前記第１のモードに遷移させる
    請求項１に記載の制御装置。
  8. 前記自己位置推定部は、
    前記センサ情報を時系列情報として蓄積する時系列情報蓄積部と、
    前記時系列情報蓄積部に蓄積された時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部と、
    前記センサ情報によりセンシングされる現在の情報である現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部とを含み、
    前記第１のモードにおいて、前記第１の推定モデルである、前記時系列情報蓄積部に蓄積された時系列情報を用いた、前記時系列情報自己位置推定部により推定された前記時系列情報自己位置を自己位置推定結果として採用し、
    前記第３のモードにおいて、前記第２の推定モデルである、前記現在情報を用いた、前記現在情報自己位置推定部により推定された前記現在情報自己位置を自己位置推定結果として採用する
    請求項７に記載の制御装置。
  9. 前記予め予測できない姿勢の変化は、前記自己位置および姿勢の変化に連続性がなくなる状態への変化であり、
    前記第１のモードは通常モードであり、前記第２のモードは自己位置不定モードであり、前記第３のモードは現在情報自己位置推定モードである
    請求項８に記載の制御装置。
  10. 前記動作モードが、前記第３のモードである、前記現在情報自己位置推定モードの場合、前記予め予測できない姿勢の変化が検出されたとき、前記自己位置推定部は、前記動作モードを、前記第２のモードである、前記自己位置不定モードに遷移させる
    請求項９に記載の制御装置。
  11. 前記自己位置および姿勢の変化に連続性がなくなる状態は、キッドナップ状態、足裏接地面及び車輪のスリップ状態、非固定物体の乗り降りおよびセンサ異常状態を含む
    請求項９に記載の制御装置。
  12. 前記予め予測できない姿勢の変化は、事故発生状態への変化であり、
    前記第１のモードは通常モードであり、前記第２のモードは事故直後モードであり、前記第３のモードは制御回復モードである
    請求項８に記載の制御装置。
  13. 前記事故発生状態は、人、物体、および他機・他車との衝突、パンク、部品欠け、部品および貨物落下状態を含む
    請求項１２に記載の制御装置。
  14. 前記時系列情報は、LIDARにより検出される３次元点群情報、および車輪エンコーダにより検出される位置情報、姿勢、速度、加速度、および角速度であり、
    時系列情報自己位置推定部は、前記時系列情報に基づいて、カルマンフィルタまたは粒子フィルタを用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力し、
    前記現在情報は、ステレオカメラにより撮像される視差画像であり、
    現在情報自己位置推定部は、前記視差画像よりデプス画像を生成し、前記デプス画像より画像特徴量を抽出して、前記画像特徴量に基づいて自己位置を推定し、推定結果を現在情報自己位置として出力する
    請求項８に記載の制御装置。
  15. 前記自己姿勢検出部が前記自己の姿勢を検出する際に使用する前記センサ情報は、鉛直方向の加速度を含み、
    前記自己姿勢検出部は、
    前記鉛直方向の加速度の変化の周波数成分を抽出する周波数成分抽出部と、
    前記周波数成分抽出部により抽出された周波数成分より特徴量を抽出する特徴量抽出部と、
    前記特徴量に基づいて、前記自己の姿勢を検出する姿勢検出部とを含む
    請求項１に記載の制御装置。
  16. 既知の姿勢に対するセンサ情報に基づいて、学習により、前記姿勢と、前記周波数成分の特徴量とが対応付けられた状態で登録されたデータベースをさらに含み、
    前記姿勢検出部は、前記特徴量に基づいて、前記データベースより、対応する姿勢を検索することにより、前記姿勢を検出する
    請求項１５に記載の制御装置。
  17. 前記データベースは、前記既知の姿勢に対するセンサ情報に基づいて、ニューラルネットワークを用いた学習により、前記姿勢と、前記周波数成分の特徴量とが対応付けられた状態で登録される
    請求項１６に記載の制御装置。
  18. センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出処理と、
    前記自己姿勢検出処理による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出処理と、
    センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出処理により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定処理とを含む
    制御方法。
  19. センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出部と、
    前記自己姿勢検出部による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出部と、
    センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定部と
    してコンピュータを機能させるプログラム。
  20. センサ情報に基づいて、自己の姿勢を検出する自己姿勢検出部と、
    前記自己姿勢検出部による検出結果に基づいて、予め予測できない姿勢の変化を検出する姿勢変化検出部と、
    センサ情報に基づいて、第１の推定モデルにより自己位置を推定すると共に、前記姿勢変化検出部により、前記予め予測できない姿勢の変化が検出される場合、前記第１の推定モデルと異なる第２の推定モデルにより自己位置を推定する自己位置推定部と、
    前記自己位置推定部により推定された自己位置の周辺の状況に基づいて、行動計画を生成する行動計画生成部と、
    前記行動計画生成部により決定された行動計画に基づいて移動体の動作を制御する制御部と
    を含む移動体。
JP2019558116A 2017-12-05 2018-11-21 制御装置、および制御方法、プログラム、並びに移動体 Active JP7180612B2 (ja)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2017233094 2017-12-05
JP2017233094 2017-12-05
PCT/JP2018/042921 WO2019111701A1 (ja) 2017-12-05 2018-11-21 制御装置、および制御方法、プログラム、並びに移動体

Publications (2)

Publication Number Publication Date
JPWO2019111701A1 true JPWO2019111701A1 (ja) 2020-11-26
JP7180612B2 JP7180612B2 (ja) 2022-11-30

Family

ID=66750936

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2019558116A Active JP7180612B2 (ja) 2017-12-05 2018-11-21 制御装置、および制御方法、プログラム、並びに移動体

Country Status (5)

Country Link
US (1) US11592829B2 (ja)
EP (1) EP3722904B1 (ja)
JP (1) JP7180612B2 (ja)
KR (1) KR102625488B1 (ja)
WO (1) WO2019111701A1 (ja)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7175216B2 (ja) * 2019-02-15 2022-11-18 ルネサスエレクトロニクス株式会社 異常検知装置、異常検知システム、異常検知方法
EP3966598A4 (en) * 2019-05-07 2022-12-21 Nokia Technologies OY PRIVACY MANAGEMENT
JP7385388B2 (ja) 2019-07-22 2023-11-22 株式会社ダイヘン 自己位置推定装置
JP2021060669A (ja) * 2019-10-03 2021-04-15 ソニー株式会社 情報処理装置、情報処理方法、情報処理システム及びプログラム
FR3106665B1 (fr) * 2020-01-27 2022-02-18 Continental Automotive Procédé et dispositif de localisation d’un véhicule
US11833661B2 (en) * 2020-10-31 2023-12-05 Google Llc Utilizing past contact physics in robotic manipulation (e.g., pushing) of an object
KR102434973B1 (ko) * 2020-11-06 2022-08-22 한국항공우주연구원 우주탐사장비의 이상 검출 방법
CN112687173B (zh) * 2020-12-25 2022-06-21 安徽机电职业技术学院 一种基于主被动安全协同优化的汽车碰撞演示平台
US20220315041A1 (en) * 2021-03-30 2022-10-06 Ghost Locomotion Inc. Scheduling state transitions in an autonomous vehicle
JP7156445B1 (ja) 2021-05-18 2022-10-19 三菱電機株式会社 携帯端末、歩行ロボット、プログラム、および位置演算支援方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010079869A (ja) * 2008-08-25 2010-04-08 Murata Machinery Ltd 自律移動装置
JP2010152787A (ja) * 2008-12-26 2010-07-08 Fujitsu Ltd 環境地図生成プログラム、環境地図生成方法及び移動ロボット
JP2017134617A (ja) * 2016-01-27 2017-08-03 株式会社リコー 位置推定装置、プログラム、位置推定方法

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS5239467Y2 (ja) 1971-03-31 1977-09-07
US7103460B1 (en) 1994-05-09 2006-09-05 Automotive Technologies International, Inc. System and method for vehicle diagnostics
US7162056B2 (en) * 2002-08-16 2007-01-09 Evolution Robotics, Inc. Systems and methods for the automated sensing of motion in a mobile robot using visual data
US7689321B2 (en) * 2004-02-13 2010-03-30 Evolution Robotics, Inc. Robust sensor fusion for mapping and localization in a simultaneous localization and mapping (SLAM) system
KR101297388B1 (ko) * 2006-06-16 2013-08-19 삼성전자주식회사 위치 보정 기능을 제공하는 이동 장치 및 위치 보정 방법
KR100941418B1 (ko) * 2007-03-20 2010-02-11 삼성전자주식회사 이동 로봇의 위치 인식 방법
BRPI0910450A2 (pt) * 2008-04-24 2016-07-19 Evolution Robotics Inc limpador robótico
CN101509781B (zh) * 2009-03-20 2011-09-14 同济大学 基于单目摄像头的步行机器人定位系统
US9014848B2 (en) 2010-05-20 2015-04-21 Irobot Corporation Mobile robot system
JP2013045298A (ja) 2011-08-24 2013-03-04 Hitachi Plant Technologies Ltd 無人搬送車
US20140031980A1 (en) * 2011-11-11 2014-01-30 Jens-Steffen Gutmann Systems and methods for extending slam to multiple regions

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010079869A (ja) * 2008-08-25 2010-04-08 Murata Machinery Ltd 自律移動装置
JP2010152787A (ja) * 2008-12-26 2010-07-08 Fujitsu Ltd 環境地図生成プログラム、環境地図生成方法及び移動ロボット
JP2017134617A (ja) * 2016-01-27 2017-08-03 株式会社リコー 位置推定装置、プログラム、位置推定方法

Also Published As

Publication number Publication date
EP3722904A4 (en) 2021-01-06
EP3722904A1 (en) 2020-10-14
JP7180612B2 (ja) 2022-11-30
EP3722904B1 (en) 2022-06-01
KR20200090777A (ko) 2020-07-29
KR102625488B1 (ko) 2024-01-17
US11592829B2 (en) 2023-02-28
WO2019111701A1 (ja) 2019-06-13
US20200387162A1 (en) 2020-12-10

Similar Documents

Publication Publication Date Title
JP7180612B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
JP7151725B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
US11537131B2 (en) Control device, control method, and mobile body
JP7180670B2 (ja) 制御装置、制御方法、並びにプログラム
WO2020226085A1 (ja) 情報処理装置、情報処理方法、及びプログラム
US11014494B2 (en) Information processing apparatus, information processing method, and mobile body
JP7257737B2 (ja) 情報処理装置、自己位置推定方法、及び、プログラム
WO2020250725A1 (ja) 情報処理装置、および情報処理方法、並びにプログラム
US20200278208A1 (en) Information processing apparatus, movable apparatus, information processing method, movable-apparatus control method, and programs
US11812197B2 (en) Information processing device, information processing method, and moving body
WO2019203022A1 (ja) 移動体、情報処理装置、情報処理方法、及びプログラム
US20200230820A1 (en) Information processing apparatus, self-localization method, program, and mobile body
US11500386B2 (en) Control apparatus, control method, program, and mobile object
JP7192771B2 (ja) 情報処理装置、情報処理方法、プログラム、及び、車両
WO2021187299A1 (ja) 情報処理装置、情報処理方法およびプログラム
WO2019176278A1 (ja) 情報処理装置、情報処理方法、プログラム、及び移動体
JPWO2019049710A1 (ja) 信号処理装置、および信号処理方法、プログラム、並びに移動体
WO2021140916A1 (ja) 移動体、情報処理装置、情報処理方法、及びプログラム
JPWO2020116204A1 (ja) 情報処理装置、情報処理方法、プログラム、移動体制御装置、及び、移動体

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20211005

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20220705

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220826

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: 20221018

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20221031

R151 Written notification of patent or utility model registration

Ref document number: 7180612

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151