JP5332034B2 - 無人車両の走行経路生成方法 - Google Patents

Description

本発明は、無人車両の走行経路生成方法に関し、特に、無人車両の走行コストを最小に抑えることができる走行経路を生成する方法に関するものである。

砕石現場、鉱山などの広域の作業現場では、土砂運搬作業を行なうに際して作業者の疲労による事故の回避、省人化、作業時間の延長による生産性の向上を図るべく、有人の車両、たとえば有人のオフロードダンプトラックの代わりに無人の車両を稼動させるための無人車両走行システムが導入されている。

以下、無人車両走行システムの概要について説明する。

作業現場には、積込場、排土場、給油所、駐機所といった各エリアがある。これら各エリアがホールロードと呼ばれる整備された搬送路やアクセスロードと呼ばれるホールロードから各エリアへの引込み線や交差点により接続されている。

積込場は、ダンプトラックへ土砂を積み込む作業を行う場所であり、ホイールローダ（フロントエンドローダ）、バックホー、ショベルといった掘削作業車両による掘削作業およびダンプトラックへの土砂の積み込み作業が行なわれる。

排土場は、ダンプトラックに積み込まれた土砂の排出（排土）を行う場所であり、固定位置に排土する場合と、エリア内に規則的に排土する場合とがある。固定位置に排土する場合とは、クラッシャと呼ばれる岩石の破砕機を備えたホッパと呼ばれる投入口に排土する場合である。また、排土場では、ドーザによって、排出された土砂の均し作業が行なわれることがある。この際に荒れた路面を補修するためにドーザによって排土位置が指示され、指示された排土位置へ排土が行われることがある。

給油所には、燃料ポンプが設置されている。給油所は、駐機場に併設されていることが多い。他のダンプトラックの運行の妨げにならないように給油車を用いて給油を行う場合であっても給油所を設ける場合もある。

駐機所は、有人作業の場合は、食事やオペレータ交代などのために車両を止める場所である。無人車両走行システムにおいては、定期点検時など、安全にダンプトラックに乗り込むためにダンプトラックを引き込んだり、発進操作が行われる場所である。

ここで、無人車両の走行経路の生成方法には、従来より以下の方法が試みられている。

（ティーチングによる経路生成方法）
すなわち、ホールロードやアクセスロード、交差点、クラッシャなどを含む走路が固定可能である場合には、作業開始前にダンプトラックを有人にて実際に走行し、そのときの走行軌跡のデータを教示、記憶する。

（図面に基づく経路生成方法）
鉱山などの現場の図面に基づいて予め計算しておくことにより、予め走行経路を決定する。

しかし、上述のティーチングによる経路生成方法では、作業前に事前に車両を走行させ様々なデータを人手を介して取得する必要があるため、走行経路作成に時間と労力を要する。このため全体の作業効率が損なわれてしまう。

そして、何よりも実際の積込場や排土場においては、作業の経過に伴い積込位置や排土位置が繁雑に変わり、作業の経過に伴い積込場の大きさや形状も変わるため、予め走行経路を決定しておくことはできない。すなわち、作業中にドーザなどの作業車両のオペレータが指示する排土位置や、作業の経過に伴い時々刻々と変化するエリア形状により計算される排土位置、積込位置へ到着できる走行経路を逐次作成できるようにすることが必要となる。上述の図面に基づく経路生成方法も同様の問題がある。

そこで、データシミュレーションにより経路を生成する手法が従来より試みられている。

経路を生成する技術としては、従来より下記のものが知られている。これら経路生成の研究は、ロボティクスの分野で従来から行われている。

ａ）円弧と直線の組み合わせによる経路生成
すなわち、メッシュ、ベクトルポテンシャルを用い、直線、円弧等を組み合わせて走行経路を生成する。またランダムサーチを用いた方法もある。

ｂ）曲率連続を保つクロソイド曲線による経路生成
ｃ）多項式曲線による経路生成
ティーチングに拠ることなく、無人車両が走行すべき走行経路生成範囲等のデータが与えられた場合に、そのデータに基づき無人車両の走行経路を自動的に生成するという発明は、本出願人などにより既に特許出願され、公知となっている。

特許文献１には、１台の無人車両が検出した障害物の情報を複数台の無人車両に共通の障害物の情報として共有して、その障害物を回避する走行経路を生成するという発明が記載されている。

特許文献２には、排土領域が与えられた場合に、排土領域内に複数の目標排土点を設定して、それら複数の目標排土点に向かう複数の走行経路を生成するという発明が記載されている。
特開平１１-２９６２２９号公報 特開２０００-１３７５２２号公報

上記ａ）、ｂ）、ｃ）に掲げた経路生成技術を無人車両の走行経路生成に適用すれば、ティーチングによる経路生成方法などと比較して経路生成時間の短縮等を図れる。しかし、無人車両の制御性、生産性を考慮した場合には様々な問題がある。

まず、従来技術を無人車両走行システムにそのまま適用しても、車両走行が可能となる最低限の制約条件を満たさないことがある。

たとえば、ａ）に掲げたメッシュ、ベクトルポテンシャルを用いた経路生成方法では、車両の旋回半径が考慮されていないため、生成された走行経路では無人車両が旋回できず、車両の走行が不可能になることがある。

さらには、他の従来技術であるランダムサーチによる手法では、アルゴリズムが乱数に頼ることもあり、その性質上、走行経路生成が不能になったときに原因を究明することが困難である。

また、従来技術によって生成された走行経路は、必ずしもコスト面で最適な経路とはいえない。

すなわち、実際の作業現場では、走行に伴うコストを削減するために、消耗部品の交換頻度や故障を極力少なくし無人車両が長時間信頼性高く稼動できるようにすることが望ましい。とりわけダンプトラックのタイヤは高価でありタイヤへの負荷を減らすことが重要となる。また、コストを削減するために、走行に要する所要時間を短くして作業時間、車両の稼動時間を短くすることが望ましい。また、コストを削減するために、低燃費で走行できるようにすることが望ましい。

そこで、走行する無人車両のタイヤや車体への負荷を小さくすることができたり、走行に要する所要時間を短くしたり、低燃費で走行できたりするなど、無人車両の走行に伴うコストが最小となる走行経路を作成することが重要な課題となる。すなわち、滑らかなステアリング操作が可能で、車体の加減速が小さく、それによってタイヤや車体への負荷を小さく抑えられることができれば、走行コストを削減できる。また、走行に要する所要時間を短く、それによって作業時間、車両稼動時間を短くできれば、走行コストを削減できる。また、走行経路を低燃費で走行できれば、走行コストを削減できる。

従来技術にあっては、上述した走行に伴うコストを考慮して最適な走行経路を作成するものではない。たとえば、上記ａ）円弧と直線の組み合わせによる経路生成方法では、円弧と直線の接続点で曲率が不連続となり、車速を落とさざるを得なかったり、無理なステアリング操作を強いられたりする。このため作業効率が悪く、生産性が低下する。

同様に、ｂ）の曲率連続を保つクロソイド曲線による経路生成方法や、ｃ）の多項式曲線による経路生成方法の場合、単一の曲線では曲率の連続性はある程度補償されているものの、ある曲線と別の曲線を接続しようとすると、そのつなぎ目で曲率の不連続や曲率の変化率の不連続が生じる。このため、曲線同士のつなぎ目で車速が低下したり無理なステアリング操作を強いられたりし、同様に生産性が低下するという問題がある。

また、無人車両を狭い作業現場で進行方向を変更等する場合には、走行経路の途中にスイッチバック点を導入する必要がある。しかし、従来技術では、スイッチバック点の位置をパターン化して定められる。このため、汎用性に問題を抱えている。すなわち、スイッチバック点はパターン化して一律に定められるため、そのスイッチバック点を含む走行経路は、上述の走行コストを削減できる最適なものではない。

本発明はこうした実情に鑑みてなされたものであり、車両走行可能な最低の制約条件を満たしつつ、更に、タイヤや車体への負荷を小さく抑えたり、走行に要する所要時間を短くしたり、低燃費で走行させたりすることができる最適な走行経路を生成することによって、走行に伴うコストを最小に抑えることを解決課題とするものである。

なお、特許文献１、２に記載された発明は、走行経路を生成する際に、走行に伴うコストを考慮するものではない。

第１発明は、
無人車両の車幅、最小旋回半径を含む車両拘束条件を入力するとともに、無人車両が走行すべき走行経路生成範囲、回避すべき障害物、入口点および出口点の位置と方向を含む幾何学的拘束条件を入力する条件入力ステップと、
車両拘束条件および幾何学的拘束条件を満足し、かつカーブの大きさまたは／およびカーブの変化率を少なくともコスト要素とするコスト関数の関数値が最小になるように、走行経路を生成する走行経路生成ステップと
を含む無人車両の走行経路生成方法であることを特徴とする。

第２発明は、第１発明において、
前記カーブの大きさは、走行経路の曲率あるいはステアリング角より求めたものであり、前記カーブの変化率は、走行経路の曲率の変化率あるいはステアリング操作量より求めたものであることを特徴とする請求項１記載の無人車両の走行経路生成方法。

第３発明は、第１発明において、
走行経路生成ステップは、
スイッチバック点を導入して、無人車両の走行経路を生成するステップを含み、
スイッチバック点は、コスト関数の関数値が最小になるように定められること
を特徴とする。

第４発明は、第１発明において、
走行経路生成ステップは、
生成された走行経路が車両拘束条件または/および幾何学的拘束条件を満足していないか、あるいはスイッチバック点を導入した方がコスト関数の関数値が小さくなることを判断するステップと、
生成された走行経路が車両拘束条件または/および幾何学的拘束条件を満足しないと判断された場合あるいはスイッチバック点を導入した方がコスト関数の関数値が小さくなると判断された場合に、スイッチバック点を導入して、無人車両の走行経路を生成するス
テップとを含み、
スイッチバック点は、コスト関数の関数値が最小になるように定められること
を特徴とする。

第５発明は、第１発明において、
走行経路生成ステップは、
無人車両が走行すべき走行経路の各補間点を探索する補間点探索ステップと、
入口点から各補間点を経由して出口点に至る経路の各補間点を補間する補間ステップと
を含み、
補間点探索ステップにおける探索または/および補間ステップにおける補間は、コスト関数の関数値が最小になるように行うこと
を特徴とする。

第６発明は、第５発明において、
補間ステップにおける補間は、Ｂ−スプライン曲線を用いて行われること
を特徴とする。

すなわち、第１発明の方法は、図５（ａ）に示すように、つぎの手順からなる。

（条件入力ステップ）
無人車両２０の車幅、最小旋回半径を含む車両拘束条件を入力するとともに、無人車両２０が走行すべき走行経路生成範囲８、回避すべき障害物９、入口点１１および出口点１４の位置と方向を含む幾何学的拘束条件を入力する（ステップ２０１）。

（走行経路生成ステップ）
車両拘束条件および幾何学的拘束条件を満足し、かつカーブの大きさまたは／およびカーブの変化率を少なくともコスト要素とするコスト関数の関数値が最小になるように、走行経路１０を生成する。具体的には、カーブの大きさは、走行経路の曲率あるいはステアリング角より求め、カーブの変化率は、走行経路の曲率の変化率あるいはステアリング操作量より求める（第２発明）。すなわち、車両拘束条件および幾何学的拘束条件を満足し、かつ走行経路１０の曲率あるいはステアリング角または／および走行経路１０の曲率の変化率あるいは無人車両２０のステアリング操作量を少なくともコスト要素とするコスト関数の関数値が最小になるように、走行経路１０を生成する（ステップ２０２）。

図５（ｂ）は、第２発明の方法を説明する図であり、図５（ａ）における走行経路生成ステップ（ステップ２０１）に含まれる処理手順を示したものである。

すなわち、走行経路生成ステップ（ステップ２０１）は、つぎの手順を含む。

（スイッチバック点１２を導入して、無人車両２０の走行経路を生成するステップ）
コスト関数の関数値が最小になるようにスイッチバック点１２を定めて、無人車両２０の走行経路１０を生成する（ステップ２１１）。

図５（ｃ）は、第３発明の方法を説明する図であり、図５（ａ）における走行経路生成ステップ（ステップ２０１）に含まれる処理手順を示したものである。

すなわち、走行経路生成ステップ（ステップ２０１）は、つぎの手順を含む。

（スイッチバック点１２の必要性を判断するステップ）
生成された走行経路１０が車両拘束条件または/および幾何学的拘束条件を満足しないか、あるいはスイッチバック点１２を導入した方がコスト関数の関数値が小さくなると判断する（ステップ２２１）。
（スイッチバック点１２を導入して、無人車両２０の走行経路を生成するステップ）
生成された走行経路１０が車両拘束条件または/および幾何学的拘束条件を満足しないと判明された場合あるいはスイッチバック点１２を導入した方がコスト関数の関数値が小さくなることが判明された場合に（ステップ２２１の判断ＹＥＳ）、コスト関数の関数値が最小になるようにスイッチバック点１２を定めて、無人車両２０の走行経路１０を生成する（ステップ２２２）。

図５（ｄ）は、第４発明の方法を説明する図であり、図５（ａ）における走行経路生成ステップ（ステップ２０２）に含まれる処理手順を示したものである。

すなわち、走行経路生成ステップ（ステップ２０２）は、つぎの手順を含む。

（補間点探索ステップ）
無人車両２０が走行すべき走行経路１０の各補間点を探索する。なお、補間点は、補間すべき区間を結ぶ点のことである（ステップ２３１）。

（補間ステップ）
入口点１１から各補間点を経由して出口点１４に至る経路の各補間点を補間する（ステップ２３２）。

ここで、補間点探索ステップにおける探索または/および補間ステップにおける補間は、コスト関数の関数値が最小になるように行う。

第５発明では、第４発明における補間ステップにおける補間を、Ｂ−スプライン曲線を用いて行う（ステップ２３２）。

第１発明によれば、つぎのような効果が得られる。

ａ）車両拘束条件または/および幾何学的拘束条件を満足するように走行経路１０を生成するようにしているので、障害物９などと干渉することなく、自車の旋回能力などを超えることなく安全に走行させることができる。

ｂ）コスト関数の関数値が最小となるように走行経路１０を生成するようにしているので、
無人車両２０がその走行経路１０を走行した場合、タイヤや車体への負荷が小さくなり、走行に要する所要時間が短くなり、低燃費で走行できる。これにより走行に伴うコストが最小に抑制される。

第２発明、第３発明によれば、つぎのような効果が得られる。

ｃ）スイッチバック点１２を、コスト関数の関数値が最小になるように定めるようにしているため、そのスイッチバック点１２を含む走行経路１０を無人車両２０が走行した場合、タイヤや車体への負荷が小さくなり、走行に要する所要時間が短くなり、低燃費で走行できる。これにより走行に伴うコストが最小に抑制される。

第４発明は、第１発明の走行経路生成ステップを特定する発明である。

第５発明は、第４発明の補間ステップを特定する発明である。

第５発明によれば、つぎのような効果が得られる。

ｄ）Ｂ-スプライン曲線を用いて各補間点同士を補間しているので、曲率や曲率変化率が連続となる滑らかな形状の走行経路１０を生成することができる。このため従来技術に比較して無人車両１０が容易に追従できる走行経路１０を生成することができる。また、従来技術で問題であった経路における曲率の不連続点や曲率の変化率の不連続点がないため、車速を落とす必要がなくなるとともに無理なステアリング操作を強いられたりすることがない。このため生産性の向上が図られる。

以下、図面を参照して本発明に係る無人車両の走行経路生成方法の実施の形態について説明する。なお、本実施形態では、無人車両としてオフロードダンプトラックを想定している。

図１は、無人車両走行システムの全体構成を示すとともに、作業現場の全体図を示している。

作業現場１には、積込場２、排土場３、給油所４、駐機所５といった各エリアがある。これら各エリアがホールロード６と呼ばれる整備された搬送路やアクセスロード７と呼ばれるホールロード６から各エリアへの引込み線や交差点により接続されている。

積込場２は、ダンプトラック２０へ土砂を積み込む作業を行う場所であり、ホイールローダ（フロントエンドローダ）、バックホー、ショベルといった掘削作業車両９１による掘削作業およびダンプトラック２０への土砂の積み込み作業が行なわれる。

排土場３は、ダンプトラック２０に積み込まれた土砂の排出（排土）を行う場所であり、固定位置に排土する場合と、エリア内に規則的に排土する場合とがある。固定位置に排土する場合とは、クラッシャと呼ばれる岩石の破砕機を備えたホッパと呼ばれる投入口９２に排土する場合である。また、排土場３では、ドーザ９３によって、排出された土砂の均し作業が行なわれることがある。この際に荒れた路面を補修するためにドーザ９３によって排土位置が指示され、指示された排土位置へ排土が行われることがある。

給油所４には、燃料ポンプが設置されている。給油所４は、駐機場５に併設されていることが多い。他のダンプトラック２０の運行の妨げにならないように給油車を用いて給油を行う場合であっても給油所を設ける場合もある。

駐機所５は、有人作業の場合は、食事やオペレータ交代などのために車両を止める場所である。無人車両走行システムにおいては、定期点検時など、安全にダンプトラック２０に乗り込むためにダンプトラック２０を引き込んだり、発進操作が行われる場所である。

図２は、実施形態の走行経路１０が生成される経路生成範囲８の様子を例示している。経路生成範囲８は、たとえば上述の排土場３である。本実施形態では、土砂などの積荷を搭載したダンプトラックが無人車両２０として走行経路１０に沿って誘導走行されて経路生成範囲８内を走行する場合を想定する。すなわち、一例として、無人車両２０は、経路生成範囲８に入口点１１から入り、スイッチバック点１２に向かい、スイッチバック点１２でスイッチパックを行い、排土点である停止点（スポット点）１３で停止して、土砂を排出する作業を行い、出口点１４に向かい、出口点１４から経路生成範囲８を出るという走行経路１０を辿る。ただし、無人車両２０は、経路生成範囲８内の障害物９を回避するように走行経路１０に沿って走行する。こうした経路生成範囲８内に生成され、障害物９を回避しつつ入口点１１から出口１４まで無人車両２０が走行する経路のことを、本実施形態では、「走行経路」というものとする。

無人車両２０は、車体前方に運転席（キャブ）が設けられ、車体後方に荷台（ベッセル、ボディ）が設けられ、前輪と後輪が備えられた前輪ステアリングの車両である。

スイッチバック点１２の前後では、無人車両２０の進行方向は、前進方向から後進方向に、あるいは後進方向から前進方向に変化する。

無人車両２０は、停止点（スポット点；排土点）１３に向けて、後進状態で進入する。

図３は、実施形態の無人車両走行システムのブロック図を示している。

作業現場１には、多数の車両２０、２０…を管理、監視する管制装置３０が設けられている。管制装置３０には、処理部３１とデータ入力部３２と通信部３３とが設けられている。一方、車両２０には、通信部２１と処理部２２と位置計測部２３と走行機構部２４と操舵機構部２５と走行経路記憶部２６が設けられている。

車両２０の位置計測部２３では、自己の車両位置が計測される。位置計測の手段としては、たとえば車両２０に設けられたタイヤ回転数センサとジャイロが使用される。これらタイヤ回転数センサの出力信号とジャイロの出力信号とに基づいて、車両位置が計測される。またＧＰＳ衛星９４（図１）から送信される信号をＧＰＳアンテナで受信し、ＧＰＳセンサで検出することにより車両位置を計測してもよい。

車両２０で計測された位置データは、処理部２２で処理され通信部２１を介して管制装置３０に送信される。管制装置３０の通信部３３では、複数の車両２０、２０…から送信された位置データを受信する。受信した位置データは、複数の車両２０の管理、監視に使用される。

管制装置３０のデータ入力部３２には、後述するように、無人車両２０の車幅、最小旋回半径を含む車両拘束条件が入力されるとともに、無人車両２０が走行すべき走行経路生成８の範囲、回避すべき障害物９、入口点１１および出口点１４の位置と方向を含む幾何学的拘束条件が入力される。

走行経路１０は、無人車両２０から要求がある前に管制装置３０で生成し保存、記憶しておく場合（図４（ａ））と、無人車両２０から要求がある毎に生成する場合（図４（ｂ）、（ｃ））とがある。さらに無人車両２０から要求がある毎に走行経路１０を生成する場合でも管制装置３０で走行経路１０を生成する場合（図４（ｂ））と管制装置３０から無人車両２０に走行経路１０を生成するためのデータを送り無人車両２０で走行経路１０を生成する場合（図４（ｃ））がある。

各場合の処理手順につき図４を併せ参照して説明する。

無人車両２０から要求がある前に管制装置３０で走行経路１０を生成し保存、記憶しておく場合には、図４（ａ）に示すように、車両拘束条件および幾何学的拘束条件のデータ入力があると（ステップ１０１）、処理部３１で車両拘束条件および幾何学的拘束条件に基づいて走行経路１０を生成する処理が行われ、生成された走行経路１０のデータが記憶される（ステップ１０２）。無人車両２０から走行経路１０を要求する旨の指令が通信部２１から送信されると（ステップ１０３）、通信部３３で受信され、処理部３１は、この指令を受けて、記憶された走行経路１０のデータを取り出し、通信部３３に送出し、通信部２１に向けて送信する（ステップ１０４）。

車両２０の処理部２２は、受信された走行経路１０のデータに基づいて車両２０を走行させ操舵するための走行指令および操舵指令を生成する。これら走行指令および操舵指令は、走行機構部２４および操舵機構部２５に出力される。この結果車両２０は、管制装置３０から送信された走行経路１０に沿って走行、操舵される（ステップ１０５）。

無人車両２０から要求がある毎に管制装置３０で走行経路１０を生成する場合には、図４（ｂ）に示すように、まず、車両拘束条件および幾何学的拘束条件のデータが入力される（ステップ１０１）。

そして、無人車両２０から走行経路１０を要求する旨の指令が通信部２１から送信されると（ステップ１０３）、通信部３３で受信され、処理部３１は、この指令を受けて、車両拘束条件および幾何学的拘束条件に基づいて走行経路１０を生成する処理を行う（ステップ１０２）。そして、走行経路１０のデータを通信部３３に送出し、通信部２１に向けて送信する（ステップ１０４）。車両２０の処理部２２は、受信された走行経路１０のデータに基づいて車両２０を走行させ操舵するための走行指令および操舵指令を生成する。これら走行指令および操舵指令は、走行機構部２４および操舵機構部２５に出力される。この結果車両２０は、管制装置３０から送信された走行経路１０に沿って走行、操舵される（ステップ１０５）。

無人車両２０から要求がある毎に管制装置３０から無人車両２０に走行経路１０を生成するためのデータを送り無人車両２０で走行経路１０を生成する場合には、図４（ｃ）に示すように、まず、車両拘束条件および幾何学的拘束条件のデータが入力される（ステップ１０１）。そして、無人車両２０から走行経路１０を生成するためのデータを要求する旨の指令が通信部２１から送信されると（ステップ１０６）、通信部３３で受信され、処理部３１は、この指令を受けて、車両拘束条件および幾何学的拘束条件のデータを通信部３３に送出し、通信部２１に向けて送信する（ステップ１０７）。車両２０の処理部２２は、受信された車両拘束条件および幾何学的拘束条件のデータに基づいて走行経路１０を生成する処理を行う（ステップ１０２）。つぎに、生成された走行経路１０のデータに基づいて車両２０を走行させ操舵するための走行指令および操舵指令を生成する。これら走行指令および操舵指令は、走行機構部２４および操舵機構部２５に出力される。この結果車両２０は、管制装置３０から送信された走行経路１０に沿って走行、操舵される（ステップ１０５）。

つぎに、図５を参照して、本実施形態の走行経路１０の生成方法の基本的処理手順について説明する。

図５（ａ）は、上述の図４（ａ）、（ｂ）、（ｃ）におけるステップ１０１のデータ入力処理およびステップ１０２の走行経路生成処理に相当する部分を抜き出したものである。

すなわち、本実施形態の方法は、図５（ａ）に示すように、つぎの手順からなる。

なお、つぎの手順以降に出てくるコスト要素は、無人車両２０の運転にかかわるコストの要素であり、具体的には無人車両が走行する経路の長さ、走行経路上のカーブ等の曲率、無人車両のステアリング操作量を要素とするものである。つまり、走行時には、走行、制動が行なわれ、タイヤ、ブレーキ等の磨耗が生じる。同様にカーブの曲率が小さい、あるいはステアリング操作量が大きく（据え切り等）なればタイヤの負荷が大きくなりタイヤの磨耗が増大する。またこのような状態では走行速度が低下し生産性の低下を招くこととなるのでコスト要素に影響を与える。

また、コスト関数は、前記内容のものを関数化したものである。

（条件入力ステップ）
無人車両２０の車幅、最小旋回半径を含む車両拘束条件を入力するとともに、無人車両２０が走行すべき走行経路生成範囲８、回避すべき障害物９、入口点１１および出口点１４の位置と方向を含む幾何学的拘束条件を入力する（ステップ２０１）。

（走行経路生成ステップ）
車両拘束条件および幾何学的拘束条件を満足し、かつカーブの大きさまたは／およびカーブの変化率を少なくともコスト要素とするコスト関数の関数値が最小になるように、走行経路１０を生成する。具体的には、カーブの大きさは、走行経路の曲率あるいはステアリング角より求め、カーブの変化率は、走行経路の曲率の変化率あるいはステアリング操作量より求める。すなわち、車両拘束条件および幾何学的拘束条件を満足し、かつ走行経路の曲率あるいはステアリング角または／および走行経路の曲率の変化率あるいは無人車両のステアリング操作量を少なくともコスト要素とするコスト関数の関数値が最小になるように、走行経路を生成する（ステップ２０２）。

ここで、コスト関数Ｅについて定義する。以下、コスト関数Ｅについて説明する。

最適性を考慮した経路生成を行う場合、距離だけではなく曲率の大きさやその変化率も問題となる。本実施例では、後述する改良ダイクストラ法により経路探索を行う。その際に各頂点に辿りつくまでのコストを距離だけではなく、以下のように曲率の大きさの推定値、曲率の変化率の推定値を加え、コストを見積もる。また、始点及び終点における車両方向も以下のように考慮して探索を行う。

・始点、終点における車両方向の考慮
始点、終点における車両方向を考慮するために、図４０に示すように仮想点Ｐs、Ｐgを配置する、ただし、

とする。

・ 曲率
曲線における曲率κは、接線傾斜θの変動率と定義され、以下の式（１）で表される。

κ＝ｄθ／ｄｓ …（１）
そこで、図４１に示すように、以下の式（２）に接合点Ｐiにおける辺の角度変化Δθiを前後の辺の長さの平均値で割ることにより接合点Ｐiにおける曲率の推定値κiを見積もることとする。

ここで、スタート点Ｐ0、ゴール点Ｐnにおける曲率の推定値κ0、κnは、それぞれ三点Ｐ0、Ｐs、Ｐ1、三点Ｐn-1、Ｐg、Ｐnを通る円を曲率円と近似することで求める（図４０参照）。図４２に示すように、三点ＰA、ＰB、ＰCを通る円の中心Ｐ0（ｘ0、ｙ0）は下記式（３）、（４）から求めることができる。よって、半径ｒは下記式（５）で求められる。

この半径ｒの逆数として曲率κを下記式（６）のごとく見積もる。

κ＝１／ｒ …（６）
・ 曲率変化率
上記のごとく求めた曲率の推定値κから、下記式（７）に示すように辺の接合点Ｐiにおける曲率変化率の値を見積もることとする。ただし、始点Ｐ0における曲率変化率を０とする。

・ コスト関数Ｅ
以上より、距離だけでなく曲率の大きさ、曲率変化率も考慮したコスト関数Ｅを下記（８）式のごとく定義する。式（８）における第一項は曲率を小さくすることで車両の旋回半径を小さくする項である。同第二項は曲率変化率を小さくすることで単位距離当たりのステアリング操作量を小さくする項である。同第三項は走行距離を短くする項である。ただし、α、β、γは重み定数とする。

上記式（７）において、第一項の「走行経路の曲率」の代わりに「車両のステアリング角」を用いてもよい。また第二項の「走行経路の曲率の変化率」の代わりに「車両のステアリング操作量」を用いてもよい。

図５（ｂ）は、図５（ａ）における走行経路生成ステップ（ステップ２０１）に含まれる処理手順を示したものである。

すなわち、走行経路生成ステップ（ステップ２０１）は、つぎの手順を含む。

（スイッチバック点１２を導入して、無人車両２０の走行経路を生成するステップ）
コスト関数の関数値が最小になるようにスイッチバック点１２を定めて、無人車両２０の走行経路１０を生成する（ステップ２１１）。

図５（ｃ）は、図５（ａ）における走行経路生成ステップ（ステップ２０１）に含まれる処理手順を示したものである。

すなわち、走行経路生成ステップ（ステップ２０１）は、つぎの手順を含む。

（スイッチバック点１２の必要性を判断するステップ）
生成された走行経路１０が車両拘束条件または/および幾何学的拘束条件を満足しないか、あるいはスイッチバック点１２を導入した方がコスト関数の関数値が小さくなると判断する（ステップ２２１）。
（スイッチバック点１２を導入して、無人車両２０の走行経路を生成するステップ）
生成された走行経路１０が車両拘束条件または/および幾何学的拘束条件を満足しないと判明された場合あるいはスイッチバック点１２を導入した方がコスト関数の関数値が小さくなることが判明された場合に（ステップ２２１の判断ＹＥＳ）、コスト関数の関数値が最小になるようにスイッチバック点１２を定めて、無人車両２０の走行経路１０を生成する（ステップ２２２）。

図５（ｄ）は、図５（ａ）における走行経路生成ステップ（ステップ２０１）に含まれる処理手順を示したものである。

すなわち、走行経路生成ステップ（ステップ２０１）は、つぎの手順を含む。

（補間点探索ステップ）
無人車両２０が走行すべき走行経路１０の各補間点を探索する（ステップ２３１）。

（補間ステップ）
入口点１１から各補間点を経由して出口点１４に至る経路の各補間点を補間する（ステップ２３２）。

ただし、補間点探索ステップにおける探索または/および補間ステップにおける補間は、コスト関数の関数値が最小になるように行う。

また、補間ステップにおける補間は、Ｂ−スプライン曲線を用いて行う。

以下、上述の走行経路生成方法の内容について具体的に説明する。

（車両拘束条件）
図６（ａ）は、無人車両２０のサイズ、走行、操縦等の限界としての拘束条件を示した表である。図６（ｂ）は、無人車両２０の上面図であり、無人車両２０の各サイズを示す。図６（ｃ）は、無人車両２０が旋回する様子を示した上面図である。図７（ａ）は、無人車両２０がステアリングを切る様子を示した上面図である。

車両拘束条件は、無人車両２０の車両基準点２０ｃから車体前端までの距離ＦＳｉｚｅ（図６（ａ）、（ｂ）参照）、車両基準点２０ｃから車体後端までの距離ＲＳｉｚｅ（図６（ａ）、（ｂ）参照）、車幅Ｗｉｄｔｈ（図６（ａ）、（ｂ）参照）、ホイールベースＷＢＬ（図６（ａ）、（ｂ）参照）、最小旋回半径（図６（ａ）、（ｃ）参照）、最高前進速度、最高後進速度、最大加速度、最大減速度、最大許容後進加速度、単位時間当たりの平均的最大ステアリング操作量（図６（ａ）、図７（ａ）参照）などである。

（幾何学的拘束条件）
図７（ｂ）は、無人車両２０が走行する領域の拘束条件である幾何学的拘束条件を示したものである。無人車両２０が走行すべき走行経路生成範囲８、回避すべき障害物９、入口点１１の位置と方向、出口点１４の位置と方向、停止点１３の位置の方向、スイッチバック点１２の位置と方法などが幾何学的拘束条件となる。

（経路の最適性条件）
無人車両２０の走行コストを削減するためには、無人車両２０の走行に要する所要時間を短くすること、低燃費であること、タイヤや車体への負荷を少なくすることが必要である。低燃費にするには、加減速度を小さくすること、総距離を短くすることなどが必要がである。また、車体やタイヤへの負荷を少なくするためには、単位距離当たりのステアリング操作量が少ないこと（曲率の変化率が小さいこと）、滑らかにステアリング操作が行えること（ステアリングの操作量の変化量が小さいこと）が必要である。なお、曲率の２次微分値を用いることによりステアリングの操作量の変化量の近似値とすることができる。

本実施形態では、上記の車両拘束条件、幾何学的拘束条件に加えて、上述するコストを考慮した経路の最適性条件が満たされるように走行経路１０を生成するようにしている。

（基本的経路の生成）
以下では、走行経路１０の基本的経路の生成方法について説明する。

図８は、基本的経路の生成方法の処理手順を示す。以下、図８を参照して説明する。

図８に示す処理は、図５に示す走行経路生成ステップ（ステップ２０１）の具体的処理手順を示したものである。

（ミンコフスキー和（Minkowski Sum）計算）
幾何学的拘束条件にミンコフスキ和の計算手法を適用して、経路生成範囲８、障害物９を膨張させる処理を行う。具体的には、無人車両２０の車幅分の直径の円盤を経路生成範囲８、障害物９の境界に沿って移動させ、その総和となる部分を膨張範囲とする。膨張した経路生成範囲８、障害物９の外側を無人車両２０が走行できる範囲と定めることにより、無人車両２０が経路生成範囲８、障害物９に干渉しないようにすることができる。

図１６（ａ）、（ｂ）は、ミンコフスキー和の計算を説明するための図である。

図１６（ａ）は、入力された経路生成範囲８と障害物９を示している。

経路生成範囲８の境界８ａ、障害物９の境界９ａおよび走行すべき円盤の境界は、多角形に近似されて、ミンコフスキー和の計算が行われる。本実施例では、経路生成範囲８の境界や障害物９と干渉せずに無人車両２０を移動させるために、経路生成範囲８の境界や障害物９を車幅の半分だけ拡大する。この量は、無人車両２０が走行している場合に障害物に干渉させないための最小の量である。このため、必ずしも干渉を妨げるというものではない。走行中の無人車両２０の向きは多様である。このため、適した量を見積もることは困難である。具体的には、セットＡとセットＢの和を、

と定義するミンコフスキー和を用いる。

図１６（ｂ）は、経路生成範囲８、障害物９と円盤とのミンコフスキー和を示す。経路生成範囲８はその境界８ａが外形線８ｂまで膨張される。同様に障害物９はその境界９ａが外形線９ｂまで膨張される。

ミンコフスキ和で見積もられた経路生成範囲８と障害物９の外形線８ｂ、９ｂは、円弧と直線で示される。円弧状になっている部分は、後述の可視グラフを引くために、円弧上に数点を取り、外形を多角形とする。

以上がミンコフスキー和計算処理の内容である（ステップ３０１）。

（可視グラフ（Visibility Graph）の作成）
つぎに、ミンコフスキー和で見積もられた経路生成範囲８と障害物９に対して可視グラフを引く。可視グラフとは、経路生成範囲８と障害物９の外形線（多角形）の各頂点同士を相互に結んだ辺の集合のことであって、障害物９を通り抜けない互いに「見える」ように結んだ辺の集合のことである。可視グラフは、最短距離を求めるためによく用いられるグラフである。ここで、最適な経路を求める場合、各頂点における可視辺が接線とならないとき最短経路となり得ない辺であるため、そのような接線とならない辺は予め除かれる。

図１７は、ミンコフスキー和された経路生成範囲８と障害物９の外形線８ｂ、９ｂに対して引かれた可視グラフを例示する。ミンコフスキー和された経路生成範囲８と障害物９の外形線８ｂ、９ｂが円弧となっている部分については、弧長を五等分する点をとり、多角形とした。

ある２頂点に対して、「互いに見える」とき、つまり２頂点を結んだ線分が、どのミンコフスキー和された経路生成範囲８、障害物９の外形線８ｂ、９ｂにも交差しないとき、互いに可視であるとして、それらを結ぶ線分（可視辺）を引く。障害物９上の隣り合う頂点からなる辺は可視辺となる。

最短距離を求める場合、各頂点において可視辺が接線とならない辺は最短距離となり得ない辺であるので、この辺は取り除かれる。凹形状となるときには接線にはならない。

接線か否かを判定する方法を図１８を用いて説明する。

同図１８に示すように、頂点Ｐから可視グラフ上のある点Ｑへ向かうベクトルをＶとし、またＰの両隣の頂点へ向うベクトルを

とする。このとき

と表される。上記（３．１）式における係数α、βを求めて、この２つの係数α、βの関係が
αβ＜０
となるとき接線であると判定する。

つぎに、２頂点において、可視辺であり、この接線であるという条件を満たしているときのみ、線分を引く。こうして図１７に示す可視グラフから接線にならない辺を取り除いたグラフを図１９に示す。

ただし、この時点では、入口点１１、出口点１４、スイッチバック点１２、停止点１３の方向は考慮されていない。

以上が可視グラフを作成する処理の内容である（ステップ３０２）。

（改良ダイクストラ法による最適となる経路候補の辺（補間点候補）の探索）
つぎに改良ダイクストラ法を適用して最適となる経路候補の辺を探索する。

さて、一般的なダイクストラ法は、複数個の多角形障害物があるエリアの始点から終点までの最短経路をグラフ理論により求める手法である。しかし、無人車両２０の経路生成の場合は、距離だけではなく曲率の大きさや曲率の変化率がコスト上、問題となる。そこで、本実施形態では、一般的なダイクストラ法を拡張した探索法を適用する。

まず、はじめに図２０を用いて、基本的な距離を重みとする一般的なダイクストラ法について説明する。

図２０は、始点から終点までの最短距離となる経路を示している。

図２０に示す経路を求めるアルゴリムはおよそ以下のとおりである。すなわち、予め可視グラフで辺を形成している頂点ｉ、ｊに対して距離を計算し、最大ｎ×ｎの２次元配列ＭのＭij,Ｍjiに格納しておく。ただし、辺を形成していない場合には、十分大きな値を格納する。始点から可視グラフのすべての辺へ等速度で広がっていくように調べていき、各頂点における始点からの最短距離を求める。１点づつ最短距離となる頂点を調べていき、終点まで辿りついたら終了させる。

このように距離を各辺の重みとすることで、重みが最小となる辺の組み合わせ、つまり総距離が最小となる辺の組み合わせを探索する。

こうして図２０に示すごとく、最短距離となる経路が求められる。

ただし、一般的なダイクストラ法では、入口点１１、出口点１４、スイッチバック点１２、停止点１３の方向は考慮されていない。

これに対して本実施例では、一般的なダイクストラ法を改良した改良ダイクストラ法を適用する。改良ダイクストラ法では、各辺に対して距離に加えて、予想できる曲率の大きさ、単位距離当たりのステアリング操作量等を予測して重みを決めることで無人車両２０にとって最適となる経路候補の辺の組み合わせを探索するものである。つまり、走行経路長に加えて走行経路１０の曲率、無人車両２０のステアリング操作量（曲率の変化率）をコスト要素とするコスト関数の関数値が最小になるように、経路候補の辺の組み合わせ、つまり補間点の候補を探索する。また、当然、経路の始点と始点の方向を考慮して探索を行う。

コスト関数Ｅは、以下のように定義される。

ただし、κは曲率、θはステアリング操作量、ｓは始点からの距離である。また、ｗ1、ｗ2、ｗ3は重み定数である。上記式の第１項は、旋回半径（曲率κの逆数）を大きくするための項、第２項は、単位距離当たりのステアリング操作量を少なくするための項、第３項は、走行距離を短くする項である。各項を小さくして、コスト関数Ｅの関数値を小さくすれば、旋回半径が大きくなり、単位距離当たりのステアリング操作量が少なくなり、走行距離が短くなる。

ステアリング角θと曲率κの関係は、ホイールベース長をWBLとすると、下記式の関係が成立する。

よって、右辺第１項は、下記式のごとくなる。

ただし、ｗ´1＝ｗ1／WBLであり、θは小さいとし、また区間毎の曲率は一定として近似する。

また、右辺第２項は、図２１に示すように、ある頂点の周りの２つの直線の変化角をステアリング操作量とし、それをその区間の距離で割ることで単位距離当たりのステアリング操作量として近似する。よって上記（３．２）式は、

のようになる。

始点と終点では方向が与えられているため以下のような補正が必要となる。

図２２におけるＰ点のコストを求めるには、始点の方向ベクトルと始点からつぎの点へのベクトルからなる角度を２分する直線上に、始点の向きを考慮するため、仮想的に点Ｓtを配置する。また、Ｐから終点までのベクトルと終点の方向ベクトルの角度を２分する点Ｇtを同様に配置する。これらにより、始点と終点の向きを考慮に入れた経路を求めることができる。計算に用いる点は、たとえばDouglas-Peucker Algorithumによって特徴点のみを抽出して行う。これは、点の数が増えるとコストが増えるためである。計算アルゴリズムは、ダイクストラ法のアルゴリズムと同様であり、この方法により得られる結果は、図２３に示すごとくなる。ここで図２０に示す一般的なダイクストラ法を適用した結果では、始点と終点の向きは考慮されなかったが、図２３に示す改良ダイクストラ法を適用した結果では、始点と終点の向きが考慮されているのがわかる。

以上が改良ダイクストラ法による処理の内容である（ステップ３０３）。

（Ｂ−スプライン曲線による補間）
つぎに、改良ダイクストラ法を適用して得られた辺を結ぶ頂点を補間点として、たとえば３次のＢ-スプライン曲線を用いて補間を行う。ただし、３次以外のＢスプライン曲線でで補間してもよい。

以下では、ｎ＋１個の補間点｛Ｑk｝、ｋ＝０，…，ｎと、始点と終点における向きが与えられている場合のＢ-スプライン補間曲線の求め方を説明する。

Ｂ-スプライン曲線を描くには、制御点とノットベクトルが必要である。そこで、まず、ノットベクトルの求め方を示す。

弧長の和をｄとすると、

となり、そのとき、

とすると、ノットベクトルの各成分は、ｐ次の場合には、

として求める。

つぎに、次数が３次の場合の制御点を求める。

制御点を｛Ｐi｝とする。ここで、始点と終点は向きが与えられているため、

となり、Ｐ0，Ｐ1，Ｐn+1，Ｐn+2は直接求めることができる。ここでＤiはそれぞれの単位ベクトルである。

３次のＢ-スプライン曲線で補間点は、

のようになる。ただし、ｋ＝１，…，ｎ−１である。

とおくと、

の対角システムを解くことにより制御点｛Ｐi｝を求める。このシステムは、未知数（ｎ−１）個の線形システムである。図２４に補間したＢ−スプライン曲線を例示する。

以上がＢ-スプライン曲線による補間処理の内容である（ステップ３０４）。

改良ダイクストラ法で得られた補間点の候補は、ミンコフスキ和された障害物９等の上にあることがある。このため、その補間点候補から求められる曲線は、無人車両２０の旋回限界を満たさないなど、必ずしも最適な経路とはいえない。よって、補間点候補が無人車両２０の最小旋回半径などの車両拘束条件を満たすか否かの判断を行うことにより、補間点の移動が必要か否かを判断する（ステップ３０５）。

（補間点の最適化）
補間点の移動が必要な場合（ステップ３０５の判断ＹＥＳ）には、補間点の移動を行い、補間点を最適化する。これは、改良ダイクストラ法で得られた補間点の近辺に任意の格子を設け、その格子点の中から、曲率やステアリング操作量をコスト要素とするコスト関数の関数値が最小となる点を新たな補間点とすることにより行う。

すなわち、ここまでに得られた補間点は、ミンコフスキー和された障害物上にあるため、その補間点を通過するように描かれた曲線は、図２５（ａ）に修正前経路として示すように、無人車両２０の最小旋回半径を満たさないなど、必ずしも適した経路とはなっていない。そこで、ここまでに得られた補間点に基づいて補間点を適宜移動して補間点の最適化を行う。

補間点を最適化するためのコスト関数（目的関数エネルギー）Ｅを

と定義し、その関数値の最小化を行う。ここで、ｗ4、ｗ5は重み定数である。上述の式（３．５）と同様の近似により、

となる。補間点近辺に車両幅Ｗｉｄｔｈの二分の一となる幅となる格子を作り、その格子点の中からコスト関数Ｅの関数値が最小となる格子点を新たな補間点とする。

図２５（ｂ）に、新たな補間点とその補間点を通過するように描かれた修正後経路を示す（ステップ３０６）。

（最適補間）
補間点が同じであっても、補間点を通過する曲線の形状によって、無人車両２０にかかるコストは異なるものとなる。そこで、曲率やステアリング操作量をコスト要素とするコスト関数の関数値が最小となるように各補間点を通過する曲線の形状を求める。具体的には、曲線の始点、終点における接線ベクトルα、βの大きさを最適化することにより行う。通常、曲線の始点、終点におけるα、βは単位ベクトルであり、大きさは与えられていない。方向ベクトルα、βの大きさによって各補間点を結ぶ曲線の曲率などが変動する。そこで、コストが最小となるように方向ベクトルα、βの大きさを調整して各補間点を結ぶ曲線の曲率を最適なものとする。

以下、具体的に説明する。

図２６は、上述の（３．９）式に示す係数αと上述の（３．１０）式に示す係数βの値によって補間点同士を結ぶ曲線の形状が変化する様子を示したものである。

そこで、以下の式をコスト関数（目的関数エネルギー）として、その関数値が最小になるように係数α、βを決定する。

上記式の右辺第１項は、曲率の絶対値を小さくする項である。同右辺第２項は、単位距離当たりのステアリング操作量を少なくする項である。

ここで、上述した（３．１３）式を解くことにより、制御点は、

のように表される。

よって、Ｂ-スプライン曲線の定義式

より、

となる。ただし、

となる。同様に、

となる。これらの式を上述の（３．１６）式に適用することでコスト関数Ｅが求められる。このコスト関数Ｅの関数値を最小化するために、たとえばSteepest Descent法を用いて最適化を行う。

Steepst Descent法は、最適化問題に用いられる手法である。この方法は、図２７に示すように、ポテンシャル面の傾き（一階微分）のみから、系のエネルギーが小さくなる方向へ更新を繰り返すことにより、より小さくなる関数値Ｅ1、Ｅ2、Ｅ3、Ｅ4を順次探索し、最終的に最小値Ｅminを探索するというものである。計算方法の手順は以下のようになる。

１． まず初期値

を入力する。

２． 状態ｘがｘ(k)のとき状態ｘの値を以下のように更新する。

ここで、λは一回で更新する数値の割合を決めるパラメータであり、通常、λは小さな正の値をとる。

３．

となるか（ただし、ωはしきい値）または、

となるまで状態ｘの更新を繰り返す。

ここで、状態ｘの更新割合λについて説明する。

更新割合λは正の値であり、また、

の条件を満たす必要がある。また、精度の良い最適化には、λは極めて微小な値が望ましいが、更新の回数が増えて計算に時間がかかってしまう。そこで、図２８に示すようにλをパラメータとしたコスト関数（目的関数）Ｅ（λ）を、下に凸な二次関数で近似が可能となるように適当に三点を与え、近似を行う。この近似した二次関数の最小値をとる値λを状態ｘの更新割合と定める。この手法により効率的な計算が可能となる（ステップ３０７）。

（最大曲率の低減手法の適用）
３次のＢ−スプライン曲線は曲率連続であり、経路に適した曲線である。しかし、無人車両２０の最小旋回半径よりも小さい経路半径では物理的に走行できない。そこで、最小の経路半径を大きくする、つまり最大曲率を小さくする方法を適用することにより、経路生成を行い易くする。

Ｂ−スプライン曲線は、曲線上の点を求めるために影響する制御点をノットベクトルの間隔により変えることができる。よって、曲率が大きいところのノット間隔を大きくすれば、その付近の制御点の影響が広範囲にわたる。これにより、元の曲線の凸形状よりも緩やかな凸形状になる。このことを利用したノットベクトルの変更方法を以下に示す。

すなわち、Ｂ−スプライン曲線は、Bezier曲線がノットの間隔ごとに連続的につながっているものである。そこで、各ノットごとに、

を求める。ここで、曲率κには、最大曲率値Ｃmaxを割り当て、ｄｓには、各ノットの間隔Δｕ＝ｕi+1−ｕiを用いる。そこで各曲線の最大曲率を

ノット間隔をΔｕ1，Δｕ2，…，Δｕi，…とする。ノットの間隔を変えるために、新しい間隔比を

とする。ただし、Δｕ´iは新しいノット間隔比である。この操作を最大曲率を満たすまで、もしくは、一定回数繰り返す（ステップ３０８）。

（無人車両２０との干渉チェック）
つぎに、無人車両２０が障害物８等と干渉しないか否かをチェックする（ステップ３０９）。

無人車両２０との干渉があると判断された場合には（ステップ３０９の判断ＹＥＳ）、手順はステップ３０６に戻り、補間点の移動を再度行う。

無人車両２０との干渉がないと判断された場合には（ステップ３０９の判断ＮＯ）、処理を終える（ステップ３１０）。

以上が基本的経路の生成方法の内容である。

さて、無人車両２０が停止点１３にて排土作業（あるいは積込作業）を行うとき、後進状態で停止点１３に進行する必要がある。このときには、経路生成範囲８内に単独のスイッチバック点１２を設けて、無人車両２０の車体の進行方向の転換（切り返し）を行う必要がある。また、上述の基本的経路生成の方法で得られた曲線で無人車両２０の最小旋回半径を満たさないときにも、二点のスイッチバック点１２、１２を設けて、切り返しを行う必要がある。

スイッチバックの１点には、位置（ｘ、ｙ）と向き（θ）の３つの未知数がある。スイッチバック点１２の位置（ｘ、ｙ）、そのスイッチバック点１２における無人車両２０の向き（θ）によって、得られる走行経路１０は異なるものとする。また、これらの値により、無人車両２０にかかる負荷が大きく異なる。

そこでスイッチバック点１２を定める際にもコスト関数を定義して、コスト関数の関数値が最小になるように計算して、走行経路１０を最適化する。スイッチバック点１２を定める際に用いられるコスト関数のコスト要素は、後述するように曲率、ステアリング操作量、経路長、ステアリングの据えきり量からなる。

スイッチバック点１２を最適化する処理を行う場合、経路生成範囲８内にはスイッチバック点１２の候補となる点が無数に存在する。また、局所的な最適解も多く存在する。このため局所探索法は好ましくない。そこで、大域探索法として、シミュレーテッド アニーリング（Simulated Anneaiing）法を用いてスイッチバック点１２の最適化を行う。

（スイッチバックの必要性判定）
スイッチバックの必要性があるとする判定基準は、以下に列挙するとおりである。

ａ）始点からの移動方向が後進かつ終点到達時の移動方向が前進
ｂ）始点からの移動方向が前進かつ終点到達時の移動方向が後進
ｃ）生成された経路が車両拘束条件を満足しない場合、たとえば最小旋回半径を満足しない場合である。

ｄ）生成された経路が幾何学的拘束条件を満足しない場合、たとえば障害物９に干渉するおそれがある場合である。ただし、可視グラフ上での経路が存在しない場合は、スイッチバック点を増やすことにより改善されないので経路生成は失敗と判断する。

ｅ）スイッチバックを導入した方がコスト関数の関数値が小さくなる場合。つまり最適化のためにスイッチバックを挿入した場合と、スイッチバックを挿入しない場合とを対比して、スイッチバックを挿入した方がコスト関数の関数値が小さくなる場合にスイッチバックの必要性があると判断する。

無人車両２０の最小旋回半径を満足しないときにスイッチバックを導入する場合を図２９に例示する。

すなわち、図２９（ａ）に示すように、始点から終点までの経路がスイッチバックなく生成されたとする。しかし、この経路は、無人車両２０の最小旋回半径を満足しない。そこで、図２９（ｂ）に示すように、二点のスイッチバック点１２、１２を経路の途中に設けて経路を生成し、無人車両２０が最小旋回半径以上の旋回半径で走行できるようにする。

つぎに、スイッチバック点１２の最適化に用いられるコスト関数について説明する。

スイッチバック点１２を含めた経路のコスト関数Ｅは、以下のような各コスト要素からなる。
コスト関数Ｅ＝Ａ×旋回半径コスト
＋Ｂ×ステアリング操作量コスト
＋Ｃ×ステアリング操作の滑らかさコスト
＋Ｄ×経路長コスト
＋Ｅｅ×スイッチバック点におけるすえ切りコスト

（３．２５）
ここで、Ａ、Ｂ、Ｃ、Ｄ、Ｅｅは、どのコストを重要視するかを規定する重み定数である。また、旋回半径コスト、ステアリング操作量コスト、ステアリング操作の滑らかさコスト、経路長コスト、スイッチバック点におけるすえ切りコストは、以下のように定義される。

（旋回半径コスト）
旋回半径コストは、曲率から求められる瞬時コストを経路に沿って全積分することによって得られる。例えば、経路に沿ってｓ移動した位置における曲率κ（ｓ）が、しきい値κ0（ｓ）より大きくなるときに、その地点における最高速度Vmax（ｓ）に到達できないとすると、

であるから、κ0（ｓ）を用いて曲率から求められる瞬時コストＥρ（ｓ）を次のように定義する。

このとき、旋回半径コストは、以下のとおりとなる。

曲率κとステアリング角θの関係、
ｔａｎ（θ）＝ホイールベース長×κ
により、曲率κ（ｓ）に対応するステアリング角θ（ｓ）、しきい値κ0（ｓ）に対応するステアリング角θ0（ｓ）を求め、瞬時旋回半径コストＥθ（ｓ）、旋回半径コストを下記のように求めてもよい。

としてもよい。

（ステアリング操作量コスト）
ステアリング操作量コストは、ステアリング操作量から求められる瞬時コストを経路に沿って線積分することによって得られる。例えば、経路に沿ってｓ移動した位置において、単位距離当たりのステアリング操作量ｄθ（ｓ）がｄθ0（ｓ）より大きくなると、その地点の最高速度Vmax（ｓ）に到達できないとすると、

であるから、ｄ0（θ）を用いてステアリング操作量から求められる瞬時コストＥdθ（ｓ）を次のように定義する。

このときステアリング操作量コストは、以下のとおりとなる。

η｜ｄθ（ｓ）｜は、低速域におけるふらつき防止効果向上のための項であり、ηは重み定数である。

また、ｄθ0（ｓ）の式を曲率κ（ｓ）で解くと、対応する曲率の変化量ｄκ0（ｓ）は、

である。これを用いて、瞬時コストＥdκ（ｓ）、ステアリング操作量コストを下記のように求めてもよい。

すなわち、ステアリング操作量コストは、曲率の変化量の経路沿いの線積分として求められる。

（ステアリング操作の滑らかさコスト）
ステアリング操作の滑らかさコストは、ステアリング操作量の変化量から求められる瞬時コストを経路に沿って線積分することによって得られる。例えば、瞬時コストおよびステアリング操作の滑らかさコストは、以下のとおりとなる。

ステアリング操作の滑らかさコストは、曲率の２次微分値の経路沿いの線積分となるように求めてもよい。この場合、瞬時コストおよびステアリング操作の滑らかさコストは、以下のとおりとなる。

（経路長コスト）
経路長コストは、
経路長コスト＝経路長
なる式で求めることができる。ただし、前進と後進とで著しく走破性能が異なる場合などにおいては、
経路長コスト＝前進コスト×前進経路長＋後進コスト×後進経路長
のように、コースの性質に合わせて重み付けをして求めてもよい。

（スイッチバック点におけるすえ切りコスト）
スイッチバック点およびスイッチバック点の極近傍においてステアリング操作を行う経路となっているときには、すえ切りが発生する。このため、単位距離当たりのステアリング操作量ｄθ（ｓ）をスイッチバック点の前後の短い区間に限定して線積分することによって、スイッチバック点におけるすえ切りコストを計算することができる。

すなわち、ｎ番目のスイッチバック点が経路に沿ってｓn移動した位置にあるとすると、短い距離定数Ｌを用いて、次のように、スイッチバック点におけるすえ切りコストを定義することができる。

また、スイッチバック点におけるすえ切りコストは、つぎのとおり、曲率の変化率の線積分となるように求めてもよい。

以上がスイッチバック点１２を定める際のコスト関数Ｅの内容である。

シミュレーテッド アニーリング（Simulated Anneaiing；以下、ＳＡと略す）法を用いてスイッチバック点１２の最適化を行うアルゴリズムは、図９に示される。

さて、シミュレーテッド アニーリング（ＳＡ）は、モンテカルロ法的な最適化手法である。その名称は、金属工学における「焼きなまし」から来ている。焼きなましとは、金属材料を熱した後で徐々に冷やし、結晶を成長させてその欠陥を減らす作業である。熱によって原子は初期の内部エネルギーがローカルな極小状態から離され、よりエネルギーの高い状態へ遷移する。ゆっくり冷却することで、原子は初期状態よりも内部エネルギーがさらに極小な状態を得る可能性が多くなる。ＳＡは焼きなまし法を計算で模倣したものである。局所探索法では局所最適解に陥った場合の対処を考慮する必要がある。しかし、ＳＡの場合には、そのような必要がない。ＳＡでは、探索空間の各点“ｓ”は上記の“状態”に対応し、最小化すべき目的関数Ｅ（ｓ）が内部エネルギーに対応する。以下では、スイッチバック点一つにつき三つのパラメータ（ｘ、ｙ、θ）をもつＳＡの計算過程を説明する。

すなわち、まず、アニーリングの温度Ｔ、スイッチバック点１２の位置、方向の初期値の設定を行う。

状態ｓの初期設定、つまりスイッチバック点１２の初期値設定は、スイッチバック点１２が一点設けられる場合と二点設けられる場合とで異なる。

（一点設定の場合）
図３０に示すように、経路の曲線上の停止点１３から一定の距離Ｌ1の点（曲線長がＬ1に満たない場合、曲線のパラメータにおける中点）から曲線の凸側法線方向へ一定の距離Ｌ2分だけ距離をとった点を初期のスイッチバック点１２とする。また、法線方向をそのスイッチバック点１２における方向θとして指定する。

（二点設定の場合）
図３１（ａ）に示すように、経路上で曲率半径が無人車両２０の最小旋回半径を満たさない部分（破線部で示す）の始めの点をＰ(i1)とし、曲率最大値となる点を越えて曲率半径が無人車両２０の最小旋回半径を満たさない部分の終わりの点をＰ(i2)とする。また、パラメータ上でi1,i2の中点となる点をＱ（i0）とする。

つぎに、図３１（ｂ）に示すように、点Ｐ(i1)を基準としパラメータ降順に曲線長が一定距離Ｌ3分になる点をＱ(i3)、同様に昇順に一定距離Ｌ4分になる点をＱ(i4)とする。求めたＱ(i3) 、Ｑ(i4)から曲線の凸側法線方向へ一定距離Ｌ5だけ移動した点の位置をそれぞれスイッチバック点１２、１２の初期値（ｘ1,ｙ1）、（ｘ2,ｙ2）とする。また、その法線方向をスイッチバック点１２、１２における方向（θ1）、（θ2）として指定する（ステップ４０１）。

以下、初期温度Ｔからアニーリングを行う（ステップ４０２〜４０７）。

すなわち、各ステップでは、ＳＡのヒューリスティック（heuristik；正解導入方法）は、現在状態ｓoldのいくつかの近傍ｓnewを検討し、現在状態ｓのままでよいか、いずれかの近傍状態に遷移するのがよいかを確率的に決定する。その際にシステムが最終的にエネルギーの低い状態へ向かうように考慮する。遷移確率は、

である。ただし

である。ｋはボルツマン定数、Ｔは温度である。

遷移条件は、
Ｐ＞Ｒ （３．４３）
であり、Ｒは、０＜Ｒ＜１を満たす無作為の実数である。

式（３．４１）のように、ＥnewがＥoldより小さいときには必ず遷移する。しかし、 ＥnewがＥoldより大きいときには確率により遷移可能となる。これは局所的な最適解に張り付いてしまうことを防ぐためである。

状態ｓ（ｘ、ｙ、θ）は温度Ｔに依存させて変化させる。その具体的な変化は以下のようになる。

ここで、

は経路生成範囲８のバウンディングボックス（bounding box；境界線を表す四角いエリア）におけるそれぞれの方向の辺の半分の値を最大値となる無作為の数である。

は、範囲−π〜πの範囲の無作為な数である。

このアニーリングのステップは温度Ｔを徐々に上げていき、系のエネルギーの変化率が十分に大きくなるまで、たとえば１００００％になるまで、あるいは予定された計算時間が尽きるまで、たとえば繰り返し２００回になるまで行う。

以下、手順を説明する。

まず、乱数を用いて無作為な数を取得し、温度Ｔに依存させながら状態ｓ（ｘ、ｙ、θ）を変化させ、上記式（３．４４）のごとく、スイッチバック点１２（位置ｘ、ｙ、方向θ）を変更する（ステップ４０２）。

つぎに、変更されたスイッチバック点１２を通過するように基本的経路を生成する。基本的経路は、Ｂ−スプライン曲線を用いて生成される。前述の図８のステップ３０２ないしステップ３１０の処理が実行されて、スイッチバック点１２を通過する基本的経路が生成される（ステップ４０３）。

つぎに、生成された基本経路の経路形状コストＥを算出する。すなわち、生成された基本経路について、前述の（３．２５）式のコスト関数Ｅを計算する（ステップ４０４）。

つぎに、遷移条件を満たすか否かが判断される。上述したようにコスト関数Ｅの関数値Ｅnewが以前のものＥoldよりも小さいときには、必ず遷移するが、コスト関数Ｅの関数値Ｅnewが以前のものＥoldよりも大きいときには確率により遷移可能となる（ステップ４０５）。

遷移条件を満たす場合には、スイッチバック点１２（位置ｘ、ｙ、方向θ）を更新する（ステップ４０６）。

しかし、遷移条件を満たさない場合には、ステップ４０７に移行し、クーリング条件を満たすか否かを判断する。上述したように一定回数のカウント値を超えた場合あるいはコスト関数Ｅの変化が十分に小さくなったときをもって、クーリング条件が満たされたとして、つぎのステップ４０８のクーリングに移行する。

クーリングでは、アニーリング終了時の温度Ｔから温度を徐々に下げていく。このようにすると、システムは最初のうちはエネルギー関数Ｅの小さな変化を無視して最適解を求めて探索空間の広い領域をさまよい、徐々にエネルギーの低い領域に向かって探索範囲を狭めていき、最終的に最もエネルギーの低い状態に降りていく。クーリングスケジュールには、様々な方法がある。ただし、最終的には温度Ｔ＝０で終わらなければならない。本実施形態では、図３２に示すように、

なる式で示すクーリングスケジュールでクーリングを行う。ただし、ｉはサイクルである（０≦ｉ≦Ｎ）。

クーリングの具体定な手順は以下のとおりである。

すなわち、図３２に示すクーリングスケジュールに従い、温度Ｔを更新していく（ステップ４０８）。

つぎにクーリングの終了条件が満たされているか否かが判断される。具体的には、図３２に示すクーリングスケジュールが終了するか、あるいは、コスト関数Ｅの関数値がしきい値よりも小さくなったときをもって終了したと判断する（ステップ４０９）。クーリングの終了条件を満たす場合には、このスイッチバック点最適化アルゴリズムを終了させる（ステップ４１０）。

クーリングの終了条件を満たさない場合には、手順はステップ４０２に戻り、同様の処理を繰り返す。

つぎに、本実施形態の各実施例について説明する。

本実施例の方法には、大きく分けて「一度生成したスイッチバック点を固定する方法」と「スイッチバック点の追加に伴いスイッチバック点の最適化をやり直す方法」とがある。

図１０は、「一度生成したスイッチバック点を固定する方法」を示している。

（一度生成したスイッチバック点を固定する方法）
同図１０に示すように、まず、上述の図８のステップ３０１と同様に、経路生成範囲８、障害物９の境界に沿って車両幅を直径とする円盤を移動させて、ミンコフスキー和を計算する（ステップ５０１）。

つぎに、スイッチバック点１２が必要か否かを判断する。この場合の判断基準は、上述の（スイッチバックの必要性判定）の各判定基準のうち、以下の判断基準により行なう。

ａ）始点からの移動方向が後進かつ終点到達時の移動方向が前進
ｂ）始点からの移動方向が前進かつ終点到達時の移動方向が後進
（ステップ５０２）。

よって、入口点１１の方向が後進で出口点１４の方向が前進であるか、入口点１１の方向が前進で出口点１４の方向が後進である場合には、スイッチバック点１２が必要であると判断し（ステップ５０２の判断ＹＥＳ）、スイッチバック点１２を１箇所挿入する処理を行う。このステップ５０３の処理は、具体的には、図１１に示す「スイッチバック点の挿入フロー」にて行われる。

「スイッチバック点の挿入フロー」では、同図１１に示すように、まず、上述の図９のステップ４０１と同様に、挿入するスイッチバック点１２の初期値設定が行われる。ただし、挿入するスイッチバック点１２は、１箇所である。スイッチバック点１２の１箇所について初期値が定められる（ステップ６０１）。

つぎに「ループ２」の処理を開始して（ステップ６０２）、ステップ６０３〜６０６の処理を終了条件に至るまで繰り返す（ステップ６０７）。「ループ２」の終了条件は、ループの回数が上限に達した場合、あるいは所定の収束条件を満足した場合などである。

「ループ２」の処理が開始されると、まず「ループ３」の処理が開始され（ステップ６０３）、各経路要素毎に、後述する「経路要素の最適化」の処理が行われる（ステップ６０４）。全ての経路要素について「経路要素の最適化」処理が行われると、「ループ３」の処理を終えて（ステップ６０５）、つぎのステップ６０６に移行する。

「ループ３」で処理が行われる「経路要素」は、つぎのように定義される。

すなわち、
１） 入口点１１から最初のスイッチバック点１２に至る経路
２） ｎ番目のスイッチバック点１２から（ｎ＋１）番目のスイッチバック点に至る経路
３） 出口点１４の手前の最後のスイッチバック点１２から出口点１４に至る経路
について「ループ３」の処理が行われる。スイッチバック点１２が無い状態からスイッチバック点１２を１箇所挿入した場合には、経路要素は、「走行経路１０の入口点１１から最初のスイッチバック点１２に至る経路」、「出口点１４の手前の最後のスイッチバック点１２から出口点１４に至る経路」となる。

ステップ６０４の「経路要素の最適化」の処理は、具体的には図１２に示す「経路要素の最適化フロー」にて行われる。

「経路要素の最適化フロー」では、同図１２に示すように、まず、上述の図８のステップ３０２と同様にして、経路要素についての可視グラフが生成される。つまり「走行経路１０の入口点１１から最初のスイッチバック点１２に至る経路」、「出口点１４の手前の最後のスイッチバック点１２から出口点１４に至る経路」についての可視グラフが生成される（ステップ７０１）。

つぎに、上述の図８のステップ３０３と同様にして、改良ダイクストラ法を適用して可視グラフ上から最適となる経路候補の辺（補間点候補）が探索される（ステップ７０２）。

つぎに、上述の補間点候補を通過するようにＢ−スプライン曲線を用いて補間が行われる（ステップ７０３）。

つぎに、「ループ６」の処理が開始され（ステップ７０４）、ループ回数が上限に達するまで「ループ６」の処理が繰り返される（ステップ７０８）。

「ループ６」の処理が開始されると（ステップ７０４）、上述の図８のステップ３０５、３０６、３０７、３０８と同様にして、補間点の位置を補正するための補正量や、補間点を通過する曲線形状を補正するための補正量が計算され（ステップ７０５）、補間点の位置を補正する処理、補間点を通過する曲線形状を補正する処理が行われる（ステップ７０６）。

補正処理後、所定条件下で収束すると（ステップ７０７の判断ＹＥＳ）、「ループ６」の処理を抜けて、この図１２の「経路要素の最適化フロー」の全処理を終える。またループ回数が上限に達するまで「ループ６」の処理が繰り返されると（ステップ７０８）、この図１２の「経路要素の最適化フロー」の全処理を終える。

図１２に示す「経路要素の最適化フロー」の代わりに、図３８に示す「経路要素の最適化フロー」を実行してもよい。図３８において、図１２と同じ処理には、同じステップ番号（７０１〜７０８）を付している。図３８に示すフローにおいては、上述のステップ７０２の処理を行った結果、最適となる経路候補の辺を探索することができた場合、つまり経路が存在する場合には（ステップ７０９の判断ＹＥＳ）、ステップ７０３に進み以後図１２に示すのと同様の処理が行われるが、最適となる経路候補の辺を探索することできなかった場合、つまり経路が存在しない場合には（ステップ７０９の判断ＮＯ）、経路生成が致命的に失敗に終わったとして処理を終える（ステップ７１０）。

こうして図１１のステップ６０４に示される「経路要素の最適化」の処理が、全ての経路要素、つまり「走行経路１０の入口点１１から最初のスイッチバック点１２に至る経路」、「出口点１４の手前の最後のスイッチバック点１２から出口点１４に至る経路」について終了すると（ステップ６０５）、この部分経路上のスイッチバック点１２を補正する処理が行われる（ステップ６０６）。このステップ６０６におけるスイッチバック点１２の補正処理は、上述の図９に示す「スイッチバック点最適化アルゴリズム」にて行われる。

図９に示す「スイッチバック点最適化アルゴリズム」は、前述の終了条件（ステップ４０９）を満たすまで行われる。そして、「ループ２」の終了条件を満足すると（ステップ６０７）、この図１１に示す「スイッチバック点の挿入フロー」の全処理を終える。

以下、手順は、図１０に示すステップ５０４に移行して、「ループ１」の処理を開始する（ステップ５０４）。また、ステップ５０２でスイッチバックが必要ないと判断した場合にも（ステップ５０２の判断ＮＯ）、ステップ５０４に移行して、「ループ１」の処理を開始する（ステップ５０４）。

「ループ１」の処理が開始されると、各経路要素毎に、後述する「経路要素間の経路生成」処理が行われる（ステップ５０５）。この「ループ１」の処理が行われる経路要素は、つぎのように定義される。すなわち、
Ａ）スイッチバックがあるときは、
Ａ−１）入口点１１からスイッチバック点１２に至る経路
Ａ−２）スイッチバック点１２から走行経路１０の出口点１４に至る経路
Ｂ）スイッチバックがないときは、
Ｂ−１）入口点１１から出口点１４に至る経路
経路について「ループ１」の処理が行われる。

つぎに、経路要素毎にその経路の生成が成功したか否かが判断される（ステップ５０６）。全ての経路要素について「経路要素間の経路生成」処理が行われ（ステップ５０５）、経路生成が成功したと判断されると（ステップ５０６の判断ＹＥＳ）、「ループ１」の処理を終える（ステップ５０７）。これにより最終的に無人車両２０が走行すべき走行経路１０が取得され、この図１０に示す経路生成フローの全処理を終える（ステップ５０８）。ステップ５０６で経路生成が失敗したと判断されると（ステップ５０６の判断ＮＯ）、「ループ１」の処理を抜ける。これにより最終的に無人車両２０が走行すべき走行経路１０が取得されなかったとされ、この図１０に示す経路生成フローの全処理を終える（ステップ５０９）。

ステップ５０５における「経路要素間の経路生成」処理は、具体的には図１３に示す「経路要素間の経路生成フロー」にて行われる。

この「経路要素間の経路生成フロー」では、経路要素毎に、その経路要素の途中にスイッチバック点１２の挿入が必要かが判断され、必要に応じてスイッチバック点１２が挿入される。

すなわち、図１３の「経路要素間の経路生成フロー」では、まず、経路要素について、上述の図１２に示す「経路要素の最適化フロー」が実行される（ステップ８０１）。

つぎに、スイッチバック点１２が必要か否かを判断する。この場合の判断基準は、上述の（スイッチバックの必要性判定）の各判定基準のうち、以下の判断基準により行なう。

ｃ）生成された経路が車両拘束条件を満足しない場合、たとえば最小旋回半径を満足しない場合である。

ｄ）生成された経路が幾何学的拘束条件を満足しない場合、たとえば障害物９に干渉するおそれがある場合である。

ｅ）スイッチバック点を導入した方がコスト関数の関数値が小さくなる場合。つまり最適化のためにスイッチバックを挿入した場合と、スイッチバックを挿入しない場合とを対比して、スイッチバックを挿入した方がコスト関数の関数値が小さくなる場合にスイッチバックの必要性があると判断する（ステップ８０２）。

この結果、上記判断基準を満たしている場合には、スイッチバック点１２を挿入すると判断し（ステップ８０３の判断ＹＥＳ）、ステップ８０４に移行する。一方、上記判断基準を満たしていない場合には、スイッチバック点１２を挿入しないと判断し（ステップ８０３の判断ＮＯ）、ステップ８０９に移行する。

スイッチバック点１２を挿入しないとしてステップ８０９に移行されると、経路生成の成否が判定される（ステップ８０９）。この結果、経路の生成が成功したと判断されると（ステップ８１０の判断ＹＥＳ）、経路生成が成功したとして、この図１３に示す「経路要素間の経路生成フロー」の全処理を終える（ステップ８１１）。一方、経路の生成が失敗したと判断されると（ステップ８１０の判断ＮＯ）、経路生成が失敗したとして、この図１３に示す「経路要素間の経路生成フロー」の全処理を終える（ステップ８１２）。

スイッチバック点１２を挿入するとしてステップ８０４に移行されると、スイッチバック点１２を偶数箇所挿入する処理が行われる。これは、経路要素の始点と終点の向きを変えないようにするためである。スイッチバック点１２を２点若しくは４点あるいは場合によっては６点以上の偶数箇所挿入すれば、経路要素の始点と終点の向きをスイッチバック点１２挿入前と同じ向きにすることができる。

このスイッチバック点１２を偶数箇所挿入する処理は、上述の図１１に示す「スイッチバック点の挿入フロー」にて行われる。ただし、上述の図１１に示す説明では、スイッチバック点１２が一点挿入されるものとして説明したが、これを「スイッチバック点１２が偶数箇所挿入されたもの」として読み替えて同様の処理を行う（ステップ８０４）。

図１１に示す「スイッチバック点の挿入フロー」の全処理を終えると、手順は、ステップ８０５に移行して、「ループ４」の処理を開始する（ステップ８０５）。

「ループ４」の処理が開始されると、各経路要素毎に、この図１３に示す「経路要素間の経路生成」処理が行われる（ステップ８０６）。この「ループ４」の処理が行われる経路要素は、つぎのように定義される。

すなわち、
１） 入口点１１から最初のスイッチバック点１２に至る経路
２） ｎ番目のスイッチバック点１２から（ｎ＋１）番目のスイッチバック点に至る経路
３） 出口点１４の手前の最後のスイッチバック点１２から出口点１４に至る経路
について「ループ４」の処理が行われる。

つぎに、経路要素毎にその経路の生成が成功したか否かが判断される（ステップ８０７）。全ての経路要素について「経路要素間の経路生成」処理が行われ（ステップ８０６）、経路生成が成功したと判断されると（ステップ８０７の判断ＹＥＳ）、「ループ４」の処理を終える。そして、経路生成が成功したとして、この図１３に示す「経路要素間の経路生成フロー」の全処理を終える（ステップ８１１）。一方、経路の生成が失敗したと判断されると（ステップ８０７の判断ＮＯ）、「ループ４」を抜け、経路生成が失敗したとして、この図１３に示す「経路要素間の経路生成フロー」の全処理を終える（ステップ８１２）。

図１３に示す「経路要素間の経路生成フロー」の代わりに、図３９に示す「経路要素間の経路生成フロー」を実行してもよい。図３９において、図１３と同じ処理には、同じステップ番号（８０１、８０３〜８１２）を付している。

図１３に示すフローにおいては、上述のステップ８０２において、スイッチバック挿入前に予め、スイッチバックを挿入した場合の経路のコストと、スイッチバックを挿入しない場合の経路のコストを対比して、スイッチバックを挿入した方がコスト関数の関数値が小さくなる場合にスイッチバックの必要性があると判断し、その判断結果に従いスイッチバック点を挿入した経路を生成するようにしている。

しかし、図３９に示すフローにおいては、スイッチバックを挿入する前の経路のコストと、実際にスイッチバックを挿入した場合の経路のコストを対比して、実際にスイッチバックを挿入した場合の経路のコストが、スイッチバックを挿入する前の経路のコストよりも低ければ、スイッチバックを挿入した経路を採用するが、実際にスイッチバックを挿入した場合の経路のコストが、スイッチバックを挿入する前の経路のコストよりも高くなれば、スイッチバックを挿入する前の経路に戻し、このスイッチバックを挿入する前の経路を採用するようにする。

以下、図１３と異なるステップ８０２´、追加したステップ８１３、８１４、８１５における各処理内容について説明する。

（ステップ８０２´における処理）
図１３のステップ８０２の代わりにステップ８０２´の処理が行われる。

ステップ８０２´においては、スイッチバック点１２が必要か否かの判断と、スイッチバック点１２を挿入する前の経路のコストを評価する処理が行われる。

まず、スイッチバック点１２が必要であるとの判断基準は、以下の判断基準により行なう。

ｆ）新たなスイッチバック点１２の挿入によって、追加可能なスイッチバック数の上限値に達しないこと。

ｇ）可視グラフ上での経路が存在しないなどの致命的な経路生成失敗になっていないこと。

ｈ）スイッチバック点１２の挿入によってコスト関数の関数値が小さくなる可能性があること。たとえば致命的でない経路生成失敗になっていること、経路上に旋回角１８０°を超える大きなカーブがある場合などである。

そして、スイッチバック点１２を挿入する前の経路のコスト関数の関数値が求められる。つまりスイッチバック点挿入前のコスト評価が行われる。

（ステップ８１３における処理）
ステップ８１３においては、実際にスイッチバック点１２を挿入した後の経路のコスト関数の関数値が求められる。つまりスイッチバック点挿入後のコスト評価が行われる。

（ステップ８１４および８１５における処理）
ステップ８１４においては、スイッチバック点挿入前のコスト評価で得られたコスト関数の関数値と、スイッチバック点挿入後のコスト評価で得られるコスト関数の関数値とが対比される。この判断の結果、実際にスイッチバック点１２を挿入した後の経路のコストが、スイッチバック点１２を挿入する前の経路のコストよりも低ければ（ステップ８１４の判断ＹＥＳ）、スイッチバック点１２を挿入した後の経路を採用し、経路生成が成功したとするが（ステップ８１１）、実際にスイッチバック点１２を挿入した後の経路のコストが、スイッチバック点１２を挿入する前の経路のコストよりも高くなれば（ステップ８１４の判断ＮＯ）、スイッチバック点１２を挿入する前の経路に戻し（ステップ８１５）、このスイッチバック点１２を挿入する前の経路を採用し、経路生成が成功したとする（ステップ８１１）。

以上のようにして図１０に示す経路生成フローの処理が行われる。

つぎに、「スイッチバック点の追加に伴いスイッチバック点の最適化をやり直す方法」について説明する。

図１４は、「スイッチバック点の追加に伴いスイッチバック点の最適化をやり直す方法」を示している。

（スイッチバック点の追加に伴いスイッチバック点の最適化をやり直す方法）
同図１４に示すように、ステップ９０１〜９０３では、図１０のステップ５０１〜５０３と同様の処理が行われる。

つぎに、「ループ１」に入り（ステップ９０４）、ステップ９０５〜９０８の処理が繰り返し行なわれる。

ステップ９０５では、図１５に示す「経路の最適化フロー」が実行される。

図１５に示す「経路の最適化フロー」では、まず「ループ２」に入り（ステップ１００１）、「ループ２」の処理が終了条件を満足するまで繰り返し行なわれる。この終了条件は、ループ回数が上限値に達したか、所定の収束条件を満足したかなどである（ステップ１００６）。

「ループ２」に入ると、「ループ３」に入り（ステップ１００２）、各構成要素毎に、上述の図１２に示す「経路要素の最適化フロー」が行われる。この「ループ３」の処理が行われる経路要素は、つぎのように定義される。すなわち、
Ａ）スイッチバックがあるときは、
Ａ−１）入口点１１からスイッチバック点１２に至る経路
Ａ−２）スイッチバック点１２から走行経路１０の出口点１４に至る経路
Ｂ）スイッチバックがないときは、
Ｂ−１）入口点１１から出口点１４に至る経路
経路について「ループ３」の処理が行われる。

全ての経路要素について「経路要素の最適化」処理が行われると、「ループ３」の処理を終える（ステップ１００４）。

つぎに、全てのスイッチバック点１２について、その位置、方向を補正する処理が行われる（ステップ１００５）。

このステップ１００５における全てのスイッチバック点１２の補正処理は、上述の図９に示す「スイッチバック点最適化アルゴリズム」にて行われる。

図９に示す「スイッチバック点最適化アルゴリズム」は、前述の終了条件（ステップ４０９）を満たすまで行われる。そして、「ループ２」の終了条件を満足すると（ステップ１００６）、この図１５に示す「経路の最適化フロー」の全処理を終える。

手順は、図１４のステップ９０６に戻り、スイッチバック点１２が必要か否かを判断する。この場合の判断基準は、上述の（スイッチバックの必要性判定）の各判定基準のうち、以下の判断基準により行なう。

ｃ）生成された経路が車両拘束条件を満足しない場合、たとえば最小旋回半径を満足しない場合である。

ｄ）生成された経路が幾何学的拘束条件を満足しない場合、たとえば障害物９に干渉するおそれがある場合である。

ｅ）スイッチバック点を導入した方がコスト関数の関数値が小さくなる場合。つまり最適化のためにスイッチバックを挿入した場合と、スイッチバックを挿入しない場合とを対比して、スイッチバックを挿入した方がコスト関数の関数値が小さくなる場合にスイッチバックの必要性があると判断する（ステップ９０６）。

この結果、上記判断基準を満たしている場合には、スイッチバック点１２を挿入すると判断し（ステップ９０７の判断ＹＥＳ）、ステップ９０８に移行する。一方、上記判断基準を満たしていない場合には、スイッチバック点１２を挿入しないと判断し（ステップ９０７の判断ＮＯ）、「ループ１」を抜け、この図１４に示す「経路生成フロー」の全処理を終える。

スイッチバック点１２を挿入するとしてステップ９０８に移行されると、スイッチバック点１２を偶数箇所挿入する処理が行われる。

このスイッチバック点１２を偶数箇所挿入する処理は、上述の図１１に示す「スイッチバック点の挿入フロー」にて行われる。ただし、上述の図１１に示す説明では、スイッチバック点１２が一点挿入されるものとして説明したが、これを「スイッチバック点１２が偶数箇所挿入されたもの」として読み替えて同様の処理を行う（ステップ９０８）。

「ループ１」の終了条件が満足されると、この図１４に示す「経路生成フロー」の全処理を終える。

以下、上述の実施例で生成された走行経路１０の各経路生成例を例示する。

（経路生成例１）
図３３（ａ）は、経路生成例１で与えられる幾何学的条件であり、８ａが経路生成範囲８の外形線、９ａが障害物９の外形線を示している。入口点１１、出口点１４、停止点１３の各点における方向を矢印にて示している。経路生成例１における停止点１３は、排土を行う場所であり、無人車両２０は後進状態で停止点１３へ到達しなければならない。

図３３（ｂ）は、経路生成範囲８の外形線８ａ、障害物９の外形線９ａと円盤とのミンコフスキー和をとったときの経路生成範囲８の外形線８ｂ、障害物９の外形線９ｂを示す。

図３３（ｃ）は、入口点１１を原点とする走行距離を横軸として、走行経路１０の曲率の分布と最小旋回半径による制限値を示す。図３３（ｃ）に示すように、入口点１１からスイッチバック点１２、スイッチバック点１２から停止点１３、停止点１３から出口点１４までは、それぞれ別の曲線で表される。このため、曲線の接続点では、曲率は連続にならない。無人車両２０が静止している点においてステアリング操作は理論上は可能であるが、実際には、地面の状態や積み荷重などにより多大な操作ができない場合があることや、タイヤへの負荷が大きくなることから、望ましくない。しかし、式（３．１６）に示すコスト関数Ｅの始点と終点付近で重みを大きくすることや、スイッチバック点算出のためのコスト関数Ｅ（（３．２５）式）においてスイッチバック点１２前後の曲率変化を小さくするための関数を用いていることなどから、大きな差はなく、十分に、無人車両２０を最小旋回半径以上の旋回半径で走行させることが可能である。

図３３（ｄ）は、入口点１１を原点とする走行距離を横軸として、理論最大速度Ｖthを示す。図３３（ｄ）において、Ｖgは、走行経路１０の曲率半径ρによる制限であり、
Ｖg＝√（αρ）
により求められる。αは最大許容後進加速度である。また、図３３（ｄ）において、Ｖstは、ステアリング操作量による制限であり、
Ｖst＝β／γ
により求められる。βは、単位時間当たりの最大ステアリング操作量、γは、単位距離当たりのステアリング操作量である。また、図３３（ｄ）において、Ｖmaxは、無人車両２０の最大前（後）進速度である。これらの制限の下、最大加（減）速度による加減速のみで理論最大速度Ｖthが求められる。理論最大速度Ｖthは、旋回半径やステアリング操作量の影響を受けない速度が望ましい。すなわち、旋回半径が大きく、単位距離当たりのステアリング操作量が少ない走行経路１０が望ましい。本実施形態では、これら旋回半径、単位距離当たりのステアリング操作量をコスト要素としてコスト最小となるように走行経路１０を求めるようにしたので、図３３（ｄ）に示すように妥当な理論最大速度Ｖthが得られた。

（経路生成例２）
図３４（ａ）、（ｂ）、（ｃ）、（ｄ）は経路生成例２を示す図であり、上述の経路生成例１の図３３（ａ）、（ｂ）、（ｃ）、（ｄ）に対応する図である。

経路生成例２では、無人車両２０は停止点１３に後進状態で入り、積み込みを行う。本来、スイッチバック点１２は、停止点１３に近く後進距離が短いことが望ましい。しかし、この経路生成例２では、停止点１３付近に障害物９があり、その条件では車両拘束条件を満たすスイッチバック点１２が存在しない。本実施形態では、ＳＡ（大域探索法）を適用しているため、図３４（ｂ）に示すように最適なスイッチバック点１２の探索を行うことができる。

（経路生成例３）
図３５（ａ）、（ｂ）、（ｃ）、（ｄ）は経路生成例３を示す図であり、上述の経路生成例１の図３３（ａ）、（ｂ）、（ｃ）、（ｄ）に対応する図である。

経路生成例３では、無人車両２０は停止点１３に後進状態で入り排土を行う。経路生成例３の経路生成範囲８には、多数の障害物９が存在するが、これら多数の障害物９を回避した最適な走行経路１０を生成することができた。

（経路生成例４）
図３６（ａ）、（ｂ）、（ｃ）、（ｄ）は経路生成例４を示す図であり、上述の経路生成例１の図３３（ａ）、（ｂ）、（ｃ）、（ｄ）に対応する図である。

経路生成例４では、無人車両２０は停止点１３に後進状態で入り排土を行う。経路生成例４では、無人車両２０の最小旋回半径を１２ｍとした場合を示している。図３６（ｂ）に示すように、走行経路１０の全行程を通してスイッチバック点１２は一点で済むものの、出口点１４に至る経路要素１０ａにおいては、曲率は最小旋回半径１２ｍに応じた大きな値となる。

（経路生成例５）
図３７（ａ）、（ｂ）、（ｃ）、（ｄ）は経路生成例５を示す図であり、上述の経路生成例１の図３３（ａ）、（ｂ）、（ｃ）、（ｄ）に対応する図である。

経路生成例５の幾何学的条件（図３７（ａ））は、経路生成例４の幾何学的条件（図３６ （ａ））と同一である。

地面のぬかるみや無人車両２０の積載量により車両拘束条件は変わる。経路生成例５では、無人車両２０の最小旋回半径を２０ｍとし、経路生成例４の最小旋回半径１２ｍよりも大きくした場合を示している。図３７（ｂ）に示すように、出口点１４に至る経路要素１０ａにおいてその曲率は経路生成例４のものよりも小さくなっているのがわかる。ただし、走行経路１０の全行程を通してスイッチバック点１２は三点必要となる。

このように本実施形態によれば、車両拘束条件の変更に対して柔軟かつ的確に走行経路１０を生成することができる。

以上説明した実施形態では、図５（ｄ）のステップ２３１の補間点探索ステップ、つまり無人車両２０が走行すべき走行経路１０の各補間点を探索するステップを、図８に示すように、
ミンコフスキー和を計算するステップ（ステップ３０１）と、
可視グラフを生成するステップ（ステップ３０２）と、
改良ダイクストラ法により最適となる経路候補の辺（補間点候補）を探索するステップ（ステップ３０３）と
を含むステップで行うものとして説明した。

また、図５（ｄ）のステップ２３２の補間ステップ、つまり入口点１１から各補間点を経由して出口点１４に至る経路の各補間点を補間するステップを、図８に示すように、
Ｂ−スプライン曲線によリ補間するステップ（ステップ３０４）
を含むステップで行うものとして説明した。

しかし、これはあくまでも一例であり、他の探索方法を用いて補間点を探索したり、他の補間手法により補間点を補間してもよい。たとえばクロソイド曲線を用いて補間点を補間してもよく、３次関数曲線を用いて補間点を補間してもよく、サイン半波長逓減曲線を用いて補間点を補間してもよい。

また、実施形態では、補間点探索ステップにおいて補間点を探索する際および補間ステップにおいて補間を行う際に、コスト関数の関数値が最小になるように処理を行うようにしている。すなわち、図８のステップ３０３では、コスト関数の関数値が最小になるように、経路候補の辺の組み合わせ（補間点の候補）を探索しており、図８のステップ３０７では、コスト関数の関数値が最小となるように方向ベクトルα、βの大きさを調整して補間すべき曲線の曲率を最適なものとしている。しかし、本発明としては、補間点探索ステップにおいて補間点を探索する際のみにコスト関数の関数値が最小になるように処理を行ってもよく、補間ステップにおいて補間を行う際にのみにコスト関数の関数値が最小になるように処理を行ってもよい。

図１は、無人車両走行システムの全体構成を示すとともに、作業現場の全体を示した図である。 図２は、実施形態の走行経路が生成される経路生成範囲の様子を例示した図である。 図３は、実施形態の無人車両走行システムのブロック図である。 図４（ａ）、（ｂ）、（ｃ）は、無人車両と管制装置の間で行われる処理の手順を示すフローチャートである。 本発明の各方法を説明するために用いたフローチャートである。 図６（ａ）は、無人車両のサイズ、走行、操縦等の限界としての拘束条件を示した表であり、図６（ｂ）は、無人車両の上面図であり、無人車両の各サイズを示した図であり、図６（ｃ）は、無人車両２０が旋回する様子を示した上面図である。 図７（ａ）は、無人車両がステアリングを切る様子を示した上面図であり、図７（ｂ）は、無人車両が走行する領域の拘束条件である幾何学的拘束条件を示した図である。 図８は、基本的経路の生成方法の処理手順を示したフローチャートである。 図９は、シミュレーテッド アニーリング（ＳＡ）法を用いてスイッチバック点の最適化を行うアルゴリズムを示すフローチャートである。 図１０は、「一度生成したスイッチバック点を固定する方法」を説明するための経路生成フローである。 図１１は、スイッチバック点の挿入フローである。 図１２は、経路要素の最適化フローである。 図１３は、経路要素間の経路生成フローである。 図１４は、「スイッチバック点の追加に伴いスイッチバック点の最適化をやり直す方法」を説明するための経路生成フローである。 図１５は、経路の最適化フローである。 図１６（ａ）、（ｂ）は、ミンコフスキー和の計算を説明するための図である。 図１７は、ミンコフスキー和された経路生成範囲と障害物の外形線に対して引かれた可視グラフを例示した図である。 図１８は、接線か否かを判定する方法を説明する図である。 図１９は、図１７に示す可視グラフから接線にならない辺を取り除いたグラフを示した図である。 図２０は、始点から終点までの最短距離となる経路を示した図である。 図２２はコスト関数を説明するために用いた図である。 図２２はコスト関数を説明するために用いた図である。 図２３は、改良ダイクストラ法を適用した結果を示す図である。 図２４は、補間したＢ−スプライン曲線を例示した図である。 図２５（ａ）、（ｂ）は、補間点を修正する前と後の経路を示した図である。 図２６は、補間点同士を結ぶ曲線の形状が変化する様子を示した図である。 、図２７は、最適補間の手法を説明するために用いた図である。 、図２８は、最適補間の計算を効率的に行うための方法を説明するために用いた図である。 図２９（ａ）、（ｂ）は、無人車両の最小旋回半径を満足しないときにスイッチバックを導入する場合を説明する図である。 図３０は、スイッチバック点を一点設ける場合の初期設定の方法を説明する図である。 図３１（ａ）、（ｂ）は、スイッチバック点を二点設ける場合の初期設定の方法を説明する図である。 図３２は、クーリングスケジュールを例示する図である。 図３３（ａ）、（ｂ）、（ｃ）、（ｄ）は、経路生成例１を説明するために用いた図である。 図３４（ａ）、（ｂ）、（ｃ）、（ｄ）は、経路生成例２を説明するために用いた図である。 図３５（ａ）、（ｂ）、（ｃ）、（ｄ）は、経路生成例３を説明するために用いた図である。 図３６（ａ）、（ｂ）、（ｃ）、（ｄ）は、経路生成例４を説明するために用いた図である。 図３７（ａ）、（ｂ）、（ｃ）、（ｄ）は、経路生成例５を説明するために用いた図である。 図３８は、図１２に示す経路要素の最適化フローに代わる経路要素の最適化フローである。 図３９は、図１３に示す経路要素間の経路生成フローに代わる経路要素間の経路生成フローである。 図４０は、コスト関数の定義を説明するために用いた図である。 図４１は、コスト関数の定義を説明するために用いた図である。 図４２は、コスト関数の定義を説明するために用いた図である。

符号の説明

１０ 走行経路 ２０ 無人車両

Claims (5)

1. 無人車両の車幅、最小旋回半径を含む車両拘束条件を入力するとともに、無人車両が走行すべき走行経路の生成範囲、回避すべき障害物、入口点および出口点の位置と方向を含む幾何学的拘束条件を入力する条件入力ステップと、
   車両拘束条件および幾何学的拘束条件を満足し、かつ
   少なくとも、前記走行経路の曲率を小さくすることで前記無人車両の旋回半径を小さくするための第１項と、前記走行経路の曲率変化率を小さくすることで単位距離当たりステアリング操作量を小さくするための第２項と、前記走行経路の走行距離を短くするための第３項とからなるコスト関数であって、
   前記第１項は、前記走行経路の微小区間についての前記走行経路の曲率あるいは前記無人車両のステアリング角を、前記走行経路に沿って積分して求められ、
   前記第２項は、前記走行経路の微小区間についての前記走行経路の曲率変化率あるいは前記無人車両のステアリング操作量を、前記走行経路に沿って積分して求められ、
   前記第３項は、前記走行経路の微小区間についての前記走行経路の移動距離を、前記走行経路に沿って積分して求められるコスト関数
   の関数値が最小になるように、走行経路を生成する走行経路生成ステップと
   を含むことを特徴とする無人車両の走行経路生成方法。
2. 前記コスト関数には、前記走行経路上のスイッチバック点におけるすえ切りコストを小さくするための第４項が追加され、
   前記第４項は、前記スイッチバック点前後の経路の微小区間についての前記無人車両のステアリング操作量あるいは前記走行経路の曲率変化率を、前記スイッチバック点前後の経路に沿って積分して求められるものであり、
   前記走行経路生成ステップは、
   前記スイッチバック点を導入して、無人車両の走行経路を生成するステップを含み、
   前記スイッチバック点は、前記コスト関数の関数値が最小になるように定められること
   を特徴とする請求項１記載の無人車両の走行経路生成方法。
3. 走行経路生成ステップは、
   生成された走行経路が車両拘束条件または/および幾何学的拘束条件を満足していないか、あるいはスイッチバック点を導入した方がコスト関数の関数値が小さくなることを判断するステップと、
   生成された走行経路が車両拘束条件または/および幾何学的拘束条件を満足しないと判断された場合あるいはスイッチバック点を導入した方がコスト関数の関数値が小さくなると判断された場合に、スイッチバック点を導入して、無人車両の走行経路を生成するステップとを含み、
   スイッチバック点は、コスト関数の関数値が最小になるように定められること
   を特徴とする請求項１記載の無人車両の走行経路生成方法。
4. 前記走行経路生成ステップは、
   無人車両が走行すべき走行経路の各補間点を探索する補間点探索ステップと、
   入口点から各補間点を経由して出口点に至る経路の各補間点を補間する補間ステップと
   を含み、
   補間点探索ステップにおける探索または/および補間ステップにおける補間は、コスト関数の関数値が最小になるように行うこと
   を特徴とする請求項１記載の無人車両の走行経路生成方法。
5. 補間ステップにおける補間は、Ｂ−スプライン曲線を用いて行われること
   を特徴とする請求項４記載の無人車両の走行経路生成方法。

Images