JP7455704B2 - 移動体制御システムおよび移動体制御方法 - Google Patents

Description

本発明は、移動体制御システムおよび移動体制御方法に関する。

近年、自動車の分野で自動運転が進展している。例えば、特許文献１に示す、インフラ側に設置された固定カメラで検出した情報に基づき、自動車を制御する走行支援制御装置が挙げられる。

具体的には特許文献１には、「自動運転走行が可能な制御対象車両を含む所定領域を撮像して画像情報を取得する前記制御対象車両以外に配置された少なくとも１つのカメラと、前記カメラにより取得された前記画像情報に基づき、前記所定領域内での前記制御対象車両の前記自動運転走行を支援する走行支援部と、前記カメラにより取得された前記画像情報に基づき、前記制御対象車両の位置を把握する位置把握部と、前記位置把握部により把握された前記制御対象車両の前記位置に基づき、前記カメラにより取得された前記画像情報から少なくとも制御対象車両領域を除外する制御対象車両領域除外部と、を備え、前記走行支援部は、前記制御対象車両領域除外部により前記制御対象車両領域が除外された前記所定領域内での障害物を認識しながら前記制御対象車両の前記自動運転走行の支援を行う」走行支援制御装置が開示されている（特許文献１の請求項１参照）。

特開２０１６－５７６７７号公報

特許文献１に開示の従来技術によれば、制御対象車両を障害物と誤認識せずに、所定領域内での障害物を認識しながら所望の位置に車両を誘導することが可能である。

しかしながら、特許文献１に開示の従来技術では、外乱（センサ故障、光の差し込み、霧、地震によるセンサの設置向きのずれ等）によって障害物が見えない場合、障害物が存在するにも関わらず、存在しないと誤判定して車両を誘導してしまい、制御対象車両が障害物に衝突の危険がある、という問題がある。

各種外乱に対して、外乱により検知できないこと（例：太陽光がセンサに差し込むこと）を検出して、車両の誘導の制御を止めるなどの仕組みを導入することもできるが、予めすべての外乱を想定することは難しいため、想定外の外乱に対する安全を担保することは難しい。

また特許文献１に開示の従来技術では、外乱がなくても、想定する人や物体と異なる想定外の人や物体を検知することができず、その結果、障害物が存在するにも関わらず、存在しないと誤判定してしまうという問題もある。

本発明は、上記課題に鑑みてなされたものであり、想定外のセンサの外乱があった場合や、想定する人や物体と異なる想定外の人や物体が障害物として存在する場合において、移動体が移動する軌道の安全性を確保できるようにすることを目的とする。

上記目的を達成するために、本発明の移動体制御システムは、移動領域における移動体の移動を制御する移動体制御システムであって、前記移動領域のうち前記移動体が安全に移動可能な領域を定義した情報である安全状態を記憶する安全状態記憶部と、センサによって取得された現在の前記移動領域の状態に関する情報である現在状態と、前記安全状態とに基づいて、前記移動領域から前記移動体が安全に移動可能な安全領域を抽出する安全領域抽出部とを有することを特徴とする。

本発明によれば、想定外のセンサの外乱があった場合や、想定する人や物体と異なる想定外の人や物体が障害物として存在する場合において、移動体が移動する軌道の安全性を確保できる。

本発明の第１の実施形態における移動体制御システムの概略構成を示す図。 本発明の第１の実施形態における管理装置および制御装置をコンピュータで構成した場合のハードウェア図。 本発明の第１の実施形態における移動体制御システムが動作する環境の一例を示す図。 本発明の第１の実施形態における移動体制御システムの機能構成を示すブロック図。 本発明の第１の実施形態における安全状態を示す図。 本発明の第１の実施形態における現在のセンサデータを示す図。 本発明の第１の実施形態における安全領域および非安全領域を示す図。 本発明の第１の実施形態における軌道設定の計画の概要を示す図。 本発明の第１の実施形態における安全領域抽出部の処理のフローチャートを示す図。 本発明の第１の実施形態の変形例１における移動体制御システムの機能構成を示すブロック図。 本発明の第１の実施形態の変形例２における移動体制御システムの機能構成を示すブロック図。 本発明の第２の実施形態における移動体制御システムの概略構成を示す図。 本発明の第２の実施形態における移動体制御システムの機能構成を示すブロック図。 本発明の第３の実施形態における移動体制御システムの概略構成を示す図。 本発明の第３の実施形態における移動体制御システムが動作する環境の一例を示す図。 本発明の第３の実施形態における移動体制御システムの機能構成を示すブロック図。 本発明の第４の実施形態における移動体制御システムが動作する環境の一例を示す図。 本発明の第４の実施形態における移動体制御システムの機能構成を示すブロック図。

以下、本発明の実施形態について、図面を参照して説明する。なお、以下に説明する実施形態は特許請求の範囲に係る発明を限定するものではなく、また実施形態の中で説明されている諸要素およびその組合せの全てが発明の解決手段に必須であるとは限らない。また発明の構成に必須だが周知である構成については、図示および説明を省略する場合がある。また各図に示す各要素の数は一例であって、図示に限られるものではない。

以下では、数字のみからなる符号に、アルファベットまたは数字の添え字を付加して、同一名称の複数の要素を区別する。ただし添え字を除いた数字のみからなる符号で、同一名称の複数の要素を総称する場合がある。

以下では、移動体として自律移動ロボットを例に説明する。自律移動ロボットは、例えば自律移動する車両であるが、歩行ロボットであってもよい。また移動体は、自律的に操舵する車両に限らず、遠隔から操舵される車両、人が搭乗して操舵する車両でもよい。また車両は、乗用車や、作業車両、貨物車等であってもよい。

本発明は、センサで外部を認識し、認識結果に基づきに軌道を計画し、計画した軌道に沿って移動するようにアクチュエータを制御する移動ロボット、自動車、鉄道システム、マニピュレータ型ロボット等の移動体制御システムに関する。

（第１の実施形態）
（移動体制御システム１の概略構成）
図１は、本発明の第１の実施形態における移動体制御システム１の概略構成を示す図である。本実施形態における移動体制御システム１は、移動ロボット２と、移動ロボット２の走行環境を認識するセンサ３と、センサ３でセンシングした安全状態を移動ロボット２に搭載された制御装置５に記憶させる管理装置４と、安全状態とセンサ３の出力に基づいてアクチュエータへの制御指令を算出する制御装置５と、アクチュエータ６への指令に基づき図示しない複数の車輪および図示しないステアリング装置を駆動するアクチュエータ６と、を有する。

なおセンサ３は、３次元的な点群データが得られるセンサが望ましく、ステレオカメラやＬｉｄａｒ、ＴＯＦセンサ等が望ましいが、それに限定しない。また、センサは一つである必要はなく、複数のセンサを組み合わせてもよい。複数のセンサを用いることで、システムの安全性や可動範囲を高めることが可能となる。

（管理装置４および制御装置５のハードウェア構成）
図２は、本発明の第１の実施形態における管理装置４および制御装置５をコンピュータで構成した場合のハードウェア図である。管理装置４または制御装置５として機能するコンピュータは、バスにそれぞれ接続されたＣＰＵ（Central Processing Unit：中央処理ユニット）２０ａと、ＲＯＭ（Read Only Memory）２０ｂと、ＲＡＭ（Random Access Memory）２０ｃを備える。さらに、制御装置５は、不揮発性ストレージ２０ｄと、ネットワークインタフェース２０ｅと、入力部２０ｆと、表示部２０ｇとを備える。

制御装置５をコンピュータで構成した場合、ＣＰＵ２０ａは、後述する安全状態記憶部３３についての演算処理や、安全領域抽出部３４や立体物抽出部３５、行動予測部３６、死角抽出部３７、リスク予測部３８などの認知関連の処理部や、軌道計画部３９、制御部４０などの計画および制御に関する演算処理を実行するソフトウェアのプログラムコードを、ＲＯＭ２０ｂから読み出して実行する演算処理部である。ＲＡＭ２０ｃには、演算処理の途中に発生した変数やパラメータ等が一時的に書き込まれる。

不揮発性ストレージ２０ｄには、例えば、ＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）などの大容量の情報記憶部が用いられる。不揮発性ストレージ２０ｄは、「安全状態」と定義した点群データを保存している。「安全状態」については後述する。

ネットワークインタフェース２０ｅには、例えば、ＮＩＣ（Network Interface Card）などが用いられる。ネットワークインタフェース２０ｅは、例えば管理装置４から伝送される安全状態と定義した点群データや、センサ３から伝送される点群データを受信する。

入力部２０ｆは、上記安全状態の点群データを作成、更新などを行う際の各種データの入力処理を行う。

表示部２０ｇには、制御装置５の作動状況が表示される。また、表示部２０ｇは、安全状態の点群データやセンサの点群データの表示を行う。

なお、制御装置５を図２に示すコンピュータで構成するのは一例であり、コンピュータ以外のその他の演算処理を行う装置で構成してもよい。例えば、制御装置５が行う機能の一部または全部を、ＦＰＧＡ（Field Programmable Gate Array）やＡＳＩＣ（Application Specific Integrated Circuit）などのハードウェアによって実現してもよい。

また、制御装置５が入力部２０ｆや表示部２０ｇを備える構成とした点についても一例であり、制御装置５として、入力部２０ｆや表示部２０ｇのいずれか一方、又は双方を備えないコンピュータとして構成してもよい。

上記では制御装置５の実施形態を記したが、同じように管理装置４も類似のコンピュータで構成することが可能である。また管理装置４と制御装置５の機能および構成の統合および分散の形態は、後述するように複数ある。

（移動体制御システム１が動作する環境）
図３は、本発明の第１の実施形態における移動体制御システム１が動作する環境の一例を示す図である。図３に示すように、上方から移動領域（移動面）をセンシングするセンサ３の検知範囲に、移動ロボット２、障害物１１、１２が存在している。ここで、移動ロボット２は簡単のため１台のみを示しているが、複数台の移動ロボットを対象としてもよい。またセンサ３の設置位置は移動領域の上方に限らず、移動領域の全域をセンシングできれば、何れの設置位置でもよい。

管理装置４は、移動ロボット２や障害物１１、１２を含まない、センサ３で検知した走行面（例えば床面、地面、軌条等）のみの情報を予め安全状態として記憶し移動ロボット２に送信している。走行面は、移動ロボット２の移動領域であり、障害物が存在しない領域に移動ロボット２が移動する軌道を設定し得る移動面である。

移動ロボット２は、安全状態と現在のセンサ情報に基づき、後述する処理によって安全な領域を抽出し、安全領域に基づいて軌道を生成し、障害物を回避しながら所望の位置へ移動する。センサは、インフラストラクチャー（道路や軌条、橋梁、駐車場などの交通基盤等を含む社会基盤設備）に設置されたセンサである。ここでは、簡単のためセンサが１台である場合を示す。しかし、センサ３は１台に限定する必要はなく、複数台のセンサを用いてよい。また、第３の実施例で後述するがオンボードセンサを利用してもかまわない。

（移動体制御システム１の機能構成）
図４Ａは、本発明の第１の実施形態における移動体制御システム１の機能構成を示すブロック図である。図４Ｂは、本発明の第１の実施形態における安全状態を示す図である。図４Ｃは、本発明の第１の実施形態における現在のセンサデータを示す図である。図４Ｄは、本発明の第１の実施形態における安全領域および非安全領域を示す図である。図４Ｅは、本発明の第１の実施形態における軌道設定の計画の概要を示す図である。

管理装置４の安全状態決定部３１は、センサ３で検出した移動ロボット２が安全に走行できる走行面の点群データ、すなわちセンサ３の死角がない状態のセンサデータを安全状態（移動ロボット２が走行可能な走行面）として安全状態の候補として算出する。

安全状態決定部３１は、安全状態の候補に、センサ３の死角および立体物が含まれるか否かを判定し、死角が存在する場合には死角に対して死角情報を示すタグを付与し、立体物が含まれる場合には立体物に対して物体情報を示すタグを付与する。後述の様に、オペレータは、タグを手掛かりに、当該の死角および立体物が、安全状態に含めてよい死角および立体物であるかを判断できる。すなわち安全状態決定部３１は、オペレータによるタグに基づく安全状態の候補の判断結果に応じて、安全状態を決定または更新する。

管理装置４は、上記安全状態の候補を表示部２０ｇに表示し、安全状態として登録してよいかを図示しないオペレータに問い合わせる。オペレータは、タグを手掛かりに安全状態の候補を確認し、上記走行面のセンサデータを安全状態と許可するかどうか判定する。許可する場合は、オペレータは、入力部２０ｆにその旨を入力する。

安全状態決定部３１は、許可情報に基づき、上記走行面の点群データをボクセルのデータ形式に変換し、安全状態記憶部３３に送信する。一方許可しない場合は、オペレータは、安全状態決定部３１が、入力部２０ｆから受け取った不許可の情報に基づき、センサ３の情報をもとに、安全状態の候補を再度算出し、オペレータに再度問い合わせる。安全状態の更新の際は、差分を強調表示すると、オペレータに差分に対する注意を促すことができ、オペレータは安全状態登録の作業が効率的に行える。

安全状態記憶部３３は、安全状態を図示しないデータベースに格納する。この安全状態を定義、記憶する処理は、移動ロボット２が動作する前に実施する必要がある。しかし、移動ロボットを動作させる前に毎回実施する必要はなく、移動体制御システム１を出荷するタイミングや初回起動するタイミング、保守するタイミング、環境条件が変わったタイミングで実施すると効率的に運用できる。

また安全状態決定部３１は、センサ３によって取得された現在のセンサデータ環境（静的情報）の変化（例えば壁に棚が設置された等の物体の新規設置や配置換えや、草木の成長等）を、センサデータと安全状態の差分から検知し、差分がある場合に安全状態の更新が必要であるか否かを判定し、更新の要不要をオペレータに通知する。またこのタイミングで安全状態の定義および記憶の再処理を、自動またはオペレータの許可に応じて行ってもよい。

なお本実施例では、オペレータが安全状態を最終決定しているが、安全状態決定部３１が、安全状態を自動で生成してもよい。ここで、安全状態として、センサ３によって取得されたセンサデータのうち、死角がない状態、すなわち床面や地面、軌条などを保存する。死角がある状態、例えば（棚等の障害物の裏に死角ができている状態）は安全状態として保存してもよいが、その場合棚の裏に死角があることを記録することが望ましい。こうすることで、移動体制御システム１は、死角からの飛び出しリスクをシステム上で確実に判定し、死角からの飛び出しを考慮した軌道を生成できる。

さらに移動ロボット２が、図示しないオンボードセンサを有する車両であれば、オンボードセンサで死角を見ることで、移動ロボットからみて死角が発生しないように軌道を計画することができ、より安全性を向上できる。センサ３は、障害物１１、１２、および移動ロボット２を含む現在の点群データを安全領域抽出部３４に出力する。

続いて、本発明の一つの特徴である安全領域抽出部３４を説明する。安全領域抽出部３４は、処理の詳細は図５を用いて後述するが、上述した安全状態（図４Ｂ参照）とセンサ３で検出している現在のセンサデータ（図４Ｃ参照）を単位領域（例えば図４Ｄ～図４Ｅの走行面上のグリッドの各グリッドセル）ごとに比較する。そして安全領域抽出部３４は、安全状態とセンサデータの距離が所定値以下であれば安全と判定する一方で、所定値以上である場合は安全でないと判定する。

こうすることで、走行面が検知できている領域は安全領域と判定することができる。さらに、走行面が検知できていない障害物１１、１２が存在する領域、さらに障害物１２によって生じる死角により走行面が検知できない領域は、安全でないと非安全領域と判定される。このようにして図４Ｄに示すように、領域ごとに安全領域および非安全領域の何れであるかが判定される。なお、処理は３次元で処理することが望ましいが、図３では簡単のため、２次元で表している。

続いて、立体物抽出部３５は、安全状態とセンサ情報の差分より、立体物として障害物１１、１２を抽出する。続いて、行動予測部３６は、抽出された立体物の形状や色などから立体物の属性を分類し、この属性と立体物の現在の位置および速度（速さおよびその向き）とに基づき、立体物の将来（例えば、３秒先まで）の位置および速度（速さおよびその向き）を予測する（図４Ｅに示す行動予測の矢印参照）。本実施形態で示す行動予測部３６は、センサ情報をもとに立体物の移動または行動を予測しているが、地図情報（例えば、歩行者エリア、進入禁止エリア）などをもとに移動または行動を予測することで予測精度が向上する。

上記処理と並行して、死角抽出部３７は、安全状態と現在のセンサデータを比較することで、立体物により生じる死角を推定する（図４Ｄの死角情報の斜線部分参照）。

続いて、リスク予測部３８は、死角の範囲や領域の大きさを表す死角情報に基づき、死角から障害物が飛び出してくるリスクを推定する。例えば、死角情報が所定値よりも大きい場合は、人や他のロボットが飛び出すリスクがあると判定する（図４Ｅの飛び出しリスクありの斜線部分参照）。死角情報が所定値よりも小さい場合は、飛び出すリスクがないと判定する。このように、死角の大きさに基づいて、死角からの飛び出しのリスクを判定することが望ましい。リスク予測部３８は、死角に加えて、図示しない地図情報（通路等）に基づいて、飛び出しリスクを予測してもよい。こうすることでより精度の良いリスク推定が可能となる。

以上で算出した安全領域と、立体物の将来の位置および速度と、死角からの飛び出しリスクと、図示しない移動ロボット２の目標位置に基づいて、軌道計画部３９は、安全な領域を通る自車の目標軌道を計画する（図４Ｅ参照）。続いて、制御部４０は、目標軌道に追従するように各種のアクチュエータ６（ステアリング用モータや、推進用のモータ）を制御する。

続いて、安全領域抽出部のフローチャートについて図５を用いて説明する。図５は、本発明の第１の実施形態における安全領域抽出部３４の処理のフローチャートを示す図である。

Ｓ４１では、安全領域抽出部３４は、現在のセンサデータと安全状態の点群データをボクセルのデータ形式に変換する。ここで、現在のセンサデータと安全状態の各センサデータのボクセルを同様のサイズとすることが望ましい。次に、Ｓ４２では、安全領域抽出部３４は、全てのボクセルごとに、現在のセンサデータと安全状態の距離を算出する。ここで現在のセンサデータと安全状態のうちの何れかにボクセルデータが存在しない場合は、距離が無限大となる。

続いて、Ｓ４３では、安全領域抽出部３４は、単位領域ごとに現在のセンサデータと安全状態の距離が所定値以下であるか判定する。単位領域とは、移動ロボット２の走行面（移動領域）を分割した各領域である。単位領域ごとの現在のセンサデータと安全状態の距離とは、各単位領域におけるボクセルごとの現在のセンサデータと安全状態の距離の合計や平均等である。

安全領域抽出部３４は、Ｓ４３がＹｅｓ（距離が所定値以下）である場合はＳ４４に進み、現在のセンサデータの該当単位領域が安全状態であると判定する一方、Ｎｏ（距離が所定値を超過）である場合は、Ｓ４５に進み、現在のセンサデータの該当単位領域が安全状態でないと判定する。

なおＳ４２およびＳ４３は、現在のセンサデータと、安全状態のセンサデータとの差分を検出する手法の一例であり、差分に基づいて移動ロボット２が安全に移動可能であるか否かを検出できれば、他の手法を用いてもよい。

以上説明した移動体制御システム１により、移動ロボット２は立体物や死角からの飛び出しリスクを避け、安全な領域を通って目標位置へ移動することができる。さらに、移動体制御システム１は、システムが安全と考える安全状態を定義し、安全状態と同一と見なせない場合は安全でないと判定するシステムとしている。すなわち、例えば外乱（例：センサ故障、光の差し込み、霧、地震によるセンサの設置向きのずれ等）により、安全状態と定義した走行面と同じ状態を検知できない場合は、安全でない非安全領域と判定することができる。また想定外の物体が障害物として存在する場合においても同様に、安全状態と定義した走行面と同一状態を検知できないことから非安全領域を検出できる。

よって、本実施形態の移動体制御システム１は、従来技術では安全を確保することが難しい、外乱や想定外の物体により認識に影響が出て安全状態が検知できない場合に、システムが想定した安全状態でない非安全領域（危険領域）であると判定することができ、非安全領域を避けて（全ての領域が安全でないと判定した場合は停止するように）ロボットを制御することができる。その結果、システムの安全性を向上させることができる。

なお移動ロボット２にドライバが乗車し、ドライバが移動ロボット２を運転しても構わない。その場合、軌道計画部３９は、図示しないステアリングの操舵角度および移動ロボット２の速度に基づき、移動ロボット２の予測進路を計算する。さらに、軌道計画部３９は、予測進路、安全領域、立体物の将来の位置、移動ロボット２の速度、および、死角からの飛び出しリスクに基づき、移動ロボット２が安全領域を逸脱しないような各アクチュエータの指令値を生成する。以上により、ドライバの運転によって走行する移動ロボット２が、安全領域から逸脱することを防ぐことができる。

（第１の実施形態の変形例）
なお移動体制御システム１は、図４Ａを参照して説明した機能構成に限らず、移動ロボット２、管理装置４、および制御装置５が有する各要素が種々の態様で各装置に配置されていてもよい。

図６Ａは、本発明の第１の実施形態の変形例１における移動体制御システム１Ｂの機能構成を示すブロック図である。移動体制御システム１Ｂは、図４Ａに示す第１の実施形態における移動体制御システム１と比較して、次の点が異なる。すなわち移動ロボット２Ｂは、制御部４０およびアクチュエータ６のみを備える。管理装置４Ｂは、制御部４０を含まない制御装置５Ｂを備える。移動体制御システム１Ｂがこの構成を取ることで、移動ロボット２Ｂは、管理装置４Ｂから、軌道計画部３９によって計画された目標軌道の情報を受信し、受信した目標軌道に従って移動する。

また図６Ｂは、本発明の第１の実施形態の変形例２における移動体制御システム１Ｃの機能構成を示すブロック図である。移動体制御システム１Ｃは、図４Ａに示す第１の実施形態における移動体制御システム１と比較して、次の点が異なる。

すなわち移動ロボット２Ｃは、軌道計画部３９、制御部４０およびアクチュエータ６のみを備える。管理装置４Ｃは、軌道計画部３９および制御部４０を含まない制御装置５Ｃを備える。移動体制御システム１Ｃがこの構成を取ることで、移動ロボット２Ｃは、管理装置４Ｃから、安全領域抽出部３４によって抽出された安全領域、行動予測部３６によって予測された障害物の予測行動、リスク予測部３８によって予測された障害物の死角からの物体の飛び出しリスクの各情報を受信し、受信したこれらの情報に基づいて軌道計画部３９で計画した目標軌道に従って移動する。

なお移動体制御システム１Ｂにおいて、制御装置５Ｂが管理装置４Ｂから独立した構成であってもよい。同様に移動体制御システム１Ｃにおいて、制御装置５Ｃが管理装置４Ｃから独立した構成であってもよい。

本実施形態では、図３に示すように、センサ３が設置された１つの移動領域内を移動ロボット２が移動する場合を示した。しかしこれに限らず、複数の移動領域を繋げ、何れか１つの移動領域に設定される目標位置に向かって複数の移動領域を横断するように軌道を計画する構成を取ることもできる。

（第２の実施形態）
本実施形態では、安全状態を記憶するセンサとして、制御装置５Ｄが外界を認識するためのセンサ３とは別のセンサ６１を利用するように、例えば自動運転の分野における点群地図や高精度地図（High Definition Map）を活用した実施形態である。第１の実施形態と同一の機能を有する部分については、説明を省略する。

（移動体制御システム１Ｄの概略構成）
図７は、本発明の第２の実施形態における移動体制御システム１Ｄの概略構成を示す図である。移動体制御システム１Ｄは、管理装置４Ｄにおいて、センサ６１で取得したデータを基に安全状態の点群データを生成し、制御装置５において、生成した点群データに基づき移動ロボット２を制御する。センサ６１は、３次元の距離センサであるＬｉｄａｒ、ステレオカメラ、ＴｏＦセンサなどが望ましいが、それに限定しない。

（移動体制御システム１Ｄの機能構成）
図８は、本発明の第２の実施形態における移動体制御システム１Ｄの機能構成を示すブロック図である。管理装置４の安全状態決定部３１は、センサ６１で検出した移動ロボット２が安全に走行できる走行面の点群データ、すなわち死角がない状態のセンサデータを安全状態の候補として算出する。なお本実施形態では、安全状態を決定する管理装置４を含む構成とするが、これに限らず、安全状態の情報として第三者が生成した点群地図や高精度地図等を活用してもよい。

安全領域抽出部３４は、センサ３の点群データを、安全状態記憶部３３に保存した安全状態のボクセルデータと同一サイズのボクセルデータに変換し、両者の距離を比較する。距離が所定値以下であれば安全であると判定し、所定値以上であれば安全でないと判定する。

本実施形態の移動体制御システム１Ｄでは、安全状態を生成するセンサ６１と、移動ロボット２の外界を検出するセンサ３が異なる（安全状態のソースとして高精度地図を利用する）場合においても、ボクセルデータ形式に一般化し、現在のセンサデータと安全状態を比較することで、安全領域を抽出する。よって本実施形態によれば、第１の実施形態の効果に加え、第三者が提供する高精度な地図情報を用いて安全状態を決定するので、高精度かつ客観性が高い安全状態を決定でき、安全領域の抽出精度を向上させることができる。

（第３の実施形態）
本実施形態は、移動ロボットの周辺環境を検出するセンサ３を移動ロボットに搭載している実施形態である。第１の実施形態と同一の機能を有する部分については説明を省略する。

（移動体制御システム１Ｅの概略構成）
図９は、本発明の第３の実施形態における移動体制御システム１Ｅの概略構成を示す図である。本実施形態では、センサ３が移動ロボット２Ｅに搭載されている。センサ３の情報は、管理装置４に無線通信を介して入力される。さらに移動ロボット２Ｅは、ＧＮＳＳ（Global Navigation Satellite System）８１を備え、移動ロボット２Ｅの絶対位置をセンシングできるようになっている。屋内では、ＧＮＳＳ８１の代わりにＩＭＵ（Inertial
Measurement Unit）を用いることが望ましい。屋外および屋内に応じてＧＮＳＳ８１およびＩＭＵの何れかが用いられ後述する自己位置推定が実現される。

図１０は、本発明の第３の実施形態における移動体制御システム１Ｅが動作する環境の一例を示す図である。図１０に示すように、ＧＮＳＳ８１により移動ロボット２Ｅの絶対位置を、センサ３により移動ロボット２の周辺状態を検出する構成となっている。移動ロボット２Ｅがセンサ３を有しているため、移動ロボット２が移動することでセンサ３の位置および姿勢が変化する。

図１１は、本発明の第３の実施形態における移動体制御システム１Ｅの機能構成を示すブロック図である。図１１において、安全状態記憶部３３に記憶されている安全状態の情報は、第１または第２の何れかの実施形態と同様に作成されたものである。

先ずＧＮＳＳ８１で検出した移動ロボット２Ｅの絶対位置と、センサ３でセンシングした点群データと、安全状態記憶部３３に記憶された安全状態の点群データとが自己位置推定部１０２に入力される。

なおセンサ３によって取得された移動ロボット２Ｅの周辺の点群データはセンサ座標系で表されており、安全状態の点群データは地面に固定された固定座標系で表されている。先ず、自己位置推定部１０２は、図示しない速度および操舵角情報に基づき、オドメトリにより、計算周期の前の時刻と現在の時刻との相対的な移動量を所定の計算周期（例：１０ｍｓｅｃ）で計算する。

次に、自己位置推定部１０２は、センサ３により取得された移動ロボット２Ｅの周辺の点群データと安全状態の点群データとに基づき、ＩＣＰ（Iterative Closest Point）マッチングなどの手法を用いて、世界座標系における自己位置を所定の計算周期（例：１００ｍｓｅｃ）でマッチングする。

自己位置推定部１０２は、所定の周期（例：２０ｍｓｅｃ）で受信したＧＮＳＳ８１の絶対位置と、ＩＣＰマッチングで算出した移動ロボットのマッチング位置と、オドメトリで算出した計算周期の前の時刻と現在の時刻との相対的な移動量に基づき、例えばカルマンフィルタ等の手法を用いて、世界座標系における移動ロボットの位置および姿勢を算出する。なお、自己位置推定には多くの手法が提案されており、本実施形態以外の方法を採用してもよい。

次に、座標変換部１０３は、世界座標系における移動ロボット位置および姿勢に基づき、センサ３で取得した移動ロボット２の周辺の点群データを、センサ座標系から固定座標系に変換する。それ以降の処理は、第１の実施形態と同様のため省略する。

なお、本実施形態では、安全状態の点群データと、センサで取得した移動ロボット周辺の点群データを固定座標系に変換し比較したが、逆に安全状態の点群データをセンサ座標系に変換し、移動ロボット周辺の点群データと比較してもよい。データ量に比例する計算量などに応じて、変換方法を選択すればよい。

以上のように、センサ３および自己位置推定機能を搭載した移動ロボット２Ｅであっても、推定した自己位置に基づく座標変換によって、安全状態の点群データと、センサ３でセンシングした移動ロボット２Ｅの周辺の点群データとを、同一の座標系で比較し、安全領域を抽出することができる。

（第４の実施形態）
第４の実施形態は、列車１１１を移動体とし、列車１１１が走行する軌条１１２上を移動体の走行面とする実施形態である。本実施形態における移動体制御システム１Ｆは、軌条１１２に沿って設置された複数のセンサ３が出力する情報を基に、安全状態と現在のセンサデータとを比較し、安全を確保しながら走行するシステムである。第１の実施形態と同一の機能を有する部分については、説明を省略する。

図１２は、本発明の第４の実施形態における移動体制御システム１Ｆが動作する環境の一例を示す図である。列車１１１が走行する軌条１１２に沿って、複数のセンサ３および停止限界決定装置１１３の組が連続的に設置されている。センサ３が検知した軌条１１２の現在状態（図１２の縦線縞部分）の出力情報に基づいて、対応する停止限界決定装置１１３が停止限界を生成し、生成された停止限界を超えないようなブレーキパタンが生成され、ブレーキパタンに基づいて列車１１１の走行が制御される。

図１３は、本発明の第４の実施形態における移動体制御システム１Ｆの機能構成を示すブロック図である。停止限界決定装置１１３の安全領域抽出部３４は、安全状態記憶部３３に記憶されている障害物のない軌条の点群情報と、センサ３でセンシングした現在の軌条の点群情報とを領域ごとに比較する。そして安全領域抽出部３４は、ボクセルデータ化した障害物のない軌条の点群情報と現在の軌条の点群情報との距離が所定値以下であるボクセルが、領域内に一定数以上存在する場合に当該領域を安全領域と判定し、領域内に一定数未満存在する場合に当該領域を非安全領域と判定する。

続いて、停止限界決定部１２１は、列車１１１（図１２）の進行方向の情報と、安全領域抽出部３４によって判定された安全領域に基づき停止限界（この位置を超えたら危険であるという位置）を生成する。停止限界決定装置１１３は、列車１１１に搭載されている制御装置５に停止限界を送信する。

制御装置５Ｆに実装されているブレーキパタン計画部１２２は、列車１１１のブレーキ性能と、停止限界決定部１２１によって生成された停止限界とに基づき、停止限界前で停止するための“位置に関連付けられた速度上限のパタン”（以下ブレーキパタンと呼ぶ）を生成する。一方で、走行パタン計画部１２３は、列車１１１の運行ダイヤや軌条１１２のカーブ曲率などで設けられた制限速度の情報に基づき、位置に関連付けられた速度パタン（以下走行パタンと呼ぶ）を生成する。目標速度生成部１２５は、ブレーキパタンと走行パタンをセレクトロー（低い速度を選択する）することで、目標速度パタンを生成する。

追従制御部１２６は、目標速度生成部１２５によって生成された目標速度パタンから現在の位置に対応した目標速度を計算する。そして追従制御部１２６は、目標速度と、速度センサ１２４によって検出された現在の列車の速度とに基づき、目標ノッチを計算する。追従制御部１２６は、モータ駆動システムを含み、計算した目標ノッチに従って、列車１１１を駆動するモータであるアクチュエータ６を制御することで、列車１１１の速度を制御する。

本実施形態では、軌条１１２上を走行する列車１１１において、軌条１１２に沿って連続的に設置されたセンサ３でセンシングした点群データと安全状態の点群データとを比較することで、安全領域を抽出することができる。よって、移動体制御システム１Ｆにより、列車１１１は、軌条１１２上の立体物との衝突位置よりも列車１１１側に停止限界を設け、停止限界前で停止し、事故発生を防ぐことができる。

上述した実施形態は、本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。さらに、上述した複数の実施形態および変形例において、本発明の主旨を変えない範囲内で、装置またはシステム構成の変更や、一部の構成または処理手順の省略や入れ替え、組み合わせを行ってもよい。さらに、図４Ａ、図８、図１１、図１３で説明したブロック図では、制御線や情報線は説明上必要と考えられるものだけを示しており、製品上必ずしも全ての制御線や情報線を示しているとは限らない。実際には殆ど全ての構成が相互に接続されていると考えてもよい。

１…移動体制御システム、２…移動ロボット、３…センサ、４…管理装置、５…制御装置、６…アクチュエータ、１１…障害物（作業員）、１２…障害物（棚）、３１…安全状態決定部、３３…安全状態記憶部、３４…安全領域抽出部、３５…立体物抽出部、３６…行動予測部、３７…死角抽出部、３８…リスク予測部、３９…軌道計画部、４０…制御部、６１…センサ、８１…ＧＮＳＳ、１０２…自己位置推定、１０３…座標変換部、１１１…列車、１１２…軌条、１１３…停止限界決定装置、１２１…停止限界決定部、１２２…ブレーキパタン計画部、１２３…走行パタン計画部、１２４…速度センサ、１２５…目標速度生成部、１２６…追従制御部

Claims (13)

1. 移動領域における移動体の移動を制御する移動体制御システムであって、
   前記移動領域のうち前記移動体が安全に移動可能な領域を定義した第１の点群データで表される情報である安全状態を記憶する安全状態記憶部と、
   センサによって取得された現在の前記移動領域の状態に関する第２の点群データで表される情報である現在状態の該第２の点群データを第２のボクセルデータへ変換し、前記第１の点群データを第１のボクセルデータへ変換し、前記移動領域における単位領域毎に該第１のボクセルデータと該第２のボクセルデータとの距離を比較し、該距離が所定値以下の場合に該単位領域が前記安全状態に該当すると判定し、該距離が該所定値を超過する場合に該単位領域が該安全状態に該当しないと判定することで、該移動領域から前記移動体が安全に移動可能な安全領域を抽出する安全領域抽出部と、
   前記現在状態と前記安全状態とに基づいて前記現在状態から立体物を抽出する立体物抽出部と、
   前記立体物抽出部によって抽出された立体物の属性、現在位置、および速度に基づいて、該立体物の移動または行動に基づく将来位置を予測する行動予測部と、
   前記安全領域と前記将来位置とに基づいて、前記移動体が目標位置まで移動する軌道を計画する、または、前記移動体が該安全領域を逸脱せず移動するように該移動体を駆動するアクチュエータへの指令を計画する軌道計画部と
   を有することを特徴とする移動体制御システム。
2. 前記現在状態と前記安全状態とに基づいて、前記現在状態から前記センサの死角を抽出する死角抽出部と、
   前記死角から物体が飛び出してくる飛び出しリスクを予測するリスク予測部と、をさらに有し、
   前記軌道計画部は、
   前記安全領域と前記将来位置と前記飛び出しリスクとに基づいて前記軌道を計画する
   ことを特徴とする請求項１に記載の移動体制御システム。
3. 前記軌道に追従するように前記移動体の移動を制御する制御部
   をさらに有することを特徴とする請求項１に記載の移動体制御システム。
4. 前記安全領域抽出部は、
   前記移動領域を分割した単位領域ごとに、前記現在状態と前記安全状態との差分に基づいて前記移動体が安全に移動可能であるか否かを判定する
   ことを特徴とする請求項１に記載の移動体制御システム。
5. 前記現在状態に基づいて前記安全状態を決定または更新する安全状態決定部
   をさらに有することを特徴とする請求項１に記載の移動体制御システム。
6. 前記安全状態決定部は、
   前記安全状態を決定または更新する際には、前記現在状態を、前記安全状態の候補として表示部に表示し、
   前記安全状態を更新する際には、更新差分を強調表示する
   ことを特徴とする請求項５に記載の移動体制御システム。
7. 前記安全状態決定部は、
   前記現在状態と前記安全状態とに基づいて前記安全状態の更新が必要であるか否かを判定し、判定結果をオペレータに通知する
   ことを特徴とする請求項５に記載の移動体制御システム。
8. 前記安全状態決定部は、
   前記安全状態の候補に、前記センサの死角が含まれる場合には該死角に対して死角情報を示すタグを付与し、立体物が含まれる場合には該立体物に対して物体情報を示すタグを付与し、
   オペレータによる前記タグに基づく前記候補の判断に応じて、前記安全状態を決定または更新する
   ことを特徴とする請求項５に記載の移動体制御システム。
9. 前記安全状態は、前記センサとは異なる他のセンサによって取得されたセンサ情報に基づいて決定された情報である
   ことを特徴とする請求項１に記載の移動体制御システム。
10. 前記センサは、社会基盤設備に設置されているセンサである
    ことを特徴とする請求項１に記載の移動体制御システム。
11. 前記移動体は、
    前記安全状態記憶部と、
    前記センサと、
    前記安全状態における前記移動体の自己位置を推定する自己位置推定部と、
    前記自己位置に基づいて、前記現在状態と前記安全状態とが同一座標系の情報になるように座標変換を行う座標変換部と、
    前記同一座標系の情報に変換された前記現在状態と前記安全状態とに基づいて前記安全領域を抽出する前記安全領域抽出部と
    を有することを特徴とする請求項１に記載の移動体制御システム。
12. 前記移動体は、前記移動領域である軌条上を走行する列車であり、
    前記移動体制御システムは、
    前記軌条の連続する区間ごとに前記センサと共に設置された停止限界決定装置と、
    前記列車に搭載された制御装置と、を有し、
    前記停止限界決定装置は、
    自装置が設置されている前記軌条の区間において前記列車が安全に走行可能な領域を定義した安全状態を記憶する前記安全状態記憶部と、
    前記センサによって取得された現在の前記軌条の区間の状態に関する情報である現在状態と、前記安全状態とに基づいて、前記軌条の区間から前記列車が安全に走行可能な安全領域を抽出する前記安全領域抽出部と、
    前記安全領域に基づいて前記列車の停止限界を決定する停止限界決定部と、を有し、
    前記制御装置は、
    前記停止限界に基づいて、前記列車が停止限界前で停止するためのブレーキパタンを計画するブレーキパタン計画部と、
    前記ブレーキパタンに基づいて前記列車の速度を制御する追従制御部と、を有する
    ことを特徴とする請求項１に記載の移動体制御システム。
13. 移動領域における移動体の移動を制御する移動体制御システムが行う移動体制御方法であって、
    前記移動体制御システムは、
    前記移動領域のうち前記移動体が安全に移動可能な領域を定義した第１の点群データで表される情報である安全状態を記憶する安全状態記憶部を有し、
    前記移動体制御システムが、
    センサによって取得された現在の前記移動領域の状態に関する第２の点群データで表される情報である現在状態の該第２の点群データを第２のボクセルデータへ変換し、前記第１の点群データを第１のボクセルデータへ変換し、前記移動領域における単位領域毎に該第１のボクセルデータと該第２のボクセルデータとの距離を比較し、該距離が所定値以下の場合に該単位領域が前記安全状態に該当すると判定し、該距離が該所定値を超過する場合に該単位領域が該安全状態に該当しないと判定することで、該移動領域から前記移動体が安全に移動可能な安全領域を抽出し、
    前記現在状態と前記安全状態とに基づいて前記現在状態から立体物を抽出し、
    抽出された前記立体物の属性、現在位置、および速度に基づいて、該立体物の移動または行動に基づく将来位置を予測し、
    前記安全領域と前記将来位置とに基づいて、前記移動体が目標位置まで移動する軌道を計画する、または、前記移動体が該安全領域を逸脱せず移動するように該移動体を駆動するアクチュエータへの指令を計画し、
    前記軌道に追従するように前記移動体の移動を制御する
    各処理を含むことを特徴とする移動体制御方法。

Images