JP5275152B2 - 走行方向制御装置および移動体 - Google Patents

Description

本発明は、自律走行する移動体の走行方向を制御する技術に関する。

移動体が、与えられた経路を追従し、目的地に到達するためには、以下の２つの機能が必要となる。

（１）目的地および通過点などから与えられる経路を追従する機能
（２）移動体の周囲に存在する障害物を回避する機能
移動体は、通過すべき理想的な経路が与えられ、理想的な経路の走行を試みる。移動体が理想的な経路を走行中、あらかじめ知ることのできない障害物を検出した場合には、衝突を回避する必要がある。移動体は、経路を修正し、障害物を回避したのち再び理想的な経路に復帰する。このような制御を繰り返し、目的地に到達する。

従来、機能（１）については、連続関数で記述された経路に対して非線形制御理論を適用し、誤差なく追従する方法が非特許文献１に示されている。非特許文献１では、また、通過点または通過点を結ぶ線分と移動体の位置から目標方向を定め、これに追従するように方向を制御する方法が示されている。以下、後者の方法を単にＬｉｎｅ Ｏｆ Ｓｉｇｈｔ（ＬＯＳ）と呼ぶ。

機能（２）については、移動体の周囲をメッシュ状に分割し、障害物の有無により回避経路をＡ＊探索アルゴリズムなどのグラフ探索手法を用いて決定する方法が知られている。また、障害物からは斥力が、目標位置からは引力が発生すると設定することでポテンシャルの勾配方向に移動体の経路を設定する方法が知られている。

特許文献１では、障害物を検出した場合に新たな通過点を設定し、先に与えられた理想的な経路を修正してこれに追従するように車両を制御する技術が示されている。さらに、障害物と移動体の幅を考慮し、通過可能な方向に経路を修正する方法が特許文献２および非特許文献２に示されている。

以上説明したように、さまざまな手法が提案されているが、実際の運用に際しては、前述の機能（１）および機能（２）を組みあわせることとなる。組み合わせによっては計算手順の複雑化やメモリ使用量が増加する。アルゴリズムの複雑化に伴い移動体の動作の予測が困難となる。

非特許文献２には目標経路を追従するための通常走行モードおよび障害物回避をするための回避モードを切り替える方法が開示されている。しかし、経路追従あるいは障害物回避のどちらを重視し、どのような選択、制御するかについては開示されていない。

特許第３８４４２４７号公報 特開２００８−２３４４４７号公報

Ｔ．Ｉ．Ｆｏｓｓｅｎ、"Ｍａｒｉｎｅ Ｃｏｎｔｒｏｌ Ｓｙｓｔｅｍｓ"、Ｍａｒｉｎｅ Ｃｙｂｅｒｎｅｔｉｃｓ（２００２） 油谷 篤志、他４名、"落ち葉の多い路面における測域センサ情報に基づく障害物回避"、ロボットメカトロニクス講演会２００８ 資料２Ｐ２−Ｄ２０

本発明は上記の問題に鑑み、経路追従あるいは障害物回避という２つの機能を状況に応じて選択し、より目的に沿った自律走行を可能とすることを課題とする。

課題を解決するための手段および発明の効果

本発明の走行方向制御装置は、移動体の現在位置を計測する位置計測部と、前記移動体が向いている現在の方向を計測する方向計測部と、前記移動体の現在位置を基準として障害物の方向および距離を計測する障害物計測部と、前記移動体の出発点と、前記移動体が前記出発点に位置するときに目的地として設定される第１の目的地との位置情報を記憶する目的地記憶部と、前記位置計測部と前記方向計測部と前記障害物計測部と前記目的地記憶部との出力に基づいて前記移動体の目標方向を算出する自律走行制御部と、前記自律走行制御部において算出された前記目標方向に従い、前記移動体の走行方向を制御する走行方向制御部とを備える。

前記自律走行制御部は、前記自律走行制御部が前記第１の目的地を前記移動体の目的地に設定している場合、前記位置計測部と前記目的地記憶部との出力に基づいて、現在位置を中心として所定の半径を有する円と、前記第１の目的地と前記出発点とを結ぶ線分との交点のうち前記第１の目的地に近い方の点の方向を第１の方向に決定する第１方向決定部と、前記障害物計測部の出力に基づいて前記障害物までの距離が第１しきい値を超える方向の集合である第２方向集合を決定する第２方向決定部と、前記第１方向と前記第２方向集合とから前記目標方向を決定する目標方向決定部とを備える。

前記目標方向決定部は、前記移動体の現在の方向における前記障害物までの距離が前記第１しきい値以下の場合には、現在の方向との差が最小となる前記第２方向集合から選択された第３方向と、前記第１方向との差が最小となる前記第２方向集合から選択された第４方向とを決定し、現在の方向と前記第３方向との差が前記第１方向と前記第４方向との差よりも小さい場合は前記第３方向を前記目標方向として決定し、現在の方向と前記第３方向との差が前記第１方向と前記第４方向との差よりも大きい場合は前記第４方向を前記目標方向として決定する第１決定部を含む。

経路追従と現在方位角の保持とを適切に選択し、無理のない走行を維持して目的地へ向かうことができる。

本発明の好ましい実施の形態において、前記目標方向決定部は、現在の方向と前記第１方向との差が第２しきい値を下回る場合であって、前記移動体の現在の方向における前記障害物までの距離が前記第１しきい値を超える場合には、前記第４方向を前記目標方向として決定する第２決定部を含む。

前方に障害物が存在しない場合には、精度よく経路を追従できる。

本発明の好ましい実施の形態によれば、走行方向制御装置はさらに、現在、障害物を回避走行中であるか否かを設定する回避フラグを管理する回避フラグ管理部を備え、前記自律走行制御部は、前記第２方向集合の中で現在方向との差が第３しきい値以下となる方向の集合である第５方向集合を決定する第５方向決定部を含み、前記目標方向決定部は、現在の方向と前記第１方向の差が第２しきい値以上の場合であって、かつ、前記移動体の現在方向における前記障害物までの距離が前記第１しきい値を超え、さらに、前記回避フラグがＯＮである場合には、現在方向との差が最小となる前記第５方向集合から選択された第６方向を前記目標方向として決定する第３決定部を含む。

障害物の回避中には、前方に障害物がない場合であっても、現在方位角が経路に追従するための方位角に近づくまでは緩やかに針路変更が行われる。

本発明の好ましい実施の形態によれば、前記目標方向決定部は、現在方向と前記第１方向の差が第２しきい値以上の場合であって、かつ、前記移動体の現在方向における前記障害物までの距離が前記第１しきい値を超え、さらに、前記回避フラグがＯＦＦである場合には、前記第４方向を前記目標方向として決定する第４決定部を含む。

障害物の回避中でない場合には、精度よく経路を追従できる。

自律走行制御により移動体が走行するルートを示す図である。 実施の形態に係る移動体のブロック図である。 通過点情報テーブルを示す図である。 自律走行制御部のブロック図である。 局所座標系とＬＯＳアルゴリズムにより決定される初期目標方向θｓを示す図である。 自律走行制御のフローチャートである。 ＬＯＳ半径Ｒと初期目標方向θｓとの関係を示す図である。 ＬＯＳ半径Ｒと初期目標方向θｓとの関係を示す図である。 現在方向θｃ、初期目標方向θｓ、最終目標方向θｄおよび車体幅Ａを示す図である。 各候補方向における走行可能距離Ｄｉを示す図である。 各候補方向と走行可能距離Ｄｉとを表すテーブルを示す図である。 最終目標方向θｄを決定するためのフローチャートである。 最終目標方向θｄとして選択される方向ΨおよびΨｓを示す図である。 しきい値Ｌと走行可能距離Ｄｉとの関係を示す図である。 しきい値Ｌと走行可能距離Ｄｉとの関係を示す図である。 ステップＳ３０４において選択される最終目標方向θｄを示す図である。 ステップＳ３０６において選択される最終目標方向θｄを示す図である。 ステップＳ３０７において選択される最終目標方向θｄを示す図である。 ステップＳ３１０において選択される最終目標方向θｄを示す図である。 ステップＳ３１１において選択される最終目標方向θｄを示す図である。 ステップＳ３１３において選択される最終目標方向θｄを示す図である。 ステップＳ３１４において選択される最終目標方向θｄを示す図である。 重み付け係数ｗと最終目標方向θｄとの関係を示す図である。 重み付け係数ｗと最終目標方向θｄとの関係を示す図である。 周囲を障害物で包囲された場合に移動体の動作を制御するフローチャートである。

｛１．自律走行制御の概要｝
以下、図面を参照しつつ本発明の実施の形態について説明する。図１は、本実施の形態に係る自律走行制御に基づいて移動体１が走行するルートを示す図である。移動体１は、出発点３１を出発し、最終目的地３６を目指して走行する。移動体１は、途中、通過点３２、３３、３４、３５を通過する。移動体１は、理想的には、出発点３１と通過点３２、通過点３２と通過点３３、通過点３３と通過点３４、通過点３４と通過点３５、通過点３５と最終目的地３６とを結ぶ直線上を走行する。つまり、移動体１は、出発点３１と通過点３２とを結ぶ線分４１、通過点３２と通過点３３とを結ぶ線分４２、通過点３３と通過点３４とを結ぶ線分４３、通過点３４と通過点３５とを結ぶ線分４４、通過点３５と最終目的地３６とを結ぶ線分４５上を走行する。

しかし、実際の環境では、理想的に線分４１〜４５上のルート、つまり理想ルートを走行できるとは限らない。理想ルート上には、予期していなかった状況が発生する。図１には、理想ルート上に障害物２、３が出現している状態を示している。移動体１は、後述する自律走行アルゴリズムに従い、障害物２および障害物３を回避しつつ最終目的地３６を目指して走行する。

｛２．システムの全体構成｝
本実施の形態の移動体１は、車体の左右に独立して駆動可能な車輪を備える差動二輪型の車両である。左右の車輪にはそれぞれモータが取り付けられており、独立して車輪速度を制御することができる。左右の車輪の回転速度を調整することで、走行速度および走行方向の設定を行うことができる。また、左右の車輪の回転速度を調整することで、移動体１に旋回動作を行わせることができる。

図２は移動体１のブロック図である。移動体１は、自律走行制御部１１、ＤＧＰＳ（Ｄｉｆｆｅｒｅｎｔｉａｌ Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）１２、慣性計測装置１３、レーザレンジファインダ１４、通過点情報記憶部１５、車輪制御部１６および左右の車輪１７，１７を備えている。

ＤＧＰＳ１２と慣性計測装置１３とは外界情報計測部として機能する。ＤＧＰＳ１２は、移動体１の現在の緯度・経度情報を取得する。精度はやや劣るものの通常のＧＰＳを利用することもできる。慣性計測装置１３は、移動体の方向、加速度および角速度を計測するＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）である。

車輪制御部１６は、左右の車輪１７，１７の駆動制御を行う機能とあわせて、内部情報計測部としての機能を備える。車輪制御部１６は、車輪１７，１７を駆動するモータに取り付けられたパルスエンコーダから車輪１７，１７の回転速度を算出する。車輪制御部１６により得られた左右の車輪１７，１７の回転速度から、オドメトリと呼ばれる手法を用い、移動体１の現在位置を算出することができる。

レーザレンジファインダ１４は、障害物計測部として機能する。レーザレンジファインダ１４は、移動体１から周囲の障害物までの距離、移動体１から見た障害物の方向を計測する。本実施の形態ではレーザレンジファインダ１４は、車体前部に取り付けられている。レーザレンジファインダ１４は、様々な方向に対して順に赤外線レーザーを照射し、障害物２からの反射光を解析することで、障害物２までの距離と方向を計測する。

通過点情報記憶部１５は、出発点３１、通過点３２〜３５および最終目的地３６の位置情報（たとえば、緯度・経度情報）を記憶した記憶媒体であり、本実施の形態ではフラッシュメモリを利用している。

図３は、通過点情報記憶部１５に格納された通過点情報テーブルを示す図である。Ｎｏ１、２、・・・は、それぞれ出発点３１、通過点３２〜３５および最終目的地３６に付与されたＮｏである。通過点情報テーブルは、出発点３１、通過点３２〜３５および最終目的地３６の緯度・経度情報（座標）を記録している。

自律走行制御部１１は、外界情報計測部、内部情報計測部、障害物計測部から出力された情報と、通過点情報記憶部１５に記憶された通過点情報とから移動体１の目標速度および目標角速度を算出する。

車輪制御部１６は、自律走行制御部１１から出力された目標速度および目標角速度を左右車輪速度に変換し、目標車輪速度に追従するようにモータを制御する。

｛３．自律走行制御部の詳細｝
図４は、自律走行制御部１１をより詳細に説明するためのブロック図である。自律走行制御部１１は大きく分けてナビゲーション部、ガイダンス部、制御部の３つのブロックで構成される。

＜３−１．ナビゲーション部＞
ナビゲーション部は、拡張カルマンフィルタ（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｅｒ、ＥＫＦ）１１６を備える。

ＤＧＰＳ１２および慣性計測装置１３から得られた情報は、図５に示すように、北方向をｘ軸、東方向をｙ軸とする局所平面座標系へ変換される。ＤＧＰＳ１２から得られた緯度・経度情報は、局所平面座標系における座標（Ｘ，Ｙ）へ変換される。慣性計測装置１３から得られた移動体１の方向は、ｘ軸方向を０とし時計回りに正の値をとる方向θ（ｒａｄ）へ変換される。

車輪制御部１６で計測された左右の車輪１７，１７の回転速度、慣性計測装置１３で計測された移動体１の現在の方向θ、ＤＧＰＳ１２で計測された移動体１の現在の座標（Ｘ，Ｙ）は、拡張カルマンフィルタ１１６に入力される。拡張カルマンフィルタ１１６は、入力情報に基づいて推定情報（Ｘｃ，Ｙｃ，θｃ）を算出する。Ｘｃ，Ｙｃ，θｃは、それぞれ推定された現在の移動体１のｘ座標、ｙ座標、方向である。移動体１の現在の座標（Ｘｃ，Ｙｃ）は、ＤＧＰＳ１２から得られた緯度・経度情報と左右の車輪１７，１７の回転速度から予測される。移動体１の現在の方向θｃは、慣性計測装置１３から得られた現在の方向θと左右の車輪１７，１７の回転速度から予測される。拡張カルマンフィルタ１１６は、数１式で示される差動二輪車両の運動学モデルに基づいて適用される。

数１式において、ｔはサンプリング時刻、Ｔ（ｓ）はサンプリング周期、ｖは前進速度、ωは角速度である。

＜３−２．ガイダンス部＞
ガイダンス部は、目標方向算出部１１１を備える。目標方向算出部１１１は、大域的な経路追従と局所的な障害物回避とを組み合わせ、最終目標方向θｄ（ｒａｄ）を算出する。

目標方向算出部１１１のフローチャートを図６に示す。目標方向算出部１１１は、まず、ＬＯＳ（Ｌｉｎｅ ｏｆ Ｓｉｇｈｔ）アルゴリズムにより初期目標方向θｓを算出する（ステップＳ１）。大域的な経路追従手法としてＬＯＳを用い、経路追従のための初期目標方向θｓを数２式により求める。

数２式において、関数ａｔａｎ２は、座標（Ｘ，Ｙ）を入力し、逆正接（アークタンジェント）をラジアン（角度）を単位として−π〜πの範囲で出力する関数である。

(Ｘｓ，Ｙｓ)は数３式の関係より得られる。

(Ｘｋ，Ｙｋ)（ｋ＝１，・・・，ｎ）は通過点３２〜３５（Ｗａｙ Ｐｏｉｎｔ）の座標であり、ＲはＬＯＳ半径と呼ぶパラメータである。

再び図５を参照し、ＬＯＳの概念を説明する。図に示すように、移動体１は、理想ルートから離れた場所を走行している。このとき、移動体１を中心とした半径Ｒ（ＬＯＳ半径）の円を考える。移動体１は、半径Ｒの円の中に出発点３１、通過点３２〜３５、最終目的地３６のいずれかの点が入ると、その次の通過点３２〜３５、または最終目的地３６を目的地として設定する。図５の場合は、通過点３２が一旦半径Ｒの円内に入ったため、通過点３３が現時点での目的地となっている。これは数４式の様に表される。つまり（Ｘｋ，Ｙｋ）がＬＯＳ半径の内部に含まれた場合には、現在目標としている通過点（Ｘｋ，Ｙｋ）を次の通過点（Ｘｋ＋１，Ｙｋ＋１）に変更するのである。図５の場合、さらに移動体１が進行し、半径Ｒの円内に通過点３３が入れば、次の目的地が通過点３４となる。そして半径Ｒの円と線分４２との交点のうち、現時点での目的地３３に近い点を復帰点ＲＰとすると、移動体１の現在の座標（Ｘｃ，Ｙｃ）から復帰点ＲＰに向かう方向が初期目標方向θｓである。

図７に示すように、ＬＯＳ半径Ｒを大きく設定すると、経路から多少ずれていても、現在の方向θｃと初期目標方向θｓとの差は比較的小さい。図８に示すように、ＬＯＳ半径Ｒを小さく設定すると、経路からわずかに離れるだけで現在方向θｃと初期目標方向θｓの差が大きくなる。

再び図６を参照する。次に、目標方向算出部１１１は、障害物までの走行可能距離を算出する。局所的な障害物回避を行うために、非特許文献２のＳｅｎｓｏｒ−Ｂａｓｅｄ Ｎａｖｉｇａｔｉｏｎ法を利用して障害物２までの走行可能距離Ｄｉ（ｉ＝１，・・・，ｎ）を算出する。

図９に示すように、左右たとえば３０ｃｍずつマージンを加えた車体幅Ａを考える。そして、図１０に示すように、様々な候補方向に関して車体幅Ａを横幅、候補方向を高さとする長方形Ｂを考える。長方形Ｂの高さは、長方形Ｂの領域が障害物２と接触するまで延伸することによって決定される。この長方形Ｂの高さが走行可能距離Ｄｉとなる。ある候補方向に対して障害物２が十分遠方である場合には、走行可能距離Ｄｉをある規定値（１０ｍ、１００ｍなど）に設定すればよい。

本実施の形態では、レーザレンジファインダ１４のスキャンデータから、３１方向の候補方向を考える。具体的には、車両正面方向を中心に左右π／２（ｒａｄ）の範囲にπ／３０（ｒａｄ）刻みで、３１方向の候補方向を考える。３１の候補方向それぞれについて、前進可能な走行可能距離Ｄｉ（ｍ）（ｉ＝１，・・・，３１）を取得する。目標方向算出部１１１は、図１１に示すように、各候補方向の走行可能距離Ｄｉをメモリに記憶する。

再び図６を参照する。次に、目標方向算出部１１１は、現在方向θｃ、初期目標方向θs、および走行可能距離Ｄｉ（ｉ＝１，・・・，ｎ）から、最終目標方向θｄを算出する（ステップＳ３）。

ステップ３の詳細なフローチャートを図１２に示す。ステップ３の処理の中で用いる用語の定義は、次のとおりである。

図１３に示すように、３１の候補方向の中で走行可能距離Ｄｉがしきい値Ｌを超えている方向であって、θｓとの差が局所平面座標系で最小となるものを方向Ψｓとする。また、３１の候補方向の中で走行可能距離Ｄｉがしきい値Ｌを超えている方向であって、現在方向θｃとの差が局所平面座標系で最小となるものを方向Ψとする。方向ΨおよびΨｓは、現在方向θｃを基準とする相対角度である。

図１４に示すように、しきい値Ｌを比較的小さく設定すれば、走行可能距離Ｄｉがしきい値Ｌを超える方向を多く選択可能となる。比較的障害物２の近くまで近づくことを可とする制御となる。走行可能な距離が少しでもあれば少しずつ方位を変更して前進し、細かな進路変更を行いながら経路追従を行わせることができる。

図１５に示すように、しきい値Ｌを比較的大きく設定すれば、走行可能距離Ｄｉがしきい値Ｌを超える方向が減少する。障害物２をより早く回避しようとする制御である。ゆったりとした進路変更を行いながら、最終目的地３６を目指す制御に向いている。

図１２のフローチャートを参照する。目標方向算出部１１１は、まず、３１の候補方向の中で走行可能距離Ｄｉがしきい値Ｌを超えるものがあるかどうかを判定する（ステップＳ３０１）。

走行可能距離Ｄｉの中でしきい値Ｌを超えるものがある場合（ステップＳ３０１でＹＥＳ）、初期目標方向θｓと現在方向θｃとの差がしきい値εを下回るか否かを判定する（ステップＳ３０２）。ここで、しきい値εは充分に小さい値（例えば３°）が設定される。ステップＳ３０２の条件を満たす場合とは、初期目標方向θｓと現在方向θｃとのずれが十分に小さい場合である。

初期目標方向θｓと現在方向θｃとの差がしきい値εを下回る場合（ステップＳ３０２でＹＥＳ）、候補方向の中で正面方向（ｉ＝１６）に対応する走行可能距離Ｄ１６がしきい値Ｌを超えるかどうかを判定する（ステップＳ３０３）。

走行可能距離Ｄ１６がしきい値Ｌを超える場合（ステップＳ３０３でＹＥＳ）、すなわち、現在方向θｃについて継続して走行可能と判断できる場合には、図１６に示すように、最終目標方向θｄとして、θｃ＋Ψｓを設定する（ステップＳ３０４）。さらに、目標方向算出部１１１は、障害物回避フラグＯｂｓｔ＿ｆｌａｇをＯＦＦに設定する。

障害物回避フラグＯｂｓｔ＿ｆｌａｇは、現在、移動体１が障害物を回避するための動作を行っているか否かを設定するフラグである。ステップＳ３０４に遷移するケースは、正面前方に障害物が存在しない場合（ステップＳ３０３でＹＥＳ）であるので、障害物回避フラグＯｂｓｔ＿ｆｌａｇはＯＦＦに設定される。目標方向算出部１１１は、障害物回避フラグＯｂｓｔ＿ｆｌａｇをメモリに格納している。

走行可能距離Ｄ１６がしきい値Ｌ以下である場合（ステップＳ３０３でＮＯ）には、Ψｓ−（θｓ−θｃ）と重み付け係数ｗが乗算されたΨとの大小関係を比較する（ステップＳ３０５）。走行可能距離Ｄ１６がしきい値Ｌ以下である場合とは、正面前方に障害物が存在し、現在方向θｃについて継続して走行不可能と判断できる場合である。

Ψｓ−（θｓ―θｃ）が重み付けされた方向ｗΨ以下である場合（ステップＳ３０５でＹＥＳ）、図１７に示すように、最終目標方向θｄとして、θｃ＋Ψｓを設定する（ステップＳ３０６）。ステップＳ３０６に遷移するのは、正面前方に障害物２が存在し、障害物回避をしている状態である（ステップＳ３０３でＮＯ）。目標方向算出部１１１は、障害物回避フラグＯｂｓｔ＿ｆｌａｇをＯＮに設定し、移動体１が現在、障害物２を回避動作中であることを設定する。

Ψｓ−（θｓ―θｃ）が重み付けされた方向ｗΨ以下である場合とは、障害物２を回避するにあたって、現在方向θｃの維持を重視するよりも、初期目標方向θｓに近づくことを目標としつつ回避動作を行うことが有利と判断される場合である。

Ψｓ−（θｓ―θｃ）が重み付けされた方向ｗΨより大きい場合（ステップＳ３０５でＮＯ）、図１８に示すように、最終目標方向θｄとして、θｃ＋Ψを設定する（ステップＳ３０７）。ステップＳ３０７に遷移するのは、正面前方に障害物４が存在し、障害物回避をしている状態である（ステップＳ３０３でＮＯ）。目標方向算出部１１１は、障害物回避フラグＯｂｓｔ＿ｆｌａｇをＯＮに設定し、移動体１が現在、障害物４を回避動作中であることを設定する。

Ψｓ−（θｓ―θｃ）が重み付けされた方向ｗΨより大きい場合とは、障害物４を回避するにあたって、初期目標方向θｓに近づくことを重視するよりも、現在方向θｃを維持しつつ回避動作を行うことが有利と判断される場合である。

ステップＳ３０２に戻る。初期目標方向θｓと現在方向θｃの差がしきい値ε以上である場合（ステップＳ３０２でＮＯ）、走行可能距離Ｄ１６がしきい値Ｌを超えるかどうかを判定する（ステップＳ３０８）。ステップＳ３０２の条件を満たさない場合とは、初期目標方向θｓと現在方向θｃとが比較的大きくずれている場合である。

走行可能距離Ｄ１６がしきい値Ｌを超える場合（ステップＳ３０８でＹＥＳ）、すなわち、現在方向θｃについて継続して走行可能と判断できる場合には、障害物回避フラグＯｂｓｔ＿ｆｌａｇがＯＮになっているかどうかを判定する（ステップＳ３０９）。

障害物回避フラグＯｂｓｔ＿ｆｌａｇがＯＮである場合（ステップＳ３０９でＹＥＳ）、図１９に示すように、最終目標方向θｄとして、θｃ＋Ψａを設定する（ステップＳ３１０）。

現在方向θｃを中心として、制限領域ＬＡが設定される。３１の候補方向の中で制限領域ＬＡに属する方向を制限方向とする。方向Ψａは、制限方向の中で走行可能距離Ｄｉがしきい値Ｌを超える方向であって、かつ、初期目標方向θｓとの差が最小となる方向である。障害物回避動作中には、初期目標方向θｓに向かって、極端に方向が変更されることを回避し、滑らかな走行が行われるよう制御する。

障害物回避フラグＯｂｓｔ＿ｆｌａｇがＯＦＦである場合（ステップＳ３０９でＮＯ）、図２０に示すように、最終目標方向θｄとして、θｃ＋Ψｓを設定する（ステップＳ３１１）。障害物回避動作中でない場合には、初期目標方向θｓになるべく追従するよう制御を行い、早期に理想ルートへの復帰を試みる。

走行可能距離Ｄ１６がしきい値Ｌ以下である場合（ステップＳ３０８でＮＯ）には、Ψｓ−（θｓ−θｃ）と重み付け係数ｗが乗算された方向Ψとの大小関係を比較する（ステップＳ３１２）。

Ψｓ−（θｓ―θｃ）が重み付けされた方向ｗΨ以下である場合（ステップＳ３１２でＹＥＳ）、図２１に示すように、最終目標方向θｄとして、θｃ＋Ψｓを設定する（ステップＳ３１３）。ステップＳ３１３に遷移するのは、正面前方に障害物２が存在し、障害物回避をしている状態である（ステップＳ３０８でＮＯ）。目標方向算出部１１１は、障害物回避フラグＯｂｓｔ＿ｆｌａｇをＯＮに設定し、移動体１が現在、障害物２を回避動作中であることを設定する。

Ψｓ−（θｓ―θｃ）が重み付けされた方向ｗΨ以下である場合とは、障害物２を回避するにあたって、現在方向θｃの維持を重視するよりも、初期目標方向θｓに近づくことを目標としつつ回避動作を行うことが有利と判断される場合である。

Ψｓ−（θｓ―θｃ）が重み付けされた方向ｗΨより大きい場合（ステップＳ３１２でＮＯ）、図２２に示すように、最終目標方向θｄとして、θｃ＋Ψを設定する（ステップＳ３１４）。ステップＳ３１４に遷移するのは、正面前方に障害物４が存在し、障害物回避をしている状態である（ステップＳ３０８でＮＯ）。目標方向算出部１１１は、障害物回避フラグＯｂｓｔ＿ｆｌａｇをＯＮに設定し、移動体１が現在、障害物４を回避動作中であることを設定する。

Ψｓ−（θｓ―θｃ）が重み付けされた方向ｗΨより大きい場合とは、障害物４を回避するにあたって、初期目標方向θｓに近づくことを重視するよりも、現在方向θｃを維持しつつ回避動作を行うことが有利と判断される場合である。

ステップＳ３０６、Ｓ３０７、Ｓ３１３およびＳ３１４において、重み付け係数ｗを利用した。重み付け係数ｗは、前方に障害物があるときに現在方向θｃを極力保持すべきか、あるいはできるだけ経路追従を行うかの優先度合いを規定するパラメータである。重み付け係数ｗが大きい場合は経路追従を、小さい場合は現在方向θｃの保持を優先的に行う制御となる。

経路の大半を直線が占めているような状況では、直進性を高めるために重み付け係数ｗを小さめ、たとえば０．５〜２程度に設定することが好ましい。一方で、経路の大半を曲線が占めているような状況では、経路追従性を高めるために重み付け係数ｗを大きめ、たとえば２以上に設定することが好ましい。ｗを１以下に設定すれば、現在方向θｃの保持をより優先させることができる。ｗを４以上あるいは５以上などに設定すれば、経路追従性を非常に高くすることができる。

図２３は、ｗ＝２を設定した例である。この場合、ステップＳ３０５あるいはＳ３１２の条件を比較的満たし易くなり、経路追従性に少し重点をおいた制御となる。図２４は、ｗ＝０．５を設定した例である。この場合、ステップＳ３０５あるいはステップＳ３１２の条件を満たすことが難しくなり、現在方向θｃの保持に重点が置かれる。

ステップＳ３０１に戻る。３１の候補方向の中でいずれの方向についても走行可能距離Ｄｉがしきい値Ｌ以下である場合（ステップＳ３０１でＮＯ）、ステップＳ３１５の停止処理に推移する。

図２５は、ステップＳ３０１の停止処理のフローチャートである。周囲が障害物２で囲まれて移動不可となった場合とは、たとえばしきい値Ｌが０．５ｍであって、走行可能距離Ｄｉがすべて０．５ｍ未満になったような場合である。

目標方向算出部１１１は、一定時間、たとえば１０秒間の停止（ｖｄ＝０）を車輪制御部１６に指示する（ステップＳ３１５１）。１０秒間経過後（ステップＳ３１５２でＹＥＳ）、角速度ωｄで移動体１を旋回させる（ステップＳ３１５３）。この後、図１２のステップＳ３０１に復帰し、前進可能な方向を探索する。一周して前進可能な方向が見つからなかった場合は、さらにこのプロセスを繰り返してもよいし、走行不可能として停止してもよい。

目標方向算出部１１１は、以上説明したステップＳ３０１〜Ｓ３１５までの処理を終えると、図６のフローチャートを終了する。目標方向算出部１１１は、ステップＳ１〜Ｓ３の処理を終えた後、所定のタイミングで図６のフローチャートで示す処理を再開する。このようにしてステップＳ１〜Ｓ３を繰り返しながら最終目的地３３へと向かう。

＜３−３．制御部＞
再び図４を参照する。目標方向算出部１１１において算出された最終目標方向θｄは、演算部１１２に入力される。また、演算部１１２は、拡張カルマンフィルタ１１６から出力された現在方向θｃを入力する。演算部１１２は、最終目標方向θｄから現在方向θｃを減算する。この減算値は、増幅部１１３において増幅されて目標角速度ωｄ（ｒａｄ／ｓ）として出力される。

速度制御部１１４は、レーザレンジファインダ１４から障害物２〜４の情報（角度、距離）を、目標方向算出部１１１から最終目標方向θｄを入力する。速度制御部１１４は、障害物２〜４の情報と最終目標方向θｄとから目標速度ｖｄ（ｍ／ｓ）を算出し、車輪制御部１６に出力する。

ｖｄは、走行可能距離の統計量（最大値、平均値）や経路からのクロストラック誤差などの状態量に応じて前進速度マップから決定される速度を採用する。ただし、経路に設定された最大速度は超えないよう設定される。また、ωｄは単純な比例制御則ωｄ＝Ｋｐ（θｄ−θｃ）を用いる。ここでＫｐは比例ゲインである。

算出されたｖｄおよびωｄは車輪制御部１６に与えられ、左右の車輪１７，１７の速度が制御される。このようにして、経路追従と障害物を回避しつつ、左右の車輪１７，１７を制御し、最終目的地３６へ向かって移動体１は走行する。

｛４．各種パラメータの調整｝
図７、図８等を用いて説明したＬＯＳ半径Ｒは、経路追従のための初期目標方向θｓの変化の度合いを規定するパラメータである。ＬＯＳ半径Ｒが大きい場合は、移動体１が経路から多少離れても初期目標方向θｓが現在方向θｃから急激に変化することはない。経路追従精度は低下するが、左右に振れることなく滑らかな走行が可能となる。ＬＯＳ半径Ｒが小さい場合は、移動体１が経路からわずかでも離れると初期目標方向θｓが大きく変化する。経路上で左右に振れる割合が大きくなるが、経路追従精度は向上する。

ＬＯＳ半径Ｒは追従精度と滑らかな走行動作のトレードオフを定めるパラメータである。比較的道幅が広く、追従精度がそれほど要求されない場所では、ＬＯＳ半径Ｒを大きめ、たとえば車両の全長の３〜５倍程度に設定することが好ましい。道幅が狭く、追従精度が要求される場所では、ＬＯＳ半径Ｒを小さめ、たとえば車両の全長の２倍程度に設定することが好ましい。

走行可能距離Ｄｉのしきい値Ｌは、障害物回避をどの時点で開始するかの度合いを規定するパラメータである。しきい値Ｌが大きい場合は障害物２〜４を遠くから回避するようになるが、周囲に障害物が増えた場合は走行可能方向が限られる。しきい値Ｌが小さい場合は障害物２〜４の間近で回避するようになるが、周囲に障害物が増えても走行可能方向が多く存在する可能性が高い。

しきい値Ｌは障害物回避と経路追従の優先の度合いを規定するパラメータである。たとえば、周囲に障害物が少ないような状況であっては、しきい値Ｌを大きめ、たとえば５〜１０ｍ程度に設定することで、余裕を持って障害物を回避することができる。周囲に障害物が多いような状況であっても、しきい値Ｌを小さめ、たとえば１〜３ｍ程度に設定することで、可能な限り経路追従を行うことができる。

以上述べたように、各パラメータは周囲の環境に応じて定められるべきである。たとえば、通過すべき経路の道幅や障害物の有無などが事前に判明しているような場合は、ＧＰＳなどの緯度・経度情報に対応させてＬＯＳ半径Ｒ、しきい値Ｌおよび重み付け係数ｗを変化させればよい。環境に応じて好適な経路追従ならびに障害物回避を実現することができる。

また、自律走行制御部１１は、周囲の障害物２〜４との距離、周囲に存在する障害物２〜４の数、最終目的地３６までの距離、理想ルートからのずれ幅などに応じてＬＯＳ半径Ｒ、しきい値Ｌおよび重み付け係数ｗを決定してもよい。もちろん、これらパラメータを走行ルートに応じて決定し、走行中は一定としてもよい。

また、本発明では現在位置に応じて各パラメータを調整するように構成したが、これ以外の方法で調整するように構成してもよい。移動体１にカメラを搭載し、周囲の道路状況（道幅、交通量）などが認識可能であれば、この情報に基づいて各パラメータを調整してもよい。

１ 移動体
１１ 自律走行制御部
１２ ＤＧＰＳ
１３ 慣性計測装置
１４ レーザレンジファインダ
１５ 通過点情報記憶部
θｃ 現在方向
θｄ（Ψ，Ψｓ） 最終目標方向
θｓ 初期目標方向

Claims (14)

1. 走行方向を制御する装置であって、
   移動体の現在位置を計測する位置計測部と、
   前記移動体が向いている現在の方向を計測する方向計測部と、
   前記移動体の現在位置を基準として障害物の方向および距離を計測する障害物計測部と、
   前記移動体の出発点と、前記移動体が前記出発点に位置するときに目的地として設定される第１の目的地との位置情報を記憶する目的地記憶部と、
   前記位置計測部と前記方向計測部と前記障害物計測部と前記目的地記憶部との出力に基づいて前記移動体の目標方向を算出する自律走行制御部と、
   前記自律走行制御部において算出された前記目標方向に従い、前記移動体の走行方向を制御する走行方向制御部とを備え、
   前記自律走行制御部は、
   前記自律走行制御部が前記第１の目的地を前記移動体の目的地に設定している場合、前記位置計測部と前記目的地記憶部との出力に基づいて、現在位置を中心として所定の半径を有する円と、前記第１の目的地と前記出発点とを結ぶ線分との交点のうち前記第１の目的地に近い方の点の方向を第１の方向に決定する第１方向決定部と、
   前記障害物計測部の出力に基づいて前記障害物までの距離が第１しきい値を超える方向の集合である第２方向集合を決定する第２方向決定部と、
   前記第１方向と前記第２方向集合とから前記目標方向を決定する目標方向決定部とを備え、
   前記目標方向決定部は、
   前記移動体の現在の方向における前記障害物までの距離が前記第１しきい値以下の場合には、現在の方向との差が最小となる前記第２方向集合から選択された第３方向と、前記第１方向との差が最小となる前記第２方向集合から選択された第４方向とを決定し、現在の方向と前記第３方向との差が前記第１方向と前記第４方向との差よりも小さい場合は前記第３方向を前記目標方向として決定し、現在の方向と前記第３方向との差が前記第１方向と前記第４方向との差よりも大きい場合は前記第４方向を前記目標方向として決定する第１決定部を含む走行方向制御装置。
2. 請求項１に記載の走行方向制御装置であって、
   前記目標方向決定部は、
   現在の方向と前記第１方向との差が第２しきい値を下回る場合であって、前記移動体の現在の方向における前記障害物までの距離が前記第１しきい値を超える場合には、前記第４方向を前記目標方向として決定する第２決定部を含む走行方向制御装置。
3. 請求項１または請求項２に記載の走行方向制御装置であって、さらに、
   現在、障害物を回避走行中であるか否かを設定する回避フラグを管理する回避フラグ管理部を備え、
   前記自律走行制御部は、
   前記第２方向集合の中で現在方向との差が第３しきい値以下となる方向の集合である第５方向集合を決定する第５方向決定部を含み、
   前記目標方向決定部は、
   現在の方向と前記第１方向の差が第２しきい値以上の場合であって、かつ、前記移動体の現在方向における前記障害物までの距離が前記第１しきい値を超え、さらに、前記回避フラグがＯＮである場合には、前記第１方向との差が最小となる前記第５方向集合から選択された第６方向を前記目標方向として決定する第３決定部を含む走行方向制御装置。
4. 請求項１または請求項２に記載の走行方向制御装置であって、さらに、
   現在、障害物を回避走行中であるか否かを設定する回避フラグを管理する回避フラグ管理部を備え、
   前記目標方向決定部は、
   現在方向と前記第１方向の差が第２しきい値以上の場合であって、かつ、前記移動体の現在方向における前記障害物までの距離が前記第１しきい値を超え、さらに、前記回避フラグがＯＦＦである場合には、前記第４方向を前記目標方向として決定する第４決定部を含む走行方向制御装置。
5. 請求項１〜請求項４のいずれかに記載の走行方向制御装置において、
   前記第１決定部は、
   現在の方向と前記第３方向との差あるいは前記第１方向と前記第４方向との差のうち、一方の差に重み付けをした上で前記目標方向を決定する重み付け決定部を含む走行方向制御装置。
6. 請求項３または４に記載の走行方向制御装置であって、
   前記回避フラグ管理部は、
   前記移動体の現在の方向における前記障害物までの距離が前記第１しきい値以下である場合、前記回避フラグをＯＮに設定するフラグＯＮ設定部を含む走行方向制御装置。
7. 請求項３または４に記載の走行方向制御装置であって、
   前記回避フラグ管理部は、
   現在の方向と前記第１方向との差が第２しきい値を下回る場合であって、前記移動体の現在の方向における前記障害物までの距離が前記第１しきい値を超える場合、前記回避フラグをＯＦＦに設定するフラグＯＦＦ設定部を含む走行方向制御装置。
8. 請求項１〜請求項７のいずれかに記載の走行方向制御装置であって、
   前記第２方向決定部は、
   前記移動体の幅に所定のマージンを加えた幅を横幅、候補方向を高さとする長方形領域を考え、前記長方形領域が障害物とはじめて接触する位置で前記候補方向の高さを決定し、前記候補方向の高さが前記第１しきい値を超える場合、前記候補方向を前記第２目標方向集合に含める車体幅考慮部を含む走行方向制御装置。
9. 請求項１〜請求項８のいずれかに記載の走行方向制御装置であって、
   前記目標方向決定部は、
   前記移動体の現在位置に応じて前記第１しきい値を決定する第１しきい値決定部を含む走行方向制御装置。
10. 請求項５に記載の走行方向制御装置であって、
    前記目標方向決定部は、
    前記移動体の現在位置に応じて前記重み付け値を決定する重み付け値決定部を含む走行方向制御装置。
11. 請求項１に記載の走行方向制御装置であって、
    前記第１方向決定部は、
    前記移動体の現在位置に応じて所定の半径を決定する半径決定部を含む走行方向制御装置。
12. 請求項１に記載の走行方向制御装置であって、
    前記目的地記憶部は、前記第１の目的地の次に目的地として設定される第２の目的地の位置情報をさらに記憶し、
    前記自律走行制御部は、
    前記自律走行制御部が前記第１の目的地を移動体の目的地に設定している場合でかつ、前記位置計測部と前記目的地記憶部との出力に基づいて、現在位置を中心として所定の半径を有する円の中に前記第１の目的地が入ったとき、前記第２の目的地を前記移動体の目的地に変更する目的地変更部をさらに備える走行方向制御装置。
13. 請求項１２に記載の走行方向制御装置であって、
    前記第１の方向決定部は、前記自律走行制御部が前記第２の目的地を前記移動体の目的地に設定している場合、前記位置計測部と前記目的地記憶部との出力に基づいて、現在位置を中心として所定の半径を有する円と、前記第２の目的地と前記第１の目的地とを結ぶ線分との交点のうち前記第２の目的地に近い方の点の方向を前記第１方向に決定する走行方向制御装置。
14. 請求項１〜請求項１３のいずれかに記載の走行方向制御装置を備える移動体。

Images