JP7257433B2 - ビークルの経路生成方法、ビークルの経路生成装置、ビークル及びプログラム - Google Patents

Description

本発明は、ビークルの経路生成方法、ビークルの経路生成装置、ビークル及びプログラムに関する。

ビークルを目的位置まで移動させるためには、ビークルの移動経路が設定される。このようなビークルは、障害物を回避しつつ目的位置まで到達することが求められる。例えば特許文献１には、移動する障害物を回避するように、経路を生成する旨が記載されている。

特開２０２０－００４０９５号公報

しかし、障害物を回避しつつ目的位置まで到達するための経路設定には、改善の余地があり、障害物を回避可能な経路を適切に設定することが求められている。

本開示は、上述した課題を解決するものであり、障害物を回避可能な経路を適切に設定可能な、ビークルの経路生成方法、ビークルの経路生成装置、ビークル及びプログラムを提供することを目的とする。

上述した課題を解決し、目的を達成するために、本開示に係るビークルの経路生成方法は、ビークルの位置の情報を取得するステップと、前記ビークルの目的位置の情報を取得するステップと、障害物の位置の情報を取得するステップと、前記ビークルの位置、前記ビークルの大きさ、及び前記障害物の位置に基づき、先読みステップ毎の前記ビークルが前記障害物に干渉しない旨の制約条件を設定するステップと、前記先読みステップ毎の前記ビークルの位置と前記目的位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づき最適化計算を行い、前記ビークルの移動経路を算出するステップと、を含む。

上述した課題を解決し、目的を達成するために、本開示に係るビークルの経路生成装置は、ビークルの位置の情報を取得する自己位置情報取得部と、前記ビークルの目的位置の情報を取得する目的位置情報取得部と、障害物の位置の情報を取得する障害物情報取得部と、計算実行部と、を含み、前記計算実行部は、前記ビークルの位置、前記ビークルの大きさ、及び前記障害物の位置に基づき、先読みステップ毎の前記ビークルが前記障害物に干渉しない旨の制約条件を設定し、前記先読みステップ毎の前記ビークルの位置と前記目的位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づき最適化計算を行い、前記ビークルの移動経路を算出する。

上述した課題を解決し、目的を達成するために、本開示に係るビークルは、前記ビークルの経路生成装置を含む。

上述した課題を解決し、目的を達成するために、本開示に係るプログラムは、ビークルの位置の情報を取得するステップと、前記ビークルの目的位置の情報を取得するステップと、障害物の位置の情報を取得するステップと、前記ビークルの位置、前記ビークルの大きさ、及び前記障害物の位置に基づき、先読みステップ毎の前記ビークルが前記障害物に干渉しない旨の制約条件を設定するステップと、前記先読みステップ毎の前記ビークルの位置と前記目的位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づき最適化計算を行い、前記ビークルの移動経路を算出するステップと、を、コンピュータに実行させる。

本開示によれば、障害物を回避可能な経路を適切に設定できる。

図１は、第１実施形態に係るビークル制御システムの模式図である。 図２は、管理システムの模式的なブロック図である。 図３は、第１実施形態に係るビークルの模式的なブロック図である。 図４は、第１実施形態に係る制御部の模式的なブロック図である。 図５は、制約条件を説明するための模式図である。 図６は、本実施形態に係る制御装置の処理フローを説明するフローチャートである。 図７は、制約条件の他の例を説明するための模式図である。

以下に添付図面を参照して、本発明の好適な実施形態を詳細に説明する。なお、この実施形態により本発明が限定されるものではなく、また、実施形態が複数ある場合には、各実施形態を組み合わせて構成するものも含むものである。

（第１実施形態）
（制御システム）
図１は、第１実施形態に係るビークル制御システムの模式図である。図１に示すように、第１実施形態に係る制御システム１は、ビークル１０と管理システム１２とを含む。ビークル１０は、自動で移動可能な移動体であるが、それに限られず、運転者が操作する有人の移動体であってもよい。ビークル１０は、地面を走行する移動体としてもよいし、空中を飛行する移動体としてもよいし、水中を移動する移動体としてもよい。したがって、ビークル１０は、３次元で移動できる移動体も含むが、以下では説明のため、２次元平面を移動する場合として説明する。２次元平面を移動するビークル１０としては、例えば、ＡＧＦ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｆｏｒｋｌｉｆｔ）やＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）などが挙げられるが、ビークル１０の種類は任意であってよい。以下、ビークル１０が移動可能な領域、すなわちビークル１０が移動を予定している領域を、領域ＡＲとする。領域ＡＲは、本実施形態では２次元平面であり、水平方向に沿った一方向を、Ｘ方向とし、水平方向に沿った方向であってＸ方向と直交する方向を、Ｙ方向とする。なお、領域ＡＲは、３次元空間であってもよい。

本実施形態においては、ビークル１０は、移動経路Ｒに沿って移動する。移動経路Ｒは、領域ＡＲに存在する障害物Ｏを回避しつつ目的位置Ｐに向かうように、設定される。障害物Ｏは、ビークル１０が回避する対象となる対象物である。障害物Ｏは、本実施形態では、位置が変化する可能性がある任意の対象物や、あるタイミングでは存在するが別のタイミングでは存在しなくなる可能性がある任意の対象物であってよい。言い換えれば、本実施形態の障害物Ｏは、永続的に位置が固定されて位置が既知の構造物ではないといえる。なお、障害物Ｏは、無生物であることに限られず、例えば人などの生物であってもよい。移動経路Ｒの設定方法については後述する。

（管理システム）
管理システム１２は、ビークル１０を管理するシステムであり、本実施形態では、ビークル１０の目的位置Ｐを設定する。管理システム１２は、本実施形態ではＷＭＳ（Ｗａｒｅｈｏｕｓｅ Ｍａｎａｇｅｍｅｎｔ Ｓｙｓｔｅｍ）であるが、ＷＭＳに限られず任意のシステムであってよく、例えばいわゆる地上システムであってもよい。

図２は、管理システムの模式的なブロック図である。管理システム１２は、コンピュータであり、図２に示すように、通信部２０と、記憶部２２と、制御部２４とを含む。通信部２０は、ビークル１０などの外部の装置と通信を行う通信モジュールであり、例えばアンテナなどである。管理システム１２は、無線通信で通信を行うが、通信方式は任意であってよい。記憶部２２は、制御部２４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）のような主記憶装置と、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）などの外部記憶装置とのうち、少なくとも１つ含む。記憶部２２が記憶する制御部２４用のプログラムは、管理システム１２が読み取り可能な記録媒体に記憶されていてもよい。

制御部２４は、演算装置であり、例えばＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの演算回路を含む。制御部２４は、目的位置情報取得部２６を含む。制御部２４は、記憶部２２からプログラム（ソフトウェア）を読み出して実行することで、目的位置情報取得部２６を実現して、その処理を実行する。なお、制御部２４は、１つのＣＰＵによってこの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、目的位置情報取得部２６を、ハードウェア回路で実現してもよい。

目的位置情報取得部２６は、ビークル１０の目的位置Ｐの位置情報を取得する。目的位置情報取得部２６は、例えばビークル１０が実行する作業内容を設定し、作業内容に応じて目的位置Ｐを設定する。ただし、目的位置情報取得部２６による目的位置Ｐの位置情報の取得方法は任意であり、例えばユーザにより指定されてもよい。目的位置情報取得部２６は、ビークル１０の目的位置Ｐの位置情報を、通信部２０を介して、そのビークル１０に送信する。

なお、管理システム１２は必須の構成でなく、例えば、ビークル１０が目的位置Ｐを設定してもよい。

（ビークル）
図３は、第１実施形態に係るビークルの模式的なブロック図である。図３に示すように、ビークル１０は、制御装置３０と、通信部３２と、自己位置検出部３４と、障害物検出部３６と、動力部３８とを有する。

（通信部）
通信部３２は、外部の装置と通信を行う通信モジュールであり、例えばアンテナなどである。ビークル１０は、無線通信で通信を行うが、通信方式は任意であってよい。本実施形態では、ビークル１０は、通信部３２を介して管理システム１２と通信して、情報の送受信を行う。

（自己位置検出部）
自己位置検出部３４は、ビークル１０の位置及び姿勢、すなわち自己位置及び自己姿勢を検出する装置である。ビークル１０の位置とは、本実施形態においては、領域ＡＲ内においてビークル１０が位置している座標を指す。ビークル１０の姿勢とは、ビークル１０が向いている方向を指し、本実施形態では、方向Ｘ及び方向Ｙに直交する方向から見た場合のビークル１０の向き（回転角度）を指す。以降においても、特に断りがない限り、「位置」及び「姿勢」は、同様の意味を指す。自己位置検出部３４は、任意の方法で位置及び姿勢を検出してよいが、例えば、自己位置検出部３４の具体的構成例として、グローバル・ポジショニング・システム（ＧＰＳ：Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）等の測位システムを利用して位置を検出するための測位装置が挙げられる。また例えば、自己位置検出部３４は、所定の起点に対する位置を検出する慣性航法装置であってもよい。また例えば、自己位置検出部３４は、レーザ光により位置及び姿勢を検出するものであってもよい。この場合例えば、領域ＡＲに反射体が設けられており、自己位置検出部３４が反射体に向けてレーザ光を照射し、反射体から反射したレーザ光を検出することで、位置及び姿勢が検出できる。

（障害物検出部）
障害物検出部３６は、障害物Ｏの位置及び姿勢を検出するセンサである。障害物検出部３６は、障害物Ｏの位置及び姿勢を検出可能であれば任意のセンサであってよく、例えば、２Ｄ－ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）、３Ｄ－ＬｉＤＡＲ、カメラなどであってよい。

（動力部）
動力部３８は、ビークル１０を移動させる動力として機能する。動力部３８の具体的構成は、ビークル１０の運用形態に応じる。一例として、ビークル１０が地上を走行するビークルである場合、動力部３８は、複数の車輪と、当該複数の車輪の一部又は全部を駆動する原動機を含む。ここに例示した動力部３８の具体的構成はあくまで一例であってこれに限られるものでない。動力部３８は、ビークル１０を移動可能にする動力として機能すればよい。

（制御装置）
制御装置３０は、ビークル１０の動作を制御する装置である。制御装置３０は、コンピュータであり、制御部４０と記憶部４２とを備える。記憶部４２は、制御部４０の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭと、ＲＯＭのような主記憶装置と、ＨＤＤなどの外部記憶装置とのうち、少なくとも１つ含む。記憶部４２が記憶する制御部４０用のプログラムは、制御装置３０が読み取り可能な記録媒体に記憶されていてもよい。

図４は、第１実施形態に係る制御部の模式的なブロック図である。制御部４０は、演算装置であり、例えばＣＰＵなどの演算回路を含む。図４に示すように、制御部４０は、目的位置情報取得部５０と、自己位置情報取得部５２と、障害物情報取得部５４と、計算実行部５６と、駆動制御部５８とを含む。制御部４０は、記憶部４２からプログラム（ソフトウェア）を読み出して実行することで、目的位置情報取得部５０と自己位置情報取得部５２と障害物情報取得部５４と計算実行部５６と駆動制御部５８とを実現して、それらの処理を実行する。なお、制御部４０は、１つのＣＰＵによってこれらの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、これらの処理を実行してもよい。また、目的位置情報取得部５０と自己位置情報取得部５２と障害物情報取得部５４と計算実行部５６と駆動制御部５８との少なくとも一部を、ハードウェア回路で実現してもよい。

（目的位置情報取得部）
目的位置情報取得部５０は、ビークル１０の目標とする移動先である目的位置Ｐの位置情報を取得する。目的位置情報取得部５０は、通信部３２を介して、管理システム１２から、目的位置Ｐの位置情報を取得する。ただし、目的位置情報取得部５０は、管理システム１２から目的位置Ｐの位置情報を取得することに限られず、自身で目的位置Ｐを設定してもよい。

（自己位置情報取得部）
自己位置情報取得部５２は、ビークル１０自身の位置及び姿勢の情報を取得する。自己位置情報取得部５２は、自己位置検出部３４を制御して、ビークル１０自身の位置情報（座標情報）及び姿勢情報（向きを示す情報）を取得する。以下、位置情報及び姿勢情報を合わせて、適宜、位置姿勢情報と記載する。自己位置情報取得部５２は、所定時間毎に、逐次、ビークル１０の位置姿勢情報を取得する。ただし、自己位置情報取得部５２は、自身でビークル１０の位置及び姿勢を検出することに限られず、例えば、管理システム１２などの外部の装置がビークル１０の位置及び姿勢を検出し、自己位置情報取得部５２が、その検出結果をビークル１０の位置姿勢情報として取得してもよい。

（障害物情報取得部）
障害物情報取得部５４は、障害物Ｏの位置の情報を取得する。障害物情報取得部５４は、障害物検出部３６を制御して、障害物Ｏの位置情報を取得する。障害物情報取得部５４は、所定時間毎に、逐次、障害物Ｏの位置情報を取得する。ただし、障害物情報取得部５４は、自身で障害物Ｏの位置を検出することに限られず、例えば、管理システム１２などの外部の装置が障害物Ｏの位置を検出し、障害物情報取得部５４が、その検出結果を障害物Ｏの位置姿勢情報として取得してもよい。なお、障害物情報取得部５４は、障害物Ｏの位置に加えて、障害物Ｏの姿勢も取得してよい。

（計算実行部）
計算実行部５６は、最適化計算を実行して、最適化されたビークル１０の移動経路Ｒを実現できるビークル１０の駆動条件を算出する。駆動条件とは、ビークル１０の動力部３８を動作させるための入力値を指す。本実施形態では、計算実行部５６は、最適化計算の実行のために、先読みステップ毎のビークル１０の位置が障害物Ｏに干渉しない旨の制約条件を設定する。先読みステップとは、現在（障害物Ｏの位置やビークル１０の位置及び姿勢が直近で検出されたタイミング）以降の、離散化した各時刻を意味する。そして、計算実行部５６は、先読みステップ毎のビークル１０の位置と目的位置Ｐとの偏差が小さくなるほど評価が高くなる評価関数と、制約条件とに基づき、最適化計算を行い、最適化計算で最適化されたビークル１０の移動経路Ｒを実現できるビークルの駆動条件を、算出する。以下、計算実行部５６による最適化計算を具体的に説明する。なお、以降では、Ｘ方向及びＹ方向の二次元座標平面上でのビークル１０の運動を例として説明するが、三次元座標上での運動モデルに対しても適用可能である。

（先読みステップ毎のビークルの位置）
ここで、計算実行部５６によって算出される最適化された移動経路Ｒは、先読みステップ毎の（現在以降の離散時刻毎の）ビークル１０の位置及び姿勢の集合といえ、未来の経路であるといえる。ビークル１０のＸ方向及びＹ方向での位置（座標）を［ｘ，ｙ］^Ｔ、ビークル１０の姿勢をθ、先読みステップ（離散時刻）をｋとすると、先読みステップｋでのビークル１０の位置及び姿勢は、すなわち先読みステップｋでの移動経路Ｒは、次の式（１）で表される。なお、Ｔは転置を示す。なお、上述のように、本実施形態の説明は、二次元平面座標上での運動を例としたものであり、例えば三次元座標上での運動モデルとする場合には、以降の式などは三次元座標に合わせたものとなる。

この場合、ビークル１０の運動モデルは、次の式（２）のように表される。

式（２）のΔｔは、（ｋ）から（ｋ＋１）への遷移に係る時間幅である。Δｔは、移動経路Ｒの更新周期（移動経路Ｒを更新してから次に移動経路Ｒを更新するまでの時間幅）と同じであってもよいが、例えば、更新周期よりも短くてもよいし、更新周期よりも長くてもよい。Δｔを更新周期より短くすることで、詳細な移動経路Ｒを生成でき、Δｔを更新周期より長くすることで、計算負荷を低減できる。また、ｕ（ｋ）は、システムの入力であり、ビークル１０の駆動条件を指す。本実施形態の例では、ｕ（ｋ）は、ビークル１０の速度ｖ（ｋ）、ビークル１０の操舵角Φ（ｋ）を用いて、次の式（３）のように表される。

二次元平面座標上での運動を想定した場合、システムの状態方程式であるｆは、一例として、次の式（４）のように表される。なお、Ｌは、ホイールベースを指し、例えば前輪と後輪との間の距離である。

（制約条件）
計算実行部５６は、最適化計算に用いる制約条件を設定する。計算実行部５６は、ビークル１０の位置、ビークル１０の大きさ、及び障害物Ｏの位置に基づき、先読みステップ毎のビークル１０が障害物Ｏに干渉しない旨を、制約条件として設定する。すなわち、本実施形態での制約条件は、それぞれの先読みステップにおいて、その大きさのビークル１０が障害物Ｏに干渉しない、というものである。なお、ビークル１０の大きさとは、本実施形態の例では、ビークル１０の幅及び長さ（Ｘ方向及びＹ方向における長さ）である。ビークル１０の大きさは、例えば設計情報として記憶部４２などに予め記憶されており、計算実行部５６は、記憶部４２からビークル１０の大きさの情報を読み出す。ただし、計算実行部５６は、ビークル１０の大きさの情報を任意の方法で取得してよい。

図５は、制約条件を説明するための模式図である。制約条件の設定方法についてより具体的に説明する。図５に示すように、計算実行部５６は、ビークル１０の大きさに基づき、先読みステップでのビークル１０の位置を含む領域であるビークル領域ＡＲを設定する。計算実行部５６は、先読みステップでのビークル１０の位置を、ビークル１０の大きさに対応する面積分広げた領域を、ビークル領域ＡＲとして設定する。そして、計算実行部５６は、障害物Ｏの位置が、先読みステップ毎のビークル領域ＡＲの範囲外となることを、制約条件として設定する。すなわち、計算実行部５６は、先読みステップ毎にビークル領域ＡＲを設定し、各先読みステップにおいて障害物Ｏの位置がビークル領域ＡＲの範囲外となることを、制約条件として設定する。なお、図５の例では、先読みステップでのビークル１０の位置［ｘ（ｋ），ｙ（ｋ）］を中心とし、ビークル１０のＸ方向の長さＤｘを長径としＹ方向の長さＤｙを短径とする楕円形の領域を、ビークル領域ＡＲとして設定している。ただし、ビークル領域ＡＲの設定方法はこれに限られず任意であり、例えばビークル領域ＡＲを楕円形にしなくてもよい。

本実施形態では、計算実行部５６は、制約条件を設定する際に、先読みステップ毎のビークル１０の位置及び姿勢を、仮に設定する。そして、計算実行部５６は、ビークル１０の座標系における障害物Ｏの位置（座標）を、先読みステップ毎に算出する。すなわち、計算実行部５６は、障害物情報取得部５４によって取得された、方向Ｘ及び方向Ｙの座標系における障害物Ｏの位置（座標）を、先読みステップでのビークル１０の位置を座標中心とした位置（座標）に変換する。例えば、計算実行部５６は、次の式（５）を用いて、ビークル１０の座標系における障害物Ｏの位置を算出する。

なお、［ｘ’_ｏｂ，ｙ’_ｏｂ］が、障害物情報取得部５４によって取得された方向Ｘ及び方向Ｙの座標系における障害物Ｏの位置であり、［ｘ_ｏｂ（ｋ），ｙ_ｏｂ（ｋ）］^Ｔが、先読みステップｋにおける、ビークル１０の座標系における障害物Ｏの位置である。

そして、計算実行部５６は、制約条件として、次の式（６）を設定する。式（６）は、各先読みステップにおいて、障害物Ｏの位置が、楕円形のビークル領域ＡＲの範囲外となることを示した式である。ただし、式（６）は、ビークル領域ＡＲを楕円形とした場合の制約条件の式の一例であり、実際の制約条件の式は、ビーク領域ＡＲの形状や大きさに応じて設定される。

なお、このように各先読みステップで制約条件を解くことができるのは、運動モデルをもとに、先読みステップでのビークル１０の位置及び姿勢を予測計算して仮に設定しているためである。

（最適化計算）
計算実行部５６は、上記のように設定した制約条件と、先読みステップ毎のビークル１０の位置と目的位置Ｐとの偏差が小さくなるほど評価が高くなる評価関数とに基づき、最適化計算を実行する。すなわち、計算実行部５６は、モデル予測制御の考え方を用い、制約条件を満たす移動経路を予測（先読）し、各先読みステップでのビークル１０と目的位置Ｐの関係において、距離が短いほど、評価が高くなる評価関数を用いて、ビークル１０の各予測ステップの位置に関するビークル１０と目的位置の偏差を評価して、移動経路Ｒを特定する。これにより、計算実行部５６は、最適な移動経路Ｒを、すなわち先読みステップ毎の最適なビークル１０の位置及び姿勢を、特定できる。言い換えれば、計算実行部５６は、先読みステップ毎のビークル１０の位置が障害物Ｏに干渉しない旨の制約条件を満たし、かつ、先読みステップ毎のビークル１０の位置と目的位置Ｐとの偏差が最小となるような、先読みステップ毎の最適なビークル１０の位置及び姿勢（すなわち最適な移動経路Ｒ）を、特定する。

本実施形態では、移動経路Ｒを最適化する問題Ｖ（ｕ，ｐ，ｋ）が、次の式（７）で表されると想定する。

ここで、Ｊ（ｕ，ｐ，ｋ）は、最適化の目的を表す評価関数であり、経路計画であれば目的位置への接近や到達が含まれるような関数に設定される。本実施形態の例では、計算実行部５６は、評価関数Ｊ（ｕ，ｐ，ｋ）を、次の式（８）のように設定する。

ここで、ｐ_ｒ＝［ｘ（ｋ），ｙ（ｋ），θ（ｋ）］^Ｔは、すなわち式（８）でのｘ_ｒ、ｙ_ｒ、θｒは、目的となる位置及び姿勢である。また、Ａ_１、Ａ_２、Ａ_３は、重み係数であり任意に設定されてよい。この評価関数の元では、ビークル１０の各先読みステップｋにおける位置及び姿勢（ｐ（ｋ））と目的の位置及び姿勢（ｐ_ｒ）との偏差が小さくなるように最適化される。すなわち、ビークル１０が目的の位置及び姿勢に最短距離で近づくような最適化結果となる。

以上のような最適化計算を実行することで、計算実行部５６は、最適化計算のアウトプット（最適解）として、先読みステップ毎の駆動条件（システムの入力）であるｕ（ｋ）の値を得る。計算実行部５６が得る先読みステップ毎の駆動条件は、最適化された移動経路Ｒを実現するための駆動条件であるといえる。計算実行部５６は、最適化計算で得られた先読みステップ毎の駆動条件に基づき、最適化された移動経路Ｒを算出してよい。例えば、計算実行部５６は、最適化計算で得られたｕ（ｋ）を式（２）に代入して、最適化された移動経路Ｒを算出してもよい。

なお、最適化問題を解く手法としては、例として逐次二次計画法のような非線形最適化手法を用いてよい。ここで用いる最適化手法は、制約条件を課した最適化問題を解くことが可能である必要がある。なお、例えば式（２）や制約条件が非線形関数になる場合には、非線形最適化を用いることが好ましく、線形最適化手法は適さない可能性がある。

（駆動制御部）
駆動制御部５８は、計算実行部５６が取得した駆動条件に基づき、ビークル１０を制御する。駆動制御部５８は、計算実行部５６が取得した駆動条件で、動力部３８を駆動する。これにより、駆動制御部５８は、ビークル１０を、最適化された移動経路Ｒに沿って移動させる。

なお、以上の説明では、計算実行部５６は、最適化計算を行うことで、最適化された移動経路Ｒを実現するための、先読みステップ毎の駆動条件をアウトプットとして算出していたが、それに限られない。例えば、計算実行部５６は、最適化計算を行うことで、最適化された移動経路Ｒを算出してもよい。この場合、計算実行部５６は、最適化された移動経路Ｒに基づき、先読みステップ毎の駆動条件を算出してよい。また、制御装置３０の計算実行部５６が移動経路Ｒを算出し、別の装置やプログラムが、制御装置３０から移動経路Ｒの情報を取得して、駆動条件を算出してもよい。すなわち、制御装置３０は、移動経路Ｒを設定する経路生成装置であると言い換えることもできる。

（処理フロー）
次に、本実施形態の処理フローを説明する。図６は、本実施形態に係る制御装置の処理フローを説明するフローチャートである。移動経路Ｒを設定する場合、制御装置３０は、目的位置情報取得部５０により、目的位置Ｐの位置情報を取得する。また、制御装置３０は、自己位置情報取得部５２及び障害物情報取得部５４により、ビークル１０の位置姿勢情報と、障害物Ｏの位置情報とを取得する（ステップＳ１０）。そして、制御装置３０は、計算実行部５６により、ビークル１０の位置姿勢情報及び目的位置Ｐの位置情報に基づき、先読みステップ毎の、ビークル１０の位置及び姿勢を仮に設定する（ステップＳ１２）。そして、計算実行部５６は、先読みステップ毎のビークル１０の位置及び姿勢と、ビークル１０の大きさと、障害物Ｏの位置情報とに基づき、障害物Ｏとビークル１０とが干渉しない旨の制約条件を設定し（ステップＳ１６）、設定した制約条件と、評価関数とに基づき、最適化計算を実行する（ステップＳ１８）。最適解が得られなかった場合には（ステップＳ２０；Ｎｏ）、ステップＳ１２に戻り、ビークル１０の位置及び姿勢を設定し直し、最適解が得られるまで計算を繰り返す。最適化計算の結果、最適解が得られた場合には（ステップＳ２０；Ｙｅｓ）、計算実行部５６は、最適解として、最適な移動経路Ｒを実現できる駆動条件を取得する。駆動制御部５８は、取得された駆動条件に基づき、ビークル１０を制御して（ステップＳ２４）、移動経路Ｒに沿ってビークル１０を移動させる。そして、移動経路Ｒを更新する場合には（ステップＳ２６；Ｙｅｓ）、すなわち例えば更新周期に達したら、ステップＳ１０に戻り、ビークル１０及び障害物Ｏの最新位置を取得して、それに基づき以降の処理を繰り返して移動経路Ｒを更新する。移動経路Ｒを更新しない場合であって（ステップＳ２６；Ｎｏ）、処理を終了しない場合には（ステップＳ２８；Ｎｏ）、ステップＳ２４に戻り、今の移動経路Ｒでの移動を継続させる。処理を終了する場合には（ステップＳ２８；Ｙｅｓ）、本処理を終了する。

（効果）
このように、本実施形態においては、最適化計算を用いて移動経路Ｒを設定するため、障害物Ｏを回避するための制約条件を、式などを用いて適切に設定することが可能となり、目的位置に向かいつつ障害物Ｏを回避する移動経路Ｒを、適切に設定できる。また、本実施形態においては、ビークル１０の大きさも考慮して、制約条件を設定しており、例えば障害物Ｏがビークル領域ＡＲの範囲外となることを、制約条件としている。これにより、障害物Ｏを回避可能な経路を適切に設定することができる。さらに言えば、ビークル１０の予測した位置にビークル１０の大きさも反映できるため、障害物Ｏと重なることを禁止する占有領域（ここではビークル領域ＡＲ）を過大に設定する必要がなく、迂回経路を小さくすることもできる。また、目的との偏差を小さくする評価関数を用いることで、障害物Ｏを回避しつつ、最短の経路を生成することができる。なお、最適化計算に制約条件を課す方法は、上記の説明での方法に限られず、例えば、目的関数Ｖ（ｕ，ｐ，ｋ）や評価関数Ｊ（ｕ，ｐ，ｋ）に制約条件を組み込んでよい。また、以上の説明では、障害物Ｏが単数の場合を例にしたが、障害物Ｏを複数としてもよい。

また、本実施形態においては、最適化計算を用いて移動経路Ｒを設定するため、実際のビークル１０のキネマティクスを考慮することで、実現可能な移動経路Ｒを生成できる。例えば、非ホロノミック系で真横に移動できない運動拘束をもつ車両の場合、その運動拘束を考慮した移動経路Ｒを生成できる。このような運動拘束についての条件は、例えば式（２）に反映できる。

なお、本実施形態においては、ビークル１０が制御装置３０を有し、ビークル１０が有する制御装置３０が、最適化計算などの移動経路Ｒの設定処理を行っていた。ただし、最適化計算などの移動経路Ｒの設定処理は、ビークル１０が行うことに限られず、管理システム１２などの外部の装置が行ってもよい。この場合、ビークル１０は、最適化計算で得られた駆動条件や移動経路Ｒを外部の装置から取得して、取得した駆動条件や移動経路Ｒを用いて、ビークル１０（動力部３８）を制御してもよい。言い換えれば、制御装置３０は、ビークル１０が有していてもよいし、ビークル１０以外の装置が有していてもよい。さらに言えば、目的位置情報取得部５０と自己位置情報取得部５２と障害物情報取得部５４と計算実行部５６との機能は、ビークル１０が有していてもよいし、他の装置が有していてもよい。

（制約条件の他の例）
以下、最適化計算に用いる制約条件の他の例について説明する。以降の例でも、駆動条件をアウトプットとして、移動経路Ｒを算出してもよいし、最適化された移動経路Ｒを実現可能な駆動条件を算出してもよい。

（障害物の移動量の考慮）
例えば、計算実行部５６は、障害物Ｏの移動量Δｄにも基づき、先読みステップ毎のビークル１０の位置が障害物Ｏに干渉しない旨の制約条件を設定してよい。ここでの移動量Δｄは、障害物Ｐが移動すると予測される距離を指す。この場合、計算実行部５６は、現在（障害物Ｏの位置を取得したタイミング）から先読みステップのタイミングまでの間に障害物Ｏが移動する移動量Δｄの情報を取得し、その移動量Δｄだけ障害物Ｏが移動した場合にも、ビークル１０の位置が障害物Ｏに干渉しないことを、制約条件として設定する。例えば、計算実行部５６は、次の式（９）を用いて、移動量Δｄを算出してよい。

ここで、ｖ_ｏｂは、障害物Ｏの速度である。ｖ_ｏｂは、任意の方法で取得されてよい。例えば、ｖ_ｏｂは、予め設定されていてもよいし、計算実行部５６によって検出されてもよい。計算実行部５６は、ｖ_ｏｂを検出する場合は、速度計で検出した値を取得してもよいし、例えば自己位置情報取得部５２が取得したタイミング毎のビークル１０の位置に基づき算出してもよい。

図７は、制約条件の他の例を説明するための模式図である。本例においては、計算実行部５６は、先読みステップでのビークル１０の位置を、ビークル１０の大きさと移動量Δｄとに対応する面積分広げた領域を、ビークル領域ＡＲとして設定する。そして、計算実行部５６は、障害物Ｏの位置が、先読みステップ毎のビークル領域ＡＲの範囲外となることを、制約条件として設定する。これにより、障害物Ｏが移動したとしても、ビークル１０に干渉することを抑制できる。

（障害物の大きさの考慮）
例えば、計算実行部５６は、障害物Ｏの大きさにも基づき、先読みステップ毎のビークル１０の位置が障害物Ｏに干渉しない旨の制約条件を設定してよい。この場合、計算実行部５６は、障害物Ｏの大きさの情報を取得し、その大きさの障害物Ｏに対してビークル１０が干渉しないことを、制約条件として設定する。なお、障害物Ｏの大きさとは、本実施形態の例では、障害物Ｏの幅及び長さ（Ｘ方向及びＹ方向における長さ）である。障害物Ｏは、障害物Ｏの大きさを任意の方法で取得してよく、例えば障害物検出部３６の検出結果から障害物Ｏの大きさを算出してもよいし、予め設定された障害物Ｏの大きさの情報を取得してもよい。

本例においては、計算実行部５６は、先読みステップでのビークル１０の位置を、ビークル１０の大きさと障害物Ｏの大きさとに対応する面積分広げた領域を、ビークル領域ＡＲとして設定する。そして、計算実行部５６は、障害物Ｏの位置が、先読みステップ毎のビークル領域ＡＲの範囲外となることを、制約条件として設定する。これにより、障害物Ｏがビークル１０に干渉することを、より好適に抑制できる。なお、計算実行部５６は、障害物Ｏの移動量Δｄと、障害物Ｏの大きさとの両方を制約条件に組み込んでよい。この場合例えば、計算実行部５６は、先読みステップでのビークル１０の位置を、ビークル１０の大きさと障害物Ｏの大きさと障害物Ｏの移動量Δｄとに対応する面積分広げた領域を、ビークル領域ＡＲとして設定する。

（駆動条件の考慮）
例えば、計算実行部５６は、駆動条件が所定範囲となることも、制約条件として設定してよい。すなわち、計算実行部５６は、移動速度や操舵角度などの、ビークル１０の性能や運用条件によって決まるシステムの入力の上下限値を設定する。これにより、ビークル１０が実現可能な移動経路Ｒを、適切に生成できる。例えば、計算実行部５６は、駆動条件を所定範囲とする制約条件を、次の式（１０）のように設定してよい。

なお、ｕ_ｍｉｎは駆動条件の下限値であり、ｕ_ｍａｘは駆動条件の上限値であり、ビークル１０の性能や運用条件などに基づき、それぞれ任意に設定されてよい。

また、計算実行部５６は、駆動条件の単位時間当たりの変化量が所定範囲になることも、制約条件として設定してよい。すなわち、計算実行部５６は、移動加速度や操舵角加速度などの、ビークル１０の性能や運用条件によって決まるシステムの入力の時間変化レートの上下限値を設定して、実現可能な移動経路Ｒを生成する。例えば、計算実行部５６は、駆動条件を所定範囲とする制約条件を、次の式（１１）のように設定してよい。

なお、ａ_ｍｉｎは駆動条件の単位時間当たりの変化量の下限値であり、ａ_ｍａｘは駆動条件の単位時間当たりの変化量の上限値であり、ビークル１０の性能や運用条件などに基づき、それぞれ任意に設定されてよい。また、例えば式（１１）の｛ｕ（ｋ＋１）－ｕ（ｋ）／Δｔ｝の項を、二乗した値としたり、絶対値としたりしてもよい。

（ビークルの位置の考慮）
例えば、計算実行部５６は、先読みステップでのビークル１０の位置及び姿勢が所定範囲となることも、制約条件として設定してよい。すなわち、計算実行部５６は、構造物など予め位置が既知の構造物がある領域など、ビークル１０が進入できない領域を予め把握しておき、ビークル１０が進入できる範囲内に、ビークル１０が位置するように、制約条件を設定する。これにより、ビークル１０が実現可能な移動経路Ｒを、適切に生成できる。例えば、計算実行部５６は、ビークル１０の位置及び姿勢を所定範囲とする制約条件を、次の式（１２）のように設定してよい。

なお、ｐ_ｍｉｎはビークル１０の位置及び姿勢の下限値であり、ｐ_ｍａｘはビークル１０の位置及び姿勢の上限値であり、構造物の位置などに基づき、それぞれ任意に設定されてよい。なお、ここではビークル１０の位置及び姿勢が所定範囲となることを、制約条件として設定していたが、ビークル１０の位置及び姿勢の両方に制約条件をかけることに限られず、ビークル１０の位置と姿勢との少なくとも一方が所定範囲となることを、制約条件としてよい。

（第２実施形態）
次に、第２実施形態について説明する。第１実施形態においては、駆動条件をビークル１０の速度及び操舵角としていたが、第２実施形態においては、駆動条件をビークル１０の速度及び旋回曲率とする点で、第１実施形態とは異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。第２実施形態でも、駆動条件をアウトプットとして、移動経路Ｒを算出してもよいし、最適化された移動経路Ｒを実現可能な駆動条件を算出してもよい。

第２実施形態においては、計算実行部５６は、駆動条件であるシステムの入力を、ビークル１０の速度とビークル１０の旋回曲率とする。旋回曲率とは、現在の操舵角でビークル１０が移動したと仮定した場合の、ビークル１０が描く円の半径（曲率半径）の逆数を指す。例えば、先読みステップｋにおける旋回曲率ｃ（ｋ）は、次の式（１３）のように定義される。

第１実施形態では、ビークルの速度と操舵角とを駆動条件としたため、ビークル１０の運動モデルは、式（２）のように表されていたが、第２実施形態のように駆動条件をビークル１０の速度と旋回曲率とした場合、ビークル１０の運動モデルは、例えば次の式（１４）のように表される。

第２実施形態においても、計算実行部５６は、駆動条件をビークル１０の速度と旋回曲率に変更した点以外は、第１実施形態と同様の方法で最適化計算を実施し、駆動条件の最適解を得る。ただし、第２実施形態における駆動条件の最適解は、ビークル１０の速度と旋回曲率として、取得される。計算実行部５６は、最適解として得られた旋回曲率を、例えば式（１３）に代入して、操舵角を算出して、その操舵角を駆動条件として、ビークル１０を制御してもよい。

なお、第２実施形態のように旋回曲率を駆動条件とした場合でも、駆動条件に制約条件を設けることが可能である。この場合例えば、旋回曲率そのものに制限値を設定してもよいし、次の式（１５）のように旋回曲率を操舵角に変換して制限値を設定してもよい。なお、ｃ_ｍａｘは、旋回曲率の制限値であり、Φ_ｍａｘは、操舵角の制限値である。

第２実施形態のように、旋回曲率を駆動条件とすることで、最適化計算において三角関数のｔａｎΦを排除できるため、計算負荷を低減することが可能となる。

（本開示の効果）
以上説明したように、本開示に係るビークル１０の経路生成方法は、ビークル１０の位置の情報を取得するステップと、ビークル１０の目的位置Ｐの情報を取得するステップと、障害物Ｏの位置の情報を取得するステップと、ビークル１０の位置、ビークル１０の大きさ、及び障害物Ｏの位置に基づき、先読みステップ毎のビークル１０が障害物Ｏに干渉しない旨の制約条件を設定するステップと、先読みステップ毎のビークル１０の位置と目的位置Ｐとの偏差が小さくなるほど評価が高くなる評価関数と、制約条件とに基づき最適化計算を行い、ビークル１０の移動経路Ｒを算出するステップと、を含む。本方法によると、ビークル１０の大きさも考慮して制約条件を設定し、最適化計算を用いて移動経路Ｒを設定することで、障害物Ｏを回避可能な経路を適切に設定できる。さらに言えば、ビークル１０の予測した位置にビークル１０の大きさも反映できるため、障害物Ｏと重なることを禁止する占有領域（ここではビークル領域ＡＲ）を過大に設定する必要がなく、迂回経路を小さくすることもできる。

制約条件を設定するステップにおいては、ビークル１０の大きさに基づき、先読みステップでのビークル１０の位置を含む領域であるビークル領域ＡＲを設定し、障害物Ｏの位置が、先読みステップ毎のビークル領域ＡＲの範囲外となることを、制約条件として設定する。本方法によると、このようにビークル領域ＡＲを制約条件に用いることで、障害物Ｏを回避可能な経路を適切に設定できる。

制約条件を設定するステップにおいては、障害物Ｏの移動量にも基づき、前記ビークル領域を設定する。このように障害物Ｏの移動量もビークル領域ＡＲに含めることで、障害物Ｏを回避可能な経路を適切に設定できる。

制約条件を設定するステップにおいては、障害物の大きさにも基づき、前記ビークル領域を設定する。このように障害物Ｏの大きさもビークル領域ＡＲに含めることで、障害物Ｏを回避可能な経路を適切に設定できる。

制約条件を設定するステップにおいては、ビークル１０の駆動条件が所定範囲となることも、制約条件として設定する。このように駆動条件にも制約条件を課すことで、ビークル１０が実現可能な移動経路Ｒを、適切に生成できる。

制約条件を設定するステップにおいては、先読みステップでのビークル１０の位置が所定範囲となることも、制約条件として設定する。このようにビークル１０の位置にも制約条件を課すことで、ビークル１０が実現可能な移動経路Ｒを、適切に生成できる。

本経路生成方法は、ビークル１０の移動経路Ｒに基づきビークル１０の駆動条件を算出するステップをさらに含む。これにより、最適化された移動経路Ｒを実現可能なビークル１０の駆動条件が算出されて、ビークル１０を適切に制御できる。

本経路生成方法は、ビークル１０の駆動条件を、ビークル１０の速度及び操舵角としてよい。ビークル１０の速度及び操舵角を駆動条件として最適化計算を行うことで、アウトプットとしても速度及び操舵角を得ることができ、ビークル１０を適切に制御できる。

本経路生成方法は、ビークル１０の駆動条件を、ビークル１０の速度及び旋回曲率としてよい。ビークル１０の旋回曲率を駆動条件として最適化計算を行うことで、最適化計算の計算負荷を低減して、処理時間を短縮できる。

本経路生成方法は、設定されたビークル１０の駆動条件に基づきビークル１０を制御するステップを含む。本経路生成方法によると、ビークル１０に適切に障害物Ｏを回避させることができる。

本開示の制御装置３０(経路生成装置)は、ビークル１０の位置の情報を取得する自己位置情報取得部５２と、ビークル１０の目的位置Ｐの情報を取得する目的位置情報取得部５０と、障害物Ｏの位置の情報を取得する障害物情報取得部５４と、計算実行部５６とを含む。計算実行部５６は、ビークル１０の位置、ビークル１０の大きさ、及び障害物Ｏの位置に基づき、先読みステップ毎のビークル１０が障害物Ｏに干渉しない旨の制約条件を設定する。そして、計算実行部５６は、先読みステップ毎のビークル１０の位置と目的位置Ｐとの偏差が小さくなるほど評価が高くなる評価関数と、制約条件とに基づき最適化計算を行い、ビークル１０の移動経路Ｒを算出する。この制御装置３０によると、障害物Ｏを回避可能な経路を適切に設定できる。

本開示のビークル１０は、制御装置３０を含む。このビークル１０は、障害物Ｏを回避できる。

本開示のプログラムは、ビークル１０の位置の情報を取得するステップと、ビークル１０の目的位置Ｐの情報を取得するステップと、障害物Ｏの位置の情報を取得するステップと、ビークル１０の位置、ビークル１０の大きさ、及び障害物Ｏの位置に基づき、先読みステップ毎のビークル１０が障害物Ｏに干渉しない旨の制約条件を設定するステップと、先読みステップ毎のビークル１０の位置と目的位置Ｐとの偏差が小さくなるほど評価が高くなる評価関数と、制約条件とに基づき最適化計算を行い、ビークル１０の移動経路Ｒを算出するステップと、をコンピュータに実行させる。本プログラムによると、障害物Ｏを回避可能な経路を適切に設定できる。

以上、本発明の実施形態を説明したが、この実施形態の内容により実施形態が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。さらに、前述した実施形態の要旨を逸脱しない範囲で構成要素の種々の省略、置換又は変更を行うことができる。

１０ ビークル
３０ 制御装置（経路生成装置）
５０ 目的位置情報取得部
５２ 自己位置情報取得部
５４ 障害物情報取得部
５６ 計算実行部
５８ 駆動制御部
Ｏ 障害物
Ｐ 目的位置

Claims (13)

1. ビークルの位置の情報を取得するステップと、
   前記ビークルの目的位置の情報を取得するステップと、
   障害物の位置の情報を取得するステップと、
   前記ビークルの位置、前記ビークルの大きさ、及び前記障害物の位置に基づき、先読みステップ毎の前記ビークルが前記障害物に干渉しない旨の制約条件を設定するステップと、
   前記先読みステップ毎の前記ビークルの位置と前記目的位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づき、前記ビークルが前記制約条件を満たしつつ前記目的位置に最短距離で近づくような最適化計算を行い、前記ビークルの移動経路を算出するステップと、
   を含み、
   前記制約条件を設定するステップでは、前記障害物の位置を、前記先読みステップ毎の前記ビークルの座標系における位置に変換して、先読みステップ毎の前記ビークルが前記障害物に干渉しない旨の制約条件を設定する、
   ビークルの経路生成方法。
2. 前記制約条件を設定するステップにおいては、前記ビークルの大きさに基づき、前記先読みステップでの前記ビークルの位置を含む領域であるビークル領域を設定し、前記障害物の位置が、前記先読みステップ毎の前記ビークル領域の範囲外となることを、前記制約条件として設定する、請求項１に記載のビークルの経路生成方法。
3. 前記制約条件を設定するステップにおいては、前記障害物の移動量にも基づき、前記ビークル領域を設定する、請求項２に記載のビークルの経路生成方法。
4. 前記制約条件を設定するステップにおいては、前記障害物の大きさにも基づき、前記ビークル領域を設定する、請求項２又は請求項３に記載のビークルの経路生成方法。
5. 前記制約条件を設定するステップにおいては、前記ビークルの駆動条件が所定範囲となることも、前記制約条件として設定する、請求項１から請求項４のいずれか１項に記載のビークルの経路生成方法。
6. 前記制約条件を設定するステップにおいては、前記先読みステップでの前記ビークルの位置が所定範囲となることも、前記制約条件として設定する、請求項１から請求項５のいずれか１項に記載のビークルの経路生成方法。
7. 前記ビークルの移動経路に基づき、前記ビークルの駆動条件を算出するステップを更に含む、請求項１から請求項６のいずれか１項に記載のビークルの経路生成方法。
8. 前記ビークルの駆動条件を、前記ビークルの速度及び操舵角とする、請求項７に記載のビークルの経路生成方法。
9. 前記ビークルの駆動条件を、前記ビークルの速度及び旋回曲率とする、請求項７に記載のビークルの経路生成方法。
10. 設定された前記ビークルの駆動条件に基づき前記ビークルを制御するステップを含む、請求項７から請求項９のいずれか１項に記載のビークルの経路生成方法。
11. ビークルの位置の情報を取得する自己位置情報取得部と、
    前記ビークルの目的位置の情報を取得する目的位置情報取得部と、
    障害物の位置の情報を取得する障害物情報取得部と、
    計算実行部と、
    を含み、
    前記計算実行部は、
    前記ビークルの位置、前記ビークルの大きさ、及び前記障害物の位置に基づき、先読みステップ毎の前記ビークルが前記障害物に干渉しない旨の制約条件を設定し、
    前記先読みステップ毎の前記ビークルの位置と前記目的位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づき、前記ビークルが前記制約条件を満たしつつ前記目的位置に最短距離で近づくような最適化計算を行い、前記ビークルの移動経路を算出し、
    前記制約条件を設定する際には、前記障害物の位置を、前記先読みステップ毎の前記ビークルの座標系における位置に変換して、先読みステップ毎の前記ビークルが前記障害物に干渉しない旨の制約条件を設定する、
    ビークルの経路生成装置。
12. 請求項１１に記載のビークルの経路生成装置を含む、ビークル。
13. ビークルの位置の情報を取得するステップと、
    前記ビークルの目的位置の情報を取得するステップと、
    障害物の位置の情報を取得するステップと、
    前記ビークルの位置、前記ビークルの大きさ、及び前記障害物の位置に基づき、先読みステップ毎の前記ビークルが前記障害物に干渉しない旨の制約条件を設定するステップと、
    前記先読みステップ毎の前記ビークルの位置と前記目的位置との偏差が小さくなるほど評価が高くなる評価関数と、前記制約条件とに基づき、前記ビークルが前記制約条件を満たしつつ前記目的位置に最短距離で近づくような最適化計算を行い、前記ビークルの移動経路を算出するステップと、
    を、コンピュータに実行させ、
    前記制約条件を設定するステップでは、前記障害物の位置を、前記先読みステップ毎の前記ビークルの座標系における位置に変換して、先読みステップ毎の前記ビークルが前記障害物に干渉しない旨の制約条件を設定する、
    プログラム。

Images