7626649 システム、方法および作業車両

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2025-01-27

(45)【発行日】2025-02-04

(54)【発明の名称】システム、方法および作業車両

(51)【国際特許分類】

   G05D 1/43 20240101AFI20250128BHJP

   E02F 9/20 20060101ALI20250128BHJP

【ＦＩ】

G05D1/43

E02F9/20 N

【請求項の数】 9

(21)【出願番号】P 2021061381

(22)【出願日】2021-03-31

(65)【公開番号】P2022157259

(43)【公開日】2022-10-14

【審査請求日】2024-02-13

(73)【特許権者】

【識別番号】000001236

【氏名又は名称】株式会社小松製作所

(74)【代理人】

【識別番号】110001634

【氏名又は名称】弁理士法人志賀国際特許事務所

(72)【発明者】

【氏名】岩永 圭弘

(72)【発明者】

【氏名】野寄 敬博

【審査官】杉山 悟史

(56)【参考文献】

【文献】特開２０２１－０３３６８５（ＪＰ，Ａ）

【文献】中国特許出願公開第１１１７９６５８８（ＣＮ，Ａ）

【文献】特表２０２０－５３４６２１（ＪＰ，Ａ）

【文献】特開２０１０－０７３０８０（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／００ ～ １／８７

Ｅ０２Ｆ ９／２０

(57)【特許請求の範囲】

【請求項1】

車両が自動走行するための軌道を探索するシステムであって、
プロセッサを備え、
前記プロセッサは、
前記車両の周囲を表すマップデータに含まれる障害物に前記車両が接触しないことを含む第１制約条件に基づいて、前記車両の初期位置から目標位置へ移動するための経路である前記車両の位置及び姿勢を要素とする系列を探索する経路探索部と、
前記経路の各要素に係る位置を含み前記車両の幅方向に広がる領域からの逸脱距離に応じてペナルティ値が増加し、前記領域内に前記車両が位置するときのペナルティ値が一定となる第２制約条件を生成する制約条件生成部と、
前記第２制約条件に基づいて、前記車両の初期位置から目標位置へ移動するための軌道である前記車両の位置、姿勢、速度及び舵角を要素とする系列を探索する軌道探索部と、
を備えるシステム。

【請求項2】

前記経路探索部は、完全性を有する第１探索アルゴリズムを用いて前記経路を探索し、
前記軌道探索部は、前記第１探索アルゴリズムより要素数に対する計算量の増加が緩やかな第２探索アルゴリズムを用いて前記軌道を探索する
請求項１に記載のシステム。

【請求項3】

前記制約条件生成部は、前記障害物の位置と前記経路の各要素に係る前記位置及び前記姿勢とに基づいて、前記第２制約条件を生成する
請求項１に記載のシステム。

【請求項4】

前記経路探索部は前記車両を表すモデルに基づいて前記経路を探索し、
前記モデルは、前記車両の単位時間後の位置を、前記車両の現在の位置と、前記車両の舵角から求められる旋回半径と、前記車両の現在の姿勢角と、前記車両の単位時間後の姿勢角とに基づいて表す
請求項１から請求項３の何れか１項に記載のシステム。

【請求項5】

車両が自動走行するための軌道を探索する方法であって、
前記車両の周囲を表すマップデータに含まれる障害物に前記車両が接触しないことを含む第１制約条件に基づいて、前記車両の初期位置から目標位置へ移動するための経路である前記車両の位置及び姿勢を要素とする系列を探索するステップと、
前記経路の各要素に係る位置を含み前記車両の幅方向に広がる領域からの逸脱距離に応じてペナルティ値が増加し、前記領域内に前記車両が位置するときのペナルティ値が一定となる第２制約条件を生成するステップと、
前記第２制約条件に基づいて、前記車両の初期位置から目標位置へ移動するための軌道である前記車両の位置、姿勢、速度及び舵角を要素とする系列を探索するステップと、
を備える方法。

【請求項6】

前記経路を探索するステップは、完全性を有する第１探索アルゴリズムを用いて前記経路を探索し、
前記軌道を探索するステップは、前記第１探索アルゴリズムより要素数に対する計算量の増加が緩やかな第２探索アルゴリズムを用いて前記軌道を探索する
請求項５に記載の方法。

【請求項7】

前記第２制約条件を生成するステップは、前記障害物の位置と前記経路の各要素に係る前記位置及び前記姿勢とに基づいて、前記第２制約条件を生成する
請求項５に記載の方法。

【請求項8】

前記経路を探索するステップは、前記車両を表すモデルに基づいて前記経路を探索し、
前記モデルは、前記車両の単位時間後の位置を、前記車両の現在の位置と、前記車両の舵角から求められる旋回半径と、前記車両の現在の姿勢角と、前記車両の単位時間後の姿勢角とに基づいて表す
請求項５から請求項７の何れか１項に記載の方法。

【請求項9】

車体と、
前記車体を走行させるための走行装置と、
プロセッサとを備える作業車両であって、
前記プロセッサは、
前記作業車両の周囲を表すマップデータに含まれる障害物に前記作業車両が接触しないことを含む第１制約条件に基づいて、前記作業車両の初期位置から目標位置へ移動するための経路である前記作業車両の位置及び姿勢を要素とする系列を探索する経路探索部と、
前記経路の各要素に係る位置を含み前記作業車両の幅方向に広がる領域からの逸脱距離に応じてペナルティ値が増加し、前記領域内に前記作業車両が位置するときのペナルティ値が一定となる第２制約条件を生成する制約条件生成部と、
前記第２制約条件に基づいて、前記作業車両の初期位置から目標位置へ移動するための軌道である前記作業車両の位置、姿勢、速度及び舵角を要素とする系列を探索する軌道探索部と、
前記軌道に従って前記走行装置を制御する走行制御部と、
を備える作業車両。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、システム、方法および作業車両に関する。

【背景技術】

【0002】

砕石現場や鉱山などの作業現場では、作業車両の自動運転システムが導入されることがある。自動運転システムは、作業車両を目的位置へ到達させるための軌道を計算し、当該軌道に基づいて作業車両を走行させる。特許文献１には、無人車両の走行経路生成方法に係る技術が開示されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１０－０７３０８０号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

車両が自動走行するための軌道計算は、最適化問題である。また軌道の計算において、作業現場に存在する障害物は、制約条件として与えられる。しかしながら、軌道の探索に用いる数理モデルは、車両の位置、姿勢、舵角、速度、加速度などの多次元のベクトルで表され、また障害物の位置及び形状は必ずしも単純ではなく制約条件の非線形性が高くなる可能性がある。そのため、軌道の計算は計算量が多くなり、又は局所解への収束が生じるため、オンラインで車両の走行軌道を計算することは困難である。一方で、計算量を削減するためにモデルの次元数を低減すると、得られる軌道において、削減した次元に係る動作が不連続となる可能性がある。
本開示の目的は、少ない計算量で連続的な軌道を得ることができる軌道探索装置および軌道探索方法を提供することにある。

【課題を解決するための手段】

【0005】

本発明の第１の態様は、車両が自動走行するための軌道を探索するシステムであって、プロセッサを備える。プロセッサは、障害物の位置を表す第１制約条件に基づいて、車両の初期位置から目標位置へ移動するための経路である車両の位置及び姿勢を要素とする系列を探索する経路探索部と、経路からの逸脱距離に応じてペナルティ値が増加する第２制約条件を生成する制約条件生成部と、第２制約条件に基づいて、車両の初期位置から目標位置へ移動するための軌道である車両の位置、姿勢、速度及び舵角を要素とする系列を探索する軌道探索部と、を備える。

【0006】

本発明の第２の態様は、車両が自動走行するための軌道を探索する方法である。当該方法は、以下の処理を備える。第１の処理は、障害物の位置を表す第１制約条件に基づいて、車両の初期位置から目標位置へ移動するための経路である前記車両の位置及び姿勢を要素とする系列を探索する。第２の処理は、経路からの逸脱距離に応じてペナルティ値が増加する第２制約条件を生成する。第３の処理は、第２制約条件に基づいて、車両の初期位置から目標位置へ移動するための軌道である車両の位置、姿勢、速度及び舵角を要素とする系列を探索する。

【0007】

本発明の第３の様態は、作業車両であって、車体と、車体を走行させるための走行装置と、プロセッサとを備える。プロセッサは、障害物の位置を表す第１制約条件に基づいて、作業車両の初期位置から目標位置へ移動するための経路である車両の位置及び姿勢を要素とする系列を探索する経路探索部と、経路からの逸脱距離に応じてペナルティ値が増加する第２制約条件を生成する制約条件生成部と、第２制約条件に基づいて、作業車両の初期位置から目標位置へ移動するための軌道である作業車両の位置、姿勢、速度及び舵角を要素とする系列を探索する軌道探索部と、軌道に従って前記走行装置を制御する走行制御部と、を備える。

【発明の効果】

【0008】

上記態様の少なくとも１つによれば、軌道探索装置は、少ない計算量で連続的な軌道を得ることができる。

【図面の簡単な説明】

【0009】

【図1】第１の実施形態に係る作業車両の側面図である。

【図2】第１の実施形態に係る制御装置の構成を示す概略ブロック図である。

【図3】第１の実施形態に係るマップデータの一例を示す図である。

【図4】第１の実施形態に係る許容範囲系列の例を示す図である。

【図5】第１の実施形態に係る許容範囲系列の生成方法を示すフローチャートである。

【図6】第１の実施形態に係る第１矩形及び第２矩形の関係を示す図である。

【図7】第１の実施形態に係る制御装置の自動走行処理を示すフローチャートである。

【図8】第１の実施形態に係るモデルにおける時間刻み幅と算出結果の位置との関係を示す図である。

【図9】比較例に係るモデルにおける時間刻み幅と算出結果の位置との関係を示す図である。

【図10】他の実施形態に係る自動運転システムの構成を示す概略図である。

【発明を実施するための形態】

【0010】

〈第１の実施形態〉
《作業車両の構成》
以下、図面を参照しながら実施形態について詳しく説明する。
図１は、第１の実施形態に係る作業車両の側面図である。
第１の実施形態に係る作業車両１００は、ホイールローダである。作業車両１００は、自動運転により走行する。すなわち作業車両１００は無人走行車である。作業車両１００は、車体１１０、作業機１２０、制御装置１４０を備える。

【0011】

車体１１０は、前車体１１１、後車体１１２、およびステアリングアクチュエータ１１３を備える。前車体１１１と後車体１１２とは車体１１０の上下方向に伸びるステアリング軸回りに回動可能に取り付けられている。前車体１１１の下部には一対の前輪ＷＦが設けられる。後車体１１２の下部には一対の後輪ＷＲが設けられる。

【0012】

後車体１１２は、動力源１１２１及び動力伝達装置１１２２を備える。動力源１１２１は、例えばディーゼルエンジンである。動力伝達装置１１２２は、入力軸に入力される駆動力を変速して出力軸から出力する。動力伝達装置１１２２の出力軸は、図示しないフロントアクスルを介して前輪ＷＦに接続され、図示しないリアアクスルを介して後輪ＷＲに接続される。フロントアクスルは前車体１１１を走行可能に支持する。リアアクスルは後車体１１２を走行可能に支持する。フロントアクスルおよびリアアクスルには、図示しない制動装置を備える。動力伝達装置１１２２には、回転センサ１１２３が設けられる。回転センサ１１２３は、動力伝達装置１１２２の出力軸の回転数を計測する。制御装置１４０は、回転センサ１１２３の計測データに基づいて作業車両１００の速度及び加速度を特定することができる。また、後車体１１２の上部には、検出装置１１２４及び測位装置１１２５が設けられる。検出装置１１２４は、作業車両１００の周囲に存在する物体の三次元位置を検出する。検出装置１１２４の例としては、ＬｉＤａｒ（Light Detection and Ranging）、ステレオカメラ、ＵＷＢ（Ultra Wide Band）測距装置などが挙げられる。検出装置１１２４は、例えば検出方向が前方を向くように設けられる。測位装置１１２５は、ＧＮＳＳ（Global Navigation Satellite System）等によって作業車両１００のグローバル座標における位置及び姿勢（姿勢角）を計測する。位置は、例えば緯度及び経度によって表される。姿勢は、例えば北を基準方位とした後車体１１２の方位角によって表される。以下、作業車両１００の位置及び姿勢を、作業車両１００のポーズともいう。

【0013】

ステアリングアクチュエータ１１３は、例えば油圧シリンダである。ステアリングアクチュエータ１１３の基端部は後車体１１２に取り付けられ、先端部は前車体１１１に取り付けられる。ステアリングアクチュエータ１１３は、作動油によって伸縮することで、前車体１１１と後車体１１２とのなす角度を規定する。つまり、ステアリングアクチュエータ１１３の伸縮により、前輪ＷＦの舵角が規定される。ステアリングアクチュエータ１１３には、ステアリングアクチュエータ１１３のストローク長を計測するストロークセンサ１１３１が設けられる。制御装置１４０は、ストロークセンサ１１３１の計測データに基づいて作業車両１００の舵角を特定することができる。

【0014】

動力伝達装置１１２２、制動装置及びステアリングアクチュエータ１１３は、車体１１０を走行させるための走行装置である。

【0015】

作業機１２０は、土砂等の作業対象物の掘削および運搬に用いられる。作業機１２０は、車体１１０の前部に設けられる。
制御装置１４０は、検出装置１１２４の検出データに基づいて、作業車両１００を走行させる。

【0016】

《制御装置１４０の構成》
図２は、第１の実施形態に係る制御装置１４０の構成を示す概略ブロック図である。
制御装置１４０は、プロセッサ２１０、メインメモリ２３０、ストレージ２５０、インタフェース２７０を備えるコンピュータである。

【0017】

ストレージ２５０は、一時的でない有形の記憶媒体である。ストレージ２５０の例としては、磁気ディスク、光ディスク、光磁気ディスク、半導体メモリ等が挙げられる。ストレージ２５０は、制御装置１４０のバスに直接接続された内部メディアであってもよいし、インタフェース２７０または通信回線を介して制御装置１４０に接続される外部メディアであってもよい。ストレージ２５０は、作業車両１００を制御するためのプログラムを記憶する。

【0018】

プログラムは、制御装置１４０に発揮させる機能の一部を実現するためのものであってもよい。例えば、プログラムは、ストレージ２５０に既に記憶されている他のプログラムとの組み合わせ、または他の装置に実装された他のプログラムとの組み合わせによって機能を発揮させるものであってもよい。なお、他の実施形態においては、制御装置１４０は、上記構成に加えて、または上記構成に代えてＰＬＤ（Programmable Logic Device）などのカスタムＬＳＩ（Large Scale Integrated Circuit）を備えてもよい。ＰＬＤの例としては、ＰＡＬ(Programmable Array Logic)、ＧＡＬ(Generic Array Logic)、ＣＰＬＤ(Complex Programmable Logic Device)、ＦＰＧＡ（Field Programmable Gate Array）が挙げられる。この場合、プロセッサによって実現される機能の一部または全部が当該集積回路によって実現されてよい。

【0019】

《ソフトウェア構成》
プロセッサ２１０は、プログラムを実行することで、取得部２１１、マップ生成部２１３、目標決定部２１２、経路探索部２１４、制約条件生成部２１５、軌道探索部２１６、走行制御部２１７を備える。制御装置１４０は、軌道探索装置の一例である。

【0020】

取得部２１１は、ストロークセンサ１１３１、回転センサ１１２３、検出装置１１２４及び測位装置１１２５の計測データを取得する。これにより、取得部２１１は、作業車両１００の位置、姿勢角、舵角、速度及び加速度、並びに作業車両１００の周囲に存在する障害物の位置を特定する。

【0021】

マップ生成部２１３は、取得部２１１が取得した検出装置１１２４の計測データと測位装置１１２５の計測データとを用いて作業車両１００の周囲を表すマップデータを生成する。マップ生成部２１３は、例えばＳＬＡＭ（Simultaneous Localization and Mapping）技術によってマップデータを生成する。マップデータは、例えばグローバル座標系に係る占有格子地図によって表される。図３は、第１の実施形態に係るマップデータの一例を示す図である。またマップデータの各格子には、障害物からの距離を表すスコアが付される。障害物が存在する位置を示す格子のスコアはゼロであり、障害物からの距離が遠いほどスコアは高くなる。スコアは、例えば最も近傍の障害物からのマンハッタン距離によって表される。

【0022】

検出装置１１２４は、後車体１１２に固定されているため、マップ生成部２１３は、検出装置１１２４の計測データが示す障害物の位置を、測位装置１１２５の計測データが示す位置に基づいて平行移動させ、姿勢角に基づいて回転させることで、グローバル座標系における障害物の位置を特定することができる。マップ生成部２１３が生成したマップデータは、メインメモリ２３０に記録される。

【0023】

目標決定部２１２は、作業車両１００の目標ポーズ（目標位置）を決定する。例えば目標決定部２１２は、作業機１２０に土砂が保持されていない場合に、予め指定された積込場の位置及び積込場に相対する姿勢角を目標ポーズに決定する。また目標決定部２１２は、作業機１２０に土砂が保持されている場合に、予め指定された放土場の位置及び放土場に相対する姿勢角を目標ポーズに決定する。このとき例えば目標決定部２１２は、マップデータに基づいて、積込場及び放土場の位置を修正してもよい。

【0024】

経路探索部２１４は、取得部２１１が取得した作業車両１００の現在のポーズ（初期位置）から、目標決定部２１２が決定した目標ポーズまでを結ぶ経路を探索する。「経路」とは、ポーズの順序集合によって表される。つまり、経路探索部２１４が求める経路は、作業車両１００の位置と姿勢角の系列である。経路探索部２１４は、動的計画法などの完全性を有する探索アルゴリズム（第１探索アルゴリズム）により経路を探索する。動的計画法に係るアルゴリズムの例としては、ダイクストラ法やHybrid A*などが挙げられる。Hybrid A*を用いる場合、ヒューリスティックコストは、Reeds Shepp法やLaplace Potential法によって求めることができる。Reeds Shepp法は、障害物や曲率の連続性を無視したときの非ホロノミック系の最短経路を求める手法である。Laplace Potential法は、非ホロノミック制約を無視したときの最短経路を求める手法である。
経路探索部２１４による探索の制約条件（第１制約条件）は、マップ生成部２１３が生成したマップデータが表す障害物に接触しないこと、終点におけるポーズが目標ポーズと一致すること、及び舵角が所定の可動範囲内となることである。経路探索部２１４による探索の評価関数は、走行距離が短いほど値が小さくなる関数である。経路探索部２１４による探索に用いるモデルは、式（１）で表される。

【0025】

【数1】

【0026】

式（１）において、ｘ［ｋ］は、時刻ｋにおけるｘ座標（例えば緯度）を示す。ｙ［ｋ］は、時刻ｋにおけるｙ座標（例えば経度）を示す。φ［ｋ］は、時刻ｋにおける姿勢角を示す。δ［ｋ］は、時刻ｋにおける舵角を示す。ｖ［ｋ］は、時刻ｋにおける速度を示す。ｋ＋１は、時刻ｋの単位時間後の時刻を示す。Δｔは、時間刻み幅を示す。Ｌは、アーティキュレート中心から前輪ＷＦまでの長さを示す。つまり、経路探索部２１４が用いる数理モデルは、作業車両１００のｘ座標、ｙ座標及び姿勢角の３要素のモデルであって、舵角及び速度を要素に含まない。
動的計画法に係る探索アルゴリズムは、完全性を有する一方で、モデルや入力の次元数に対して計算量が指数関数的に増大する。そのため、第１の実施形態に係る経路探索部２１４は、モデルの要素数を小さくすることで計算量を抑えることができる。

【0027】

なお、式（１）においてＬ／ｔａｎ（δ［ｋ］）は、作業車両１００の旋回半径を表す。つまり、式（１）において、ｘ座標の増分は、姿勢角φ［ｋ］の正弦と姿勢角φ［ｋ＋１］の正弦との差に、時刻ｋにおける作業車両１００の旋回半径を乗算した値として表される。同様に、式（１）において、ｙ座標の増分は、姿勢角φ［ｋ］の余弦と姿勢角φ［ｋ＋１］の余弦との差に、時刻ｋにおける作業車両１００の旋回半径を乗算した値として表される。

【0028】

経路探索部２１４は、式（１）に示すモデルに基づく探索により、作業車両１００のポーズの系列を経路として取得する。なお、式（１）に示すモデルは時刻をインデックスとするが、制約条件生成部２１５において経路のインデックスは時刻を表すものとして扱われない。例えば、経路におけるポーズｘ［ｉ］は、時刻ｉにおけるポーズではなく、単にｉ番目のポーズを示す。一方で、経路探索部２１４によって得られた舵角及び速度の時系列は、後述する軌道探索部２１６の初期値として用いられる。なお、経路探索部２１４が用いるモデルは、ポーズのモデルであって、舵角及び速度を要素に含まないため、探索された舵角及び加速度の系列は連続性に欠けたものとなる。つまり、経路探索部２１４が探索した経路は、曲率が不連続であったり、不要なスイッチバックを含んだりすることがある。

【0029】

制約条件生成部２１５は、経路探索部２１４が得た経路に基づいて、軌道探索部２１６による探索に用いる制約条件（第２制約条件）を生成する。軌道探索部２１６による探索に用いる制約条件は、作業車両１００の位置が経路探索部２１４で得た経路を含む許容範囲系列内に位置することである。制約条件は、許容範囲系列からの逸脱距離に応じてペナルティ値が増加し、許容範囲系列内に作業車両１００が位置するときのペナルティ値がゼロとなるペナルティ関数によって表される。図４は、第１の実施形態に係る許容範囲系列の例を示す図である。許容範囲系列は、矩形で表される許容範囲の系列である。許容範囲は、矩形の４つの頂点の座標によって表される。制約条件生成部２１５は、マップ生成部２１３が生成したマップデータ及び経路探索部２１４が生成した経路に基づいて許容範囲系列を決定する。制約条件生成部２１５によって経路が許容範囲系列に変換されることで、経路の曲率の不連続や不要なスイッチバックを無視することができる。

【0030】

制約条件生成部２１５は、以下の手順で許容範囲系列を生成する。図５は、第１の実施形態に係る許容範囲系列の生成方法を示すフローチャートである。
制約条件生成部２１５は、経路に係るポーズの系列からポーズを１つずつ選択し（ステップＳ５１）、選択されたポーズについて以下のステップＳ５２～Ｓ５６の処理を実行する。制約条件生成部２１５は、ステップＳ５１で選択したポーズに基づいて、メインメモリ２３０に記録されたマップデータ上に、作業車両１００の外形を表す第１矩形Ｒ１を配置する（ステップＳ５２）。制約条件生成部２１５は、マップデータのうち第１矩形Ｒ１の４つの辺上に設定された複数の点と重複する複数の格子のスコアに基づいて、各点から最も近い障害物までの距離（ユークリッド距離）を算出する（ステップＳ５３）。図６は、第１の実施形態に係る第１矩形Ｒ１及び第２矩形Ｒ２の関係を示す図である。第１矩形Ｒ１が２つの長辺Ｅ１及びＥ２並びに２つの短辺Ｅ３及びＥ４から構成される場合に、例えば第１矩形の４つの頂点ｐ１、ｐ３、ｐ６、ｐ８と、長辺Ｅ１を３分する２つの点ｐ４、ｐ５、長辺Ｅ２を３分する２つの点ｐ９、ｐ１０、短辺Ｅ３を２分する点ｐ２、及び短辺Ｅ４を２分する点ｐ７が設定される。第１の実施形態に係るスコアは障害物からのマンハッタン距離を表すため、障害物までの距離ｄは、スコアｎを用いて、以下の式（２）によって求めることができる。

【0031】

【数2】

【0032】

制約条件生成部２１５は、マップデータに基づいて、第１矩形の４つの辺それぞれについて、ステップＳ５３で求めた距離ｄのうち最もが短いものを特定する（ステップＳ５４）。制約条件生成部２１５は、ステップＳ５４で求めた距離ｄを用いて、以下の式（３）によって許容範囲を表す第２矩形Ｒ２の４つの頂点Ｐ１，Ｐ２，Ｐ３，Ｐ４の位置を求める（ステップＳ５５）。

【0033】

【数3】

【0034】

つまり、許容範囲は、作業車両１００を表す第１矩形Ｒ１の中心点ｐ０を内包する第２矩形Ｒ２で表される。

【0035】

制約条件生成部２１５は、ステップＳ５１で選択したポーズに基づいて算出した許容範囲の長方形を配置することで、グローバル座標系における許容範囲の位置及び方位角を決定する（ステップＳ５６）。
制約条件生成部２１５は、上記のステップＳ５２からステップＳ５６までの処理を経路のすべてのポーズについて実行することで、許容範囲系列を得ることができる。

【0036】

図２に示す軌道探索部２１６は、取得部２１１が取得した作業車両１００の現在のポーズから、目標決定部２１２が決定した目標ポーズまでを結ぶ軌道を探索する。「軌道」とは、時間をパラメータとしたポーズの系列によって表される。つまり、軌道探索部２１６は、作業車両１００の時刻毎のポーズ、並びに当該時刻毎のポーズを実現するための舵角及び速度の系列を、軌道として求める。軌道探索部２１６は、評価関数の二次近似を用いる探索アルゴリズム（第２探索アルゴリズム）により軌道を探索する。評価関数の二次近似を用いる探索アルゴリズムの例としては、微分動的計画法、ｉＬＱＲ（iterative Linear Quadratic Regulator）、ＳＱＰ（Sequential Quadratic Programming）などが挙げられる。
軌道探索部２１６による探索は、走行時間が短いほど値が小さくなる評価関数と、制約条件生成部２１５が生成したペナルティ関数の和によって得られる評価値が最小となるように実行される。軌道探索部２１６による探索においては、式（１）に示す時間刻み幅Δｔも最適化対象となる。

【0037】

評価関数の二次近似を用いる探索アルゴリズムは、動的計画法に係る探索アルゴリズムと比較して次元数に対する計算量の増加が緩やかである。そのため、舵角、速度及び時間刻み幅を最適化の対象に加えても計算量の爆発は生じない。一方で、評価関数の二次近似を用いる探索アルゴリズムは、制約条件の非線形性が強いと局所解への収束が生じることがあるなど完全性を有しない。そのため、制約条件生成部２１５が生成した線形関数で表すことができる制約条件を用いることで、問題を単純化し、局所解への収束を防ぐことができる。

【0038】

走行制御部２１７は、軌道探索部２１６が求めた軌道に従って、ステアリングアクチュエータ１１３、動力源１１２１、動力伝達装置１１２２及び制動装置を制御する。すなわち、走行制御部２１７は、軌道に係る系列から現在時刻に係る舵角及び速度を読み出し、当該舵角を実現するようにステアリングアクチュエータ１１３を制御し、また当該速度を実現するように動力源１１２１、動力伝達装置１１２２及び制動装置を制御する。

【0039】

《制御装置１４０の動作》
図７は、第１の実施形態に係る制御装置１４０の自動走行処理を示すフローチャートである。
制御装置１４０の目標決定部２１２は、作業車両１００の状態に基づいて目標ポーズを決定する（ステップＳ１）。取得部２１１は、ストロークセンサ１１３１、回転センサ１１２３、検出装置１１２４及び測位装置１１２５の計測データを取得する（ステップＳ２）。走行制御部２１７は、ステップＳ２で取得した測位装置１１２５の計測データに基づいて、ステップＳ１で決定した目標ポーズと現在のポーズの誤差が所定の閾値以下であるか否かを判定する（ステップＳ３）。

【0040】

目標ポーズとの誤差が閾値より大きい場合（ステップＳ３：ＮＯ）、ステップＳ２で取得した検出装置１１２４の計測データに基づいてメインメモリ２３０に記録されたマップデータを更新する（ステップＳ４）。経路探索部２１４は、作業車両１００の現在のポーズから、ステップＳ１で決定した目標ポーズまでを結ぶ経路を探索する（ステップＳ５）。

【0041】

次に、制約条件生成部２１５は、ステップＳ５で探索された経路に基づいて、図５に示す許容範囲系列生成処理を実行する（ステップＳ６）。制約条件生成部２１５は、生成された許容範囲系列に基づいてペナルティ関数を生成する（ステップＳ７）。軌道探索部２１６は、ステップＳ７で生成されたペナルティ関数を用いて作業車両１００の現在のポーズから目標ポーズまでを結ぶ軌道を探索する（ステップＳ８）。このとき軌道探索部２１６は、ステップＳ５の経路探索において得られたポーズ、舵角及び速度の系列を探索の初期値に用いる。

【0042】

走行制御部２１７は、ステップＳ８で求めた軌道に従って、ステアリングアクチュエータ１１３、動力源１１２１、動力伝達装置１１２２及び制動装置を制御する（ステップＳ９）。そして制御装置１４０は処理をステップＳ２に戻す。

【0043】

ステップＳ３において目標ポーズとの誤差が閾値より大きい場合（ステップＳ３：ＮＯ）、制御装置１４０は自動走行処理を終了する。制御装置１４０は、自動掘削処理や自動放土処理を実行した後、再度自動走行処理を実行する。

【0044】

《作用・効果》
第１の実施形態に係る制御装置１４０は、障害物の位置を表す第１制約条件に基づいて、作業車両１００の初期位置から目標位置へ移動するための経路を探索し、経路からの逸脱距離に応じてペナルティ値が増加する第２制約条件（ペナルティ関数）に基づいて軌道を探索する。このように、制御装置１４０は、制約条件を変えて二段階で軌道を探索する。軌道の探索に用いる制約条件を、経路からの逸脱距離に応じた制約条件とすることで、軌道の探索に係る制約条件の非線形性を弱めて計算量の増加を抑えることができる。つまり、制御装置１４０は、少ない計算量で連続的な軌道を得ることができる。

【0045】

また、第１の実施形態に係る制御装置１４０は、完全性を有する探索アルゴリズムを用いて経路を探索し、要素数に対する計算量の増加が緩やかな第２探索アルゴリズムを用いて軌道を探索する。これにより、制御装置１４０は、計算量を抑え、かつ局所解へ収束せずに経路及び軌道を得ることができる。

【0046】

また第１の実施形態に係る軌道の探索に用いるペナルティ関数は、経路に係る各位置を含み作業車両１００の幅方向に広がる第２矩形からの逸脱距離に応じてペナルティ値が増加し、第２矩形内に作業車両１００が位置するときのペナルティ値が一定となる関数である。このように、制御装置１４０は線形性の高いペナルティ関数を得ることができる。

【0047】

また第１の実施形態では、式（１）に示すモデルを用いて経路及び軌道の探索を行う。式（１）に示すモデルは、作業車両１００の単位時間後の位置（ｘ［ｋ＋１］，ｙ［ｋ＋１］）を、作業車両１００の現在の位置（ｘ［ｋ］，ｙ［ｋ］）と、作業車両１００の舵角δ［ｋ］から求められる旋回半径と、作業車両１００の現在の姿勢角φ［ｋ］と、作業車両１００の単位時間後の姿勢角φ［ｋ＋１］とに基づいて表す。

【0048】

図８は、第１の実施形態に係るモデルにおける時間刻み幅と算出結果の位置との関係を示す図である。図９は、比較例に係るモデルにおける時間刻み幅と算出結果の位置との関係を示す図である。
車両のモデルとして、以下の式（４）に示すものが用いられることがある。

【0049】

【数4】

【0050】

しかしながら、式（４）に示すモデルは、ｘ座標の増分及びｙ座標の増分を直線移動に近似しているため、時間の刻み幅Δｔの値が変化すると、図９に示すように位置の計算結果に誤差が生じる。これに対し、式（１）に示すモデルは、旋回半径と姿勢角とによりｘ座標の増分及びｙ座標の増分を円弧に近似しているため、図８に示すように時間の刻み幅Δｔの値が変化しても、位置の計算結果に誤差が生じない。したがって、第１の実施形態に係る制御装置１４０は、式（１）に示すモデルを用いることで、時間刻みの影響を無視することができるため、経路探索部２１４が生成した経路を容易に軌道に変換することができる。これにより、制御装置１４０は、経路探索部２１４によって求められた障害物と接触せず、目標状態に到達する軌道を初期値として軌道探索を開始することができる。初期値として目標状態に到達する軌道を設定することで、経路探索部２１４が局所解へ収束することなく適切な軌道を計算することができる。

【0051】

〈他の実施形態〉
以上、図面を参照して一実施形態について詳しく説明してきたが、具体的な構成は上述のものに限られることはなく、様々な設計変更等をすることが可能である。すなわち、他の実施形態においては、上述の処理の順序が適宜変更されてもよい。また、一部の処理が並列に実行されてもよい。
上述した実施形態に係る制御装置１４０は、単独のコンピュータによって構成されるものであってもよいし、制御装置１４０の構成を複数のコンピュータに分けて配置し、複数のコンピュータが互いに協働することで制御装置１４０として機能するものであってもよい。このとき、制御装置１４０を構成する一部のコンピュータが作業車両１００の内部に搭載され、他のコンピュータが作業車両１００の外部に設けられてもよい。

【0052】

図１０は、他の実施形態に係る自動運転システム２の構成を示す概略図である。第１の実施形態に係る作業車両１００は、内部に備える制御装置１４０の制御によって自律走行する。これに対し、図９に示す自動運転システム２は、作業車両１００と遠隔に設けられる制御サーバ８００が作業車両１００を制御する。制御サーバ８００と制御装置１４０とは無線通信により接続される。

【0053】

この場合、図２に示す各処理部は制御サーバ８００に設けられる。取得部２１１は、無線通信により各計測装置の計測データを得る。また走行制御部２１７は、無線通信により制御装置１４０に制御指令を送信する。

【0054】

また、第１の実施形態に係る制御装置１４０は、経路の探索に動的計画法を用いるが、これに限られない。例えば、他の実施形態においては、直線、円弧、ベジェ曲線、クロソイド曲線、Ｂスプライン曲線などの曲線の組み合わせによって幾何学的に経路を生成してもよい。

【0055】

また、第１の実施形態に係る制御装置１４０は、軌道の探索に評価関数の二次近似を用いる探索アルゴリズムを用いるが、これに限られない。例えば、他の実施形態においては、制御装置１４０は、数理計画問題としての再定式化によって軌道を求めてもよい。

【0056】

また、第１の実施形態に係る制御装置１４０は、軌道の探索の制約条件として、第２矩形で表される可動範囲を設定するが、これに限られない。例えば、他の実施形態に係る制約条件は、経路に係る点を通り、作業車両１００の幅方向にのみ広がる線分によって表されてもよい。

【0057】

また、第１の実施形態に係る制御装置１４０は、式（３）に基づいて第２矩形Ｒ２を求めるが、これに限られない。例えば、他の実施形態に係る制御装置１４０は、以下に示す手順で第２矩形Ｒ２を求めてもよい。制御装置１４０は、作業車両１００に最も近い障害物が存在する方向の方位角α及び当該障害物までの距離ｄを用いて、以下の式（５）によって第２矩形Ｒ２の４つの頂点Ｒ１，Ｒ２，Ｒ３，Ｒ４の位置を求めてもよい。式（５）においてＬはアーティキュレート中心から前輪ＷＦまでの長さを示し、ｌは、トレッド幅を示す。

【0058】

【数5】

【0059】

また、第１の実施形態に係る作業車両１００は、アーティキュレート車両であるホイールローダであるが、これに限られない。例えば、他の実施形態に係る車両は、ダンプトラックなどの他の車両であってもよいし、アーティキュレート車両でなくてもよい。アーティキュレート車両でない場合、例えば、舵角とは操舵される車輪と車体とのなす角度である。また、他の実施形態に係る作業車両１００は、履帯式車両であってもよいし、スキッドステアローダなどの転向できない車輪を有する車両であってもよい。この場合、例えば、左右の履帯若しくは車輪の速度差から旋回半径を求めることができる。

【符号の説明】

【0060】

１００…作業車両 １１０…車体 １１１…前車体 １１２…後車体 １１２１…動力源 １１２２…動力伝達装置 １１２３…回転センサ １１２４…検出装置 １１２５…測位装置 １１３…ステアリングアクチュエータ １１３１…ストロークセンサ １２０…作業機 １４０…制御装置 ２１０…プロセッサ ２１１…取得部 ２１２…目標決定部 ２１３…マップ生成部 ２１４…経路探索部 ２１５…制約条件生成部 ２１６…軌道探索部 ２１７…走行制御部 ２３０…メインメモリ ２５０…ストレージ ２７０…インタフェース ＷＦ…前輪 ＷＲ…後輪 ８００…制御サーバ ２…自動運転システム

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】