(57)【要約】【課題】  平方根やアークタンジェントの計算、走行方向の判断を不要とし移動速度と操舵角速度の指令値の演算を速やかに行う。【解決手段】  移動体の位置認識装置１と、予め設定された移動経路等に関する情報を記憶する情報記憶装置２と、所定時間毎に位置認識装置１と情報記憶装置２からの情報に基き移動速度と操舵角速度を演算し指令する演算装置３と、演算装置３からの移動速度指令、操舵角速度指令に基づいて移動速度、操舵角速度を制御する移動速度制御装置４、操舵角速度制御装置５を備え、演算装置３が、現時刻の設定位置Ｐl から次時刻の設定位置Ｐ2 への移動ベクトルと、現時刻の実際の位置Ｐから次時刻の設定位置Ｐ2 への移動ベクトルの内積と外積を求め、これらを用いて予め設定された移動速度と操舵角速度の指令値を修正する。【選択図】  図１
2005322156.tif 000002
【特許請求の範囲】
【請求項１】
移動体の現在位置を認識する位置認識装置と、予め設定された移動体の移動経路と移動速度と操舵角速度に関する情報を記憶する情報記憶装置と、所定時間毎に位置認識装置と情報記憶装置からの情報に基づいて移動速度と操舵角速度を演算し指令する演算装置と、演算装置からの移動速度指令に基づいて移動速度を制御する移動速度制御装置と、演算装置からの操舵角速度指令に基づいて操舵角速度を制御する操舵角速度制御装置とを備え、前記演算装置が、現時刻の設定位置から次時刻の設定位置への移動ベクトルと、現時刻の実際の位置から次時刻の設定位置への移動ベクトルとの内積を求め、得られた数値を用いて予め設定された移動速度指令を修正する演算手段、及び現時刻の設定位置から次時刻の設定位置への移動ベクトルと、現時刻の実際の位置から次時刻の設定位置への移動ベクトルとの外積を求め、得られたベクトルの方向と大きさを用いて予め設定された操舵角速度指令を修正する演算手段を有することを特徴とする移動体の移動制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車輪で移動する移動体の移動制御装置に関するものである。
【背景技術】
【０００２】
従来の移動体の移動制御装置では、移動体の現在位置を認識する位置認識装置と、予め設定さた移動経路と移動速度と操舵角速度に関する情報を記憶する情報記憶装置と、位置認識装置と情報記憶装置からの情報に基づいて所定時間毎に移動速度と操舵角速度を演算し指令する演算装置と、演算装置からの指令に基づいて移動速度と操舵角速度を制御する速度制御装置を備えている。
【０００３】
演算装置は、所定時間毎に起動し、移動体の現時刻の実際の位置と次に演算を行う時刻（以下「次時刻」という）の設定位置の情報を位置認識装置と情報記憶装置から取得し、所要の移動速度と操舵角速度とを求め、速度制御装置に指令を送る。
移動速度は、図６に示す現時刻の実際の位置Ｐと次時刻の設定位置Ｐ2 との間の距離を計算し、その距離を現時刻から次時刻までの時間ｔで除算することで求める。具体的な演算手順は図７に示す通りである。
【０００４】
操舵角速度は、図８に示す現時刻の実際の位置Ｐから次時刻の設定位置Ｐ2 までの移動ベクトルｂを求め、次にその移動ベクトルｂの方向角度θを求め、更にその移動ベクトルｂの方向角度θと移動体の現在の進行方向角度ψとの差ξを時間ｔで除算することで求める。具体的な演算手順は図９に示す通りである。
しかし、この移動制御装置では、図７に示すように、現時刻の実際の位置Ｐと次時刻の設定位置Ｐ2 との間の距離を計算するとき、それぞれ位置Ｐ、Ｐ2 の座標の要素毎に減算を行ってから二乗し、それらを加算後平方根を求める。移動速度はこれを時間ｔで除算する。さらに、速度制御装置が実速度での指令に対応していないので、指令できる数値を得るため、パラメータの乗算を行う。
【０００５】
また、この移動制御装置では、図９に示すように、現時刻の実際の位置Ｐから次時刻の設定位置Ｐ2 までの移動ベクトルｂを求め、次にその移動ベクトルｂの要素をアークタンジェント関数に代入し、方向角度θを求める。現在の進行方向角度ψは、車輪の向きを検出するエンコーダからの戻り信号にパラメータを乗算して角度に換算する。
図８（ｂ）に示すように後向きに走行している（車輪を逆転させている）場合には、図８（ａ）と同じ方向のずれでも操舵は反転させなければならないので、車輪の逆転を判断して進行方向角度ψを１８０度反転する。操舵角速度は移動ベクトルｂの方向角度θと移動体の現在の進行方向角度ψとの差ξを時間ｔで除算することで求める。そして、速度制御装置が指令できる数値を得るために更にパラメータの乗算を行う。
【０００６】
従って、移動速度及び操舵角速度の演算に時間がかかり、制御性がよくないという問題がある。
そこで、移動体の位置を求める位置認識装置と、移動体の予定経路を各点におけ移動方向と速さを共に記憶する記憶装置と、演算装置と、駆動制御装置とを備え、演算装置が現在位置から最短距離にある予定経路上の対応点における移動方向と速さを基準としてこの基準値に徐々に近付くような移動体移動方向と速さを設定し、駆動制御装置がこの設定方向と速さに基づいて移動体を予定経路に復帰させるように移動制御する移動体経路制御装置も提案されている（特許文献１参照）。
【０００７】
しかし、この移動体経路制御装置も予定経路に復帰させる方向と速度設定の演算が複雑で、復帰のための最適パラメータ算出に大量の計算が必要である。
【特許文献１】特開２０００−８９８２６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００８】
本発明は、移動体の移動制御における上記問題を解決するものであって、時間のかかる平方根の計算やアークタンジェントの計算などを行う必要がなく、後向きに走行しているか否かの判断も不要で、移動速度と操舵角速度の演算を速やかに行うことができ制御性のよい移動体の移動制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
本発明の移動体の移動制御装置は、移動体の現在位置を認識する位置認識装置と、予め設定された移動体の移動経路と移動速度と操舵角速度に関する情報を記憶する情報記憶装置と、所定時間毎に位置認識装置と情報記憶装置からの情報に基づいて移動速度と操舵角速度を演算し指令する演算装置と、演算装置からの移動速度指令に基づいて移動速度を制御する移動速度制御装置と、演算装置からの操舵角速度指令に基づいて操舵角速度を制御する操舵角速度制御装置とを備え、前記演算装置が、現時刻の設定位置から次時刻の設定位置への移動ベクトルと、現時刻の実際の位置から次時刻の設定位置への移動ベクトルとの内積を求め、得られた数値を用いて予め設定された移動速度指令を修正する演算手段、及び現時刻の設定位置から次時刻の設定位置への移動ベクトルと、現時刻の実際の位置から次時刻の設定位置への移動ベクトルとの外積を求め、得られたベクトルの方向と大きさを用いて予め設定された操舵角速度指令を修正する演算手段を有している。
【００１０】
この移動体の移動制御装置は、現時刻の実際の位置から次時刻の設定位置への移動ベクトルの、予め設定された現時刻の設定位置から次時刻の設定位置への移動ベクトルと平行な成分を、現時刻の実際の位置における移動速度として逐次速度制御を行う。
このため、演算装置が、現時刻の設定位置から次時刻の設定位置への移動ベクトルと、現時刻の実際の位置から次時刻の設定位置への移動ベクトルとの内積を求め、得られた数値を用いて予め設定された移動速度指令を修正する。時間のかかる平方根の計算を行う必要はないので、移動速度の演算を速やかに行うことができる。
【００１１】
また、演算装置が、現時刻の設定位置から次時刻の設定位置への移動ベクトルと、現時刻の実際の位置から次時刻の設定位置への移動ベクトルとの外積を求め、得られたベクトルの方向と大きさを用いて予め設定された操舵角速度指令を修正する演算手段を有することにより、現時刻の実際の位置から次時刻の設定位置への移動ベクトルの、現時刻の設定位置から次時刻の設定位置への移動ベクトルに垂直な成分のベクトルを、現時刻の実際の位置における操舵角速度の修正値として、操舵角速度制御を行う。
操舵角速度の修正において、アークタンジェントの計算などを行う必要がなく、後向きに走行しているか否かの判断も不要であるので、操舵角速度の演算を速やかに行うことができる。
【発明の効果】
【００１２】
本発明の移動体の移動制御装置は、時間のかかる平方根の計算やアークタンジェントの計算などを行う必要がなく、後向きに走行しているか否かの判断も不要で、移動速度と操舵角速度の演算を速やかに行うことができ制御性が向上する。
【発明を実施するための最良の形態】
【００１３】
図１は本発明の実施の一形態である移動体の移動制御装置の構成を示すブロック図、図２は移動速度演算の説明図、図３は移動速度演算の手順図、図４は操舵角速度演算の説明図、図５は操舵角速度演算の手順図である。
この移動体の移動制御装置は、位置認識装置１と、情報記憶装置２と、演算装置３と、移動速度制御装置４と、操舵角速度制御装置５とを備えている。
【００１４】
位置認識装置１は、車輪で移動する移動体の内外のセンサー（図示略）からの信号を受信して移動体の現在位置を認識する。情報記憶装置２は、予め設定された移動体の移動経路と、所定時間毎の設定時刻における移動体の位置と、移動速度と操舵角速度に関する情報を記憶する。
演算装置３は、所定時間毎に、位置認識装置１と情報記憶装置２からの情報に基づいて、移動速度指令値と操舵角速度指令値を演算し、移動速度指令と操舵角速度指令を出力する。
【００１５】
移動速度制御装置４は、演算装置３からの移動速度指令に基づいて移動体の移動速度を制御する。操舵角速度制御装置５は、演算装置３からの操舵角速度指令に基づいて移動体の操舵角を制御する。
この移動体の移動制御装置では、図２に示すように、移動体の現時刻の実際の位置Ｐが予め設定された経路上の現時刻の設定位置Ｐl と異なるとき、現時刻の実際の位置Ｐから次時刻の設定位置Ｐ2 への移動ベクトルｂの、予め設定された現時刻の設定位置Ｐl から次時刻の設定位置Ｐ2 への移動ベクトルａと平行な成分ｃを、現時刻の実際の位置Ｐにおける移動速度に設定して逐次速度制御を行う。
【００１６】
ここで、
【数１】
2005322156.tif 000003
であり、
【００１７】
【数２】
2005322156.tif 000004
であるので、
【００１８】
【数３】
2005322156.tif 000005
で求められる。
【００１９】
実際の演算手順は、図３に示す通りであり、移動ベクトルａと移動ベクトルｂの要素毎に、次時刻の設定位置Ｐ2 の座標値から現時刻の設定位置Ｐl の座標値を減算し、次時刻の設定位置Ｐ2 の座標値から現時刻の実際の位置Ｐの座標値を減算し、要素毎の差値同士の積を加算して内積を求める。
この内積の値を移動ベクトルａの大きさで除算して得られるｃの値を現時刻の実際の位置Ｐにおける移動速度とし、予め移動ベクトルａの大きさに対応して設定されている移動速度指令値をｃの値を用いて修正する。
移動ベクトルａの大きさは、経路を設定したときに、移動経路上のすべての時刻において計算可能なので、移動開始前に数値として計算を済ませておく。
【００２０】
この演算においては時間のかかる平方根の計算を行う必要はない。
移動体の移動方向が直線的で、現時刻の実際の位置Ｐの予め設定された経路上の現時刻の設定位置Ｐl に対する誤差が主として前後方向に生じ、左右方向への誤差が無視できるようなら速度制御のみで支障ないが、左右方向への誤差が無視できない場合もあるので、操舵角速度の制御を併せて行う。
【００２１】
この移動体の移動制御装置では、図４に示すように、移動体の現時刻の実際の位置Ｐが予め設定された経路上の現時刻の設定位置Ｐl と異なるとき、現時刻の実際の位置Ｐから次時刻の設定位置Ｐ2 への移動ベクトルｂの、予め設定された現時刻の設定位置Ｐl から次時刻の設定位置Ｐ2 への移動ベクトルａに垂直な成分のベクトルｄを、現時刻の実際の位置Ｐにおける操舵角速度の修正値として用い、操舵角速度制御を行う。
【００２２】
ここで、
【数４】
2005322156.tif 000006
（ｅ：単位ベクトル）であり、
【００２３】
【数５】
2005322156.tif 000007
であるので、
【００２４】
【数６】
2005322156.tif 000008
で求められる。
【００２５】
ベクトルｄは経路情報で設定されている進行方向に対して垂直な向きを有するので、操舵角速度の修正値として用いることができる。
ベクトルｄの大きさは、移動体の現時刻の実際の位置Ｐと予め設定された経路上の現時刻の設定位置Ｐl との誤差が大きいほど大きくなる。ベクトルｄの正負の符号は、現時刻の実際の位置Ｐが予め設定された現時刻の設定位置Ｐl より進行方向に対して左右どちら側にあるかを表す。
【００２６】
実際の計算手順は、図５に示す通りであり、ベクトルａとベクトルｂは既に移動速度制御のために計算済みであるので、その外積を求め、操舵角速度に変換するパラメータｎを乗算することで、操舵角速度の修正値を得る。
設定された操舵角速度指令値にこの修正値を加算することで、操舵角速度指令値を得る。
操舵角速度の補正において、アークタンジェントの計算などを行う必要がなく、後向きに走行しているか否かの判断も不要であるので、操舵角速度の演算を速やかに行うことができる。
【図面の簡単な説明】
【００２７】
【図１】本発明の実施の一形態である移動体の移動制御装置の構成を示すブロック図である。
【図２】移動速度演算の説明図である。
【図３】移動速度演算の手順図である。
【図４】操舵角速度演算の説明図である。
【図５】操舵角速度演算の手順図である。
【図６】従来の移動速度演算の説明図である。
【図７】従来の移動速度演算の手順図である。
【図８】従来の操舵角速度演算の説明図である。
【図９】従来の操舵角速度演算の手順図である。
【符号の説明】
【００２８】
１      位置認識装置
２      情報記憶装置
３      演算装置
４      移動速度制御装置
５      操舵角速度制御装置
Ｐ      実際の位置
Ｐl     現時刻の設定位置
Ｐ2     次時刻の設定位置
【図１】
2005322156.tif 000009
【図２】
2005322156.tif 000010
【図３】
2005322156.tif 000011
【図４】
2005322156.tif 000012
【図５】
2005322156.tif 000013
【図６】
2005322156.tif 000014
【図７】
2005322156.tif 000015
【図８】
2005322156.tif 000016
【図９】
2005322156.tif 000017