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

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214Control 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
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246Control 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/0251Control 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
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C3/00Measuring distances in line of sight; Optical rangefinders
    • G01C3/02Details
    • G01C3/06Use of electric means to obtain final indication
    • G01C3/08Use of electric radiation detectors
    • G01C3/085Use of electric radiation detectors with electronic parallax measurement
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88Lidar systems specially adapted for specific applications
    • G01S17/89Lidar systems specially adapted for specific applications for mapping or imaging
    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO 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/00Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88Lidar systems specially adapted for specific applications
    • G01S17/93Lidar systems specially adapted for specific applications for anti-collision purposes
    • G01S17/931Lidar systems specially adapted for specific applications for anti-collision purposes of land vehicles
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0217Control 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
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00Image analysis
    • G06T7/20Analysis of motion
    • G06T7/246Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00Image analysis
    • G06T7/20Analysis of motion
    • G06T7/277Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00Image analysis
    • G06T7/50Depth or shape recovery
    • G06T7/55Depth or shape recovery from multiple images
    • G06T7/593Depth or shape recovery from multiple images from stereo images
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00Indexing scheme for image analysis or image enhancement
    • G06T2207/10Image acquisition modality
    • G06T2207/10016Video; Image sequence
    • G06T2207/10021Stereoscopic video; Stereoscopic image sequence
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00Indexing scheme for image analysis or image enhancement
    • G06T2207/10Image acquisition modality
    • G06T2207/10028Range image; Depth image; 3D point clouds
    • GPHYSICS
    • G06COMPUTING; CALCULATING OR COUNTING
    • G06TIMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00Indexing scheme for image analysis or image enhancement
    • G06T2207/30Subject of image; Context of image processing
    • G06T2207/30248Vehicle exterior or interior
    • G06T2207/30252Vehicle exterior; Vicinity of vehicle
    • G06T2207/30261Obstacle

Landscapes

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

Description

本開示は、制御装置、および制御方法、プログラム、並びに移動体に関し、特に、周囲を効率よく探索できるようにした制御装置、および制御方法、プログラム、並びに移動体に関する。
ロボット等の移動体の自律的な移動を実現するためには、行動を計画する起点となる自己の位置を認識・推定する必要がある。そこで、自己の周囲の状況をセンサ等の手段で認識して、自己の位置を推定し、自律的な移動を計画する技術が提案されている。
例えば、連続的に過去に検出してきた自己位置の情報を利用して自己位置を順次推定する場合、何らかの外力により、それまでの自己位置とは全く異なる自己位置に移動してしまった場合、過去の自己位置の情報との連続性を失うことにより、自己位置を認識することができない状態となる。
このような場合、移動体は、自己位置を改めて推定するため、自らの周囲を移動しながら探索することで、周囲をセンシングして情報を取得し、取得した周囲の情報から自己位置を推定する必要がある。
この際、移動体は、ランダムに自らの周囲を動き回ると、既にセンシングが済んだ領域を繰り返しセンシングしてしまうことがあり、効率良く周囲を探索できない恐れがある。
そこで、「未探索」、「横断済み」、「縁部」、「占有済み」を示すマッピングモジュールにより、全領域カバーしながら移動させるようにする技術(特許文献1参照)を応用することが考えられる。
また、既に探索したところでセンシングした移動可能領域を示す屋内構造データを記憶し、移動できる方向に順次移動する技術(特許文献2参照)を応用することが考えられる。
さらに、障害物を探す操作、または、自機が移動可能な領域を取得するためにセンシング済み領域を記憶する技術(特許文献3,4参照)を応用することが考えられる。
特開2016-095858号公報 特開2010-217995号公報 特願2003-092347号公報 特願2002-073770号公報
しかしながら、特許文献1の技術においては、全領域をカバーして移動する技術であるため、効率よく周囲を探索することはできない。
また、特許文献2の技術においては、探索されたときの情報から得られる移動可能領域が記憶されるものであり、効率よく周囲を探索するための情報としては利用することができない。
さらに、特許文献3,4の技術においては、障害物を探す操作、または、自機が移動可能な領域を取得するためにセンシング済み領域を記憶しているが、障害物の表面はセンシングされておらず、また、記憶もされていないので、障害物の表面状態に応じて効率よく周囲を探索することができない。
本開示は、このような状況に鑑みてなされたものであり、特に、周囲を効率よく探索できるようにするものである。
本開示の一側面の制御装置は、自己位置を推定する自己位置推定部と、周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、前記障害物の表面をセンシングする表面センシング部と、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部とを含み、前記ルート計画部は、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する制御装置である。
前記優先度設定部には、前記経由地を全て通過するルートのうち、優先条件に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定させるようにすることができる。
前記優先条件は、総移動距離、または総消費電力とすることができ、前記優先度コストは、移動距離に応じて設定されるコスト、または、消費電力に応じて設定されるコストとすることができる。
前記優先度設定部は、前記経由地を全て通過するルートのうち、前記優先条件、および機体運動情報に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定させるようにすることができる。
前記機体運動情報は、前記優先条件が総移動距離である場合、最高速度、最小回転半径、および経路上の超えられる段差の高さの情報とすることができ、前記優先条件が総消費電力である場合、単位距離当たりの電力消費量、およびバッテリの充電容量の情報とすることができる。
前記ルート計画部には、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて、前記経由地を全て経由する前記ルートを計画させるようにすることができる。
前記ルート計画部には、前記経由地を全て経由する全てのルートについて、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて設定されるルートコストを算出させ、前記ルートコストが最小となるルートを選択させるようにすることができる。
前記移動条件には、優先度の高い順に前記経由地を通過するという条件、経由地の優先度を無視しても最短時間で移動するという条件、または、優先度の高い経由地ほど、早く到着するという条件を含ませるようにすることができる。
前記移動条件が、優先度の高い経由地ほど、早く到着するという条件である場合、前記ルート計画部には、前記優先度と、前記経由地までの到着時間との積を前記ルートコストとして設定し、前記全てのルートのうち、前記ルートコストが最小となるルートを選択させるようにすることができる。
前記自己位置推定部には、時系列に供給されるセンサ情報からなる時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部を含ませるようにすることができ、前記表面センシング済み領域記録部には、前記時系列情報自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録させるようにすることができる。
前記時系列情報は、LIDAR(Light Detection and Ranging、Laser Imaging Detection and Ranging)により検出される3次元点群情報、および車輪エンコーダにより検出される位置情報、姿勢、速度、加速度、および角速度とすることができ、前記障害物位置情報生成部には、前記3次元点群情報に基づいて、前記周辺の障害物の相対位置および方向からなる障害物位置情報を生成させるようにすることができる。
前記自己位置推定部には、現在のセンシング結果を出力するセンサ情報からなる現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部をさらに含ませるようにすることができる。
前記現在情報は、ステレオカメラからなる前記表面センシング部により撮像される視差画像とすることができ、前記現在情報自己位置推定部には、前記現在情報である視差画像に基づいて、デプス画像を生成させ、前記デプス画像より画像特徴量を抽出させ、前記画像特徴量と対応付けられている位置に基づいて、前記自己位置を推定させ、現在情報自己位置として出力させ、前記自己位置推定部には、前記時系列情報自己位置および前記現在情報自己位置を統合して、前記自己位置として出力させるようにすることができる。
前記位置と前記画像特徴量とが対応付けて記憶される位置画像特徴量記憶部をさらに含ませるようにすることができ、前記自己位置推定部には、前記時系列情報自己位置および前記現在情報自己位置を統合して、自己位置として出力するとき、前記現在情報自己位置に対応する画像特徴量と、前記自己位置とを対応付けて、前記位置画像特徴量記憶部に記憶させるようにすることができる。
前記自己位置が不定状態の場合、前記表面センシング済み領域記録部には、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録させるようにすることができる。
本開示の一側面の制御方法は、自己位置を推定する自己位置推定処理と、周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成処理と、前記障害物の表面をセンシングする表面センシング処理と、前記自己位置、前記障害物位置情報、および前記表面センシング処理の表面センシング可能範囲に基づいて、前記表面センシング処理の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録処理と、前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画処理とを含み、前記ルート計画処理は、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定処理を含み、前記優先度設定処理により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する制御方法である。
本開示の一側面のプログラムは、自己位置を推定する自己位置推定部と、周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、前記障害物の表面をセンシングする表面センシング部と、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部としてコンピュータを機能させ、前記ルート計画部は、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画するプログラムである。
本開示の一側面の移動体は、自己位置を推定する自己位置推定部と、周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、前記障害物の表面をセンシングする表面センシング部と、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部と、前記ルート計画部により計画された前記ルートに基づいて、行動計画を生成する行動計画生成部と、前記行動計画生成部により決定された行動計画に基づいて移動体の動作を制御する制御部とを含み、前記ルート計画部は、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する移動体である。
本開示の一側面においては、自己位置が推定され、周辺の障害物の位置からなる障害物位置情報が生成され、前記障害物の表面がセンシングされ、前記自己位置、前記障害物位置情報、および表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面が、前記障害物の表面センシング済み領域として記録され、前記障害物の表面センシング済み領域に基づいて、ルートが計画され、前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域が設定され、前記未済領域を分割した分割領域が経由地として設定され、前記経由地に優先度が設定され、設定された前記優先度に基づいて、前記経由地を経由する前記ルートが計画される。
本開示の一側面によれば、特に、効率よく周囲を探索することが可能となる。
本開示の概要を説明する移動体の構成例を示す図である。 本開示の概要を説明する図である。 本開示の移動体を制御する移動体制御システムの構成例を説明するブロック図である。 本開示の認識処理部および行動計画処理部の周辺の構成例の詳細なブロック図である。 障害物認識結果を説明する図である。 表面センシング可能範囲を説明する図である。 表面センシング済み領域を説明する図である。 優先度の設定方法を説明する図である。 自己位置推定処理を説明するフローチャートである。 図9の時系列情報自己位置推定処理を説明するフローチャートである。 図9の現在情報自己位置推定処理を説明するフローチャートである。 障害物認識処理を説明するフローチャートである。 表面センシング済み領域DB記録処理を説明するフローチャートである。 自律移動制御処理を説明するフローチャートである。 探索モード処理を説明するフローチャートである。 ステレオカメラによる変形例による表面センシング済み領域を説明する図である。 汎用のコンピュータの構成例を説明する図である。
以下に添付図面を参照しながら、本開示の好適な実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。
以下、本技術を実施するための形態について説明する。説明は以下の順序で行う。
1.本開示の概要
2.好適な実施の形態
3.変形例
4.ソフトウェアにより実行させる例
<<1.本開示の概要>>
本開示の移動体は、自己位置が不定の状態となった場合、周囲を探索しながらセンシングして、周囲の情報を取得して、新たな自己位置を推定するにあたって、効率よく周囲を探索できるようにした移動体である。
図1は、本開示の移動体11の概要となる構成例を示している。
移動体11は、例えば、ロボットなどであり、センサ群21、自律移動制御部22、およびアクチュエータ群23を備えている。
センサ群21は、移動体11の内部、および移動体11の周囲の状況の認識に必要な各種の情報を検出するセンサ21a-1乃至21a-nを備えており、検出結果を自律移動制御部22に出力する。また、センサ21a-1乃至21a-nについて特に区別する必要がない場合、単に、センサ21aと称するものとし、その他の構成についても同様に称する。
より具体的には、センサ21a-1乃至21a-nは、例えば、移動体11の周囲を撮像するカメラ、移動体11の動きを検出する加速度センサ、移動体11の周囲に存在する物体までの距離を測定するLIDAR、ToF(Time of Flight)センサ、方向を検出する地磁気センサ、ジャイロセンサ、加速度センサ、周囲の気圧の変化を検出する気圧センサ、接触の有無等を検出する接触センサ、温度を検出する温度センサ、湿度を検出する湿度センサ、PSD(Position Sensitive Detector)測距センサおよび、地球上の位置を検出するGNSS(Global Navigation Satellite System)などを含む。
自律移動制御部22は、センサ群21の各種の検出結果より、周囲の状況を認識し、認識結果に基づいて行動計画を生成し、行動計画に応じて、ロボットを駆動させるアクチュエータ群23の各種アクチュエータ23a-1乃至23a-nを動作させる。また、アクチュエータ23a-1乃至23a-nについて特に区別する必要がない場合、単に、アクチュエータ23aと称するものとし、その他の構成についても同様に称する。
より詳細には、自律移動制御部22は、認識処理部31、行動計画処理部32、および行動制御処理部33を備えている。
認識処理部31は、センサ群21より供給される検出結果に基づいて、認識処理を実行し、例えば、画像、人、物体、表情の種類、位置、属性、および自らや障害物の位置等を認識し、認識結果として行動計画処理部32に出力する。また、認識処理部31は、センサ群21より供給される時系列の情報からなる検出結果に基づいて、自己位置を推定する。また、認識処理部31は、例えば、移動体11が移動する玩具などの場合、ユーザに抱き上げられるなどして、センサ群21により供給される検出結果に基づいて、自己位置の推定ができない状態になると、動作モードを探索モードに切り替える。探索モードにおいて、認識処理部31は、周囲を探索することにより周囲を効率よくセンシングさせ、周囲のセンシング結果から改めて自己位置を推定する。この際、認識処理部31は、探索により検出された情報に基づいて、周囲のセンシング済み領域を記憶し、センシングされていない領域を効率よくセンシングできるように、センシング済み領域の情報を行動計画処理部32に出力する。
行動計画処理部32は、認識結果に基づいて、移動体11の全体の行動である、移動体11の移動に係る機器の移動の軌跡、状態変化、および速度または加速度などの行動計画を生成し、行動制御処理部33に供給する。また、探索モードにおいて、行動計画処理部32は、センシング済み領域の情報に基づいて、効率よく周囲をセンシングできるルートを計画し、計画したルートに対応する行動計画を生成する。
行動制御処理部33は、行動計画処理部32より供給される行動計画に基づいて、アクチュエータ群23のアクチュエータ23a-1乃至23a-nのそれぞれの具体的な動きを制御するための制御信号を生成し、アクチュエータ群23を動作させる。
アクチュエータ群23は、行動制御処理部33より供給される制御信号に基づいて、移動体11を具体的に動作させるアクチュエータ23a-1乃至23a-nを動作させる。より詳細には、アクチュエータ23a-1乃至23a-nは、移動体11の具体的な動き実現するモータ、サーボモータ、ブレーキ等の動作を制御信号に基づいて動作させる。
また、アクチュエータ23a-1乃至23a-nは、伸縮運動、屈伸運動、または旋回運動などを実現させる構成を含むと共に、情報を表示するLED(Light Emission Diode)やLCD(Liquid Crystal Display)などからなる表示部、および、音声を出力するスピーカなどの構成を含む。したがって、アクチュエータ群23が制御信号に基づいて、制御されることにより、移動体11を駆動させる各種の装置の動作が実現されると共に、情報が表示される、および音声が出力される。
すなわち、アクチュエータ群23のアクチュエータ23a-1乃至23a-nが制御されることにより、移動体11の移動に係る動作が制御されると共に、情報の表示や音声の出力などの各種の情報の提示も制御される。
<本開示の探索モードの概要>
移動体11は、センサ群21のセンシング結果から、何らかの原因で自己位置が不定の状態となった場合、動作モードを通常モードから探索モードに切り替える。
探索モードにおいては、移動体11は、周囲を探索しながら移動し、センサ群21が、周囲の障害物表面をセンシングする。
ここで、認識処理部31は、センサ群21により表面センシング済みの領域をデータベースとして記録する。
すなわち、例えば、図2の左部で示されるように、障害物B1乃至B3の間を移動体11-1から移動体11-2となるように移動する場合、センサ群21は、障害物B1乃至B3の表面をセンシングし、認識処理部31が、センシング済みの領域を、例えば、センシング済み表面領域R1乃至R3で示されるように記録する。
行動計画処理部32は、図2の左部で示されるように記録されているセンシング済み表面領域R1乃至R3の情報に基づいて、センシングが済んでいない領域を効率よくセンシングできるようにルートを計画し、行動計画を生成する。
より具体的には、図2の右部で示されるように、行動計画処理部32は、センシングが済んでいない領域を複数の領域に分割し、例えば、経由地P1乃至P4として設定し、経由地P1乃至P4の優先度を設定する。
そして、行動計画処理部32は、各経由地P1乃至P4を通る複数のルートを計画し、優先度と、移動体11のセンサ群21によりセンシング可能な領域や、移動に係る条件とに基づいて、ルートを決定し、決定したルートに基づいた行動計画を生成する。
結果として、自己位置が不定の状態となった場合でも、効率よく周囲を探索しながらセンシングし、周囲の情報を効率よく取得することで、迅速に自己位置を求めることが可能となり、通常モードの動作の早期復帰を実現することが可能となる。
<<2.好適な実施の形態>>
<本開示の移動体を制御する移動体制御システムの構成例>
上述した機能を実現させるための移動体11を制御する移動体制御システムについて説明する。
図3は、本開示の移動体11を制御する移動体制御システム100の概略的な機能の構成例を示すブロック図である。尚、図3の移動体制御システム100は、本技術が適用され得るロボットからなる移動体11を制御する移動体制御システムの一例であるが、他の移動体、例えば、航空機、船舶、およびマルチローターコプター(ドローン)などを制御するシステムとして適用することもできる。また、ロボットについても、車輪型のロボットや搭乗可能な自動運転車でもよいし、多足歩行型のロボットでもよい。
移動体制御システム100は、入力部101、データ取得部102、通信部103、移動体内部機器104、出力制御部105、出力部106、駆動系制御部107、駆動系システム108、記憶部109、及び、自律移動制御部110を備える。入力部101、データ取得部102、通信部103、出力制御部105、駆動系制御部107、記憶部109、及び、自律移動制御部110は、通信ネットワーク111を介して、相互に接続されている。通信ネットワーク111は、例えば、CAN(Controller Area Network)、LIN(Local Interconnect Network)、IEEE802.3 等のLAN(Local Area Network)、又は、FlexRay(登録商標)等の任意の規格に準拠した通信ネットワークやバス、あるいは規格化されていない独自の通信方式等からなる。なお、移動体制御システム100の各部は、通信ネットワーク111を介さずに、直接接続される場合もある。
なお、以下、移動体制御システム100の各部が、通信ネットワーク111を介して通信を行う場合、通信ネットワーク111の記載を省略するものとする。例えば、入力部101と自律移動制御部110が、通信ネットワーク111を介して通信を行う場合、単に入力部101と自律移動制御部110が通信を行うと記載する。
入力部101は、搭乗者が各種のデータや指示等の入力に用いる装置を備える。例えば、入力部101は、タッチパネル、ボタン、マイクロフォン、スイッチ、及び、レバー等の操作デバイス、並びに、音声やジェスチャ等により手動操作以外の方法で入力可能な操作デバイス等を備える。また、例えば、入力部101は、赤外線若しくはその他の電波を利用したリモートコントロール装置、又は、移動体制御システム100の操作に対応したモバイル機器若しくはウェアラブル機器等の外部接続機器であってもよい。入力部101は、搭乗者により入力されたデータや指示等に基づいて入力信号を生成し、移動体制御システム100の各部に供給する。
データ取得部102は、移動体制御システム100の処理に用いるデータを取得する各種のセンサ等を備え、取得したデータを、移動体制御システム100の各部に供給する。
例えば、データ取得部102は、移動体の状態等を検出するための各種のセンサを備えることでセンサ群112を構成し、図1のセンサ21a-1乃至21a-nより構成されるセンサ群21に対応する。具体的には、例えば、データ取得部102は、ジャイロセンサ、加速度センサ、慣性計測装置(IMU)、及び、アクセル等の加速入力の操作量、減速入力の操作量、方向指示入力の操作量、エンジンやモータ等の駆動装置の回転数や入出力エネルギー・燃料量、エンジンやモータ等のトルク量、若しくは、車輪や関節の回転速度やトルク等を検出するためのセンサ等を備える。
また、例えば、データ取得部102は、移動体の外部の情報を検出するための各種のセンサを備える。具体的には、例えば、データ取得部102は、ToF(Time Of Flight)カメラ、ステレオカメラ、単眼カメラ、赤外線カメラ、偏光カメラ、及び、その他のカメラ等の撮像装置を備える。また、例えば、データ取得部102は、天候又は気象等を検出するための環境センサ、及び、移動体の周囲の物体を検出するための周囲情報検出センサを備える。環境センサは、例えば、雨滴センサ、霧センサ、日照センサ、雪センサ等からなる。周囲情報検出センサは、例えば、レーザ測距センサ、超音波センサ、レーダ、LiDAR(Light Detection and Ranging、Laser Imaging Detection and Ranging)、ソナー等からなる。
さらに、例えば、データ取得部102は、移動体の現在位置を検出するための各種のセンサを備える。具体的には、例えば、データ取得部102は、GNSS(Global Navigation Satellite System)衛星からのGNSS信号を受信するGNSS受信機等を備える。
通信部103は、移動体内部機器104、並びに、移動体外部の様々な機器、サーバ、基地局等と通信を行い、移動体制御システム100の各部から供給されるデータを送信したり、受信したデータを移動体制御システム100の各部に供給したりする。なお、通信部103がサポートする通信プロトコルは、特に限定されるものではなく、また、通信部103が、複数の種類の通信プロトコルをサポートすることも可能である。
例えば、通信部103は、無線LAN、Bluetooth(登録商標)、NFC(Near Field Communication)、又は、WUSB(Wireless USB)等により、移動体内部機器104と無線通信を行う。また、例えば、通信部103は、図示しない接続端子(及び、必要であればケーブル)を介して、USB(Universal Serial Bus)、HDMI(登録商標)(High-Definition Multimedia Interface)、又は、MHL(Mobile High-definition Link)等により、移動体内部機器104と有線通信を行う。
さらに、例えば、通信部103は、基地局又はアクセスポイントを介して、外部ネットワーク(例えば、インターネット、クラウドネットワーク又は事業者固有のネットワーク)上に存在する機器(例えば、アプリケーションサーバ又は制御サーバ)との通信を行う。また、例えば、通信部103は、P2P(Peer To Peer)技術を用いて、移動体の近傍に存在する端末(例えば、歩行者若しくは店舗の端末、又は、MTC(Machine Type Communication)端末)との通信を行う。さらに、例えば、移動体11が車両の場合、通信部103は、車車間(Vehicle to Vehicle)通信、路車間(Vehicle to Infrastructure)通信、移動体と家との間(Vehicle to Home)の通信、及び、歩車間(Vehicle to Pedestrian)通信等のV2X通信を行う。また、例えば、通信部103は、ビーコン受信部を備え、道路上に設置された無線局等から発信される電波あるいは電磁波を受信し、現在位置、渋滞、通行規制又は所要時間等の情報を取得する。
移動体内部機器104は、例えば、搭乗者が有するモバイル機器若しくはウェアラブル機器、移動体に搬入され若しくは取り付けられる情報機器、及び、任意の目的地までの経路探索を行うナビゲーション装置等を含む。
出力制御部105は、移動体の搭乗者又は移動体外部に対する各種の情報の出力を制御する。例えば、出力制御部105は、視覚情報(例えば、画像データ)及び聴覚情報(例えば、音声データ)のうちの少なくとも1つを含む出力信号を生成し、出力部106に供給することにより、出力部106からの視覚情報及び聴覚情報の出力を制御する。具体的には、例えば、出力制御部105は、データ取得部102の異なる撮像装置により撮像された画像データを合成して、俯瞰画像又はパノラマ画像等を生成し、生成した画像を含む出力信号を出力部106に供給する。また、例えば、出力制御部105は、衝突、接触、危険地帯への進入等の危険に対する警告音又は警告メッセージ等を含む音声データを生成し、生成した音声データを含む出力信号を出力部106に供給する。
出力部106は、移動体の搭乗者又は移動体外部に対して、視覚情報又は聴覚情報を出力することが可能な装置を備える。例えば、出力部106は、表示装置、インストルメントパネル、オーディオスピーカ、ヘッドホン、搭乗者が装着する眼鏡型ディスプレイ等のウェアラブルデバイス、プロジェクタ、ランプ等を備える。出力部106が備える表示装置は、通常のディスプレイを有する装置以外にも、例えば、ヘッドアップディスプレイ、透過型ディスプレイ、AR(Augmented Reality)表示機能を有する装置等の運転者の視野内に視覚情報を表示する装置であってもよい。尚、出力制御部105および出力部106は、自律移動の処理には必須の構成ではないため、必要に応じて省略するようにしてもよい。
駆動系制御部107は、各種の制御信号を生成し、駆動系システム108に供給することにより、駆動系システム108の制御を行う。また、駆動系制御部107は、必要に応じて、駆動系システム108以外の各部に制御信号を供給し、駆動系システム108の制御状態の通知等を行う。
駆動系システム108は、移動体の駆動系に関わる各種の装置を備える。例えば、駆動系システム108は、4本の脚の各関節に備わった角度やトルクを指定可能なサーボモータ、ロボット自体の移動の動きを4本の足の動きに分解・置換するモーションコントローラ並びに、各モータ内のセンサや足裏面のセンサによるフィードバック制御装置を備える。
別の例では、駆動系システム108は、4基ないし6基の機体上向きのプロペラを持つモータ、ロボット自体の移動の動きを各モータの回転量に分解・置換するモーションコントローラを備える。
さらに、別の例では、駆動系システム108は、内燃機関又は駆動用モータ等の駆動力を発生させるための駆動力発生装置、駆動力を車輪に伝達するための駆動力伝達機構、舵角を調節するステアリング機構、制動力を発生させる制動装置、ABS(Antilock Brake System)、ESC(Electronic Stability Control)、並びに、電動パワーステアリング装置等を備える。尚、出力制御部105、出力部106、駆動系制御部107、および駆動系システム108は、アクチュエータ群113を構成し、図1のアクチュエータ23a-1乃至23a-nからなるアクチュエータ群23に対応する。
記憶部109は、例えば、ROM(Read Only Memory)、RAM(Random Access Memory)、HDD(Hard Disc Drive)等の磁気記憶デバイス、半導体記憶デバイス、光記憶デバイス、及び、光磁気記憶デバイス等を備える。記憶部109は、移動体制御システム100の各部が用いる各種プログラムやデータ等を記憶する。例えば、記憶部109は、ダイナミックマップ等の3次元の高精度地図、高精度地図より精度が低く、広いエリアをカバーするグローバルマップ、及び、移動体の周囲の情報を含むローカルマップ等の地図データを記憶する。
自律移動制御部110は、自動運転又は運転支援等の自律移動に関する制御を行う。具体的には、例えば、自律移動制御部110は、移動体の衝突回避あるいは衝撃緩和、移動体間距離に基づく追従移動、移動体速度維持移動、または、移動体の衝突警告の機能実現を目的とした協調制御を行う。また、例えば、自律移動制御部110は、操作者・ユーザの操作に拠らずに自律的に移動する自律移動等を目的とした協調制御を行う。自律移動制御部110は、検出部131、自己位置推定部132、状況分析部133、計画部134、及び、動作制御部135を備える。このうち、検出部131、自己位置推定部132、および状況分析部133は、認識処理部121を構成し、図1の認識処理部31に対応する。また、計画部134は、行動計画処理部122を構成し、図1の行動計画処理部32に対応する。さらに、動作制御部135は、行動制御処理部123を構成し、図1の行動制御処理部33に対応する。
検出部131は、自律移動の制御に必要な各種の情報の検出を行う。検出部131は、移動体外部情報検出部141、移動体内部情報検出部142、及び、移動体状態検出部143を備える。
移動体外部情報検出部141は、移動体制御システム100の各部からのデータ又は信号に基づいて、移動体の外部の情報の検出処理を行う。例えば、移動体外部情報検出部141は、移動体の周囲の物体の検出処理、認識処理、及び、追跡処理、並びに、物体までの距離の検出処理を行う。検出対象となる物体には、例えば、移動体、人、障害物、構造物、道路、信号機、交通標識、道路標示等が含まれる。また、例えば、移動体外部情報検出部141は、移動体の周囲の環境の検出処理を行う。検出対象となる周囲の環境には、例えば、天候、気温、湿度、明るさ、及び、路面の状態等が含まれる。移動体外部情報検出部141は、検出処理の結果を示すデータを自己位置推定部132、状況分析部133のマップ解析部151、及び、状況認識部152、並びに、動作制御部135等に供給する。
移動体内部情報検出部142は、移動体制御システム100の各部からのデータ又は信号に基づいて、移動体内部の情報の検出処理を行う。例えば、移動体内部情報検出部142は、運転者の認証処理及び認識処理、運転者の状態の検出処理、搭乗者の検出処理、及び、移動体内部の環境の検出処理等を行う。検出対象となる運転者の状態には、例えば、体調、覚醒度、集中度、疲労度、視線方向等が含まれる。検出対象となる移動体内部の環境には、例えば、気温、湿度、明るさ、臭い等が含まれる。移動体内部情報検出部142は、検出処理の結果を示すデータを状況分析部133の状況認識部152、及び、動作制御部135等に供給する。
移動体状態検出部143は、移動体制御システム100の各部からのデータ又は信号に基づいて、移動体の状態の検出処理を行う。検出対象となる移動体の状態には、例えば、速度、加速度、舵角、異常の有無及び内容、運転操作の状態、パワーシートの位置及び傾き、ドアロックの状態、並びに、その他の移動体搭載機器の状態等が含まれる。移動体状態検出部143は、検出処理の結果を示すデータを状況分析部133の状況認識部152、及び、動作制御部135等に供給する。
自己位置推定部132は、移動体外部情報検出部141、及び、状況分析部133の状況認識部152等の移動体制御システム100の各部からのデータ又は信号に基づいて、移動体の位置及び姿勢等の推定処理を行う。また、自己位置推定部132は、必要に応じて、自己位置の推定に用いるローカルマップ(以下、自己位置推定用マップと称する)を生成する。自己位置推定用マップは、例えば、SLAM(Simultaneous Localization and Mapping)等の技術を用いた高精度なマップとされる。自己位置推定部132は、推定処理の結果を示すデータを状況分析部133のマップ解析部151、及び、状況認識部152等に供給する。また、自己位置推定部132は、自己位置推定用マップを記憶部109に記憶させる。
さらに、自己位置推定部132は、センサ群112より供給される検出結果に基づいて、時系列に供給される、例えば、LIDARや車輪エンコーダからの時系列情報をデータベースに蓄積すると共に、蓄積した時系列の情報に基づいて、自己位置を推定し、時系列情報自己位置として出力する。また、自己位置推定部132は、センサ群112より供給される、例えば、ステレオカメラにより撮像された視差画像に基づいて得られるデプス画像などの現在の検出結果に基づいて、自己位置を推定し、現在情報自己位置として出力する。さらに、自己位置推定部132は、時系列情報自己位置と現在情報自己位置とを、例えば、カルマンフィルタや粒子フィルタなどを用いて統合して自己位置推定結果として出力する。尚、ここで出力される時系列情報自己位置、および現在情報自己位置は、いずれも自己位置と共に、自己位置が推定される際に求められる移動体11の方向の情報を含むものである。したがって、以降において、時系列情報自己位置、および現在情報自己位置、並びに単に自己位置と称する場合、それらには、併せて、その自己位置が推定されたときの、移動体11の方向の情報が含まれているものとする。
状況分析部133は、移動体及び周囲の状況の分析処理を行う。また、状況分析部133は、自己位置不定状態であるかに基づいて、動作モードを通常モードから探索モードに切り替えて、行動計画処理部122に対して、探索モードでの行動計画処理を実行させて、周囲の情報を効率よく探索させるように行動を計画させる。状況分析部133は、マップ解析部151、状況認識部152、及び、状況予測部153を備える。
マップ解析部151は、自己位置推定部132及び移動体外部情報検出部141等の移動体制御システム100の各部からのデータ又は信号を必要に応じて用いながら、記憶部109に記憶されている各種のマップの解析処理を行い、自律移動の処理に必要な情報を含むマップを構築する。マップ解析部151は、構築したマップを、状況認識部152、状況予測部153、並びに、計画部134のルート計画部161、行動計画部162、及び、動作計画部163等に供給する。
状況認識部152は、自己位置推定部132、移動体外部情報検出部141、移動体内部情報検出部142、移動体状態検出部143、及び、マップ解析部151等の移動体制御システム100の各部からのデータ又は信号に基づいて、移動体に関する状況の認識処理を行う。例えば、状況認識部152は、移動体の状況、移動体の周囲の状況、及び、移動体の運転者の状況等の認識処理を行う。また、状況認識部152は、必要に応じて、移動体の周囲の状況の認識に用いるローカルマップ(以下、状況認識用マップと称する)を生成する。状況認識用マップは、例えば、占有格子地図(Occupancy Grid Map)、道路地図(Lane Map)、または、点群地図(Point Cloud Map)とされる。
認識対象となる移動体の状況には、例えば、移動体の位置、姿勢、動き(例えば、速度、加速度、移動方向等)、並びに、異常の有無及び内容等が含まれる。認識対象となる移動体の周囲の状況には、例えば、周囲の静止物体の種類及び位置、周囲の動物体の種類、位置及び動き(例えば、速度、加速度、移動方向等)、周囲の道路の構成及び路面の状態、並びに、周囲の天候、気温、湿度、及び、明るさ等が含まれる。認識対象となる運転者の状態には、例えば、体調、覚醒度、集中度、疲労度、視線の動き、並びに、運転操作等が含まれる。
状況認識部152は、認識処理の結果を示すデータ(必要に応じて、状況認識用マップを含む)を自己位置推定部132及び状況予測部153等に供給する。また、状況認識部152は、状況認識用マップを記憶部109に記憶させる。また、状況認識部152は、例えば、電源オン直後の状態や、抱き上げなどであるか否かに基づいて、自己位置不定状態であるか否かを判定する。ここで、自己位置不定状態である場合には、状況認識部152は、動作モードを探索モードに切り替える。動作モードが探索モードに切り替わると、状況認識部152は、行動計画処理部122に対して、探索モードでの行動計画処理を実行させて、周囲の情報を効率よく探索させるように行動を計画させる。より具体的には、状況認識部152は、センサ群112により供給される情報に基づいて、センシング済みの領域の情報を生成し、探索モードになった場合、センシング済みの領域の情報を行動計画処理部122に出力し、周囲のうち、センシングしていない領域を効率よく探索させるように行動を計画させる。
状況予測部153は、マップ解析部151、及び状況認識部152等の移動体制御システム100の各部からのデータ又は信号に基づいて、移動体に関する状況の予測処理を行う。例えば、状況予測部153は、移動体の状況、移動体の周囲の状況、及び、運転者の状況等の予測処理を行う。
予測対象となる移動体の状況には、例えば、移動体の挙動、異常の発生、及び、移動可能距離等が含まれる。予測対象となる移動体の周囲の状況には、例えば、移動体の周囲の動物体の挙動、信号の状態の変化、及び、天候等の環境の変化等が含まれる。予測対象となる運転者の状況には、例えば、運転者の挙動及び体調等が含まれる。
状況予測部153は、予測処理の結果を示すデータを、及び状況認識部152からのデータとともに、計画部134のルート計画部161、行動計画部162、及び、動作計画部163等に供給する。
ルート計画部161は、マップ解析部151及び状況予測部153等の移動体制御システム100の各部からのデータ又は信号に基づいて、目的地までのルートを計画する。例えば、ルート計画部161は、グローバルマップに基づいて、現在位置から指定された目的地までのルートを設定する。また、例えば、ルート計画部161は、渋滞、事故、通行規制、工事等の状況、及び、運転者の体調等に基づいて、適宜ルートを変更する。ルート計画部161は、計画したルートを示すデータを行動計画部162等に供給する。
行動計画部162は、マップ解析部151及び状況予測部153等の移動体制御システム100の各部からのデータ又は信号に基づいて、ルート計画部161により計画されたルートを計画された時間内で安全に移動するための移動体の行動を計画する。例えば、行動計画部162は、発進、停止、進行方向(例えば、前進、後退、左折、右折、方向転換等)、移動速度、及び、追い越し等の計画を行う。行動計画部162は、計画した移動体の行動を示すデータを動作計画部163等に供給する。
より詳細には、行動計画部162は、それぞれルート計画部161により計画されたルートのそれぞれについて、計画された時間内で安全に移動するための移動体の行動計画の候補を行動計画候補として生成する。より具体的には、行動計画部162は、例えば、環境を格子状に区切って、到達判定および経路の重みを最適化して最良のパスを生成するA*algorithm(A star探索アルゴリズム)、道路中心線に従って経路を設定するLane algorithm、および、自己位置からインクリメンタルに到達可能な場所へのパスを適切に枝刈りしながら伸ばしていくRRT(Rapidly-exploring Random Tree) algorithmなどにより行動計画候補を生成する。
動作計画部163は、マップ解析部151及び状況予測部153等の移動体制御システム100の各部からのデータ又は信号に基づいて、行動計画部162により計画された行動を実現するための移動体の動作を計画する。例えば、動作計画部163は、加速、減速、及び、移動軌道等の計画を行う。動作計画部163は、計画した移動体の動作を示すデータを、動作制御部135等に供給する。
動作制御部135は、移動体の動作の制御を行う。
より詳細には、動作制御部135は、移動体外部情報検出部141、移動体内部情報検出部142、及び、移動体状態検出部143の検出結果に基づいて、衝突、接触、危険地帯への進入、運転者の異常、移動体の異常等の緊急事態の検出処理を行う。動作制御部135は、緊急事態の発生を検出した場合、急停止や急旋回等の緊急事態を回避するための移動体の動作を計画する。
また、動作制御部135は、動作計画部163により計画された移動体の動作を実現するための加減速制御を行う。例えば、動作制御部135は、計画された加速、減速、又は、急停止を実現するための駆動力発生装置又は制動装置の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部107に供給する。
動作制御部135は、動作計画部163により計画された移動体の動作を実現するための方向制御を行う。例えば、動作制御部135は、動作計画部163により計画された移動軌道又は急旋回を実現するためのステアリング機構の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部107に供給する。
<認識処理部の詳細な構成例>
次に、図4を参照して、図3の移動体制御システム100のうち、認識処理部31に対応する自律移動制御部110の認識処理部121の具体的な構成例について説明する。
尚、ここでは、移動体が車輪型移動ロボットからなる移動体11である例について説明するが、その他の歩行ロボットや車両などの移動体であってもよい。
センサ群112は、LIDAR201、車輪エンコーダ202、およびステレオカメラ203を備えているものとする。当然のことながら、4足歩行ロボットや車両等であれば、その他のセンサを使用するようにしてもよい。
LIDAR201は、移動体11の周囲の物体までの距離を3次元点群データとして取得し、検出部131を介して、自己位置推定部132の時系列情報自己位置推定部221、および状況認識部152の障害物認識部241に時系列情報として出力する。
車輪エンコーダ202は、移動体11の位置情報(X,Y,Z)、姿勢(クォータニオン)、速度(dx,dy,dz)、加速度(ax,ay,az)、および角速度(wx,wy,wz)を検出し、検出部131を介して、時系列情報自己位置推定部221に時系列情報として出力する。
ステレオカメラ203は、移動体11の周囲の視差画像を撮像して、検出部131を介して、デプス画像生成部226に出力する。
自己位置推定部132は、時系列情報自己位置推定部221、時系列情報DB222、現在情報自己位置推定部223、位置画像特徴量DB224、および自己位置推定結果統合部225を備えている。
時系列情報自己位置推定部221は、LIDAR201および車輪エンコーダ202より供給される、自己位置、および周囲の障害物の位置等の時系列情報を時系列情報DB222に格納する。また、時系列情報自己位置推定部221は、過去から現在までの時系列情報を必要に応じて読み出し、読み出した過去から現在までの時系列情報に基づいて、自己位置を推定し、自己位置推定結果統合部225に供給する。ここで、時系列情報自己位置推定部221は、読み出した過去から現在までの時系列情報に基づいて、例えば、SLAM(Simultaneous Localisation and Mapping)により自己位置を推定する。
時系列情報自己位置推定部221による、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」を参照されたい。尚、時系列情報自己位置推定部221により時系列情報に基づいて推定される自己位置を時系列情報自己位置と称する。
現在情報自己位置推定部223は、ステレオカメラ203より供給される視差画像に基づいて、デプス画像(距離画像)を生成し、デプス画像の画像特徴量を抽出して、位置と画像特徴量とが対応付けて記憶されている位置画像特徴量DB224より、抽出した特徴量に対応する位置の情報に基づいて、自己位置を推定し、自己位置推定結果統合部225に供給する。尚、現在情報自己位置推定部223により現在情報に基づいて推定される自己位置を現在情報自己位置と称する。
自己位置推定結果統合部225は、時系列情報自己位置と、現在情報自己位置とを、例えば、LIDAR201、車輪エンコーダ202、およびステレオカメラ203のそれぞれの状況に応じた信頼度などに応じて統合し、自己位置推定結果として状況認識部152に出力する。尚、自己位置推定結果統合部225は、求められた位置の情報と、そのタイミングで検出されたデプス画像の画像特徴量とを対応付けて位置画像特徴量224に登録する。すなわち、自己位置として推定された位置に対応付けて、そのタイミングにおけるデプス画像の特徴量を登録させることにより、一度、通った位置については、対応するデプス画像の画像特徴量が確実に存在することになるので、高精度に自己位置を推定することが可能となる。
状況認識部152は、障害物認識部241、表面センシング済み領域記録部242、表面センシング可能範囲情報記憶部243、表面センシング済み領域DB244、および探索モード切替部245を備えている。
障害物認識部241は、LIDAR201により検出された、移動体11の周囲の障害物などの物体までの距離の3次元点群情報に基づいて、移動体11の周囲の障害物を認識し、認識結果を表面センシング済み領域記録部242に出力する。
表面センシング済み領域記録部242は、表面センシング可能範囲情報記憶部243に記憶されている、ステレオカメラ203の撮像範囲となる視野角や撮像距離の情報、移動体11の方向を含む時系列情報自己位置の情報、および障害物認識結果に基づいて、ステレオカメラ203による、障害物の表面センシング済み領域を求めて、表面センシング済み領域DB244に登録する。
すなわち、時系列情報自己位置の情報(移動体11自身の方向の情報を含む)と障害物認識結果とから、例えば、図5で示されるように、障害物B1と、自らである移動体11との位置関係が明確になる。尚、図5においては、占有格子地図(Occupancy Grid Map)により、移動体11と障害物B1との位置関係が示されているが、その他のマップであってもよい。
さらに、表面センシング可能範囲情報とは、例えば、図6中の点線で示されるステレオカメラ203(図6中のセンサ群21)により障害物B1の表面をセンシングできる範囲Z1を特定するための情報である。ここで、図6の範囲Z1は、ステレオカメラ203(図6中のセンサ群21)の視野角θ1と撮像可能な距離d1とにより特定される、ステレオカメラ203により障害物B1の表面をセンシングできる範囲である。
表面センシング済み領域記録部242は、移動体11と障害物B1との位置関係の情報、および、表面センシング可能範囲情報に基づいて、センシング済みの領域(視差画像として撮像している障害物B1上の領域)を、表面センシング済み表面領域として求める。すなわち、例えば、図7の場合、障害物B1の表面のうち、点線で示される表面センシング可能範囲Z1に含まれる領域が、表面センシング済み表面領域R1として求められる。
そして、表面センシング済み領域記録部242は、求めた表面センシング済み領域の情報を表面センシング済み領域DB244に、移動体11の時系列情報推定位置や方向、および障害物認識結果と対応付けて記録する。
探索モード切替部245は、電源オンの直後であるか否かに基づいて、自己位置不定状態であるか否かを判定し、自己位置不定の状態ではない場合、通常モードによる動作モードで動作し、自己位置推定結果と、障害物認識結果とを行動計画処理部122に対して出力する。尚、ここでは、自己位置不定状態が、電源オンの直後である場合について説明するものとするが、自己位置不定状態は、それ以外の状態でもよい。自己位置不定状態は、例えば、抱き上げ状態の検出やスリップなど、急激に自己推定結果が変化し、自己位置の情報の連続性が失われるような状態であれば、その他の状態であってもよい。
行動計画処理部122は、ルート計画部161、行動計画部162、および動作計画部163を備えているが、動作モードが通常モードの場合と、探索モードでは異なる処理を実行する。
通常モードの動作においては、ルート計画部161が、自己位置推定結果と、障害物認識結果とに基づいて、目的地までのルートを計画し、行動計画部162が、計画されたルートに基づいて行動計画を生成し、動作計画部163が、行動計画に応じた動作計画を生成し、行動制御処理部123に出力する。
一方、行動計画処理部122は、探索モード切替部245から探索モードでの動作が指示されると、動作モードが探索モードに変化する。このとき、探索モード切替部245は、行動計画処理部122に対して、表面センシング済み領域DB244に格納されている表面センシング済み領域の情報を供給し、探索モードでの操作を指示する。
動作モードが探索モードの場合、自己位置不定状態であるため、改めて自己位置を特定するために、周囲を探索し、周囲の障害物の位置などを効率的に収集できるように行動を計画する必要がある。
ルート計画部161は、探索モード切替部245を介して、表面センシング済み領域の情報に基づいて、自己位置である移動体11の位置と方向に対して、どの向きのどの距離に障害物が存在するのかを把握する。
例えば、図8で示されるように、移動体11の周囲に障害物B21乃至B23が存在し、障害物B21乃至B23における表面のセンシングが完了している表面センシング済み領域R21乃至R23が把握できるような場合を考える。
ルート計画部161は、表面センシングが未完了の領域を複数の領域に分割し、例えば、分割した領域を個別に経由地に設定する。例えば、図8の場合、ルート計画部161は、例えば、分割した領域のそれぞれに経由地P1乃至P4を設定し、経由地P1乃至P4を全て通る、全てのパターンのルートを計画する。
ルート計画部161は、優先度決定部281を備えており、経由地P1乃至P4を全て通る、全てのパターンのルートの情報と、優先条件とに基づいて、経由地P1乃至P4のそれぞれに優先度を設定させる。
例えば、図8において、経由地P1乃至P4の全てを必ず通る、全てのルートを考えた場合の、優先条件と機体運動情報により求められる優先度コストが最小となるルートにおける経由地P1乃至P4の順序に応じて設定される値である。
ここで、優先条件は、総移動距離や総消費電力などであり、ルートを設定する上で最も優先すべき条件であり、様々な条件を設定することができる。
また、移動体11の機体運動情報とは、移動体11の運動性能に係る情報であり、例えば、優先条件が総移動距離である場合、移動体11の最高速度、最高加速度、最小回転半径、および経路上の超えられる段差の高さなどである。さらに、優先条件が総消費電力である場合、移動体11の機体運動情報とは、移動体11の単位距離の電力消費とバッテリの充電容量などである。
さらに、優先度コストとは、優先条件と機体運動情報に基づいて設定されるコストであり、優先条件が総移動距離であれば、移動距離に係るコストであり、優先条件が総消費電力であれば、電力消費に係るコストである。ルート計画部161の優先度決定部281は、全てのルートに対する優先度コストを算出し、優先度コストが最小になるルートにおける経由地の通過順序を優先度に設定する。
例えば、経由地P1,P2,P3,P4の順序で移動する第1のルートと、経由地P1,P3,P2,P4の順序で移動する第2のルートとがあり、この2つのルートについて総移動距離を優先条件とするときの優先度コストを考える。仮に、第1のルートより、第2のルートの方が、総移動距離が短く、優先度コストが小さい場合、優先度は、第2のルートにおける経由地の通過順序で設定される。すなわち、第2のルートにおける経由地P1乃至P4の順序に従って、経由地P1の優先度が第1位に設定され、経由地P3の優先度が第2位に設定され、経由地P2の優先度が第3位に設定され、経由地P4の優先度が第4位に設定される。
そして、ルート計画部161は、各経由地P1乃至P4を通る全てのルートを計画し、優先度と、移動体11のセンサ群21によりセンシング可能な領域や、移動条件とに基づいて、ルート毎にルートコストを計算し、最もルートコストが低いルートを選択し、選択したルートに基づいた行動計画を生成する。
ここで、移動条件とは、探索するルートを設定する上での移動に係る優先すべき条件であり、例えば、経由地は優先度の高い順に移動する、経由地の優先度を無視しても最短時間で移動するといった移動に係る優先すべき条件であり、自由に設定することができる。
また、ルートコストとは、経由地の優先度と、ルート設定に優先される移動条件とに基づいて設定される、全ての可能なルート集合におけるそのルートの相対的な良しあしを定量的に表す手段であるである。例えば、経路として選択可能な空間を格子状に区切って、到達判定を行い、さらに、経路毎の重みを最適化してコストが最小となる最良のパス(ルート)を設定するA*(スター)アルゴリズムを用いてルートを設定させる場合を考える。
このとき、優先度の高い経由地ほど、早く到着するという移動条件を満たすルートを設定させるには、経由地毎の優先度と、対応する経由地までの移動時間との積をルートコストに設定し、ルートコストが最小となるルートを選択させるようにすることで、複数の考えられるルートの中から、優先度の高い経由地ほど、早く到着するルートが設定される。
ルート計画部161は、このような処理により、効率よく周囲の障害物の表面センシングを実現させるための探索に必要とされるルートを計画する。
これにより、自己位置不定状態となった場合でも、効率よく周囲を探索しながらセンシングすることにより、効率よく周囲の情報を取得して、改めて自己位置を推定することが可能となる。結果として、自己位置不定状態に陥っても、迅速に自己位置を求められる状態に復帰することができ、動作モードを探索モードから通常モードに早期に復帰させることが可能となる。
<自己位置推定処理>
次に、図9のフローチャートを参照して、自己位置推定処理について説明する。
ステップS11において、時系列情報自己位置推定部221は、時系列情報自己位置推定処理を実行して、時系列情報自己位置を推定して、自己位置推定結果統合部225、および表面センシング済み領域記録部に出力する。尚、時系列情報自己位置推定処理については、図10のフローチャートを参照して、後述する。
ステップS12において、現在情報自己位置推定部223は、現在情報自己位置推定処理を実行して、現在情報自己位置を推定して、自己位置推定結果統合部225に出力する。尚、現在情報自己位置推定処理については、図11のフローチャートを参照して、後述する。
ステップS13において、自己位置推定結果統合部225は、時系列情報自己位置および現在情報自己位置を、例えば、カルマンフィルタや粒子フィルタにより統合し、統合した自己位置の情報を自己位置推定結果として状況認識部152に出力する。
ステップS14において、自己位置推定結果統合部225は、自己位置推定結果と、その自己位置推定結果を特定するために利用した現在情報である視差画像より求められたデプス画像の特徴量とを対応付けて位置画像特徴量DB224に登録する。
すなわち、自己位置として推定された位置と、その位置を特定するために利用された現在情報である視差画像より求められたデプス画像の特徴量とが対応付けられて位置画像特徴量DB224に登録される。これにより、一度通過した位置に対応する現在情報である視差画像より求められたデプス画像の特徴量が、位置に対応付けられて新たな情報として蓄積される。結果として、再び同一の位置を通過するときには、高精度に自己位置推定することが可能となる。
また、以上の処理により、時系列情報自己位置と現在情報自己位置とが順次求められて、相互に統合された位置が自己位置推定結果として出力される。
<時系列情報自己位置推定処理>
次に、図10のフローチャートを参照して、時系列情報自己位置推定処理について説明する。
ステップS31において、LIDAR201は、移動体11の周囲の物体までの距離を3次元点群データとして検出し、検出部131を介して、自己位置推定部132の時系列情報自己位置推定部221に時系列情報として出力する。
ステップS32において、車輪エンコーダ202は、移動体11の位置情報、姿勢、速度、加速度、および角速度を検出し、検出部131を介して、時系列情報自己位置推定部221に時系列情報として出力する。
ステップS33において、時系列情報自己位置推定部221は、最新の時系列情報を取得し、時系列情報DB222に登録する。
ステップS34において、時系列情報自己位置推定部221は、時系列情報DB222に登録されている最新の時系列情報と、過去の時系列情報とから、例えば、カルマンフィルタや粒子フィルタを用いて、自己位置を推定する。そして、時系列情報自己位置推定部221は推定結果である自己位置を時系列情報自己位置情報として自己位置推定結果統合部225に出力する。
以上の処理により、LIDAR201および車輪エンコーダ202により検出される時系列情報が順次検出されて、時系列情報DB222に順次蓄積されて、時系列に蓄積された過去から現在までの時系列に蓄積された時系列情報に基づいて、自己位置が推定されて、時系列情報自己位置として自己位置推定結果統合部225に出力される。
<現在情報自己位置推定処理>
次に、図8のフローチャートを参照して、現在情報自己位置推定処理について説明する。
ステップS51において、ステレオカメラ203は、移動体11の周囲の視差画像を撮像して、検出部131を介して、現在情報自己位置推定部223に現在情報として出力する。
ステップS52において、現在情報自己位置推定部223は、ステレオカメラ203より供給される視差画像に基づいて、デプス画像(距離画像)を生成する。
ステップS53において、現在情報自己位置推定部223は、生成したデプス画像に基づいて、画像特徴量を抽出する。
ステップS54において、現在情報自己位置推定部223は、位置と画像特徴量とが対応付けて記憶されている位置画像特徴量DB224より、抽出した画像特徴量に対応する位置の情報に基づいて、自己位置を推定する。そして、現在情報自己位置推定部223は、推定結果である自己位置を現在情報自己位置として自己位置推定結果統合部225に供給する。
以上の処理により、ステレオカメラ203により視差画像が現在情報として撮像され、視差画像よりデプス画像が生成され、生成されたデプス画像の画像特徴量が抽出されて、画像特徴量に対応付けて登録されている位置情報に基づいて、現在情報自己位置が推定され、自己位置推定結果統合部225に出力される。
<障害物認識処理>
次に、図12のフローチャートを参照して、障害物認識処理について説明する。
ステップS71において、LIDAR201は、移動体11の周囲の物体までの距離を3次元点群データとして検出し、検出部131を介して、自己位置推定部132の時系列情報自己位置推定部221に時系列情報として出力する。尚、この処理は、図10のフローチャートを参照して説明したステップS31の処理と同様である。
ステップS72において、障害物認識部241は、移動体11の周囲の物体までの距離を3次元点群データに基づいて、移動体11の周囲の障害物の位置を認識する。
ステップS73において、障害物認識部241は、認識している移動体11の周囲の障害物の位置の情報に基づいて、格子状地図を生成して、表面センシング済み領域記録部242、および探索モード切替部245に出力する。
以上の処理により、移動体11の周囲の障害物の位置の情報を表す、障害物認識結果が、例えば、図5で示されるような格子状地図として生成されて、表面センシング済み領域記録部242、および探索モード切替部245に出力される。
<表面センシング済み領域記録処理>
次に、図13のフローチャートを参照して、表面センシング済み領域記録処理について説明する。尚、表面センシング済み領域記録処理は、後述する動作モードが探索モードになったときに実行される処理である。
ステップS81において、表面センシング済み領域記録部242は、探索モード切替部245より表面センシング済み領域記録処理の開始が指示されたか否かを判定し、開始が指示されるまで、同様の処理が繰り返される。そして、ステップS81において、表面センシング済み領域記録処理の開始が指示された場合、処理は、ステップS82に進む。
ステップS82において、表面センシング済み領域記録部242は、表面センシング済み領域DB244をリセットする。
ステップS93において、表面センシング済み領域記録部242は、障害物認識部241より障害物認識結果を取得する。
ステップS94において、表面センシング済み領域記録部242は、時系列情報自己位置推定部221より時系列情報自己位置の情報を取得する。
ステップS95において、表面センシング済み領域記録部242は、時系列情報自己位置、障害物認識結果より取得できる障害物位置、および表面センシング可能範囲情報記憶部243に記憶されている表面センシング可能範囲の情報に基づいて、図7を参照して説明したように、表面センシング済み領域を求めて、表面センシング済み領域DB244に記録する。
ステップS96において、表面センシング済み領域記録部242は、処理の終了が指示されたか否かを判定し、終了が指示されていない場合、処理は、ステップS93に戻る。すなわち、処理の終了が指示されるまで、ステップS93乃至S96の処理が繰り返されて、順次、表面センシング済み領域の情報が、表面センシング済み領域DB244に蓄積される。
そして、ステップS95において、処理の終了が指示された場合、処理は、終了する。
以上の処理により、探索モードにおいて、移動体11の周囲のうち、ステレオカメラ203によりにより撮像されている(センシング済みの)障害物の表面の領域の情報が、例えば、図8で示されるように、障害物B21乃至B23の表面センシング済み領域R21乃至R23として蓄積される。
<自律移動制御処理>
次に、図14のフローチャートを参照して、図4の移動体制御システム100における自律移動制御処理について説明する。
ステップS121において、探索モード切替部245は、自己位置不定状態であるか否かを判定する。例えば、探索モード切替部245は、例えば、電源投入直後であるか否かに基づいて、自己位置不定状態であるか否かを判定する。
ステップS121において、自己位置不定状態ではないと判定された場合、処理は、ステップS122に進む。
ステップS122において、探索モード切替部245は、自己位置推定結果と、障害物認識結果とを行動計画処理部122の計画部134におけるルート計画部161に出力する。
ステップS123において、ルート計画部161は、目的地までのルートを計画し、行動計画部162に出力する。
ステップS124において、行動計画部162は、目的地までのルートに基づいて、行動を計画し、行動計画として動作計画部163に出力する。
ステップS125において、動作計画部163は、行動計画部162より供給された行動計画に基づいて、動作を計画し、動作計画として行動制御処理部123に出力する。
ステップS126において、行動制御処理部123は、動作計画に基づいて、アクチュエータ群113の各アクチュエータ23aの動作を制御して、移動体11の行動を制御する。
ステップS127において、終了が指示されたか否かが判定され、終了が指示されていないとみなされた場合、処理は、ステップS121に戻る。すなわち、自己位置不定状態とならない限り、ステップS121乃至S127の処理、すなわち、動作状態が通常モードである場合の処理が繰り返されて、移動体11は、自律的な移動を継続する。
一方、ステップS121において、自己位置不定状態であるとみなされた場合、処理は、ステップS128に進む。
ステップS128において、探索モード切替部245は、動作モードを探索モードに切り替えて、探索モード処理を実行させ、効率よく移動体11の周囲を探索して、ステレオカメラにより周囲の情報を迅速に取得できるように制御する。そして、現在情報自己位置が適切に推定できる状態になるまで、探索モードの処理が継続され、現在情報自己位置が推定できる状態になった場合、動作モードが、探索モードから通常モードに復帰する。
<探索モード処理>
次に、図15のフローチャートを参照して、探索モード処理について説明する。
ステップS151において、探索モード切替部245は、表面センシング済み領域記録部242に対して、表面センシング済み領域DB生成処理を開始するように指示する。この指示により、図13のフローチャートを参照して説明した表面センシング済み領域DB生成処理が開始される。
ステップS152において、探索モード切替部245は、表面センシング済み領域DB244より表面センシング済み領域の情報を行動計画処理部122の計画部134におけるルート計画部161に出力する。
ステップS153において、ルート計画部161は、表面センシング済み領域の情報に基づいてセンシングしていない領域を複数に分割し、分割した各領域を経由地として設定する。
ステップS154において、ルート計画部161は、分割した各領域からなる全経由地を通過するルートの全てのパターンを計画する。
ステップS155において、ルート計画部161は、優先度決定部281を制御して、図8を参照して説明したように、優先条件に応じて、計画されたルート毎の優先度コストを求めて、優先度が最小となるルートに基づいて経由地の優先度を設定させる。
ステップS156において、ルート計画部161は、経由地の優先度と、移動条件に基づいて、全てのルートについてルートコストを算出する。
ステップS157において、ルート計画部161は、ルートコストが最小となるルートを探索ルートに決定し、行動計画部162に出力する。
ステップS158において、行動計画部162は、決定した探索ルートに基づいて、行動を計画し、行動計画として動作計画部163に出力する。
ステップS159において、動作計画部163は、行動計画部162より供給された行動計画に基づいて、動作を計画し、動作計画として行動制御処理部123に出力する。
ステップS160において、行動制御処理部123は、動作計画に基づいて、アクチュエータ群113の各アクチュエータ23aの動作を制御して、移動体11の行動を制御する。
ステップS161において、探索モード切替部245は、自己位置不定状態でなくなったか否かにより、探索モード処理を終了させるか否かを判定する。自己位置不定状態が電源オン直後であるような場合については、電源オンから所定時間が経過したか否か、または、時系列情報が所定数以上蓄積されて時系列情報としての信頼度が高くなったか否かなどにより、自己位置不定状態であるか否かが判定されるようにしてもよい。
ステップS161において、自己位置不定状態が継続しており、探索モード処理を終了させない場合、処理は、ステップS152に戻る。すなわち、探索モード処理が継続される限り、ステップS152乃至S161の処理が繰り返されて、刻々と変化する表面センシング済み領域の情報に基づいて、探索モードにおける探索ルートが繰り返し設定されて、移動体11が周辺の探索を継続する。そして、自己位置不定状態ではなくなった場合、処理は、ステップS162に進む。
ステップS162において、探索モード切替部245は、表面センシング済み領域記録部242に対して、表面センシング済み領域DB生成処理の終了を指示し、処理は終了する。
以上の処理により、自己位置不定状態になった場合、動作モードが探索モードに切り替えられて、ステレオカメラ203により撮像される表面センシング済み領域から、センシングされていない、すなわち、撮像されていない障害物の表面が、移動体11の周囲の情報として効率よく撮像されるように、探索する探索ルートを設定することが可能となる。
すなわち、探索モードにおいては、自己位置不定状態であるため、目的地へのルートが設定できない状態であるので、目的地に向かって移動するためのルートではなく、周辺を効率よく探索することが重視されたルートが設定されることになる。
結果として、探索モードから通常モードへと迅速に切り替えることが可能となり、自己位置不定状態から迅速に自己位置が特定できる状態にすることが可能となり、通常モードに迅速に復帰することが可能となる。
尚、以上においては、現在情報自己位置推定にあたり、現在情報としてステレオカメラ203により撮像された視差画像に基づいて、デプス画像を生成し、生成されたデプス画像の画像特徴量を用いる例について説明してきたが、デプス画像が生成できれば、ステレオカメラ203を用いた視差画像以外であってもよい。
例えば、現在情報を検出する構成としては、ステレオカメラ203に代えて、ToF (Time of flight)センサを用いるようにしてもよい。Tofセンサを用いる場合、表面センシング可能範囲は、最低距離、最大距離、視野角で表現される。
また、以上においては、表面センシング可能範囲は、可能な範囲と不可能な範囲とからなる、例えば、0と1の2値で表現されるものである例について説明してきたが、3値以上の刻みを持った値、例えば、0乃至1の間を刻んだ浮動小数点数値で表現してもよい。このような表現とすることにより、表面センシング可能範囲は、センシング状態や障害物との距離または角度によって、センシングし易い範囲や、センシングし難い範囲を表現することが可能となる。これにより、例えば、表面センシングし難い範囲については、表面センシングし易い範囲よりも、単位時間当たりで、表面センシングする回数を増やすようにするなどして、表面センシングの精度を、表面センシングし易い範囲と同等にできるような処理をしてもよい。
さらに、以上においては、障害物認識結果を、図5で示されるような方形状の集合により表現される2次元の占有格子地図(Occupancy Grid Map)を用いて表現する例について説明してきたが、立方体の集合からなる3次元の占有格子地図を用いて表現するようにしてもよい。これにより、表面センシング可能範囲についても、水平方向および垂直方向の2次元の固定サイズの方形の集合として表現するようにしてもよいし、水平方向、垂直方向、および高さ方向の3次元の固定サイズの立方体の集合として表現するようにしてもよい。
また、表面センシング済み領域DB243に登録されている表面センシング済み領域の情報は、記録状態が継続している時間の長さに応じて、障害物認識結果と共に消去されるようにしてもよい。
すなわち、所定時間が経過するとき、表面センシング済み領域の情報が、対応付けて登録されている障害物認識結果の情報と共に削除されることにより、探索モード処理において、一度センシングした領域であっても、再び、表面センシングさせるようにしてもよく、これにより、再び、表面センシングするためのルートが設定されるようになる。
このように、表面センシング済みの領域の情報が、時間の経過に伴って削除されることにより、一度表面センシングした領域であっても、時間が経過したところで、再び表面センシングさせるようにすることができ、例えば、周辺の障害物の形状や位置が変化してしまうような状況であっても周辺の情報を適切に取得することができる。
さらに、表面センシング済み領域の情報は、例えば、予め定められた忘却関数に従い、時間と共に、その値が減少していくように削除するようにしてもよいし、例えば、表面センシング済み領域の有無が0または1の2値で表現される場合、0になるように削除するようにしてもよい。
また、忘却関数は、記録後の経過時間t、記録時間の最大値Tに対し、定められた係数kを乗じた値max(0,T-kt)(max(A,B)はAまたはBのうち大きい方を選択する関数である)や、定められた係数kをかけた値kT/tとしてもよい。
さらに、忘却関数は、記録後の経過時間t、記録の最大値Tに対し、定められた忘却時間Bを用いて、T(0<t<=B)、0(B<t)とするようにしてもよい。
また、以上においては、移動体11が車輪型移動ロボットである場合に、時系列情報として、LIDAR201と車輪エンコーダ202により検出される情報を用い、現在情報としてステレオカメラ203により撮像される視差画像を用いる例について説明してきた。
しかしながら、時系列情報および現在情報については、これに限らず、移動体11の形態に合わせたものとしてもよい。すなわち、移動体11がドローンである場合には、時系列情報を、LIDARとIMU(慣性計測装置)や加速度センサにより検出される情報にして、現在情報をGNSSにより取得される情報にしてもよい。
また、移動体11が車両である場合には、時系列情報を、LIDARにより検出される情報にして、現在情報をGNSSにより取得される情報にしてもよい。さらに、移動体11が多足歩行ロボットである場合には、時系列情報を、LIDAR、IMU(慣性計測装置)、およびアクチュエータに付属のセンサにより検出される情報にして、現在情報をステレオカメラ、IMU(慣性計測装置)、およびアクチュエータに付属のセンサにより取得される情報にしてもよい。
さらに、ステレオカメラ203に代えて、LIDAR、およびToFセンサを用いるようにしてもよい。
<<3.変形例>>
以上においては、図16の左部で示されるように、移動体11の矢印で示される移動方向に対して、移動方向に対して前方が撮像方向とされているステレオカメラ203が設けられている場合、ステレオカメラ203の表面センシング可能範囲が、視野角θ1、および撮像可能距離d1で固定されている例について説明してきた。
しかしながら、例えば、図16の中央部で示されるように、ステレオカメラ203-101に対して撮像方向を左に向けたステレオカメラ203-102から、右に向けたステレオカメラ203-103までの範囲で、水平方向に左右に首を振るような動作(パン動作)ができるようにしてもよい。
この場合、表面センシング可能範囲Zは、ステレオカメラ203-101乃至203-103のそれぞれの表面センシング可能範囲Z1-1乃至Z1-3の全範囲を含んだ範囲となる。
尚、図16の中央部においては、水平方向に左右にステレオカメラ203に首を振るように動作(パン動作)をさせる例が示されているが、さらに、垂直方向に首を振れるような動作(チルト動作)ができるようにすると、さらに、表面センシング可能範囲を広くすることができる。さらに、ステレオカメラ203にズーム機能を持たせると、水平方向および垂直方向に対して広範囲で、かつ、撮像可能距離を遠くまで伸ばすことができるので、表面センシング可能範囲を、さらに拡大させることが可能となる。
また、ステレオカメラ203の撮像可能な距離については、例えば、図16の右部で示されるように、ステレオカメラ203の表面センシング範囲Z1の視野角θ1で、かつ、撮像距離d1からなる範囲Z1よりも、十分に広い、視野角θ3で、かつ、距離d2となる範囲Z3を照明できる照明301を設けるようにしてもよい。このような構成とすることで、例えば、ステレオカメラ203の撮像距離が照度による限界であった場合には、照度が高くなることにより、例えば、撮像可能距離を距離d1よりも遠い距離d3とすることが可能となる。
さらに、図16の中央部と右部との構成を組み合わせるようにして、ステレオカメラ203の撮像方向のパンチルトに合わせて、照明301についてもパンチルトするようにしてもよい。このようにすることでも、表面センシング可能範囲を広くすることが可能となる。
このように表面センシング可能範囲を広くすることで、単位時間当たりの周囲より検出できる情報量が増えることになるので、表面センシング速度を実質的に高速にすることが可能となり、ルートコストを計算する上でも、よりコストを低減させることができる。結果として、探索モードから通常モードへの復帰をより高速にすることが可能となる。
<<4.ソフトウェアにより実行させる例>>
ところで、上述した一連の処理は、ハードウェアにより実行させることもできるが、ソフトウェアにより実行させることもできる。一連の処理をソフトウェアにより実行させる場合には、そのソフトウェアを構成するプログラムが、専用のハードウェアに組み込まれているコンピュータ、または、各種のプログラムをインストールすることで、各種の機能を実行することが可能な、例えば汎用のコンピュータなどに、記録媒体からインストールされる。
図17は、汎用のコンピュータの構成例を示している。このパーソナルコンピュータは、CPU(Central Processing Unit)1001を内蔵している。CPU1001にはバス1004を介して、入出力インタフェース1005が接続されている。バス1004には、ROM(Read Only Memory)1002およびRAM(Random Access Memory)1003が接続されている。
入出力インタフェース1005には、ユーザが操作コマンドを入力するキーボード、マウスなどの入力デバイスよりなる入力部1006、処理操作画面や処理結果の画像を表示デバイスに出力する出力部1007、プログラムや各種データを格納するハードディスクドライブなどよりなる記憶部1008、LAN(Local Area Network)アダプタなどよりなり、インターネットに代表されるネットワークを介した通信処理を実行する通信部1009が接続されている。また、磁気ディスク(フレキシブルディスクを含む)、光ディスク(CD-ROM(Compact Disc-Read Only Memory)、DVD(Digital Versatile Disc)を含む)、光磁気ディスク(MD(Mini Disc)を含む)、もしくは半導体メモリなどのリムーバブルメディア1011に対してデータを読み書きするドライブ1010が接続されている。
CPU1001は、ROM1002に記憶されているプログラム、または磁気ディスク、光ディスク、光磁気ディスク、もしくは半導体メモリ等のリムーバブルメディア1011ら読み出されて記憶部1008にインストールされ、記憶部1008からRAM1003にロードされたプログラムに従って各種の処理を実行する。RAM1003にはまた、CPU1001が各種の処理を実行する上において必要なデータなども適宜記憶される。
以上のように構成されるコンピュータでは、CPU1001が、例えば、記憶部1008に記憶されているプログラムを、入出力インタフェース1005及びバス1004を介して、RAM1003にロードして実行することにより、上述した一連の処理が行われる。
コンピュータ(CPU1001)が実行するプログラムは、例えば、パッケージメディア等としてのリムーバブルメディア1011に記録して提供することができる。また、プログラムは、ローカルエリアネットワーク、インターネット、デジタル衛星放送といった、有線または無線の伝送媒体を介して提供することができる。
コンピュータでは、プログラムは、リムーバブルメディア1011をドライブ1010に装着することにより、入出力インタフェース1005を介して、記憶部1008にインストールすることができる。また、プログラムは、有線または無線の伝送媒体を介して、通信部1009で受信し、記憶部1008にインストールすることができる。その他、プログラムは、ROM1002や記憶部1008に、あらかじめインストールしておくことができる。
なお、コンピュータが実行するプログラムは、本明細書で説明する順序に沿って時系列に処理が行われるプログラムであっても良いし、並列に、あるいは呼び出しが行われたとき等の必要なタイミングで処理が行われるプログラムであっても良い。
尚、図17におけるCPU1001が、図3における自律移動制御部110の機能を実現させる。また、図17における記憶部1008が、図3における記憶部109を実現する。
また、本明細書において、システムとは、複数の構成要素(装置、モジュール(部品)等)の集合を意味し、すべての構成要素が同一筐体中にあるか否かは問わない。したがって、別個の筐体に収納され、ネットワークを介して接続されている複数の装置、及び、1つの筐体の中に複数のモジュールが収納されている1つの装置は、いずれも、システムである。
なお、本開示の実施の形態は、上述した実施の形態に限定されるものではなく、本開示の要旨を逸脱しない範囲において種々の変更が可能である。
例えば、本開示は、1つの機能をネットワークを介して複数の装置で分担、共同して処理するクラウドコンピューティングの構成をとることができる。
また、上述のフローチャートで説明した各ステップは、1つの装置で実行する他、複数の装置で分担して実行することができる。
さらに、1つのステップに複数の処理が含まれる場合には、その1つのステップに含まれる複数の処理は、1つの装置で実行する他、複数の装置で分担して実行することができる。
尚、本開示は、以下のような構成も取ることができる。
<1> 自己位置を推定する自己位置推定部と、
周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、
前記障害物の表面をセンシングする表面センシング部と、
前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する表面センシング済み領域記録部と、
前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部と
を含む制御装置。
<2> 前記表面センシング済み領域記録部は、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する
<1>に記載の制御装置。
<3> 前記ルート計画部は、
前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、
前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する
<2>に記載の制御装置。
<4> 前記優先度設定部は、前記経由地を全て通過するルートのうち、優先条件に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定する
<3>に記載の制御装置。
<5> 前記優先条件は、総移動距離、または総消費電力であり、
前記優先度コストは、移動距離に応じて設定されるコスト、または、消費電力に応じて設定されるコストである
<4>に記載の制御装置。
<6> 前記優先度設定部は、前記経由地を全て通過するルートのうち、前記優先条件、および機体運動情報に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定する
<4>に記載の制御装置。
<7> 前記機体運動情報は、
前記優先条件が総移動距離である場合、最高速度、最小回転半径、および経路上の超えられる段差の高さの情報であり、
前記優先条件が総消費電力である場合、単位距離当たりの電力消費量、およびバッテリの充電容量の情報である
<6>に記載の制御装置。
<8> 前記ルート計画部は、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて、前記経由地を全て経由する前記ルートを計画する
<3>に記載の制御装置。
<9> 前記ルート計画部は、前記経由地を全て経由する全てのルートについて、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて設定されるルートコストを算出し、前記ルートコストが最小となるルートを選択する
<3>に記載の制御装置。
<10> 前記移動条件は、優先度の高い順に前記経由地を通過するという条件、経由地の優先度を無視しても最短時間で移動するという条件、または、優先度の高い経由地ほど、早く到着するという条件を含む
<9>に記載の制御装置。
<11> 前記移動条件が、優先度の高い経由地ほど、早く到着するという条件である場合、前記ルート計画部は、前記優先度と、前記経由地までの到着時間との積を前記ルートコストとして設定し、前記全てのルートのうち、前記ルートコストが最小となるルートを選択する
<10>に記載の制御装置。
<12> 前記自己位置推定部は、
時系列に供給されるセンサ情報からなる時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部を含み、
前記表面センシング済み領域記録部は、前記時系列情報自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する
<1>に記載の制御装置。
<13> 前記時系列情報は、LIDAR(Light Detection and Ranging、Laser Imaging Detection and Ranging)により検出される3次元点群情報、および車輪エンコーダにより検出される位置情報、姿勢、速度、加速度、および角速度であり、
前記障害物位置情報生成部は、前記3次元点群情報に基づいて、前記周辺の障害物の相対位置および方向からなる障害物位置情報を生成する
<12>に記載の制御装置。
<14> 前記自己位置推定部は、
現在のセンシング結果を出力するセンサ情報からなる現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部をさらに含む
<12>に記載の制御装置。
<15> 前記現在情報は、ステレオカメラからなる前記表面センシング部により撮像される視差画像であり、
前記現在情報自己位置推定部は、前記現在情報である視差画像に基づいて、デプス画像を生成し、前記デプス画像より画像特徴量を抽出し、前記画像特徴量と対応付けられている位置に基づいて、前記自己位置を推定し、現在情報自己位置として出力し、
前記自己位置推定部は、前記時系列情報自己位置および前記現在情報自己位置を統合して、前記自己位置として出力する
<14>に記載の制御装置。
<16> 前記位置と前記画像特徴量とが対応付けて記憶される位置画像特徴量記憶部をさらに含み、
前記自己位置推定部は、前記時系列情報自己位置および前記現在情報自己位置を統合して、自己位置として出力するとき、前記現在情報自己位置に対応する画像特徴量と、前記自己位置とを対応付けて、前記位置画像特徴量記憶部に記憶させる
<15>に記載の制御装置。
<17> 前記自己位置が不定状態の場合、前記表面センシング済み領域記録部は、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する
<1>乃至<16>のいずれかに記載の制御装置。
<18> 自己位置を推定する自己位置推定処理と、
周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成処理と、
前記障害物の表面をセンシングする表面センシング処理と、
前記自己位置、前記障害物位置情報、および前記表面センシング処理の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する表面センシング済み領域記録処理と、
前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画処理と
を含む制御方法。
<19> 自己位置を推定する自己位置推定部と、
周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、
前記障害物の表面をセンシングする表面センシング部と、
前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する表面センシング済み領域記録部と、
前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部と
してコンピュータを機能させるプログラム。
<20> 自己位置を推定する自己位置推定部と、
周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、
前記障害物の表面をセンシングする表面センシング部と、
前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する表面センシング済み領域記録部と、
前記障害物の表面センシング済み領域に基づいて、ルートを計画する計画部と、
前記計画部により計画された前記ルートに基づいて、行動計画を生成する行動計画生成部と、
前記行動計画生成部により決定された行動計画に基づいて移動体の動作を制御する制御部と
を含む移動体。
11 移動体, 21 センサ群, 21a,21a-1乃至21a-n センサ, 22 自律移動制御部, 23 アクチュエータ群, 23a,23a-1乃至23a-n アクチュエータ, 31 認識処理部, 32 行動計画処理部, 33 行動制御処理部, 41 ルート計画部, 42 行動計画部, 43 動作計画部, 102 データ取得部, 105 出力制御部, 106 出力部, 107 駆動系制御部, 108 駆動系システム, 110 自律移動制御部, 112 センサ群, 113 アクチュエータ群, 121 認識処理部, 122 行動計画処理部, 123 行動制御処理部, 134 計画部, 161 ルート計画部, 162 行動計画部, 163 動作計画部, 201 LIDAR, 202 車輪エンコーダ, 203 ステレオカメラ, 221 時系列情報自己位置推定部, 222 時系列情報DB, 223 現在情報自己位置推定部, 224 位置画像特徴量DB, 225 自己位置推定結果統合部, 241 障害物認識部, 242 表面センシング済み領域記録部, 243 表面センシング済み可能範囲情報記憶部, 244 表面センシング済み領域DB, 245 探索モード判定部

Claims (18)

  1. 自己位置を推定する自己位置推定部と、
    周辺の障害物の位置からなる障害物位置情報を生成する障害物位置情報生成部と、
    前記障害物の表面をセンシングする表面センシング部と、
    前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記表面センシング部の表面センシング可能範囲内の前記障害物の表面を、前記障害物の表面センシング済み領域として記録する表面センシング済み領域記録部と、
    前記障害物の表面センシング済み領域に基づいて、ルートを計画するルート計画部とを含み、
    前記ルート計画部は、
    前記障害物の表面センシング済み領域、および前記障害物位置情報に基づいて、前記障害物の表面センシングが済んでいない未済領域を設定し、前記未済領域を分割した分割領域を経由地として設定し、前記経由地に優先度を設定する優先度設定部を含み、
    前記優先度設定部により設定された前記優先度に基づいて、前記経由地を経由する前記ルートを計画する
    制御装置。
  2. 前記優先度設定部は、前記経由地を全て通過するルートのうち、優先条件に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定する
    請求項に記載の制御装置。
  3. 前記優先条件は、総移動距離、または総消費電力であり、
    前記優先度コストは、移動距離に応じて設定されるコスト、または、消費電力に応じて設定されるコストである
    請求項に記載の制御装置。
  4. 前記優先度設定部は、前記経由地を全て通過するルートのうち、前記優先条件、および機体運動情報に基づいて設定されるコストである優先度コストが最小となるルートにおける経由地の通過順序に応じて、前記経由地の優先度を設定する
    請求項に記載の制御装置。
  5. 前記機体運動情報は、
    前記優先条件が総移動距離である場合、最高速度、最小回転半径、および経路上の超えられる段差の高さの情報であり、
    前記優先条件が総消費電力である場合、単位距離当たりの電力消費量、およびバッテリの充電容量の情報である
    請求項に記載の制御装置。
  6. 前記ルート計画部は、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて、前記経由地を全て経由する前記ルートを計画する
    請求項に記載の制御装置。
  7. 前記ルート計画部は、前記経由地を全て経由する全てのルートについて、前記優先度設定部により前記経由地毎に設定される前記優先度、および移動条件に基づいて設定されるルートコストを算出し、前記ルートコストが最小となるルートを選択する
    請求項に記載の制御装置。
  8. 前記移動条件は、優先度の高い順に前記経由地を通過するという条件、経由地の優先度を無視しても最短時間で移動するという条件、または、優先度の高い経由地ほど、早く到着するという条件を含む
    請求項に記載の制御装置。
  9. 前記移動条件が、優先度の高い経由地ほど、早く到着するという条件である場合、前記ルート計画部は、前記優先度と、前記経由地までの到着時間との積を前記ルートコストとして設定し、前記全てのルートのうち、前記ルートコストが最小となるルートを選択する
    請求項に記載の制御装置。
  10. 前記自己位置推定部は、
    時系列に供給されるセンサ情報からなる時系列情報を用いて、前記自己位置を推定し、推定結果を時系列情報自己位置として出力する時系列情報自己位置推定部を含み、
    前記表面センシング済み領域記録部は、前記時系列情報自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する
    請求項1に記載の制御装置。
  11. 前記時系列情報は、LIDAR(Light Detection and Ranging、Laser Imaging Detection and Ranging)により検出される3次元点群情報、および車輪エンコーダにより検出される位置情報、姿勢、速度、加速度、および角速度であり、
    前記障害物位置情報生成部は、前記3次元点群情報に基づいて、前記周辺の障害物の相対位置および方向からなる障害物位置情報を生成する
    請求項10に記載の制御装置。
  12. 前記自己位置推定部は、
    現在のセンシング結果を出力するセンサ情報からなる現在情報に基づいて、前記自己位置を推定し、現在情報自己位置として出力する現在情報自己位置推定部をさらに含む
    請求項10に記載の制御装置。
  13. 前記現在情報は、ステレオカメラからなる前記表面センシング部により撮像される視差画像であり、
    前記現在情報自己位置推定部は、前記現在情報である視差画像に基づいて、デプス画像を生成し、前記デプス画像より画像特徴量を抽出し、前記画像特徴量と対応付けられている位置に基づいて、前記自己位置を推定し、現在情報自己位置として出力し、
    前記自己位置推定部は、前記時系列情報自己位置および前記現在情報自己位置を統合して、前記自己位置として出力する
    請求項12に記載の制御装置。
  14. 前記位置と前記画像特徴量とが対応付けて記憶される位置画像特徴量記憶部をさらに含み、
    前記自己位置推定部は、前記時系列情報自己位置および前記現在情報自己位置を統合して、自己位置として出力するとき、前記現在情報自己位置に対応する画像特徴量と、前記自己位置とを対応付けて、前記位置画像特徴量記憶部に記憶させる
    請求項13に記載の制御装置。
  15. 前記自己位置が不定状態の場合、前記表面センシング済み領域記録部は、前記自己位置、前記障害物位置情報、および前記表面センシング部の表面センシング可能範囲に基づいて、前記障害物の表面センシング済み領域を記録する
    請求項1に記載の制御装置。
  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 探索装置及び探索方法及び探索プログラム
US9026302B2 (en) 2009-11-06 2015-05-05 Irobot Corporation Methods and systems for complete coverage of a surface by an autonomous robot
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
DE112018005910T5 (de) 2020-07-30
US11537131B2 (en) 2022-12-27
JPWO2019098082A1 (ja) 2020-11-19
US20200333790A1 (en) 2020-10-22
WO2019098082A1 (ja) 2019-05-23

Similar Documents

Publication Publication Date Title
JP7156305B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
JP7180612B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
JP7151725B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
JP7180670B2 (ja) 制御装置、制御方法、並びにプログラム
WO2019098002A1 (ja) 情報処理装置、情報処理方法、プログラム、及び移動体
WO2020226085A1 (ja) 情報処理装置、情報処理方法、及びプログラム
WO2019082669A1 (ja) 情報処理装置、情報処理方法、プログラム、及び、移動体
US20200278208A1 (en) Information processing apparatus, movable apparatus, information processing method, movable-apparatus control method, and programs
WO2020031812A1 (ja) 情報処理装置、情報処理方法、情報処理プログラム、及び移動体
JP2020079997A (ja) 情報処理装置、情報処理方法、及びプログラム
JPWO2019073795A1 (ja) 情報処理装置、自己位置推定方法、プログラム、及び、移動体
CN112810603B (zh) 定位方法和相关产品
JP7147142B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
WO2021033591A1 (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