JP2017010292A - Ａｇｖ軌道計算装置及び方法 - Google Patents

Abstract

【課題】自動搬送車を最適な軌道で目標位置姿勢にすることを可能とする。【解決手段】自動搬送車の軌道計算を行い、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算装置であって、目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、自動搬送車の軌道計算を行う、ＡＧＶ軌道計算装置を用いることで、自動搬送車を最適な軌道で目標位置姿勢にすることを可能とする。【選択図】図１

Description

本発明は、自動搬送車（以下、ＡＧＶ）を特定の位置及び姿勢（角度）（以下、位置姿勢）に移動させるためのＡＧＶの軌道を求める、ＡＧＶ軌道計算装置及び方法に関する。

これまでのＡＧＶは、磁気テープを利用した移動がメインだったが、このようにすると磁気テープ上しか移動できないため、自律的に軌道を計算する手法が望まれている。

下記非特許文献１では、車両型ロボットの軌道計算として、位置情報だけでなく姿勢情報を考慮した（ｘ，ｙ，θ）の３次元空間を状態空間（コンフィギュレーション空間）として用いて、位置だけでなく姿勢も考慮した軌道計画を行っている。また、クロソイド曲線を導入することで、ロボットの操舵角が急激に変化しないよう考慮している。

下記特許文献１では、駐車支援のために、動作開始位置、動作開始姿勢（以下、開始位置姿勢）、停止位置、停止姿勢が定まった際に、１度の切り返しで駐車が可能か判断する手法、及び、１度の切り返しで駐車が可能であった場合の軌道計算を行う手法が提案されている。軌道計算手法については、最小回転半径に基づいた単一パラメータで制御されるクロソイド曲線とその前後の直線の組み合わせを用いている。

下記特許文献２では、道路形状をクロソイド曲線のフィッティングにより算出する手法が提案されている。クロソイド曲線の算出には道路の半径といった道路形状から統計的に近似するクロソイド曲線のパラメータを求める参照テーブルを用いた。

下記特許文献３では、位置情報と姿勢情報の３次元の状態空間（コンフィギュレーション空間）を使用して軌道計画を行っている。ポテンシャル法と呼ばれる手法を用いて、障害物を回避しながら到達目標位置及び到達目標姿勢（以下、目標位置姿勢）に到達する軌道を算出することができる。

下記特許文献４では、軌道を直線と曲線（ベジエ曲線、円弧、クロソイド曲線）の組み合わせで決定する搬送ロボットを考案している。

特開２０１０‐１８０７４号公報 特開２００５‐２１４８３９号公報 特開２０１１‐８９３５４号公報 特許第４８７７５２０号公報

項警宇，田崎勇一，鈴木達也，「車両型移動ロボットの走行安全性制約に基づく可変分解能状態グラフ地図生成」，第５５回自動制御連合講演会，Ｎｏｖ１７‐１８，２０１２ 京都

上記非特許文献１の方法では、コンフィギュレーション空間を使って位置と姿勢の両方を考慮するだけでなく、滑らかな操舵角の変化に対応するためにクロソイド曲線を使用した軌道計算ができるが、開始位置姿勢及び目標位置姿勢によってその都度軌道計算が可能か算出する必要があるために計算コストが高く、また、最小回転半径を満たすクロソイド曲線で軌道計算を行うため、実現可能な軌道ではあるものの、なだらかに曲がればいいシーンであっても操舵角の変化における回転加速度の高い曲がり方（以下、負荷の高い曲がり方）をする可能性があるという問題がある。

上記特許文献１の方法においても、上記特許文献１と同様の問題があり、単一パラメータのクロソイド曲線しか扱っていないため、負荷の高い曲がり方をしてしまい、また、開始位置姿勢及び目標位置姿勢によってその都度軌道計算が可能か算出する必要があるために計算コストが高く、さらに、実現可能な開始位置姿勢と目標位置姿勢の組み合わせが少ないという問題がある。

上記特許文献２の方法では、様々な操舵角の回転加速度を持ったクロソイド曲線を算出することが可能であり、上記特許文献１，２のような問題は起こらないが、もともとクロソイド曲線形状をした道路の情報を用いてクロソイド曲線を近似する手法であり、そういった事前情報がない場合には対応できない。また、近似した曲線を用いるため特定の位置姿勢にという要求を満たすことが困難である。

上記特許文献３の方法では、コンフィギュレーション空間の考え方を利用しているが、軌道計算はクロソイド曲線を用いておらず、また軌道の回転半径も場合によって大きく変わるため、負荷の高い曲がり方も許容してしまっている。

上記特許文献４の方法では、直線と曲線の組み合わせ方の詳細について考慮されておらず、最適な軌道を計算する方法が不明である。

本発明は、上述のような技術的状況に鑑みてなされたものであり、ＡＧＶを最適な軌道で目標位置姿勢にすることを可能とする、ＡＧＶ軌道計算装置及び方法を提供することを目的とする。

上記課題を解決する第１の発明に係るＡＧＶ軌道計算装置は、
自動搬送車の軌道計算を行い、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算装置であって、
前記目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、前記自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、前記自動搬送車の軌道計算を行う
ことを特徴とする。

上記課題を解決する第２の発明に係るＡＧＶ軌道計算装置は、
上記第１の発明に係るＡＧＶ軌道計算装置において、
予め、前記自動搬送車の様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さを前記軌道の参照テーブルとして保存しておき、
前記自動搬送車の走行時に、前記参照テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
ことを特徴とする。

上記課題を解決する第３の発明に係るＡＧＶ軌道計算装置は、
上記第２の発明に係るＡＧＶ軌道計算装置において、
前記目標位置姿勢に到達可能な前記クロソイド曲線と前記直線との組み合わせを計算し、その際使用した前記クロソイド曲線のパラメータ及び前記直線の長さを前記参照テーブルとして保存しておく
ことを特徴とする。

上記課題を解決する第４の発明に係るＡＧＶ軌道計算装置は、
上記第３の発明に係るＡＧＶ軌道計算装置において、
前記目標位置姿勢のデータを入力する入力部と、
クロソイド曲線のパラメータ及び直線の長さの範囲を設定する設定部と、
前記設定部で設定された範囲で前記目標位置姿勢を始点とした様々な軌道を算出する計算部と、
コンフィギュレーション空間に対応する前記参照テーブルを用意し、前記計算部で算出される軌道が通る前記コンフィギュレーション空間上の各点に対応する前記参照テーブルに、前記計算部の軌道算出に用いた前記クロソイド曲線のパラメータ及び前記直線の長さを登録する登録部とを備えることで、
予め前記参照テーブルを作成する
ことを特徴とする。

上記課題を解決する第５の発明に係るＡＧＶ軌道計算装置は、
上記第４の発明に係るＡＧＶ軌道計算装置において、
前記自動搬送車の走行時の位置姿勢を入力する位置姿勢入力部を備え、
前記計算部は、
所定時刻毎に、前記位置姿勢入力部で入力された前記自動搬送車の走行時の位置姿勢において、予め前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さの組み合わせが存在する場合は、軌道計算を行い、存在しない場合は、より前記目標位置姿勢の近くまで前記自動搬送車を移動させ、前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さを用いて、軌道計算を行うことで、
前記自動搬送車の走行時に、前記参照テーブルを用いた前記自動搬送車の軌道計算を行う
ことを特徴とする。

上記課題を解決する第６の発明に係るＡＧＶ軌道計算装置は、
上記第４又は５の発明に係るＡＧＶ軌道計算装置において、
前記登録部は、
前記参照テーブルに、前記クロソイド曲線のパラメータ及び前記直線の長さを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する
ことを特徴とする。

上記課題を解決する第７の発明に係るＡＧＶ軌道計算装置は、
上記第１の発明に係るＡＧＶ軌道計算装置において、
対称単位クロソイド曲線テーブルを作成し、
前記対称単位クロソイド曲線テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
ことを特徴とする。

上記課題を解決する第８の発明に係るＡＧＶ軌道計算装置は、
上記第７の発明に係るＡＧＶ軌道計算装置において、
始点位置姿勢及び目標位置姿勢を入力する入力部と、
前記対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な前記対称単位クロソイド曲線テーブルを予め求める、対称単位クロソイド曲線テーブル計算部と、
前記始点位置姿勢、前記目標位置姿勢、及び、軌道計算で必要な前記対称単位クロソイド曲線テーブルを取り出し、前記始点と前記終点との間で軌道計算を行い、複数の軌道データを求める、軌道計算部と、
前記複数の軌道データの中から軌道を選択する軌道選択部とを備える
ことを特徴とする。

上記課題を解決する第９の発明に係るＡＧＶ軌道計算装置は、
上記第８の発明に係るＡＧＶ軌道計算装置において、
前記軌道計算部は、
幾何学的に中間点を設定し、前記始点と前記中間点、及び、前記中間点と前記終点との間に、対称単位クロソイド曲線を拡大又は縮小した対称クロソイド曲線、あるいは、直線を当てはめて、軌道計算を行う
ことを特徴とする。

上記課題を解決する第１０の発明に係るＡＧＶ軌道計算装置は、
上記第８又は９の発明に係るＡＧＶ軌道計算装置において、
軌道選択部は、
予め設定された最大曲率及び軌道長に基づき、前記複数の軌道データの中から最適な軌道を選択する
ことを特徴とする。

上記課題を解決する第１１の発明に係るＡＧＶ軌道計算方法は、
自動搬送車の軌道を計算し、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算方法であって、
前記目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、前記自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、前記自動搬送車の軌道計算を行う
ことを特徴とする。

上記課題を解決する第１２の発明に係るＡＧＶ軌道計算方法は、
上記第１１の発明に係るＡＧＶ軌道計算方法において、
予め、前記自動搬送車の様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さを前記軌道の参照テーブルとして保存しておき、
前記自動搬送車の走行時に、前記参照テーブルを用いて目標位置姿勢で停止させる軌道を計算する
ことを特徴とする。

上記課題を解決する第１３の発明に係るＡＧＶ軌道計算方法は、
上記第１２の発明に係るＡＧＶ軌道計算方法において、
前記目標位置姿勢に到達可能な前記クロソイド曲線と前記直線との組み合わせを計算し、その際使用した前記クロソイド曲線のパラメータ及び前記直線の長さを前記参照テーブルとして保存しておく
ことを特徴とする。

上記課題を解決する第１４の発明に係るＡＧＶ軌道計算方法は、
上記第１３の発明に係るＡＧＶ軌道計算方法において、
前記目標位置姿勢のデータを入力し、
クロソイド曲線のパラメータ及び直線の長さの範囲を設定し、
設定された前記クロソイド曲線のパラメータ及び前記直線の長さの範囲で前記目標位置姿勢を始点とした様々な軌道を算出し、
コンフィギュレーション空間に対応する前記参照テーブルを用意し、算出された軌道が通る前記コンフィギュレーション空間上の各点に対応する前記参照テーブルに、軌道の算出に用いた前記クロソイド曲線のパラメータ及び前記直線の長さを登録することで、
予め前記参照テーブルを作成する
ことを特徴とする。

上記課題を解決する第１５の発明に係るＡＧＶ軌道計算方法は、
上記第１４の発明に係るＡＧＶ軌道計算方法において、
前記自動搬送車の走行時の位置姿勢を入力し、
所定時刻毎に、前記自動搬送車の走行時の位置姿勢において、予め登録された前記クロソイド曲線のパラメータ及び前記直線の長さの組み合わせが存在する場合は、軌道計算を行い、存在しない場合は、より前記目標位置姿勢の近くまで前記自動搬送車を移動させ、前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さを用いて、軌道計算を行うことで、
前記自動搬送車の走行時に、前記参照テーブルを用いた前記自動搬送車の軌道計算を行う
ことを特徴とする。

上記課題を解決する第１６の発明に係るＡＧＶ軌道計算方法は、
上記第１４又は１５の発明に係るＡＧＶ軌道計算方法において、
前記参照テーブルに、前記クロソイド曲線のパラメータ及び前記直線の長さを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する
ことを特徴とする。

上記課題を解決する第１７の発明に係るＡＧＶ軌道計算方法は、
上記第１１の発明に係るＡＧＶ軌道計算方法において、
対称単位クロソイド曲線テーブルを作成し、
前記対称単位クロソイド曲線テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
ことを特徴とする。

上記課題を解決する第１８の発明に係るＡＧＶ軌道計算方法は、
上記第１７の発明に係るＡＧＶ軌道計算方法において、
始点位置姿勢及び前記目標位置姿勢を入力し、
前記対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な前記対称単位クロソイド曲線テーブルを予め求め、
前記始点位置姿勢、前記目標位置姿勢、及び、軌道計算で必要な前記対称単位クロソイド曲線テーブルに基づき、前記始点と前記終点との間で軌道計算を行い、複数の軌道データを求め、
前記複数の軌道データの中から軌道を選択する
ことを特徴とする。

上記課題を解決する第１９の発明に係るＡＧＶ軌道計算方法は、
上記第１８の発明に係るＡＧＶ軌道計算方法において、
幾何学的に中間点を設定し、前記始点と前記中間点、及び、前記中間点と前記終点との間に、対称単位クロソイド曲線を拡大又は縮小した対称クロソイド曲線、あるいは、直線を当てはめて、軌道計算を行う
ことを特徴とする。

上記課題を解決する第２０の発明に係るＡＧＶ軌道計算方法は、
上記第１８又は１９の発明に係るＡＧＶ軌道計算方法において、
予め設定された最大曲率及び軌道長に基づき、前記複数の軌道データの中から最適な軌道を選択する
ことを特徴とする。

本発明に係るＡＧＶ軌道計算装置及び方法によれば、ＡＧＶを最適な軌道で目標位置姿勢にすることができる。

ＡＧＶの概略図である。（ａ）は上面図、（ｂ）は側面図である。 ｘ‐ｙ座標上に描かれたクロソイド曲線の例を示すグラフである。 コンフィギュレーション空間において事前に計算した軌道の例を示すグラフである。 ＡＧＶの位置、姿勢、操舵角、及び、タイヤ間の距離の定義を説明するグラフである。 ２点間を２つのクロソイド曲線で結んだ軌道例を示すグラフである。 本発明の実施例１に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。 本発明の実施例１に係るＡＧＶ軌道計算装置の「参照テーブル作成」についての手順を示すフローチャートである。 本発明の実施例１に係るＡＧＶ軌道計算装置の「参照テーブルのパラメータを用いた軌道計算」についての手順を示すフローチャートである。 対称クロソイド曲線のグラフである。 対称クロソイド曲線＋直線の組み合わせによる軌道を示すグラフである。 対称クロソイド曲線＋対称クロソイド曲線の組み合わせによる軌道を示すグラフである。 始点の角度が１５０度の対称クロソイド曲線のグラフである。 本発明の実施例２に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。 本発明の実施例２に係るＡＧＶ軌道計算装置による基本的な軌道計算処理の全体のフローチャートである。 接続角τまでの単位クロソイド曲線のグラフである。 対称単位クロソイド曲線の生成を説明するグラフである。 本発明の実施例２に係るＡＧＶ軌道計算装置による軌道計算を示すフローチャートである。 絶対座標から相対座標への変換を説明するグラフである。 単一の対称クロソイド曲線からなる軌道を説明するグラフである。 単一の対称クロソイド＋直線からなる軌道を説明するグラフである。 直線＋単一の対称クロソイドからなる軌道を説明するグラフである。 二つの対称クロソイド曲線からなる軌道を説明するである。 同じ始点‐終点間での複数の軌道例を示すグラフである。 本発明の実施例３に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。 本発明の実施例３に係るＡＧＶ軌道計算装置による軌道計算処理を示すフローチャートである。 本発明の実施例３に係るＡＧＶ軌道計算装置による軌道計算処理を示すフローチャートである。 始点角及び終点角が大きい場合に計算された軌道の一例を示すグラフである。 始点角及び終点角が大きい場合に計算された軌道の第１軌道〜第３軌道を示すグラフである。 始点角及び終点角が大きい場合に計算された軌道の他の例を示すグラフである。

本発明に係るＡＧＶ軌道計算装置及び方法は、ＡＧＶを「特定の位置」に「特定の姿勢」で停止させるものである。以下、本発明に係るＡＧＶ軌道計算装置及び方法を実施例にて図面を用いて説明する。

［実施例１］
本発明の実施例１に係るＡＧＶ軌道計算装置及び方法について、図１〜８を用いて説明する。

図１（ａ）（ｂ）に示すように、前輪として配される可動輪２、後輪として配される従輪３、及び、可動輪２と従輪３との間に配される操舵輪としての駆動輪４を備えているＡＧＶ１の軌道を考える際は、ＡＧＶ１の駆動輪４を操舵輪とし、可動輪２を無視した車両と同様の操舵角を持つステアリング式と考えればよい。しかしながら、通常ＡＧＶは静止状態で操舵角を変化させる機構（パワーステアリング機能）を持たないため、回転半径が急激に変化するような軌道には対応できない。

そこで、滑らかな操舵角の変化で実現できる軌道が必要になるが、この軌道算出は、計算コストが非常に高く、リアルタイム軌道計算が困難であるという問題がある。

さらには、「ベルトコンベアの付近に決められた姿勢でＡＧＶを設置する」といった要求があった場合、位置情報だけでなく、姿勢情報まで考慮した軌道計算を行う必要があり、これにはより多くの計算コストを必要とする。

ここで、上述の「特定の位置」及び「特定の姿勢」（目標位置姿勢）が既知であり、ＡＧＶが移動する空間の地図も既知であるという条件で、ＡＧＶの動作開始位置姿勢は未知であるという状態での軌道計算を考える。このとき、「ＡＧＶの移動モデル」、「ＡＧＶの（位置に加えて）姿勢」、さらに、「ＡＧＶが移動しやすい軌道」について、それぞれ考慮する必要がある。

まず、「ＡＧＶの移動モデル」としては、車両と同様の操舵輪を持つステアリング式と考えればよい。しかしながら、ＡＧＶは上述のごとく、回転半径が急激に変化するような軌道には対応できないことに留意する必要がある。

次に、「ＡＧＶの姿勢」については、上記非特許文献１及び上記特許文献３にあるコンフィギュレーション空間を用いることで解決できる。

さらに、「ＡＧＶが移動しやすい軌道」については、操舵角が滑らかに変化する（等回転加速度で変化する）場合の軌道であるクロソイド曲線を用いることで解決できる。

ただし、ＡＧＶの負担を軽減するために、実現可能な最速の操舵角の回転加速度よりも、ゆっくりした操舵角の回転加速度の軌道を優先する。

クロソイド曲線が有するパラメータは「回転加速度」及び「曲線長」である。この回転加速度の大小によって、急な曲りの曲線になるか、なだらかな曲りの曲線になるかが決定し、曲線長によって、どこまで曲がり続けるかが決まる（曲線長を非常に長く設定すると、螺旋のような曲線を描く）。

ｘ‐ｙ座標上に描かれたクロソイド曲線の例を図２に示す。クロソイド曲線は、三角関数の積分を数式内に持つことが知られており、回転加速度は三角関数の周期を、曲線長は積分区間を、それぞれ変化させるパラメータである。

クロソイド曲線にはこのような複雑な数式を用いるため、クロソイド曲線を使った軌道計算というのは解析的に求めることができず、計算コストの高い手法が必要となる。既存の手法・発明では、回転加速度を固定したり、曲線長のとれる範囲を限定したりするなどして、計算コストを下げているが、それでは最適な軌道とは言えず、上述したように問題がある。

そこで、本実施例では、目標位置姿勢が事前に分かっていることを利用し、目標位置姿勢に到達可能な様々なクロソイド曲線と直線の組み合わせを事前に計算し、その際使用したクロソイド曲線のパラメータと直線の長さを、参照テーブルのパラメータとして保存しておくことで、リアルタイム軌道計算を実現する。

コンフィギュレーション空間における、事前に計算した軌道の例を図３に示す。コンフィギュレーション空間は、適当に選択した座標系での並進運動ｘ，ｙと姿勢θとで張られる３次元パラメータ空間（ｘ，ｙ，θ）である。図中の●は目標位置姿勢、×，▽，■は開始位置姿勢、破線は軌道である。

以下、本実施例における具体的なＡＧＶの軌道計算について説明する。

まずＡＧＶは、直線運動時を除き、等速運動、等回転加速度運動すると仮定する。図４に示すように、この場合のＡＧＶ１の位置を（ｘ，ｙ）、姿勢をθ、速度をｖ、旋回角速度をω、旋回加速度をα、操舵角をξ、タイヤ間（操舵輪である可動輪２と従輪３との間）の距離をＬとする。

回転角速度ωと操舵角ξとの間に線形の関係が存在すると仮定すると、下記（１）式のようになる。

このような条件の場合、想定するＡＧＶ１の軌道（直線運動部分を除く）は、時間ｔを用いて下記（２）式のように計算できる。

ここで、Ｃ（（α／π）^1/2ｔ）及びＳ（（α／π）^1/2ｔ）はフレネル積分である。また、ＡＧＶ１の旋回開始時刻をｔ＝０とすると、時刻ｔ＝０のときの位置姿勢（ｘ（０），ｙ（０），θ（０））及び操舵角ξは全て０である。このクロソイド曲線は曲率が時刻ｔと共に小さくなる。

実際に軌道計算をする際には、この曲率が小さくなるクロソイド曲線と曲率が大きくなるクロソイド曲線とを組み合わせてＡＧＶ１の位置姿勢（ｘ（ｔ），ｙ（ｔ），θ（ｔ））を制御する。

図５に示す状況であれば、下記（３）式によってＡＧＶ１の軌道が描かれる。ただし、旋回開始時（ｔ＝０）と旋回終了時の操舵角ξは０度であり、姿勢がθから−θに変わる状況を考える。また、ＡＧＶ１が点ｐ（旋回開始位置）から点ｐ´（旋回終了位置）まで辿り着くのにかかる所要時間をＴ、途中の中間点のＡ点まで辿り着くのにかかる所要時間をＴ_Aとする。

ここでは、ＡＧＶ１の姿勢がθ_sからθ_eに変わる、つまり、θ_s＞０かつθ_e＜０の場合を例として考えるが、実際には、他にもθ_s＞０かつθ_e＞０、θ_s＜０かつθ_e＞０、θ_s＜０かつθ_e＜０の場合が存在し、その都度異なるクロソイド曲線の組み合わせが必要な点に留意する。

実際に上記（３）式などを用いて軌道計算する際には、ＡＧＶ１の目標位置姿勢及び速度ｖは固定し、回転加速度及び曲線長については取り得る様々な値で複数の軌道を作成する。また、クロソイド曲線の前後に直線を組み合わせることで、様々な軌道に対応する。

これらの軌道の最中の回転加速度、時刻、及び、クロソイド曲線の前後の直線長（直線の長さ）を、コンフィギュレーション空間の暫定到達点にパラメータとして保存しておき、実際のＡＧＶ１が目標位置付近に到達した際に位置姿勢パラメータを用いて最も近い暫定到達点を算出する。

算出された暫定到達点から、上述のごとく登録されている回転加速度、時刻、及び、クロソイド曲線の前後の直線長を使い、目標位置姿勢に向かって、上述の式で求められる軌道で移動する。

参照テーブルへのパラメータ登録時に、同じ位置姿勢を通る軌道は複数存在する場合も想定されるが、その場合は、回転加速度の低い軌道を優先的に登録する。

これにより、可能な限りＡＧＶ１に負荷のかからない（つまり操舵角ξの回転角加速度が低い）滑らかな曲線を使用した軌道計算が実現できる。

本実施例に係るＡＧＶ軌道計算装置は、大きく分けて、事前に計算しておく「参照テーブルの作成」と、実際のリアルタイム軌道計算時の「参照テーブルのパラメータを用いた軌道計算」とに分けることができる。なお、実際のリアルタイム軌道計算とは、ＡＧＶ１の走行中の軌道計算時という意味である。

図６は、本実施例に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。なお、図６では、参照テーブル作成の手順と参照テーブルのパラメータを用いた軌道計算の手順とで分けて装置構成を表しており、２つの手順に同一の構成要件を用いる箇所は、同一の構成要件を２つのブロックに分割して模式的に表している。

本実施例に係るＡＧＶ軌道計算装置は、図６に示すように、記憶部１０、地図・目標位置姿勢入力部１１、パラメータ設定部１２、パラメータ取得部１３、軌道計算部１４、参照テーブル登録部１５、参照テーブル部１６、及び、ＡＧＶ位置姿勢入力部１７を備えている。

地図・目標位置姿勢入力部１１は、ＡＧＶが移動する範囲の環境地図と目標位置姿勢を指定したデータを入力し、記憶部１０へ保管する。

参照テーブル登録部１５は、コンフィギュレーション空間に対応する参照テーブルを参照テーブル部１６に用意する。また、参照テーブル登録部１５は、ある軌道のある地点におけるパラメータを、対応する参照テーブルに保管する。すなわち、後述の軌道計算部１４で算出される軌道が通るコンフィギュレーション空間上の各点に対応する参照テーブルに、軌道計算部１４の軌道算出に用いたパラメータを登録する。

また、参照テーブル登録部１５は、参照テーブルに、軌道計算部１４の軌道算出に用いたパラメータを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する。

パラメータ設定部１２では、クロソイド曲線のパラメータである回転加速度と曲線長、及びそれらのパラメータの離散化幅を設定し、それらの組み合わせを記憶部１０に保管する。さらに、直線長の範囲も設定し記憶部１０に保管する。

パラメータ取得部１３では、記憶部１０及び参照テーブル部１６に保管された各パラメータを適宜抽出する。すなわち、参照テーブル作成を行う場合には、記憶部１０に保管された各パラメータを抽出して参照テーブル登録部１５に入力し、参照テーブルのパラメータを用いた軌道計算を行う場合には、参照テーブル部１６に保管された参照テーブルを抽出して軌道計算部１４に入力する。

軌道計算部１４は、参照テーブル作成を行う場合には、パラメータ設定部１２から記憶部１０に保管されたパラメータ範囲で目標位置姿勢を始点とした様々な軌道を算出する。

また、軌道計算部１４は、参照テーブルのパラメータを用いた軌道計算を行う場合には、まず、ＡＧＶ位置姿勢入力部１７から記憶部１０に保管されたＡＧＶの現在の位置姿勢において参照テーブルに登録されたパラメータがあるか否か調査する。パラメータが存在した場合は、軌道計算を行う。存在しなかった場合は、より目標位置姿勢の近くまでＡＧＶを移動させる。さらにその後、軌道計算部１４は、参照テーブルを参照した位置姿勢から登録されていたパラメータを使用して軌道計算を行う。そして、計算結果に基づきＡＧＶを制御する。軌道計算部１４は、これらの処理を所定時刻毎に行う。

すなわち、軌道計算部１４では、パラメータ取得部１３によって入力されたパラメータ、目的位置姿勢、若しくは、ＡＧＶ位置姿勢入力部１７によって入力された現在の位置姿勢を使用して、所定時刻毎の位置姿勢を計算し、計算結果に基づきＡＧＶを制御する。

記憶部１０では、クロソイド曲線パラメータ、直線長、上記環境地図、開始位置姿勢、及び、目標位置姿勢などのデータを保管する。

参照テーブル部１６では、参照テーブル登録部１５により入力された参照テーブル及びパラメータを保管する。記憶部１０は処理毎に初期化されたものを使用するのに対し、参照テーブルは一度登録したものを何度も使用するため、参照テーブル部１６は別の枠として確保している。

ＡＧＶ位置姿勢入力部１７では、ＡＧＶの現在の位置姿勢を入力し記憶部１０に保管する。

以上が、本実施例に係るＡＧＶ軌道計算装置の装置構成の説明である。以下、本実施例に係るＡＧＶ軌道計算装置の手順について、図７，８を用いて説明する。

図７は、「参照テーブル作成」についての手順を示すフローチャートである。

ステップＳ１では、地図・目標位置姿勢入力部１１により、ＡＧＶが移動する範囲の環境地図、及び、ＡＧＶの目標位置姿勢を入力する。

ステップＳ２では、参照テーブル登録部１５により、コンフィギュレーション空間に対応する参照テーブルを用意する。用意された参照テーブルは参照テーブル部１６に保管される。

ステップＳ３では、パラメータ設定部１２により、クロソイド曲線のパラメータ、直線の長さの範囲を設定する。これらは、記憶部１０に保管される。

ステップＳ４‐１，Ｓ４‐２では、軌道算出部１４により、ステップＳ３のパラメータの範囲で目標位置姿勢を始点とした様々な軌道を算出する。なお、このパラメータはパラメータ取得部１３により記憶部１０から抽出される。ステップＳ４‐１では、パラメータがステップＳ３の範囲内であるか否かを判断し、範囲内であればステップＳ４‐２において軌道を算出し、範囲内でなければ処理を終了する。

ステップＳ５では、ステップＳ４‐２で算出された軌道が通るコンフィギュレーション空間上の各点に対応する参照テーブルに、ステップＳ４‐２の軌道で用いたパラメータを登録する。

このようにして、ステップＳ１〜Ｓ５では、予め、ＡＧＶの様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さをＡＧＶの軌道の参照テーブルのパラメータとして保存しておく。より詳しくは、目標位置姿勢に到達可能なクロソイド曲線と直線との組み合わせを計算し、その際使用したクロソイド曲線のパラメータ及び直線の長さを参照テーブルとして保存しておく。

図８は、「参照テーブルのパラメータを用いた軌道計算」についての手順を示すフローチャートである。

ステップＳ１１では、地図・目標位置姿勢入力部１１により、ＡＧＶが移動する範囲の環境地図、及び、ＡＧＶの目標位置姿勢を入力する。

ステップＳ１２では、パラメータ取得部１３により、参照テーブル部１６からステップＳ１〜Ｓ５によって作成された参照テーブルを参照テーブル部１６から抽出し、軌道計算部１４に入力する。

ステップＳ１３では、ＡＧＶ位置姿勢入力部１７により、ＡＧＶの現在の位置姿勢を入力する。入力された現在の位置姿勢のデータは記憶部１０に保管される。

ステップＳ１４‐１では、軌道計算部１４により、パラメータ取得部１３及びＡＧＶ位置姿勢入力部１７からのデータに基づき、ＡＧＶの現在の位置姿勢において、参照テーブルに登録されたパラメータがあるか否かを調査する。あればステップＳ１５に、なければステップＳ１４‐２に移行する。

ステップＳ１４‐２では、ＡＧＶの移動を行う。ステップＳ１４‐２終了後、ステップＳ１３に移行する。

ステップＳ１５では、参照テーブルを参照した位置姿勢から登録されていたパラメータを使用して軌道計算を行い、計算結果に基づきＡＧＶの制御を行う。

このようにして、ステップＳ１１〜Ｓ１５では、ＡＧＶの走行時に、参照テーブルを用いて目標位置姿勢で停止させる軌道を計算し、ＡＧＶの制御を行う。

本実施例に係るＡＧＶ軌道計算装置及び方法によれば、ＡＧＶを滑らかな操舵角変化で移動可能な軌道を作成できる。

また、本実施例に係るＡＧＶ軌道計算装置及び方法によれば、目標位置姿勢に到達可能な様々なクロソイド曲線と直線の組み合わせを事前に計算し、その際使用したクロソイド曲線のパラメータと直線の長さを、参照テーブルとして保存しておくことで、リアルタイム軌道計算が可能である。さらに、磁気テープ等を必要としない自律的な軌道計算が可能であり、開始位置姿勢及び目標位置姿勢の組み合わせを多く取ることも可能であり、特定の位置姿勢に高精度に到達可能な軌道計算が可能である。

また、本実施例に係るＡＧＶ軌道計算装置及び方法によれば、参照テーブルへのパラメータ登録時に、回転加速度の低い軌道を優先的に登録することで、ＡＧＶになるべく負荷のかからない軌道計算、すなわち、滑らかな曲線を使用した軌道計算を行うことができる。

［実施例２］
本実施例では、実施例１のようにある程度時間をかけて目的位置姿勢毎に参照テーブルを作成するのではなく、一つの対称単位クロソイド曲線テーブルの作成だけで、様々な始点及び終点の位置姿勢に対応することで、処理時間を短縮することを可能とするものである。

すなわち、本実施例に係るＡＧＶ軌道計算装置及び方法は、対称単位クロソイド曲線テーブルを作成し、この対称単位クロソイド曲線テーブルを用いて目標位置姿勢で停止させる軌道を計算するものである。

なお、本実施例及び実施例３では、ＡＧＶの動作開始位置を始点、到達目標位置を終点と呼称し、始点の位置姿勢を始点位置姿勢、終点の位置姿勢を終点位置姿勢と呼称する。始点位置姿勢及び終点位置姿勢は、実施例１における開始位置姿勢及び目標位置姿勢と同義である。

以下、本実施例に係るＡＧＶ軌道計算装置及び方法について、図９〜２３を用いて説明する。

クロソイド曲線を使って軌道を計算する場合、直線・円弧を含めると以下の組み合わせが考えられる。
・直線とクロソイド曲線（曲率０の端点）との接続
・曲率χの円弧とクロソイド曲線（曲率χの端点）との接続
・接続する端点の曲率は同じだが、曲率変化率の異なる２つのクロソイド曲線

以上、全てを考慮して始点・終点の軌道を計算すると、組み合わせの量が非常に多くなりすぎるため、以下の条件を設定する。

まず、図９に示すように、基本となるクロソイド曲線は左右対称とする。これを対称クロソイド曲線と呼ぶ。始点と終点とを結ぶ直線に対し、始点での角度はθ、終点での角度は−θで共に曲率は０である。また、始点から中点までの曲率変化率をαとして、中点での曲率が最大（角度は上記直線に対し０）となり、中点から終点までの曲率変化率が−αとなる（ここで、曲率変化率αは、始点の曲率をχ_s、中点の曲率をχ_m、始点‐中点間の曲線長をｌとしたときに、α＝（χ_m−χ_s）／ｌとしている）。

次に、上記対称クロソイド曲線のみで表現できない場合（始点角をθ_s、終点角度をθ_eとしたときにθ_s≠−θ_e）は、図１０に示すように、対称クロソイド曲線＋直線、又は、直線＋対称クロソイド曲線の組み合わせ、あるいは、図１１に示すように、対称クロソイド曲線＋対称クロソイド曲線の組み合わせ（二つの対称クロソイド曲線）により、軌道を表現する。

また、対称クロソイド曲線は、始点の角度を大きくしていくと、図１２に示すように、ある時点でループ状の形状となる（図１２は、始点の角度が１５０度の場合）。単純化のため、ループ状にならない角度（約１３１．６度まで）での対称クロソイド曲線を使用する。

以上の条件の下、以下で説明する方法により対称クロソイド曲線を用いた軌道を複数計算する。その際、中間点がある場合は、再帰的に、始点と中間点、中間点と終点について、改めて以下の方法で軌道を計算する。なお、中間点とは、軌道の経由点であり、かつ、クロソイド曲線と直線あるいは２つの曲線同士の接続点である。

図１３は、本実施例に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。本実施例に係るＡＧＶ軌道計算装置は、記憶部２０、始点・終点位置姿勢入力部２１、パラメータ設定部２２、対称単位クロソイド曲線テーブル計算部２３、軌道計算部２４、軌道選択部２５、及び、軌道出力部２６を備える。

始点・終点位置姿勢入力部２１は、動作を開始する始点の位置座標と姿勢（始点位置姿勢）、及び、目標地点である終点の位置座標と姿勢（終点位置姿勢）を、入力（設定）する。入力された始点位置姿勢及び終点位置姿勢は、記憶部２０に保管される。

パラメータ設定部２２は、対称単位クロソイド曲線（後述）テーブル作成時の角度分解能、及び、軌道の最大曲率などのパラメータを設定する。設定されたパラメータは記憶部２０に保管される。

記憶部２０は、始点位置姿勢、終点位置姿勢、パラメータ設定部２２により設定されたパラメータ、対称単位クロソイド曲線テーブル、及び、軌道などのデータを保管する。

対称単位クロソイド曲線テーブル計算部２３は、対称単位クロソイド曲線テーブルの作成を行う。すなわち、対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な対称単位クロソイド曲線テーブルを予め求め、記憶部２０に保管する。

軌道計算部２４は、軌道計算を行う。すなわち、記憶部２０から始点位置姿勢、終点位置姿勢、及び、軌道計算で必要な対称単位クロソイド曲線テーブルを取り出し、始点‐終点間の軌道計算を行って軌道リスト（複数の軌道テータ）を求め、軌道選択部２５へ送る。

軌道選択部２５は、軌道の選択を行う。すなわち、パラメータ設定部２２により設定されたパラメータを記憶部２０から読み込み、軌道計算部２４により作成され送られてきた軌道リスト（複数の軌道のデータ）の中から軌道を選択し、記憶部２０に保管する。

軌道出力部２６は、得られた軌道を記憶部２０から取り出し、外部へ出力する。

本実施例に係るＡＧＶ軌道計算装置及び方法の基本的な軌道計算処理の全体のフローチャートを図１４に示す。

ステップＳ２１では、対称単位クロソイド曲線テーブル計算部２３によって、対称単位クロソイド曲線テーブルの作成を行う。すなわち、軌道計算で用いるクロソイド曲線を予め角度毎に計算しておき、曲線テーブルとして作成する。

ステップＳ２２では、始点・終点位置姿勢入力部２１によって、始点位置姿勢の設定を行う。

ステップＳ２３では、始点・終点位置姿勢入力部２１によって、終点位置姿勢の設定を行う。

ステップＳ２４では、軌道計算部２４によって軌道計算を行う。すなわち、入力した始点位置姿勢及び終点位置姿勢に基づき、始点‐終点間の軌道を計算する。

ステップＳ２５では、軌道選択部２５によって軌道の選択を行う。すなわち、ステップＳ２６の軌道計算では、始点‐終点間に複数の軌道が作成されるが、その複数の軌道から１つを選択する。

以下、上記ステップＳ２１〜Ｓ２５のそれぞれの処理の詳細を説明する。

［ステップＳ２１について］
クロソイド曲線を求めるためには、下記式（４）のとおり積分を計算する必要がある。なお、下記式（４）において、Ｌは曲線長、τは曲線の接線角、Ａはスケール（拡大率）を表すクロソイドパラメータである。

軌道を計算する度に積分計算を行うと時間がかかってしまうため、対称単位クロソイド曲線テーブル計算部２３によって、予め必要な角度毎に対称単位クロソイド曲線を計算したものをテーブル（対称単位クロソイド曲線テーブル）として用意し、軌道計算時には必要な角度に対応する対称単位クロソイド曲線をテーブルから参照して拡大あるいは縮小して適用する。なお、クロソイドパラメータＡを１としたときのクロソイド曲線を単位クロソイド曲線といい、それを対称にしたものを対称単位クロソイド曲線という。

始点角度τの対称単位クロソイド曲線を求めるには、図１５に示すとおり、まず上記式（４）（ただしＡ＝１）を用いて接続角がτになるところまでの単位クロソイド曲線を計算する。次に図１６のようにｘ軸で反転、τの回転、ｘ´中心に反転複製を経ることで対称単位クロソイド曲線を生成することができる。

このときの対称単位クロソイド曲線の各値は下記のとおりとなる。
・始点角度：τ
・始点‐終点間距離：２ｘ´
・始点‐終点間曲線長：２ｌ_τ＝２（２τ）^1/2
・ピーク座標：（ｘ´，ｙ´）
・最大曲率：ｌ_τ＝（２τ）^1/2
・曲率変化率：１
以上の値を始点角度毎に対称単位クロソイド曲線テーブルに曲線の座標と共に保持する。

また、参考用として、クロソイドパラメータＡでスケールを変化させた対称クロソイド曲線の各値は以下のとおりとなる。
・始点角度：τ
・始点‐終点間距離：２Ａｘ´
・始点‐終点間曲線長：２Ａｌ_τ＝２Ａ（２τ）^1/2
・ピーク座標：（Ａｘ´，Ａｙ´）
・最大曲率：ｌ_τ／Ａ＝（２τ）^1/2／Ａ
・曲率変化率：１／Ａ²

［ステップＳ２２，Ｓ２３について］
始点・終点位置姿勢入力部２１によって、始点位置姿勢（ｘ´_s，ｙ´_s，θ´_s）、及び、終点位置姿勢（ｘ´_e，ｙ´_e，θ´_e）を設定する。

［ステップＳ２４について］
ここで、軌道計算部２４によって、始点‐終点間の軌道を計算する。この軌道計算のフローチャートを図１７に示す。この軌道計算の処理は、始点及び終点での角度によって処理が分岐し、大まかには以下の軌道計算処理が行われる。そしてこの処理により、始点・終点間に一つだけでなく複数の軌道（下記ａ，ｂ，ｃ）が計算される。
ａ．一つの対称クロソイド曲線による軌道
ｂ．一つの対称クロソイド曲線と直線による軌道
ｃ．二つの対称クロソイド曲線による軌道

なお、任意の始点位置姿勢及び終点位置姿勢に対して軌道計算をする際、簡略化のため、図１８に示すように、絶対座標から、始点位置を原点とし終点をｘ軸上に置く相対座標に変換する。これにより、始点位置姿勢は（０，０，θ_s）（ただし、θ_s＝θ´_s−θ_r）、終点位置姿勢は（ｘ_e，０，θ_e）（ただし、ｘ_e＝（ｘ´_e−ｘ´_s）²＋（ｙ´_e−ｙ´_s）²）^1/2，θ_e＝θ´_e−θ_r）となり、始点終点間距離はｘ_eとなる。以下の軌道計算は、このような相対座標系で行っているものとする。なお、θ_rについては後述する。

ステップＳ２４‐１では、入力された始点位置姿勢及び終点位置姿勢に基づき、単一の対称クロソイド曲線で表現することが可能か否かを判断する。可能であればステップＳ２４‐２へ、可能でなければステップＳ２４‐４へ移行する。なお、図１９に示すように、始点角（始点の姿勢）θ_sと終点角（終点の姿勢）−θ_eとがθ_s＝−θ_eの関係にあるとき、単一の対称クロソイド曲線だけで表現することが可能となる（軌道を計算することができる）。

ステップＳ２４‐２，Ｓ２４‐３は、上記「ａ．一つ（単一）の対称クロソイド曲線による軌道」の計算を行うものである。

ステップＳ２４‐２では、始点角θ_sから対称単位クロソイド曲線テーブル内の対称単位クロソイド曲線を選択する。

ステップＳ２４‐３では、始点角θ_sと始点終点間距離ｘ_eから拡大率（クロソイドパラメータＡ）を算出し、対称クロソイド曲線の軌道を生成する。

上記ステップＳ２１において予め求めた対称単位クロソイド曲線テーブル内にある、始点角θ_sの対称単位クロソイド曲線の始点終点間距離を、ｄ_θsとすると、クロソイドパラメータＡは、
Ａ＝ｘ_e／ｄ_θs （５）
となる。これにより対称単位クロソイド曲線をＡ倍に拡大したものが算出したい軌道となる。

また、このときの始点終点間曲線長は、（上記「始点終点間曲線長：２Ａｌ_τ＝２Ａ（２τ）^1/2」より）２Ａ（２θ_s）^1/2、最大曲率は、（上記「最大曲率：ｌ_τ／Ａ＝（２τ）^1/2／Ａ」より）（２θ_s）^1/2／Ａとなる。

ステップＳ２４‐４では、始点角θ_sと終点角θ_eの符号が異なるか否か判断する。すなわち、ｓｇｎ（θ_s）≠ｓｇｎ（θ_e）かｓｇｎ（θ_s）＝ｓｇｎ（θ_e）かを判断する。異なる場合（ｓｇｎ（θ_s）≠ｓｇｎ（θ_e））はステップＳ２４‐５へ移行し、同一の場合（ｓｇｎ（θ_s）＝ｓｇｎ（θ_e））はステップＳ２４‐１４へ移行する。

ステップＳ２４‐５〜Ｓ２４‐１３は、上記「ｂ．一つの対称クロソイド曲線と直線による軌道」の計算を行うものである。

すなわち、始点角θ_s及び終点角θ_eが、θ_s≠−θ_e（上記ステップＳ２４‐１）であり、かつ、ｓｇｎ（θ_s）≠ｓｇｎ（θ_e）（上記ステップＳ２４‐４）の場合、対称クロソイド曲線の前後どちらかに直線を加えて軌道の計算を行う。

ステップＳ２４‐５では、始点角θ_sが終点角θ_eより大きいか否かを判断する。大きい場合（｜θ_s｜＞｜θ_e｜）はステップＳ２４‐６へ移行し、そうでない場合（｜θ_s｜＜｜θ_e｜）はステップＳ２４‐１０へ移行する。

ステップＳ２４‐６〜Ｓ２４‐９は、前半を対称クロソイド曲線とし後半を直線とする処理であり、ステップＳ２４‐１０〜Ｓ２４‐１３は、前半を直線とし後半を対称クロソイド曲線とする処理である。

｜θ_s｜＞｜θ_e｜の場合、まず、ステップＳ２４‐６において、中間点（対称クロソイド曲線から直線に切り替わる点）ｐ_cを作成する。そして、図２０に示すように、中間点において、対称クロソイド曲線の端点に直線が付加される。

図２０より、対称クロソイド曲線の始点角θ_cは、
θ_c＝（θ_s−θ_e）／２ （６）
となる。

また、対称クロソイド曲線自体の傾き角（二点間（ここでは始点と中間点）を結ぶ直線のｘ軸からの傾斜角）θ_rは、
θ_r＝（θ_s＋θ_e）／２ （７）
となる。

さらに、対称クロソイド曲線の端点である中間点ｐ_c（ｘ_c，ｙ_c）は、下記連立方程式（８）
ｙ_c＝ｘ_cｔａｎθ_r
ｙ_c＝（ｘ_e−ｘ_c）ｔａｎ（−θ_e） （８）
から求めて、下記式（９）となる。
ｘ_c＝ｘ_e（−ｔａｎθ_e／（ｔａｎθ_r−ｔａｎθ_e））
ｙ_c＝ｘ_cｔａｎθ_r （９）

ステップＳ２４‐７では、前半（対称クロソイド曲線）の軌道計算を行う。始点ｐ_s（０，０）と中間点ｐ_c（ｘ_c，ｙ_c）、及び、対称クロソイド曲線の始点角θ_cを用いて軌道計算を再帰的に呼び出して、上記ステップＳ２４‐２，Ｓ２４‐３の方法で対称クロソイド曲線部分の軌道計算を行う。

ステップＳ２４‐８では、後半の直線軌道計算を行う。対称クロソイド曲線の曲線長をｌ、最大曲率をχとしたとき、対称クロソイド曲線＋直線による軌道の長さは、ｌ＋（（ｘ_e−ｘ_c）²＋ｙ_c ²）^1/2、最大曲率はχとなる。

そして、ステップＳ２４‐９において、直線部分を対称クロソイド曲線に付加（結合）する。

一方、｜θ_s｜＜｜θ_e｜の場合、まず、ステップＳ２４‐１０において、中間点（直線から対称クロソイド曲線に切り替わる点）ｐ_cを作成する。図２１に示すように、対称クロソイド曲線の前に直線が付加される。上記（６）（７）式と同様に、θ_c＝（θ_s−θ_e）／２，θ_r＝（θ_s＋θ_e）／２となり、対称クロソイド曲線の端点である中間点ｐ_c（ｘ_c，ｙ_c）は、
ｘ_c＝ｘ_e（−ｔａｎθ_r／（ｔａｎθ_s−ｔａｎθ_r））
ｙ_c＝ｘ_cｔａｎθ_s （１０）
となる。

これにより、まず、ステップＳ２４‐１１において前半の直線部分の軌道計算を行う。

その後、ステップＳ２４‐１２において、中間点ｐ_c（ｘ_c，ｙ_c）と終点ｐ_e（ｘ_e，０）、及び、対称クロソイド曲線の始点角θ_cを用いて軌道計算を再帰的に呼び出して、上記ステップＳ２４‐２，Ｓ２４‐３の方法で対称クロソイド曲線部分の軌道計算を行う。

対称クロソイド曲線の曲線長をｌ、最大曲率をχとしたとき、以上の直線＋対称クロソイド曲線による軌道の長さは、ｌ＋（ｘ_c ²＋ｙ_c ²）^1/2、最大曲率はχとなる。

そして、ステップＳ２４‐１３において、対称クロソイド曲線を直線部分に付加（結合）する。

ステップＳ２４‐１４〜Ｓ２４‐１７は、上記「ｃ．二つの対称クロソイド曲線による軌道」の計算を行うものである。すなわち、始点角θ_sと終点角θ_eがｓｇｎ（θ_s）＝ｓｇｎ（θ_e）（符号が同じ）場合などでは、一つの対称クロソイド曲線だけでは軌道を生成することができない。この場合は、一つではなく二つの対称クロソイド曲線を用いる。

ステップＳ２４‐１４では、中間点を作成する。図２２に二つの対称クロソイド曲線（第１，２対称クロソイド曲線）を用いる場合を示す。二つの対称クロソイド曲線を接続（結合）する場合には、接続点（中間点）ｐ_c（ｘ_c，ｙ_c）の位置やそこでの姿勢によって様々な形状が考えられるため、単純化のために以下の二つの条件をつける。

・点ｐ_cにおける角度θ_mを、始点角θ_s及び終点角θ_eから
θ_m＝−（θ_s＋θ_e）／２ （１１）
とする。
・接続点（中間点）ｐ_c（ｘ_c，ｙ_c）について、ｘ_c＝ｘ_e／２となるようにする。

これにより、二等辺三角形ｐ_sｐ_eｐ_cの二辺ｐ_sｐ_c及びｐ_cｐ_eを底辺とし、かつ、ｍ₁ｐ_cｍ₂を直線とする二つの二等辺三角形ｍ₁ｐ_sｐ_c及びｍ₂ｐ_cｐ_eを作成することができる。

まず、二等辺三角形ｍ₁ｐ_sｐ_cのうち、角度θ_c1は、θ_s及びθ_mから
θ_c1＝（θ_s−θ_m）／２ （１２）
として求めることができる。

二等辺三角形ｍ₂ｐ_cｐ_eの角度θ_c2も同様に、θ_m及びθ_eから
θ_c2＝（θ_m−θ_e）／２ （１３）
となる。

これにより、二等辺三角形ｐ_sｐ_eｐ_cの角度θ_rは下記（１４）式で求めることができる。
θ_r＝θ_s−θ_C1＝−θ_c2−θ_e （１４）

よって、点ｐ_c（ｘ_c，ｙ_c）は、
ｘ_c＝ｘ_e／２
ｙ_c＝ｘ_cｔａｎθ_r （１５）
となる。

これにより、まずステップＳ２４‐１５において、前半（第１対称クロソイド曲線）の軌道計算を行う。すなわち、始点ｐ_s（０，０）と中間点ｐ_c（ｘ_c，ｙ_c）、及び、θ_c1を用いて軌道計算を再帰的に呼び出して、上記ステップＳ２４‐２，Ｓ２４‐３の方法で一つ目の対称クロソイド曲線部分の軌道計算を行う。

その後、ステップＳ２４‐１６において、後半（第２対称クロソイド曲線）の軌道計算を行う。すなわち、中間点ｐ_c（ｘ_c，ｙ_c）と終点ｐ_e（ｘ_e，０）、及び、θ_c2を用いて軌道計算を再帰的に呼び出して、上記ステップＳ２４‐２，Ｓ２４‐３の方法で二つ目の対称クロソイド曲線部分の軌道計算を行う。

そして、ステップＳ２４‐１７において、二つ目の対称クロソイド曲線を一つ目の対称クロソイド曲線に付加（結合）する。

以上の二つの対称クロソイド曲線による軌道の長さは、それぞれの対称クロソイド曲線長を足し合せたものとなり、最大曲率は、それぞれの対称クロソイド曲線の最大曲率のどちらか大きい方となる。

以上がステップＳ２４の説明である。このようにして、軌道計算部２４は、幾何学的に中間点を設定し、始点と中間点、及び、中間点と終点との間に、対称単位クロソイド曲線を拡大又は縮小した対称クロソイド曲線、あるいは、直線を当てはめて、軌道計算を行う。

ステップＳ２５では、軌道選択部２５によって、軌道の選択を行う。ステップＳ２４で説明した方法により、始点‐終点間の軌道を求めることができる。このとき、図２３に示すように、同じ始点‐終点間であっても、複数の軌道が得られる。そのため、この中から最適な軌道を選択する。選択の手順は以下の（１）（２）のとおりである。

１．最大曲率がしきい値χ_max以下であり、かつ始点から終点までの軌道長が最も短いもの。
２．上記１で軌道上に障害物がある場合、最大曲率がしきい値χ_max以下のものから短い順に障害物がないものを選択する。
ステップＳ２５では、以上により軌道を決定する。

本実施例に係るＡＧＶ軌道計算装置及び方法によれば、積分式の繰り返し計算は、初めの対称単位クロソイド曲線テーブルの作成時だけであるため、軌道計算時の計算量が少なく、高速になる。

また、本実施例に係るＡＧＶ軌道計算装置及び方法によれば、複数の軌道候補を作成してその中から最適なものを選択するので、様々な始点及び終点の位置姿勢に対応可能である。

さらに、本実施例に係るＡＧＶ軌道計算装置及び方法によれば、幾何学的な計算によって軌道が計算されるため、二分法、ＮＲ法、あるいは、最急降下法などによるパラメータ推定を行う必要がない。

そして、本実施例に係るＡＧＶ軌道計算装置及び方法によれば、実施例１のように参照テーブルを作成する必要がなく、一つの対称単位クロソイド曲線テーブルの作成だけで、様々な始点及び終点の位置姿勢に対応することができる。

［実施例３］
始点角や終点角が大きい場合、非常に大きい軌道が作成される場合がある。その場合は、実施例２の基本的な考え方に加えて、始点角及び終点角が開いている（絶対値が９０度以上の大きな角度である）場合の処理を追加して対処することができる。

本実施例に係るＡＧＶ軌道計算装置及び方法について、図２４〜２８を用いて説明する。

図２４は、本実施例に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。本実施例に係るＡＧＶ軌道計算装置は、記憶部２０、始点・終点位置姿勢入力部２１、パラメータ設定部２２、対称単位クロソイド曲線テーブル計算部２３、軌道計算部３４、軌道選択部２５、及び、軌道出力部２６を備える。

このうち、記憶部２０、始点・終点位置姿勢入力部２１、パラメータ設定部２２、対称単位クロソイド曲線テーブル計算部２３、軌道選択部２５、及び、軌道出力部２６については、実施例２と同様であるため説明を省略し、以下では軌道計算部３４での処理を中心に説明する。

本実施例では、図１４に示した基本的な軌道計算の全体フローチャートに変更はないが、図１７に示した軌道計算部２４による軌道計算に、始点・終点角が開いている場合の処理を追加する。このフローチャートを図２５Ａ，２５Ｂに示す。なお、図２５ＡのステップＳ２４‐１〜Ｓ２４‐１７は図１７と同様であるため、以下では説明を省略する。

図２５Ａ，２５Ｂに示す、軌道計算部３４による処理では、実施例２における軌道計算部２４による「ａ．一つの対称クロソイド曲線による軌道」「ｂ．一つの対称クロソイド曲線と直線による軌道」「ｃ．二つの対称クロソイド曲線による軌道」の処理（図２５Ａ）に加えて、以下の処理（図２５Ｂ）を行う。
ｄ．一つの対称クロソイド曲線で作成できる軌道を二つ以上の対称クロソイド曲線に分割
ｅ．一つの対称クロソイド曲線＋直線で作成できる軌道を二つの対称クロソイド曲線に分割

ステップＳ２４‐１８（「ａ．一つの対称クロソイド曲線による軌道」のステップＳ２４‐２，Ｓ２４‐３の処理後）では、始点姿勢（始点角）と終点姿勢（終点角）とが開いているか否かを判断する。開いていればステップＳ２４‐１９へ移行し、開いていなければ処理を終了する。

ステップＳ２４‐１９〜Ｓ２４‐２２は、上記「ｄ．一つの対称クロソイド曲線で作成できる軌道を二つ以上の対称クロソイド曲線に分割」の計算を行うものである。これにより、図２６の実線に示すように、軌道を小さくすることができる。

ステップＳ２４‐１９では、中間点ｐ_c（ｘ_c，ｙ_c）を作成する（ｘ_c＝ｘ_e／２）。

ステップＳ２４‐２０では、始点位置姿勢及び中間点での位置姿勢から、前半（始点ｐ_s（０，０）‐中間点ｐ_c（ｘ_c，ｙ_c）間の対称クロソイド曲線）の軌道計算を行う。

ステップＳ２４‐２１では、中間点での位置姿勢及び終点位置姿勢から、後半（中間点ｐ_c（ｘ_c，ｙ_c）‐終点ｐ_e（ｘ_e，０）間の対称クロソイド曲線）の軌道計算を行う。

ステップＳ２４‐２２では、二つの対称クロソイド曲線の軌道を結合する。

すなわち、始点角θ_sと終点角θ_eとがθ_s＝−θ_eの関係にある場合は、ステップＳ２４‐２，Ｓ２４‐３のように一つの対称クロソイド曲線で軌道計算できるが、角度が大きい場合は、一つの対称クロソイド曲線だけで計算せず、「ｃ．二つの対称クロソイド曲線による軌道」のステップＳ２４‐１４〜Ｓ２４‐１７の処理と同様の処理を行うことで、図２６の実線に示すように、軌道を小さくすることができる。

また、幾何学的計算を利用し新たに中間点を設けることで、図２７に示すように、軌道パターンを増やすことができる。

（第１軌道）
始点ｐ_s（０，０）から中間点ｐ_c2（ｘ_e，ｙ_c）において１８０度向きを変える対称クロソイド曲線と、そこから終点ｐ_e（ｘ_e，０）へ向かう対称クロソイド曲線との組み合わせ（図２７の第１軌道）

（第２軌道）
上記第１軌道とは反対の形状、すなわち、中間点ｐ_c1（０，ｙ_c）において終点ｐ_e（ｘ_e，０）の向きの１８０度反対方向へ向く対称クロソイド曲線と、そこから終点ｐ_e（ｘ_e，０）へ１８０度向きを変える対称クロソイド曲線との組み合わせ（図２７の第２軌道）

（第３軌道）
上記第１軌道及び上記第２軌道で生成された中間点ｐ_c1（０，ｙ_c），ｐ_c2（ｘ_e，ｙ_c）を互いの結合部分とする、３つの対称クロソイド曲線を使った軌道（図２７の第３軌道）

なお、図２７において、θ_r＝θ_s−π／２，ｙ_c＝ｘ_eｔａｎθ_rである。

ステップＳ２４‐２３（「ｂ．一つの対称クロソイド曲線と直線による軌道」「ｃ．二つの対称クロソイド曲線による軌道」の処理後）では、始点姿勢と終点姿勢とが開いているか否かを判断する。開いていればステップＳ２４‐２４へ移行し、開いていなければ処理を終了する。

ステップＳ２４‐２４〜Ｓ２４‐２７は、上記「ｅ．一つの対称クロソイド曲線＋直線で作成できる軌道を二つの対称クロソイド曲線に分割」の計算を行うものである。これにより、図２８の実線に示すように、軌道を小さくすることができる。

ステップＳ２４‐２４では、中間点ｐ_c（ｘ_c，ｙ_c）を作成する。具体的には、始点ｐ_s（０，０）からθ_r1の角度に延びた直線と、終点ｐ_e（ｘ_e，０）からθ_r2の角度に延びた直線との交点を中間点ｐ_c（ｘ_c，ｙ_c）とする。ただし、θ_r1＝θ_s−π／２，θ_r2＝（θ_e＋θ_s−π）／２である。

ステップＳ２４‐２５では、始点位置姿勢及び中間点での位置姿勢から、前半（始点ｐ_s（０，０）‐中間点ｐ_c（ｘ_c，ｙ_c）間の対称クロソイド曲線）の軌道計算を行う。

ステップＳ２４‐２６では、中間点での位置姿勢及び終点位置姿勢から、後半（中間点ｐ_c（ｘ_c，ｙ_c）‐終点ｐ_e（ｘ_e，０）間の対称クロソイド曲線）の軌道計算を行う。

ステップＳ２４‐２７では、二つの対称クロソイド曲線の軌道を結合する。

すなわち、始点角θ_sと終点角θ_eとがθ_s≠−θ_eであり、かつｓｇｎ（θ_s）≠ｓｇｎ（θ_e）の場合は、ステップＳ２４‐５〜Ｓ２４‐１３のように一つの対称クロソイド曲線と直線で軌道計算できるが、例えば始点角θ_sが大きい場合は、図２８に示すように、中間点ｐ_c（ｘ_c，ｙ_c）を設定し二つの対称クロソイド曲線から軌道を求めることができる。

本実施例に係るＡＧＶ軌道計算装置及び方法によれば、始点角及び終点角が大きく開いた場合でも、幾何学的な計算を用いることで、コンパクトな経路を計算することができる。

本発明は、ＡＧＶ軌道計算装置及び方法として好適である。

１ ＡＧＶ（自動搬送車）
２ 可動輪
３ 従輪
４ 駆動輪
１０，２０ 記憶部
１１ 地図・目標位置姿勢入力部
１２ パラメータ入力部
１３ パラメータ取得部
１４，２４，３４ 軌道計算部
１５ 参照テーブル登録部
１６ 参照テーブル部
１７ ＡＧＶ位置姿勢入力部
２１ 始点・終点位置姿勢入力部
２２ パラメータ設定部
２３ 対称単位クロソイド曲線テーブル計算部
２５ 軌道選択部
２６ 軌道出力部

Claims (20)

1. 自動搬送車の軌道計算を行い、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算装置であって、
   前記目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、前記自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、前記自動搬送車の軌道計算を行う
   ことを特徴とする、ＡＧＶ軌道計算装置。
2. 予め、前記自動搬送車の様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さを前記軌道の参照テーブルとして保存しておき、
   前記自動搬送車の走行時に、前記参照テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
   ことを特徴とする、請求項１に記載のＡＧＶ軌道計算装置。
3. 前記目標位置姿勢に到達可能な前記クロソイド曲線と前記直線との組み合わせを計算し、その際使用した前記クロソイド曲線のパラメータ及び前記直線の長さを前記参照テーブルとして保存しておく
   ことを特徴とする、請求項２に記載のＡＧＶ軌道計算装置。
4. 前記目標位置姿勢のデータを入力する入力部と、
   クロソイド曲線のパラメータ及び直線の長さの範囲を設定する設定部と、
   前記設定部で設定された範囲で前記目標位置姿勢を始点とした様々な軌道を算出する計算部と、
   コンフィギュレーション空間に対応する前記参照テーブルを用意し、前記計算部で算出される軌道が通る前記コンフィギュレーション空間上の各点に対応する前記参照テーブルに、前記計算部の軌道算出に用いた前記クロソイド曲線のパラメータ及び前記直線の長さを登録する登録部とを備えることで、
   予め前記参照テーブルを作成する
   ことを特徴とする、請求項３に記載のＡＧＶ軌道計算装置。
5. 前記自動搬送車の走行時の位置姿勢を入力する位置姿勢入力部を備え、
   前記計算部は、
   所定時刻毎に、前記位置姿勢入力部で入力された前記自動搬送車の走行時の位置姿勢において、予め前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さの組み合わせが存在する場合は、軌道計算を行い、存在しない場合は、より前記目標位置姿勢の近くまで前記自動搬送車を移動させ、前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さを用いて、軌道計算を行うことで、
   前記自動搬送車の走行時に、前記参照テーブルを用いた前記自動搬送車の軌道計算を行う
   ことを特徴とする、請求項４に記載のＡＧＶ軌道計算装置。
6. 前記登録部は、
   前記参照テーブルに、前記クロソイド曲線のパラメータ及び前記直線の長さを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する
   ことを特徴とする、請求項４又は５に記載のＡＧＶ軌道計算装置。
7. 対称単位クロソイド曲線テーブルを作成し、
   前記対称単位クロソイド曲線テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
   ことを特徴とする、請求項１に記載のＡＧＶ軌道計算装置。
8. 始点位置姿勢及び前記目標位置姿勢を入力する入力部と、
   前記対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な前記対称単位クロソイド曲線テーブルを予め求める、対称単位クロソイド曲線テーブル計算部と、
   前記始点位置姿勢、前記目標位置姿勢、及び、軌道計算で必要な前記対称単位クロソイド曲線テーブルを取り出し、前記始点と前記終点との間で軌道計算を行い、複数の軌道データを求める、軌道計算部と、
   前記複数の軌道データの中から軌道を選択する軌道選択部とを備える
   ことを特徴とする、請求項７に記載のＡＧＶ軌道計算装置。
9. 前記軌道計算部は、
   幾何学的に中間点を設定し、前記始点と前記中間点、及び、前記中間点と前記終点との間に、対称単位クロソイド曲線を拡大又は縮小した対称クロソイド曲線、あるいは、直線を当てはめて、軌道計算を行う
   ことを特徴とする、請求項８に記載のＡＧＶ軌道計算装置。
10. 軌道選択部は、
    予め設定された最大曲率及び軌道長に基づき、前記複数の軌道データの中から最適な軌道を選択する
    ことを特徴とする、請求項８又は９に記載のＡＧＶ軌道計算装置。
11. 自動搬送車の軌道を計算し、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算方法であって、
    前記目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、前記自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、前記自動搬送車の軌道計算を行う
    ことを特徴とする、ＡＧＶ軌道計算方法。
12. 予め、前記自動搬送車の様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さを前記軌道の参照テーブルとして保存しておき、
    前記自動搬送車の走行時に、前記参照テーブルを用いて目標位置姿勢で停止させる軌道を計算する
    ことを特徴とする、請求項１１に記載のＡＧＶ軌道計算方法。
13. 前記目標位置姿勢に到達可能な前記クロソイド曲線と前記直線との組み合わせを計算し、その際使用した前記クロソイド曲線のパラメータ及び前記直線の長さを前記参照テーブルとして保存しておく
    ことを特徴とする、請求項１２に記載のＡＧＶ軌道計算方法。
14. 前記目標位置姿勢のデータを入力し、
    クロソイド曲線のパラメータ及び直線の長さの範囲を設定し、
    設定された前記クロソイド曲線のパラメータ及び前記直線の長さの範囲で前記目標位置姿勢を始点とした様々な軌道を算出し、
    コンフィギュレーション空間に対応する前記参照テーブルを用意し、算出された軌道が通る前記コンフィギュレーション空間上の各点に対応する前記参照テーブルに、軌道の算出に用いた前記クロソイド曲線のパラメータ及び前記直線の長さを登録することで、
    予め前記参照テーブルを作成する
    ことを特徴とする、請求項１３に記載のＡＧＶ軌道計算方法。
15. 前記自動搬送車の走行時の位置姿勢を入力し、
    所定時刻毎に、前記自動搬送車の走行時の位置姿勢において、予め登録された前記クロソイド曲線のパラメータ及び前記直線の長さの組み合わせが存在する場合は、軌道計算を行い、存在しない場合は、より前記目標位置姿勢の近くまで前記自動搬送車を移動させ、前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さを用いて、軌道計算を行うことで、
    前記自動搬送車の走行時に、前記参照テーブルを用いた前記自動搬送車の軌道計算を行う
    ことを特徴とする、請求項１４に記載のＡＧＶ軌道計算方法。
16. 前記参照テーブルに、前記クロソイド曲線のパラメータ及び前記直線の長さを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する
    ことを特徴とする、請求項１４又は１５に記載のＡＧＶ軌道計算方法。
17. 対称単位クロソイド曲線テーブルを作成し、
    前記対称単位クロソイド曲線テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
    ことを特徴とする、請求項１１に記載のＡＧＶ軌道計算方法。
18. 始点位置姿勢及び前記目標位置姿勢を入力し、
    前記対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な前記対称単位クロソイド曲線テーブルを予め求め、
    前記始点位置姿勢、前記目標位置姿勢、及び、軌道計算で必要な前記対称単位クロソイド曲線テーブルに基づき、前記始点と前記終点との間で軌道計算を行い、複数の軌道データを求め、
    前記複数の軌道データの中から軌道を選択する
    ことを特徴とする、請求項１７に記載のＡＧＶ軌道計算方法。
19. 幾何学的に中間点を設定し、前記始点と前記中間点、及び、前記中間点と前記終点との間に、対称単位クロソイド曲線を拡大又は縮小した対称クロソイド曲線、あるいは、直線を当てはめて、軌道計算を行う
    ことを特徴とする、請求項１８に記載のＡＧＶ軌道計算方法。
20. 予め設定された最大曲率及び軌道長に基づき、前記複数の軌道データの中から最適な軌道を選択する
    ことを特徴とする、請求項１８又は１９に記載のＡＧＶ軌道計算方法。

Images