軌道追従制御方法および装置
【課題】 移動体を目標軌道に安定に追従させるに必要な制御指令を適切に得て、その軌道追従性能を高めることのできる軌道追従制御方法を提供する。
【解決手段】 目標軌道上の目標位置に対する前記移動体の前後方向のずれ量ｘeと前記移動体の横方向のずれ量ｙeとをそれぞれ求めると共に、前記目標軌道上での目標姿勢に対する前記移動体の向きずれ角φeをそれぞれ求め、上記向きのずれ角φeに応じて前記移動体の目標移動速度ｕd,γdと前記ずれ量ｘe,ｙeとから前記移動体に対する前後方向速度指令値ｕr、横方向速度指令値νr、および旋回速度指令値γrをそれぞれ計算する。そしてこれらの各速度指令値ｕr,νr,γrに従って前記移動体の進行速度、横行速度、および旋回速度をそれぞれ制御する。
【解決手段】 目標軌道上の目標位置に対する前記移動体の前後方向のずれ量ｘeと前記移動体の横方向のずれ量ｙeとをそれぞれ求めると共に、前記目標軌道上での目標姿勢に対する前記移動体の向きずれ角φeをそれぞれ求め、上記向きのずれ角φeに応じて前記移動体の目標移動速度ｕd,γdと前記ずれ量ｘe,ｙeとから前記移動体に対する前後方向速度指令値ｕr、横方向速度指令値νr、および旋回速度指令値γrをそれぞれ計算する。そしてこれらの各速度指令値ｕr,νr,γrに従って前記移動体の進行速度、横行速度、および旋回速度をそれぞれ制御する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、目標軌道に追従させて移動体を移動させるに必要な制御指令値を進行速度、横行速度および旋回速度としてそれぞれ得て、前記移動体を前記目標軌道に高精度に追従させて移動させることのできる軌道追従制御方法および装置に関する。
【背景技術】
【０００２】
予め設定された目標軌道に追従させて移動体を移動させる技術は、工場等における無人搬送車の走行制御に用いられる。この種の無人搬送車の軌道追従制御は、専ら、無人搬送車に搭載したセンサを用いて予め路面（床面）に敷設した走行ラインを検出し、該無人搬送車の上記走行ラインからずれ量を求めながら、このずれ量を零（０）とするように無人搬送車を操舵制御することにより実現される。
【０００３】
またこのような軌道追従制御を滑らかに行う手法として、非ホロノミックの手法を採用し、目標軌道に対する無人走行車の位置や向きのずれ角に応じてその走行速度と操舵角とを制御することが提唱されている（例えば特許文献１を参照）。また非ホロノミックな拘束の下での運動制御システムにおいて、旋回運動により目標軌道に対する移動体の左右方向のずれを補正しながら該移動体の向きを補正することで軌道追従制御を行うことが提唱されている（例えば非特許文献１を参照）。
【特許文献１】特開平１１−３２７６４１号公報
【非特許文献１】日本ロボット学会誌１１巻６号，８３７〜８４４頁，１９９３年
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら上述した制御の手法においては、移動体の向きのずれ角が変化すると、その速度や操舵角をそれぞれ制御するアクチュエータをずれ量に応じて同じ量だけ駆動しても、目標軌道への戻り方が異なると言う不具合がある。即ち、ずれ量に比例してアクチュエータを駆動しても、幅広い動作範囲で適切な追従性能を得ることが困難であった。特に低速時に操舵しても横行・旋回運動が殆ど生じることがないので、操舵によってその偏差（ずれ量）が少なくなることがない。これ故、操舵に対するフィードバック制御系に、徒に大きな負荷が掛かると言う問題があった。
【０００５】
本発明はこのような事情を考慮してなされたもので、その目的は、移動体の移動速度に拘わることなく、目標軌道からのずれ量に応じて上記移動体を目標軌道に安定に追従させるに必要な制御指令を適切に得て、その軌道追従性能を高めることのできる軌道追従制御方法および制御装置を提供することにある。
【課題を解決するための手段】
【０００６】
上述した目的を達成するべく本発明に係る軌道追従制御方法は、目標軌道に追従させて移動体を移動制御するに際して、
<ａ> 前記目標軌道上の目標位置に対する前記移動体の前後方向のずれ量ｘ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に応じてそれぞれ一意に決定するだけで、上記無人搬送車（移動体）を目標軌道Ｌに沿って走行させることが可能となり、その制御も簡単なので実用的利点が多大である。しかも４輪（前輪および後輪）をそれぞれ操舵するといえども、無人搬送車（移動体）が不本意に目標軌道（走行ライン）から離れていく等の不具合を未然に防ぐことが可能となる。
【００３２】
尚、本発明は上述した実施形態に限定されるものではない。例えばずれ量の検出手段やずれ量検出に用いるセンサについては、移動体の種別やその仕様に応じて定めれば良いものである。また制御指令値の演算手段については、専用のハードウェア回路を構築して演算処理を高速度に実行するようにしても良いが、マイクロコンピュータ等を用いてソフトウェア処理により実行するようにしても良い。その他、本発明はその要旨を逸脱しない範囲で種々変形して実施することができる。
【図面の簡単な説明】
【００３３】
【図１】本発明に係る軌道追従制御方法の基本的な制御原理を示す図。
【図２】本発明に係る軌道追従制御装置の概略的な処理概念とその処理手順を示す図。
【符号の説明】
【００３４】
Ａ ずれ検出処理手段
Ｂ 指令値演算手段
Ｃ 移動制御手段
【技術分野】
【０００１】
本発明は、目標軌道に追従させて移動体を移動させるに必要な制御指令値を進行速度、横行速度および旋回速度としてそれぞれ得て、前記移動体を前記目標軌道に高精度に追従させて移動させることのできる軌道追従制御方法および装置に関する。
【背景技術】
【０００２】
予め設定された目標軌道に追従させて移動体を移動させる技術は、工場等における無人搬送車の走行制御に用いられる。この種の無人搬送車の軌道追従制御は、専ら、無人搬送車に搭載したセンサを用いて予め路面（床面）に敷設した走行ラインを検出し、該無人搬送車の上記走行ラインからずれ量を求めながら、このずれ量を零（０）とするように無人搬送車を操舵制御することにより実現される。
【０００３】
またこのような軌道追従制御を滑らかに行う手法として、非ホロノミックの手法を採用し、目標軌道に対する無人走行車の位置や向きのずれ角に応じてその走行速度と操舵角とを制御することが提唱されている（例えば特許文献１を参照）。また非ホロノミックな拘束の下での運動制御システムにおいて、旋回運動により目標軌道に対する移動体の左右方向のずれを補正しながら該移動体の向きを補正することで軌道追従制御を行うことが提唱されている（例えば非特許文献１を参照）。
【特許文献１】特開平１１−３２７６４１号公報
【非特許文献１】日本ロボット学会誌１１巻６号，８３７〜８４４頁，１９９３年
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら上述した制御の手法においては、移動体の向きのずれ角が変化すると、その速度や操舵角をそれぞれ制御するアクチュエータをずれ量に応じて同じ量だけ駆動しても、目標軌道への戻り方が異なると言う不具合がある。即ち、ずれ量に比例してアクチュエータを駆動しても、幅広い動作範囲で適切な追従性能を得ることが困難であった。特に低速時に操舵しても横行・旋回運動が殆ど生じることがないので、操舵によってその偏差（ずれ量）が少なくなることがない。これ故、操舵に対するフィードバック制御系に、徒に大きな負荷が掛かると言う問題があった。
【０００５】
本発明はこのような事情を考慮してなされたもので、その目的は、移動体の移動速度に拘わることなく、目標軌道からのずれ量に応じて上記移動体を目標軌道に安定に追従させるに必要な制御指令を適切に得て、その軌道追従性能を高めることのできる軌道追従制御方法および制御装置を提供することにある。
【課題を解決するための手段】
【０００６】
上述した目的を達成するべく本発明に係る軌道追従制御方法は、目標軌道に追従させて移動体を移動制御するに際して、
<ａ> 前記目標軌道上の目標位置に対する前記移動体の前後方向のずれ量ｘ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に応じてそれぞれ一意に決定するだけで、上記無人搬送車（移動体）を目標軌道Ｌに沿って走行させることが可能となり、その制御も簡単なので実用的利点が多大である。しかも４輪（前輪および後輪）をそれぞれ操舵するといえども、無人搬送車（移動体）が不本意に目標軌道（走行ライン）から離れていく等の不具合を未然に防ぐことが可能となる。
【００３２】
尚、本発明は上述した実施形態に限定されるものではない。例えばずれ量の検出手段やずれ量検出に用いるセンサについては、移動体の種別やその仕様に応じて定めれば良いものである。また制御指令値の演算手段については、専用のハードウェア回路を構築して演算処理を高速度に実行するようにしても良いが、マイクロコンピュータ等を用いてソフトウェア処理により実行するようにしても良い。その他、本発明はその要旨を逸脱しない範囲で種々変形して実施することができる。
【図面の簡単な説明】
【００３３】
【図１】本発明に係る軌道追従制御方法の基本的な制御原理を示す図。
【図２】本発明に係る軌道追従制御装置の概略的な処理概念とその処理手順を示す図。
【符号の説明】
【００３４】
Ａ ずれ検出処理手段
Ｂ 指令値演算手段
Ｃ 移動制御手段
【特許請求の範囲】
【請求項１】
目標軌道に追従させて移動体の移動を制御するに際し、
前記目標軌道上の目標位置に対する前記移動体の前後方向のずれ量ｘ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と前記移動体の横方向のずれ量ｙ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はそれぞれ正の制御ゲイン
として求めるものである請求項５に記載の軌道追従制御装置。
【図１】
【図２】
【図２】
【公開番号】特開２００６−９２４２４（Ｐ２００６−９２４２４Ａ）
【公開日】平成１８年４月６日（２００６．４．６）
【国際特許分類】
【出願番号】特願２００４−２７９３２７（Ｐ２００４−２７９３２７）
【出願日】平成１６年９月２７日（２００４．９．２７）
【出願人】（０００００００９９）石川島播磨重工業株式会社 (5,014)
【Ｆターム（参考）】
【公開日】平成１８年４月６日（２００６．４．６）
【国際特許分類】
【出願日】平成１６年９月２７日（２００４．９．２７）
【出願人】（０００００００９９）石川島播磨重工業株式会社 (5,014)
【Ｆターム（参考）】
[ Back to top ]