JP2008065755A - 移動装置 - Google Patents

Abstract

【課題】移動装置において、移動障害物が存在する環境下においても高速な移動を可能とすること。
【解決手段】移動装置は、外界情報取得手段３１、障害物検出手段３２、障害物特徴推定手段３３、衝突予測手段３４、回避経路計画手段３５、回避軌道生成手段３６を備える。衝突予測手段３４は、障害物の位置および特徴と移動装置の姿勢情報および軌道情報とから当該障害物との衝突予測をする。回避経路計画手段３５は、衝突が予測された場合、障害物を回避するための障害物の左右もしくは障害物の周囲を通過する経由点を算出し、経由点を通る１つまたは複数の回避経路を計画する。回避軌道生成手段３６は、障害物と干渉することのないように経由点を調整し、かつ移動装置が走行可能な滑らかな回避軌道を生成する。
【選択図】図３

Description

本発明は、移動装置に係り、特に静止もしくは移動している障害物の存在する環境で作業を行う移動装置に好適なものである。本発明は、人やその他の高速な移動装置や、静的もしくは動的な障害物が存在する環境下で、人と同等以上の速度で移動可能な移動装置に利用することができ、自動搬送装置、自動清掃装置、警備ロボット、この他に屋外の不整地を移動する救助装置および探査機や自動運転車両、工場、病院、オフィス、道路、不整地など様々な環境でのサービスロボットなどに利用することができる。

近年、計算技術、センシング技術、通信技術など様々な電子技術の発展に伴い、特定環境で人が行う作業の自動化技術が急速に進歩している。このなか、現在、人の操縦を伴わずに自動的に移動する装置である自律移動装置の実用化開発も盛んに行われている。実用化されている自律移動装置には、自動搬送装置、自動清掃装置、警備ロボットなどがある。これらは基本的に整地かつ整備された環境の中を走行するものであるが、屋外の不整地を移動する救助装置もしくは探査機、道路を走行する自律運転車両、駅やオフィスなど人の存在する複雑な環境で作業を行うロボットなどといった自律移動装置の実用化が期待されている。このような状況の下、作業環境内に障害物がある場合でも自律的にそれを避けて移動することを可能とする障害物回避技術の需要が高まっている。

局所的な作業環境において未知の静止障害物を回避する従来技術には、デジタルグラフ探索法（例えば特許文献１参照）、ステップ探索法（例えば特許文献２参照）、ポテンシャル法（例えば特許文献３参照）などが開示されている。デジタルグラフ探索法は、作業環境を細かいグリッドに分割し、障害物の無いグリッドのみを移動しながら目的地に到達する道順を探索する技術である。ステップ探索法は、移動装置をステップ毎に制御し、次のステップとして障害物の無い領域の点を選択して障害物の回りを迂回する技術である。また、ポテンシャル法は、障害物の存在領域に高いポテンシャル場、目的地に低いポテンシャル場があると仮定し、障害物から斥力と目的地から引力の合力を用いて移動装置を制御する技術である。特に特許文献３では、移動障害物の存在確率をポテンシャル場に反映させた確率ポテンシャル場を用いることにより、移動障害物の回避を可能とすることを開示している。

特開２００３−２６６３４９号公報 特開平７−６４６３３号公報 特開２００３−２４１８３６号公報

前述のように、様々な環境での自律移動装置の活躍が期待されているが、特に、人に代わって作業を遂行するという観点から、人や他の高速な移動装置が存在する環境下で、人と同等以上の速度で移動可能な自律移動装置の要求がある。そのためには高速に移動する障害物を避けながら自らも高速に移動する必要があり、これを実現するには、実時間での障害物検出と回避軌道計算を行い、かつ同時に高速に移動中においても移動装置が安定に走行可能な連続した滑らかな軌道を生成する必要がある。例えば人が通常に歩く程度の約１．３ｍ／ｓ程度の速度で移動する場合、同等の速度で移動する移動装置と人とのすれ違い状況では２ｍ／ｓ以上の速度で互いに接近するので、人と共生するためには、１００〜数１００ｍｓ程度の短い時間で回避軌道を生成し、即座に走行軌道を修正することを可能とする必要がある。

しかしながら、上述のデジタルグラフ探索法やステップ探索法のような従来技術では、静止障害物のみを対象としており、移動障害物に対応できないという問題がある。また、デジタルグラフ探索法やポテンシャル法のような従来技術では、実時間計算に限界があるという問題がある。さらに、上述の従来技術では、移動装置が安定に高速走行可能な滑らかな軌道を生成できないという問題がある。

本発明の目的は、移動障害物が存在する環境下においても高速な移動が可能となる移動装置を提供することにある。

前記目的を達成するために、本発明は以下のような手段により実現する。
（１）静止もしくは移動している障害物の存在する環境下で移動する移動装置において、
外界情報を取得する外界情報取得手段と、前記外界情報取得手段からの外界情報と前記移動装置の姿勢情報とを蓄積した時系列情報から外界に存在する物体を検出し、移動に障害となる可能性のある物体である障害物を抽出する障害物検出手段と、前記障害物検出手段により検出された１つもしくは複数の前記障害物の前記時系列情報から、障害物の大きさまたは形状または障害物の形状変化と障害物の位置および移動パターンとを含む障害物の特徴を推定する障害物特徴推定手段と、前記障害物の位置および特徴と前記移動装置の姿勢情報および軌道情報とから当該障害物との衝突予測をする衝突予測手段と、この衝突が予測された場合、前記障害物を回避するための障害物の左右もしくは障害物の周囲を通過する経由点を算出し、前記経由点を通る１つまたは複数の回避経路を計画する回避経路計画手段と、前記障害物と干渉することのないように前記経由点を調整し、かつ前記移動装置が走行可能な滑らかな回避軌道を生成する回避軌道生成手段と、を具備し、障害物を追跡し、実時間で滑らかな回避軌道を生成するようにする。
これにより１つもしくは複数の移動障害物が存在する環境下においても高速な移動が可能となる。
（２）前記生成した滑らかな回避軌道情報を用いて前記移動装置の状態もしくは操作情報を画像もしくは音声を用いて提示もしくは指示する手段と、前記生成した滑らかな回避軌道情報を用いて前記移動装置の方向制御状態を変更する操舵手段と、前記生成した滑らかな回避軌道情報を用いて走行状態を変更する走行制御手段とのうちの少なくとも一つを具備し、運転者もしくは操作者の支援を行うことにより、自動車などの車両において安全性の向上や実時間での回避シミュレーションが可能になる。
（３）前記衝突予測手段による前記障害物との衝突予測、もしくは前記回避経路計画手段による前記移動装置自身の移動方向、移動速度変更または前記経由点の算出は、前記障害物の位置を固定し、前記移動装置が当該障害物との相対移動ベクトルで移動する空間である障害物固定相対空間を用いて算出するようにする。これにより、経由点の計算が幾何学的に解け、経由点の算出、衝突予測および回避処理が高速に実現できる
（４）前記障害物特徴推定手段は、前記障害物の形状を円または多角形または１つもしくは複数の直線を含む単純図形に近似する手段と、前記障害物の移動パターンを等速直線移動として近似する手段と、前記装置自身の移動パターンを等速直線移動として近似する手段とのうちの少なくとも一つを具備する。これにより、障害物の衝突予測および経由点算出が飛躍的に容易になる。
（５）前記衝突予測手段は、前記障害物と衝突する危険性のある領域および前記障害物と前記移動装置との安全間隔を考慮した領域を用意し、回避の緊急性に応じてこれらの領域を選択して使用する。これにより、回避動作の安全性が高まる。
（６）前記回避経路計画手段は、前記移動装置の空走期間を考慮し、現在の軌道上の所定期間後の位置から新たな回避計画を行うことで、現在の軌道から回避軌道へ滑らかに接続する。これにより現在の軌道から回避軌道への滑らかに接続することが容易になる。
（７）前記回避経路計画手段は、前記複数の回避経路から最終的に回避経路を選択するとき、最短経路、到達時間最短、角度変化最小、速度変化最小、移動障害物の後方を通過すること、回避経路の幅の広さのうちの少なくとも一つを評価して、優先順位を設定する。これにより、最適な経路の選択が可能となる。
（８）前記障害物特徴推定手段は、前記障害物の形状の変化が大きい場合、大きな変化部分を独立した移動障害物として捉えて障害物回避処理を行う。これにより、衝突予測および回避処理が高速に実現できる。
（９）前記回避軌道生成手段は、前記滑らかな回避軌道が経由点を周囲８方向にずらした回避軌道を評価することを繰り返し、曲率の変化が滑らかかつ極小となる経由点を選択することで回避軌道が最も滑らかになるように経由点を移動して探索する。これにより、最適な軌道の生成が可能となる。
（１０）回避中は前記移動装置自身が障害物との干渉を軽減するための姿勢に変更する動作統合管理部を具備する。これにより、衝突危険性が少なくなり、かつ行動範囲が広がる。
（１１）前記回避経路計画手段は、障害物を回避するための最後の経由点以降を、前記移動装置自身の移動目標方向の直線もしくは目的点に接続する。これにより、回避完了後も安定した軌道が生成できる。
（１２）前記障害物検出手段は、前記時系列情報を、前記外界情報を取得した時刻、前記外界情報から検出した物体の位置、大きさおよび形状、その時刻の前記移動装置の自己位置、方向および自己姿勢、異なる時刻の前記検出した物体のデータの関連付け情報により構成する。これにより、障害物の検出、障害物の形状もしくはその変化と移動パターンの処理が実現できる。

本発明の移動装置によれば、移動障害物が存在する環境下においても高速な移動が可能となる。

以下、本発明の複数の実施形態について図を用いて説明する。
（第１実施形態）
本発明の第１実施形態の移動装置を図１から図８を用いて説明する。

まず、本実施形態の移動装置について、図１を用いて簡単に説明する。図１は人の存在する環境下で作業を行うことを目的とした移動装置１の構成を説明する図であり、図１（ａ）はその移動装置１の正面図、図１（ｂ）その移動装置１の側面図である。

移動装置１は、倒立振子型移動ロボットであり、大きく分けて移動機構２と上体３とに２分される。移動機構２は、左右の車輪４、５と、これらを回転駆動する走行モータ６、７とを備える。移動機構２の上部には、移動機構２のＸ，Ｙ，Ｚ軸方向の加速度を検出する３軸の加速度センサ９と、鉛直方向を基準とした姿勢及び鉛直方向周りの方位を検出する姿勢方位センサ８とが設けられている。上体３には、作業用のアーム１３や、対人インターフェース機能を持つ頭部１４などの作業装置が搭載されている。さらに、上体３には、ロボット全体を制御する制御装置１１が内蔵されており、また、走行領域に存在する走行方向の障害物の位置及び形状を計測する外界センサ１２が設けられている。

以上の構成の移動機構２と上体３は、Ｘ軸方向に回転自由度を持つ揺動機構１０で結合されており、揺動機構１０の位置制御により、移動機構２に対する上体３の姿勢を任意に設定できる。

この移動装置１は、人の早歩きの速度と言われる１．６ｍ／ｓ以上の速度で移動することが可能である。そのため、障害物回避処理を非常に高速に実施する必要がある。例えば、同じ速度の障害物が対向している場合、１秒間に３．２ｍ以上近づくことになる。従って、障害物回避処理時間は、空走期間と実際に回避走行する期間とを考慮すると、短い方が良く、例えば１００ｍｓ以下が望ましい。また、移動装置１が２輪で移動するために機敏に方向転換ができるが、安定に高速移動するためには滑らかな走行軌道を生成することが望ましい。

次に、図２を用いて、本実施形態における障害物回避処理部２３とその他の制御部２０〜２２との関係を簡単に説明する。図２は本実施形態の移動装置１の制御装置の構成を示す図である。

本実施形態の障害物回避機能は、障害物回避処理部２３で処理され、動作統合管理部２０によって管理される。

障害物回避処理部２３は、外界センサ１２から移動装置周辺の環境データを逐次受信し、また同時に、動作統合管理部２０から移動装置１の姿勢、位置、軌道などのデータを受け取る。そして、障害物回避処理部２３は、これらのデータを基に移動装置周辺の物体の位置、存在領域、移動速度の計算を行い、この計算結果を基に前記物体との衝突計算を行う。この衝突計算で衝突の可能性がある場合、障害物回避処理部２３は、回避経路および回避軌道の計算を行い、動作統合管理部２０へ衝突可能性の検出データや回避軌道データなどを送信する。

動作統合管理部２０は、各制御部２１〜２３と双方向の通信を行い、ウォッチドッグ管理、時刻管理、データ統合管理、状態遷移シーケンスに従った動作タイミング管理などを行う。例えば、アーム動作と走行動作を連動して行う場合、動作統合管理部２０は、実際に動作した軌道や状態などの情報をアーム制御部２１及び走行制御部２２から逐次受け取り、アーム１３が所定の位置に来たときに走行制御部２２に所定の位置に移動するコマンドを送信し、また所定の走行条件を満たしたときにアーム制御部２１にアーム動作コマンドを送信する。

また、動作統合管理部２０は、障害物回避処理部２３から衝突可能性を示唆する信号を受け取った場合は、現在走行している軌道から、障害物回避処理部２３から送信された回避軌道へ所定のタイミングで切り替え、その軌道データを走行制御部２２に送信する。さらに、動作統合管理部２０は、この他にも、視聴覚処理部や各種外界センサ処理部、さらに音声処理部、環境地図および自己位置管理部、走行データ生成部などの情報統合および管理を行っている。

なお、前記各処理部は説明のため便宜上分割して図示しているが、本発明はこれに限定されるものではない。各処理は、物理的に、１つのプロセッサで行っても、複数に分割しても、または各処理自体が複数のデバイスで処理されていても構わない。

次に、図３から図８を参照しながら移動装置１の障害物回避方法を説明する。

障害物回避処理部２３は、図３に示すように、外界情報取得手段３１で外界情報を取得し、この取得した外界情報を基に障害物検出手段３２で障害物を検出し、この検出した障害物の特徴を障害物特徴推定手段３３で推定し、推定した障害物特徴を基に衝突予測手段３４で衝突の有無を予測し、衝突する場合に回避経路計画手段３５で回避経路を計画し、この計画を基に回避軌道生成手段３６で滑らかな回避軌道を生成する。以下、各手段３１〜３６を具体的に説明する。

まず、図３に示す外界情報取得手段３１は、周囲の物体の大きさや形状と物体までの距離などの外界情報とその情報を取得した時刻情報とを取得する。従って、外界情報取得手段３１は、外部に存在する物体の位置と形状が検出できる外界センサ１２とその制御装置とにより構成される。

外界センサ１２には、レーザレンジファインダ（以下、ＬＲＦという）、画像センサ、光センサ、超音波センサ、ミリ波レーダや、またはそれらを組み合わせたものを利用することができる。前述のように実時間で回避動作を開始する必要があるため、センサ処理時間は小さく抑える必要がある。例えば、ＬＲＦは、赤外線などのレーザをある一点を中心として所定の角度間隔（例えば０．５度間隔）でスキャンし、その各点で物体までの距離を計測するセンサであり、図４（ａ）に示す点列のように、所定の角度θで測定した物体までの距離データｄを得ることが出来る。現在の技術ではＬＲＦを用いると、周囲に存在する物体までの距離データを、約１０ｍｓ程度で得ることが出来る。

そして、図３に示す障害物検出手段３２は、外界情報取得手段３１で取得した前記外界情報を用いて周囲に存在する物体の検出を行い、その物体が障害物かどうか判定する。図４を用いて障害物検出手段３２の処理を簡単に説明する。

まず、図４（ａ）の点のデータ列から物体４３や壁４４を検出するために、例えば距離値ｄの急激な変化点などを検出して、連続する点を纏まりごとに分割し、それぞれセグメント４５、４６とする。図４（ａ）は移動装置４１に搭載された外界センサであるＬＲＦ１２のレーザスキャン面を上から見た図であり、測定されたスキャンデータを２次元座標系にプロットしたものである。

次いで、各セグメント４５、４６の代表位置（重心など）、大きさ、形状などのセグメント情報を計算し、図４（ｂ）に示す初期セグメントリスト４７を作成する。この初期セグメントリスト４７には、スキャンデータの番号など、元のスキャンデータへのリンクも記載する。この初期セグメントリスト４７は、スキャンごとに作成される。

次いで、外界情報を取得した時刻情報から、その時刻の自己情報を動作統合管理部２０から得る。ここで、自己情報とは、環境の所定位置を基準とした座標系（絶対座標系）における自己の位置、方向、速度、加速度、角速度、アームなど自己姿勢、把持あるいは付随しているものの存在領域などである。

次いで、前記初期セグメントリスト４７のデータと前記自己情報とを用い、前記絶対座標系での位置関係、大きさ、形状などの情報を元に、現在見えている物体が、前回もしくは以前のスキャンに見えていたどのセグメントかを判断し、関連付けを行い、その結果を現在のセグメントリスト４８に記載する。

ここでは、図４（ｃ）に示すように、まず、過去のセグメントリストから、前回のセグメントリスト４９の各セグメントの移動速度ベクトルを予測し、その移動速度ベクトルを用いて各セグメントの、前記外界情報を取得した時刻での予測位置を算出する。

次いで、その予測位置に近いセグメントを初期セグメントリスト４７から検索する。さらに、大きさおよび形状を比較する。大きく変化していない場合は、通常のセグメント５０として過去のセグメントとの関連付け情報を付与し、現在のセグメントリスト４８に記載する。もし、大きく変化している場合は、融合あるいは分離が起きている可能性があるので、その近傍のセグメントが該当するかどうか検索する。融合している場合は、融合セグメント５１として、分離している場合は、分離セグメント５２として、現在のセグメントリスト４８に記載する。また、前にある物体の後ろに隠れて見えなくなる（オクルージョン）場合が存在し、予測位置がセグメントの背後にあり、前回存在したセグメントが小さくなる（分離含む）か、もしくは消滅した場合は、オクルージョンセグメント５３としてその予測位置を最新位置としたセグメント情報を現在のセグメントリスト４８に記載する。また、前回存在して今回存在しないセグメントはオクルージョンとし、融合を検索した後にどちらにも該当しない場合には消滅セグメント５４として、その予測位置を最新位置としたセグメント情報を現在のセグメントリスト４８に記載する。消滅セグメント５４は、所定時間内に該当するものが無い場合、リストから削除する。さらに前回存在しない新しいセグメントは、分離を検索した後、該当するものが無い場合は新セグメント５５としたセグメント情報を現在のセグメントリスト４８に記載する。以上の様にして、検出されたセグメントは、前回のセグメントとのリンクが行われ、その移動追跡が常に行われる。

次いで、セグメントの中で、位置と平均速度ベクトルが類似するものは、同一物体として判断し、この関連付けおよび物体の位置などの物体情報５８を記載した検出物体リスト５６を作成する。これは、人の足などの様に、同一物体が分離して検出される点を考慮して行う。この現在の検出物体リスト５６には、過去の検出物体リスト５７も参照し、過去データとの関連を記載する。さらに、これらの情報から、物体の位置を算出する。以上の処理は、外界情報の時間間隔が短い方が、状態変化が少ないため、関連付けが容易となる。

次いで、障害物の検出処理を行う。ここでは、前記検出物体から既知の物体の判定処理を行い、障害物候補リスト５９と非障害物リスト６０を作成する。既知の物体とは、アームなどの自己自身および持ち物などの付帯物や、動作が既知で衝突可能性の無い装置、床、天井、壁、柱などである。ここでは、過去のリストとの関連が対応付けられているものはその情報を用い、それ以外のものは前記動作統合管理部２０からの自己情報と地図データから既知物体の存在領域を計算し、前記検出物体がその領域に存在する場合は既知物体として判定する。障害物候補リスト５９には、壁や柱などの既知の静止障害物、未知静止障害物、既知移動障害物、未知移動障害物がある。

以上の様に、障害物検出手段３２では、過去の履歴からセグメントおよび物体を追跡し、１もしくは複数のセグメントからなる検出物体が移動装置１の移動に障害と成る物体かどうかを判定する。

そして、図３に示す障害物特徴推定手段３３は、障害物候補リスト５９に記載された障害物の存在領域推定処理６１や移動パターン推定処理６２などの障害物特徴の認識処理を図５に示すように行う。ここでは、障害物候補リスト５９に記載された物体の過去のデータを参照して、形状と大きさおよび移動量（ベクトル）などの障害物の時系列情報を用いる。

存在領域の推定処理６１の第１ステップ６１ａでは、まず、複数のセグメントから構成されている障害物の各セグメントについて、位置あるいは特徴点の追跡を行い、回転やオクルージョンなどのために現在見えていないが過去に見えていた部分の補間など、輪郭推定処理を行い、その最大輪郭を算出する。現在見えているセグメントの情報のみでは、例えば人の足の片側にオクルージョンが発生している場合、人である障害物の大きさや位置が、見えている片足のセグメントのみで計算され、実際のものと大きくずれてしまう。従って、このようなオクルージョンなどを補正するために、輪郭予測処理を行い、障害物の大きさや位置を正確に算出する。また、その逆に、それまでの情報からでは予測できない高速な輪郭変化は、その変化部分もしくはセグメントを別の移動物体として分割する。なお、存在領域の推定処理６１では、より正確な情報を取得するために、対象となっている物体の高さの異なるスキャンの外界情報を使用することや、画像センサを使用した外形の輪郭推定結果を融合しても良い。

次いで、存在領域の推定処理６１の第２ステップ６１ｂとして、検出物体を、前記１つもしくは複数のセグメント輪郭を含む円や多角形または直線の集合などで単純図形として近似した障害物仮想存在領域の決定と、その中心位置（代表位置）の決定とを行う。この処理は、この後の障害物回避処理の計算量を減らすために行う。移動パターンの推定処理６２では、前記中心位置（代表位置）を用い、過去の移動ベクトルの変化から今後の移動ベクトルを統計的処理により予測する。この予測移動ベクトルは確率分布として、移動ベクトルの主成分、ベクトル変化成分、誤差成分などから構成される。特に、ベクトル変化があまり大きくないときは、直線移動とその誤差として近似する。ここで、微小時間内では直線移動していると近似するのは、この後の障害物回避処理において計算量を減らすためである。

そして、図３に示す衝突予測手段３４は、前記障害物の特徴情報と前記自己情報を用いて、障害物と移動装置自身（自己）との衝突危険性の予測を図６に示すように行う。ここでは、まず障害物との衝突を安全に避けるために自己が立ち入らない領域を決定し、その領域との干渉がないか検査する。図６は、衝突予測について説明する図であり、以下、図６を用いて説明する。図６（ａ）は自己７０と障害物７１の現在の位置を示している。説明を簡単にするために図では仮想存在領域７２は円で示されている。

まず、前記障害物仮想存在領域７２に、自己の姿勢や付属物に応じた自己の存在領域の中心からの最大距離７３や、すれ違う時に空けておくべき間隔７４などを拡張した領域を衝突危険領域７５とする。例えば、前記障害物特徴推定手段３３で算出した前記移動ベクトルの誤差成分は、すれ違い間隔７４として加算する。自己位置がこの領域に入ると衝突する可能性がかなり高いと考える。但し、自己の存在領域は衝突危険領域７５に加味したので、以降、自己は大きさの無い点７０と考える。

次いで、回避開始タイミングの調整もしくは回避動作の安全性を高めるために、安全距離７６を考慮する。この安全距離７６を考慮した領域を衝突注意領域７７とする。安全距離７６としては、移動速度に応じた余裕領域や歩行者のパーソナルスペースと呼ばれる人が危険と感じない領域を考慮する。図６（ａ）では、この安全距離７６を進行方向に関係なく一定距離としている。また、障害物のパーソナルスペースとして進行方向７８を長軸とした楕円領域７９を考えると、図６（ｂ）に示す衝突注意領域８０となる。自己のパーソナルスペースとして、進行方向８１を長軸とした楕円領域８２を考えると、図６（ｃ）に示す衝突注意領域８３となる。以上の様に自己が立ち入らない領域を考慮すると、歩行者に不安を感じさせない回避が実現できる。前記衝突領域は、障害物について行う。また、この衝突注意領域は複数用意しておき、緊急性などの状況に応じて選択して使用しても良い。

次いで、この領域と自己との干渉を調べることで衝突予測を行う。ここでは、自己点７０を、自己と障害物との相対移動ベクトルで移動させた相対移動軌跡８４が衝突注意領域７７もしくは衝突危険領域７５と干渉が無いか検査することで衝突予測を行う。干渉した場合は、衝突フラグを設定する。ここで、衝突フラグは緊急性もしくは危険性に伴い、多段階に判定する。例えば、衝突までの時間が２秒未満であれば衝突危険フラグ、２秒より大きく５秒未満であれば衝突注意フラグ、それ以上は衝突予報フラグとする。

この検査を全障害物について演算する。ここで、この演算は障害物位置を固定し、自己が相対ベクトルで移動する空間を仮定しているので、この空間を障害物位置固定相対空間と呼ぶ。

衝突検査において特に障害物８５と自己８６ともに直線等速移動する場合を、図６（ｄ）を用いて説明する。この場合、相対移動ベクトル８７の方向が衝突危険領域８６の境界方向８８、８９との間に存在する場合を衝突と判定する。このとき、自己から障害物の位置ベクトルをＰ→９０、相対移動ベクトルの単位成分をＶｅ→９１、接線方向の単位ベクトルをＣｅ→９２、衝突注意領域と境界方向との接点と障害物中心位置までの距離９３をＲｄ、自己８６の位置から衝突危険領域８６への接点９４までの距離をＬｄとしたときの衝突判定式を次の式（１）に示す。

この衝突判定式を満たす場合は、衝突までの時間を計算し、衝突フラグを設定する。このように、障害物８５と自己８６の移動パターンを等速直線移動に近似し障害物位置固定相対空間にて衝突予測処理を行うと非常に高速に演算が可能となる。従って、障害物８５および自己８６の移動ベクトルは、可能な範囲で直線等速移動などの簡単な動作モデルに近似し、誤差量を衝突危険領域の安全距離として加算して衝突計算を行い、計算負荷を軽減すると良い。特に、自己８６の移動に関しては、可能な限り直線等速移動を基本とした計画を行うと、計算時間が短縮でき、かつ予測が容易になる。以下の説明では、障害物もしくは自己の移動ベクトルは直線等速移動として説明する。

そして、図３に示す回避経路計画手段３５は、前記衝突フラグが設定された場合、障害物を回避するために回避経路を計画する。ここでの回避経路とは、移動装置が実際に走行する正確な軌道ではなく、障害物の右もしくは左などの所定の経由点もしくは回避領域を通ることを規定する大まかなものである。

ここでは、まず衝突フラグをチェックし、衝突の緊急性を調べ、対応した回避動作の経路計画を行う。例えば衝突フラグの種類が衝突危険フラグであった場合は、早急に回避動作を開始する必要がある。選択する回避動作としては、その緊急性に伴い、自己の移動方向を衝突注意領域の左もしくは右を通過するように変更、衝突危険領域の左もしくは右を通過するように変更、衝突危険領域に進入しないように減速、衝突危険領域に進入しないように移動して停止などがある。ここで、複数の衝突注意領域を用意している場合は、衝突フラグの種類に応じて選択する必要がある。例えば衝突予報フラグの場合は、その一番大きなものを使用する。このようにすることにより、余裕のある安全な回避が実現できる。

以下に、回避経路の経由点を求める方法について図７を用いて簡単に説明する。

まず、図７（ａ）は、障害物を回避するために自己移動ベクトルを変更する方法の例を示している。図７（ａ）では、障害物が１つの場合の、ある時刻の障害物位置固定相対空間である。自己１００は単位時間移動ベクトル１０１で移動し、障害物１０２は単位時間移動ベクトル１０３で移動している。従って、単位時間相対移動ベクトル１１１は、自己１００の単位時間移動ベクトル１０１を平行移動したベクトル１０８と、障害物１０２の単位時間移動ベクトル１０３を平行移動して方向を逆にしたベクトルＳ→１０７との合成ベクトルとなる。障害物１０２を回避するためには、単位時間相対移動ベクトル１１１の先端を、自己１００と障害物１０２の衝突注意領域１０４との接線１０５、１０６の外側になるように変更すると良い。例えば自己１００が移動速度を維持して方向変換のみで回避する場合、自己の移動ベクトル１０８は、ベクトルＶ→１０９に変更する。ここで、接線方向１０５に接する方向の単位ベクトルをＣ→１１０とすると、変更後の自己の単位時間移動ベクトルＶ→１０９は、次の式（２）で表される。

図７（ｂ）は２つの障害物の衝突までの時間が接近している場合を想定し、２つの障害物を同時に回避する例を示している。この場合も障害物が１つの場合と同様にして回避ベクトルを決定する。図７（ｂ）には、移動速度を維持する回避ベクトル１１４、１１５、加速して回避する回避ベクトル１１６、減速して回避する回避ベクトル１１７を示しているが、元の自己の単位移動ベクトル１１３に対し、変更が容易なものを選択する。以上のように、自己の回避移動ベクトルは、障害物位置固定相対空間を用いて決定する。

次いで、絶対空間上の経由直線および経由点を求める。図７（ｃ）は、現在の自己位置１２１と、現在の障害物位置１２３、最接近時の自己位置１２０と最接近時の障害物位置１２２の、絶対空間上の位置関係を示している。ここでは、まず、前記相対空間上で障害物との最接近点（例えば図７（ａ）の点１１２）に到達する時間を求める。そのときの絶対空間上の回避移動ベクトル１１８から、図７（ｃ）に示すように絶対空間上の回避直線１１９と最接近点１２０を求める。ここでは、この最接近点１２０を経由点とする。但し、経由点は、以降の軌道に依存し、最終的には軌道生成時に決定される。

これ以降、次の障害物を回避する必要がある場合は、この経由点を始点として、前記処理を繰り返し行う。このように、複数の障害物が移動環境に中に散在している場合は、まず考慮に入れる障害物の限定と、障害物を回避する順序の決定とを行う必要がある。

この考慮に入れる障害物の限定は、自己と障害物との距離、自己と障害物とが接近するまでの時間、などにより決定する。例えば、すべての障害物について、前記の相対位置空間において、自己が現在の速度を維持した場合、相対移動ベクトルが衝突危険領域の境界線に接するための自己の回避移動ベクトルおよび衝突までの時間を算出し、その時間が１０秒あるいは５秒以内の障害物のみ衝突可能性のある障害物として考慮する。

次いで、障害物を回避する順序の決定を行い、その順序にて、前記経由点と回避経路の算出を行う。順序の決定は、衝突可能性のある障害物につき、全通りの可能性を検索しても良いし、衝突までの時間が最も短い障害物を優先して検索し、検索途中で適当な回避経路が見つかった時点で打ち切っても良い。例えば、図７（ｄ）に示すように、移動装置が開始点１２５から目標点１２６までの間に、障害物１２７、１２８、１２９が存在する場合、障害物１つにつき、左右２点の経由点が存在するので、回避順序の組み合わせは１９２通り存在する。但し、経由点が他の障害物の危険領域上に存在する場合、経由点から直接目標点に到達できる場合は以降の組み合わせは探索しない。例えば回避経路１３０の回避順序は、障害物１２７の右、障害物１２８の右、目標点である。同様に回避経路１３１は、障害物１２７の左、障害物１２８の右、目標点の順となり、回避経路１３２は、障害物１２９の左、目標点の順となる。経由点１３３からは目標点へ直接到達できるので、障害物１２９の左右の経由点を通る経路は探索しない。

ここでは、移動障害物について示したが、静止障害物も同様に処理する。また目標点の代わりに目標方向に接続されることもある。目標点あるいは目標方向は、動作統合管理部２０が設定する。以上のようにして、複数の回避可能な経路の候補を算出する。

このようにして、障害物を回避する左右およびその周辺の点を経由点としてそのいずれかを通る複数の経路が生成される。次いで、これらの経路について、最短距離、角度変化最小、速度変化最小、到達時間最短、移動障害物の後方を通過すること、経路幅の広さなど幾つかの評価項目に従い、順位付けする。

そして、図３に示す回避軌道生成手段３６は、前記の選択された回避経路の各経由点の近傍を通り、移動装置が走行可能な滑らかな回避軌道を生成する。図８は本実施形態の回避軌道生成を説明する図である。ここでは経由点１４０もしくはその近傍を通る滑らかな曲線１４１、経由点１４２を通る曲線１４４、もしくは経由点１４２の近傍を通る滑らかな曲線１４３を生成する。前記回避経路は逐次更新されるので、回避軌道は、例えば直近の数秒程度の経路につき行う。本発明は曲線の関数生成方法を限定するものではないが、曲線の生成は、一般的に知られているスプライン関数、ペジェ関数、各点の拘束条件および曲率変化の拘束条件などを用いた高次関数、移動装置の制御モデルに従った曲線にて生成する。

生成した曲線は、実空間の衝突注意領域もしくは衝突危険領域との干渉を検査する。衝突計算は、前述の相対空間で実施する。衝突注意領域は安全領域が十分大きく取られているので、多少の干渉は問題とならないが、衝突危険領域に干渉する場合は、経由点を移動して新たな経由点１４５を用いて再度曲線を生成する。このとき、同時にその曲線が実際に走行できるか確認するために、移動装置のモデルに基づき、曲率制限、曲率変化の制限、速度制限などの条件を満たしているか検査する。経由点の移動は、周囲８方向に僅かにずらした経由点を用いた回避軌道を評価し、曲率の変化が滑らかかつ極小となる経由点を選択しても良い。計算量を減らすため、制約条件の検査は主に経由点の近傍で行う。これらの処理を複数回繰りかえし、収束しない場合は、前記複数経路の他の経路を選択して、再度軌道生成を行う。生成された軌道は、動作統合管理部２０を通して、走行制御部２２に送られる。走行制御部２２は、実際に移動装置がこの軌道に追従するための制御を行う。

また、現在の位置から回避経路を計画しても、空走期間の存在により計画した軌道の追従が困難となるのを避けるため、図８では、現在位置１４６に対して、所定時間後の位置１４０から次の回避計画を行っている。このとき、軌道生成手段は、所定の空走期間の位置１４７から新たな回避経路１４８へ接続する滑らかな曲線を生成する。この方法により、簡単に現在の走行軌道から新たな回避軌道へ滑らかに接続する曲線が生成できる。以上の様に経由点を基に、滑らかな軌道を生成することで、高速移動時においても安定かつ確実に計画した軌道上を走行できる。

さらには、図２に示す動作統合管理部２０は回避実行制御を行っており、衝突フラグと回避軌道が設定されると、規定の軌道から回避軌道へ切り替えを行う。回避動作の実行状況を監視し、回避が完了すると次の目的地までの経路に切り替える。また、回避中、動作統合管理部２０は、自己状態を障害物との衝突を避けるために安全姿勢に姿勢変更することも行う。

以上、本実施形態によれば、移動装置１において、実時間で高速移動する障害物との衝突予測、回避経路の計算、実現可能な回避軌道の生成が可能になる。また、滑らかな回避軌道が実現可能となる。従って、移動装置１において、高速な移動障害物の回避が可能となり、高速な移動障害物が存在する環境下においても高速な移動が可能となる。
（第２実施形態）
次に、本発明の第２実施形態について図９を用いて説明する。図９は本発明の第２実施形態の移動装置１５の説明図である。この第２実施形態は、次に述べる点で第１実施形態と相違するものであり、その他の点については第１実施形態と基本的には同一であるので、重複する説明を省略する。この第２実施形態のものにおいて、第１実施形態と共通する構成においては同じ効果を奏するものである。

この第２実施形態における移動装置１５は、障害物１７を検出するセンシング領域１９を持つ外界センサ１６を有し、検出した障害物１７を安全に回避するための回避動作を行う自動車の例である。このときの回避動作とは、自動的に回避を行う以外に、画像や音による回避経路および操舵角や速度などの提示もしくは指示、さらには操舵装置や走行制御装置の直接的もしくは間接的な制御などの運転支援も含まれる。ここで間接的制御とは、たとえば操舵装置の負荷制御を行い、運転者に安全な操作を誘導することを示す。

この第２実施形態の移動装置１５も前記移動装置１と同様に、移動障害物が存在する環境の中で高速に移動するため、障害物１７との衝突を回避するための回避動作を演算する処理を高速に行い、かつ滑らかな回避軌道１８を計算する必要がある。特に、自動車は３０ｍ／ｓ程度の速度が出るため、急な方向転換は危険であり、滑らかな走行軌道を計算する課題は非常に重要である。また、速度が速いので単位時間に移動できる範囲が広くなり、外界センサ１６は前述の外界センサ１２よりも広い範囲１９を検出する必要があり、同時に広範囲の環境について回避処理計算を行わなければならず、計算時間の短縮も重要な課題となる。

この第２実施形態はこれらの課題を解決するためになされたものであり、第２実施形態における移動装置１５も移動装置１の場合と同様に本発明の適用範囲となる。これにより、運転者は移動装置を操作する際に、安全性の高い操作が可能となる。

従って、前記課題を有している移動装置は本発明が利用可能であるが、それ以外にも実際に移動を伴わないが擬似的な移動を行うシミュレータに類推される装置も本発明の適用範囲となる。

本発明の第１実施形態の移動装置の構成を説明する図である。 第１実施形態の移動装置の制御装置の構成を示す図である。 第１実施形態の障害物回避機能を実現するための処理の流れを示す図である。 第１実施形態の障害物検出手段の処理を説明する図である。 第１実施形態の障害物特徴推定手段の処理について説明する図である。 第１実施形態の衝突予測について説明する図である。 第１実施形態の経由点を求める方法について説明する図である。 第１実施形態の回避軌道生成を簡単に説明する図である。 本発明の第２実施形態の移動装置を説明する図である。

符号の説明

１…移動装置、２…移動機構、３…上体、４，５…車輪、６，７…走行モータ、８…姿勢方位センサ、９…加速度センサ、１０…揺動機構、１１…制御装置、１２…外界センサ、１３…アーム、１４…頭部、１５…移動装置、１６…外界センサ、１７…障害物、１８…回避軌道、１９…センシング領域、４１…移動装置、４２…ＬＲＦ、５０…セグメント、５１…融合セグメント、５２…分離セグメント、５３…オクルージョンセグメント、５４…消滅セグメント、５５…新セグメント。

Claims (12)

1. 静止もしくは移動している障害物の存在する環境下で移動する移動装置において、
   外界情報を取得する外界情報取得手段と、
   前記外界情報取得手段からの外界情報と前記移動装置の姿勢情報とを蓄積した時系列情報から外界に存在する物体を検出し、移動に障害となる可能性のある物体である障害物を抽出する障害物検出手段と、
   前記障害物検出手段により検出された１つもしくは複数の前記障害物の前記時系列情報から、障害物の大きさまたは形状または障害物の形状変化と障害物の位置および移動パターンとを含む障害物の特徴を推定する障害物特徴推定手段と、
   前記障害物の位置および特徴と前記移動装置の姿勢情報および軌道情報とから当該障害物との衝突予測をする衝突予測手段と、
   この衝突が予測された場合、前記障害物を回避するための障害物の左右もしくは障害物の周囲を通過する経由点を算出し、前記経由点を通る１つまたは複数の回避経路を計画する回避経路計画手段と、
   前記障害物と干渉することのないように前記経由点を調整し、かつ前記移動装置が走行可能な滑らかな回避軌道を生成する回避軌道生成手段と、を具備することを特徴とする移動装置。
2. 請求項１記載の移動装置において、前記生成した滑らかな回避軌道情報を用いて前記移動装置の状態もしくは操作情報を画像もしくは音声を用いて提示もしくは指示する手段と、前記生成した滑らかな回避軌道情報を用いて前記移動装置の方向制御状態を変更する操舵手段と、前記生成した滑らかな回避軌道情報を用いて走行状態を変更する走行制御手段とのうちの少なくとも一つを具備することを特徴とする移動装置。
3. 請求項１または２記載の移動装置において、前記衝突予測手段による前記障害物との衝突予測、もしくは前記回避経路計画手段による前記移動装置自身の移動方向、移動速度変更または前記経由点の算出は、前記障害物の位置を固定し、前記移動装置が当該障害物との相対移動ベクトルで移動する空間である障害物固定相対空間を用いて算出することを特徴とする移動装置。
4. 請求項１または２記載の移動装置において、前記障害物特徴推定手段は、前記障害物の形状を円または多角形または１つもしくは複数の直線を含む単純図形に近似する手段と、前記障害物の移動パターンを等速直線移動として近似する手段と、前記移動装置自身の移動パターンを等速直線移動として近似する手段とのうちの少なくとも一つを具備することを特徴とする移動装置。
5. 請求項１または２記載の移動装置において、前記衝突予測手段は、前記障害物と衝突する危険性のある領域および前記障害物と前記移動装置との安全間隔を考慮した領域を用意し、回避の緊急性に応じてこれらの領域を選択して使用することを特徴とする移動装置。
6. 請求項１または２記載の移動装置において、前記回避経路計画手段は、前記移動装置の空走期間を考慮し、現在の軌道上の所定期間後の位置から新たな回避計画を行うことで、現在の軌道から回避軌道へ滑らかに接続することを特徴とする移動装置。
7. 請求項１または２記載の移動装置において、前記回避経路計画手段は、前記複数の回避経路から最終的に回避経路を選択するとき、最短経路、到達時間最短、角度変化最小、速度変化最小、移動障害物の後方を通過すること、回避経路の幅の広さのうちの少なくとも一つを評価して、優先順位を設定することを特徴とする移動装置。
8. 請求項１または２記載の移動装置において、前記障害物特徴推定手段は、前記障害物の形状の変化が大きい場合、大きな変化部分を独立した移動障害物として捉えて障害物回避処理を行うことを特徴とする移動装置。
9. 請求項１または２記載の移動装置において、前記回避軌道生成手段は、前記滑らかな回避軌道が経由点を周囲８方向にずらした回避軌道を評価することを繰り返し、曲率の変化が滑らかかつ極小となる経由点を選択することで回避軌道が最も滑らかになるように経由点を移動して探索することを特徴とする移動装置。
10. 請求項１または２記載の移動装置において、回避中は前記移動装置自身が障害物との干渉を軽減するための姿勢に変更する動作統合管理部を具備することを特徴とする移動装置。
11. 請求項１または２記載の移動装置において、前記回避経路計画手段は、障害物を回避するための最後の経由点以降を、前記移動装置自身の移動目標方向の直線もしくは目的点に接続することを特徴とする移動装置。
12. 請求項１または２記載の移動装置において、前記障害物検出手段は、前記時系列情報を、前記外界情報を取得した時刻、前記外界情報から検出した物体の位置、大きさおよび形状、その時刻の前記移動装置の自己位置、方向および自己姿勢、異なる時刻の前記検出した物体のデータの関連付け情報により構成することを特徴とする移動装置。

Images