JP2024036932A - 移動体制御システム、その制御方法、プログラム、及び移動体 - Google Patents

Abstract

【課題】本発明は、高精度地図を用いることなく移動体の経路を好適に生成することにある。
【解決手段】本移動体制御システムは、撮像画像を取得し、撮像画像に含まれる障害物を検出し、移動体の周辺の領域を分割し、分割した領域ごとに検出された障害物の占有を示す占有地図を生成する。また、本移動体制御システムは、占有地図において現在位置からの距離が遠いほどコストが高い第１コストと、占有地図において目標位置からの距離が遠いほどコストが高い第２コストと、占有地図において過去の経路からの距離が遠いほどコストが高い第３コストとに基づいて、検出した障害物を回避する現在位置から目標位置までの大域経路を生成する。
【選択図】図７

Description

本発明は、移動体制御システム、その制御方法、プログラム、及び移動体に関する。

近年、超小型モビリティ（マイクロモビリティとも称する。）と呼ばれる、乗車定員が１～２名程度の電動車両や、人に対して種々のサービスを提供する移動型ロボットなどの小型の移動体が知られている。このような移動体には、目的地までの走行経路を周期的に生成しながら自律走行を行うものがある。

特許文献１は、地図及びルート情報に基づいて第１の軌跡を生成し、第１の軌跡を障害物情報等に基づいて経路及び速度を最適化し、最適化した経路及び速度に基づいて自動運転車を制御する第２の軌跡を生成することを開示している。

特開２０１９－１２８９６２号公報

上記従来技術ではサーバから提供される高精度な地図情報に基づいて経路計画を行っている。しかし、小型の移動体においてはハードウェア資源が乏しく、そのような高精度な地図情報を記憶する領域や、大量の地図情報を高速に取得する通信デバイスを確保することは難しい。また、小型の移動体が走行する地域においては、高精度な地図が整備されていない可能性もあるため、そのような地図情報を利用することなく経路を生成する必要がある。

一方、経路の最適化を行うために、目的地までの距離や障害物までの距離に基づくコスト関数を用いることが知られている。このようなコスト関数では、障害物を回避した経路を生成可能であるものの、探索範囲が広く処理量が増大するという課題がある。ハードウェア資源が乏しい小型の移動体において、リアルタイムで周期的に経路を生成する際の処理負荷を低減することは非常に重要である。

本発明は、上記課題に鑑みてなされ、高精度地図を用いることなく移動体の経路を好適に生成することにある。

本発明によれば、例えば移動体であって、撮像画像を取得する取得手段と、前記撮像画像に含まれる障害物を検出する検出手段と、前記移動体の周辺の領域を分割し、分割した領域ごとに前記検出手段によって検出された障害物の占有を示す占有地図を生成する地図生成手段と、前記占有地図において現在位置からの距離が遠いほどコストが高い第１コストと、前記占有地図において目標位置からの距離が遠いほどコストが高い第２コストと、前記占有地図において過去の経路からの距離が遠いほどコストが高い第３コストとに基づいて、検出した前記障害物を回避する前記現在位置から前記目標位置までの大域経路を生成する経路生成手段とを備えることを特徴とする。

本発明によれば、高精度地図を用いることなく移動体の経路を好適に生成し、処理量を低減することができる。

本実施形態に係る移動体のハードウェアの構成例を示すブロック図 本実施形態に係る移動体の制御構成を示すブロック図 本実施形態に係る移動体の機能構成を示すブロック図 本実施形態に係る占有格子地図を示す図 本実施形態に係る占有格子地図の生成方法を示す図 本実施形態に係る大域経路及び局所経路を示す図 本実施形態に係る大域経路の生成方法を示す図 本実施形態に係る大域経路を最適化する方法を示す図 本実施形態に係る移動体の走行を制御する処理手順を示すフローチャート 本実施形態に係る経路を生成する詳細な処理手順を示すフローチャート

以下、添付図面を参照して実施形態を詳しく説明する。なお、以下の実施形態は特許請求の範囲に係る発明を限定するものではなく、また実施形態で説明されている特徴の組み合わせの全てが発明に必須のものとは限らない。実施形態で説明されている複数の特徴のうち二つ以上の特徴が任意に組み合わされてもよい。また、同一若しくは同様の構成には同一の参照番号を付し、重複した説明は省略する。

＜移動体の構成＞
図１を参照して、本実施形態に係る移動体１００の構成について説明する。図１（Ａ）は本実施形態に係る移動体１００の側面を示し、図１（Ｂ）は移動体１００の内部構成を示している。図中矢印Ｘは移動体１００の前後方向を示しＦが前をＲが後を示す。矢印Ｙ、Ｚは移動体１００の幅方向（左右方向）、上下方向を示す。

移動体１００は、バッテリ１１３を搭載しており、例えば、主にモータの動力で移動する超小型モビリティである。超小型モビリティとは、一般的な自動車よりもコンパクトであり、乗車定員が１又は２名程度の超小型車両である。本実施形態では、移動体１００の一例として三輪の超小型モビリティを例に説明するが、本発明を限定する意図はなく例えば四輪車両や鞍乗型車両であってもよい。また、本発明の車両は、乗り物に限らず、荷物を積載して人の歩行に並走する車両や、人を先導する車両であってもよい。さらに、本発明には、四輪や二輪等の車両に限らず、自立移動が可能な歩行型ロボットなども適用可能である。

移動体１００は、走行ユニット１１２を備え、バッテリ１１３を主電源とした電動自律式車両である。バッテリ１１３は例えばリチウムイオンバッテリ等の二次電池であり、バッテリ１１３から供給される電力により走行ユニット１１２によって移動体１００は自走する。走行ユニット１１２は、左右一対の前輪１２０と、尾輪（従動輪）１２１とを備えた三輪車である。走行ユニット１１２は四輪車の形態等、他の形態であってもよい。移動体１００は、一人用又は二人用の座席１１１を備える。

走行ユニット１１２は操舵機構１２３を備える。操舵機構１２３はモータ１２２ａ、１２２ｂを駆動源として一対の前輪１２０の舵角を変化させる機構である。一対の前輪１２０の舵角を変化させることで移動体１００の進行方向を変更することができる。尾輪１２１は個別に駆動源を有しておらず一対の前輪１２０の駆動に追従して動作する従動輪である。また、尾輪１２１は移動体１００の車体に対して旋回部を有して連結される。当該旋回部は、尾輪１２１の回転とは別に尾輪１２１の向きが変化するように回転する。このように、本実施形態に係る移動体１００は、尾輪付きの差動二輪型モビリティを採用するものであるが、これに限るものではない。

移動体１００は、移動体１００の前方の平面を認識する検知ユニット１１４を備える。検知ユニット１１４は、移動体１００の前方を監視する外界センサであり、本実施形態の場合、移動体１００の前方の画像を撮像する撮像装置である。本実施形態では、検知ユニット１１４は、例えば、２つのレンズなどの光学系とそれぞれのイメージセンサとを有するステレオカメラを例に説明する。しかし、撮像装置に代えて又は追加して、レーダやライダ（Light Detection and Ranging）を採用することも可能である。また、本実施形態では、移動体１００の前方にのみ設ける例について説明するが、本発明を限定する意図はなく、移動体１００の後方や左右に設けてもよい。

本実施形態に係る移動体１００は、検知ユニット１１４を用いて移動体１００の前方領域を撮像し、撮像画像から障害物を検出する。さらに移動体１００は、移動体１００の周辺領域を格子状に分割し、各格子（以下では、グリッドとも称する。）に障害物情報を蓄積した占有格子地図を生成しながら走行を制御する。占有格子地図の詳細については後述する。

＜移動体の制御構成＞
図２は、本実施形態に係る移動体１００の制御系のブロック図である。ここでは本発明を実施する上で必要な構成を主に説明する。従って、以下で説明する構成に加えてさらに他の構成が含まれてもよい。また、本実施形態では、以下で説明する各部が移動体１００に含まれるものとして説明するが、本発明を限定する意図はなく、複数のデバイスを含む移動体制御システムとして実現されてもよい。例えば、制御ユニット１３０の一部の機能が通信可能に接続されたサーバ装置によって実現されてもよいし、検知ユニット１１４やＧＮＳＳセンサ１３４が外部デバイスとして設けられてもよい。移動体１００は、制御ユニット（ＥＣＵ）１３０を備える。制御ユニット１３０は、ＣＰＵに代表されるプロセッサ、半導体メモリ等の記憶デバイス、外部デバイスとのインタフェース等を含む。記憶デバイスにはプロセッサが実行するプログラムやプロセッサが処理に使用するデータ等が格納される。プロセッサ、記憶デバイス、インタフェースは、移動体１００の機能別に複数組設けられて互いに通信可能に構成されてもよい。

制御ユニット１３０は、検知ユニット１１４の検知結果、操作パネル１３１の入力情報、音声入力装置１３３から入力された音声情報、ＧＮＳＳセンサ１３４からの位置情報、及び通信ユニット１３６を介した受信情報を取得して、対応する処理を実行する。制御ユニット１３０は、モータ１２２ａ、１２２ｂの制御（走行ユニット１１２の走行制御）、操作パネル１３１の表示制御、スピーカ１３２の音声による移動体１００の乗員への報知、情報の出力を行う。

音声入力装置１３３は、移動体１００の乗員の音声を収音する。制御ユニット１３０は、入力された音声を認識して、対応する処理を実行可能である。ＧＮＳＳ(Global Navigation Satellite system)センサ１３４は、ＧＮＳＳ信号を受信して移動体１００の現在位置を検知する。記憶装置１３５は、検知ユニット１１４による撮像画像、障害物情報、過去に生成した経路、及び占有格子地図等を記憶する記憶デバイスである。記憶装置１３５にも、プロセッサが実行するプログラムやプロセッサが処理に使用するデータ等が格納されてよい。記憶装置１３５は、制御ユニット１３０によって実行される音声認識や画像認識用の機械学習モデルの各種パラメータ（例えばディープニューラルネットワークの学習済みパラメータやハイパーパラメータなど）を格納してもよい。

通信ユニット１３６は、例えば、Ｗｉ‐Ｆｉや第５世代移動体通信などの無線通信を介して外部装置である通信装置１４０と相互に通信を行う。通信装置１４０は、例えばスマートフォンであるが、これに限らず、イヤフォン型の通信端末であってもよいし、パーソナルコンピュータ、タブレット端末、ゲーム機などであってもよい。通信装置１４０は、例えば、Ｗｉ‐Ｆｉや第５世代移動体通信などの無線通信を介してネットワークに接続する。

通信装置１４０を所有するユーザは、当該通信装置１４０を介して移動体１００に対して指示を行うことができる。当該指示は、例えば移動体１００をユーザが所望する位置へ呼び寄せて合流するための指示を含む。当該指示を受信すると、移動体１００は指示に含まれる位置情報に基づいて目標位置を設定する。なお、移動体１００はこのような指示以外にも、検知ユニット１１４の撮像画像から目標位置を設定したり、移動体１００に乗車するユーザからの操作パネル１３１を介した指示に基づいて目標位置を設定したりすることができる。撮像画像から目標位置を設定する場合は、例えば撮像画像内に移動体１００に向かって手を挙げている人を検出し、検出された人の位置を推定して目標位置として設定する。

＜移動体の機能構成＞
次に、図３を参照して、本実施形態に係る移動体１００の機能構成について説明する。ここで説明する機能構成は、制御ユニット１３０において、例えばＣＰＵがＲＯＭ等のメモリに格納されたプログラムをＲＡＭに読み出して実行することにより実現される。なお、以下で説明する機能構成は、本発明を説明する上で必要な機能のみについて説明するものであり、移動体１００に実際に含まれる機能構成の全てを説明するものではない。つまり、本発明に係る移動体１００の機能構成を以下で説明する機能構成に限定するものではない。

ユーザ指示取得部３０１は、ユーザからの指示を受け付ける機能を有し、操作パネル１３１を介したユーザ指示や、通信ユニット１３６を介して通信装置１４０等の外部装置からユーザ指示、音声入力装置１３３を介してユーザの発話による指示を受け付けることができる。ユーザ指示には上述したように、移動体１００の目標位置（目的地とも称する。）を設定する指示や、移動体１００の走行制御に関わる指示が含まれる。

画像情報処理部３０２は、検知ユニット１１４によって取得された撮像画像を処理する。具体的には、画像情報処理部３０２は、検知ユニット１１４によって取得されたステレオ画像から深度画像を作成して３次元点群化する。３次元点群化された画像データは、移動体１００の走行を妨げる障害物を検出するために利用される。また、画像情報処理部３０２は、画像情報を処理する機械学習モデルを含み、当該機械学習モデルの学習段階の処理や推論段階の処理を実行してもよい。画像情報処理部３０２の機械学習モデルは、例えば、ディープニューラルネットワーク（ＤＮＮ）を用いた深層学習アルゴリズムの演算を行って、画像情報に含まれる立体物等を認識する処理を行うことができる。

格子地図生成部３０３は、３次元点群の画像データに基づいて、所定サイズ（例えば、２０ｍＸ２０ｍの領域において各セルが１０ｃｍＸ１０ｃｍ）のグリッドマップを作成する。これは、３次元点群のデータ量が多くリアルタイム処理が困難であるため、軽量化することを目的としている。当該グリッドマップは、例えば、グリッド内点群の最大高さと最小高さの差を示すグリッドマップ（当該セルが段差であるかどうかを表現）と、基準点からグリッド内点群の最大高さを示すグリッドマップ（当該セルの地形形状を表現）とを含んで構成される。さらに、格子地図生成部３０３は、生成したグリッドマップに含まれるスパイクノイズやホワイトノイズを除去し、所定以上の高さを有する障害物を検知し、グリッドごとに障害物となる立体物が存在するか否かを示す占有格子地図を生成する。

経路生成部３０４は、ユーザ指示取得部３０１によって設定された目標位置に対しての移動体１００の走行経路を生成する。具体的には、経路生成部３０４は、高精度地図の障害物情報を必要とすることなく、検知ユニット１１４の撮像画像から格子地図生成部３０３によって生成された占有格子地図を用いて経路を生成する。なお、検知ユニット１１４が移動体１００の前方領域を撮像するステレオカメラであるため、他方向の障害物を認識することができない。従って、移動体１００は、視野角外の障害物への衝突や袋小路でのスタックを回避するため、検出した障害物情報を所定期間記憶しておくことが望ましい。これにより、移動体１００は、過去に検出した障害物と、リアルタイムで検出した障害物との両方を考慮して経路生成を行うことができる。

また、経路生成部３０４は、占有格子地図を用いて大域経路（global path）を周期的に生成し、さらに大域経路に追従するように局所経路（local path）を周期的に生成する。つまり、大域経路によって局所経路の目標位置が決定される。また、各径路の生成周期として、本実施形態では大域経路の生成周期を１００ｍｓとし、局所経路の生成周期を５０ｍｓとするが、本発明を限定するものではない。大域経路を生成するアルゴリズムとしては、ＲＲＴ（Rapid-exploring Random Tree）、ＰＲＭ（Probabilistic Road Map）、Ａ＊など種々のアルゴリズムが知られている。本実施形態に係る経路生成部３０４は、グリッドのセルをノードとして扱う場合の相性と再現性を考慮してＡ＊のアルゴリズムをベースとし、更に計算量を削減するために、当該アルゴリズムを改良した手法を用いる。当該手法の詳細については後述する。また、移動体１００として尾輪付きの差動二輪型のモビリティを採用しているため、経路生成部３０４は従動輪である尾輪１２１を考慮した局所経路を生成する。

走行制御部３０５は、局所経路に従って移動体１００の走行を制御する。具体的には、走行制御部３０５は、局所経路に従って走行ユニット１１２を制御して移動体１００の速度及び角速度を制御する。更に、走行制御部３０５は、運転者の各種操作に応じて走行を制御する。走行制御部３０５は、運転者の操作によって局所経路の運転計画にずれが生じた場合において、再度、経路生成部３０４によって生成された新たな局所経路を取得して走行を制御してもよいし、使用中の局所経路からのずれを解消するように移動体１００の速度及び角速度を制御してもよい。

＜占有格子地図＞
図４は、本実施形態に係る障害物情報を含む占有格子地図４００を示す。本実施形態に係る移動体１００は、高精度地図の障害物情報に頼らずに走行するため、障害物情報はすべて検知ユニット１１４の認識結果から取得する。このとき視野角外の障害物への衝突や袋小路でのスタック回避のため、障害物情報を記憶する必要がある。そこで、本実施形態では、障害物情報を記憶する方法として、ステレオ画像の３次元点群の情報量の削減と経路計画での扱いやすさの観点から占有格子地図を利用する。

本実施形態に係る格子地図生成部３０３は、移動体１００の周辺領域を格子状に分割し、格子ごとに、障害物の有無を示す情報を含ませた占有格子地図を生成する。なお、ここでは、所定領域を格子状に分割する例で説明するが、格子状に分割するのではなく、他の形状で分割して、分割した領域ごとに障害物の有無を示す占有地図を作成するようにしてもよい。占有格子地図４００は、移動体１００の周辺の例えば４０ｍＸ４０ｍや２０ｍｘ２０ｍのサイズの領域を周辺領域とし、当該領域を２０ｃｍＸ２０ｃｍや１０ｃｍｘ１０ｃｍの格子（グリッド）に分割され、移動体１００の移動に応じて動的に設定する。つまり、占有格子地図４００は、移動体１００の移動に合わせて常に移動体１００が中心にくるようにシフトされ、リアルタイムで変動する領域である。なお、領域のサイズについては、移動体１００のハードウェア資源に応じて任意に設定することができる。

また、占有格子地図４００には、検知ユニット１１４による撮像画像から検出された障害物の有無情報が各グリッドに定義される。有無情報としては、例えば走行可能領域を”０”とし、走行不可能領域（即ち、障害物有り）を”１”として定義される。図４において４０１は、障害物が存在するグリッドを示す。障害物が存在する領域は、移動体１００が通過できない領域を示し、例えば５ｃｍ以上の立体物で構成される。従って、移動体１００は、これらの障害物４０１を回避するように、経路を生成する。

＜障害物情報の蓄積＞
図５は、本実施形態に係る占有格子地図における障害物情報の蓄積について説明する。５００は、移動体１００の移動に合わせて移動するローカルマップを示す。ローカルマップ５００は、格子地図上のｘ軸方向、ｙ軸方向に対する移動体１００の移動に応じてシフトされる。ローカルマップ５００は、例えば移動体１００のｘ軸方向への移動量Δｘに応じて、５０１の点線領域を削除して、５０２の実線領域を追加する様子を示す。削除する領域は移動体１００の進行方向とは反対の領域となり、追加する領域は当該進行方向の領域となる。同様に、ｙ軸方向についても移動体１００の移動に応じて領域の削除及び追加が行われる。また、ローカルマップ５００は、過去に検出された障害物情報を蓄積している。なお、削除領域に含まれるグリッドに障害物が存在する場合には、当該障害物情報についてはローカルマップ５００からは削除するものの、一定期間の間、ローカルマップ５００とは別に保持しておくことが望ましい。このような情報は、例えば移動体１００が進路を変更して再度、削除領域がローカルマップ５００内に含まれることとなった場合に有効であり、移動体１００の障害物に対する回避精度を向上させることができる。また、蓄積情報を利用することにより再度障害物の検出を行う必要が無く、処理負荷を低減することができる。

５１０は、移動体１００の検知ユニット１１４によって撮像された撮像画像から移動体１００の前方に存在する障害物の検出情報を示す障害物検出マップを示す。障害物検出マップ５１０は、リアルタイムの情報を示すものであり、検知ユニット１１４から取得される撮像画像に応じて周期的に生成される。なお、人や車両など移動する障害物も想定されるため、移動体１００の前方領域である検知ユニット１１４の視野角内５１１においては、過去に検出された障害物を固定して蓄積するのではなく、周期的に生成された障害物検出マップ５１０によって更新することが望ましい。これにより、移動する障害物についても認識することができ、必要以上に回避する経路の生成を防止することができる。一方で、移動体１００の後方領域（厳密には、検知ユニット１１４の視野角外）については、ローカルマップ５００に示すように、過去に検出された障害物が蓄積される。これにより、例えば、前方領域に障害物が検出され、迂回路を生成する際に、通過した障害物への衝突を回避した経路を容易に生成することができる。

５２０は、ローカルマップ５００と障害物検出マップ５１０とを加算して生成された占有格子地図を示す。このように、占有格子地図５２０は、リアルタイムで変動するローカルマップ及び障害物検出情報と、過去に検出して蓄積した障害物情報とを合成した格子地図として生成される。

＜経路生成＞
図６は、本実施形態に係る移動体１００において生成される走行経路を示す。本実施形態に係る経路生成部３０４は、設定された目標位置６０１に応じて、占有格子地図を用いて大域経路（global path）６０２を周期的に生成し、さらに大域経路に追従するように局所経路（local path）６０３を周期的に生成する。大域経路及び局所経路のそれぞれの生成手法については後述する。

目標位置６０１は、種々の指示に基づいて設定される。例えば、移動体１００に乗車している乗員からの指示や、移動体１００外のユーザからの指示が含まれる。乗員からの指示は、操作パネル１３１や音声入力装置１３３を介して行われる。操作パネル１３１を介する指示は、操作パネル１３１に表示した格子地図の所定のグリッドを指定する方法でもよい。この場合は、各グリッドのサイズを大きく設定し、より広範囲の地図上から選択可能としてもよい。音声入力装置１３３を介する指示は、周辺の物標を目印とする指示でもよい。物標は、発話情報に含まれる通行人、看板、標識、自動販売機など野外に設置される設備、窓や入口などの建物の構成要素、道路、車両、二輪車、などを含んでよい。音声入力装置１３３を介する指示を受け付けると、経路生成部３０４は、検知ユニット１１４によって取得された撮像画像から指定された物標を検出して目標位置として設定する。

これらの音声認識や画像認識については、機械学習モデルを利用する。機械学習モデルは、例えば、ディープニューラルネットワーク（ＤＮＮ）を用いた深層学習アルゴリズムの演算を行って、発話情報や画像情報に含まれる場所名、建造物などのランドマーク名、店舗名、物標の名称などを認識する。音声認識のＤＮＮは、学習段階の処理を行うことにより学習済みの状態となり、新たな発話情報を学習済みのＤＮＮに入力することにより新たな発話情報に対する認識処理（推論段階の処理）を行うことができる。また、画像認識のＤＮＮは、画像内に含まれる通行人、看板、標識、自動販売機など野外に設置される設備、窓や入口などの建物の構成要素、道路、車両、二輪車の認識を行うことができる。

また、移動体１００外のユーザからの指示については、所有する通信装置１４０を介した指示を通信ユニット１３６を介して移動体１００へ通知したり、図６に示すように移動体１００に向けて手を挙げるなどの動作によって移動体１００を呼び寄せることも可能である。通信装置１４０による指示は、上記乗員からの指示と同様に操作入力や音声入力によって行われる。

＜大域経路の生成手法＞
以下では、図７及び図８を参照して、本実施形態に係る大域経路の生成手法について説明する。本実施形態においては、大域経路計画は、静的条件下における最適性と再現性があるＡ＊アルゴリズムを拡張する（改良型Ａ＊）。更に改良型Ａ＊をＴｈｅｔａ＊を用いて最適化することにより、グリッドに依存しない経路を生成する。

（改良型Ａ＊）
図７は、本実施形態に係る改良型Ａ＊アルゴリズムを示す。Ａ＊アルゴリズムはグリッド上のセルをノードとして扱い、毎周期で全探索を行うため、新たな障害物や目標位置の変更に対応できる利点がある。一方で、Ａ＊は毎周期の全探索の弊害として、計算量が大きく、多少のノイズの影響を受けて大きくパスが変化し、ふらつきの要因となってしまう。Ａ＊では、探索ノードの拡張は実際の移動コストとheuristic関数により決定される。heuristic関数としては、目標位置までの推定距離が用いられる。ここで、本実施形態によれば、さらに過去に計画した経路情報（例えば、前回生成した経路情報）を利用して探索ノードを決定する。このように、本実施形態によれば、改良型Ａ＊として、過去の経路を考慮することにより、探索エリアの削減と大規模な経路変化（ふらつき）の抑制を実現する。

７００は、格子地図（グリッドマップ）において、移動体１００の現在位置からの距離が遠いほどコストが高い第１コストをグリッドごとに定義するコストマップを示す。つまり、コストマップ７００は、移動体１００の移動コストをグリッドごとに定義したものであり、毎周期において生成される。７０１は、移動体１００の現在位置を示し、大域経路の開始位置となる。７０２は、生成する経路の目標位置に向けて、大域経路の終了位置となる。終了位置については、最終的な目標位置でもよく、目標位置への中途となる中継位置であってもよい。また、図中における「大」の四角は、当該位置に向けてコストが高くなることを示し、「小」の四角は当該位置に向けてコストが小さくなることを示す。「∞」は障害物が存在するグリッドのコストを無限大にすることを示す。したがって、コストマップは障害物を考慮した第１コストが定義される。

７１０は、グリッドマップにおいて、目標位置からの距離が遠いほどコストが高い第２コストをグリッドごとに定義するheuristicマップを示す。つまり、heuristicマップ７１０は、目標位置からの推定距離をグリッド ごとに定義したものである。７１０においては、右上方向が目標位置として設定されているため、右上に近づくほどコストが低く、左下に近づくほどコストが高く定義されている。

７２０は、探索ノードの決定に利用するグリッドマップであり、当該グリッドマップにおいて、過去の経路からの距離が遠いほどコストが高い第３コストをグリッドごとに定義する過去パスマップを示す。つまり、過去パスマップ７２０は、過去パスの有無をグリッド ごとに定義したものである。７２１は、過去パスを示す。本実施形態では、過去パス７２１は前回生成した大域経路を示す。しかし、本発明を限定する意図はなく、過去数回分の過去パスの累積経路を利用するものであってもよい。過去パスマップ７２０は、過去パスが通過したグリッドを”０”とし、未通過のグリッドを”１”としてグリッドマップを生成し、当該グリッドマップにガウシアンフィルタや平均化フィルタ等を適用して生成される。

これらのグリッドマップを用いて、本実施形態に係る改良型Ａ＊の探索ノードｉ＊を以下の数式１に示す評価関数で決定することができる。

ここで、ＯＰＥＮはＡ＊のＯＰＥＮＬＩＳＴに含まれるグリッドのセルのindexの集合を示す。Ｃｉ、Ｈｉ、Ｐｉはそれぞれ第１コスト、第２コスト、第３コストの値を示す。ｋは係数を示す。上記数式１により、ＯＰＥＮＬＩＳＴ内で過去パス７２１が通過した領域付近を優先的に探索することが可能になる。経路生成部３０４は、過去パス７２１が通過した領域を探索した場合であっても、目標位置までの経路が発見できない場合、Ａ＊と同様に探索範囲を広げ、経路が見つかるまで探索を継続する。

７３０は、このような改良型Ａ＊を適用して、障害物を回避するように生成された大域経路７３１を示す。上述したように、本実施形態が適用する改良型Ａ＊によれば、過去パスを考慮するため全探索を行う必要がなく、探索範囲を大幅に縮小できる。また、数式１に示すように、評価関数の計算についても行列の加算のみであり計算量を抑えることができる。さらに、障害物が存在するグリッドのコストを「∞」に設定することにより、障害物のギリギリを通過する経路を生成しづらいという効果がある。

（Ｔｈｅｔａ＊）
図８は、改良型Ａ＊によって生成した大域経路を最適化する手法を示す。８００は、改良型Ａ＊によって生成された大域経路７３１を示す。８０１は各グリッドに対応するノードを示す。改良型Ａ＊によって生成される経路は、大域経路７３１に示すようにノードに依存した経路となり、障害物が存在しないグリッド間においても直進しない経路となる可能性があり、最適な経路とならない場合がある。したがって、本実施形態によれば、上記改良型Ａ＊によって生成された大域経路７３１に対してＴｈｅｔａ＊アルゴリズムを適用することによって経路を最適化する。

８１０は、Ｔｈｅｔａ＊を適用して大域経路７３１を最適化した大域経路８１１を示す。点線で示す大域経路８１１は、改良型Ａ＊によって生成された実線で示す大域経路７３１と比較して、より直線的な経路となっていることが分かる。例えば、最適化前の大域経路７３１は、ノード８１２を中心に前後に障害物４０１に沿った軌道を形成しており、走行制御としては複数回の左右への操舵が必要となる。一方、最適化後の大域経路８１１は、ノード８１２を中心に前後に直線的に軌道が形成されており、最短経路で且つ操舵回数の少ない経路となっている。

８２０は、８１０の点線領域における改良型Ａ＊とＴｈｅｔａ＊のノード探索アルゴリズムを示す。改良型Ａ＊では、実線矢印８２１に示すように、距離が最小となるようなノードを親ノードから子ノードの方向へ探索、即ち、順方向へ探索して軌道が生成される。一方、Ｔｈｅｔａ＊による最適化では、点線矢印８２２に示すように、逆方向に子ノードから親ノードへ距離が最小となるように大域経路７３１を最適化する。つまり、Ｔｈｅｔａ＊では、最適な親ノードが探索される。これにより、グリッド（ノード）に依存しない最適な経路を生成することができる。 また、改めて移動コストに基づき最適な親ノード探索をするため、過去のパスへの過度な依存を防止することができる。

＜局所経路の生成手法＞
続いて、局所経路の生成手法について説明する。経路生成部３０４は、生成された大域経路に追従するように局所経路を生成する。 局所経路計画の手法としては、ＤＷＡ（Dynamic Window Approach）や、ＭＰＣ（Model Predictive Control）、clothoid tentacles、 ＰＩＤ(Proportional-Integral-Differential)制御など種々の手法が存在する。本実施形態では、一例としてＤＷＡを用いる場合について説明するが、本発明を限定する意図はなく、他の手法を用いてもよい。ＤＷＡは、運動学や加速度などの制約を考慮できる点で広く応用されている。本実施形態に係る移動体１００としては、尾輪付きの差動二輪型のモビリティであり、人が乗車することを想定しているため小型のモビリティの中でも比較的大型に分類されるものである。したがって、尾輪１２１の角度がモビリティの運動に大きく影響し、従来の差動二輪モデルによるＤＷＡでは目標軌道と実際の軌道が異なり、危険があるため、本実施形態においてＤＷＡを拡張し、尾輪角についての制約を導入する。

尾輪１２１は従動輪であるが、尾輪１２１の角度が移動体１００の進行方向と大きく異なる場合に、尾輪による地面からの反力が大きくなり、尾輪の向きが進行方向に戻るときに反力が急激に小さくなる。このとき尾輪１２１の向きに従ったｙａｗ方向の角速度が発生し、車両の動きが大きく乱れＤＷＡによる予測軌道と大きく離れることがある。これによる衝突を防ぐため、従来のＤＷＡの制約に加え、尾輪角に伴う制約を導入する。まず、差動二輪モビリティの尾輪角を以下の数式（２）のように推定する。
δ＝－ａｒｃｔａｎ（Ｌω／ｖ）・・・数式（２）
ここで、δは尾輪１２１の角度（尾輪角）を示し、ｖは移動体１００の速度を示し、ωは角速度を示し、Ｌはホイールベースを示す。

ＤＷＡは、速度の制約や加速度の制約、衝突の制約を考慮した速度と角速度の範囲（Ｗｉｎｄｏｗ）から最適な速度と加速度の組み合わせを決定するアルゴリズムである。これに加え、本実施形態では、尾輪角度に基づく制約を導入する。尾輪角度が進行方向と異なる際に、速度が大きいと尾輪反力を受けるため、尾輪角に応じた速度と角速度の制限を行う。例えば、速度、角速度の制約範囲（速度の最大値、最小値と、角速度の最大値、最小値）を、尾輪角が進行方向と同じ方向に戻るまで低速で走行し、尾輪１２１が進行方向に従動した後、最大速度での走行が可能となるように設ける。これにより、尾輪角の急激な変化による車両の動きの乱れを防ぎ、連続的な動きが可能になる。

＜移動体の基本制御＞
図９は、本実施形態に係る移動体１００の基本制御を示すフローチャートである。以下で説明する処理は、制御ユニット１３０において、例えばＣＰＵがＲＯＭ等のメモリに格納されたプログラムをＲＡＭに読み出して実行することにより実現される。

Ｓ１０１で制御ユニット１３０は、ユーザ指示取得部３０１によって受信したユーザ指示に基づいて、移動体１００の目標位置を設定する。ユーザ指示については、上述したように、種々の方法で受け付けることができる。続いて、Ｓ１０２で制御ユニット１３０は、検知ユニット１１４によって移動体１００の前方領域を撮像し、撮像画像を取得する。取得した撮像画像は画像情報処理部３０２によって処理され、深度画像が作成され３次元点群化される。Ｓ１０３で制御ユニット１３０は、３次元点群化された画像から、例えば５ｃｍ以上の立体物となる障害物を検出する。Ｓ１０４で制御ユニット１３０は、検出された障害物と、移動体１００の位置情報とに応じて、移動体１００を中心とした所定領域の占有格子地図を生成する。

次に、Ｓ１０５で制御ユニット１３０は、経路生成部３０４によって移動体１００の走行経路を生成する。経路生成部３０４は、上述したように、占有格子地図と、上記第１コスト乃至第３コストとを用いて大域経路を生成し、生成した大域経路に従って局所経路を生成する。続いて、Ｓ１０６で制御ユニット１３０は、生成した局所経路に従って移動体１００の速度及び角速度を決定し、走行を制御する。その後、Ｓ１０７で制御ユニット１３０は、移動体１００が目標位置へ到達したかどうかをＧＮＳＳセンサ１３４からの位置情報に基づいて判断し、目標位置へ到達していない場合は処理をＳ１０２に戻して、占有格子地図を更新しながら経路を生成し、走行を制御する処理を繰り返し行う。一方、目標位置へ到達した場合は、本フローチャートの処理を終了する。

＜経路生成制御＞
図１０は、本実施形態に係る経路生成制御（Ｓ１０５）の詳細な処理手順を示すフローチャートである。以下で説明する処理は、制御ユニット１３０において、例えばＣＰＵがＲＯＭ等のメモリに格納されたプログラムをＲＡＭに読み出して実行することにより実現される。

Ｓ２０１で制御ユニット１３０は、移動体１００の現在位置からの距離が遠いほどコストが高い第１コストをグリッドごとに定義する第１コストマップを生成する。続いて、Ｓ２０２で制御ユニット１３０は、目標位置からの距離が遠いほどコストが高い第２コストをグリッドごとに定義する第２コストマップを生成する。さらに、Ｓ２０３で制御ユニット１３０は、過去の経路からの距離が遠いほどコストが高い第３コストをグリッドごとに定義する第３コストマップを生成する。

続いて、Ｓ２０４で制御ユニット１３０は、Ｓ２０１～Ｓ２０３で生成した第１乃至第３のコストマップと、Ｓ１０４で生成した占有格子地図の情報とを用いて、上記改良型Ａ＊アルゴリズムに従って大域経路を生成する。続いて、Ｓ２０５で制御ユニット１３０は、生成した大域経路を上記Ｔｈｅｔａ＊アルゴリズムによって最適化し、最適化した大域経路を生成する。その後、Ｓ２０６で制御ユニット１３０は、Ｓ２０５で最適化した大域経路に追従するように、局所経路を生成し、本フローチャートの処理を終了する。なお、大域経路を生成して局所経路を生成する流れで説明しているが、必ずしも当該順序で各経路生成が行われるわけではない。これは大域経路の生成周期と局所経路の生成周期が異なるためである。例えば、大域経路の生成周期が１００ｍｓで局所領域の生成周期が５０ｍｓである場合には、生成された大域経路に従って、局所経路の生成が２回行われることとなる。

＜実施形態のまとめ＞
１．上記実施形態の移動体制御システム（例えば、１００）は、
撮像画像を取得する取得手段と、（１１４）
前記撮像画像に含まれる障害物を検出する検出手段と、（１３０、３０２、３０３）
前記移動体の周辺の領域を分割し、分割した領域ごとに前記検出手段によって検出された障害物の占有を示す占有地図を生成する地図生成手段と、（３０３）
前記占有地図において現在位置からの距離が遠いほどコストが高い第１コストと、前記占有地図において目標位置からの距離が遠いほどコストが高い第２コストと、前記占有地図において過去の経路からの距離が遠いほどコストが高い第３コストとに基づいて、検出した前記障害物を回避する前記現在位置から前記目標位置までの大域経路を生成する経路生成手段とを備える（３０４、Ｓ２０１～Ｓ２０４、図７）。

この実施形態によれば、占有地図上の実時間経路計画において、過去に経路計画を行ったグリッドを優先的に探索するため、パスのあったところが低くなるように疑似的なポテンシャルマップを生成し、評価関数に利用する。よって、本発明によれば、高精度地図を用いることなく移動体の経路を好適に生成することができる。

２．上記実施形態の移動体制御システムでは、前記経路生成手段は、さらに、距離が最小となる親ノードを探索するＴｈｅｔａ＊アルゴリズムを用いて、生成した前記大域経路を最適化する（３０４、Ｓ２０６、図８）。

この実施形態によれば、グリッドマップ上においてノードに依存することなく、直線的な経路を生成することができる。

３．上記実施形態の移動体制御システムでは、前記第３コストは、ガウシアンフィルタ又は平均化フィルタを用いて生成される（７２０、Ｓ２０３）。

この実施形態によれば、過去パスに基づくコストを容易に作成することができる。

４．上記実施形態の移動体制御システムでは、前記経路生成手段は、前記過去の経路に沿ってフィルタを用いることにより前記第３コストを取得する（７２０、Ｓ２０３）。

この実施形態によれば、より効率的に過去パスを利用することができる。

５．上記実施形態の移動体制御システムでは、前記過去の経路とは、前記経路生成手段によって前回生成された経路である。

この実施形態によれば、直前のパスに従うことで、探索範囲をより低減することができる。

６．上記実施形態の移動体では、前記第１コストは、さらに、前記検出手段によって検出された前記障害物に基づいて決定される（７００、Ｓ２０１）。

この実施形態によれば、障害物のギリギリを通過する経路の生成を回避することができる。

７．上記実施形態の移動体制御システムでは、前記取得手段は、前記移動体の前方領域の撮像画像を取得し（Ｓ１０２）、
前記地図生成手段は、前記取得手段によって取得されない領域の障害物について、前記検出手段によって過去に検出された障害物の情報を用いて前記占有地図を生成する（Ｓ１０３、Ｓ１０４、図５）。

この実施形態によれば、移動体が旋回して引き返すような場合であっても、過去に検出した障害物情報を用いることができ、それらの障害物に対する衝突や袋小路でのスタックを回避することができる。

８．上記実施形態の移動体制御システムでは、前記経路生成手段は、
前記大域経路に追従するように、ＤＷＡ（Dynamic Window Approach）に基づいて前記移動体の局所経路をさらに生成する（Ｓ２０６）。

この実施形態によれば、従動輪の向きを考慮した走行制御を行うことができる。

９．上記実施形態の移動体制御システムは、前記局所経路に基づいて前記移動体の速度を及び角速度を決定し、走行を制御する走行制御手段をさらに備える（Ｓ１０６）。

この実施形態によれば、従動輪の向きを考慮して走行制御を行うことができる。

１０．上記実施形態の移動体制御システムでは、前記経路生成手段は、前記大域経路及び前記局所経路をそれぞれ異なる周期で生成する。

この実施形態によれば、不要な経路生成を削減し、処理負荷を低減することができる。

１１．上記実施形態の移動体制御システムでは、前記局所経路の生成周期は、前記大域経路の生成周期よりも短い。

この実施形態によれば、不要な経路生成を削減し、処理負荷を低減することができる。

１２．上記実施形態の移動体制御システムでは、前記取得手段は、ステレオカメラであり（１１４）、
前記検出手段は、前記ステレオカメラによって撮像されたステレオ画像の画像データを３次点群化する（Ｓ１０３）。

この実施形態によれば、処理量を低減することができ、リアルタイム処理を好適に実現することができる。

１３．上記実施形態の移動体制御システムでは、前記地図生成手段は、前記移動体の周辺の領域を格子状に分割し、格子ごとに前記検出手段によって検出された障害物の占有を示す占有格子地図を、前記占有地図として生成する。

この実施形態によれば、所定の平面領域をｘ、ｙ方向に容易に分割することができるとともに、所定範囲を漏れなくカバーすることができる。

１４．上記実施形態の移動体（例えば、１００）は、
撮像画像を取得する取得手段と、（１１４）
前記撮像画像に含まれる障害物を検出する検出手段と、（１３０、３０２、３０３）
前記移動体の周辺の領域を分割し、分割した領域ごとに前記検出手段によって検出された障害物の占有を示す占有地図を生成する地図生成手段と、（３０３）
前記占有地図において現在位置からの距離が遠いほどコストが高い第１コストと、前記占有地図において目標位置からの距離が遠いほどコストが高い第２コストと、前記占有地図において過去の経路からの距離が遠いほどコストが高い第３コストとに基づいて、検出した前記障害物を回避する前記現在位置から前記目標位置までの大域経路を生成する経路生成手段とを備える（３０４、Ｓ２０１～Ｓ２０４、図７）。

この実施形態によれば、占有地図上の実時間経路計画において、過去に経路計画を行ったグリッドを優先的に探索するため、パスのあったところが低くなるように疑似的なポテンシャルマップを生成し、評価関数に利用する。よって、本発明によれば、高精度地図を用いることなく移動体の経路を好適に生成することができる。

１００…移動体、１１１…座席、１１２…走行ユニット、１１３…バッテリ、１１４…検知ユニット、１２０…前輪、１２１…尾輪、１２２ａ、１２２ｂ…モータ、１２３…操舵機構、１３０…制御ユニット、１３１…操作パネル、１３２…スピーカ、１３３…音声入力装置、１３４…ＧＮＳＳセンサ、１３５…記憶装置、１３６…通信ユニット、１４０…通信装置、３０１…ユーザ指示取得部、３０２…画像情報処理部、３０３…格子地図生成部、３０４…経路生成部、３０５…走行制御部

Claims (16)

1. 移動体制御システムであって、
   撮像画像を取得する取得手段と、
   前記撮像画像に含まれる障害物を検出する検出手段と、
   移動体の周辺の領域を 分割し、分割した領域ごとに前記検出手段によって検出された障害物の占有を示す占有地図を生成する地図生成手段と、
   前記占有地図において現在位置からの距離が遠いほどコストが高い第１コストと、前記占有地図において目標位置からの距離が遠いほどコストが高い第２コストと、前記占有地図において過去の経路からの距離が遠いほどコストが高い第３コストとに基づいて、検出した前記障害物を回避する前記現在位置から前記目標位置までの大域経路を生成する経路生成手段と
   を備えることを特徴とする移動体制御システム。
2. 前記経路生成手段は、さらに、距離が最小となる親ノードを探索するＴｈｅｔａ＊アルゴリズムを用いて、生成した前記大域経路を最適化することを特徴とする請求項１に記載の移動体制御システム。
3. 前記第３コストは、ガウシアンフィルタ又は平均化フィルタを用いて生成されることを特徴とする請求項１に記載の移動体制御システム。
4. 前記経路生成手段は、前記過去の経路に沿ってフィルタを用いることにより前記第３コストを取得することを特徴とする請求項３に記載の移動体制御システム。
5. 前記過去の経路とは、前記経路生成手段によって前回生成された経路であることを特徴とする請求項４に記載の移動体制御システム。
6. 前記第１コストは、さらに、前記検出手段によって検出された前記障害物に基づいて決定されることを特徴とする請求項２に記載の移動体制御システム。
7. 前記取得手段は、前記移動体の前方領域の撮像画像を取得し、
   前記地図生成手段は、前記取得手段によって取得されない領域の障害物について、前記検出手段によって過去に検出された障害物の情報を用いて前記占有地図を生成することを特徴とする請求項２に記載の移動体制御システム。
8. 前記経路生成手段は、
   前記大域経路に追従するように、ＤＷＡ（Dynamic Window Approach）に基づいて前記移動体の局所経路をさらに生成することを特徴とする請求項７に記載の移動体制御システム。
9. 前記局所経路に基づいて前記移動体の速度を及び角速度を決定し、走行を制御する走行制御手段をさらに備えることを特徴とする請求項８に記載の移動体制御システム。
10. 前記経路生成手段は、前記大域経路及び前記局所経路をそれぞれ異なる周期で生成することを特徴とする請求項８に記載の移動体制御システム。
11. 前記局所経路の生成周期は、前記大域経路の生成周期よりも短いことを特徴とする請求項１０に記載の移動体制御システム。
12. 前記取得手段は、ステレオカメラであり、
    前記検出手段は、前記ステレオカメラによって撮像されたステレオ画像の画像データを３次点群化することを特徴とする請求項１に記載の移動体制御システム。
13. 前記地図生成手段は、前記移動体の周辺の領域を格子状に分割し、格子ごとに前記検出手段によって検出された障害物の占有を示す占有格子地図を、前記占有地図として生成することを特徴とする請求項２に記載の移動体制御システム。
14. 移動体であって、
    撮像画像を取得する取得手段と、
    前記撮像画像に含まれる障害物を検出する検出手段と、
    前記移動体の周辺の領域を分割し、分割した領域ごとに前記検出手段によって検出された障害物の占有を示す占有地図を生成する地図生成手段と、
    前記占有地図において現在位置からの距離が遠いほどコストが高い第１コストと、前記占有地図において目標位置からの距離が遠いほどコストが高い第２コストと、前記占有地図において過去の経路からの距離が遠いほどコストが高い第３コストとに基づいて、検出した前記障害物を回避する前記現在位置から前記目標位置までの大域経路を生成する経路生成手段と
    を備えることを特徴とする移動体。
15. 移動体制御システムの制御方法であって、
    撮像画像を取得する取得工程と、
    前記撮像画像に含まれる障害物を検出する検出工程と、
    移動体の周辺の領域を分割し、分割した領域ごとに前記検出工程で検出された障害物の占有を示す占有地図を生成する地図生成工程と、
    前記占有地図において現在位置からの距離が遠いほどコストが高い第１コストと、前記占有地図において目標位置からの距離が遠いほどコストが高い第２コストと、前記占有地図において過去の経路からの距離が遠いほどコストが高い第３コストとに基づいて、検出した前記障害物を回避する前記現在位置から前記目標位置までの大域経路を生成する経路生成工程と
    を含むことを特徴とする移動体制御システムの制御方法。
16. コンピュータを、請求項１から１３のいずれか１項に記載の移動体制御システムの各手段として機能させるためのプログラム。

Images