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

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

Info

Publication number
JP7156305B2
JP7156305B2 JP2019554175A JP2019554175A JP7156305B2 JP 7156305 B2 JP7156305 B2 JP 7156305B2 JP 2019554175 A JP2019554175 A JP 2019554175A JP 2019554175 A JP2019554175 A JP 2019554175A JP 7156305 B2 JP7156305 B2 JP 7156305B2
Authority
JP
Japan
Prior art keywords
unit
self
obstacle
information
route
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.)
Active
Application number
JP2019554175A
Other languages
English (en)
Other versions
JPWO2019098082A1 (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
Sony Group Corp
Original Assignee
Sony Corp
Sony Group 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, Sony Group Corp filed Critical Sony Corp
Publication of JPWO2019098082A1 publication Critical patent/JPWO2019098082A1/ja
Application granted granted Critical
Publication of JP7156305B2 publication Critical patent/JP7156305B2/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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • 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/0251—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 extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C3/00—Measuring distances in line of sight; Optical rangefinders
    • G01C3/02—Details
    • G01C3/06—Use of electric means to obtain final indication
    • G01C3/08—Use of electric radiation detectors
    • G01C3/085—Use of electric radiation detectors with electronic parallax measurement
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/93—Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931—Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • 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/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
    • 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/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/593—Depth or shape recovery from multiple images from stereo images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • G06T2207/10021—Stereoscopic video; Stereoscopic image sequence
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle
    • G06T2207/30261—Obstacle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Electromagnetism (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Traffic Control Systems (AREA)

Description

本開示は、制御装置、および制御方法、プログラム、並びに移動体に関し、特に、周囲を効率よく探索できるようにした制御装置、および制御方法、プログラム、並びに移動体に関する。
ロボット等の移動体の自律的な移動を実現するためには、行動を計画する起点となる自己の位置を認識・推定する必要がある。そこで、自己の周囲の状況をセンサ等の手段で認識して、自己の位置を推定し、自律的な移動を計画する技術が提案されている。
例えば、連続的に過去に検出してきた自己位置の情報を利用して自己位置を順次推定する場合、何らかの外力により、それまでの自己位置とは全く異なる自己位置に移動してしまった場合、過去の自己位置の情報との連続性を失うことにより、自己位置を認識することができない状態となる。
このような場合、移動体は、自己位置を改めて推定するため、自らの周囲を移動しながら探索することで、周囲をセンシングして情報を取得し、取得した周囲の情報から自己位置を推定する必要がある。
この際、移動体は、ランダムに自らの周囲を動き回ると、既にセンシングが済んだ領域を繰り返しセンシングしてしまうことがあり、効率良く周囲を探索できない恐れがある。
そこで、「未探索」、「横断済み」、「縁部」、「占有済み」を示すマッピングモジュールにより、全領域カバーしながら移動させるようにする技術（特許文献１参照）を応用することが考えられる。
また、既に探索したところでセンシングした移動可能領域を示す屋内構造データを記憶し、移動できる方向に順次移動する技術（特許文献２参照）を応用することが考えられる。
さらに、障害物を探す操作、または、自機が移動可能な領域を取得するためにセンシング済み領域を記憶する技術（特許文献３，４参照）を応用することが考えられる。
特開２０１６－０９５８５８号公報 特開２０１０－２１７９９５号公報 特願２００３－０９２３４７号公報 特願２００２－０７３７７０号公報
しかしながら、特許文献１の技術においては、全領域をカバーして移動する技術であるため、効率よく周囲を探索することはできない。
また、特許文献２の技術においては、探索されたときの情報から得られる移動可能領域が記憶されるものであり、効率よく周囲を探索するための情報としては利用することができない。
さらに、特許文献３，４の技術においては、障害物を探す操作、または、自機が移動可能な領域を取得するためにセンシング済み領域を記憶しているが、障害物の表面はセンシングされておらず、また、記憶もされていないので、障害物の表面状態に応じて効率よく周囲を探索することができない。
本開示は、このような状況に鑑みてなされたものであり、特に、周囲を効率よく探索できるようにするものである。
本開示の一側面の制御装置は、自己位置を推定する自己位置推定部と、周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、前記障害物の表面をセンシングする表面センシング部と、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部とを含み、前記ルート計画部は、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する制御装置である。
前記優先度設定部には、前記経由地を全て通過するルートのうち、優先条件に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定させるようにすることができる。
前記優先条件は、総移動距離、または総消費電力とすることができ、前記優先度コストは、移動距離に応じて設定されるコスト、または、消費電力に応じて設定されるコストとすることができる。
前記優先度設定部は、前記経由地を全て通過するルートのうち、前記優先条件、および機体運動情報に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定させるようにすることができる。
前記機体運動情報は、前記優先条件が総移動距離である場合、最高速度、最小回転半径、および経路上の超えられる段差の高さの情報とすることができ、前記優先条件が総消費電力である場合、単位距離当たりの電力消費量、およびバッテリの充電容量の情報とすることができる。
前記ルート計画部には、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて、前記経由地を全て経由する前記ルートを計画させるようにすることができる。
前記ルート計画部には、前記経由地を全て経由する全てのルートについて、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて設定されるルートコストを算出させ、前記ルートコストが最小となるルートを選択させるようにすることができる。
前記移動条件には、優先度の高い順に前記経由地を通過するという条件、経由地の優先度を無視しても最短時間で移動するという条件、または、優先度の高い経由地ほど、早く到着するという条件を含ませるようにすることができる。
前記移動条件が、優先度の高い経由地ほど、早く到着するという条件である場合、前記ルート計画部には、前記優先度と、前記経由地までの到着時間との積を前記ルートコストとして設定し、前記全てのルートのうち、前記ルートコストが最小となるルートを選択させるようにすることができる。
前記自己位置推定部には、時系列に供給されるセンサ情報からなる時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部を含ませるようにすることができ、前記表面センシング済み領域記録部には、前記時系列情報自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録させるようにすることができる。
前記時系列情報は、LIDAR（Light Detection and Ranging、Laser Imaging Detection and Ranging）により検出される３次元点群情報、および車輪エンコーダにより検出される位置情報、姿勢、速度、加速度、および角速度とすることができ、前記障害物位置情報生成部には、前記３次元点群情報に基づいて、前記周辺の障害物の相対位置および方向からなる障害物位置情報を生成させるようにすることができる。
前記自己位置推定部には、現在のセンシング結果を出力するセンサ情報からなる現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部をさらに含ませるようにすることができる。
前記現在情報は、ステレオカメラからなる前記表面センシング部により撮像される視差画像とすることができ、前記現在情報自己位置推定部には、前記現在情報である視差画像に基づいて、デプス画像を生成させ、前記デプス画像より画像特徴量を抽出させ、前記画像特徴量と対応付けられている位置に基づいて、前記自己位置を推定させ、現在情報自己位置として出力させ、前記自己位置推定部には、前記時系列情報自己位置および前記現在情報自己位置を統合して、前記自己位置として出力させるようにすることができる。
前記位置と前記画像特徴量とが対応付けて記憶される位置画像特徴量記憶部をさらに含ませるようにすることができ、前記自己位置推定部には、前記時系列情報自己位置および前記現在情報自己位置を統合して、自己位置として出力するとき、前記現在情報自己位置に対応する画像特徴量と、前記自己位置とを対応付けて、前記位置画像特徴量記憶部に記憶させるようにすることができる。
前記自己位置が不定状態の場合、前記表面センシング済み領域記録部には、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録させるようにすることができる。
本開示の一側面の制御方法は、自己位置を推定する自己位置推定処理と、周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成処理と、前記障害物の表面をセンシングする表面センシング処理と、前記自己位置、前記障害物位置情報、および前記表面センシング処理の表面センシング可能範囲に基づいて、前記表面センシング処理の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録処理と、前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画処理とを含み、前記ルート計画処理は、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定処理を含み、前記優先度設定処理により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する制御方法である。
本開示の一側面のプログラムは、自己位置を推定する自己位置推定部と、周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、前記障害物の表面をセンシングする表面センシング部と、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部としてコンピュータを機能させ、前記ルート計画部は、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画するプログラムである。
本開示の一側面の移動体は、自己位置を推定する自己位置推定部と、周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、前記障害物の表面をセンシングする表面センシング部と、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部と、前記ルート計画部により計画された前記ルートに基づいて、行動計画を生成する行動計画生成部と、前記行動計画生成部により決定された行動計画に基づいて移動体の動作を制御する制御部とを含み、前記ルート計画部は、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する移動体である。
本開示の一側面においては、自己位置が推定され、周辺の障害物の位置からなる障害物位置情報が生成され、前記障害物の表面がセンシングされ、前記自己位置、前記障害物位置情報、および表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面が、前記障害物の表面センシング済み領域として記録され、前記障害物の表面センシング済み領域に基づいて、ルートが計画され、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域が設定され、前記未済領域を分割した分割領域が経由地として設定され、前記経由地に優先度が設定され、設定された前記優先度に基づいて、前記経由地を経由する前記ルートが計画される。
本開示の一側面によれば、特に、効率よく周囲を探索することが可能となる。
本開示の概要を説明する移動体の構成例を示す図である。 本開示の概要を説明する図である。 本開示の移動体を制御する移動体制御システムの構成例を説明するブロック図である。 本開示の認識処理部および行動計画処理部の周辺の構成例の詳細なブロック図である。 障害物認識結果を説明する図である。 表面センシング可能範囲を説明する図である。 表面センシング済み領域を説明する図である。 優先度の設定方法を説明する図である。 自己位置推定処理を説明するフローチャートである。 図９の時系列情報自己位置推定処理を説明するフローチャートである。 図９の現在情報自己位置推定処理を説明するフローチャートである。 障害物認識処理を説明するフローチャートである。 表面センシング済み領域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）等の技術を用いた高精度なマップとされる。自己位置推定部１３２は、推定処理の結果を示すデータを状況分析部１３３のマップ解析部１５１、及び、状況認識部１５２等に供給する。また、自己位置推定部１３２は、自己位置推定用マップを記憶部１０９に記憶させる。
さらに、自己位置推定部１３２は、センサ群１１２より供給される検出結果に基づいて、時系列に供給される、例えば、LIDARや車輪エンコーダからの時系列情報をデータベースに蓄積すると共に、蓄積した時系列の情報に基づいて、自己位置を推定し、時系列情報自己位置として出力する。また、自己位置推定部１３２は、センサ群１１２より供給される、例えば、ステレオカメラにより撮像された視差画像に基づいて得られるデプス画像などの現在の検出結果に基づいて、自己位置を推定し、現在情報自己位置として出力する。さらに、自己位置推定部１３２は、時系列情報自己位置と現在情報自己位置とを、例えば、カルマンフィルタや粒子フィルタなどを用いて統合して自己位置推定結果として出力する。尚、ここで出力される時系列情報自己位置、および現在情報自己位置は、いずれも自己位置と共に、自己位置が推定される際に求められる移動体１１の方向の情報を含むものである。したがって、以降において、時系列情報自己位置、および現在情報自己位置、並びに単に自己位置と称する場合、それらには、併せて、その自己位置が推定されたときの、移動体１１の方向の情報が含まれているものとする。
状況分析部１３３は、移動体及び周囲の状況の分析処理を行う。また、状況分析部１３３は、自己位置不定状態であるかに基づいて、動作モードを通常モードから探索モードに切り替えて、行動計画処理部１２２に対して、探索モードでの行動計画処理を実行させて、周囲の情報を効率よく探索させるように行動を計画させる。状況分析部１３３は、マップ解析部１５１、状況認識部１５２、及び、状況予測部１５３を備える。
マップ解析部１５１は、自己位置推定部１３２及び移動体外部情報検出部１４１等の移動体制御システム１００の各部からのデータ又は信号を必要に応じて用いながら、記憶部１０９に記憶されている各種のマップの解析処理を行い、自律移動の処理に必要な情報を含むマップを構築する。マップ解析部１５１は、構築したマップを、状況認識部１５２、状況予測部１５３、並びに、計画部１３４のルート計画部１６１、行動計画部１６２、及び、動作計画部１６３等に供給する。
状況認識部１５２は、自己位置推定部１３２、移動体外部情報検出部１４１、移動体内部情報検出部１４２、移動体状態検出部１４３、及び、マップ解析部１５１等の移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体に関する状況の認識処理を行う。例えば、状況認識部１５２は、移動体の状況、移動体の周囲の状況、及び、移動体の運転者の状況等の認識処理を行う。また、状況認識部１５２は、必要に応じて、移動体の周囲の状況の認識に用いるローカルマップ（以下、状況認識用マップと称する）を生成する。状況認識用マップは、例えば、占有格子地図（Occupancy Grid Map）、道路地図（Lane Map）、または、点群地図（Point Cloud Map）とされる。
認識対象となる移動体の状況には、例えば、移動体の位置、姿勢、動き（例えば、速度、加速度、移動方向等）、並びに、異常の有無及び内容等が含まれる。認識対象となる移動体の周囲の状況には、例えば、周囲の静止物体の種類及び位置、周囲の動物体の種類、位置及び動き（例えば、速度、加速度、移動方向等）、周囲の道路の構成及び路面の状態、並びに、周囲の天候、気温、湿度、及び、明るさ等が含まれる。認識対象となる運転者の状態には、例えば、体調、覚醒度、集中度、疲労度、視線の動き、並びに、運転操作等が含まれる。
状況認識部１５２は、認識処理の結果を示すデータ（必要に応じて、状況認識用マップを含む）を自己位置推定部１３２及び状況予測部１５３等に供給する。また、状況認識部１５２は、状況認識用マップを記憶部１０９に記憶させる。また、状況認識部１５２は、例えば、電源オン直後の状態や、抱き上げなどであるか否かに基づいて、自己位置不定状態であるか否かを判定する。ここで、自己位置不定状態である場合には、状況認識部１５２は、動作モードを探索モードに切り替える。動作モードが探索モードに切り替わると、状況認識部１５２は、行動計画処理部１２２に対して、探索モードでの行動計画処理を実行させて、周囲の情報を効率よく探索させるように行動を計画させる。より具体的には、状況認識部１５２は、センサ群１１２により供給される情報に基づいて、センシング済みの領域の情報を生成し、探索モードになった場合、センシング済みの領域の情報を行動計画処理部１２２に出力し、周囲のうち、センシングしていない領域を効率よく探索させるように行動を計画させる。
状況予測部１５３は、マップ解析部１５１、及び状況認識部１５２等の移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体に関する状況の予測処理を行う。例えば、状況予測部１５３は、移動体の状況、移動体の周囲の状況、及び、運転者の状況等の予測処理を行う。
予測対象となる移動体の状況には、例えば、移動体の挙動、異常の発生、及び、移動可能距離等が含まれる。予測対象となる移動体の周囲の状況には、例えば、移動体の周囲の動物体の挙動、信号の状態の変化、及び、天候等の環境の変化等が含まれる。予測対象となる運転者の状況には、例えば、運転者の挙動及び体調等が含まれる。
状況予測部１５３は、予測処理の結果を示すデータを、及び状況認識部１５２からのデータとともに、計画部１３４のルート計画部１６１、行動計画部１６２、及び、動作計画部１６３等に供給する。
ルート計画部１６１は、マップ解析部１５１及び状況予測部１５３等の移動体制御システム１００の各部からのデータ又は信号に基づいて、目的地までのルートを計画する。例えば、ルート計画部１６１は、グローバルマップに基づいて、現在位置から指定された目的地までのルートを設定する。また、例えば、ルート計画部１６１は、渋滞、事故、通行規制、工事等の状況、及び、運転者の体調等に基づいて、適宜ルートを変更する。ルート計画部１６１は、計画したルートを示すデータを行動計画部１６２等に供給する。
行動計画部１６２は、マップ解析部１５１及び状況予測部１５３等の移動体制御システム１００の各部からのデータ又は信号に基づいて、ルート計画部１６１により計画されたルートを計画された時間内で安全に移動するための移動体の行動を計画する。例えば、行動計画部１６２は、発進、停止、進行方向（例えば、前進、後退、左折、右折、方向転換等）、移動速度、及び、追い越し等の計画を行う。行動計画部１６２は、計画した移動体の行動を示すデータを動作計画部１６３等に供給する。
より詳細には、行動計画部１６２は、それぞれルート計画部１６１により計画されたルートのそれぞれについて、計画された時間内で安全に移動するための移動体の行動計画の候補を行動計画候補として生成する。より具体的には、行動計画部１６２は、例えば、環境を格子状に区切って、到達判定および経路の重みを最適化して最良のパスを生成するA*algorithm（A star探索アルゴリズム）、道路中心線に従って経路を設定するLane algorithm、および、自己位置からインクリメンタルに到達可能な場所へのパスを適切に枝刈りしながら伸ばしていくRRT(Rapidly-exploring Random Tree) algorithmなどにより行動計画候補を生成する。
動作計画部１６３は、マップ解析部１５１及び状況予測部１５３等の移動体制御システム１００の各部からのデータ又は信号に基づいて、行動計画部１６２により計画された行動を実現するための移動体の動作を計画する。例えば、動作計画部１６３は、加速、減速、及び、移動軌道等の計画を行う。動作計画部１６３は、計画した移動体の動作を示すデータを、動作制御部１３５等に供給する。
動作制御部１３５は、移動体の動作の制御を行う。
より詳細には、動作制御部１３５は、移動体外部情報検出部１４１、移動体内部情報検出部１４２、及び、移動体状態検出部１４３の検出結果に基づいて、衝突、接触、危険地帯への進入、運転者の異常、移動体の異常等の緊急事態の検出処理を行う。動作制御部１３５は、緊急事態の発生を検出した場合、急停止や急旋回等の緊急事態を回避するための移動体の動作を計画する。
また、動作制御部１３５は、動作計画部１６３により計画された移動体の動作を実現するための加減速制御を行う。例えば、動作制御部１３５は、計画された加速、減速、又は、急停止を実現するための駆動力発生装置又は制動装置の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部１０７に供給する。
動作制御部１３５は、動作計画部１６３により計画された移動体の動作を実現するための方向制御を行う。例えば、動作制御部１３５は、動作計画部１６３により計画された移動軌道又は急旋回を実現するためのステアリング機構の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部１０７に供給する。
＜認識処理部の詳細な構成例＞
次に、図４を参照して、図３の移動体制御システム１００のうち、認識処理部３１に対応する自律移動制御部１１０の認識処理部１２１の具体的な構成例について説明する。
尚、ここでは、移動体が車輪型移動ロボットからなる移動体１１である例について説明するが、その他の歩行ロボットや車両などの移動体であってもよい。
センサ群１１２は、LIDAR２０１、車輪エンコーダ２０２、およびステレオカメラ２０３を備えているものとする。当然のことながら、４足歩行ロボットや車両等であれば、その他のセンサを使用するようにしてもよい。
LIDAR２０１は、移動体１１の周囲の物体までの距離を３次元点群データとして取得し、検出部１３１を介して、自己位置推定部１３２の時系列情報自己位置推定部２２１、および状況認識部１５２の障害物認識部２４１に時系列情報として出力する。
車輪エンコーダ２０２は、移動体１１の位置情報（Ｘ，Ｙ，Ｚ）、姿勢（クォータニオン）、速度（ｄｘ，ｄｙ，ｄｚ）、加速度（ａｘ，ａｙ，ａｚ）、および角速度（ｗｘ，ｗｙ，ｗｚ）を検出し、検出部１３１を介して、時系列情報自己位置推定部２２１に時系列情報として出力する。
ステレオカメラ２０３は、移動体１１の周囲の視差画像を撮像して、検出部１３１を介して、デプス画像生成部２２６に出力する。
自己位置推定部１３２は、時系列情報自己位置推定部２２１、時系列情報DB２２２、現在情報自己位置推定部２２３、位置画像特徴量DB２２４、および自己位置推定結果統合部２２５を備えている。
時系列情報自己位置推定部２２１は、LIDAR２０１および車輪エンコーダ２０２より供給される、自己位置、および周囲の障害物の位置等の時系列情報を時系列情報DB２２２に格納する。また、時系列情報自己位置推定部２２１は、過去から現在までの時系列情報を必要に応じて読み出し、読み出した過去から現在までの時系列情報に基づいて、自己位置を推定し、自己位置推定結果統合部２２５に供給する。ここで、時系列情報自己位置推定部２２１は、読み出した過去から現在までの時系列情報に基づいて、例えば、SLAM（Simultaneous Localisation and Mapping）により自己位置を推定する。
時系列情報自己位置推定部２２１による、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２２４より、抽出した特徴量に対応する位置の情報に基づいて、自己位置を推定し、自己位置推定結果統合部２２５に供給する。尚、現在情報自己位置推定部２２３により現在情報に基づいて推定される自己位置を現在情報自己位置と称する。
自己位置推定結果統合部２２５は、時系列情報自己位置と、現在情報自己位置とを、例えば、LIDAR２０１、車輪エンコーダ２０２、およびステレオカメラ２０３のそれぞれの状況に応じた信頼度などに応じて統合し、自己位置推定結果として状況認識部１５２に出力する。尚、自己位置推定結果統合部２２５は、求められた位置の情報と、そのタイミングで検出されたデプス画像の画像特徴量とを対応付けて位置画像特徴量２２４に登録する。すなわち、自己位置として推定された位置に対応付けて、そのタイミングにおけるデプス画像の特徴量を登録させることにより、一度、通った位置については、対応するデプス画像の画像特徴量が確実に存在することになるので、高精度に自己位置を推定することが可能となる。
状況認識部１５２は、障害物認識部２４１、表面センシング済み領域記録部２４２、表面センシング可能範囲情報記憶部２４３、表面センシング済み領域DB２４４、および探索モード切替部２４５を備えている。
障害物認識部２４１は、LIDAR２０１により検出された、移動体１１の周囲の障害物などの物体までの距離の３次元点群情報に基づいて、移動体１１の周囲の障害物を認識し、認識結果を表面センシング済み領域記録部２４２に出力する。
表面センシング済み領域記録部２４２は、表面センシング可能範囲情報記憶部２４３に記憶されている、ステレオカメラ２０３の撮像範囲となる視野角や撮像距離の情報、移動体１１の方向を含む時系列情報自己位置の情報、および障害物認識結果に基づいて、ステレオカメラ２０３による、障害物の表面センシング済み領域を求めて、表面センシング済み領域DB２４４に登録する。
すなわち、時系列情報自己位置の情報（移動体１１自身の方向の情報を含む）と障害物認識結果とから、例えば、図５で示されるように、障害物Ｂ１と、自らである移動体１１との位置関係が明確になる。尚、図５においては、占有格子地図（Occupancy Grid Map）により、移動体１１と障害物Ｂ１との位置関係が示されているが、その他のマップであってもよい。
さらに、表面センシング可能範囲情報とは、例えば、図６中の点線で示されるステレオカメラ２０３（図６中のセンサ群２１）により障害物Ｂ１の表面をセンシングできる範囲Ｚ１を特定するための情報である。ここで、図６の範囲Ｚ１は、ステレオカメラ２０３（図６中のセンサ群２１）の視野角θ１と撮像可能な距離ｄ１とにより特定される、ステレオカメラ２０３により障害物Ｂ１の表面をセンシングできる範囲である。
表面センシング済み領域記録部２４２は、移動体１１と障害物Ｂ１との位置関係の情報、および、表面センシング可能範囲情報に基づいて、センシング済みの領域（視差画像として撮像している障害物Ｂ１上の領域）を、表面センシング済み表面領域として求める。すなわち、例えば、図７の場合、障害物Ｂ１の表面のうち、点線で示される表面センシング可能範囲Ｚ１に含まれる領域が、表面センシング済み表面領域Ｒ１として求められる。
そして、表面センシング済み領域記録部２４２は、求めた表面センシング済み領域の情報を表面センシング済み領域DB２４４に、移動体１１の時系列情報推定位置や方向、および障害物認識結果と対応付けて記録する。
探索モード切替部２４５は、電源オンの直後であるか否かに基づいて、自己位置不定状態であるか否かを判定し、自己位置不定の状態ではない場合、通常モードによる動作モードで動作し、自己位置推定結果と、障害物認識結果とを行動計画処理部１２２に対して出力する。尚、ここでは、自己位置不定状態が、電源オンの直後である場合について説明するものとするが、自己位置不定状態は、それ以外の状態でもよい。自己位置不定状態は、例えば、抱き上げ状態の検出やスリップなど、急激に自己推定結果が変化し、自己位置の情報の連続性が失われるような状態であれば、その他の状態であってもよい。
行動計画処理部１２２は、ルート計画部１６１、行動計画部１６２、および動作計画部１６３を備えているが、動作モードが通常モードの場合と、探索モードでは異なる処理を実行する。
通常モードの動作においては、ルート計画部１６１が、自己位置推定結果と、障害物認識結果とに基づいて、目的地までのルートを計画し、行動計画部１６２が、計画されたルートに基づいて行動計画を生成し、動作計画部１６３が、行動計画に応じた動作計画を生成し、行動制御処理部１２３に出力する。
一方、行動計画処理部１２２は、探索モード切替部２４５から探索モードでの動作が指示されると、動作モードが探索モードに変化する。このとき、探索モード切替部２４５は、行動計画処理部１２２に対して、表面センシング済み領域DB２４４に格納されている表面センシング済み領域の情報を供給し、探索モードでの操作を指示する。
動作モードが探索モードの場合、自己位置不定状態であるため、改めて自己位置を特定するために、周囲を探索し、周囲の障害物の位置などを効率的に収集できるように行動を計画する必要がある。
ルート計画部１６１は、探索モード切替部２４５を介して、表面センシング済み領域の情報に基づいて、自己位置である移動体１１の位置と方向に対して、どの向きのどの距離に障害物が存在するのかを把握する。
例えば、図８で示されるように、移動体１１の周囲に障害物Ｂ２１乃至Ｂ２３が存在し、障害物Ｂ２１乃至Ｂ２３における表面のセンシングが完了している表面センシング済み領域Ｒ２１乃至Ｒ２３が把握できるような場合を考える。
ルート計画部１６１は、表面センシングが未完了の領域を複数の領域に分割し、例えば、分割した領域を個別に経由地に設定する。例えば、図８の場合、ルート計画部１６１は、例えば、分割した領域のそれぞれに経由地Ｐ１乃至Ｐ４を設定し、経由地Ｐ１乃至Ｐ４を全て通る、全てのパターンのルートを計画する。
ルート計画部１６１は、優先度決定部２８１を備えており、経由地Ｐ１乃至Ｐ４を全て通る、全てのパターンのルートの情報と、優先条件とに基づいて、経由地Ｐ１乃至Ｐ４のそれぞれに優先度を設定させる。
例えば、図８において、経由地Ｐ１乃至Ｐ４の全てを必ず通る、全てのルートを考えた場合の、優先条件と機体運動情報により求められる優先度コストが最小となるルートにおける経由地Ｐ１乃至Ｐ４の順序に応じて設定される値である。
ここで、優先条件は、総移動距離や総消費電力などであり、ルートを設定する上で最も優先すべき条件であり、様々な条件を設定することができる。
また、移動体１１の機体運動情報とは、移動体１１の運動性能に係る情報であり、例えば、優先条件が総移動距離である場合、移動体１１の最高速度、最高加速度、最小回転半径、および経路上の超えられる段差の高さなどである。さらに、優先条件が総消費電力である場合、移動体１１の機体運動情報とは、移動体１１の単位距離の電力消費とバッテリの充電容量などである。
さらに、優先度コストとは、優先条件と機体運動情報に基づいて設定されるコストであり、優先条件が総移動距離であれば、移動距離に係るコストであり、優先条件が総消費電力であれば、電力消費に係るコストである。ルート計画部１６１の優先度決定部２８１は、全てのルートに対する優先度コストを算出し、優先度コストが最小になるルートにおける経由地の通過順序を優先度に設定する。
例えば、経由地Ｐ１，Ｐ２，Ｐ３，Ｐ４の順序で移動する第１のルートと、経由地Ｐ１，Ｐ３，Ｐ２，Ｐ４の順序で移動する第２のルートとがあり、この２つのルートについて総移動距離を優先条件とするときの優先度コストを考える。仮に、第１のルートより、第２のルートの方が、総移動距離が短く、優先度コストが小さい場合、優先度は、第２のルートにおける経由地の通過順序で設定される。すなわち、第２のルートにおける経由地Ｐ１乃至Ｐ４の順序に従って、経由地Ｐ１の優先度が第１位に設定され、経由地Ｐ３の優先度が第２位に設定され、経由地Ｐ２の優先度が第３位に設定され、経由地Ｐ４の優先度が第４位に設定される。
そして、ルート計画部１６１は、各経由地Ｐ１乃至Ｐ４を通る全てのルートを計画し、優先度と、移動体１１のセンサ群２１によりセンシング可能な領域や、移動条件とに基づいて、ルート毎にルートコストを計算し、最もルートコストが低いルートを選択し、選択したルートに基づいた行動計画を生成する。
ここで、移動条件とは、探索するルートを設定する上での移動に係る優先すべき条件であり、例えば、経由地は優先度の高い順に移動する、経由地の優先度を無視しても最短時間で移動するといった移動に係る優先すべき条件であり、自由に設定することができる。
また、ルートコストとは、経由地の優先度と、ルート設定に優先される移動条件とに基づいて設定される、全ての可能なルート集合におけるそのルートの相対的な良しあしを定量的に表す手段であるである。例えば、経路として選択可能な空間を格子状に区切って、到達判定を行い、さらに、経路毎の重みを最適化してコストが最小となる最良のパス（ルート）を設定するA*（スター）アルゴリズムを用いてルートを設定させる場合を考える。
このとき、優先度の高い経由地ほど、早く到着するという移動条件を満たすルートを設定させるには、経由地毎の優先度と、対応する経由地までの移動時間との積をルートコストに設定し、ルートコストが最小となるルートを選択させるようにすることで、複数の考えられるルートの中から、優先度の高い経由地ほど、早く到着するルートが設定される。
ルート計画部１６１は、このような処理により、効率よく周囲の障害物の表面センシングを実現させるための探索に必要とされるルートを計画する。
これにより、自己位置不定状態となった場合でも、効率よく周囲を探索しながらセンシングすることにより、効率よく周囲の情報を取得して、改めて自己位置を推定することが可能となる。結果として、自己位置不定状態に陥っても、迅速に自己位置を求められる状態に復帰することができ、動作モードを探索モードから通常モードに早期に復帰させることが可能となる。
＜自己位置推定処理＞
次に、図９のフローチャートを参照して、自己位置推定処理について説明する。
ステップＳ１１において、時系列情報自己位置推定部２２１は、時系列情報自己位置推定処理を実行して、時系列情報自己位置を推定して、自己位置推定結果統合部２２５、および表面センシング済み領域記録部に出力する。尚、時系列情報自己位置推定処理については、図１０のフローチャートを参照して、後述する。
ステップＳ１２において、現在情報自己位置推定部２２３は、現在情報自己位置推定処理を実行して、現在情報自己位置を推定して、自己位置推定結果統合部２２５に出力する。尚、現在情報自己位置推定処理については、図１１のフローチャートを参照して、後述する。
ステップＳ１３において、自己位置推定結果統合部２２５は、時系列情報自己位置および現在情報自己位置を、例えば、カルマンフィルタや粒子フィルタにより統合し、統合した自己位置の情報を自己位置推定結果として状況認識部１５２に出力する。
ステップＳ１４において、自己位置推定結果統合部２２５は、自己位置推定結果と、その自己位置推定結果を特定するために利用した現在情報である視差画像より求められたデプス画像の特徴量とを対応付けて位置画像特徴量DB２２４に登録する。
すなわち、自己位置として推定された位置と、その位置を特定するために利用された現在情報である視差画像より求められたデプス画像の特徴量とが対応付けられて位置画像特徴量DB２２４に登録される。これにより、一度通過した位置に対応する現在情報である視差画像より求められたデプス画像の特徴量が、位置に対応付けられて新たな情報として蓄積される。結果として、再び同一の位置を通過するときには、高精度に自己位置推定することが可能となる。
また、以上の処理により、時系列情報自己位置と現在情報自己位置とが順次求められて、相互に統合された位置が自己位置推定結果として出力される。
＜時系列情報自己位置推定処理＞
次に、図１０のフローチャートを参照して、時系列情報自己位置推定処理について説明する。
ステップＳ３１において、LIDAR２０１は、移動体１１の周囲の物体までの距離を３次元点群データとして検出し、検出部１３１を介して、自己位置推定部１３２の時系列情報自己位置推定部２２１に時系列情報として出力する。
ステップＳ３２において、車輪エンコーダ２０２は、移動体１１の位置情報、姿勢、速度、加速度、および角速度を検出し、検出部１３１を介して、時系列情報自己位置推定部２２１に時系列情報として出力する。
ステップＳ３３において、時系列情報自己位置推定部２２１は、最新の時系列情報を取得し、時系列情報DB２２２に登録する。
ステップＳ３４において、時系列情報自己位置推定部２２１は、時系列情報DB２２２に登録されている最新の時系列情報と、過去の時系列情報とから、例えば、カルマンフィルタや粒子フィルタを用いて、自己位置を推定する。そして、時系列情報自己位置推定部２２１は推定結果である自己位置を時系列情報自己位置情報として自己位置推定結果統合部２２５に出力する。
以上の処理により、LIDAR２０１および車輪エンコーダ２０２により検出される時系列情報が順次検出されて、時系列情報DB２２２に順次蓄積されて、時系列に蓄積された過去から現在までの時系列に蓄積された時系列情報に基づいて、自己位置が推定されて、時系列情報自己位置として自己位置推定結果統合部２２５に出力される。
＜現在情報自己位置推定処理＞
次に、図８のフローチャートを参照して、現在情報自己位置推定処理について説明する。
ステップＳ５１において、ステレオカメラ２０３は、移動体１１の周囲の視差画像を撮像して、検出部１３１を介して、現在情報自己位置推定部２２３に現在情報として出力する。
ステップＳ５２において、現在情報自己位置推定部２２３は、ステレオカメラ２０３より供給される視差画像に基づいて、デプス画像（距離画像）を生成する。
ステップＳ５３において、現在情報自己位置推定部２２３は、生成したデプス画像に基づいて、画像特徴量を抽出する。
ステップＳ５４において、現在情報自己位置推定部２２３は、位置と画像特徴量とが対応付けて記憶されている位置画像特徴量DB２２４より、抽出した画像特徴量に対応する位置の情報に基づいて、自己位置を推定する。そして、現在情報自己位置推定部２２３は、推定結果である自己位置を現在情報自己位置として自己位置推定結果統合部２２５に供給する。
以上の処理により、ステレオカメラ２０３により視差画像が現在情報として撮像され、視差画像よりデプス画像が生成され、生成されたデプス画像の画像特徴量が抽出されて、画像特徴量に対応付けて登録されている位置情報に基づいて、現在情報自己位置が推定され、自己位置推定結果統合部２２５に出力される。
＜障害物認識処理＞
次に、図１２のフローチャートを参照して、障害物認識処理について説明する。
ステップＳ７１において、LIDAR２０１は、移動体１１の周囲の物体までの距離を３次元点群データとして検出し、検出部１３１を介して、自己位置推定部１３２の時系列情報自己位置推定部２２１に時系列情報として出力する。尚、この処理は、図１０のフローチャートを参照して説明したステップＳ３１の処理と同様である。
ステップＳ７２において、障害物認識部２４１は、移動体１１の周囲の物体までの距離を３次元点群データに基づいて、移動体１１の周囲の障害物の位置を認識する。
ステップＳ７３において、障害物認識部２４１は、認識している移動体１１の周囲の障害物の位置の情報に基づいて、格子状地図を生成して、表面センシング済み領域記録部２４２、および探索モード切替部２４５に出力する。
以上の処理により、移動体１１の周囲の障害物の位置の情報を表す、障害物認識結果が、例えば、図５で示されるような格子状地図として生成されて、表面センシング済み領域記録部２４２、および探索モード切替部２４５に出力される。
＜表面センシング済み領域記録処理＞
次に、図１３のフローチャートを参照して、表面センシング済み領域記録処理について説明する。尚、表面センシング済み領域記録処理は、後述する動作モードが探索モードになったときに実行される処理である。
ステップＳ８１において、表面センシング済み領域記録部２４２は、探索モード切替部２４５より表面センシング済み領域記録処理の開始が指示されたか否かを判定し、開始が指示されるまで、同様の処理が繰り返される。そして、ステップＳ８１において、表面センシング済み領域記録処理の開始が指示された場合、処理は、ステップＳ８２に進む。
ステップＳ８２において、表面センシング済み領域記録部２４２は、表面センシング済み領域DB２４４をリセットする。
ステップＳ９３において、表面センシング済み領域記録部２４２は、障害物認識部２４１より障害物認識結果を取得する。
ステップＳ９４において、表面センシング済み領域記録部２４２は、時系列情報自己位置推定部２２１より時系列情報自己位置の情報を取得する。
ステップＳ９５において、表面センシング済み領域記録部２４２は、時系列情報自己位置、障害物認識結果より取得できる障害物位置、および表面センシング可能範囲情報記憶部２４３に記憶されている表面センシング可能範囲の情報に基づいて、図７を参照して説明したように、表面センシング済み領域を求めて、表面センシング済み領域DB２４４に記録する。
ステップＳ９６において、表面センシング済み領域記録部２４２は、処理の終了が指示されたか否かを判定し、終了が指示されていない場合、処理は、ステップＳ９３に戻る。すなわち、処理の終了が指示されるまで、ステップＳ９３乃至Ｓ９６の処理が繰り返されて、順次、表面センシング済み領域の情報が、表面センシング済み領域DB２４４に蓄積される。
そして、ステップＳ９５において、処理の終了が指示された場合、処理は、終了する。
以上の処理により、探索モードにおいて、移動体１１の周囲のうち、ステレオカメラ２０３によりにより撮像されている（センシング済みの）障害物の表面の領域の情報が、例えば、図８で示されるように、障害物Ｂ２１乃至Ｂ２３の表面センシング済み領域Ｒ２１乃至Ｒ２３として蓄積される。
＜自律移動制御処理＞
次に、図１４のフローチャートを参照して、図４の移動体制御システム１００における自律移動制御処理について説明する。
ステップＳ１２１において、探索モード切替部２４５は、自己位置不定状態であるか否かを判定する。例えば、探索モード切替部２４５は、例えば、電源投入直後であるか否かに基づいて、自己位置不定状態であるか否かを判定する。
ステップＳ１２１において、自己位置不定状態ではないと判定された場合、処理は、ステップＳ１２２に進む。
ステップＳ１２２において、探索モード切替部２４５は、自己位置推定結果と、障害物認識結果とを行動計画処理部１２２の計画部１３４におけるルート計画部１６１に出力する。
ステップＳ１２３において、ルート計画部１６１は、目的地までのルートを計画し、行動計画部１６２に出力する。
ステップＳ１２４において、行動計画部１６２は、目的地までのルートに基づいて、行動を計画し、行動計画として動作計画部１６３に出力する。
ステップＳ１２５において、動作計画部１６３は、行動計画部１６２より供給された行動計画に基づいて、動作を計画し、動作計画として行動制御処理部１２３に出力する。
ステップＳ１２６において、行動制御処理部１２３は、動作計画に基づいて、アクチュエータ群１１３の各アクチュエータ２３ａの動作を制御して、移動体１１の行動を制御する。
ステップＳ１２７において、終了が指示されたか否かが判定され、終了が指示されていないとみなされた場合、処理は、ステップＳ１２１に戻る。すなわち、自己位置不定状態とならない限り、ステップＳ１２１乃至Ｓ１２７の処理、すなわち、動作状態が通常モードである場合の処理が繰り返されて、移動体１１は、自律的な移動を継続する。
一方、ステップＳ１２１において、自己位置不定状態であるとみなされた場合、処理は、ステップＳ１２８に進む。
ステップＳ１２８において、探索モード切替部２４５は、動作モードを探索モードに切り替えて、探索モード処理を実行させ、効率よく移動体１１の周囲を探索して、ステレオカメラにより周囲の情報を迅速に取得できるように制御する。そして、現在情報自己位置が適切に推定できる状態になるまで、探索モードの処理が継続され、現在情報自己位置が推定できる状態になった場合、動作モードが、探索モードから通常モードに復帰する。
＜探索モード処理＞
次に、図１５のフローチャートを参照して、探索モード処理について説明する。
ステップＳ１５１において、探索モード切替部２４５は、表面センシング済み領域記録部２４２に対して、表面センシング済み領域DB生成処理を開始するように指示する。この指示により、図１３のフローチャートを参照して説明した表面センシング済み領域DB生成処理が開始される。
ステップＳ１５２において、探索モード切替部２４５は、表面センシング済み領域DB２４４より表面センシング済み領域の情報を行動計画処理部１２２の計画部１３４におけるルート計画部１６１に出力する。
ステップＳ１５３において、ルート計画部１６１は、表面センシング済み領域の情報に基づいてセンシングしていない領域を複数に分割し、分割した各領域を経由地として設定する。
ステップＳ１５４において、ルート計画部１６１は、分割した各領域からなる全経由地を通過するルートの全てのパターンを計画する。
ステップＳ１５５において、ルート計画部１６１は、優先度決定部２８１を制御して、図８を参照して説明したように、優先条件に応じて、計画されたルート毎の優先度コストを求めて、優先度が最小となるルートに基づいて経由地の優先度を設定させる。
ステップＳ１５６において、ルート計画部１６１は、経由地の優先度と、移動条件に基づいて、全てのルートについてルートコストを算出する。
ステップＳ１５７において、ルート計画部１６１は、ルートコストが最小となるルートを探索ルートに決定し、行動計画部１６２に出力する。
ステップＳ１５８において、行動計画部１６２は、決定した探索ルートに基づいて、行動を計画し、行動計画として動作計画部１６３に出力する。
ステップＳ１５９において、動作計画部１６３は、行動計画部１６２より供給された行動計画に基づいて、動作を計画し、動作計画として行動制御処理部１２３に出力する。
ステップＳ１６０において、行動制御処理部１２３は、動作計画に基づいて、アクチュエータ群１１３の各アクチュエータ２３ａの動作を制御して、移動体１１の行動を制御する。
ステップＳ１６１において、探索モード切替部２４５は、自己位置不定状態でなくなったか否かにより、探索モード処理を終了させるか否かを判定する。自己位置不定状態が電源オン直後であるような場合については、電源オンから所定時間が経過したか否か、または、時系列情報が所定数以上蓄積されて時系列情報としての信頼度が高くなったか否かなどにより、自己位置不定状態であるか否かが判定されるようにしてもよい。
ステップＳ１６１において、自己位置不定状態が継続しており、探索モード処理を終了させない場合、処理は、ステップＳ１５２に戻る。すなわち、探索モード処理が継続される限り、ステップＳ１５２乃至Ｓ１６１の処理が繰り返されて、刻々と変化する表面センシング済み領域の情報に基づいて、探索モードにおける探索ルートが繰り返し設定されて、移動体１１が周辺の探索を継続する。そして、自己位置不定状態ではなくなった場合、処理は、ステップＳ１６２に進む。
ステップＳ１６２において、探索モード切替部２４５は、表面センシング済み領域記録部２４２に対して、表面センシング済み領域DB生成処理の終了を指示し、処理は終了する。
以上の処理により、自己位置不定状態になった場合、動作モードが探索モードに切り替えられて、ステレオカメラ２０３により撮像される表面センシング済み領域から、センシングされていない、すなわち、撮像されていない障害物の表面が、移動体１１の周囲の情報として効率よく撮像されるように、探索する探索ルートを設定することが可能となる。
すなわち、探索モードにおいては、自己位置不定状態であるため、目的地へのルートが設定できない状態であるので、目的地に向かって移動するためのルートではなく、周辺を効率よく探索することが重視されたルートが設定されることになる。
結果として、探索モードから通常モードへと迅速に切り替えることが可能となり、自己位置不定状態から迅速に自己位置が特定できる状態にすることが可能となり、通常モードに迅速に復帰することが可能となる。
尚、以上においては、現在情報自己位置推定にあたり、現在情報としてステレオカメラ２０３により撮像された視差画像に基づいて、デプス画像を生成し、生成されたデプス画像の画像特徴量を用いる例について説明してきたが、デプス画像が生成できれば、ステレオカメラ２０３を用いた視差画像以外であってもよい。
例えば、現在情報を検出する構成としては、ステレオカメラ２０３に代えて、ToF (Time of flight)センサを用いるようにしてもよい。Tofセンサを用いる場合、表面センシング可能範囲は、最低距離、最大距離、視野角で表現される。
また、以上においては、表面センシング可能範囲は、可能な範囲と不可能な範囲とからなる、例えば、０と１の２値で表現されるものである例について説明してきたが、３値以上の刻みを持った値、例えば、０乃至１の間を刻んだ浮動小数点数値で表現してもよい。このような表現とすることにより、表面センシング可能範囲は、センシング状態や障害物との距離または角度によって、センシングし易い範囲や、センシングし難い範囲を表現することが可能となる。これにより、例えば、表面センシングし難い範囲については、表面センシングし易い範囲よりも、単位時間当たりで、表面センシングする回数を増やすようにするなどして、表面センシングの精度を、表面センシングし易い範囲と同等にできるような処理をしてもよい。
さらに、以上においては、障害物認識結果を、図５で示されるような方形状の集合により表現される２次元の占有格子地図（Occupancy Grid Map）を用いて表現する例について説明してきたが、立方体の集合からなる３次元の占有格子地図を用いて表現するようにしてもよい。これにより、表面センシング可能範囲についても、水平方向および垂直方向の２次元の固定サイズの方形の集合として表現するようにしてもよいし、水平方向、垂直方向、および高さ方向の３次元の固定サイズの立方体の集合として表現するようにしてもよい。
また、表面センシング済み領域DB２４３に登録されている表面センシング済み領域の情報は、記録状態が継続している時間の長さに応じて、障害物認識結果と共に消去されるようにしてもよい。
すなわち、所定時間が経過するとき、表面センシング済み領域の情報が、対応付けて登録されている障害物認識結果の情報と共に削除されることにより、探索モード処理において、一度センシングした領域であっても、再び、表面センシングさせるようにしてもよく、これにより、再び、表面センシングするためのルートが設定されるようになる。
このように、表面センシング済みの領域の情報が、時間の経過に伴って削除されることにより、一度表面センシングした領域であっても、時間が経過したところで、再び表面センシングさせるようにすることができ、例えば、周辺の障害物の形状や位置が変化してしまうような状況であっても周辺の情報を適切に取得することができる。
さらに、表面センシング済み領域の情報は、例えば、予め定められた忘却関数に従い、時間と共に、その値が減少していくように削除するようにしてもよいし、例えば、表面センシング済み領域の有無が０または１の２値で表現される場合、０になるように削除するようにしてもよい。
また、忘却関数は、記録後の経過時間ｔ、記録時間の最大値Tに対し、定められた係数ｋを乗じた値max(0,T-kt)（max(A,B)はAまたはBのうち大きい方を選択する関数である）や、定められた係数ｋをかけた値ｋT/tとしてもよい。
さらに、忘却関数は、記録後の経過時間ｔ、記録の最大値Tに対し、定められた忘却時間Bを用いて、T(0<t<=B)、0(B<t)とするようにしてもよい。
また、以上においては、移動体１１が車輪型移動ロボットである場合に、時系列情報として、LIDAR２０１と車輪エンコーダ２０２により検出される情報を用い、現在情報としてステレオカメラ２０３により撮像される視差画像を用いる例について説明してきた。
しかしながら、時系列情報および現在情報については、これに限らず、移動体１１の形態に合わせたものとしてもよい。すなわち、移動体１１がドローンである場合には、時系列情報を、LIDARとIMU（慣性計測装置）や加速度センサにより検出される情報にして、現在情報をGNSSにより取得される情報にしてもよい。
また、移動体１１が車両である場合には、時系列情報を、LIDARにより検出される情報にして、現在情報をGNSSにより取得される情報にしてもよい。さらに、移動体１１が多足歩行ロボットである場合には、時系列情報を、LIDAR、IMU（慣性計測装置）、およびアクチュエータに付属のセンサにより検出される情報にして、現在情報をステレオカメラ、IMU（慣性計測装置）、およびアクチュエータに付属のセンサにより取得される情報にしてもよい。
さらに、ステレオカメラ２０３に代えて、LIDAR、およびToFセンサを用いるようにしてもよい。
＜＜３．変形例＞＞
以上においては、図１６の左部で示されるように、移動体１１の矢印で示される移動方向に対して、移動方向に対して前方が撮像方向とされているステレオカメラ２０３が設けられている場合、ステレオカメラ２０３の表面センシング可能範囲が、視野角θ１、および撮像可能距離ｄ１で固定されている例について説明してきた。
しかしながら、例えば、図１６の中央部で示されるように、ステレオカメラ２０３－１０１に対して撮像方向を左に向けたステレオカメラ２０３－１０２から、右に向けたステレオカメラ２０３－１０３までの範囲で、水平方向に左右に首を振るような動作（パン動作）ができるようにしてもよい。
この場合、表面センシング可能範囲Ｚは、ステレオカメラ２０３－１０１乃至２０３－１０３のそれぞれの表面センシング可能範囲Ｚ１－１乃至Ｚ１－３の全範囲を含んだ範囲となる。
尚、図１６の中央部においては、水平方向に左右にステレオカメラ２０３に首を振るように動作（パン動作）をさせる例が示されているが、さらに、垂直方向に首を振れるような動作（チルト動作）ができるようにすると、さらに、表面センシング可能範囲を広くすることができる。さらに、ステレオカメラ２０３にズーム機能を持たせると、水平方向および垂直方向に対して広範囲で、かつ、撮像可能距離を遠くまで伸ばすことができるので、表面センシング可能範囲を、さらに拡大させることが可能となる。
また、ステレオカメラ２０３の撮像可能な距離については、例えば、図１６の右部で示されるように、ステレオカメラ２０３の表面センシング範囲Ｚ１の視野角θ１で、かつ、撮像距離ｄ１からなる範囲Ｚ１よりも、十分に広い、視野角θ３で、かつ、距離ｄ２となる範囲Ｚ３を照明できる照明３０１を設けるようにしてもよい。このような構成とすることで、例えば、ステレオカメラ２０３の撮像距離が照度による限界であった場合には、照度が高くなることにより、例えば、撮像可能距離を距離ｄ１よりも遠い距離ｄ３とすることが可能となる。
さらに、図１６の中央部と右部との構成を組み合わせるようにして、ステレオカメラ２０３の撮像方向のパンチルトに合わせて、照明３０１についてもパンチルトするようにしてもよい。このようにすることでも、表面センシング可能範囲を広くすることが可能となる。
このように表面センシング可能範囲を広くすることで、単位時間当たりの周囲より検出できる情報量が増えることになるので、表面センシング速度を実質的に高速にすることが可能となり、ルートコストを計算する上でも、よりコストを低減させることができる。結果として、探索モードから通常モードへの復帰をより高速にすることが可能となる。
＜＜４．ソフトウェアにより実行させる例＞＞
ところで、上述した一連の処理は、ハードウェアにより実行させることもできるが、ソフトウェアにより実行させることもできる。一連の処理をソフトウェアにより実行させる場合には、そのソフトウェアを構成するプログラムが、専用のハードウェアに組み込まれているコンピュータ、または、各種のプログラムをインストールすることで、各種の機能を実行することが可能な、例えば汎用のコンピュータなどに、記録媒体からインストールされる。
図１７は、汎用のコンピュータの構成例を示している。このパーソナルコンピュータは、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（Light Detection and Ranging、Laser Imaging Detection and Ranging）により検出される３次元点群情報、および車輪エンコーダにより検出される位置情報、姿勢、速度、加速度、および角速度であり、
前記障害物位置情報生成部は、前記３次元点群情報に基づいて、前記周辺の障害物の相対位置および方向からなる障害物位置情報を生成する
＜１２＞に記載の制御装置。
＜１４＞ 前記自己位置推定部は、
現在のセンシング結果を出力するセンサ情報からなる現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部をさらに含む
＜１２＞に記載の制御装置。
＜１５＞ 前記現在情報は、ステレオカメラからなる前記表面センシング部により撮像される視差画像であり、
前記現在情報自己位置推定部は、前記現在情報である視差画像に基づいて、デプス画像を生成し、前記デプス画像より画像特徴量を抽出し、前記画像特徴量と対応付けられている位置に基づいて、前記自己位置を推定し、現在情報自己位置として出力し、
前記自己位置推定部は、前記時系列情報自己位置および前記現在情報自己位置を統合して、前記自己位置として出力する
＜１４＞に記載の制御装置。
＜１６＞ 前記位置と前記画像特徴量とが対応付けて記憶される位置画像特徴量記憶部をさらに含み、
前記自己位置推定部は、前記時系列情報自己位置および前記現在情報自己位置を統合して、自己位置として出力するとき、前記現在情報自己位置に対応する画像特徴量と、前記自己位置とを対応付けて、前記位置画像特徴量記憶部に記憶させる
＜１５＞に記載の制御装置。
＜１７＞ 前記自己位置が不定状態の場合、前記表面センシング済み領域記録部は、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する
＜１＞乃至＜１６＞のいずれかに記載の制御装置。
＜１８＞ 自己位置を推定する自己位置推定処理と、
周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成処理と、
前記障害物の表面をセンシングする表面センシング処理と、
前記自己位置、前記障害物位置情報、および前記表面センシング処理の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する表面センシング済み領域記録処理と、
前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画処理と
を含む制御方法。
＜１９＞ 自己位置を推定する自己位置推定部と、
周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、
前記障害物の表面をセンシングする表面センシング部と、
前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する表面センシング済み領域記録部と、
前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部と
してコンピュータを機能させるプログラム。
＜２０＞ 自己位置を推定する自己位置推定部と、
周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、
前記障害物の表面をセンシングする表面センシング部と、
前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する表面センシング済み領域記録部と、
前記障害物の表面センシング済み領域に基づいて、ルートを計画する計画部と、
前記計画部により計画された前記ルートに基づいて、行動計画を生成する行動計画生成部と、
前記行動計画生成部により決定された行動計画に基づいて移動体の動作を制御する制御部と
を含む移動体。
１１ 移動体， ２１ センサ群， ２１ａ，２１ａ－１乃至２１ａ－ｎ センサ， ２２ 自律移動制御部， ２３ アクチュエータ群， ２３ａ，２３ａ－１乃至２３ａ－ｎ アクチュエータ， ３１ 認識処理部， ３２ 行動計画処理部， ３３ 行動制御処理部， ４１ ルート計画部， ４２ 行動計画部， ４３ 動作計画部， １０２ データ取得部， １０５ 出力制御部， １０６ 出力部， １０７ 駆動系制御部， １０８ 駆動系システム， １１０ 自律移動制御部， １１２ センサ群， １１３ アクチュエータ群， １２１ 認識処理部， １２２ 行動計画処理部， １２３ 行動制御処理部， １３４ 計画部， １６１ ルート計画部， １６２ 行動計画部， １６３ 動作計画部， ２０１ LIDAR， ２０２ 車輪エンコーダ， ２０３ ステレオカメラ， ２２１ 時系列情報自己位置推定部， ２２２ 時系列情報DB， ２２３ 現在情報自己位置推定部， ２２４ 位置画像特徴量DB， ２２５ 自己位置推定結果統合部， ２４１ 障害物認識部， ２４２ 表面センシング済み領域記録部， ２４３ 表面センシング済み可能範囲情報記憶部， ２４４ 表面センシング済み領域DB， ２４５ 探索モード判定部

Claims (18)

  1. 自己位置を推定する自己位置推定部と、
    周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、
    前記障害物の表面をセンシングする表面センシング部と、
    前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、
    前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部とを含み、
    前記ルート計画部は、
    前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、
    前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する
    制御装置。
  2. 前記優先度設定部は、前記経由地を全て通過するルートのうち、優先条件に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定する
    請求項１に記載の制御装置。
  3. 前記優先条件は、総移動距離、または総消費電力であり、
    前記優先度コストは、移動距離に応じて設定されるコスト、または、消費電力に応じて設定されるコストである
    請求項２に記載の制御装置。
  4. 前記優先度設定部は、前記経由地を全て通過するルートのうち、前記優先条件、および機体運動情報に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定する
    請求項２に記載の制御装置。
  5. 前記機体運動情報は、
    前記優先条件が総移動距離である場合、最高速度、最小回転半径、および経路上の超えられる段差の高さの情報であり、
    前記優先条件が総消費電力である場合、単位距離当たりの電力消費量、およびバッテリの充電容量の情報である
    請求項４に記載の制御装置。
  6. 前記ルート計画部は、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて、前記経由地を全て経由する前記ルートを計画する
    請求項１に記載の制御装置。
  7. 前記ルート計画部は、前記経由地を全て経由する全てのルートについて、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて設定されるルートコストを算出し、前記ルートコストが最小となるルートを選択する
    請求項１に記載の制御装置。
  8. 前記移動条件は、優先度の高い順に前記経由地を通過するという条件、経由地の優先度を無視しても最短時間で移動するという条件、または、優先度の高い経由地ほど、早く到着するという条件を含む
    請求項７に記載の制御装置。
  9. 前記移動条件が、優先度の高い経由地ほど、早く到着するという条件である場合、前記ルート計画部は、前記優先度と、前記経由地までの到着時間との積を前記ルートコストとして設定し、前記全てのルートのうち、前記ルートコストが最小となるルートを選択する
    請求項８に記載の制御装置。
  10. 前記自己位置推定部は、
    時系列に供給されるセンサ情報からなる時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部を含み、
    前記表面センシング済み領域記録部は、前記時系列情報自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する
    請求項１に記載の制御装置。
  11. 前記時系列情報は、LIDAR（Light Detection and Ranging、Laser Imaging Detection and Ranging）により検出される３次元点群情報、および車輪エンコーダにより検出される位置情報、姿勢、速度、加速度、および角速度であり、
    前記障害物位置情報生成部は、前記３次元点群情報に基づいて、前記周辺の障害物の相対位置および方向からなる障害物位置情報を生成する
    請求項１０に記載の制御装置。
  12. 前記自己位置推定部は、
    現在のセンシング結果を出力するセンサ情報からなる現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部をさらに含む
    請求項１０に記載の制御装置。
  13. 前記現在情報は、ステレオカメラからなる前記表面センシング部により撮像される視差画像であり、
    前記現在情報自己位置推定部は、前記現在情報である視差画像に基づいて、デプス画像を生成し、前記デプス画像より画像特徴量を抽出し、前記画像特徴量と対応付けられている位置に基づいて、前記自己位置を推定し、現在情報自己位置として出力し、
    前記自己位置推定部は、前記時系列情報自己位置および前記現在情報自己位置を統合して、前記自己位置として出力する
    請求項１２に記載の制御装置。
  14. 前記位置と前記画像特徴量とが対応付けて記憶される位置画像特徴量記憶部をさらに含み、
    前記自己位置推定部は、前記時系列情報自己位置および前記現在情報自己位置を統合して、自己位置として出力するとき、前記現在情報自己位置に対応する画像特徴量と、前記自己位置とを対応付けて、前記位置画像特徴量記憶部に記憶させる
    請求項１３に記載の制御装置。
  15. 前記自己位置が不定状態の場合、前記表面センシング済み領域記録部は、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する
    請求項１に記載の制御装置。
  16. 自己位置を推定する自己位置推定処理と、
    周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成処理と、
    前記障害物の表面をセンシングする表面センシング処理と、
    前記自己位置、前記障害物位置情報、および前記表面センシング処理の表面センシング可能範囲に基づいて、前記表面センシング処理の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録処理と、
    前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画処理とを含み、
    前記ルート計画処理は、
    前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定処理を含み、
    前記優先度設定処理により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する
    制御方法。
  17. 自己位置を推定する自己位置推定部と、
    周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、
    前記障害物の表面をセンシングする表面センシング部と、
    前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、
    前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部としてコンピュータを機能させ、
    前記ルート計画部は、
    前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、
    前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する
    プログラム。
  18. 自己位置を推定する自己位置推定部と、
    周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、
    前記障害物の表面をセンシングする表面センシング部と、
    前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、
    前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部と、
    前記ルート計画部により計画された前記ルートに基づいて、行動計画を生成する行動計画生成部と、
    前記行動計画生成部により決定された行動計画に基づいて移動体の動作を制御する制御部とを含み、
    前記ルート計画部は、
    前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、
    前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する
    移動体。
JP2019554175A 2017-11-20 2018-11-06 制御装置、および制御方法、プログラム、並びに移動体 Active JP7156305B2 (ja)

Applications Claiming Priority (3)

Application Number Priority Date Filing Date Title
JP2017222457 2017-11-20
JP2017222457 2017-11-20
PCT/JP2018/041095 WO2019098082A1 (ja) 2017-11-20 2018-11-06 制御装置、および制御方法、プログラム、並びに移動体

Publications (2)

Publication Number Publication Date
JPWO2019098082A1 JPWO2019098082A1 (ja) 2020-11-19
JP7156305B2 true JP7156305B2 (ja) 2022-10-19

Family

ID=66538592

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2019554175A Active JP7156305B2 (ja) 2017-11-20 2018-11-06 制御装置、および制御方法、プログラム、並びに移動体

Country Status (4)

Country Link
US (1) US11537131B2 (ja)
JP (1) JP7156305B2 (ja)
DE (1) DE112018005910T5 (ja)
WO (1) WO2019098082A1 (ja)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11348269B1 (en) * 2017-07-27 2022-05-31 AI Incorporated Method and apparatus for combining data to construct a floor plan
JP7353747B2 (ja) * 2018-01-12 2023-10-02 キヤノン株式会社 情報処理装置、システム、方法、およびプログラム
KR102106100B1 (ko) * 2018-02-02 2020-05-06 엘지전자 주식회사 이동 로봇
US11287826B2 (en) * 2018-10-12 2022-03-29 Boston Dynamics, Inc. Terrain aware step planning system
US11835960B2 (en) * 2019-01-28 2023-12-05 Zebra Technologies Corporation System and method for semantically identifying one or more of an object and a location in a robotic environment
CN112013844B (zh) * 2019-05-31 2022-02-11 北京小米智能科技有限公司 建立室内环境地图的方法及装置
CN112462805B (zh) * 2020-11-19 2022-11-29 西安理工大学 基于改进蚁群算法的5g网联无人机航迹规划方法
WO2024015030A1 (en) * 2022-07-11 2024-01-18 Delivers Ai Robotik Otonom Surus Bilgi Teknolojileri A.S. A delivery system and method for a delivery robot

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004133567A (ja) 2002-10-09 2004-04-30 Hitachi Ltd 移動体およびその位置検出装置
JP2011170843A (ja) 2010-01-20 2011-09-01 Ihi Aerospace Co Ltd 経路生成装置と方法および経路生成装置を備える移動装置
JP2014194729A (ja) 2013-02-27 2014-10-09 Sharp Corp 周囲環境認識装置、それを用いた自律移動システムおよび周囲環境認識方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2002073770A (ja) 2000-09-05 2002-03-12 Ishida Co Ltd 商品情報印字システム
JP2003092347A (ja) 2002-07-10 2003-03-28 Mitsubishi Electric Corp 半導体デバイスのコンタクトホール形成方法
JP2010217995A (ja) 2009-03-13 2010-09-30 Mitsubishi Electric Corp 探索装置及び探索方法及び探索プログラム
BR112012010612A2 (pt) 2009-11-06 2017-08-15 Evolution Robotics Inc Dispositivo móvel configurado para navegar em superfície e método para navegar em superfície com dispositivo móvel
US9221396B1 (en) * 2012-09-27 2015-12-29 Google Inc. Cross-validating sensors of an autonomous vehicle

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2004133567A (ja) 2002-10-09 2004-04-30 Hitachi Ltd 移動体およびその位置検出装置
JP2011170843A (ja) 2010-01-20 2011-09-01 Ihi Aerospace Co Ltd 経路生成装置と方法および経路生成装置を備える移動装置
JP2014194729A (ja) 2013-02-27 2014-10-09 Sharp Corp 周囲環境認識装置、それを用いた自律移動システムおよび周囲環境認識方法

Also Published As

Publication number Publication date
WO2019098082A1 (ja) 2019-05-23
JPWO2019098082A1 (ja) 2020-11-19
US20200333790A1 (en) 2020-10-22
US11537131B2 (en) 2022-12-27
DE112018005910T5 (de) 2020-07-30

Similar Documents

Publication Publication Date Title
JP7156305B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
JP7180612B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
JP7151725B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
JP7180670B2 (ja) 制御装置、制御方法、並びにプログラム
WO2019098002A1 (ja) 情報処理装置、情報処理方法、プログラム、及び移動体
WO2020226085A1 (ja) 情報処理装置、情報処理方法、及びプログラム
JP6891753B2 (ja) 情報処理装置、移動装置、および方法、並びにプログラム
CN114026463B (zh) 基于部分点云的行人速度估计方法
WO2019082669A1 (ja) 情報処理装置、情報処理方法、プログラム、及び、移動体
WO2020031812A1 (ja) 情報処理装置、情報処理方法、情報処理プログラム、及び移動体
JP2020079997A (ja) 情報処理装置、情報処理方法、及びプログラム
WO2021033591A1 (ja) 情報処理装置、および情報処理方法、並びにプログラム
JPWO2019073795A1 (ja) 情報処理装置、自己位置推定方法、プログラム、及び、移動体
CN112810603B (zh) 定位方法和相关产品
JP7147142B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
WO2020129810A1 (en) Information processing apparatus, information processing method, and program
WO2019176278A1 (ja) 情報処理装置、情報処理方法、プログラム、及び移動体
WO2021140916A1 (ja) 移動体、情報処理装置、情報処理方法、及びプログラム
JP2019049475A (ja) 信号処理装置、および信号処理方法、プログラム、並びに移動体
JPWO2019049710A1 (ja) 信号処理装置、および信号処理方法、プログラム、並びに移動体
US20230150543A1 (en) Systems and methods for estimating cuboid headings based on heading estimations generated using different cuboid defining techniques
US20230237793A1 (en) False track mitigation in object detection systems
KR20230149709A (ko) 그리드에서의 양방향 경로 최적화

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20210917

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20220607

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220801

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

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20220919

R151 Written notification of patent or utility model registration

Ref document number: 7156305

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151