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

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

Info

Publication number
JP7147142B2
JP7147142B2 JP2017177753A JP2017177753A JP7147142B2 JP 7147142 B2 JP7147142 B2 JP 7147142B2 JP 2017177753 A JP2017177753 A JP 2017177753A JP 2017177753 A JP2017177753 A JP 2017177753A JP 7147142 B2 JP7147142 B2 JP 7147142B2
Authority
JP
Japan
Prior art keywords
action plan
candidates
unit
self
candidate
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
JP2017177753A
Other languages
English (en)
Other versions
JP2019053561A (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
Priority to JP2017177753A priority Critical patent/JP7147142B2/ja
Priority to US16/645,174 priority patent/US11500386B2/en
Priority to EP18780256.6A priority patent/EP3682306B1/en
Priority to CN201880057871.3A priority patent/CN111052027A/zh
Priority to PCT/JP2018/032699 priority patent/WO2019054230A1/en
Publication of JP2019053561A publication Critical patent/JP2019053561A/ja
Application granted granted Critical
Publication of JP7147142B2 publication Critical patent/JP7147142B2/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 or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/0088—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot characterized by the autonomous decision making process, e.g. artificial intelligence, predefined behaviours
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems
    • G08G1/161—Decentralised systems, e.g. inter-vehicle communication
    • G08G1/163—Decentralised systems, e.g. inter-vehicle communication involving continuous checking
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems
    • G08G1/165—Anti-collision systems for passive traffic, e.g. including static obstacles, trees
    • G—PHYSICS
    • G08—SIGNALLING
    • G08G—TRAFFIC CONTROL SYSTEMS
    • G08G1/00—Traffic control systems for road vehicles
    • G08G1/16—Anti-collision systems
    • G08G1/166—Anti-collision systems for active traffic, e.g. moving vehicles, pedestrians, bikes
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/40—Services specially adapted for particular environments, situations or purposes for vehicles, e.g. vehicle-to-pedestrians [V2P]

Description

本開示は、制御装置、および制御方法、プログラム、並びに移動体に関し、特に、自己位置が不明な状態でも、計算機により行動計画を生成し、自律的な移動を実現できるようにした制御装置、および制御方法、プログラム、並びに移動体に関する。
ロボット等の移動体の自律的な移動を実現するためには、行動を計画する起点となる自己の位置を認識・推定する必要がある。そこで、自己の周囲の状況をセンサ等の手段で認識して、自己の位置を推定し、自律的な移動を計画する技術が提案されている。
例えば、周囲の音声から、音源の直接音または反射音の可能性を複数考慮して、第１の行動と第２の行動を制御する技術が提案されている（特許文献１参照）。
また、複数のロボットの行動の適切さを表す１個の価値関数に基づいて行動を制御する技術が提案されている（特許文献２参照）。
特開２０１６－０４８４６４号公報 特開２０１６－００９３５４号公報
上述した、いずれの技術も複数の行動を計画し、制御する技術であるが、音源推定や複数台のロボットを制御するなど、それぞれ状況が特定されており、自己位置が不明な状態を想定した制御には対応していなかった。
すなわち、電源投入直後、他者や障害物との衝突直後、ユーザによる抱き上げ後、または、周辺環境の誤認等の自己位置が不明な状態では、ランダムに動くか、または、所定の状況を仮定して動作することしかできなかった。
また、近年の移動体の行動計画を生成する構成においては、直近の過去情報を保持し、直近の過去情報を用いた予測等により行動計画が生成されるが、過去情報がない、例えば、直近の自己位置に関する情報が不明瞭であったり、確信が持てなかったり、そもそも存在しない状態（不明な状態）では、行動計画を生成することができなかった。
本開示は、このような状況に鑑みてなされたものであり、特に、自己位置が不明な状態でも、行動計画を生成し、自律的な行動を実現できるようにするものである。
本開示の一側面の制御装置は、複数の自己位置候補から自己位置が一つに定まる場合は一つの前記自己位置を取得し、前記自己位置が一つに定まらない場合に前記複数の自己位置候補を取得する自己位置取得部と、周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を前記複数の自己位置候補毎に生成する行動計画候補生成部と、生成された前記複数の行動計画候補のそれぞれに事前地図情報と周辺環境の関連性に基づいた評価値を付与する行動計画候補評価部と、前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、それぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定部とを含み、前記行動計画決定部は、前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択部と、前記評価値に基づいて、前記複数の行動計画候補を融合する融合部と、前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した前記行動計画候補生成部により生成された行動計画候補を、有効な行動計画候補として判定する有効行動計画候補判定部と、前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択部により、前記評価値に基づいて、複数の有効な行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の有効な行動計画候補を融合することで、前記行動計画を決定するかを判定する乖離度判定部とを含む制御装置である。
前記行動計画候補評価部には、前記周辺の状況に基づいて推定される自己位置候補となる位置の確度に基づいて、生成された前記複数の行動計画候補のそれぞれに評価値を付与させるようにすることができる。
前記周辺の状況に基づいて推定される前記自己位置候補は、カメラ画像からの特徴点と事前知識との合致により推定されるようにすることができ、前記位置の確度は合致した特徴点数とすることができる。
前記選択部には、前記評価値に基づいて、前記複数の行動計画候補のうち、前記評価値が最良値となる行動計画候補を選択させるようにすることができる。
前記融合部には、前記複数の行動計画候補を構成する経路の通過点同士の座標の各要素に、前記評価値による重みを付加した重心位置からなる、新規の通過点を経路とする新たな行動計画候補を生成することで、前記複数の行動計画候補を融合させるようにすることができる。
前記乖離度判定部には、前記複数の行動計画候補がベクトルで表現される場合、重み付きベクトル差を前記乖離度として算出させるようにすることができる。
前記乖離度判定部には、前記複数の行動計画候補を表現するベクトルの大きさを正規化した上で、前記重み付きベクトル差を前記乖離度として算出させるようにすることができる。
前記乖離度判定部には、前記複数の行動計画候補が複数の通過点を経由することで形成される経路である場合、前記通過点間でベクトルを構成させ、構成した前記ベクトルを用いた重み付きベクトル差を前記乖離度として算出させるようにすることができる。
前記乖離度判定部には、前記複数の行動計画候補が複数の通過点を経由することで形成される経路である場合、前記通過点間の距離の差の総和を前記乖離度として算出させるようにすることができる。
前記有効行動計画候補判定部には、前記複数の行動計画候補のうち、前記所定の時間が経過して、かつ、所定の確度を有する行動計画候補を、前記有効な行動計画候補として判定させるようにすることができる。
前記自己位置取得部は、前記周辺の状況に基づいて、取得した前記自己位置候補となる位置毎に、目的地までのルート計画を生成し、前記行動計画候補生成部には、前記自己位置候補となる位置毎に生成される前記ルート計画に基づいて、前記行動計画の候補となる行動計画候補を生成させるようにすることができる。
本開示の一側面の制御方法は、複数の自己位置候補から自己位置が一つに定まる場合に、一つの前記自己位置を取得し、前記自己位置が一つに定まらない場合に、前記複数の自己位置候補を取得する自己位置取得処理と、周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を前記複数の自己位置候補毎に生成する行動計画候補生成処理と、生成された前記複数の行動計画候補のそれぞれに事前地図情報と周辺環境の関連性に基づいた評価値を付与する行動計画候補評価処理と、前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、それぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定処理とを含み、前記行動計画決定処理は、前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択処理と、前記評価値に基づいて、前記複数の行動計画候補を融合する融合処理と、前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した前記行動計画候補生成処理により生成された行動計画候補を、有効な行動計画候補として判定する有効行動計画候補判定処理と、前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択処理により、前記評価値に基づいて、複数の有効な行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の有効な行動計画候補を融合することで、前記行動計画を決定するかを判定する乖離度判定処理とを含む制御方法である。
本開示の一側面のプログラムは、複数の自己位置候補から自己位置が一つに定まる場合は一つの前記自己位置を取得し、前記自己位置が一つに定まらない場合に前記複数の自己位置候補を取得する自己位置取得部と、周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を前記複数の自己位置候補毎に生成する行動計画候補生成部と、生成された前記複数の行動計画候補のそれぞれに事前地図情報と周辺環境の関連性に基づいた評価値を付与する行動計画候補評価部と、前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、それぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定部としてコンピュータを機能させ、前記行動計画決定部は、前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択部と、前記評価値に基づいて、前記複数の行動計画候補を融合する融合部と、前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した前記行動計画候補生成部により生成された行動計画候補を、有効な行動計画候補として判定する有効行動計画候補判定部と、前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択部により、前記評価値に基づいて、複数の有効な行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の有効な行動計画候補を融合することで、前記行動計画を決定するかを判定する乖離度判定部とを含むプログラムである。
本開示の一側面の移動体は、複数の自己位置候補から自己位置が一つに定まる場合は一つの前記自己位置を取得し、前記自己位置が一つに定まらない場合に前記複数の自己位置候補を取得する自己位置取得部と、周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を前記複数の自己位置候補毎に生成する行動計画候補生成部と、生成された前記複数の行動計画候補のそれぞれに事前地図情報と周辺環境の関連性に基づいた評価値を付与する行動計画候補評価部と、前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、それぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定部と、前記行動計画決定部により決定された行動計画に基づいて移動体の動作を制御する制御部とを含み、前記行動計画決定部は、前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択部と、前記評価値に基づいて、前記複数の行動計画候補を融合する融合部と、前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した前記行動計画候補生成部により生成された行動計画候補を、有効な行動計画候補として判定する有効行動計画候補判定部と、前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択部により、前記評価値に基づいて、複数の有効な行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の有効な行動計画候補を融合することで、前記行動計画を決定するかを判定する乖離度判定部とを含む移動体である。
本開示の一側面においては、複数の自己位置候補から自己位置が一つに定まる場合は一つの前記自己位置を取得し、前記自己位置が一つに定まらない場合に前記複数の自己位置候補を取得する自己位置取得部と、周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補が前記複数の自己位置候補毎に生成され、生成された前記複数の行動計画候補のそれぞれに事前地図情報と周辺環境の関連性に基づいた評価値が付与され、前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、それぞれの前記評価値に基づいて、前記複数の行動計画候補が利用されて、前記行動計画が決定され、前記評価値に基づいて、前記複数の行動計画候補のいずれかが選択され、前記評価値に基づいて、前記複数の行動計画候補が融合され、前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した行動計画候補が、有効な行動計画候補として判定され、前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度が算出され、前記乖離度に応じて、前記評価値に基づいて、複数の有効な行動計画候補のいずれかが選択されることで、前記行動計画が決定されるか、または、前記評価値に基づいて、前記複数の有効な行動計画候補が融合されることで、前記行動計画が決定されるかが判定される。
本開示の一側面によれば、特に、自己位置が不明な状態でも、行動計画を生成し、自律的な行動を実現することが可能となる。
本開示の概要を説明する移動体の構成例を示す図である。 図１の自律移動制御部の構成例を説明する図である。 本開示の移動体を制御する移動体制御システムの構成例を説明するブロック図である。 本開示の第１の実施の形態に係る計画部の構成例の詳細なブロック図である。 図３の自律移動制御部による自律移動制御処理を説明するフローチャートである。 図４の計画部を用いた行動計画決定処理を説明するフローチャートである。 本開示の第２の実施の形態に係る計画部の構成例の詳細なブロック図である。 図７の計画部を用いた行動計画決定処理を説明する図である。 図７の計画部を用いた行動計画決定処理を説明するフローチャートである。 本開示の第３の実施の形態に係る計画部の構成例の詳細なブロック図である。 図１０の計画部を用いた行動計画決定処理を説明する図である。 図１０の計画部を用いた行動計画決定処理を説明するフローチャートである。 本開示の第３の実施の形態に係る計画部の構成例の詳細なブロック図である。 図１０の計画部を用いた行動計画決定処理を説明する図である。 図１０の計画部を用いた行動計画決定処理を説明するフローチャートである。 汎用のコンピュータの構成例を説明する図である。
以下に添付図面を参照しながら、本開示の好適な実施の形態について詳細に説明する。なお、本明細書及び図面において、実質的に同一の機能構成を有する構成要素については、同一の符号を付することにより重複説明を省略する。
以下、本技術を実施するための形態について説明する。説明は以下の順序で行う。
１．本開示の概要
２．第１の実施の形態
３．第２の実施の形態
４．第３の実施の形態
５．第４の実施の形態
６．ソフトウェアにより実行させる例
＜＜１．本開示の概要＞＞
本開示の移動体は、周囲の状況を認識し、認識結果に応じて行動計画を生成し、行動計画に基づいて動作することにより、自律移動する移動体である。
図１は、本開示の移動体１１の概要となる構成例を示している。
移動体１１は、例えば、ロボットなどであり、センサ群２１、自律移動制御部２２、およびアクチュエータ群２３を備えている。
センサ群２１は、移動体１１の内部、および移動体１１の周囲の状況の認識に必要な各種の情報を検出するセンサ２１ａ－１乃至２１ａ－ｎを備えており、検出結果を自律移動制御部２２に出力する。また、センサ２１ａ－１乃至２１ａ－ｎについて特に区別する必要がない場合、単に、センサ２１ａと称するものとし、その他の構成についても同様に称する。
より具体的には、センサ２１ａ－１乃至２１ａ－ｎは、例えば、移動体１１の周囲を撮像するカメラ、移動体１１の動きを検出する加速度センサ、移動体１１の周囲に存在する物体までの距離を測定するLIDAR、ToF（Time of Flight）センサ、方向を検出する地磁気センサ、ジャイロセンサ、加速度センサ、周囲の気圧の変化を検出する気圧センサ、接触の有無等を検出する接触センサ、温度を検出する温度センサ、湿度を検出する湿度センサ、PSD（Position Sensitive Detector）測距センサおよび、地球上の位置を検出するGNSS（Global Navigation Satellite System）などを含む。
自律移動制御部２２は、センサ群２１の各種の検出結果より、周囲の状況を認識し、認識結果に基づいて行動計画を生成し、行動計画に応じて、ロボットを駆動させるアクチュエータ群２３の各種アクチュエータ２３ａ－１乃至２３ａ－ｎを動作させる。また、アクチュエータ２３ａ－１乃至２３ａ－ｎについて特に区別する必要がない場合、単に、アクチュエータ２３ａと称するものとし、その他の構成についても同様に称する。
より詳細には、自律移動制御部２２は、認識処理部３１、行動計画処理部３２、および行動制御処理部３３を備えている。
認識処理部３１は、センサ群２１より供給される検出結果に基づいて、認識処理を実行し、例えば、画像、人、物体、表情の種類、位置、属性、および自らや障害物の位置等を認識し、認識結果として行動計画処理部３２に出力する。
行動計画処理部３２は、認識結果に基づいて、移動体１１の全体の行動である、移動体１１の移動に係る機器の移動の軌跡、状態変化、および速度または加速度などの行動計画を生成し、行動制御処理部３３に供給する。
行動制御処理部３３は、行動計画処理部３２より供給される行動計画に基づいて、アクチュエータ群２３のアクチュエータ２３ａ－１乃至２３ａ－ｎのそれぞれの具体的な動きを制御するための制御信号を生成し、アクチュエータ群２３を動作させる。
アクチュエータ群２３は、行動制御処理部３３より供給される制御信号に基づいて、移動体１１を具体的に動作させるアクチュエータ２３ａ－１乃至２３ａ－ｎを動作させる。より詳細には、アクチュエータ２３ａ－１乃至２３ａ－ｎは、移動体１１の具体的な動き実現するモータ、サーボモータ、ブレーキ等の動作を制御信号に基づいて動作させる。
また、アクチュエータ２３ａ－１乃至２３ａ－ｎは、伸縮運動、屈伸運動、または旋回運動などを実現させる構成を含むと共に、情報を表示するLED（Light Emission Diode）やLCD（Liquid Crystal Display）などからなる表示部、および、音声を出力するスピーカなどの構成を含む。したがって、アクチュエータ群２３が制御信号に基づいて、制御されることにより、移動体１１を駆動させる各種の装置の動作が実現されると共に、情報が表示される、および音声が出力される。
すなわち、アクチュエータ群２３のアクチュエータ２３ａ－１乃至２３ａ－ｎが制御されることにより、移動体１１の移動に係る動作が制御されると共に、情報の表示や音声の出力などの各種の情報の提示も制御される。
＜行動計画処理部の構成の概要＞
次に、図２を参照して、行動計画処理部３２の構成の概要について説明する。
行動計画処理部３２は、ルート計画部４１、行動計画部４２、および動作計画部４３を備えている。
ルート計画部４１は、認識処理部３１より供給される認識結果に基づいて、目的地までのルートを計画する。尚、ルートを計画するにあたって、認識結果からは自己位置が一意に求められないことがある。そこで、自己位置が不明である場合、現在の自己位置である可能性のある複数の位置が自己位置の候補として求められる。ルート計画部４１は、現在の自己位置の可能性のある複数の候補のそれぞれの位置から目的地までのルートを計画する。尚、以降においては、自己位置が不明の場合を想定し、自己位置は複数に求められ、目的地に対して計画されるルートも、複数であることを前提として説明を進める。
行動計画部４２は、現在の自己位置の候補となる複数のルートに応じて、行動計画の候補を行動計画候補として生成し、複数の行動計画候補のそれぞれについて評価値を設定し、複数の行動計画候補を評価値に応じて利用して、最終的な行動計画を決定し、動作計画部４３に出力する。
動作計画部４３は、行動計画部４２により計画された行動計画を実現するための移動体１１の動作として、例えば、加速、減速、及び、移動軌道等の計画を行い、計画した移動体１１の動作を示すデータを行動制御処理部３３に供給する。
以上の処理により、自己位置が不明であっても、認識結果に基づいて、現在の自己位置である可能性のある候補のそれぞれについて、必要に応じて複数のルートが計画され、生成された複数のルート毎に計画される行動計画候補が生成され、行動計画候補毎に評価値が設定され、評価値に応じて複数の行動計画候補が利用されて１つの行動計画が決定される。結果として、自己位置が不明であっても、行動計画を決定することが可能となるので、自己位置が不明であっても、自律行動を実現することが可能となる。
＜＜２．第１の実施の形態＞＞
＜本開示の移動体を制御する移動体制御システムの構成例＞
上述した機能を実現させるための移動体１１を制御する移動体制御システムについて説明する。
図３は、本開示の移動体１１を制御する移動体制御システム１００の概略的な機能の構成例を示すブロック図である。尚、図３の移動体制御システム１００は、本技術が適用され得るロボットからなる移動体１１を制御する移動体制御システムの一例であるが、他の移動体、例えば、航空機、船舶、およびマルチローターコプター（ドローン）などを制御するシステムとして適用することもできる。また、ロボットについても、車輪型のロボットや搭乗可能な自動運転車でもよいし、多足歩行型のロボットでもよい。
移動体制御システム１００は、入力部１０１、データ取得部１０２、通信部１０３、移動体内部機器１０４、出力制御部１０５、出力部１０６、駆動系制御部１０７、駆動系システム１０８、記憶部１０９、及び、自律移動制御部１１０を備える。入力部１０１、データ取得部１０２、通信部１０３、出力制御部１０５、駆動系制御部１０７、記憶部１０９、及び、自律移動制御部１１０は、通信ネットワーク１１１を介して、相互に接続されている。通信ネットワーク１１１は、例えば、ＣＡＮ（Controller Area Network）、ＬＩＮ（Local Interconnect Network）、IEEE802.3 等のＬＡＮ（Local Area Network）、又は、ＦｌｅｘＲａｙ（登録商標）等の任意の規格に準拠した通信ネットワークやバス、あるいは規格化されていない独自の通信方式等からなる。なお、移動体制御システム１００の各部は、通信ネットワーク１１１を介さずに、直接接続される場合もある。
なお、以下、移動体制御システム１００の各部が、通信ネットワーク１１１を介して通信を行う場合、通信ネットワーク１１１の記載を省略するものとする。例えば、入力部１０１と自律移動制御部１１０が、通信ネットワーク１１１を介して通信を行う場合、単に入力部１０１と自律移動制御部１１０が通信を行うと記載する。
入力部１０１は、搭乗者が各種のデータや指示等の入力に用いる装置を備える。例えば、入力部１０１は、タッチパネル、ボタン、マイクロフォン、スイッチ、及び、レバー等の操作デバイス、並びに、音声やジェスチャ等により手動操作以外の方法で入力可能な操作デバイス等を備える。また、例えば、入力部１０１は、赤外線若しくはその他の電波を利用したリモートコントロール装置、又は、移動体制御システム１００の操作に対応したモバイル機器若しくはウェアラブル機器等の外部接続機器であってもよい。入力部１０１は、搭乗者により入力されたデータや指示等に基づいて入力信号を生成し、移動体制御システム１００の各部に供給する。
データ取得部１０２は、移動体制御システム１００の処理に用いるデータを取得する各種のセンサ等を備え、取得したデータを、移動体制御システム１００の各部に供給する。
例えば、データ取得部１０２は、移動体の状態等を検出するための各種のセンサを備えることでセンサ群１１２を構成し、図１のセンサ２１ａ－１乃至２１ａ－ｎより構成されるセンサ群２１に対応する。具体的には、例えば、データ取得部１０２は、ジャイロセンサ、加速度センサ、慣性計測装置（ＩＭＵ）、及び、アクセル等の加速入力の操作量、減速入力の操作量、方向指示入力の操作量、エンジンやモータ等の駆動装置の回転数や入出力エネルギー・燃料量、エンジンやモーター等のトルク量、若しくは、車輪や関節の回転速度やトルク等を検出するためのセンサ等を備える。
また、例えば、データ取得部１０２は、移動体の外部の情報を検出するための各種のセンサを備える。具体的には、例えば、データ取得部１０２は、ＴｏＦ（Time Of Flight）カメラ、ステレオカメラ、単眼カメラ、赤外線カメラ、偏光カメラ、及び、その他のカメラ等の撮像装置を備える。また、例えば、データ取得部１０２は、天候又は気象等を検出するための環境センサ、及び、移動体の周囲の物体を検出するための周囲情報検出センサを備える。環境センサは、例えば、雨滴センサ、霧センサ、日照センサ、雪センサ等からなる。周囲情報検出センサは、例えば、レーザ測距センサ、超音波センサ、レーダ、ＬｉＤＡＲ（Light Detection and Ranging、Laser Imaging Detection and Ranging）、ソナー等からなる。
さらに、例えば、データ取得部１０２は、移動体の現在位置を検出するための各種のセンサを備える。具体的には、例えば、データ取得部１０２は、ＧＮＳＳ（Global Navigation Satellite System）衛星からのＧＮＳＳ信号を受信するＧＮＳＳ受信機等を備える。
通信部１０３は、移動体内部機器１０４、並びに、移動体外部の様々な機器、サーバ、基地局等と通信を行い、移動体制御システム１００の各部から供給されるデータを送信したり、受信したデータを移動体制御システム１００の各部に供給したりする。なお、通信部１０３がサポートする通信プロトコルは、特に限定されるものではなく、また、通信部１０３が、複数の種類の通信プロトコルをサポートすることも可能である
例えば、通信部１０３は、無線ＬＡＮ、Ｂｌｕｅｔｏｏｔｈ（登録商標）、ＮＦＣ（Near Field Communication）、又は、ＷＵＳＢ（Wireless USB）等により、移動体内部機器１０４と無線通信を行う。また、例えば、通信部１０３は、図示しない接続端子（及び、必要であればケーブル）を介して、ＵＳＢ（Universal Serial Bus）、ＨＤＭＩ（High-Definition Multimedia Interface）、又は、ＭＨＬ（Mobile High-definition Link）等により、移動体内部機器１０４と有線通信を行う。
さらに、例えば、通信部１０３は、基地局又はアクセスポイントを介して、外部ネットワーク（例えば、インターネット、クラウドネットワーク又は事業者固有のネットワーク）上に存在する機器（例えば、アプリケーションサーバ又は制御サーバ）との通信を行う。また、例えば、通信部１０３は、Ｐ２Ｐ（Peer To Peer）技術を用いて、移動体の近傍に存在する端末（例えば、歩行者若しくは店舗の端末、又は、ＭＴＣ（Machine Type Communication）端末）との通信を行う。さらに、例えば、移動体１１が車両の場合、通信部１０３は、車車間（Vehicle to Vehicle）通信、路車間（Vehicle to Infrastructure）通信、移動体と家との間（Vehicle to Home）の通信、及び、歩車間（Vehicle to Pedestrian）通信等のＶ２Ｘ通信を行う。また、例えば、通信部１０３は、ビーコン受信部を備え、道路上に設置された無線局等から発信される電波あるいは電磁波を受信し、現在位置、渋滞、通行規制又は所要時間等の情報を取得する。
移動体内部機器１０４は、例えば、搭乗者が有するモバイル機器若しくはウェアラブル機器、移動体に搬入され若しくは取り付けられる情報機器、及び、任意の目的地までの経路探索を行うナビゲーション装置等を含む。
出力制御部１０５は、移動体の搭乗者又は移動体外部に対する各種の情報の出力を制御する。例えば、出力制御部１０５は、視覚情報（例えば、画像データ）及び聴覚情報（例えば、音声データ）のうちの少なくとも１つを含む出力信号を生成し、出力部１０６に供給することにより、出力部１０６からの視覚情報及び聴覚情報の出力を制御する。具体的には、例えば、出力制御部１０５は、データ取得部１０２の異なる撮像装置により撮像された画像データを合成して、俯瞰画像又はパノラマ画像等を生成し、生成した画像を含む出力信号を出力部１０６に供給する。また、例えば、出力制御部１０５は、衝突、接触、危険地帯への進入等の危険に対する警告音又は警告メッセージ等を含む音声データを生成し、生成した音声データを含む出力信号を出力部１０６に供給する。
出力部１０６は、移動体の搭乗者又は移動体外部に対して、視覚情報又は聴覚情報を出力することが可能な装置を備える。例えば、出力部１０６は、表示装置、インストルメントパネル、オーディオスピーカ、ヘッドホン、搭乗者が装着する眼鏡型ディスプレイ等のウェアラブルデバイス、プロジェクタ、ランプ等を備える。出力部１０６が備える表示装置は、通常のディスプレイを有する装置以外にも、例えば、ヘッドアップディスプレイ、透過型ディスプレイ、ＡＲ（Augmented Reality）表示機能を有する装置等の運転者の視野内に視覚情報を表示する装置であってもよい。尚、出力制御部１０５および出力部１０６は、自律移動の処理には必須の構成ではないため、必要に応じて省略するようにしてもよい。
駆動系制御部１０７は、各種の制御信号を生成し、駆動系システム１０８に供給することにより、駆動系システム１０８の制御を行う。また、駆動系制御部１０７は、必要に応じて、駆動系システム１０８以外の各部に制御信号を供給し、駆動系システム１０８の制御状態の通知等を行う。
駆動系システム１０８は、移動体の駆動系に関わる各種の装置を備える。例えば、駆動系システム１０８は、4本の脚の各関節に備わった角度やトルクを指定可能なサーボモータ、ロボット自体の移動の動きを4本の足の動きに分解・置換するモーションコントローラ並びに、各モータ内のセンサや足裏面のセンサによるフィードバック制御装置を備える。
別の例では、駆動系システム１０８は、4基ないし6基の機体上向きのプロペラを持つモーター、ロボット自体の移動の動きを各モータの回転量に分解・置換するモーションコントローラを備える。
さらに、別の例では、駆動系システム１０８は、内燃機関又は駆動用モータ等の駆動力を発生させるための駆動力発生装置、駆動力を車輪に伝達するための駆動力伝達機構、舵角を調節するステアリング機構、制動力を発生させる制動装置、ＡＢＳ（Antilock Brake System）、ＥＳＣ（Electronic Stability Control）、並びに、電動パワーステアリング装置等を備える。尚、出力制御部１０５、出力部１０６、駆動系制御部１０７、および駆動系システム１０８は、アクチュエータ群１１３を構成し、図１のアクチュエータ２３ａ－１乃至２３ａ－ｎからなるアクチュエータ群２３に対応する。
記憶部１０９は、例えば、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）、ＨＤＤ（Hard Disc Drive）等の磁気記憶デバイス、半導体記憶デバイス、光記憶デバイス、及び、光磁気記憶デバイス等を備える。記憶部１０９は、移動体制御システム１００の各部が用いる各種プログラムやデータ等を記憶する。例えば、記憶部１０９は、ダイナミックマップ等の３次元の高精度地図、高精度地図より精度が低く、広いエリアをカバーするグローバルマップ、及び、移動体の周囲の情報を含むローカルマップ等の地図データを記憶する。
自律移動制御部１１０は、自動運転又は運転支援等の自律移動に関する制御を行う。具体的には、例えば、自律移動制御部１１０は、移動体の衝突回避あるいは衝撃緩和、移動体間距離に基づく追従移動、移動体速度維持移動、または、移動体の衝突警告の機能実現を目的とした協調制御を行う。また、例えば、自律移動制御部１１０は、操作者・ユーザの操作に拠らずに自律的に移動する自律移動等を目的とした協調制御を行う。自律移動制御部１１０は、検出部１３１、自己位置推定部１３２、状況分析部１３３、計画部１３４、及び、動作制御部１３５を備える。このうち、検出部１３１、自己位置推定部１３２、および状況分析部１３３は、認識処理部１２１を構成し、図１の認識処理部３１に対応する。また、計画部１３４は、行動計画処理部１２２を構成し、図１の行動計画処理部３２に対応する。さらに、動作制御部１３５は、行動制御処理部１２３を構成し、図１の行動制御処理部３３に対応する。
検出部１３１は、自律移動の制御に必要な各種の情報の検出を行う。検出部１３１は、移動体外部情報検出部１４１、移動体内部情報検出部１４２、及び、移動体状態検出部１４３を備える。
移動体外部情報検出部１４１は、移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体の外部の情報の検出処理を行う。例えば、移動体外部情報検出部１４１は、移動体の周囲の物体の検出処理、認識処理、及び、追跡処理、並びに、物体までの距離の検出処理を行う。検出対象となる物体には、例えば、移動体、人、障害物、構造物、道路、信号機、交通標識、道路標示等が含まれる。また、例えば、移動体外部情報検出部１４１は、移動体の周囲の環境の検出処理を行う。検出対象となる周囲の環境には、例えば、天候、気温、湿度、明るさ、及び、路面の状態等が含まれる。移動体外部情報検出部１４１は、検出処理の結果を示すデータを自己位置推定部１３２、状況分析部１３３のマップ解析部１５１、及び、状況認識部１５２、並びに、動作制御部１３５等に供給する。
移動体内部情報検出部１４２は、移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体内部の情報の検出処理を行う。例えば、移動体内部情報検出部１４２は、運転者の認証処理及び認識処理、運転者の状態の検出処理、搭乗者の検出処理、及び、移動体内部の環境の検出処理等を行う。検出対象となる運転者の状態には、例えば、体調、覚醒度、集中度、疲労度、視線方向等が含まれる。検出対象となる移動体内部の環境には、例えば、気温、湿度、明るさ、臭い等が含まれる。移動体内部情報検出部１４２は、検出処理の結果を示すデータを状況分析部１３３の状況認識部１５２、及び、動作制御部１３５等に供給する。
移動体状態検出部１４３は、移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体の状態の検出処理を行う。検出対象となる移動体の状態には、例えば、速度、加速度、舵角、異常の有無及び内容、運転操作の状態、パワーシートの位置及び傾き、ドアロックの状態、並びに、その他の移動体搭載機器の状態等が含まれる。移動体状態検出部１４３は、検出処理の結果を示すデータを状況分析部１３３の状況認識部１５２、及び、動作制御部１３５等に供給する。
自己位置推定部１３２は、移動体外部情報検出部１４１、及び、状況分析部１３３の状況認識部１５２等の移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体の位置及び姿勢等の推定処理を行う。また、自己位置推定部１３２は、必要に応じて、自己位置の推定に用いるローカルマップ（以下、自己位置推定用マップと称する）を生成する。自己位置推定用マップは、例えば、ＳＬＡＭ（Simultaneous Localization and Mapping）等の技術を用いた高精度なマップとされる。自己位置推定部１３２は、推定処理の結果を示すデータを状況分析部１３３のマップ解析部１５１、及び、状況認識部１５２等に供給する。また、自己位置推定部１３２は、自己位置推定用マップを記憶部１０９に記憶させる。尚、ここで推定される自己位置は、自己位置が不明な状態において推定される自己位置も含み、自己位置が不明であることから、複数の位置が自己位置の候補として推定される場合も含む。したがって、自己位置推定部１３２は、候補として推定される複数の自己位置のそれぞれに対して必要に応じてローカルマップを生成する。また、図３において、自己位置推定部１３２以降に引き出される矢印が複数本数であるのは、候補となる複数の自己位置が出力されることを表現するためである。
状況分析部１３３は、移動体及び周囲の状況の分析処理を行う。状況分析部１３３は、マップ解析部１５１、状況認識部１５２、及び、状況予測部１５３を備える。
マップ解析部１５１は、自己位置推定部１３２及び移動体外部情報検出部１４１等の移動体制御システム１００の各部からのデータ又は信号を必要に応じて用いながら、記憶部１０９に記憶されている各種のマップの解析処理を行い、自律移動の処理に必要な情報を含むマップを構築する。マップ解析部１５１は、構築したマップを、状況認識部１５２、状況予測部１５３、並びに、計画部１３４のルート計画部１６１、行動計画部１６２、及び、動作計画部１６３等に供給する。マップ解析部１５１は、複数の自己位置が候補として推定される場合、そのそれぞれについて、自律移動の処理に必要な情報を含むマップを構築する。
状況認識部１５２は、自己位置推定部１３２、移動体外部情報検出部１４１、移動体内部情報検出部１４２、移動体状態検出部１４３、及び、マップ解析部１５１等の移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体に関する状況の認識処理を行う。例えば、状況認識部１５２は、移動体の状況、移動体の周囲の状況、及び、移動体の運転者の状況等の認識処理を行う。また、状況認識部１５２は、必要に応じて、移動体の周囲の状況の認識に用いるローカルマップ（以下、状況認識用マップと称する）を生成する。状況認識用マップは、例えば、占有格子地図（Occupancy Grid Map）、道路地図（Lane Map）、または、点群地図（Point Cloud Map）とされる。状況認識部１５２は、複数の自己位置が候補として推定される場合、そのそれぞれについて、移動体に関する状況の認識処理を行う。
認識対象となる移動体の状況には、例えば、移動体の位置、姿勢、動き（例えば、速度、加速度、移動方向等）、並びに、異常の有無及び内容等が含まれる。認識対象となる移動体の周囲の状況には、例えば、周囲の静止物体の種類及び位置、周囲の動物体の種類、位置及び動き（例えば、速度、加速度、移動方向等）、周囲の道路の構成及び路面の状態、並びに、周囲の天候、気温、湿度、及び、明るさ等が含まれる。認識対象となる運転者の状態には、例えば、体調、覚醒度、集中度、疲労度、視線の動き、並びに、運転操作等が含まれる。
状況認識部１５２は、認識処理の結果を示すデータ（必要に応じて、状況認識用マップを含む）を自己位置推定部１３２及び状況予測部１５３等に供給する。また、状況認識部１５２は、状況認識用マップを記憶部１０９に記憶させる。
状況予測部１５３は、マップ解析部１５１、及び状況認識部１５２等の移動体制御システム１００の各部からのデータ又は信号に基づいて、移動体に関する状況の予測処理を行う。例えば、状況予測部１５３は、移動体の状況、移動体の周囲の状況、及び、運転者の状況等の予測処理を行う。状況予測部１５３は、複数の自己位置が候補として推定される場合、そのそれぞれについて、移動体に関する状況の予測処理を行う。
予測対象となる移動体の状況には、例えば、移動体の挙動、異常の発生、及び、移動可能距離等が含まれる。予測対象となる移動体の周囲の状況には、例えば、移動体の周囲の動物体の挙動、信号の状態の変化、及び、天候等の環境の変化等が含まれる。予測対象となる運転者の状況には、例えば、運転者の挙動及び体調等が含まれる。
状況予測部１５３は、予測処理の結果を示すデータを、及び状況認識部１５２からのデータとともに、計画部１３４のルート計画部１６１、行動計画部１６２、及び、動作計画部１６３等に供給する。
ルート計画部１６１は、マップ解析部１５１及び状況予測部１５３等の移動体制御システム１００の各部からのデータ又は信号に基づいて、目的地までのルートを計画する。例えば、ルート計画部１６１は、グローバルマップに基づいて、現在位置から指定された目的地までのルートを設定する。また、例えば、ルート計画部１６１は、渋滞、事故、通行規制、工事等の状況、及び、運転者の体調等に基づいて、適宜ルートを変更する。ルート計画部１６１は、計画したルートを示すデータを行動計画部１６２等に供給する。ルート計画部１６１は、複数の自己位置が推定される場合、そのそれぞれについて、目的地までのルートを計画する。
行動計画部１６２は、マップ解析部１５１及び状況予測部１５３等の移動体制御システム１００の各部からのデータ又は信号に基づいて、ルート計画部１６１により計画されたルートを計画された時間内で安全に移動するための移動体の行動を計画する。例えば、行動計画部１６２は、発進、停止、進行方向（例えば、前進、後退、左折、右折、方向転換等）、移動速度、及び、追い越し等の計画を行う。行動計画部１６２は、計画した移動体の行動を示すデータを動作計画部１６３等に供給する。行動計画部１６２は、複数の自己位置が推定される場合、それぞれの自己位置に対応する行動計画を行動計画候補として計画すると共に、それぞれに評価値を設定し、評価値に基づいて、複数の行動計画候補を用いて行動計画を決定する。
動作計画部１６３は、マップ解析部１５１及び状況予測部１５３等の移動体制御システム１００の各部からのデータ又は信号に基づいて、行動計画部１６２により計画された行動を実現するための移動体の動作を計画する。例えば、動作計画部１６３は、加速、減速、及び、移動軌道等の計画を行う。動作計画部１６３は、計画した移動体の動作を示すデータを、動作制御部１３５等に供給する。
動作制御部１３５は、移動体の動作の制御を行う。
より詳細には、動作制御部１３５は、移動体外部情報検出部１４１、移動体内部情報検出部１４２、及び、移動体状態検出部１４３の検出結果に基づいて、衝突、接触、危険地帯への進入、運転者の異常、移動体の異常等の緊急事態の検出処理を行う。動作制御部１３５は、緊急事態の発生を検出した場合、急停止や急旋回等の緊急事態を回避するための移動体の動作を計画する。
また、動作制御部１３５は、動作計画部１６３により計画された移動体の動作を実現するための加減速制御を行う。例えば、動作制御部１３５は、計画された加速、減速、又は、急停止を実現するための駆動力発生装置又は制動装置の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部１０７に供給する。
動作制御部１３５は、動作計画部１６３により計画された移動体の動作を実現するための方向制御を行う。例えば、動作制御部１３５は、動作計画部１６３により計画された移動軌道又は急旋回を実現するためのステアリング機構の制御目標値を演算し、演算した制御目標値を示す制御指令を駆動系制御部１０７に供給する。
＜行動計画処理部の構成例＞
次に、図４を参照して、図３の移動体制御システム１００のうち、行動計画処理部３２に対応する行動計画処理部１２２を実現する計画部１３４の具体的な構成例について説明する。
自己位置が不明などの場合、周囲の認識結果だけでは、自己位置を一意に特定できないことがあり、このようなとき、認識結果から可能性の高い複数の自己位置が候補として特定されることがある。
例えば、ビルの内部を移動する場合、各フロアの構造は類似していることが多く、例えば、周囲の状況を撮像した画像だけでは、何階のフロアであるかを特定することができない場合がある。このような場合については、候補となり得るフロアの全てについて、自己位置の候補とする必要がある。
そこで、ルート計画部１６１は、自己位置の候補となるそれぞれに対して、目的地までのルートを計画し、計画したルートを示すデータを行動計画部１６２に供給する。
行動計画部１６２は、複数の行動計画候補生成部２０１－１乃至２０１－ｎおよび行動計画候補評価部２０２－１乃至２０２－ｎを備えている。尚、行動計画候補生成部２０１－１乃至２０１－ｎ、および行動計画候補評価部２０２－１乃至２０２－ｎについて、特に区別する必要がない場合、単に、行動計画候補生成部２０１、および行動計画候補評価部２０２と称するものとし、その他の構成についても同様に称する。
行動計画候補生成部２０１－１乃至２０１－ｎは、それぞれルート計画部１６１により計画された複数のルートのそれぞれについて、計画された時間内で安全に移動するための移動体の行動計画の候補を行動計画候補として生成する。より具体的には、行動計画候補生成部２０１は、例えば、環境を格子状に区切って、到達判定および経路の重みを最適化して最良のパスを生成するA*algorithm（A star探索アルゴリズム）、道路中心線に従って経路を設定するLane algorithm、および、自己位置からインクリメンタルに到達可能な場所へのパスを適切に枝刈りしながら伸ばしていくRRT(Rapidly-exploring Random Tree) algorithmなどにより行動計画候補を生成する。尚、図４においては、ルート計画部１６１より２本の矢印から行動計画候補生成部２０１－１，２０１－ｎに対してルートを示すデータが供給されるように描かれているが、実際には、ｎ個のルートが計画された場合、ｎ個のルートのデータが、それぞれ行動計画候補生成部２０１－１乃至２０１－ｎに供給される。
行動計画候補評価部２０２－１乃至２０２－ｎは、それぞれ行動計画候補生成部２０１－１乃至２０１－ｎにより生成された行動計画候補に評価値を設定し、行動計画決定部２０３に出力する。
より具体的には、行動計画候補評価部２０２は、例えば、行動計画候補生成部２０１により行動計画候補が生成される際に使用された、自己位置推定部１３２により候補として推定された自己位置の確度に基づいて、評価値を設定する。ここで、自己位置の確度とは、例えば、GNSSなどの場合、自己位置を特定する際に使用された衛星数や、周囲の画像を用いたマッチングにより、マッチングできた物体数などであり、この例においては、衛星数が多いほど、また、マッチングした物体数が多いほど確度が高い。また、行動計画候補評価部２０２は、行動計画候補生成部２０１により行動計画候補が計画される際に使用された認識結果に基づいて、事前地図情報と現在周辺環境のマッチング度合いにより評価値を設定する。
尚、設定する評価値については、さらに多くの複数の評価値を組み合わせるようにしてもよい。また、図３においては、例えば、ｎ個の複数の行動計画候補のそれぞれに対して、それぞれ行動計画候補生成部２０１－１乃至２０１－ｎ、および行動計画候補評価部２０２－１乃至２０２－ｎが設けられる例が示されているが、１個の行動計画候補生成部２０１と、１個の行動計画候補評価部２０２とで、ｎ個の行動計画候補について、例えば、ｎ回の処理により実現するようにしてもよい。
行動計画決定部２０３は、行動計画候補生成部２０１－１乃至２０１－ｎより供給される複数の行動計画候補と、それぞれの評価値に基づいて、行動計画を決定する。より詳細には、行動計画決定部２０３は、選択部２１１を備えており、複数の行動計画候補のうち、評価値が最高値となる行動計画候補、または、評価値が最低値となる行動計画候補を行動計画として決定する。
＜自律移動制御処理＞
次に、図５のフローチャートを参照して、図３の自律移動制御部１１０による自律移動制御処理について説明する。
ステップＳ１１において、データ取得部１０２は、移動体の状態等を検出するための各種のセンサにより検出された検出結果となるデータを取得し、検出部１３１に出力する。
ステップＳ１２において、検出部１３１は、データ取得部１０２により取得されたデータに基づいて、自律移動の制御に必要とされる各種の情報を検出し、自己位置推定部１３２、状況分析部１３３、および動作制御部１３５に出力する。
ステップＳ１３において、自己位置推定部１３２は、検出結果に基づいて、自己位置を推定し、推定結果を状況分析部１３３に出力する。この際、自己位置推定部１３２は、検出結果に基づいて、一意に自己位置が推定できない場合、自己位置として推定可能な複数の候補を推定し、推定結果として状況分析部１３３に出力する。この推定結果には、自己位置の候補の確度の情報も含まれる。
より具体的には、自己位置推定部１３２は、例えば、予め移動する可能性のある空間に存在する物体の位置に対応付けて、各物体の特徴点を事前知識として記憶しておくようにし、自律的に移動する際に、データ取得部１０２の、例えば、カメラにより周囲の画像を撮像し、撮像した画像から特徴点を抽出し、事前知識として記憶されている物体の特徴点との比較により、合致する特徴点数が所定数より多い物体の位置の情報から、一意に自己位置を推定するようにしてもよい。
また、自己位置推定部１３２は、周囲の画像から抽出される特徴点と、事前知識として記憶している特徴点との比較により、合致する特徴点数が所定数より少ない物体しか周囲に存在しない場合、一意に自己位置を推定できないので、合致する特徴点数が所定数より少ない物体の位置から自己位置の候補を推定するようにしてもよい。このとき、例えば、自己位置の候補として推定される位置の周囲に存在する物体の特徴点と、周囲の画像から抽出される特徴点との比較において合致する特徴点数が、推定される自己位置の候補における確度となるようにしてもよい。
ステップＳ１４において、状況分析部１３３は、自己位置の候補毎に周囲の状況を分析し、分析結果を自己位置の候補毎に自己位置推定部１３２、および計画部１３４に出力する。
ステップＳ１５において、計画部１３４のルート計画部１６１は、自己位置の候補毎の分析結果に基づいて、目的地までのルートを計画し、行動計画部１６２に出力する。
ステップＳ１６において、行動計画部１６２は、行動計画決定処理を実行し、複数の自己位置の候補毎に計画されたルートに基づいて、複数の行動計画候補を生成し、それぞれに評価値を設定して、評価値に基づいて、行動計画を決定し、動作計画部１６３に出力する。尚、行動計画決定処理は、図６を参照して、詳細を後述する。
ステップＳ１７において、動作計画部１６３は、行動計画部１６２より供給される行動計画に従って、出力制御部１０５により出力部１０６を制御するための動作計画を生成し、駆動系制御部１０７により駆動系システム１０８を制御するための動作計画を生成する。
ステップＳ１８において、出力制御部１０５は、動作計画部１６３より供給される動作計画に基づいて、出力部１０６を制御する。また、駆動系制御部１０７は、動作計画部１６３より供給される動作計画に基づいて、駆動系システム１０８を制御する。
ステップＳ１９において、自律移動制御部１１０は、終了が指示されたか否かを判定し、終了が指示されていない場合、処理は、ステップＳ２０に進む。
ステップＳ２０において、自律移動制御部１１０は、所定時間が経過したか否かを判定し、所定時間が経過するまで、同様の処理を繰り返す。そして、ステップＳ２０において、所定時間が経過した場合、処理は、ステップＳ１１に戻る。すなわち、所定の時間間隔で、ステップＳ１１乃至Ｓ２０の処理が繰り返される。
すなわち、自己位置の候補が複数に求められる場合、すなわち、自己位置が不明である場合、候補として求められた複数の自己位置に応じて行動計画候補が生成され、行動計画候補毎に評価値が求められて、複数に求められた行動計画候補と、その評価値に基づいて行動計画が決定され、行動計画に則った移動を繰り返すことが可能となる。
結果として、自己位置が不明な状態であっても、上述した移動が繰り返されるうちに、自己の周囲の情報が徐々に集められて、自己位置の検出精度が向上されることになり、最終的には、一意に特定された自己位置の情報に則った行動計画を生成し、自律した移動を実現することが可能となる。
＜図４の計画部を用いた行動計画決定処理＞
次に、図６のフローチャートを参照して、図４の計画部１３４による行動計画決定処理について説明する。
ステップＳ３１において、行動計画候補生成部２０１－１乃至２０１－ｎは、それぞれ候補となる自己位置に応じて計画されたルートに基づいて、行動計画候補を生成し、それぞれ生成した行動計画候補を行動計画候補評価部２０２－１乃至２０２－ｎに出力する。
例えば、第１の自己位置（X=3，Y=2，確度80）に基づいて第１のルートが計画され、第２の自己位置（X=105，Y=100，確度20）に基づいて第２のルートが計画されている場合、以下のような行動計画候補が生成される。尚、自己位置のX,Yは、地図上の座標を表したものであり、確度は、自己位置の確度を0乃至100の値で表したものである。
すなわち、行動計画候補生成部２０１－１は、例えば、第１の自己位置（X=3，Y=2，確度80）に基づいて計画された第１のルートに基づいて、現在位置と次の点を結ぶ経路に相当する、例えば、第１の目標速度ベクトルを第１の行動計画候補として算出する。
また、行動計画候補生成部２０１－２は、例えば、第１の自己位置（X=105，Y=100，確度20）に基づいて計画された第２のルートに基づいて、現在位置と次の点を結ぶ経路に相当する、例えば、第２の目標速度ベクトルを第１の行動計画候補として算出する。
ステップＳ３２において、行動計画候補評価部２０２－１乃至２０２－ｎは、それぞれ行動計画候補について評価値を算出して設定する。
より詳細には、行動計画候補評価部２０２－１は、例えば、第１の目標速度ベクトルからなる第１の行動計画候補を生成するにあたって使用した第１のルートの計画に使用された、第１の自己位置（X=3，Y=2，確度80）における確度の80を、第１の行動計画候補の評価値として設定する。また、行動計画候補評価部２０２－２は、例えば、第２の目標速度ベクトルからなる第２の行動計画候補を生成するにあたって使用した第２のルートの計画に使用された、第２の自己位置（X=105，Y=100，確度20）における確度の20を評価値として設定する。
ステップＳ３３において、行動計画決定部２０３は、選択部２１１を制御して、評価値に基づいて、行動計画候補のうち、評価値が最高値となる行動計画候補を行動計画に決定する。すなわち、今の場合、第１の自己位置に基づいた行動計画候補である第１の目標速度ベクトルの評価値が80であり、第２の自己位置に基づいた行動計画候補である第２の目標速度ベクトルの評価値である20よりも大きく、行動計画候補のうち、評価値が最高値であるので、行動計画決定部２０３は、第１の目標速度ベクトルからなる第１の行動計画候補を行動計画として決定する。
尚、今の例では、評価値は、自己位置の確度に応じて設定されたため、評価値が最高値の行動計画候補が選択されることになるが、評価値が、例えば、確度の逆数により設定されるような場合、評価値が最小値となる行動計画候補が行動計画に決定されることになる。また、評価値は、自己位置の確度以外により設定される値であってもよい。
以上の処理により、自己位置が不明な状態で、自己位置が一意に特定できない状態であっても、複数の自己位置の候補のそれぞれからルートが計画され、計画されたそれぞれのルートに対して行動計画候補が生成され、生成された行動計画候補のそれぞれについて評価値が設定されて、評価値が最高値（または、評価値が最低値）の行動計画候補が行動計画として設定されることになる。結果として、確度の高い（信頼性の高い）行動計画候補が選択されるので、行動計画を適切に生成することが可能となる。
尚、以上においては、２カ所の自己位置の候補が求められ、それぞれの候補となる自己位置から目的地までのルートが計画されて、計画されたルートのそれぞれに行動計画候補が求められて、評価値が最高値の行動計画候補が行動計画として決定される例について説明してきたが、自己位置の候補は２個以上であってもよく、対応して、行動計画候補が２個以上であってもよい。この場合においても、評価値が最高値（または、評価値が最低値）の行動計画候補が選択されることで行動計画が決定される。
また、所定の時間間隔で、繰り返し自己位置が検出され、行動計画候補が繰り返し評価されて、行動計画が繰り返し生成されることにより、行動計画候補は、徐々に多くのデータを利用して求められるようになることから、自己位置の検出精度は向上することになる。すなわち、自己位置の検出精度が向上することにより、徐々に自己位置は一意に求められるようになり、最終的には、行動計画も一意に決められるようになる。
＜＜３．第２の実施の形態＞＞
以上においては、自己位置が不明であり、複数の自己位置の候補が検出される場合に、複数の自己位置の候補毎にルートが設定され、設定されたそれぞれのルートに対して行動計画候補が生成され、生成された行動計画候補のそれぞれに評価値が設定されて、評価値が最高値（または、最低値）の行動計画候補が行動計画として設定される例について説明してきた。しかしながら、複数の行動計画候補のいずれかが選択されるように行動計画が決定されると、複数の行動計画候補が全く異なったものであった場合、選択を誤ったときのダメージが大きくなる可能性がある。そこで、複数の行動計画候補がある場合、それらを評価値に基づいて融合して行動計画が決定されるようにしてもよい。
図７は、複数の行動計画候補を評価値に基づいて融合して行動計画が決定されるようにした計画部１３４の構成例が示されている。尚、図７の計画部１３４の各構成において、図４の計画部１３４の構成と同一の機能を備えた構成については、同一の符号を付しており、その説明は適宜省略する。
すなわち、図７の計画部１３４において、図４の計画部１３４と異なる点は、行動計画決定部２０３において、選択部２１１に代えて、融合部２２１が設けられている点である。
例えば、図８の左部で示されるように、現在位置Ｐ０に対して、通過点Ｐ１，Ｐ２，Ｐ３で移動する経路Ｃ１が第１の行動計画候補として求められ、通過点Ｐ１１，Ｐ１２，Ｐ１３で移動する経路Ｃ２が第２の行動計画候補として求められる場合を考える。
ここで、第１の行動計画候補である経路Ｃ１の評価値が20であり、第２の行動計画候補である経路Ｃ２の評価値が80であるとき、図８の右部で示されるように、融合部２２１は、通過点Ｐ１，Ｐ１１との間の20:80（＝1/8：1/2）の中間位置に新規の通過点Ｐ２１を設定し、通過点Ｐ２，Ｐ１２との間の20:80（＝1/8：1/2）の中間位置に新規の通過点Ｐ２２を設定し、通過点Ｐ３，Ｐ１３との間の20:80（＝1/8：1/2）の中間位置に新規の通過点Ｐ２３を設定し、通過点Ｐ０，Ｐ２１，Ｐ２２，Ｐ２３からなる新規の経路Ｃ１１を設定することで、第１の行動計画候補となる経路Ｃ１と、第２の行動計画候補となる経路Ｃ２とを融合した新規の経路Ｃ１１を、行動計画として決定する。
すなわち、図８の例においては、２つの行動計画候補について、それぞれの評価値に応じた重みを付して中間位置を設定することで、評価値に応じて２つの行動計画候補を融合して、１つの行動計画を決定している。
このため、複数の行動計画候補の評価値を反映させた行動計画を実現することが可能となる。
＜図７の計画部を用いた行動計画決定処理＞
次に、図９のフローチャートを参照して、図７の計画部１３４による行動計画決定処理について説明する。
ステップＳ５１において、行動計画候補生成部２０１－１乃至２０１－ｎは、それぞれ自己位置に応じて計画されたルートに基づいて、行動計画候補を生成し、それぞれ生成した行動計画候補を行動計画候補評価部２０２－１乃至２０２－ｎに出力する。
ステップＳ５２において、行動計画候補評価部２０２－１乃至２０２－ｎは、それぞれ行動計画候補のそれぞれについて評価値を算出し設定する。
ステップＳ５３において、行動計画決定部２０３は、融合部２２１を制御して、評価値に基づいて、例えば、図８を参照して説明したように、複数の行動計画候補を融合することにより行動計画を決定する。
以上の処理により、自己位置が不明な状態で、自己位置が一意に特定できない状態であっても、複数の自己位置の候補のそれぞれからルートが計画され、計画されたそれぞれのルートに対して行動計画候補が生成され、生成された行動計画候補のそれぞれについて評価値が設定されて、評価値に応じて行動計画候補が融合されて、行動計画として設定されることになる。
このため、評価値が高い行動計画候補と、評価値が低い行動計画候補とが評価値に応じて融合されるので、いずれか１つの行動計画候補が誤って選択されることにより生じるリスクを低減させつつ、所定の確度の行動計画を適切に設定することが可能となる。
尚、以上においては、２カ所の自己位置の候補が求められ、それぞれの候補となる自己位置から目的地までのルートが計画されて、計画されたルートのそれぞれに行動計画候補が求められて、評価値に応じて行動計画候補が融合されて行動計画として決定される例について説明してきたが、自己位置の候補は２個以上であってもよく、対応して、行動計画候補が２個以上であっても、評価値に応じて融合されるようにしてもよい。例えば、３点以上の場合は、各要素の座標値に、評価値からなる重みを付して、各要素の重心位置を求めるようにしてもよい。
＜＜４．第３の実施の形態＞＞
以上においては、複数の行動計画候補を評価値に基づいて融合して行動計画を決定する例について説明してきたが、行動計画候補のそれぞれが完全に乖離した経路などである場合、融合すると、全く異なる経路となってしまう恐れがある。
そこで、複数の行動計画候補が生成される場合、複数の行動計画候補間の乖離度を算出して、乖離度が所定値より小さく、複数の行動計画候補がある程度類似しているときには、複数の行動計画候補を評価値に基づいて融合することで行動計画を決定し、乖離度が所定値よりも大きく、複数の行動計画候補がそれぞれ大きく異なるときには、複数の行動計画候補のいずれかを評価値により選択することで行動計画を決定するようにしてもよい。
尚、以降において、図６のフローチャートを参照して説明した、複数の行動計画候補のいずれかを評価値により選択することで行動計画を決定する行動計画決定処理を、選択型行動計画決定処理と称する。また、図９のフローチャートを参照して説明した、複数の行動計画候補を評価値に基づいて融合することで行動計画を決定する行動計画決定処理を融合型行動計画決定処理と称する。
図１０は、複数の行動計画候補が計画される場合に、複数の行動計画候補の乖離度を算出して、選択型行動計画決定処理か、または、融合型行動計画決定処理かを判定し、行動計画決定処理を実現するようにした計画部１３４の構成例である。尚、図４，図７の計画部１３４と同一の機能を備えた構成については、同一の符号を付しており、その説明は適宜省略する。
すなわち、図１０の計画部１３４において、図４，図７の計画部１３４と異なる点は、選択部２１１および融合部２２１がいずれも設けられており、さらに、乖離度判定部２３１が設けられている点である。
乖離度判定部２３１は、複数の行動計画候補が求められている場合、複数の行動計画候補間の乖離度を算出し、所定値との比較により、所定値よりも大きく乖離が大きいと判定した場合、選択型行動計画決定処理を選択し、所定値との比較により、所定値よりも小さく乖離が小さいと判定した場合、融合型行動計画決定処理を選択する。
より具体的には、乖離度判定部２３１は、複数の行動計画候補が、例えば、図１１の左部で示されるようなベクトルＶ３１，Ｖ３２である場合、重み付きベクトルの差をもって、乖離度を判定するようにしてもよい。
すなわち、重み付きベクトルの差とは、例えば、ベクトルＶ３１，Ｖ３２がそれぞれ（ｘ１，ｙ１，ｚ１）および（ｘ２，ｙ２，ｚ２）で表されるときには、以下の式（１）により定義される。
Ｅｓｔ＝ａ×（ｘ１－ｘ２）2
＋ｂ×（ｙ１－ｙ２）2
＋ｃ×（ｚ１－ｚ２）2
・・・（１）
ここで、Ｅｓｔは、乖離度である。また、ａ，ｂ，ｃは、それぞれｘ，ｙ，ｚ軸方向に対する重みであり、例えば、ａ＝ｂ＝ｃ＝１でもよい。
また、式（１）により乖離度Ｅｓｔが設定される場合、同じ方向で長さが違う場合でも乖離度が大きくなる可能性があるが、例えば、以下の式（２）で示されるように、ベクトル毎に正規化するようにして、同じ方向である場合には乖離度を小さくしてもよい。
Ｅｓｔ＝ａ×（ｘ１／Ｌ１－ｘ２／Ｌ２）2
＋ｂ×（ｙ１／Ｌ１－ｙ２／Ｌ２）2
＋ｃ×（ｚ１／Ｌ１－ｚ２／Ｌ２）2
・・・（２）
ここで、Ｌ１は、√（ｘ１2＋ｙ１2＋ｚ１2）であり、Ｌ２は、√（ｘ２2＋ｙ２2＋ｚ２2）である。
さらに、図１１の右部における上段で示されるように、行動計画候補が経路Ｃ１，Ｃ２で表現される場合、以下の式（３）で示されるように、経路Ｃ１を構成する通過点Ｐ１，Ｐ２，Ｐ３と、経路Ｃ２を構成する通過点Ｐ１１，Ｐ１２，Ｐ１３とのそれぞれの通過点間の差Ｌ１１（＝Ｐ１－Ｐ１１），Ｌ１２（＝Ｐ２－Ｐ１２），Ｌ１３（＝Ｐ３－Ｐ１３）の和を乖離度Ｅｓｔとしてもよい。
Ｅｓｔ＝Ｌ１１＋Ｌ１２＋Ｌ１３
・・・（３）
また、図１１の右部の中段で示されるように、経路Ｃ１，Ｃ２のそれぞれにおける通過点間をベクトルとして表現し、図１１の右部の下段で示されるように、それぞれのベクトル間の乖離度を加算するようにしてもよい。
すなわち、図１１の右部の中段においては、経路Ｃ１については、通過点Ｐ０乃至Ｐ１によりベクトルＶ１が定義され、通過点Ｐ１乃至Ｐ２によりベクトルＶ２が定義され，通過点Ｐ２乃至Ｐ３によりベクトルＶ３が定義されている。また、経路Ｃ２については、通過点Ｐ０乃至Ｐ１１によりベクトルＶ１１が定義され、通過点Ｐ１１乃至Ｐ１２によりベクトルＶ１２が定義され，通過点Ｐ１２乃至Ｐ１３によりベクトルＶ１３が定義されている。
乖離度判定部２３１は、図１１の右部の下段で示されるように、ベクトルＶ１，Ｖ１１間の乖離度Ｅｓｔ１、ベクトルＶ２，Ｖ１２間の乖離度Ｅｓｔ２、およびベクトルＶ３，Ｖ１３間の乖離度Ｅｓｔ３をそれぞれ求めて、以下の式（４）で示されるように、それぞれを加算して乖離度Ｅｓｔとして求め、所定値との比較により乖離度を判定するようにしてもよい。
Ｅｓｔ＝Ｅｓｔ１＋Ｅｓｔ２＋Ｅｓｔ３
・・・（４）
このように定義された行動計画候補間の乖離度に基づいて、複数の行動計画候補を用いて、選択型行動計画決定処理を採用するか、または、融合型行動計画決定処理を採用するかが判定される。
＜図１０の計画部による行動計画決定処理＞
次に、図１２のフローチャートを参照して、図１０の計画部１３４による行動計画決定処理について説明する。
ステップＳ７１において、行動計画候補生成部２０１－１乃至２０１－ｎは、それぞれ自己位置に応じて計画されたルートに基づいて、行動計画候補を計画し、それぞれ計画した行動計画候補を行動計画候補評価部２０２－１乃至２０２－ｎに出力する。
ステップＳ７２において、行動計画候補評価部２０２－１乃至２０２－ｎは、それぞれ行動計画候補のそれぞれについて評価値を設定し、行動計画決定部２０３に出力する。
ステップＳ７３において、行動計画決定部２０３は、乖離度判定部２３１を制御して、複数の行動計画候補間の乖離度を算出させる。例えば、図１１を参照して説明したような手法により、乖離度判定部２３１は、乖離度を算出する。
ステップＳ７４において、乖離度判定部２３１は、乖離度が所定値よりも大きく、行動計画候補間には大きな乖離があるか否かを判定する。ステップＳ７４において、乖離度が所定値よりも大きく、乖離が大きいと判定された場合、処理は、ステップＳ７５に進む。
ステップＳ７５において、行動計画決定部２０３は、複数の行動計画候補の乖離が大きいので、選択型行動計画決定処理を実行し、複数の行動計画候補のそれぞれの評価値に基づいて、評価値が最高値（または、評価値が最低値）の行動計画候補を選択し、行動計画として決定する。
一方、ステップＳ７４において、乖離度が所定値よりも小さく、乖離が小さいと判定された場合、処理は、ステップＳ７６に進む。
ステップＳ７６において、行動計画決定部２０３は、複数の行動計画候補の乖離が小さいので、融合型行動計画決定処理を実行し、複数の行動計画候補のそれぞれの評価値に基づいて、複数の行動計画候補を融合し、行動計画として決定する。
以上の処理により、複数の行動計画候補について、乖離度が小さいときは、相互の類似性が高く、複数の行動計画候補を評価値に応じて融合して行動計画を決定することで、多くの行動計画候補に基づいた高精度な行動計画を生成することが可能となる。また、複数の行動計画候補について、乖離度が大きいときは、相互の類似性が低いので、複数の行動計画候補のうち、評価値が最高値（または、最低値）の行動計画候補を行動計画として決定することで、確度の高い行動計画候補を行動計画にすることが可能となる。
＜＜５．第４の実施の形態＞＞
以上においては、複数の行動計画候補が計画されるとき、乖離度に応じて、選択型行動計画決定処理、または、融合型行動計画決定処理がなされる例について説明してきたが、例えば、自己位置の候補数が増えるような場合、新たに行動計画候補を生成する行動計画候補生成部２０１が起動することになる。このとき、それまでに、行動計画候補を生成し続けていた行動計画候補生成部２０１－ｘと、新たに、行動計画候補を計画し始める行動計画候補生成部２０１－ｙとが存在することになる。
ところで、図３でも示されるように、自己位置推定部１３２は、状況分析部１３３から状況分析結果がフィードバックされることにより、推定される自己位置の精度は、時間の経過に従って向上する。したがって、それまでに候補とされていた自己位置に対応付けて生成される行動計画候補と、新たな自己位置の候補とされた情報に基づいた行動計画候補とでは、精度に差が生じており、当然のことながら、新たな自己位置の候補とされた情報に基づいて、新たに生成された行動計画候補の精度は、それまでに行動計画候補とされていた自己位置に対応付けて生成される行動計画候補よりも低い。
そこで、新たに候補とされた自己位置に対応付けて求められる行動計画候補については、所定時間だけフィードバックが繰り返されるまで、無効の状態とし、所定時間が経過した後から有効な行動計画の候補として利用するようにしてもよい。
図１３は、新たに候補とされた自己位置に対して、新たに計画される行動計画候補については、所定時間だけ無効な行動計画候補とし、所定時間が経過した後に有効な行動計画候補として利用するようにした計画部１３４の構成例を示している。尚、図１３の計画部１３４において、図１０の計画部１３４と同様の機能を備えた構成については、同一の符号を付しており、その説明は適宜省略する。
すなわち、図１３の計画部１３４において、図１０の計画部１３４と異なる点は、新たに有効行動計画候補判定部２４１を備えた点である。
有効行動計画候補判定部２４１は、行動計画候補生成部２０１により生成された行動計画候補であって、行動計画候補評価部２０２より評価値が付されたもののうち、行動計画候補の生成が開始されてから所定時間が経過していないものについては、無効な行動計画候補として設定し、所定時間が経過したものを有効な行動計画候補に設定する。
乖離度判定部２３１は、有効な行動計画候補のみを用いて乖離度を算出し、乖離度に応じて、選択部２１１が選択型行動計画決定処理を実行して、有効な行動計画候補から評価値が最高値のものを選択して行動計画を決定するか、または、融合部２２１が、融合型行動計画決定処理を実行して、有効な行動計画候補を評価値に応じて融合して行動計画を決定する。
ここで、行動計画候補生成部２０１－１が、第１の自己位置の候補に対応して設定される第１のルートに基づいて行動計画候補ＣＰ１を計画し、行動計画候補生成部２０１－２が、第２の自己位置の候補に対応して設定される第２のルートに基づいて行動計画候補ＣＰ２を計画するものとする。
このとき、例えば、図１４で示されるように、時刻ｔ１から行動計画候補生成部２０１－１が行動計画候補ＣＰ１の計画を開始しているものとする。このとき、行動計画候補生成部２０１－２は、行動計画候補を計画していない。
有効行動計画候補判定部２４１は、行動計画候補生成部２０１－１が、行動計画候補ＣＰ１の計画を開始した時刻ｔ１から、所定時間Ｔ１が経過した時刻ｔ２以降において、行動計画候補ＣＰ１を有効な行動計画候補とみなして、有効な行動計画候補ＣＰ１を用いて行動計画を決定する。
また、時刻ｔ３において、候補となる自己位置が増えて、行動計画候補生成部２０１－２が、第２の自己位置の候補に対応して設定される第２のルートに基づいて行動計画候補ＣＰ２の計画を開始する。この時点では、行動計画候補ＣＰ２の精度は低いので、無効な行動計画候補とみなし、行動計画決定処理には利用せず、時刻ｔ３以降においても、有効とみなされる行動計画候補ＣＰ１のみが行動計画決定処理に利用される。
さらに、行動計画候補生成部２０１－２が、第２の自己位置の候補に対応して設定される第２のルートに基づいて行動計画候補ＣＰ２の計画を開始してから所定時間Ｔ２が経過した時刻ｔ４以降においては、有効行動計画候補判定部２４１は、行動計画候補ＣＰ２についても有効な行動計画候補とみなし、選択部２１１が、行動計画候補ＣＰ１，ＣＰ２のいずれか評価値に基づいて選択して行動計画を決定するか、または、融合部２２１が、行動計画候補ＣＰ１，ＣＰ２を評価値に基づいて融合して行動計画を決定する。
尚、以上においては、行動計画候補の生成が開始された時刻からの経過時間に応じて、所定時間を超えるまでは無効な行動計画候補とされ、所定時間を経過した後は、有効な行動計画候補とする例について説明してきたが、行動計画候補としての信頼性の問題であるので、候補となる自己位置の確度が所定値よりも高い自己位置に基づいて計画されたルートにより計画された行動計画候補については、行動計画候補の計画が開始された当初から有効な行動計画候補とみなすようにしてもよい。また、行動計画候補の計画が開始されてからの経過時間と、確度とを総合的に組み合わせて、有効な行動計画候補とするか否かを判定するようにしてもよい。
また、行動計画候補が、有効であるか、または無効であるかの判定には、行動計画候補の生成が開始されてからの経過時間や確度以外の条件も含めて判定するようにしてもよい。
＜図１３の計画部による行動計画決定処理＞
次に、図１５のフローチャートを参照して、図１３の計画部１３４による行動計画決定処理について説明する。
ステップＳ９１において、行動計画候補生成部２０１－１乃至２０１－ｎは、それぞれ候補となる自己位置に応じて計画されたルートに基づいて、行動計画候補を生成し、それぞれ生成した行動計画候補を行動計画候補評価部２０２－１乃至２０２－ｎに出力する。
ステップＳ９２において、行動計画候補評価部２０２－１乃至２０２－ｎは、それぞれ行動計画候補のそれぞれについて評価値を設定し、行動計画決定部２０３に出力する。
ステップＳ９３において、有効行動計画候補判定部２４１は、未処理の行動計画候補のいずれかを処理対象行動計画候補として設定する。
ステップＳ９４において、有効行動計画候補判定部２４１は、処理対象行動計画候補が所定の条件を満たした有効な行動計画候補であるか否かを判定する。より詳細には、有効行動計画候補判定部２４１は、処理対象行動計画候補を生成した行動計画候補生成部２０１が行動計画候補の生成を開始してからの経過時間が所定時間より長いか、若しくは、候補となる自己位置の確度が所定の閾値よりも高いか、または、これらの条件を全て満たすか否かなど、所定の条件を満たすか否かにより有効な行動計画候補であるか否かを判定する。
ステップＳ９４において、有効な行動計画候補であるとみなされた場合、ステップＳ９５において、有効行動計画候補判定部２４１は、処理対象行動計画候補を有効な行動計画候補とみなして、有効であることを示すフラグを処理対象行動計画候補に設定する。
一方、ステップＳ９４において、有効な行動計画候補ではないとみなされた場合、ステップＳ９６において、有効行動計画候補判定部２４１は、処理対象行動計画候補を無効な行動計画候補とみなして、無効であることを示すフラグを処理対象行動計画候補に設定する。
ステップＳ９７において、有効行動計画候補判定部２４１は、未処理の行動計画候補が存在するか否かを判定し、未処理の行動計画候補が存在する場合、処理は、ステップＳ９３に戻る。
すなわち、全ての行動計画候補が、有効であるか、または無効であるかに分類されてフラグが設定されるまで、ステップＳ９３乃至Ｓ９７の処理が繰り返される。ステップＳ９３乃至Ｓ９７の処理により、全ての行動計画候補が有効であるか、または、無効であるかに分類され、ステップＳ９７において、未処理の行動計画候補がないとみなされた場合、処理は、ステップＳ９８に進む。
ステップＳ９８において、行動計画決定部２０３は、乖離度判定部２３１を制御して、複数の有効な行動計画候補間の乖離度を算出する。
ステップＳ９９において、乖離度判定部２３１は、乖離度が所定の閾値よりも大きく、有効な行動計画候補間には大きな乖離があるか否かを判定する。ステップＳ９９において、乖離度が所定の閾値よりも大きく、乖離が大きいと判定された場合、処理は、ステップＳ１００に進む。
ステップＳ１００において、行動計画決定部２０３は、複数の有効な行動計画候補の乖離が大きいので、選択型行動計画決定処理を実行し、複数の有効な行動計画候補のそれぞれの評価値に基づいて、評価値が最高値（または、評価値が最低値）の有効な行動計画候補を選択し、行動計画として決定する。
一方、ステップＳ９９において、乖離度が所定の閾値よりも小さく、乖離が小さいと判定された場合、処理は、ステップＳ１０１に進む。
ステップＳ１０１において、行動計画決定部２０３は、複数の有効な行動計画候補の乖離が小さいので、融合型行動計画決定処理を実行し、複数の有効な行動計画候補のそれぞれの評価値に基づいて、複数の有効な行動計画候補を融合し、行動計画として決定する。
以上の処理により、複数の行動計画候補について、行動計画候補の計画が開始されてからの経過時間や、行動計画候補に対応する自己位置の精度などの条件に応じて、有効な行動計画候補が設定され、有効な行動計画候補のみが用いられて行動計画が決定されることになるので、ある程度、精度の高い行動計画候補のみが用いられて、行動計画が決定されることになるので、決定される行動計画の精度を向上させることが可能となる。また、途中で自己位置の候補が増えるような際に、精度の低い行動計画候補が利用されることで、行動計画の精度の低下を抑制させることが可能となる。
＜＜６．ソフトウェアにより実行させる例＞＞
ところで、上述した一連の処理は、ハードウェアにより実行させることもできるが、ソフトウェアにより実行させることもできる。一連の処理をソフトウェアにより実行させる場合には、そのソフトウェアを構成するプログラムが、専用のハードウェアに組み込まれているコンピュータ、または、各種のプログラムをインストールすることで、各種の機能を実行することが可能な、例えば汎用のコンピュータなどに、記録媒体からインストールされる。
図１６は、汎用のコンピュータの構成例を示している。このパーソナルコンピュータは、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１００１が、図３における自律移動制御部１１０の機能を実現させる。また、図１６における記憶部１００８が、図３における記憶部１０９を実現する。
また、本明細書において、システムとは、複数の構成要素（装置、モジュール（部品）等）の集合を意味し、すべての構成要素が同一筐体中にあるか否かは問わない。したがって、別個の筐体に収納され、ネットワークを介して接続されている複数の装置、及び、１つの筐体の中に複数のモジュールが収納されている１つの装置は、いずれも、システムである。
なお、本開示の実施の形態は、上述した実施の形態に限定されるものではなく、本開示の要旨を逸脱しない範囲において種々の変更が可能である。
例えば、本開示は、１つの機能をネットワークを介して複数の装置で分担、共同して処理するクラウドコンピューティングの構成をとることができる。
また、上述のフローチャートで説明した各ステップは、１つの装置で実行する他、複数の装置で分担して実行することができる。
さらに、１つのステップに複数の処理が含まれる場合には、その１つのステップに含まれる複数の処理は、１つの装置で実行する他、複数の装置で分担して実行することができる。
尚、本開示は、以下のような構成も取ることができる。
＜１＞ 周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を生成する行動計画候補生成部と、
生成された前記複数の行動計画候補にそれぞれ評価値を付与する行動計画候補評価部と、
前記複数の行動計画候補、およびそれぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定部と
を含む制御装置。
＜２＞ 前記行動計画候補生成部は、前記周辺の状況に基づいて推定される自己位置の候補となる位置毎に、前記行動計画の候補となる行動計画候補を生成する
＜１＞に記載の制御装置。
＜３＞ 前記行動計画候補評価部は、前記周辺の状況に基づいて推定される自己位置の候補となる位置の確度に基づいて、生成された前記複数の行動計画候補にそれぞれ評価値を付与する
＜１＞または＜２＞に記載の制御装置。
＜４＞ 前記周辺の状況に基づいて推定される自己位置は、カメラ画像からの特徴点と事前知識との合致により推定され、
前記位置の確度は合致した特徴点数である
＜３＞に記載の制御装置。
＜５＞ 前記行動計画決定部は、
前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択部をさらに含み、
前記選択部により選択された行動計画候補を前記行動計画に決定する
＜１＞に記載の制御装置。
＜６＞ 前記選択部は、前記評価値に基づいて、前記複数の行動計画候補のうち、前記評価値が最良値となる行動計画候補を選択し、
前記行動計画決定部は、前記選択部により選択された行動計画候補を前記行動計画に決定する
＜５＞に記載の制御装置。
＜７＞ 前記行動計画決定部は、
前記評価値に基づいて、前記複数の行動計画候補を融合する融合部をさらに含み、
前記融合部により前記複数の行動計画候補が融合されたものを、前記行動計画に決定する
＜１＞に記載の制御装置。
＜８＞ 前記融合部は、前記複数の行動計画候補を構成する経路の通過点同士の座標の各要素に、前記評価値による重みを付加した重心位置からなる、新規の通過点を経路とする新たな行動計画候補を生成することで、前記複数の行動計画候補を融合し、
前記行動計画決定部は、前記融合部により前記複数の行動計画候補が融合されたものを前記行動計画に決定する
＜７＞に記載の制御装置。
＜９＞ 前記行動計画決定部は、前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する
＜１＞に記載の制御装置。
＜１０＞ 前記行動計画決定部は、
前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択部と、
前記評価値に基づいて、前記複数の行動計画候補を融合する融合部と、
前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択部により、前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の行動計画候補を融合することで、前記行動計画を決定するかを判定する乖離度判定部とをさらに含む
＜９＞に記載の制御装置。
＜１１＞ 前記乖離度判定部は、前記複数の行動計画候補がベクトルで表現される場合、重み付きベクトル差を前記乖離度として算出する
＜１０＞に記載の制御装置。
＜１２＞ 前記乖離度判定部は、前記複数の行動計画候補を表現するベクトルの大きさを正規化した上で、前記重み付きベクトル差を前記乖離度として算出する
＜１１＞に記載の制御装置。
＜１３＞ 前記乖離度判定部は、前記複数の行動計画候補が複数の通過点を経由することで形成される経路である場合、前記通過点間でベクトルを構成し、構成した前記ベクトルを用いた重み付きベクトル差を前記乖離度として算出する
＜１０＞に記載の制御装置。
＜１４＞ 前記乖離度判定部は、前記複数の行動計画候補が複数の通過点を経由することで形成される経路である場合、前記通過点間の距離の差の総和を前記乖離度として算出する
＜１０＞に記載の制御装置。
＜１５＞ 前記行動計画決定部は、
前記複数の行動計画候補のうち、所定の条件を満たす有効な行動計画候補を判定する有効行動計画候補判定部をさらに含み、
前記乖離度判定部は、前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択部により、前記評価値に基づいて、前記複数の有効な行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の有効な行動計画候補を融合することで、前記行動計画を決定するかを判定する
＜１０＞に記載の制御装置。
＜１６＞ 前記有効行動計画候補判定部は、前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した前記行動計画候補生成部により生成された行動計画候補を、所定の条件を満たす有効な行動計画候補として判定する
＜１５＞に記載の制御装置。
＜１７＞ 前記有効行動計画候補判定部は、前記複数の行動計画候補のうち、所定の確度を有する行動計画候補を、前記所定の条件を満たす有効な行動計画候補として判定する
＜１５＞に記載の制御装置。
＜１８＞ 前記周辺の状況に基づいて、自己位置の候補となる位置毎に、目的地までのルート計画を生成するルート計画生成部をさらに含み、
前記行動計画候補生成部は、前記自己位置の候補となる位置毎に生成される前記ルート計画に基づいて、前記行動計画の候補となる行動計画候補を生成する
＜１＞乃至＜１７＞のいずれかに記載の制御装置。
＜１９＞ 周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を生成する行動計画候補生成処理と、
生成された前記複数の行動計画候補にそれぞれ評価値を付与する行動計画候補評価処理と、
前記複数の行動計画候補、およびそれぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定処理と
を含む制御方法。
＜２０＞ 周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を生成する行動計画候補生成処理と、
生成された前記複数の行動計画候補にそれぞれ評価値を付与する行動計画候補評価処理と、
前記複数の行動計画候補、およびそれぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定処理と
してコンピュータを機能させるプログラム。
＜２１＞ 周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を生成する行動計画候補生成部と、
生成された前記複数の行動計画候補にそれぞれ評価値を付与する行動計画候補評価部と、
前記複数の行動計画候補、およびそれぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定部と、
前記行動計画決定部により決定された行動計画に基づいて移動体の動作を制御する制御部と
を含む移動体。
１１ 移動体， ２１ センサ群， ２１ａ，２１ａ－１乃至２１ａ－ｎ センサ， ２２ 自律移動制御部， ２３ アクチュエータ群， ２３ａ，２３ａ－１乃至２３ａ－ｎ アクチュエータ， ３１ 認識処理部， ３２ 行動計画処理部， ３３ 行動制御処理部， ４１ ルート計画部， ４２ 行動計画部， ４３ 動作計画部， １０２ データ取得部， １０５ 出力制御部， １０６ 出力部， １０７ 駆動系制御部， １０８ 駆動系システム， １１０ 自律移動制御部， １１２ センサ群， １１３ アクチュエータ群， １２１ 認識処理部， １２２ 行動計画処理部， １２３ 行動制御処理部， １３４ 計画部， １６１ ルート計画部， １６２ 行動計画部， １６３ 動作計画部， ２０１，２０１－１乃至２０１－ｎ 行動計画候補生成部， ２０２，２０２－１乃至２０２－ｎ 行動計画候補評価部， ２０３ 行動計画決定部， ２１１ 選択部， ２２１ 融合部， ２３１ 乖離度判定部， ２４１ 有効行動計画候補判定部

Claims (14)

  1. 複数の自己位置候補から自己位置が一つに定まる場合は一つの前記自己位置を取得し、前記自己位置が一つに定まらない場合に前記複数の自己位置候補を取得する自己位置取得部と、
    周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を前記複数の自己位置候補毎に生成する行動計画候補生成部と、
    生成された前記複数の行動計画候補のそれぞれに事前地図情報と周辺環境の関連性に基づいた評価値を付与する行動計画候補評価部と、
    前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、それぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定部とを含み、
    前記行動計画決定部は、
    前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択部と、
    前記評価値に基づいて、前記複数の行動計画候補を融合する融合部と、
    前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した前記行動計画候補生成部により生成された行動計画候補を、有効な行動計画候補として判定する有効行動計画候補判定部と、
    前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択部により、前記評価値に基づいて、複数の有効な行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の有効な行動計画候補を融合することで、前記行動計画を決定するかを判定する乖離度判定部とを含む
    制御装置。
  2. 前記行動計画候補評価部は、前記周辺の状況に基づいて推定される自己位置候補となる位置の確度に基づいて、生成された前記複数の行動計画候補のそれぞれに評価値を付与する
    請求項１に記載の制御装置。
  3. 前記周辺の状況に基づいて推定される前記自己位置候補は、カメラ画像からの特徴点と事前知識との合致により推定され、
    前記位置の確度は合致した特徴点数である
    請求項２に記載の制御装置。
  4. 前記選択部は、前記評価値に基づいて、前記複数の行動計画候補のうち、前記評価値が最良値となる行動計画候補を選択する
    請求項１に記載の制御装置。
  5. 前記融合部は、前記複数の行動計画候補を構成する経路の通過点同士の座標の各要素に、前記評価値による重みを付加した重心位置からなる、新規の通過点を経路とする新たな行動計画候補を生成することで、前記複数の行動計画候補を融合する
    請求項１に記載の制御装置。
  6. 前記乖離度判定部は、前記複数の行動計画候補がベクトルで表現される場合、重み付きベクトル差を前記乖離度として算出する
    請求項１に記載の制御装置。
  7. 前記乖離度判定部は、前記複数の行動計画候補を表現するベクトルの大きさを正規化した上で、前記重み付きベクトル差を前記乖離度として算出する
    請求項６に記載の制御装置。
  8. 前記乖離度判定部は、前記複数の行動計画候補が複数の通過点を経由することで形成される経路である場合、前記通過点間でベクトルを構成し、構成した前記ベクトルを用いた重み付きベクトル差を前記乖離度として算出する
    請求項１に記載の制御装置。
  9. 前記乖離度判定部は、前記複数の行動計画候補が複数の通過点を経由することで形成される経路である場合、前記通過点間の距離の差の総和を前記乖離度として算出する
    請求項１に記載の制御装置。
  10. 前記有効行動計画候補判定部は、前記複数の行動計画候補のうち、前記所定の時間が経過して、かつ、所定の確度を有する行動計画候補を、前記有効な行動計画候補として判定する
    請求項１に記載の制御装置。
  11. 前記自己位置取得部は、前記周辺の状況に基づいて、取得した前記複数の自己位置候補毎に、目的地までのルート計画を生成し、
    前記行動計画候補生成部は、前記自己位置候補となる位置毎に生成される前記ルート計画に基づいて、前記行動計画の候補となる行動計画候補を生成する
    請求項１に記載の制御装置。
  12. 複数の自己位置候補から自己位置が一つに定まる場合に、一つの前記自己位置を取得し、前記自己位置が一つに定まらない場合に、前記複数の自己位置候補を取得する自己位置取得処理と、
    周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を前記複数の自己位置候補毎に生成する行動計画候補生成処理と、
    生成された前記複数の行動計画候補のそれぞれに事前地図情報と周辺環境の関連性に基づいた評価値を付与する行動計画候補評価処理と、
    前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、それぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定処理とを含み、
    前記行動計画決定処理は、
    前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択処理と、
    前記評価値に基づいて、前記複数の行動計画候補を融合する融合処理と、
    前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した前記行動計画候補生成処理により生成された行動計画候補を、有効な行動計画候補として判定する有効行動計画候補判定処理と、
    前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択処理により、前記評価値に基づいて、複数の有効な行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の有効な行動計画候補を融合することで、前記行動計画を決定するかを判定する乖離度判定処理とを含む
    制御方法。
  13. 複数の自己位置候補から自己位置が一つに定まる場合は一つの前記自己位置を取得し、前記自己位置が一つに定まらない場合に前記複数の自己位置候補を取得する自己位置取得部と、
    周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を前記複数の自己位置候補毎に生成する行動計画候補生成部と、
    生成された前記複数の行動計画候補のそれぞれに事前地図情報と周辺環境の関連性に基づいた評価値を付与する行動計画候補評価部と、
    前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、それぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定部としてコンピュータを機能させ、
    前記行動計画決定部は、
    前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択部と、
    前記評価値に基づいて、前記複数の行動計画候補を融合する融合部と、
    前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した前記行動計画候補生成部により生成された行動計画候補を、有効な行動計画候補として判定する有効行動計画候補判定部と、
    前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択部により、前記評価値に基づいて、複数の有効な行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の有効な行動計画候補を融合することで、前記行動計画を決定するかを判定する乖離度判定部とを含む
    プログラム。
  14. 複数の自己位置候補から自己位置が一つに定まる場合は一つの前記自己位置を取得し、前記自己位置が一つに定まらない場合に前記複数の自己位置候補を取得する自己位置取得部と、
    周辺の状況に基づいて、行動計画の候補となる複数の行動計画候補を前記複数の自己位置候補毎に生成する行動計画候補生成部と、
    生成された前記複数の行動計画候補のそれぞれに事前地図情報と周辺環境の関連性に基づいた評価値を付与する行動計画候補評価部と、
    前記複数の行動計画候補間の乖離度を算出し、前記乖離度に応じて、それぞれの前記評価値に基づいて、前記複数の行動計画候補を利用して、前記行動計画を決定する行動計画決定部と、
    前記行動計画決定部により決定された行動計画に基づいて移動体の動作を制御する制御部とを含み、
    前記行動計画決定部は、
    前記評価値に基づいて、前記複数の行動計画候補のいずれかを選択する選択部と、
    前記評価値に基づいて、前記複数の行動計画候補を融合する融合部と、
    前記複数の行動計画候補のうち、前記行動計画候補の生成を開始してから所定の時間が経過した前記行動計画候補生成部により生成された行動計画候補を、有効な行動計画候補として判定する有効行動計画候補判定部と、
    前記複数の行動計画候補のうち、前記有効な行動計画候補間の乖離度を算出し、前記乖離度に応じて、前記選択部により、前記評価値に基づいて、複数の有効な行動計画候補のいずれかを選択することで、前記行動計画を決定するか、または、前記評価値に基づいて、前記複数の有効な行動計画候補を融合することで、前記行動計画を決定するかを判定する乖離度判定部とを含む
    移動体。
JP2017177753A 2017-09-15 2017-09-15 制御装置、および制御方法、プログラム、並びに移動体 Active JP7147142B2 (ja)

Priority Applications (5)

Application Number Priority Date Filing Date Title
JP2017177753A JP7147142B2 (ja) 2017-09-15 2017-09-15 制御装置、および制御方法、プログラム、並びに移動体
US16/645,174 US11500386B2 (en) 2017-09-15 2018-09-04 Control apparatus, control method, program, and mobile object
EP18780256.6A EP3682306B1 (en) 2017-09-15 2018-09-04 Action plan generation when the own position is unknown
CN201880057871.3A CN111052027A (zh) 2017-09-15 2018-09-04 自身位置未知时的行动计划生成
PCT/JP2018/032699 WO2019054230A1 (en) 2017-09-15 2018-09-04 ACTION PLAN PRODUCTION WHEN THE CLEAN POSITION IS UNKNOWN

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2017177753A JP7147142B2 (ja) 2017-09-15 2017-09-15 制御装置、および制御方法、プログラム、並びに移動体

Publications (2)

Publication Number Publication Date
JP2019053561A JP2019053561A (ja) 2019-04-04
JP7147142B2 true JP7147142B2 (ja) 2022-10-05

Family

ID=63720748

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2017177753A Active JP7147142B2 (ja) 2017-09-15 2017-09-15 制御装置、および制御方法、プログラム、並びに移動体

Country Status (5)

Country Link
US (1) US11500386B2 (ja)
EP (1) EP3682306B1 (ja)
JP (1) JP7147142B2 (ja)
CN (1) CN111052027A (ja)
WO (1) WO2019054230A1 (ja)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109189060B (zh) * 2018-07-25 2021-01-12 博众精工科技股份有限公司 移动机器人的点镇定控制方法及装置
JP7460328B2 (ja) * 2019-03-20 2024-04-02 Thk株式会社 移動ロボット、移動ロボットの制御システム、移動ロボットの制御方法
CN113492411B (zh) * 2021-09-10 2021-11-30 季华实验室 机器人抓取路径规划方法、装置、电子设备及存储介质

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008158868A (ja) 2006-12-25 2008-07-10 Toyota Motor Corp 移動体、及びその制御方法
JP2014123306A (ja) 2012-12-21 2014-07-03 Secom Co Ltd 自律飛行ロボット
US20140278095A1 (en) 2013-03-15 2014-09-18 Applied Minds, Llc Method and apparatus for two-stage planning
JP2016218673A (ja) 2015-05-19 2016-12-22 トヨタ自動車株式会社 経路計画装置、及び経路計画方法
US20170147003A1 (en) 2002-12-17 2017-05-25 Irobot Corporation Systems and methods for using multiple hypotheses in a visual simultaneous localization and mapping system
JP2017161284A (ja) 2016-03-08 2017-09-14 アイシン・エィ・ダブリュ株式会社 自動運転支援装置及びコンピュータプログラム

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06180235A (ja) * 1992-12-14 1994-06-28 Pioneer Electron Corp ナビゲーション装置
US20060212188A1 (en) * 2003-02-27 2006-09-21 Joel Kickbusch Method and apparatus for automatic selection of alternative routing through congested areas using congestion prediction metrics
US20050075921A1 (en) * 2003-10-03 2005-04-07 Frederick Hayes-Roth Open community model for exchanging information in dynamic environments
US20080183506A1 (en) * 2007-01-25 2008-07-31 Jeffrey Mamorsky Method for quantifying risk for use in underwriting and insuring such risk and method for measuring the adequacy of the security required under the fiduciary prudence and prohibited transaction provisions of the employee retirement security act of 1974
US7689311B2 (en) * 2007-05-29 2010-03-30 Palo Alto Research Center Incorporated Model-based planning using query-based component executable instructions
WO2009065638A1 (en) * 2007-11-24 2009-05-28 Routerank Ltd Personalized real-time location-based travel management
JP6020326B2 (ja) * 2013-04-16 2016-11-02 富士ゼロックス株式会社 経路探索装置、自走式作業装置、プログラム及び記録媒体
CN103576686B (zh) * 2013-11-21 2017-01-18 中国科学技术大学 一种机器人自主导引及避障的方法
JP6174523B2 (ja) 2014-06-25 2017-08-02 日本電信電話株式会社 行動制御システム、その方法及びプログラム
JP6221158B2 (ja) 2014-08-27 2017-11-01 本田技研工業株式会社 自律行動ロボット、及び自律行動ロボットの制御方法
CN104407616B (zh) * 2014-12-03 2017-04-26 沈阳工业大学 基于免疫网络算法的移动机器人动态路径规划方法
JP6112123B2 (ja) * 2015-01-19 2017-04-12 トヨタ自動車株式会社 自動運転装置
US9910441B2 (en) * 2015-11-04 2018-03-06 Zoox, Inc. Adaptive autonomous vehicle planner logic

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170147003A1 (en) 2002-12-17 2017-05-25 Irobot Corporation Systems and methods for using multiple hypotheses in a visual simultaneous localization and mapping system
JP2008158868A (ja) 2006-12-25 2008-07-10 Toyota Motor Corp 移動体、及びその制御方法
JP2014123306A (ja) 2012-12-21 2014-07-03 Secom Co Ltd 自律飛行ロボット
US20140278095A1 (en) 2013-03-15 2014-09-18 Applied Minds, Llc Method and apparatus for two-stage planning
JP2016218673A (ja) 2015-05-19 2016-12-22 トヨタ自動車株式会社 経路計画装置、及び経路計画方法
JP2017161284A (ja) 2016-03-08 2017-09-14 アイシン・エィ・ダブリュ株式会社 自動運転支援装置及びコンピュータプログラム

Also Published As

Publication number Publication date
EP3682306B1 (en) 2022-03-30
US20210026356A1 (en) 2021-01-28
EP3682306A1 (en) 2020-07-22
CN111052027A (zh) 2020-04-21
WO2019054230A1 (en) 2019-03-21
US11500386B2 (en) 2022-11-15
JP2019053561A (ja) 2019-04-04

Similar Documents

Publication Publication Date Title
US11822341B2 (en) Control device, control method, and mobile object to estimate the mobile object's self-position
EP3722904B1 (en) Moving body, control method and control program
JP7156305B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
US20200231176A1 (en) Control apparatus, control method, program, and moving body
JP6891753B2 (ja) 情報処理装置、移動装置、および方法、並びにプログラム
JP7180670B2 (ja) 制御装置、制御方法、並びにプログラム
WO2020226085A1 (ja) 情報処理装置、情報処理方法、及びプログラム
US11822334B2 (en) Information processing apparatus, information processing method, and program for control of a moving body capable of autonomous movement
JP7257737B2 (ja) 情報処理装置、自己位置推定方法、及び、プログラム
JP7147142B2 (ja) 制御装置、および制御方法、プログラム、並びに移動体
WO2021153176A1 (ja) 自律移動装置、自律移動制御方法、並びにプログラム
WO2021033591A1 (ja) 情報処理装置、および情報処理方法、並びにプログラム
WO2021024805A1 (ja) 情報処理装置、および情報処理方法、並びにプログラム
US11906970B2 (en) Information processing device and information processing method
JP7135690B2 (ja) 情報処理装置および方法、プログラム、並びに移動体制御システム
WO2019176278A1 (ja) 情報処理装置、情報処理方法、プログラム、及び移動体
WO2021140916A1 (ja) 移動体、情報処理装置、情報処理方法、及びプログラム

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20200721

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20211102

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220104

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20220329

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220520

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

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20220905

R151 Written notification of patent or utility model registration

Ref document number: 7147142

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151