JP2006092424A - 軌道追従制御方法および装置 - Google Patents

Abstract

【課題】 移動体を目標軌道に安定に追従させるに必要な制御指令を適切に得て、その軌道追従性能を高めることのできる軌道追従制御方法を提供する。
【解決手段】 目標軌道上の目標位置に対する前記移動体の前後方向のずれ量ｘ_eと前記移動体の横方向のずれ量ｙ_eとをそれぞれ求めると共に、前記目標軌道上での目標姿勢に対する前記移動体の向きずれ角φ_eをそれぞれ求め、上記向きのずれ角φ_eに応じて前記移動体の目標移動速度ｕ_d,γ_dと前記ずれ量ｘ_e,ｙ_eとから前記移動体に対する前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rをそれぞれ計算する。そしてこれらの各速度指令値ｕ_r,ν_r,γ_rに従って前記移動体の進行速度、横行速度、および旋回速度をそれぞれ制御する。
【選択図】 図２

Description

本発明は、目標軌道に追従させて移動体を移動させるに必要な制御指令値を進行速度、横行速度および旋回速度としてそれぞれ得て、前記移動体を前記目標軌道に高精度に追従させて移動させることのできる軌道追従制御方法および装置に関する。

予め設定された目標軌道に追従させて移動体を移動させる技術は、工場等における無人搬送車の走行制御に用いられる。この種の無人搬送車の軌道追従制御は、専ら、無人搬送車に搭載したセンサを用いて予め路面（床面）に敷設した走行ラインを検出し、該無人搬送車の上記走行ラインからずれ量を求めながら、このずれ量を零（０）とするように無人搬送車を操舵制御することにより実現される。

またこのような軌道追従制御を滑らかに行う手法として、非ホロノミックの手法を採用し、目標軌道に対する無人走行車の位置や向きのずれ角に応じてその走行速度と操舵角とを制御することが提唱されている（例えば特許文献１を参照）。また非ホロノミックな拘束の下での運動制御システムにおいて、旋回運動により目標軌道に対する移動体の左右方向のずれを補正しながら該移動体の向きを補正することで軌道追従制御を行うことが提唱されている（例えば非特許文献１を参照）。
特開平１１−３２７６４１号公報 日本ロボット学会誌１１巻６号，８３７〜８４４頁，１９９３年

しかしながら上述した制御の手法においては、移動体の向きのずれ角が変化すると、その速度や操舵角をそれぞれ制御するアクチュエータをずれ量に応じて同じ量だけ駆動しても、目標軌道への戻り方が異なると言う不具合がある。即ち、ずれ量に比例してアクチュエータを駆動しても、幅広い動作範囲で適切な追従性能を得ることが困難であった。特に低速時に操舵しても横行・旋回運動が殆ど生じることがないので、操舵によってその偏差（ずれ量）が少なくなることがない。これ故、操舵に対するフィードバック制御系に、徒に大きな負荷が掛かると言う問題があった。

本発明はこのような事情を考慮してなされたもので、その目的は、移動体の移動速度に拘わることなく、目標軌道からのずれ量に応じて上記移動体を目標軌道に安定に追従させるに必要な制御指令を適切に得て、その軌道追従性能を高めることのできる軌道追従制御方法および制御装置を提供することにある。

上述した目的を達成するべく本発明に係る軌道追従制御方法は、目標軌道に追従させて移動体を移動制御するに際して、
<ａ> 前記目標軌道上の目標位置に対する前記移動体の前後方向のずれ量ｘ_eと前記移動体の横方向のずれ量ｙ_eとをそれぞれ求めると共に、前記目標軌道上での目標姿勢に対する前記移動体の向きずれ角φ_eをそれぞれ求め、
<ｂ> 上記向きのずれ角φ_eに応じて前記移動体の目標移動速度ｕ_d,γ_dと前記ずれ量ｘ_e,ｙ_eとから前記移動体に対する前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rをそれぞれ計算し、
<ｃ> これらの各速度指令値ｕ_r,ν_r,γ_rに従って前記移動体の進行速度、横行速度、および旋回速度をそれぞれ制御することを特徴としている。

具体的には請求項２に記載するように前記目標軌道上での目標姿勢は、前記目標位置における前記目標軌道の接線方向として求められ、また前記目標位置および目標姿勢に対するずれ量は、前記移動体に搭載したセンサにより前記目標軌道までの距離を計測して、或いは予め記憶した目標位置の絶対座標（ｘ_d,ｙ_d,φ_d）と前記移動体の絶対位置ｘ,ｙとその姿勢φとから計算して求められる。

ちなみに前記移動体が自動車や船舶のように２次元平面内を移動する場合には、請求項３に記載するように前記横方向のずれ量ｙ_eを前記目標軌道に対する前記移動体の左右方向の偏差として求め、また前記移動体の向き角のずれ量φ_eを前記目標軌道の接線方向に対する前記移動体のヨー角として求めるようにすれば良い。また前記移動体が航空機や潜水艦のように３次元空間を移動する場合には、請求項４に記載するように前記横方向のずれ量ｙ_eを前記目標軌道に対する前記移動体の左右方向および上下方向の偏差としてそれぞれ求め、また前記移動体の向き角のずれ量φ_eを前記目標軌道の接線方向に対する前記移動体の前後方向のヨー角、ピッチ角およびロール角としてそれぞれ求めるようにすれば良い。

また本発明に係る軌道追従制御は、
<Ａ> 目標軌道上の目標位置に対する移動体の前後方向のずれ量ｘ_eと前記移動体の横方向のずれ量ｙ_eとをそれぞれ求めると共に、前記目標軌道上での目標姿勢に対する前記移動体の向きのずれ角φ_eをそれぞれ求めるセンシング手段と、
<Ｂ> 上記向き角のずれ量φ_eに応じて前記移動体の目標移動速度ｕ_d,γ_dと前記ずれ量ｘ_e,ｙ_eとから前記移動体に対する前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rをそれぞれ計算するマイクロコンピュータ等の演算手段と、
<Ｃ> これらの各速度指令値ｕ_r,ν_r,γ_rに従って前記移動体の挙動を制御するアクチュエータ等を駆動して前記移動体の進行速度、横行速度、および旋回速度をそれぞれ制御する移動制御手段と
を備えたことを特徴としている。

具体的には前記演算手段は、請求項６に記載するように前記移動体の進行目標速度をｕ_d、横行目標速度をν_d、および旋回目標速度をγ_dとして、前記移動体に対する前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rを
ｕ_r＝ｕ_dcosφ_e＋Ｋ_xｘ_e
ν_r＝ν_d＋Ｋ_yｕ_dｙ_e
γ_r＝γ_d＋Ｋ_pｕ_dsinφ_e
但し、Ｋ_x,Ｋ_y,Ｋ_pはそれぞれ正の制御ゲイン
として求めるように構成される。

上記構成の軌道追従制御方法によれば、向きのずれ角φ_eに応じて前記移動体の目標移動速度ｕ_d,γ_dと目標軌道に対するずれ量ｘ_e,ｙ_eとから前記移動体に対する前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rをそれぞれ求め、前記移動体の進行速度、横行速度、および旋回速度をそれぞれ独立に制御するので、向きのずれ角φ_eに応じた進行・旋回動作により目標軌道に精度良く追従させて移動体を移動させることが可能となる。

ちなみに従来では、例えば前述した非特許文献１に紹介されるように前後方向速度指令値ｕ_rを求めて移動体の前後方向のずれを補正しながら、旋回速度指令値γ_rを求めて前記移動体の左右方向のずれおよび向き角のずれを補正しているが、この場合、左右方向のずれ補正と向き角のずれ補正とをどのような関係で、つまりどのようにトレードオフして作用させるかを決定することが困難である。この点、本願発明においては進行方向速度指令値ｕ_rに加えて横方向速度指令値ν_rと旋回速度指令値γ_rとを独立に求めるので、左右方向のずれ補正と向き角のずれ補正とを個別に制御することが可能となる。従って目標軌道に対するずれ補正を的確に行って上記目標軌道に対する追従精度を高めることが可能となる。

特に前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rを
ｕ_r＝ｕ_dcosφ_e＋Ｋ_xｘ_e
ν_r＝ν_d＋Ｋ_yｕ_dｙ_e
γ_r＝γ_d＋Ｋ_pｕ_dsinφ_e
として求めることで、向きのずれ角φ_eに応じて非線形の制御を行うことになるので、例えば向きのずれ角φ_eが大きくても目標軌道から離れる方向への移動をなくすことができる。換言すれば向きのずれ角φ_eが大きいにも拘わらずその速度を高くした場合、移動体が目標軌道から離れる虞があるが、上述した制御によればずれ角角φ_eを加味して横行速度と旋回速度とを決定するので、その挙動を適正化して目標軌道からのずれを抑制することが可能となる。また前述した各指令値ｕ_r,ν_r,γ_rを、目標進行速度ｕ_dに応じて決定される補正量を加味してそれぞれ求めるので、例えば低速走行時における不本意な横行運動や旋回運動を防ぐことができる。従って目標軌道に対する追従性を高めることができる等の効果が奏せられる。つまり無駄な操舵制御を減らすことができ、その制御負担も軽減することができ、更には外乱の影響を軽減することができる等の効果が奏せられる。

以下、図面を参照して本発明の一実施形態に係る軌道追従制御方法および制御装置について説明する。
図１はこの実施形態に係る軌道追従制御方法の基本的な制御原理を示すもので、Ｌは予め設定された目標軌道、Ｍは上記目標軌道Ｌに追従して移動制御される移動体である。目標軌道Ｌは、例えば港湾の敷地内の予め設定された経路に沿って複数の磁性素子を敷設する等して規定された走行ラインからなり、移動体Ｍは上記走行ラインＬに沿って走行駆動される無人搬送車からなる。この無人搬送車（移動体）Ｍの上記走行ライン（目標軌道）Ｌに沿った走行（移動）は、基本的には無人搬送車（移動体）Ｍの現在位置と走行ライン（目標軌道）Ｌ上での目標位置とのずれに応じて該無人搬送車（移動体）Ｍの走行速度を制御すると共に、その操舵角を制御して行われる。

即ち、本発明に係る軌道追従制御方法は、移動体Ｍにおける制御すべき点、具体的には移動体Ｍの挙動を表現する代表点、例えば移動体Ｍにおける重心Ｇの現在位置（ｘ,ｙ）およびその向き（姿勢）φと、前記目標軌道Ｌ上における或る制御時点での目標位置（ｘ_d,ｙ_d）およびその向きφ_dとの差（ずれ量）に従い、例えばその走行速度と操舵角とをそれぞれ制御することによって行われる。尚、前記目標位置の向きφ_dは、前記目標位置（ｘ_d,ｙ_d）における前記目標軌道Ｌの接線方向として定義される。

即ち、目標軌道Ｌに対する移動体Ｍの追従制御は、図２に軌道追従制御装置の概略的な構成とその処理概念を示すように、先ず移動体Ｍの現在位置・姿勢（ｘ,ｙ,φ）と、現時点における前記目標軌道Ｌ上の目標位置・姿勢（ｘ_d,ｙ_d,φ_d）に対する前記移動体Ｍの前後方向のずれ量ｘ_e（＝ｘ_d−ｘ）、前記移動体Ｍの左右方向のずれ量ｙ_e（＝ｙ_d−ｙ）、および前記移動体Ｍの向きのずれ角φ_e（＝φ_d−φ）をそれぞれ求める［処理Ａ］。次いで後述するように、これらのずれ量（ｘ_e,ｙ_e,φ_e）に従って前記移動体Ｍの前後方向への移動速度（進行速度）ｕ_r、左右方向への移動速度（横行速度）ν_r、および旋回速度γ_rをそれぞれその制御指令値として求め［処理Ｂ］、これらの制御指令値ｕ_r,ν_r,γ_rに従って当該移動体Ｍの移動を制御する［処理Ｃ］ことによって行われる。この移動体Ｍの移動制御は、具体的には移動体Ｍの移動速度（走行速度）を制御すると共に、その操舵角を制御することによって行われる。

具体的には上記ずれ量（ｘ_e,ｙ_e,φ_e）の検出は、例えば移動体Ｍに搭載した各種のセンサ（例えば磁気センサ、超音波センサ、トランスポンダ、距離計等）を用いて目標軌道Ｌまでの距離を直接測定して求められる。或いは予め記憶した目標軌道Ｌを特定するデータ系列から目標位置の絶対座標（ｘ_d,ｙ_d,φ_d）を求めると共に、移動体Ｍの絶対位置・姿勢（ｘ,ｙ,φ）をＧＰＳ情報等から求め、これらの情報に従って前記ずれ量（ｘ_e,ｙ_e,φ_e）を

として計算により求めるようにすれば良い。
ちなみにこのようなずれ量（ｘ_e,ｙ_e,φ_e）に基づいてそのずれを補正し、これによって目標軌道Ｌに対する追従制御を行う手法については、前述した非特許文献１に詳しく紹介される通りである。具体的には、例えば
ｕ_r＝ｕ_dcosφ_e＋Ｋ_rｘ_e
γ_r＝γ_d＋Ｋ_yｕ_dｙ_e＋Ｋ_pｕ_dsinφ_e
但し、Ｋ_x,Ｋ_y,Ｋ_pはそれぞれ正の制御ゲイン
として、上記ずれを補正するための前記移動体Ｍの進行指令速度ｕ_rと旋回指令速度γ_rとを求めて移動体Ｍの移動を制御すれば、目標軌道Ｌに対する追従制御を行うことが可能となる。即ち、所定の進行速度の下で移動体Ｍの旋回運動により左右方向のずれを補正しながら、その向きのずれを補正すれば良い。しかしこの場合、左右方向のずれｙ_eと向きのずれφ_eとをどのような関係で補正するかが問題となり、一般にその制御ゲインＫ_y,Ｋ_pを最適設定することは困難である。

そこで本発明に係る軌道追従制御方法においては、前述したずれ量（ｘ_e,ｙ_e,φ_e）をそれぞれ独立に補正するための制御指令値を、そのときの前記移動体Ｍの目標移動速度ｕ_d,γ_dと向き角のずれ量φ_eとに応じて
ｕ_r＝ｕ_dcosφ_e＋Ｋ_xｘ_e …(１)
ν_r＝ν_d＋Ｋ_yｕ_dｙ_e …(２)
γ_r＝γ_d＋Ｋ_pｕ_dsinφ_e …(３)
としてそれぞれ求めるようにしている［処理Ｂ］。

即ち、移動体Ｍの前後方向に対する進行速度指令値ｕ_rを、そのときの目標移動速度ｕ_dと向き角のずれ量φ_eとにより定まる速度に、目標位置との前後方向のずれ量ｘ_eに応じて定まる補正量を加えて決定する。また移動体Ｍの左右方向に対する横行速度指令値ν_rを、そのときの横行速度ν_dに前記移動体の目標移動速度ｕ_dと左右方向のずれ量ｙ_eとにより定まる補正量を加えて決定する。更にはその旋回速度指令値γ_rを、そのときの旋回速度γ_dに前記移動体の目標移動速度ｕ_dと向き角のずれ量φ_eとにより定まる補正量を加えて決定する。尚、上記横行速度ν_dについては、前述した目標移動速度ｕ_dとそのときの移動体Ｍの向き角φから求めるようにすれば良く、また旋回速度γ_dについてはその解くの移動体Ｍの操舵角から求められる旋回半径ｒ等に従って求めるようにすれば良い。

そして移動体Ｍの目標位置に対するずれ量（ｘ_e,ｙ_e,φ_e）に従って該移動体Ｍに対する制御指令値を進行速度指令値ｕ_r、横行速度指令値ν_r、および旋回速度指令値γ_rとしてそれぞれ求めた後、これらの速度指令値ｕ_r,ν_r,γ_rに従って移動体Ｍの挙動を制御するアクチュエータの作動を制御する。具体的には移動体Ｍの走行速度を決定するモータやエンジンの回転数を制御すると共に、その操舵角を決定するステアリングや舵の操作量を制御し、これによって移動体Ｍの進行速度、横行速度、および旋回速度をそれぞれ制御する［処理Ｃ］。

このようにして移動体Ｍの進行速度、横行速度、および旋回速度をそれぞれ個別に制御すれば、上述したずれ量（ｘ_e,ｙ_e,φ_e）をそれぞれ独立に補正することが可能となるので、移動体Ｍを目標軌道Ｌに沿って精度良く追従させて移動させることが可能となる。しかも各速度指令値ｕ_r,ν_r,γ_rに対する補正を前述したずれ量（ｘ_e,ｙ_e,φ_e）に基づいてそれぞれ独立に行い得るので、その制御ゲインＫ_x,Ｋ_y,Ｋ_pについてもそれぞれ独立に最適設定することが可能となる。

特に上述した追従制御によれば、向きのずれ角φ_eに応じて移動体Ｍの進行動作と旋回動作とをそれぞれ制御することが可能となるので、定性的には向きのずれ角φ_eが大きくても移動体Ｍが目標軌道Ｌから離れていくような不本意な挙動をなくすことができる。即ち、前述した式(１)(３)にそれぞれ示すように向きのずれ角φ_eに応じて非線形に決定される値を含む指令値に従ってその進行速度と旋回速度とがそれぞれ制御されることになるので、仮に向きのずれ角φ_eが大きい場合であっても、移動体Ｍが目標軌道Ｌから次第に離れるような不本意な挙動をなくすことが可能となる。

また前述した式(２)(３)にそれぞれ示されるように、そのときの横行速度ν_dおよび旋回速度γ_dを、目標移動速度ｕ_dとずれ量（ｙ_e,φ_e）とを掛け合わせることで決定される補正量に従って補正してその横行速度指令値ν_rと旋回速度指令値γ_rとをそれぞれ求めるので、目標移動速度ｕ_dが小さいときには上記横行速度指令値ν_rおよび旋回速度指令値γ_rをそれぞれ小さく抑えて横行運動や旋回運動が生じ難くすることができる。

尚、移動体Ｍの目標移動速度ｕ_dが微小なときには、仮に操舵角を大きく設定しても横行や旋回が殆ど生じることがないので、前記目標移動速度ｕ_dが所定の閾値に満たないとき、前述した横行速度および旋回速度をそれぞれ零に抑えるような制限を加えることが望ましい。具体的には目標移動速度ｕ_dが小さいときには前述したずれ量（ｘ_e,ｙ_e,φ_e）に基づいて算出した横行速度指令値ν_rおよび旋回速度指令値γ_rに基づく横行・旋回操作が生じないようにし、これによって無駄な操舵制御をなくすことが好ましい。換言すれば低速移動時に操舵しても、これによって横行・旋回運動が生じないような場合、上述した制御により横行速度および旋回速度をそれぞれ零に抑える制限を加えることで、無駄な操舵自体を減らすことが可能となる。

そして上述した横行・旋回運動の下で移動体Ｍの進行速度ｕ_rが制御されるので、移動体Ｍの目標軌道Ｌに対する追従移動が効果的に保証されることになる。特に向きのずれ角φ_eに応じて移動体Ｍの進行速度、横行速度、および旋回速度がそれぞれ独立に制御されることになるので、目標軌道Ｌに沿って追従性良く移動体Ｍを移動（走行）させることが可能となる等の多大なる効果が奏せられる。

尚、上述した制御形態は、港湾の敷地内等において無人搬送車（移動体）Ｍを目標軌道Ｌに沿って走行制御するような２次元平面での追従制御を例にしたものであるが、潜水艦や航空機のように３次元空間において目標軌道に追従制御させる場合にも同様に適用することが可能である。この場合には、移動体Ｍの３次元空間における座標位置（ｘ,ｙ,ｚ）と、目標軌道Ｌ上の目標座標（ｘ_d,ｙ_d,ｚ_d）とからその前後方向のずれ量ｘ_e（＝ｘ_d−ｘ）を求めると共に、横方向のずれ量ｙ_eについては前記目標軌道Ｌに対する前記移動体Ｍの左右方向のずれ量ｙ_e（＝ｙ_d−ｙ）および上下方向のずれ量ｚ_e（＝ｚ_d−ｚ）としてそれぞれ求めるようにすれば良い。また前記移動体Ｍの向き角のずれ量φ_eについては前記目標軌道Ｌの接線方向に対する前記移動体Ｍの前後方向のヨー角φ_ye、ピッチ角φ_peおよびロール角φ_reとしてそれぞれ求めるようにすれば良い。

そしてその制御指令値を、上記各ずれ量ｘ_e,ｙ_e,ｚ_e,φ_ye，φ_pe,φ_reをそれぞれ補正する進行速度指令値ｘ_r、２種類の横行速度指令値ｙ_r,ｚ_r、および３種類の旋回速度指令値φ_yr，φ_pr,φ_rrとしてそれぞれ求め、その進行速度・横行速度・旋回速度をそれぞれ独立に制御するようにすれば良い。この場合においても目標進行速度が小さく、横行運動および旋回運動が殆ど生じない場合には、その操舵を行わないように制御することが好ましいことは言うまでもない。

以上説明したように本発明によれば、目標軌道Ｌに対する移動体Ｍの向きのずれ角φ_eに応じて移動体Ｍの挙動（移動）を制御する前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rをそれぞれ求めるので、移動体Ｍの進行速度、横行速度、および旋回速度をそれぞれ制御して該移動体Ｍを目標軌道Ｌに高精度に追従させて移動させることができる。具体的には前後方向速度指令値ｕ_rに応じてその進行速度（走行速度）を制御し、また横方向速度指令値ν_rと旋回速度指令値γ_rとに応じてその操舵角を制御することで、目標軌道Ｌに追従させて移動体Ｍを移動させることができ。その追従性を十分に高めることが可能となる。

特に移動体Ｍが４輪操舵型の無人搬送車のような場合、前輪の操舵角と後輪の操舵角とを向きのずれ角φ_eに応じてそれぞれ一意に決定するだけで、上記無人搬送車（移動体）を目標軌道Ｌに沿って走行させることが可能となり、その制御も簡単なので実用的利点が多大である。しかも４輪（前輪および後輪）をそれぞれ操舵するといえども、無人搬送車（移動体）が不本意に目標軌道（走行ライン）から離れていく等の不具合を未然に防ぐことが可能となる。

尚、本発明は上述した実施形態に限定されるものではない。例えばずれ量の検出手段やずれ量検出に用いるセンサについては、移動体の種別やその仕様に応じて定めれば良いものである。また制御指令値の演算手段については、専用のハードウェア回路を構築して演算処理を高速度に実行するようにしても良いが、マイクロコンピュータ等を用いてソフトウェア処理により実行するようにしても良い。その他、本発明はその要旨を逸脱しない範囲で種々変形して実施することができる。

本発明に係る軌道追従制御方法の基本的な制御原理を示す図。 本発明に係る軌道追従制御装置の概略的な処理概念とその処理手順を示す図。

符号の説明

Ａ ずれ検出処理手段
Ｂ 指令値演算手段
Ｃ 移動制御手段

Claims (6)

1. 目標軌道に追従させて移動体の移動を制御するに際し、
   前記目標軌道上の目標位置に対する前記移動体の前後方向のずれ量ｘ_eと前記移動体の横方向のずれ量ｙ_eとをそれぞれ求めると共に、前記目標軌道上での目標姿勢に対する前記移動体の向きずれ角φ_eをそれぞれ求め、
   上記向きのずれ角φ_eに応じて前記移動体の目標移動速度ｕ_d,γ_dと前記ずれ量ｘ_e,ｙ_eとに従って前記移動体に対する前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rをそれぞれ計算し、
   これらの各速度指令値ｕ_r,ν_r,γ_rに従って前記移動体の進行速度、横行速度、および旋回速度をそれぞれ制御することを特徴とする軌道追従制御方法。
2. 前記目標軌道上での目標姿勢は、前記目標位置における前記目標軌道の接線方向として求められるものであって、
   前記目標位置および目標姿勢に対するずれ量は、前記移動体に搭載したセンサにより前記目標軌道までの距離を計測して、或いは予め記憶した目標位置の絶対座標（ｘ_d,ｙ_d,φ_d）と前記移動体の絶対位置ｘ,ｙとその姿勢φとから計算して求められるものである請求項１に記載の軌道追従制御方法。
3. 前記移動体は２次元平面内を移動するものであって、
   前記横方向のずれ量ｙ_eは前記目標軌道に対する前記移動体の左右方向の偏差として求められ、前記移動体の向き角のずれ量φ_eは前記目標軌道の接線方向に対する前記移動体のヨー角として求められるものである請求項１に記載の軌道追従制御方法。
4. 前記移動体は３次元空間を移動するものであって、
   前記横方向のずれ量ｙ_eは前記目標軌道に対する前記移動体の左右方向および上下方向の偏差としてそれぞれ求められ、前記移動体の向き角のずれ量φ_eは前記目標軌道の接線方向に対する前記移動体の前後方向のヨー角、ピッチ角およびロール角としてそれぞれ求められるものである請求項１に記載の軌道追従制御方法。
5. 移動体に搭載され、予め設定した目標軌道に追従させて該移動体の移動を制御する軌道追従制御装置であって、
   前記目標軌道上の目標位置に対する前記移動体の前後方向のずれ量ｘ_eおよび横方向のずれ量ｙ_eをそれぞれ求めると共に、前記目標軌道上での目標姿勢に対する前記移動体の向きのずれ角φ_eをそれぞれ求めるセンシング手段と、
   上記向きのずれ角φ_eに応じて前記移動体の目標移動速度ｕ_d,γ_dと前記ずれ量ｘ_e,ｙ_eとから前記移動体に対する前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rをそれぞれ計算する演算手段と、
   これらの各速度指令値ｕ_r,ν_r,γ_rに従って前記移動体の進行速度、横行速度、および旋回速度をそれぞれ制御する移動制御手段と
   を具備したことを特徴とする軌道追従制御装置。
6. 前記演算手段は、前記移動体の進行目標速度をｕ_d、横行目標速度をν_d、および旋回目標速度をγ_dとして、前記移動体に対する前後方向速度指令値ｕ_r、横方向速度指令値ν_r、および旋回速度指令値γ_rを
   ｕ_r＝ｕ_dcosφ_e＋Ｋ_xｘ_e
   ν_r＝ν_d＋Ｋ_yｕ_dｙ_e
   γ_r＝γ_d＋Ｋ_pｕ_dsinφ_e
   但し、Ｋ_x,Ｋ_y,Ｋ_pはそれぞれ正の制御ゲイン
   として求めるものである請求項５に記載の軌道追従制御装置。

Images