(57)【要約】【課題】自動搬送車を最適な軌道で目標位置姿勢にすることを可能とする。【解決手段】自動搬送車の軌道計算を行い、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算装置であって、目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、自動搬送車の軌道計算を行う、ＡＧＶ軌道計算装置を用いることで、自動搬送車を最適な軌道で目標位置姿勢にすることを可能とする。【選択図】図１
2017010292.tif 000002
【特許請求の範囲】
【請求項１】
自動搬送車の軌道計算を行い、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算装置であって、
前記目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、前記自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、前記自動搬送車の軌道計算を行う
ことを特徴とする、ＡＧＶ軌道計算装置。
【請求項２】
予め、前記自動搬送車の様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さを前記軌道の参照テーブルとして保存しておき、
前記自動搬送車の走行時に、前記参照テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
ことを特徴とする、請求項１に記載のＡＧＶ軌道計算装置。
【請求項３】
前記目標位置姿勢に到達可能な前記クロソイド曲線と前記直線との組み合わせを計算し、その際使用した前記クロソイド曲線のパラメータ及び前記直線の長さを前記参照テーブルとして保存しておく
ことを特徴とする、請求項２に記載のＡＧＶ軌道計算装置。
【請求項４】
前記目標位置姿勢のデータを入力する入力部と、
クロソイド曲線のパラメータ及び直線の長さの範囲を設定する設定部と、
前記設定部で設定された範囲で前記目標位置姿勢を始点とした様々な軌道を算出する計算部と、
コンフィギュレーション空間に対応する前記参照テーブルを用意し、前記計算部で算出される軌道が通る前記コンフィギュレーション空間上の各点に対応する前記参照テーブルに、前記計算部の軌道算出に用いた前記クロソイド曲線のパラメータ及び前記直線の長さを登録する登録部とを備えることで、
予め前記参照テーブルを作成する
ことを特徴とする、請求項３に記載のＡＧＶ軌道計算装置。
【請求項５】
前記自動搬送車の走行時の位置姿勢を入力する位置姿勢入力部を備え、
前記計算部は、
所定時刻毎に、前記位置姿勢入力部で入力された前記自動搬送車の走行時の位置姿勢において、予め前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さの組み合わせが存在する場合は、軌道計算を行い、存在しない場合は、より前記目標位置姿勢の近くまで前記自動搬送車を移動させ、前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さを用いて、軌道計算を行うことで、
前記自動搬送車の走行時に、前記参照テーブルを用いた前記自動搬送車の軌道計算を行う
ことを特徴とする、請求項４に記載のＡＧＶ軌道計算装置。
【請求項６】
前記登録部は、
前記参照テーブルに、前記クロソイド曲線のパラメータ及び前記直線の長さを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する
ことを特徴とする、請求項４又は５に記載のＡＧＶ軌道計算装置。
【請求項７】
対称単位クロソイド曲線テーブルを作成し、
前記対称単位クロソイド曲線テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
ことを特徴とする、請求項１に記載のＡＧＶ軌道計算装置。
【請求項８】
始点位置姿勢及び前記目標位置姿勢を入力する入力部と、
前記対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な前記対称単位クロソイド曲線テーブルを予め求める、対称単位クロソイド曲線テーブル計算部と、
前記始点位置姿勢、前記目標位置姿勢、及び、軌道計算で必要な前記対称単位クロソイド曲線テーブルを取り出し、前記始点と前記終点との間で軌道計算を行い、複数の軌道データを求める、軌道計算部と、
前記複数の軌道データの中から軌道を選択する軌道選択部とを備える
ことを特徴とする、請求項７に記載のＡＧＶ軌道計算装置。
【請求項９】
前記軌道計算部は、
幾何学的に中間点を設定し、前記始点と前記中間点、及び、前記中間点と前記終点との間に、対称単位クロソイド曲線を拡大又は縮小した対称クロソイド曲線、あるいは、直線を当てはめて、軌道計算を行う
ことを特徴とする、請求項８に記載のＡＧＶ軌道計算装置。
【請求項１０】
軌道選択部は、
予め設定された最大曲率及び軌道長に基づき、前記複数の軌道データの中から最適な軌道を選択する
ことを特徴とする、請求項８又は９に記載のＡＧＶ軌道計算装置。
【請求項１１】
自動搬送車の軌道を計算し、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算方法であって、
前記目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、前記自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、前記自動搬送車の軌道計算を行う
ことを特徴とする、ＡＧＶ軌道計算方法。
【請求項１２】
予め、前記自動搬送車の様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さを前記軌道の参照テーブルとして保存しておき、
前記自動搬送車の走行時に、前記参照テーブルを用いて目標位置姿勢で停止させる軌道を計算する
ことを特徴とする、請求項１１に記載のＡＧＶ軌道計算方法。
【請求項１３】
前記目標位置姿勢に到達可能な前記クロソイド曲線と前記直線との組み合わせを計算し、その際使用した前記クロソイド曲線のパラメータ及び前記直線の長さを前記参照テーブルとして保存しておく
ことを特徴とする、請求項１２に記載のＡＧＶ軌道計算方法。
【請求項１４】
前記目標位置姿勢のデータを入力し、
クロソイド曲線のパラメータ及び直線の長さの範囲を設定し、
設定された前記クロソイド曲線のパラメータ及び前記直線の長さの範囲で前記目標位置姿勢を始点とした様々な軌道を算出し、
コンフィギュレーション空間に対応する前記参照テーブルを用意し、算出された軌道が通る前記コンフィギュレーション空間上の各点に対応する前記参照テーブルに、軌道の算出に用いた前記クロソイド曲線のパラメータ及び前記直線の長さを登録することで、
予め前記参照テーブルを作成する
ことを特徴とする、請求項１３に記載のＡＧＶ軌道計算方法。
【請求項１５】
前記自動搬送車の走行時の位置姿勢を入力し、
所定時刻毎に、前記自動搬送車の走行時の位置姿勢において、予め登録された前記クロソイド曲線のパラメータ及び前記直線の長さの組み合わせが存在する場合は、軌道計算を行い、存在しない場合は、より前記目標位置姿勢の近くまで前記自動搬送車を移動させ、前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さを用いて、軌道計算を行うことで、
前記自動搬送車の走行時に、前記参照テーブルを用いた前記自動搬送車の軌道計算を行う
ことを特徴とする、請求項１４に記載のＡＧＶ軌道計算方法。
【請求項１６】
前記参照テーブルに、前記クロソイド曲線のパラメータ及び前記直線の長さを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する
ことを特徴とする、請求項１４又は１５に記載のＡＧＶ軌道計算方法。
【請求項１７】
対称単位クロソイド曲線テーブルを作成し、
前記対称単位クロソイド曲線テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
ことを特徴とする、請求項１１に記載のＡＧＶ軌道計算方法。
【請求項１８】
始点位置姿勢及び前記目標位置姿勢を入力し、
前記対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な前記対称単位クロソイド曲線テーブルを予め求め、
前記始点位置姿勢、前記目標位置姿勢、及び、軌道計算で必要な前記対称単位クロソイド曲線テーブルに基づき、前記始点と前記終点との間で軌道計算を行い、複数の軌道データを求め、
前記複数の軌道データの中から軌道を選択する
ことを特徴とする、請求項１７に記載のＡＧＶ軌道計算方法。
【請求項１９】
幾何学的に中間点を設定し、前記始点と前記中間点、及び、前記中間点と前記終点との間に、対称単位クロソイド曲線を拡大又は縮小した対称クロソイド曲線、あるいは、直線を当てはめて、軌道計算を行う
ことを特徴とする、請求項１８に記載のＡＧＶ軌道計算方法。
【請求項２０】
予め設定された最大曲率及び軌道長に基づき、前記複数の軌道データの中から最適な軌道を選択する
ことを特徴とする、請求項１８又は１９に記載のＡＧＶ軌道計算方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自動搬送車（以下、ＡＧＶ）を特定の位置及び姿勢（角度）（以下、位置姿勢）に移動させるためのＡＧＶの軌道を求める、ＡＧＶ軌道計算装置及び方法に関する。
【背景技術】
【０００２】
これまでのＡＧＶは、磁気テープを利用した移動がメインだったが、このようにすると磁気テープ上しか移動できないため、自律的に軌道を計算する手法が望まれている。
【０００３】
下記非特許文献１では、車両型ロボットの軌道計算として、位置情報だけでなく姿勢情報を考慮した（ｘ，ｙ，θ）の３次元空間を状態空間（コンフィギュレーション空間）として用いて、位置だけでなく姿勢も考慮した軌道計画を行っている。また、クロソイド曲線を導入することで、ロボットの操舵角が急激に変化しないよう考慮している。
【０００４】
下記特許文献１では、駐車支援のために、動作開始位置、動作開始姿勢（以下、開始位置姿勢）、停止位置、停止姿勢が定まった際に、１度の切り返しで駐車が可能か判断する手法、及び、１度の切り返しで駐車が可能であった場合の軌道計算を行う手法が提案されている。軌道計算手法については、最小回転半径に基づいた単一パラメータで制御されるクロソイド曲線とその前後の直線の組み合わせを用いている。
【０００５】
下記特許文献２では、道路形状をクロソイド曲線のフィッティングにより算出する手法が提案されている。クロソイド曲線の算出には道路の半径といった道路形状から統計的に近似するクロソイド曲線のパラメータを求める参照テーブルを用いた。
【０００６】
下記特許文献３では、位置情報と姿勢情報の３次元の状態空間（コンフィギュレーション空間）を使用して軌道計画を行っている。ポテンシャル法と呼ばれる手法を用いて、障害物を回避しながら到達目標位置及び到達目標姿勢（以下、目標位置姿勢）に到達する軌道を算出することができる。
【０００７】
下記特許文献４では、軌道を直線と曲線（ベジエ曲線、円弧、クロソイド曲線）の組み合わせで決定する搬送ロボットを考案している。
【先行技術文献】
【特許文献】
【０００８】
【特許文献１】特開２０１０‐１８０７４号公報
【特許文献２】特開２００５‐２１４８３９号公報
【特許文献３】特開２０１１‐８９３５４号公報
【特許文献４】特許第４８７７５２０号公報
【非特許文献】
【０００９】
【非特許文献１】項警宇，田崎勇一，鈴木達也，「車両型移動ロボットの走行安全性制約に基づく可変分解能状態グラフ地図生成」，第５５回自動制御連合講演会，Ｎｏｖ１７‐１８，２０１２  京都
【発明の概要】
【発明が解決しようとする課題】
【００１０】
上記非特許文献１の方法では、コンフィギュレーション空間を使って位置と姿勢の両方を考慮するだけでなく、滑らかな操舵角の変化に対応するためにクロソイド曲線を使用した軌道計算ができるが、開始位置姿勢及び目標位置姿勢によってその都度軌道計算が可能か算出する必要があるために計算コストが高く、また、最小回転半径を満たすクロソイド曲線で軌道計算を行うため、実現可能な軌道ではあるものの、なだらかに曲がればいいシーンであっても操舵角の変化における回転加速度の高い曲がり方（以下、負荷の高い曲がり方）をする可能性があるという問題がある。
【００１１】
上記特許文献１の方法においても、上記特許文献１と同様の問題があり、単一パラメータのクロソイド曲線しか扱っていないため、負荷の高い曲がり方をしてしまい、また、開始位置姿勢及び目標位置姿勢によってその都度軌道計算が可能か算出する必要があるために計算コストが高く、さらに、実現可能な開始位置姿勢と目標位置姿勢の組み合わせが少ないという問題がある。
【００１２】
上記特許文献２の方法では、様々な操舵角の回転加速度を持ったクロソイド曲線を算出することが可能であり、上記特許文献１，２のような問題は起こらないが、もともとクロソイド曲線形状をした道路の情報を用いてクロソイド曲線を近似する手法であり、そういった事前情報がない場合には対応できない。また、近似した曲線を用いるため特定の位置姿勢にという要求を満たすことが困難である。
【００１３】
上記特許文献３の方法では、コンフィギュレーション空間の考え方を利用しているが、軌道計算はクロソイド曲線を用いておらず、また軌道の回転半径も場合によって大きく変わるため、負荷の高い曲がり方も許容してしまっている。
【００１４】
上記特許文献４の方法では、直線と曲線の組み合わせ方の詳細について考慮されておらず、最適な軌道を計算する方法が不明である。
【００１５】
本発明は、上述のような技術的状況に鑑みてなされたものであり、ＡＧＶを最適な軌道で目標位置姿勢にすることを可能とする、ＡＧＶ軌道計算装置及び方法を提供することを目的とする。
【課題を解決するための手段】
【００１６】
上記課題を解決する第１の発明に係るＡＧＶ軌道計算装置は、
自動搬送車の軌道計算を行い、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算装置であって、
前記目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、前記自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、前記自動搬送車の軌道計算を行う
ことを特徴とする。
【００１７】
上記課題を解決する第２の発明に係るＡＧＶ軌道計算装置は、
上記第１の発明に係るＡＧＶ軌道計算装置において、
予め、前記自動搬送車の様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さを前記軌道の参照テーブルとして保存しておき、
前記自動搬送車の走行時に、前記参照テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
ことを特徴とする。
【００１８】
上記課題を解決する第３の発明に係るＡＧＶ軌道計算装置は、
上記第２の発明に係るＡＧＶ軌道計算装置において、
前記目標位置姿勢に到達可能な前記クロソイド曲線と前記直線との組み合わせを計算し、その際使用した前記クロソイド曲線のパラメータ及び前記直線の長さを前記参照テーブルとして保存しておく
ことを特徴とする。
【００１９】
上記課題を解決する第４の発明に係るＡＧＶ軌道計算装置は、
上記第３の発明に係るＡＧＶ軌道計算装置において、
前記目標位置姿勢のデータを入力する入力部と、
クロソイド曲線のパラメータ及び直線の長さの範囲を設定する設定部と、
前記設定部で設定された範囲で前記目標位置姿勢を始点とした様々な軌道を算出する計算部と、
コンフィギュレーション空間に対応する前記参照テーブルを用意し、前記計算部で算出される軌道が通る前記コンフィギュレーション空間上の各点に対応する前記参照テーブルに、前記計算部の軌道算出に用いた前記クロソイド曲線のパラメータ及び前記直線の長さを登録する登録部とを備えることで、
予め前記参照テーブルを作成する
ことを特徴とする。
【００２０】
上記課題を解決する第５の発明に係るＡＧＶ軌道計算装置は、
上記第４の発明に係るＡＧＶ軌道計算装置において、
前記自動搬送車の走行時の位置姿勢を入力する位置姿勢入力部を備え、
前記計算部は、
所定時刻毎に、前記位置姿勢入力部で入力された前記自動搬送車の走行時の位置姿勢において、予め前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さの組み合わせが存在する場合は、軌道計算を行い、存在しない場合は、より前記目標位置姿勢の近くまで前記自動搬送車を移動させ、前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さを用いて、軌道計算を行うことで、
前記自動搬送車の走行時に、前記参照テーブルを用いた前記自動搬送車の軌道計算を行う
ことを特徴とする。
【００２１】
上記課題を解決する第６の発明に係るＡＧＶ軌道計算装置は、
上記第４又は５の発明に係るＡＧＶ軌道計算装置において、
前記登録部は、
前記参照テーブルに、前記クロソイド曲線のパラメータ及び前記直線の長さを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する
ことを特徴とする。
【００２２】
上記課題を解決する第７の発明に係るＡＧＶ軌道計算装置は、
上記第１の発明に係るＡＧＶ軌道計算装置において、
対称単位クロソイド曲線テーブルを作成し、
前記対称単位クロソイド曲線テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
ことを特徴とする。
【００２３】
上記課題を解決する第８の発明に係るＡＧＶ軌道計算装置は、
上記第７の発明に係るＡＧＶ軌道計算装置において、
始点位置姿勢及び目標位置姿勢を入力する入力部と、
前記対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な前記対称単位クロソイド曲線テーブルを予め求める、対称単位クロソイド曲線テーブル計算部と、
前記始点位置姿勢、前記目標位置姿勢、及び、軌道計算で必要な前記対称単位クロソイド曲線テーブルを取り出し、前記始点と前記終点との間で軌道計算を行い、複数の軌道データを求める、軌道計算部と、
前記複数の軌道データの中から軌道を選択する軌道選択部とを備える
ことを特徴とする。
【００２４】
上記課題を解決する第９の発明に係るＡＧＶ軌道計算装置は、
上記第８の発明に係るＡＧＶ軌道計算装置において、
前記軌道計算部は、
幾何学的に中間点を設定し、前記始点と前記中間点、及び、前記中間点と前記終点との間に、対称単位クロソイド曲線を拡大又は縮小した対称クロソイド曲線、あるいは、直線を当てはめて、軌道計算を行う
ことを特徴とする。
【００２５】
上記課題を解決する第１０の発明に係るＡＧＶ軌道計算装置は、
上記第８又は９の発明に係るＡＧＶ軌道計算装置において、
軌道選択部は、
予め設定された最大曲率及び軌道長に基づき、前記複数の軌道データの中から最適な軌道を選択する
ことを特徴とする。
【００２６】
上記課題を解決する第１１の発明に係るＡＧＶ軌道計算方法は、
自動搬送車の軌道を計算し、所定の目標位置姿勢で停止させる制御を行う、ＡＧＶ軌道計算方法であって、
前記目標位置姿勢に基づき、クロソイド曲線を一部又は全部に用いて、前記自動搬送車の軌道についてのテーブルを作成し、該テーブルに基づき、前記自動搬送車の軌道計算を行う
ことを特徴とする。
【００２７】
上記課題を解決する第１２の発明に係るＡＧＶ軌道計算方法は、
上記第１１の発明に係るＡＧＶ軌道計算方法において、
予め、前記自動搬送車の様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さを前記軌道の参照テーブルとして保存しておき、
前記自動搬送車の走行時に、前記参照テーブルを用いて目標位置姿勢で停止させる軌道を計算する
ことを特徴とする。
【００２８】
上記課題を解決する第１３の発明に係るＡＧＶ軌道計算方法は、
上記第１２の発明に係るＡＧＶ軌道計算方法において、
前記目標位置姿勢に到達可能な前記クロソイド曲線と前記直線との組み合わせを計算し、その際使用した前記クロソイド曲線のパラメータ及び前記直線の長さを前記参照テーブルとして保存しておく
ことを特徴とする。
【００２９】
上記課題を解決する第１４の発明に係るＡＧＶ軌道計算方法は、
上記第１３の発明に係るＡＧＶ軌道計算方法において、
前記目標位置姿勢のデータを入力し、
クロソイド曲線のパラメータ及び直線の長さの範囲を設定し、
設定された前記クロソイド曲線のパラメータ及び前記直線の長さの範囲で前記目標位置姿勢を始点とした様々な軌道を算出し、
コンフィギュレーション空間に対応する前記参照テーブルを用意し、算出された軌道が通る前記コンフィギュレーション空間上の各点に対応する前記参照テーブルに、軌道の算出に用いた前記クロソイド曲線のパラメータ及び前記直線の長さを登録することで、
予め前記参照テーブルを作成する
ことを特徴とする。
【００３０】
上記課題を解決する第１５の発明に係るＡＧＶ軌道計算方法は、
上記第１４の発明に係るＡＧＶ軌道計算方法において、
前記自動搬送車の走行時の位置姿勢を入力し、
所定時刻毎に、前記自動搬送車の走行時の位置姿勢において、予め登録された前記クロソイド曲線のパラメータ及び前記直線の長さの組み合わせが存在する場合は、軌道計算を行い、存在しない場合は、より前記目標位置姿勢の近くまで前記自動搬送車を移動させ、前記参照テーブルに登録された前記クロソイド曲線のパラメータ及び前記直線の長さを用いて、軌道計算を行うことで、
前記自動搬送車の走行時に、前記参照テーブルを用いた前記自動搬送車の軌道計算を行う
ことを特徴とする。
【００３１】
上記課題を解決する第１６の発明に係るＡＧＶ軌道計算方法は、
上記第１４又は１５の発明に係るＡＧＶ軌道計算方法において、
前記参照テーブルに、前記クロソイド曲線のパラメータ及び前記直線の長さを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する
ことを特徴とする。
【００３２】
上記課題を解決する第１７の発明に係るＡＧＶ軌道計算方法は、
上記第１１の発明に係るＡＧＶ軌道計算方法において、
対称単位クロソイド曲線テーブルを作成し、
前記対称単位クロソイド曲線テーブルを用いて前記目標位置姿勢で停止させる軌道を計算する
ことを特徴とする。
【００３３】
上記課題を解決する第１８の発明に係るＡＧＶ軌道計算方法は、
上記第１７の発明に係るＡＧＶ軌道計算方法において、
始点位置姿勢及び前記目標位置姿勢を入力し、
前記対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な前記対称単位クロソイド曲線テーブルを予め求め、
前記始点位置姿勢、前記目標位置姿勢、及び、軌道計算で必要な前記対称単位クロソイド曲線テーブルに基づき、前記始点と前記終点との間で軌道計算を行い、複数の軌道データを求め、
前記複数の軌道データの中から軌道を選択する
ことを特徴とする。
【００３４】
上記課題を解決する第１９の発明に係るＡＧＶ軌道計算方法は、
上記第１８の発明に係るＡＧＶ軌道計算方法において、
幾何学的に中間点を設定し、前記始点と前記中間点、及び、前記中間点と前記終点との間に、対称単位クロソイド曲線を拡大又は縮小した対称クロソイド曲線、あるいは、直線を当てはめて、軌道計算を行う
ことを特徴とする。
【００３５】
上記課題を解決する第２０の発明に係るＡＧＶ軌道計算方法は、
上記第１８又は１９の発明に係るＡＧＶ軌道計算方法において、
予め設定された最大曲率及び軌道長に基づき、前記複数の軌道データの中から最適な軌道を選択する
ことを特徴とする。
【発明の効果】
【００３６】
本発明に係るＡＧＶ軌道計算装置及び方法によれば、ＡＧＶを最適な軌道で目標位置姿勢にすることができる。
【図面の簡単な説明】
【００３７】
【図１】ＡＧＶの概略図である。（ａ）は上面図、（ｂ）は側面図である。
【図２】ｘ‐ｙ座標上に描かれたクロソイド曲線の例を示すグラフである。
【図３】コンフィギュレーション空間において事前に計算した軌道の例を示すグラフである。
【図４】ＡＧＶの位置、姿勢、操舵角、及び、タイヤ間の距離の定義を説明するグラフである。
【図５】２点間を２つのクロソイド曲線で結んだ軌道例を示すグラフである。
【図６】本発明の実施例１に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。
【図７】本発明の実施例１に係るＡＧＶ軌道計算装置の「参照テーブル作成」についての手順を示すフローチャートである。
【図８】本発明の実施例１に係るＡＧＶ軌道計算装置の「参照テーブルのパラメータを用いた軌道計算」についての手順を示すフローチャートである。
【図９】対称クロソイド曲線のグラフである。
【図１０】対称クロソイド曲線＋直線の組み合わせによる軌道を示すグラフである。
【図１１】対称クロソイド曲線＋対称クロソイド曲線の組み合わせによる軌道を示すグラフである。
【図１２】始点の角度が１５０度の対称クロソイド曲線のグラフである。
【図１３】本発明の実施例２に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。
【図１４】本発明の実施例２に係るＡＧＶ軌道計算装置による基本的な軌道計算処理の全体のフローチャートである。
【図１５】接続角τまでの単位クロソイド曲線のグラフである。
【図１６】対称単位クロソイド曲線の生成を説明するグラフである。
【図１７】本発明の実施例２に係るＡＧＶ軌道計算装置による軌道計算を示すフローチャートである。
【図１８】絶対座標から相対座標への変換を説明するグラフである。
【図１９】単一の対称クロソイド曲線からなる軌道を説明するグラフである。
【図２０】単一の対称クロソイド＋直線からなる軌道を説明するグラフである。
【図２１】直線＋単一の対称クロソイドからなる軌道を説明するグラフである。
【図２２】二つの対称クロソイド曲線からなる軌道を説明するである。
【図２３】同じ始点‐終点間での複数の軌道例を示すグラフである。
【図２４】本発明の実施例３に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。
【図２５Ａ】本発明の実施例３に係るＡＧＶ軌道計算装置による軌道計算処理を示すフローチャートである。
【図２５Ｂ】本発明の実施例３に係るＡＧＶ軌道計算装置による軌道計算処理を示すフローチャートである。
【図２６】始点角及び終点角が大きい場合に計算された軌道の一例を示すグラフである。
【図２７】始点角及び終点角が大きい場合に計算された軌道の第１軌道〜第３軌道を示すグラフである。
【図２８】始点角及び終点角が大きい場合に計算された軌道の他の例を示すグラフである。
【発明を実施するための形態】
【００３８】
本発明に係るＡＧＶ軌道計算装置及び方法は、ＡＧＶを「特定の位置」に「特定の姿勢」で停止させるものである。以下、本発明に係るＡＧＶ軌道計算装置及び方法を実施例にて図面を用いて説明する。
【００３９】
［実施例１］
本発明の実施例１に係るＡＧＶ軌道計算装置及び方法について、図１〜８を用いて説明する。
【００４０】
図１（ａ）（ｂ）に示すように、前輪として配される可動輪２、後輪として配される従輪３、及び、可動輪２と従輪３との間に配される操舵輪としての駆動輪４を備えているＡＧＶ１の軌道を考える際は、ＡＧＶ１の駆動輪４を操舵輪とし、可動輪２を無視した車両と同様の操舵角を持つステアリング式と考えればよい。しかしながら、通常ＡＧＶは静止状態で操舵角を変化させる機構（パワーステアリング機能）を持たないため、回転半径が急激に変化するような軌道には対応できない。
【００４１】
そこで、滑らかな操舵角の変化で実現できる軌道が必要になるが、この軌道算出は、計算コストが非常に高く、リアルタイム軌道計算が困難であるという問題がある。
【００４２】
さらには、「ベルトコンベアの付近に決められた姿勢でＡＧＶを設置する」といった要求があった場合、位置情報だけでなく、姿勢情報まで考慮した軌道計算を行う必要があり、これにはより多くの計算コストを必要とする。
【００４３】
ここで、上述の「特定の位置」及び「特定の姿勢」（目標位置姿勢）が既知であり、ＡＧＶが移動する空間の地図も既知であるという条件で、ＡＧＶの動作開始位置姿勢は未知であるという状態での軌道計算を考える。このとき、「ＡＧＶの移動モデル」、「ＡＧＶの（位置に加えて）姿勢」、さらに、「ＡＧＶが移動しやすい軌道」について、それぞれ考慮する必要がある。
【００４４】
まず、「ＡＧＶの移動モデル」としては、車両と同様の操舵輪を持つステアリング式と考えればよい。しかしながら、ＡＧＶは上述のごとく、回転半径が急激に変化するような軌道には対応できないことに留意する必要がある。
【００４５】
次に、「ＡＧＶの姿勢」については、上記非特許文献１及び上記特許文献３にあるコンフィギュレーション空間を用いることで解決できる。
【００４６】
さらに、「ＡＧＶが移動しやすい軌道」については、操舵角が滑らかに変化する（等回転加速度で変化する）場合の軌道であるクロソイド曲線を用いることで解決できる。
【００４７】
ただし、ＡＧＶの負担を軽減するために、実現可能な最速の操舵角の回転加速度よりも、ゆっくりした操舵角の回転加速度の軌道を優先する。
【００４８】
クロソイド曲線が有するパラメータは「回転加速度」及び「曲線長」である。この回転加速度の大小によって、急な曲りの曲線になるか、なだらかな曲りの曲線になるかが決定し、曲線長によって、どこまで曲がり続けるかが決まる（曲線長を非常に長く設定すると、螺旋のような曲線を描く）。
【００４９】
ｘ‐ｙ座標上に描かれたクロソイド曲線の例を図２に示す。クロソイド曲線は、三角関数の積分を数式内に持つことが知られており、回転加速度は三角関数の周期を、曲線長は積分区間を、それぞれ変化させるパラメータである。
【００５０】
クロソイド曲線にはこのような複雑な数式を用いるため、クロソイド曲線を使った軌道計算というのは解析的に求めることができず、計算コストの高い手法が必要となる。既存の手法・発明では、回転加速度を固定したり、曲線長のとれる範囲を限定したりするなどして、計算コストを下げているが、それでは最適な軌道とは言えず、上述したように問題がある。
【００５１】
そこで、本実施例では、目標位置姿勢が事前に分かっていることを利用し、目標位置姿勢に到達可能な様々なクロソイド曲線と直線の組み合わせを事前に計算し、その際使用したクロソイド曲線のパラメータと直線の長さを、参照テーブルのパラメータとして保存しておくことで、リアルタイム軌道計算を実現する。
【００５２】
コンフィギュレーション空間における、事前に計算した軌道の例を図３に示す。コンフィギュレーション空間は、適当に選択した座標系での並進運動ｘ，ｙと姿勢θとで張られる３次元パラメータ空間（ｘ，ｙ，θ）である。図中の●は目標位置姿勢、×，▽，■は開始位置姿勢、破線は軌道である。
【００５３】
以下、本実施例における具体的なＡＧＶの軌道計算について説明する。
【００５４】
まずＡＧＶは、直線運動時を除き、等速運動、等回転加速度運動すると仮定する。図４に示すように、この場合のＡＧＶ１の位置を（ｘ，ｙ）、姿勢をθ、速度をｖ、旋回角速度をω、旋回加速度をα、操舵角をξ、タイヤ間（操舵輪である可動輪２と従輪３との間）の距離をＬとする。
【００５５】
回転角速度ωと操舵角ξとの間に線形の関係が存在すると仮定すると、下記（１）式のようになる。
【００５６】
【数１】
2017010292.tif 000003
【００５７】
このような条件の場合、想定するＡＧＶ１の軌道（直線運動部分を除く）は、時間ｔを用いて下記（２）式のように計算できる。
【００５８】
【数２】
2017010292.tif 000004
【００５９】
ここで、Ｃ（（α／π）1/2ｔ）及びＳ（（α／π）1/2ｔ）はフレネル積分である。また、ＡＧＶ１の旋回開始時刻をｔ＝０とすると、時刻ｔ＝０のときの位置姿勢（ｘ（０），ｙ（０），θ（０））及び操舵角ξは全て０である。このクロソイド曲線は曲率が時刻ｔと共に小さくなる。
【００６０】
実際に軌道計算をする際には、この曲率が小さくなるクロソイド曲線と曲率が大きくなるクロソイド曲線とを組み合わせてＡＧＶ１の位置姿勢（ｘ（ｔ），ｙ（ｔ），θ（ｔ））を制御する。
【００６１】
図５に示す状況であれば、下記（３）式によってＡＧＶ１の軌道が描かれる。ただし、旋回開始時（ｔ＝０）と旋回終了時の操舵角ξは０度であり、姿勢がθから−θに変わる状況を考える。また、ＡＧＶ１が点ｐ（旋回開始位置）から点ｐ´（旋回終了位置）まで辿り着くのにかかる所要時間をＴ、途中の中間点のＡ点まで辿り着くのにかかる所要時間をＴAとする。
【００６２】
【数３】
2017010292.tif 000005
【００６３】
ここでは、ＡＧＶ１の姿勢がθsからθeに変わる、つまり、θs＞０かつθe＜０の場合を例として考えるが、実際には、他にもθs＞０かつθe＞０、θs＜０かつθe＞０、θs＜０かつθe＜０の場合が存在し、その都度異なるクロソイド曲線の組み合わせが必要な点に留意する。
【００６４】
実際に上記（３）式などを用いて軌道計算する際には、ＡＧＶ１の目標位置姿勢及び速度ｖは固定し、回転加速度及び曲線長については取り得る様々な値で複数の軌道を作成する。また、クロソイド曲線の前後に直線を組み合わせることで、様々な軌道に対応する。
【００６５】
これらの軌道の最中の回転加速度、時刻、及び、クロソイド曲線の前後の直線長（直線の長さ）を、コンフィギュレーション空間の暫定到達点にパラメータとして保存しておき、実際のＡＧＶ１が目標位置付近に到達した際に位置姿勢パラメータを用いて最も近い暫定到達点を算出する。
【００６６】
算出された暫定到達点から、上述のごとく登録されている回転加速度、時刻、及び、クロソイド曲線の前後の直線長を使い、目標位置姿勢に向かって、上述の式で求められる軌道で移動する。
【００６７】
参照テーブルへのパラメータ登録時に、同じ位置姿勢を通る軌道は複数存在する場合も想定されるが、その場合は、回転加速度の低い軌道を優先的に登録する。
【００６８】
これにより、可能な限りＡＧＶ１に負荷のかからない（つまり操舵角ξの回転角加速度が低い）滑らかな曲線を使用した軌道計算が実現できる。
【００６９】
本実施例に係るＡＧＶ軌道計算装置は、大きく分けて、事前に計算しておく「参照テーブルの作成」と、実際のリアルタイム軌道計算時の「参照テーブルのパラメータを用いた軌道計算」とに分けることができる。なお、実際のリアルタイム軌道計算とは、ＡＧＶ１の走行中の軌道計算時という意味である。
【００７０】
図６は、本実施例に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。なお、図６では、参照テーブル作成の手順と参照テーブルのパラメータを用いた軌道計算の手順とで分けて装置構成を表しており、２つの手順に同一の構成要件を用いる箇所は、同一の構成要件を２つのブロックに分割して模式的に表している。
【００７１】
本実施例に係るＡＧＶ軌道計算装置は、図６に示すように、記憶部１０、地図・目標位置姿勢入力部１１、パラメータ設定部１２、パラメータ取得部１３、軌道計算部１４、参照テーブル登録部１５、参照テーブル部１６、及び、ＡＧＶ位置姿勢入力部１７を備えている。
【００７２】
地図・目標位置姿勢入力部１１は、ＡＧＶが移動する範囲の環境地図と目標位置姿勢を指定したデータを入力し、記憶部１０へ保管する。
【００７３】
参照テーブル登録部１５は、コンフィギュレーション空間に対応する参照テーブルを参照テーブル部１６に用意する。また、参照テーブル登録部１５は、ある軌道のある地点におけるパラメータを、対応する参照テーブルに保管する。すなわち、後述の軌道計算部１４で算出される軌道が通るコンフィギュレーション空間上の各点に対応する参照テーブルに、軌道計算部１４の軌道算出に用いたパラメータを登録する。
【００７４】
また、参照テーブル登録部１５は、参照テーブルに、軌道計算部１４の軌道算出に用いたパラメータを登録する際に、同じ位置姿勢地点で通る軌道が複数存在する場合は、回転加速度が低い軌道を優先的に登録する。
【００７５】
パラメータ設定部１２では、クロソイド曲線のパラメータである回転加速度と曲線長、及びそれらのパラメータの離散化幅を設定し、それらの組み合わせを記憶部１０に保管する。さらに、直線長の範囲も設定し記憶部１０に保管する。
【００７６】
パラメータ取得部１３では、記憶部１０及び参照テーブル部１６に保管された各パラメータを適宜抽出する。すなわち、参照テーブル作成を行う場合には、記憶部１０に保管された各パラメータを抽出して参照テーブル登録部１５に入力し、参照テーブルのパラメータを用いた軌道計算を行う場合には、参照テーブル部１６に保管された参照テーブルを抽出して軌道計算部１４に入力する。
【００７７】
軌道計算部１４は、参照テーブル作成を行う場合には、パラメータ設定部１２から記憶部１０に保管されたパラメータ範囲で目標位置姿勢を始点とした様々な軌道を算出する。
【００７８】
また、軌道計算部１４は、参照テーブルのパラメータを用いた軌道計算を行う場合には、まず、ＡＧＶ位置姿勢入力部１７から記憶部１０に保管されたＡＧＶの現在の位置姿勢において参照テーブルに登録されたパラメータがあるか否か調査する。パラメータが存在した場合は、軌道計算を行う。存在しなかった場合は、より目標位置姿勢の近くまでＡＧＶを移動させる。さらにその後、軌道計算部１４は、参照テーブルを参照した位置姿勢から登録されていたパラメータを使用して軌道計算を行う。そして、計算結果に基づきＡＧＶを制御する。軌道計算部１４は、これらの処理を所定時刻毎に行う。
【００７９】
すなわち、軌道計算部１４では、パラメータ取得部１３によって入力されたパラメータ、目的位置姿勢、若しくは、ＡＧＶ位置姿勢入力部１７によって入力された現在の位置姿勢を使用して、所定時刻毎の位置姿勢を計算し、計算結果に基づきＡＧＶを制御する。
【００８０】
記憶部１０では、クロソイド曲線パラメータ、直線長、上記環境地図、開始位置姿勢、及び、目標位置姿勢などのデータを保管する。
【００８１】
参照テーブル部１６では、参照テーブル登録部１５により入力された参照テーブル及びパラメータを保管する。記憶部１０は処理毎に初期化されたものを使用するのに対し、参照テーブルは一度登録したものを何度も使用するため、参照テーブル部１６は別の枠として確保している。
【００８２】
ＡＧＶ位置姿勢入力部１７では、ＡＧＶの現在の位置姿勢を入力し記憶部１０に保管する。
【００８３】
以上が、本実施例に係るＡＧＶ軌道計算装置の装置構成の説明である。以下、本実施例に係るＡＧＶ軌道計算装置の手順について、図７，８を用いて説明する。
【００８４】
図７は、「参照テーブル作成」についての手順を示すフローチャートである。
【００８５】
ステップＳ１では、地図・目標位置姿勢入力部１１により、ＡＧＶが移動する範囲の環境地図、及び、ＡＧＶの目標位置姿勢を入力する。
【００８６】
ステップＳ２では、参照テーブル登録部１５により、コンフィギュレーション空間に対応する参照テーブルを用意する。用意された参照テーブルは参照テーブル部１６に保管される。
【００８７】
ステップＳ３では、パラメータ設定部１２により、クロソイド曲線のパラメータ、直線の長さの範囲を設定する。これらは、記憶部１０に保管される。
【００８８】
ステップＳ４‐１，Ｓ４‐２では、軌道算出部１４により、ステップＳ３のパラメータの範囲で目標位置姿勢を始点とした様々な軌道を算出する。なお、このパラメータはパラメータ取得部１３により記憶部１０から抽出される。ステップＳ４‐１では、パラメータがステップＳ３の範囲内であるか否かを判断し、範囲内であればステップＳ４‐２において軌道を算出し、範囲内でなければ処理を終了する。
【００８９】
ステップＳ５では、ステップＳ４‐２で算出された軌道が通るコンフィギュレーション空間上の各点に対応する参照テーブルに、ステップＳ４‐２の軌道で用いたパラメータを登録する。
【００９０】
このようにして、ステップＳ１〜Ｓ５では、予め、ＡＧＶの様々な軌道に対する、クロソイド曲線のパラメータ及び直線の長さをＡＧＶの軌道の参照テーブルのパラメータとして保存しておく。より詳しくは、目標位置姿勢に到達可能なクロソイド曲線と直線との組み合わせを計算し、その際使用したクロソイド曲線のパラメータ及び直線の長さを参照テーブルとして保存しておく。
【００９１】
図８は、「参照テーブルのパラメータを用いた軌道計算」についての手順を示すフローチャートである。
【００９２】
ステップＳ１１では、地図・目標位置姿勢入力部１１により、ＡＧＶが移動する範囲の環境地図、及び、ＡＧＶの目標位置姿勢を入力する。
【００９３】
ステップＳ１２では、パラメータ取得部１３により、参照テーブル部１６からステップＳ１〜Ｓ５によって作成された参照テーブルを参照テーブル部１６から抽出し、軌道計算部１４に入力する。
【００９４】
ステップＳ１３では、ＡＧＶ位置姿勢入力部１７により、ＡＧＶの現在の位置姿勢を入力する。入力された現在の位置姿勢のデータは記憶部１０に保管される。
【００９５】
ステップＳ１４‐１では、軌道計算部１４により、パラメータ取得部１３及びＡＧＶ位置姿勢入力部１７からのデータに基づき、ＡＧＶの現在の位置姿勢において、参照テーブルに登録されたパラメータがあるか否かを調査する。あればステップＳ１５に、なければステップＳ１４‐２に移行する。
【００９６】
ステップＳ１４‐２では、ＡＧＶの移動を行う。ステップＳ１４‐２終了後、ステップＳ１３に移行する。
【００９７】
ステップＳ１５では、参照テーブルを参照した位置姿勢から登録されていたパラメータを使用して軌道計算を行い、計算結果に基づきＡＧＶの制御を行う。
【００９８】
このようにして、ステップＳ１１〜Ｓ１５では、ＡＧＶの走行時に、参照テーブルを用いて目標位置姿勢で停止させる軌道を計算し、ＡＧＶの制御を行う。
【００９９】
本実施例に係るＡＧＶ軌道計算装置及び方法によれば、ＡＧＶを滑らかな操舵角変化で移動可能な軌道を作成できる。
【０１００】
また、本実施例に係るＡＧＶ軌道計算装置及び方法によれば、目標位置姿勢に到達可能な様々なクロソイド曲線と直線の組み合わせを事前に計算し、その際使用したクロソイド曲線のパラメータと直線の長さを、参照テーブルとして保存しておくことで、リアルタイム軌道計算が可能である。さらに、磁気テープ等を必要としない自律的な軌道計算が可能であり、開始位置姿勢及び目標位置姿勢の組み合わせを多く取ることも可能であり、特定の位置姿勢に高精度に到達可能な軌道計算が可能である。
【０１０１】
また、本実施例に係るＡＧＶ軌道計算装置及び方法によれば、参照テーブルへのパラメータ登録時に、回転加速度の低い軌道を優先的に登録することで、ＡＧＶになるべく負荷のかからない軌道計算、すなわち、滑らかな曲線を使用した軌道計算を行うことができる。
【０１０２】
［実施例２］
本実施例では、実施例１のようにある程度時間をかけて目的位置姿勢毎に参照テーブルを作成するのではなく、一つの対称単位クロソイド曲線テーブルの作成だけで、様々な始点及び終点の位置姿勢に対応することで、処理時間を短縮することを可能とするものである。
【０１０３】
すなわち、本実施例に係るＡＧＶ軌道計算装置及び方法は、対称単位クロソイド曲線テーブルを作成し、この対称単位クロソイド曲線テーブルを用いて目標位置姿勢で停止させる軌道を計算するものである。
【０１０４】
なお、本実施例及び実施例３では、ＡＧＶの動作開始位置を始点、到達目標位置を終点と呼称し、始点の位置姿勢を始点位置姿勢、終点の位置姿勢を終点位置姿勢と呼称する。始点位置姿勢及び終点位置姿勢は、実施例１における開始位置姿勢及び目標位置姿勢と同義である。
【０１０５】
以下、本実施例に係るＡＧＶ軌道計算装置及び方法について、図９〜２３を用いて説明する。
【０１０６】
クロソイド曲線を使って軌道を計算する場合、直線・円弧を含めると以下の組み合わせが考えられる。
・直線とクロソイド曲線（曲率０の端点）との接続
・曲率χの円弧とクロソイド曲線（曲率χの端点）との接続
・接続する端点の曲率は同じだが、曲率変化率の異なる２つのクロソイド曲線
【０１０７】
以上、全てを考慮して始点・終点の軌道を計算すると、組み合わせの量が非常に多くなりすぎるため、以下の条件を設定する。
【０１０８】
まず、図９に示すように、基本となるクロソイド曲線は左右対称とする。これを対称クロソイド曲線と呼ぶ。始点と終点とを結ぶ直線に対し、始点での角度はθ、終点での角度は−θで共に曲率は０である。また、始点から中点までの曲率変化率をαとして、中点での曲率が最大（角度は上記直線に対し０）となり、中点から終点までの曲率変化率が−αとなる（ここで、曲率変化率αは、始点の曲率をχs、中点の曲率をχm、始点‐中点間の曲線長をｌとしたときに、α＝（χm−χs）／ｌとしている）。
【０１０９】
次に、上記対称クロソイド曲線のみで表現できない場合（始点角をθs、終点角度をθeとしたときにθs≠−θe）は、図１０に示すように、対称クロソイド曲線＋直線、又は、直線＋対称クロソイド曲線の組み合わせ、あるいは、図１１に示すように、対称クロソイド曲線＋対称クロソイド曲線の組み合わせ（二つの対称クロソイド曲線）により、軌道を表現する。
【０１１０】
また、対称クロソイド曲線は、始点の角度を大きくしていくと、図１２に示すように、ある時点でループ状の形状となる（図１２は、始点の角度が１５０度の場合）。単純化のため、ループ状にならない角度（約１３１．６度まで）での対称クロソイド曲線を使用する。
【０１１１】
以上の条件の下、以下で説明する方法により対称クロソイド曲線を用いた軌道を複数計算する。その際、中間点がある場合は、再帰的に、始点と中間点、中間点と終点について、改めて以下の方法で軌道を計算する。なお、中間点とは、軌道の経由点であり、かつ、クロソイド曲線と直線あるいは２つの曲線同士の接続点である。
【０１１２】
図１３は、本実施例に係るＡＧＶ軌道計算装置の装置構成を示すブロック図である。本実施例に係るＡＧＶ軌道計算装置は、記憶部２０、始点・終点位置姿勢入力部２１、パラメータ設定部２２、対称単位クロソイド曲線テーブル計算部２３、軌道計算部２４、軌道選択部２５、及び、軌道出力部２６を備える。
【０１１３】
始点・終点位置姿勢入力部２１は、動作を開始する始点の位置座標と姿勢（始点位置姿勢）、及び、目標地点である終点の位置座標と姿勢（終点位置姿勢）を、入力（設定）する。入力された始点位置姿勢及び終点位置姿勢は、記憶部２０に保管される。
【０１１４】
パラメータ設定部２２は、対称単位クロソイド曲線（後述）テーブル作成時の角度分解能、及び、軌道の最大曲率などのパラメータを設定する。設定されたパラメータは記憶部２０に保管される。
【０１１５】
記憶部２０は、始点位置姿勢、終点位置姿勢、パラメータ設定部２２により設定されたパラメータ、対称単位クロソイド曲線テーブル、及び、軌道などのデータを保管する。
【０１１６】
対称単位クロソイド曲線テーブル計算部２３は、対称単位クロソイド曲線テーブルの作成を行う。すなわち、対称単位クロソイド曲線を角度毎に計算し、軌道計算で必要な対称単位クロソイド曲線テーブルを予め求め、記憶部２０に保管する。
【０１１７】
軌道計算部２４は、軌道計算を行う。すなわち、記憶部２０から始点位置姿勢、終点位置姿勢、及び、軌道計算で必要な対称単位クロソイド曲線テーブルを取り出し、始点‐終点間の軌道計算を行って軌道リスト（複数の軌道テータ）を求め、軌道選択部２５へ送る。
【０１１８】
軌道選択部２５は、軌道の選択を行う。すなわち、パラメータ設定部２２により設定されたパラメータを記憶部２０から読み込み、軌道計算部２４により作成され送られてきた軌道リスト（複数の軌道のデータ）の中から軌道を選択し、記憶部２０に保管する。
【０１１９】
軌道出力部２６は、得られた軌道を記憶部２０から取り出し、外部へ出力する。
【０１２０】
本実施例に係るＡＧＶ軌道計算装置及び方法の基本的な軌道計算処理の全体のフローチャートを図１４に示す。
【０１２１】
ステップＳ２１では、対称単位クロソイド曲線テーブル計算部２３によって、対称単位クロソイド曲線テーブルの作成を行う。すなわち、軌道計算で用いるクロソイド曲線を予め角度毎に計算しておき、曲線テーブルとして作成する。
【０１２２】
ステップＳ２２では、始点・終点位置姿勢入力部２１によって、始点位置姿勢の設定を行う。
【０１２３】
ステップＳ２３では、始点・終点位置姿勢入力部２１によって、終点位置姿勢の設定を行う。
【０１２４】
ステップＳ２４では、軌道計算部２４によって軌道計算を行う。すなわち、入力した始点位置姿勢及び終点位置姿勢に基づき、始点‐終点間の軌道を計算する。
【０１２５】
ステップＳ２５では、軌道選択部２５によって軌道の選択を行う。すなわち、ステップＳ２６の軌道計算では、始点‐終点間に複数の軌道が作成されるが、その複数の軌道から１つを選択する。
【０１２６】
以下、上記ステップＳ２１〜Ｓ２５のそれぞれの処理の詳細を説明する。
【０１２７】
［ステップＳ２１について］
クロソイド曲線を求めるためには、下記式（４）のとおり積分を計算する必要がある。なお、下記式（４）において、Ｌは曲線長、τは曲線の接線角、Ａはスケール（拡大率）を表すクロソイドパラメータである。
【０１２８】
【数４】
2017010292.tif 000006
【０１２９】
軌道を計算する度に積分計算を行うと時間がかかってしまうため、対称単位クロソイド曲線テーブル計算部２３によって、予め必要な角度毎に対称単位クロソイド曲線を計算したものをテーブル（対称単位クロソイド曲線テーブル）として用意し、軌道計算時には必要な角度に対応する対称単位クロソイド曲線をテーブルから参照して拡大あるいは縮小して適用する。なお、クロソイドパラメータＡを１としたときのクロソイド曲線を単位クロソイド曲線といい、それを対称にしたものを対称単位クロソイド曲線という。
【０１３０】
始点角度τの対称単位クロソイド曲線を求めるには、図１５に示すとおり、まず上記式（４）（ただしＡ＝１）を用いて接続角がτになるところまでの単位クロソイド曲線を計算する。次に図１６のようにｘ軸で反転、τの回転、ｘ´中心に反転複製を経ることで対称単位クロソイド曲線を生成することができる。
【０１３１】
このときの対称単位クロソイド曲線の各値は下記のとおりとなる。
・始点角度：τ
・始点‐終点間距離：２ｘ´
・始点‐終点間曲線長：２ｌτ＝２（２τ）1/2
・ピーク座標：（ｘ´，ｙ´）
・最大曲率：ｌτ＝（２τ）1/2
・曲率変化率：１
以上の値を始点角度毎に対称単位クロソイド曲線テーブルに曲線の座標と共に保持する。
【０１３２】
また、参考用として、クロソイドパラメータＡでスケールを変化させた対称クロソイド曲線の各値は以下のとおりとなる。
・始点角度：τ
・始点‐終点間距離：２Ａｘ´
・始点‐終点間曲線長：２Ａｌτ＝２Ａ（２τ）1/2
・ピーク座標：（Ａｘ´，Ａｙ´）
・最大曲率：ｌτ／Ａ＝（２τ）1/2／Ａ
・曲率変化率：１／Ａ2
【０１３３】
［ステップＳ２２，Ｓ２３について］
始点・終点位置姿勢入力部２１によって、始点位置姿勢（ｘ´s，ｙ´s，θ´s）、及び、終点位置姿勢（ｘ´e，ｙ´e，θ´e）を設定する。
【０１３４】
［ステップＳ２４について］
ここで、軌道計算部２４によって、始点‐終点間の軌道を計算する。この軌道計算のフローチャートを図１７に示す。この軌道計算の処理は、始点及び終点での角度によって処理が分岐し、大まかには以下の軌道計算処理が行われる。そしてこの処理により、始点・終点間に一つだけでなく複数の軌道（下記ａ，ｂ，ｃ）が計算される。
ａ．一つの対称クロソイド曲線による軌道
ｂ．一つの対称クロソイド曲線と直線による軌道
ｃ．二つの対称クロソイド曲線による軌道
【０１３５】
なお、任意の始点位置姿勢及び終点位置姿勢に対して軌道計算をする際、簡略化のため、図１８に示すように、絶対座標から、始点位置を原点とし終点をｘ軸上に置く相対座標に変換する。これにより、始点位置姿勢は（０，０，θs）（ただし、θs＝θ´s−θr）、終点位置姿勢は（ｘe，０，θe）（ただし、ｘe＝（ｘ´e−ｘ´s）2＋（ｙ´e−ｙ´s）2）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）2＋ｙc2）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を用いて軌道計算を再帰的に呼び出して、上記ステップＳ２４‐２，Ｓ２４‐３の方法で対称クロソイド曲線部分の軌道計算を行う。
【０１５７】
対称クロソイド曲線の曲線長をｌ、最大曲率をχとしたとき、以上の直線＋対称クロソイド曲線による軌道の長さは、ｌ＋（ｘc2＋ｙc2）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を底辺とし、かつ、ｍ1ｐcｍ2を直線とする二つの二等辺三角形ｍ1ｐsｐc及びｍ2ｐcｐeを作成することができる。
【０１６３】
まず、二等辺三角形ｍ1ｐsｐcのうち、角度θc1は、θs及びθmから
θc1＝（θs−θm）／２                                       （１２）
として求めることができる。
【０１６４】
二等辺三角形ｍ2ｐ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）を設定し二つの対称クロソイド曲線から軌道を求めることができる。
【０２０３】
本実施例に係るＡＧＶ軌道計算装置及び方法によれば、始点角及び終点角が大きく開いた場合でも、幾何学的な計算を用いることで、コンパクトな経路を計算することができる。
【産業上の利用可能性】
【０２０４】
本発明は、ＡＧＶ軌道計算装置及び方法として好適である。
【符号の説明】
【０２０５】
１  ＡＧＶ（自動搬送車）
２  可動輪
３  従輪
４  駆動輪
１０，２０  記憶部
１１  地図・目標位置姿勢入力部
１２  パラメータ入力部
１３  パラメータ取得部
１４，２４，３４  軌道計算部
１５  参照テーブル登録部
１６  参照テーブル部
１７  ＡＧＶ位置姿勢入力部
２１  始点・終点位置姿勢入力部
２２  パラメータ設定部
２３  対称単位クロソイド曲線テーブル計算部
２５  軌道選択部
２６  軌道出力部
【図１】
2017010292.tif 000007
【図２】
2017010292.tif 000008
【図３】
2017010292.tif 000009
【図４】
2017010292.tif 000010
【図５】
2017010292.tif 000011
【図６】
2017010292.tif 000012
【図７】
2017010292.tif 000013
【図８】
2017010292.tif 000014
【図９】
2017010292.tif 000015
【図１０】
2017010292.tif 000016
【図１１】
2017010292.tif 000017
【図１２】
2017010292.tif 000018
【図１３】
2017010292.tif 000019
【図１４】
2017010292.tif 000020
【図１５】
2017010292.tif 000021
【図１６】
2017010292.tif 000022
【図１７】
2017010292.tif 000023
【図１８】
2017010292.tif 000024
【図１９】
2017010292.tif 000025
【図２０】
2017010292.tif 000026
【図２１】
2017010292.tif 000027
【図２２】
2017010292.tif 000028
【図２３】
2017010292.tif 000029
【図２４】
2017010292.tif 000030
【図２５Ａ】
2017010292.tif 000031
【図２５Ｂ】
2017010292.tif 000032
【図２６】
2017010292.tif 000033
【図２７】
2017010292.tif 000034
【図２８】
2017010292.tif 000035