JP5859093B1 - 軌道追従制御装置 - Google Patents

Abstract

【課題】車輪式の移動体において、前進と後退のどちらの場合でも目標軌道への追従を実現する軌道追従制御装置を得る。【解決手段】目標軌道生成部１０と、移動体１３の状態検出部１２と、目標軌道生成部１０と状態検出部１２の出力に基づき軌道追従誤差を計算する誤差計算部１１と、誤差計算部１１の出力に基づき移動体１３への指令値を計算する指令値計算部１４からなる軌道追従制御装置において、姿勢角度誤差θｅを目標姿勢角度θｒから移動体１３の姿勢角度θｃを差し引いたものとし、指令値計算部１４において、前記指令値の計算に用いるｓｉｎθｅの係数を常に正値とする。【選択図】図１

Description

この発明は、車輪式の移動体を目標軌道に追従させるための軌道追従制御装置に関するものである。

予め設定された目標軌道、もしくは周辺環境情報の取得によりオンラインで生成された軌道に移動体を追従させる技術は、自動車の自動運転あるいは自動駐車や、自律移動ロボットに用いられる。このような移動体の多くでは移動方式として車輪が用いられるが、車輪は横方向に移動できないことから非ホロノミック拘束を有する制御対象となる。非ホロノミック拘束を有する制御対象の軌道追従制御において、従来の方法は複雑なものが多い一方、ＩＥＥＥ ＩＣＲＡ， ｐｐ．３８４−３８９，１９９０．（非特許文献１）ではシンプルな方法が提案され、理論的に目標軌道への追従を保証するとともに、実験においてもその有効性を示している。

さらに、特開２００６ー９２４２４号公報（特許文献１）では、非特許文献１の方法に対して横方向速度指令値を追加することで追従性能をさらに向上させることを提案している。

Ｋａｎａｙａｍａ,Ｙ．, ｅｔ ａｌ．， Ａ Ｓｔａｂｌｅ Ｔｒａｃｋｉｎｇ Ｃｏｎｔｒｏｌ Ｍｅｔｈｏｄ ｆｏｒ ａｎ Ａｕｔｏｎｏｍｏｕｓ Ｍｏｂｉｌｅ Ｒｏｂｏｔ， ＩＥＥＥ ＩＣＲＡ， ｐｐ．３８４−３８９，１９９０．

特開２００６−９２４２４号公報

しかしながら、前記非特許文献１の方法では移動体の目標速度が正値（前進）のときのみ目標軌道への追従を保証するため、目標速度を負値（後退）とする軌道追従制御は実現できない問題があった。移動体の後方向を速度が正の方向として座標系を取り直して非特許文献１の方法を適用することはできるが、この方法では移動したい方向が前方か後方かによって座標系を切り替えて移動体への指令値を計算しなければならない問題がある。

また、特許文献１に開示された方法における横方向速度指令には４輪操舵車両であれば従うことができるが、一般的な前輪操舵の自動車は横方向に移動的できないため適用できない問題があった。

この発明は、前記のような問題を解決するためになされたものであり、一般的な前輪操舵の自動車を含めた車輪式の移動体において、前進と後退のどちらの場合でも目標軌道への追従を実現する軌道追従制御装置を得ることを目的とするものである。

この発明に係る軌道追従制御装置は、目標軌道に追従させて移動体の移動を制御する軌道追従制御装置であって、目標速度、目標回転速度、目標位置、目標姿勢角度を生成する目標軌道生成部と、前記移動体の位置と姿勢角度を検出する状態検出部と、前記目標軌道生成部と前記状態検出部の出力に基づき軌道追従誤差を計算する誤差計算部と、前記誤差計算部の出力に基づき前記移動体への指令値を計算する指令値計算部と、を備え、
姿勢角度誤差を目標姿勢角度から前記移動体の姿勢角度を差し引いたものとし、前記指令値計算部において、前記指令値の計算に用いる前記姿勢角度誤差の正弦値、もしくは前記姿勢角度誤差の正弦値をテイラー展開で近似したものの係数を常に正値とするものである。

この発明に係る軌道追従制御装置によれば、目標速度、目標回転速度、目標位置、目標姿勢角度を生成する目標軌道生成部と、前記移動体の位置と姿勢角度を検出する状態検出部と、前記目標軌道生成部と前記状態検出部の出力に基づき軌道追従誤差を計算する誤差計算部と、前記誤差計算部の出力に基づき前記移動体への指令値を計算する指令値計算部と、を備え、姿勢角度誤差を目標姿勢角度から前記移動体の姿勢角度を差し引いたものとし、前記指令値計算部において、前記指令値の計算に用いる前記姿勢角度誤差の正弦値、もしくは前記姿勢角度誤差の正弦値をテイラー展開で近似したものの係数を常に正値とすることにより、一般的な前輪操舵の自動車を含めた車輪型の移動体において、前進と後退のどちらの場合でも目標軌道への追従を実現する軌道追従制御装置が得られる。

この発明の実施の形態１に係る軌道追従制御装置を説明するブロック線図である。 この発明の実施の形態１に係る軌道追従制御のモデル図である。 この発明の実施の形態１に係る２輪車両と４輪車両の関係を示すモデル図である。 この発明の実施の形態２に係る軌道追従制御装置を説明するブロック線図である。 この発明の実施の形態３に係る軌道追従制御装置を説明するブロック線図である。 この発明の実施の形態４に係る軌道追従制御装置を説明するブロック線図である。 この発明の実施の形態１、２、３、４に係る軌道追従制御のシミュレーション結果を示す図である。 この発明の実施の形態５に係る軌道追従制御装置を説明するブロック線図である。 この発明の実施の形態５に係る目標速度制限部の特性を表す図である。 この発明の実施の形態６に係る軌道追従制御装置を説明する図で、２輪車両と後退時の４輪車両の関係を示すモデル図である。

以下、この発明に係る軌道追従制御装置の好適な実施の形態について、図面を参照して説明する。なお、それぞれの実施の形態において、同一符号は、同一または相当部分を示すものである。

実施の形態１．
図１は、この発明の実施の形態１に係る軌道追従制御装置を説明するブロック線図である。図１において、目標軌道生成部１０は、目標速度ｖｒ、目標回転速度ωｒ、目標位置ｘｒ、ｙｒ、目標姿勢角度θｒを生成する。誤差計算部１１は、目標軌道生成部１０の出力と状態検出部１２の出力から軌道追従誤差を計算する。状態検出部１２は、移動体１３の位置ｘｃ、ｙｃ、姿勢角度θｃを検出する。姿勢角度θｃは移動体１３の前方方向とｘ軸のなす角度とする。検出方法としては、移動体１３に搭載した車輪速センサ、ロータリーエンコーダやヨーレートセンサなどに基づき状態を推定する方法、移動体１３に搭載したカメラ、超音波センサ、レーザレンジセンサなどを用いた周辺環境認識により状態を推定する方法、もしくは路面や壁面などインフラ側に設置したセンサから検出する方法などを用いることができる。

指令値計算部１４は、誤差計算部１１の出力に基づき移動体１３への指令値を計算する。指令値計算部１４は速度指令値計算部１５と回転速度指令値計算部１６からなる。速度指令値計算部１５は、目標速度ｖｒを用いたフィードフォワード制御部１５ａと、位置誤差ｘｅを用いたフィードバック制御部１５ｂからなる。回転速度指令値計算部１６は、目標回転速度ωｒを用いたフィードフォワード制御部１６ａと、位置誤差ｙｅを用いたフィードバック制御部１６ｂ、および姿勢角度誤差θｅの正弦であるｓｉｎθｅを用いたフィードバック制御部１６ｃからなる。前記特許文献1および非特許文献1ではｓｉｎθｅの係数はｖｒ＊Ｋθであり、係数の符号は目標速度ｖｒの符号に伴い変化する。しかし、実施の形態１に係る軌道追従制御装置はｓｉｎθｅの係数をα＊Ｋθ（αは正値）とすることで、目標速度ｖｒの符号によらずｓｉｎθｅの係数を正値とすることを特徴とする。

以下、この特徴により得られる効果を説明する。図２は実施の形態１に係る移動体１３の軌道追従制御の基本的なモデルを表す。図２において移動体１３は一例として２輪車両を示し、この車両は左右輪の回転速度を独立に制御できるものとする。左右輪の回転速度を同期させることで前進と後退ができる。左右輪の回転数に差を与えることで旋回ができる。２輪車両の位置をｘｃ、ｙｃ、姿勢角度をθｃとし、走行速度をｖｃ、回転速度をωｃとする。走行速度ｖｃと回転速度ωｃは車両への指令値に対応する。車輪は横方向に速度を発生することができないという幾何学的な非ホロノミック拘束に基づき、２輪車両の運動に関して次式（１）を得る。

図２おいて、移動体１３が各瞬間に追従する目標点は、目標軌道に沿って目標速度ｖｒで移動する。目標速度ｖｒは移動体１３の操作者が与えることや、移動体１３が自律的に判断して決定することができる。目標速度ｖｒの向きは目標点が軌道上を移動するに従って変化し、その回転速度を目標回転速度ωｒとする。目標軌道の与え方としては、予めメモリに保存したものを用いる方法、路面に磁気テープや白線などで敷設して走行ラインとして与える方法、あるいは移動体１３が周辺環境を認識して得た情報や外部との通信により得た情報に基づきオンラインで生成したものを用いる方法などがある。

目標点の位置をｘｒ、ｙｒ、姿勢をθｒとする。図２に示すように、移動体１３の前後方向と横方向を基準に考えると、移動体１３と目標点の位置誤差ｘｅ、ｙｅ、姿勢角度誤差θｅは次式（２）で得られる。

両辺を時間微分して式変形することで、軌道追従誤差に関する次式（３）の方程式を得る。

前記非特許文献１における軌道追従制御方法では、移動体１３への速度指令ｖｃと回転速度指令ωｃを次式（４）で与える。

上式（４）において、係数Ｋｘ、ＫｙおよびＫθは全て正値とする。実施の形態１とは異なり、上式（４）ではｓｉｎθｅの係数がｖｒ＊Ｋθであり、目標速度ｖｒの符号により（つまり目標とする進行方向が前進か後退かにより）係数の符号は変化する。軌道追従制御を達成できるかを検証するため、非特許文献１で示されているものと同様のリアプノフ関数Ｖを次式（５）でおく。

リアプノフ関数Ｖの時間微分は計算を進めると次式（６）となる。

ゆえに、目標速度ｖｒを負とした場合、リアプノフ関数の時間微分は準負定（０以下）ではない。つまり、前記（３）式で示す軌道追従誤差に関する方程式の原点ｘｅ＝０、ｙｅ＝０、θｅ＝０の安定性は保証されず、非特許文献１の方法では後退による目標軌道への追従は保証されない。一方、実施の形態１に係る軌道追従制御装置においては、移動体１３への速度指令ｖｃと回転速度指令ωｃを次式（７）で与える。

αは正値で与えるため、非特許文献１とは異なり、上式ではｓｉｎθｅの係数の符号は目標速度ｖｒの符号によらず（つまり目標とする進行方向が前進か後退かによらず）常に正値である。Ｖの時間微分は計算を進めると次式（８）となる。

従って、リアプノフ関数Ｖの時間微分は準負定（０または負）となることから、前記（３）式で示す軌道追従誤差に関する方程式の原点ｘｅ＝０、ｙｅ＝０、θｅ＝０は安定な平衡点となる。ただし、リアプノフ関数Ｖの時間微分は負定（０未満）ではないことから、前記リアプノフ関数Ｖを用いた解析では原点の漸近安定性までは保証されない。つまり追従誤差が時間経過とともに拡大することはないが、十分時間が経過したときに追従誤差が０に収束することまでは保証されない。原点の漸近安定性を少なくとも原点近傍で検証するため、前記（３）式で示す軌道追従誤差に関する方程式を原点近傍で線形近似することで次式（９）を得る。

追従誤差ベクトル（ｘｅ、ｙｅ、θｅ）の係数行列の特性多項式は次式（１０）である。

多項式の全ての係数は、目標速度ｖｒ、目標回転速度ωｒの符号によらず正値である。加えて、次式（１１）が成立する。

ゆえに、ラウス・フルビッツの安定性判別に基づき、原点ｘｅ＝０、ｙｅ＝０、θｅ＝０は漸近安定な平衡点であることが保証される。

以上に基づき、実施の形態１に係る軌道追従制御装置を用いることで、前進と後退の両方の場合において移動体１３の目標軌道への追従を実現できる。実用上の観点では、速度指令ｖｃと回転速度指令ωｃの計算におけるｃｏｓθｅとｓｉｎθｅはテイラー展開により近似したものを用いてもよい。また、図２に示す２輪車両モデルは一般的な自動車である前輪操舵の４輪車両に対応させて扱うことができる。

図３は、４輪車両のモデルを表し、車両の位置ｘｃ、ｙｃは後輪軸の中心点とする。また、前輪軸と後輪軸の距離をＬとし、前輪の操舵角度をφｃとすると、４輪車両の運動に関して次式（１２）を得る。

従って、ωｃを次式（１３）とすることで４輪車両と２輪車両の運動を前記（１）式で示す共通の式を用いて考えることができる

なお、後輪操舵車両など他の操舵方法の車両や、図１と図２で説明した車両と車輪の数が異なる３輪車両なども、同様に前式（１）を用いて運動を表現できる。ゆえに、この発明に係る軌道追従制御装置は前式（１）で運動を表現できる移動体１３を対象として広く適用できる。

実施の形態２．
次に、この発明の実施の形態２に係る軌道追従制御装置について説明する。
図４は、実施の形態２に係る軌道追従制御装置を説明するブロック線図である。図４に示すように、実施の形態２に係る軌道追従制御装置は、指令値計算部１４と移動体１３の間に指令値を変換する指令値変換部１７を設けることで、制御対象とする移動体１３の種類に応じた制御を実行できる。なお、その他の部分は実施の形態１と同様であり、同一符号を付すことにより説明を省略する。

例えば、前輪操舵の４輪車両であれば指令値計算部１４から与えられる回転速度指令値を、次式（１４）により操舵角指令値φｃに変換できる。

ただし、速度指令値ｖｃの値が０のときは０による割り算が発生するため、速度指令値ｖｃの絶対値がε以下のときは、操舵角指令値φｃの計算に用いる速度指令値ｖｃの値を次式（１５）に置き換えてもよい。

式（１５）において、ｓｇｎ（）は符号関数であり、括弧内の値の符号を取り出す。εは０による割り算を防止するための正値のパラメータである。

以上により、実施の形態２に係る軌道追従制御装置は、指令値変換部１７を導入することにより、指令値計算部１４から得られる回転速度指令値を、２輪車両以外の移動体１３に適した形に変更することができるため、適切な形で移動体１３に実装できる。

実施の形態３．
次に、この発明の実施の形態３に係る軌道追従制御装置について説明する。
図５は、実施の形態３に係る軌道追従制御装置を説明するブロック線図である。図５に示すように、実施の形態３に係る軌道追従制御装置は、実施の形態１あるいは２に対し、移動体１３の前に指令値を制限する指令値制限部１８を導入することで、移動体１３の物理的制約や乗員の乗り心地などを考慮して指令値に飽和処理などを施すことができる。なお、その他の部分は実施の形態１あるいは２と同様であり、同一符号を付すことにより説明を省略する。

例えば、実施の形態２において制御対象を前輪操舵の４輪車両とするとき、操舵角度φｃの指令値を移動体１３の機械的な制約に基づき制限できるため、現実の制御対象に則した形で実施の形態３に係る軌道追従制御装置を実装できる。また、電動パワーステアリングに用いられるモータの性能や、車輪の接地荷重により変化するステアリングまわりの負荷状態に基づき、操舵角度φｃの角速度や角加速度に制限を設けることで、機器への過度な負荷や破損を防止した軌道追従制御を実現できる。

また、指令値制限部１８において移動体１３への速度指令値ｖｃに上限を設けることで、移動体１３が目標軌道との誤差を縮小するときに過度な速度で移動することを防ぐことができる。

実施の形態４．
次に、この発明の実施の形態４に係る軌道追従制御装置について説明する。
図６は、実施の形態４に係る軌道追従制御装置を説明するブロック線図である。図６に示すように、実施の形態４に係る軌道追従制御装置は、実施の形態２、３に係る軌道追従制御装置に対し、回転速度指令値計算部１６を構成するフィードバック制御部１６ｃの調整パラメータであるαを、α＝｜ｖｒ｜とすることにより、車両の速度に応じて軌道追従性能を変化させることができる。なお、その他の部分は実施の形態２あるいは３と同様であり、同一符号を付すことにより説明を省略する。

回転速度指令値の計算では位置誤差ｙｅの係数にｖｒがあり、制御の効果が車両の速度に依存する。従って、回転速度指令値を構成するｓｉｎθｅの係数にもｖｒを付与することで、位置誤差ｙｅと姿勢角度誤差θｅによる制御の効果が車両の速度に対してともに線形に変化する。これにより、制御装置のパラメータの調整が直感的に行いやすくなる。

なお、ｖｒが正値のとき、実施の形態４に係る指令値計算部１４は前記非特許文献１と同じものとなるが、ｖｒが負値のときは異なるものとなる。ｓｉｎθｅの係数にｖｒを導入しつつも、ｖｒの符号によらずｓｉｎθｅの係数の符号を正値に保つことで、前進と後退のどちらの場合でも目標軌道への追従を実現することができる。

実施の形態１、２、３、４について、それぞれの実施の形態に係る軌道追従制御装置を用いて前輪操舵の４輪車両を制御したシミュレーション結果を図７に示す。比較のため、同一条件で指令値計算部１４として非特許文献１の方法を用いたシミュレーション結果も併せて示した。図７において、符号Ａは実施の形態１、２、３、４のそれぞれの実施の形態に係る軌道追従制御装置を用いたシミュレーション結果を示し、符号Ｂは非特許文献１の方法を用いたシミュレーション結果を示している。

目標点は目標軌道上を一定速度ｖｒで移動し、軌道追従制御装置は車両の後輪軸の中点を目標点に追従させつつ、車両の前後方向が目標点における軌道の接線と一致するように制御を行う。シミュレーションでは後退しながらの軌道追従を検証するためｖｒを負値に設定した。車両の初期位置と初期姿勢は目標軌道からずれた状態とした。

結果として、非特許文献１の制御方法では移動体１３は目標軌道から外れた走行となった。これは非特許文献１の方法はｖｒが負値の場合に軌道追従を理論的に保証していないことに起因する。一方、実施の形態１、２、３、４のそれぞれの実施の形態に係る軌道追従制御装置を用いた場合は時間経過とともに車両は軌道上の目標点へと収束しており、その効果がわかる。

実施の形態５．
次に、この発明の実施の形態５に係る軌道追従制御装置について説明する。
図８は、実施の形態４に係る軌道追従制御装置を説明するブロック線図である。図８に示すように、実施の形態１、２、３、４に係る軌道追従制御装置において、目標軌道生成部１０を、目標速度設定部１０ａ、目標速度制限部１０ｂ、停車位置・姿勢設定部１０ｃ、残り距離計算部１０ｄ、目標軌道幾何情報処理部１０ｅにより構成する。なお、その他の部分は実施の形態１、２、３、４と同様であり、同一符号を付すことにより説明を省略する。

停車位置・姿勢設定部１０ｃには、最終的な停車位置・姿勢を設定できる。これは、例えば自動駐車の場合であれば、移動体１３がセンサや外部機器との通信により取得した駐車スペースの情報に基づき設定する。

目標速度設定部１０ａには、移動体１３が軌道に沿って移動する目標速度を設定する。これをｖｒ０とする。目標速度制限部１０ｂは、目標速度設定部１０ａと残り距離計算部
１０ｄの出力に基づき、最終的に目標軌道生成部１０から出力する目標速度ｖｒを決定する。残り距離計算部１０ｄは、目標軌道幾何情報処理部１０ｅから出力される目標位置・姿勢と、停車位置・姿勢設定部１０ｃの情報に基づき、軌道上を移動する目標点の最終的な停車位置・姿勢までの距離を計算して出力する。

目標軌道幾何情報処理部１０ｅには、予めメモリに保存した軌道、路面に磁気テープや白線などで敷設して走行ラインとして与えた軌道、あるいは移動体１３が周辺環境を認識して得た情報や外部との通信により得た情報に基づきオンラインで生成した軌道の幾何情報を格納することができる。また、目標軌道幾何情報処理部１０ｅは、目標速度制限部１０ｂからの出力である目標速度ｖｒを受け取り、格納された軌道に沿って目標速度ｖｒで移動する目標点の位置・姿勢・回転速度を出力する。

以上の構成を採用した上で、例えば目標速度制限部１０ｂにおける処理を次に示す形で構成することで得られる効果について説明する。

図９に示すのは、目標速度制限部１０ｂにおいて、ｖｒ０に掛けるゲインである。ゲインは残り距離計算部１０ｄからの出力を横軸として値が変化する。図９では一例として、残り距離３ｍ以上ではゲインを１とし、３ｍ以下では０に向かって線形に減少するように設定した。従って、目標軌道幾何情報処理部１０ｅに格納された軌道に沿って、目標点は目標速度ｖｒで移動するが、停車位置に近付くにつれてｖｒの絶対値は減少する。最終的にｖｒは０となり、それに応じてωｒも０になるとともに、目標点は停車位置・姿勢設定部１０ｃの通りに停止する。

このときの指令値計算部１４の動作を考える。目標速度ｖｒが０に収束することに伴い、速度指令値計算部１５におけるｖｒ＊ｃｏｓθｅの項も０に収束する。従って、最終的にＫｘ＊ｘｅの項のみが残り、位置誤差のみに依存する制御に連続的に切り替わる。回転速度指令値計算部１６においても、最終的に残る項はα＊Ｋθ＊ｓｉｎθｅであることから、姿勢角度誤差のみに依存する制御に連続的に切り替わる。

自動駐車などでは、停車位置に近付くにつれて、目標速度で移動することよりも所望の位置に正確に移動体１３を停止させることの重要度が増していく。しかしながら、目標速度を維持する制御と、目標位置での移動体１３を停止させる制御を２種類用意して単純に切り替える方法では、切り替え時の指令値の急激な変化による衝撃で、移動体１３の故障や乗員の乗り心地悪化につながる。

そこで実施の形態５に係る軌道追従制御装置を用いることで、指令値計算部１４に示す制御系だけを用いながら、目標速度を重視する制御から目標位置へ移動体１３を停止させる制御へと連続的に切り替えることができる。

実施の形態６．
次に、この発明の実施の形態６に係る軌道追従制御装置について説明する。
前記非特許文献１の軌道追従制御方法では、目標速度ｖｒを負とした場合に目標軌道への追従が保証されないが、前輪操舵の４輪車両では以下の措置をとることで、非特許文献１における速度指令値ｖｃと回転速度指令値ωｃの計算を用いても、後退の場合に目標軌道への追従を達成することができる。

図１０は、この発明の実施の形態６に係る軌道追従制御装置を説明する図で、２輪車両と後退時の４輪車両の関係のモデル図である。
図１０に示すように、前輪操舵の４輪車両において、車両の後方向への速度を正値とするとともに、後方向とｘ軸とのなす角度を車両の姿勢角度とするように定義を切り替える
。このとき、幾何学的な条件から次式（１６）が成立する。

従って、ωｃを次式（１７）とすることで、後退の場合でも４輪車両と２輪車両の運動を共通の式（１）を用いて考えることができる。

以上より、前進と後退に応じて速度の符号と姿勢角度の取り方を切り替えることで、前進と後退の両方の場合において軌道追従を実現できる。なお、このような切り替えを用いる場合でも、実施の形態１に係る式（７）による指令値計算はそのまま用いることができる。

以上、この発明の実施の形態１から６について詳述したが、この発明は前記実施の形態に限定されるものではなく、この発明の範囲内にて種々の設計変更を行うことが可能である。

１０ 目標軌道生成部、１０ａ 目標速度設定部、１０ｂ 目標速度制限部、１０ｃ 停車位置・姿勢設定部、１０ｄ 残り距離計算部、１０ｅ 目標軌道幾何情報処理部、１１ 誤差計算部、１２ 状態検出部、１３ 移動体、１４ 指令値計算部、１５ 速度指令値計算部、１５ａ、１６ａ フィードフォワード制御部、１５ｂ、１６ｂ、１６ｃ フィードバック制御部、１６ 回転速度指令値計算部、１７ 指令値変換部、１８ 指令値制限部。

Claims (8)

1. 目標軌道に追従させて移動体の移動を制御する軌道追従制御装置であって、
   目標速度、目標回転速度、目標位置、目標姿勢角度を生成する目標軌道生成部と、
   前記移動体の位置と姿勢角度を検出する状態検出部と、
   前記目標軌道生成部と前記状態検出部の出力に基づき軌道追従誤差を計算する誤差計算部と、
   前記誤差計算部の出力に基づき前記移動体への指令値を計算する指令値計算部と、
   を備え、
   姿勢角度誤差を目標姿勢角度から前記移動体の姿勢角度を差し引いたものとし、
   前記指令値計算部において、前記指令値の計算に用いる前記姿勢角度誤差の正弦値、もしくは前記姿勢角度誤差の正弦値をテイラー展開で近似したものの係数を常に正値とすることを特徴とする軌道追従制御装置。
2. 前記指令値計算部に速度指令値計算部と回転速度指令値計算部を備え、
   前記目標速度をｖｒ、前記目標回転速度をωｒ、位置誤差をｘｅ、ｙｅ、前記姿勢角度誤差をθｅとした場合、係数Ｋｘ、Ｋｙ、Ｋθ、αをそれぞれ正値とし、
   前記速度指令値計算部は、前記移動体への速度指令値ｖｃを、ｖｃ＝ｖｒ＊ｃｏｓθｅ＋Ｋｘ＊ｘｅで計算し、
   前記回転速度指令値計算部は、前記移動体への回転速度指令値ωｃを、ωｃ＝ωｒ＋ｖｒ＊Ｋｙ＊ｙｅ＋α＊Ｋθ＊ｓｉｎθｅで計算することを特徴とする請求項１に記載の軌道追従制御装置。
3. 前記回転速度指令値計算部は、前記αを、α＝｜ｖｒ｜と設定することを特徴とすることを特徴とする請求項２に記載の軌道追従制御装置。
4. 前記速度指令値ｖｃと前記回転速度指令値ωｃの計算に、ｃｏｓθｅとｓｉｎθｅにはテイラー展開で近似したものを用いることを特徴とする請求項１から３の何れか一項に記載の軌道追従制御装置。
5. 前記指令値計算部の出力を、前記指令値を変換する指令値変換部に通すことを特徴とする請求項１から４の何れか一項に記載の軌道追従制御装置。
6. 前記指令値計算部の出力を、前記指令値を制限する指令値制限部に通すことを特徴とする、請求項１から５の何れか一項に記載の軌道追従制御装置。
7. 前記目標軌道生成部を、目標速度設定部、目標速度制限部、停車位置・姿勢設定部、残り距離計算部、目標軌道幾何情報処理部により構成し、前記停車位置・姿勢設定部には、前記移動体に与える最終的な停車位置・姿勢を設定し、前記目標速度設定部には、前記目標速度を設定し、前記残り距離計算部は、前記目標軌道幾何情報処理部から出力される目標位置・姿勢と、前記停車位置・姿勢設定部の情報に基づき、前記軌道に沿って移動する目標点の最終的な停車位置・姿勢までの距離を計算して出力し、前記目標軌道幾何情報処理部は、前記目標軌道の幾何情報を格納するとともに、前記目標速度制限部からの出力を受け取り、前記目標軌道に沿って前記出力で移動する目標点の位置・姿勢・回転速度を出力し、前記目標速度制限部は前記目標速度にゲインを乗じて目標速度を決定し、前記ゲインを前記残り距離計算部の出力に応じて変化させることを特徴とする請求項１から６の何れか一項に記載の軌道追従制御装置。
8. 前輪操舵の４輪車両を対象として、後輪軸の中心点を車両の座標とし、
   前記車両を前進させるときは、前記車両の前方向を速度の正値とし、前記車両の姿勢角
   度をｘ軸と前記車両の前方向のなす角度とし、
   前記車両を後退させるときは、前記車両の後方向を速度の正値とし、前記車両の姿勢角度をｘ軸と前記車両の後方向のなす角度とするように速度と座標の取り方を切り替えることを特徴とする、請求項１から７の何れか一項に記載の軌道追従制御装置。