JP7040308B2 - 無人搬送車の走行制御装置及び走行制御方法 - Google Patents

Description

本発明は、無人搬送車の走行制御装置及び走行制御方法に関する。

従来より、周りの環境を探索しながら目標や障害物等を検知して、移動経路を決定し、これに従って移動することにより、自律走行を行う無人搬送車が知られている。
無人搬送車は、何らかの方法で自己位置を推定し、目的地までの移動経路を決定する。

無人搬送車の自己位置の推定手法としては、オドメトリを用いたデッドレコニングと、地図のマッチングが挙げられる。
オドメトリを用いたデッドレコニングは、無人搬送車の駆動輪の回転数や旋回角度等から無人搬送車の移動量や角度変化量をオドメトリデータとして算出し、これを前回の無人搬送車の位置や姿勢角度に積算して、現在の無人搬送車の位置や姿勢角度を更新するものである。
また、地図のマッチングにおいては、無人搬送車に設けられたレーザレンジファインダー等のセンサにより周りの環境を探索、分析して局所地図を作成し、これを無人搬送車内に予め格納されている大域地図と照合、マッチングすることで、作成した局所地図に該当する大域地図上の位置を決定する。
オドメトリにおいては、路面状況や車輪のスリップ、車輪の摩耗や、無人搬送車の固体に依存した精度のばらつき等により誤差が生じることがあり、正確な自己位置推定が容易ではない。また、オドメトリデータの積算によりこれらの誤差は累積される。このため、オドメトリに地図のマッチングを組み合わせることにより、自己位置推定の精度を向上させることが、行われている。

例えば、特許文献１には、次のような移動体誘導装置が開示されている。
より詳細には、この移動体誘導装置は、周囲に存在する構造物の形状を示す形状情報を取得し、車輪の回転角に基づいて現在位置及び移動方向を含む移動体情報を取得し、これら形状情報と移動体情報とに基づいて周囲の環境を示す環境地図を作成する。
移動体誘導装置はまた、環境地図に含まれる構造物の現実の位置及び形状を示す構造物情報を取得し、環境地図と構造物情報とに基づいて現実の構造物に対する環境地図上の構造物のずれ量を演算する。移動体誘導装置は、このずれ量に基づいて移動体情報に含まれる誤差を特定し、この誤差に基づいて移動方向を決定する。

特開２０１３－２５０７９５号公報

特許文献１に開示された移動体誘導装置は、上記のように、周囲に存在する構造物の形状を示す形状情報、すなわち局所地図と、構造物情報、すなわち大域地図とを比較して演算されたずれ量を基に、誤差を特定している。
このような構成においては、構造物が周囲に存在しない場合等には、無人搬送車に登録された大域地図との十分なマッチングを行い得るだけの情報を有する局所地図を作成することができないため、ずれ量を演算し誤差を特定することができない。
仮に、周囲に構造物が存在している場合においても、人間や他の無人搬送車等が周囲を通過して局所地図の作成に失敗することがある。あるいは、無人搬送車の移動経路近辺に貨物等が一時的に仮設されて、無人搬送車に登録されている大域地図に対応するような局所地図が作成されないこともある。このような場合においても、大域地図とのマッチングによって正確に自己位置を推定することができない。
したがって、地図のマッチングに依らず、オドメトリによる自己位置推定の精度を向上させることが望まれている。

本発明が解決しようとする課題は、オドメトリによる自己位置推定の精度を向上可能な、無人搬送車の走行制御装置及び走行制御方法を提供することである。

本発明は、上記課題を解決するため、以下の手段を採用する。すなわち、本発明は、無人搬送車の走行制御装置であって、前記無人搬送車のオドメトリデータとして、前記オドメトリデータを前回取得した前回時点からの、前記無人搬送車の前記前回時点における前方向に向けての移動量である前方向移動量、前記前方向に直交する横方向に向けての移動量である横方向移動量、及び、前記無人搬送車の姿勢角度の変化量である角度変化量を取得するオドメトリデータ取得部と、前記前方向移動量、前記横方向移動量、及び前記角度変化量の各々を補正し、補正後のオドメトリデータを計算するオドメトリデータ補正部と、前記補正後のオドメトリデータを積算して前記無人搬送車の自己位置を推定する自己位置推定部と、を備えた制御部を備えている、無人搬送車の走行制御装置を提供する。

また、本発明は、無人搬送車の走行制御方法であって、前記無人搬送車のオドメトリデータとして、前記オドメトリデータを前回取得した前回時点からの、前記無人搬送車の前記前回時点における前方向に向けての移動量である前方向移動量、前記前方向に直交する横方向に向けての移動量である横方向移動量、及び、前記無人搬送車の姿勢角度の変化量である角度変化量を取得し、前記前方向移動量、前記横方向移動量、及び前記角度変化量の各々を補正し、補正後のオドメトリデータを計算し、前記補正後のオドメトリデータを積算して前記無人搬送車の自己位置を推定する、無人搬送車の走行制御方法を提供する。

本発明によれば、オドメトリによる自己位置推定の精度を向上可能な、無人搬送車の走行制御装置及び走行制御方法を提供することができる。

本発明の実施形態における無人搬送車の模式的な平面図である。 上記実施形態における無人搬送車の走行制御装置のブロック図である。 上記無人搬送車の走行制御装置における、大域地図及び無人搬送車の座標系の説明図である。 上記無人搬送車の走行制御装置における、補正後のオドメトリデータの説明図である。 上記実施形態における無人搬送車の走行制御方法のフローチャートである。 無人搬送車の横ずれの説明図である。 上記実施形態の第１変形例における、無人搬送車の走行制御装置のブロック図である。 上記実施形態の第２変形例における、無人搬送車の走行制御装置のブロック図である。

以下、本発明の実施形態について図面を参照して詳細に説明する。
本実施形態における無人搬送車の走行制御装置は、無人搬送車のオドメトリデータとして、オドメトリデータを前回取得した前回時点からの、無人搬送車の前回時点における前方向に向けての移動量である前方向移動量、前方向に直交する横方向に向けての移動量である横方向移動量、及び、無人搬送車の姿勢角度の変化量である角度変化量を取得するオドメトリデータ取得部と、前方向移動量、横方向移動量、及び角度変化量の各々を補正し、補正後のオドメトリデータを計算するオドメトリデータ補正部と、補正後のオドメトリデータを積算して無人搬送車の自己位置を推定する自己位置推定部と、を備えた制御部を備えている。

図１は、本実施形態における無人搬送車の模式的な平面図である。無人搬送車１は、筐体２、２つの駆動輪３、２つのモータ４、及び２つの自在輪５を備えている。
筐体２は、平面視上略矩形に形成されている。無人搬送車１の前方向Ｆは、図１においては左方向であり、筐体２は、矩形形状の長辺が前方向Ｆと一致するように設けられている。

駆動輪３は、第１駆動輪３Ａと第２駆動輪３Ｂを備えている。第１駆動輪３Ａは、筐体２の前側の左側に、第２駆動輪３Ｂは後側の右側に、それぞれ設けられている。駆動輪３Ａ、３Ｂの各々には、図示されない対応する操舵機構が設けられており、駆動輪３Ａ、３Ｂの各々は旋回軸３ａを中心に互いに独立して旋回することが可能な構成となっている。このため、例えば第１駆動輪３Ａと第２駆動輪３Ｂの各々が、前方向Ｆとは異なる同一方向に旋回している場合においては、無人搬送車１は、前方向Ｆとは異なる当該方向を進行方向として走行する。
モータ４は、第１モータ４Ａと第２モータ４Ｂを備えている。第１モータ４Ａは第１駆動輪３Ａに対応し、第１駆動輪３Ａを駆動する。第２モータ４Ｂは第２駆動輪３Ｂに対応し、第２駆動輪３Ｂを駆動する。

自在輪５は、筐体２の前側の右側と、後側の左側に、それぞれ設けられている。自在輪５は、例えばキャスタ式の車輪であり、無人搬送車１が駆動輪３Ａ、３Ｂにより決定された方向に進むに際し、これに追従するように回転自在に設けられている。
自在輪５は、図示されないばねにより走行面に対して付勢されるように設けられている。これにより、２つの駆動輪３Ａ、３Ｂを確実に走行面に接地させるとともに、この状態において２つの自在輪５の各々をも走行面に接地させることが可能となっている。

上記のような無人搬送車１に、走行制御装置１０が設けられている。走行制御装置１０は、エンコーダ１１、ポテンショメータ１２、距離計測器１３、及び制御部２０を備えている。

エンコーダ１１は、第１エンコーダ１１Ａと第２エンコーダ１１Ｂを備えている。
第１エンコーダ１１Ａは、第１モータ４Ａに対応して設けられており、第１モータ４Ａの出力回転軸の回転量に基づいたパルス信号を第１モータ回転信号として出力し、制御部２０へ送信する。
第２エンコーダ１１Ｂは、第２モータ４Ｂに対応して設けられており、第２モータ４Ｂの出力回転軸の回転量に基づいたパルス信号を第２モータ回転信号として出力し、制御部２０へ送信する。

ポテンショメータ１２は、第１ポテンショメータ１２Ａと第２ポテンショメータ１２Ｂを備えている。
第１ポテンショメータ１２Ａは、第１駆動輪３Ａに対応して設けられており、第１駆動輪３Ａの旋回角を電圧に変換し、第１駆動輪旋回角信号として制御部２０へ送信する。
第２ポテンショメータ１２Ｂは、第２駆動輪３Ｂに対応して設けられており、第２駆動輪３Ｂの旋回角を電圧に変換し、第２駆動輪旋回角信号として制御部２０へ送信する。

距離計測器１３は、筐体２の上部に設けられている。本実施形態においては、距離計測器１３は、レーザ発振機により発振されたレーザ光を照射し、障害物が有る場合には障害物から反射した反射光を受信することで障害物までの距離を計測する、レーザレンジファインダー（以下、ＬＲＦと記載する）である。ＬＲＦ１３は、本実施形態においては特に、ＬＲＦ１３の位置する水平面内において、３６０°の全周囲に位置する障害物までの距離を計測可能なものである。この全周囲に位置する障害物までの距離は、より詳細には、３６０°の全周囲を走査して取得した、ＬＲＦ１３を原点とする２次元平面上における障害物の座標情報である。障害物の座標情報は、例えば前方向Ｆを基準とした角度と、当該角度における障害物までの距離により表される極座標の列である。
このように、ＬＲＦ１３は、無人搬送車１の周囲に位置する障害物までの距離を計測し、制御部２０へ送信する。

図２は、無人搬送車の走行制御装置のブロック図である。無人搬送車１の制御部２０は、環境データ作成部２１、ＳＬＡＭ自己位置推定部２２、オドメトリデータ取得部２３、オドメトリデータ補正部２４、補正パラメータ設定部２５、自己位置推定部２６、及び記憶部２７を備えている。

環境データ作成部２１は、ＬＲＦ１３から、無人搬送車１の周囲に位置する障害物までの距離を受信する。
環境データ作成部２１は、この距離データを基に、無人搬送車１の周囲の局所的な地図である局所地図を作成し、記憶部２７へ送信し、記憶する。

ＳＬＡＭ自己位置推定部２２は、ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）誘導方式を基に、無人搬送車１の自己位置を推定する。
より詳細には、ＳＬＡＭ自己位置推定部２２は、記憶部２７に予め記憶されている大域地図と、環境データ作成部２１により記憶部２７に記憶された局所地図を取得する。ＳＬＡＭ自己位置推定部２２は、局所地図を回転させながら、局所地図を大域地図上で移動させ、互いに重ねあわせて照合することにより、局所地図が大域地図上で最も一致する、すなわちマッチングする配置位置、及び回転角度を取得する。制御部２０は、この配置位置及び回転角度を基にして、無人搬送車１の自己位置を推定する。
ＳＬＡＭ自己位置推定部２２は、推定した自己位置を、図３を用いて後述するような、大域地図上の座標値として記憶部２７に記憶する。

このように、環境データ作成部２１とＳＬＡＭ自己位置推定部２２は、地図のマッチングにより、無人搬送車１の自己位置を推定する。
しかし、ＬＲＦ１３により検出可能な対象物が周囲に存在しない場合等には、局所地図は大域地図と照合して自己位置を推定し得るだけの十分な情報を有しない可能性がある。
また、周囲に対象物が存在していたとしても、人間や他の無人搬送車等が周囲を通過しして局所地図の作成に失敗することがある。あるいは、無人搬送車１の移動経路近辺に貨物等が一時的に仮設されて、無人搬送車に登録されている大域地図に対応するような局所地図が作成されないこともある。このような場合においては、局所地図と大域地図の照合に失敗することがある。
このような、地図のマッチングにより無人搬送車１の自己位置が推定できない場合には特に、無人搬送車１の制御部２０は、自己位置の推定結果として、オドメトリにより導出された結果を使用する。
以下、制御部２０の、オドメトリによる自己位置推定に関する構成を説明する。

制御部２０は、オドメトリにより自己位置を推定する場合には、自己位置の推定と、推定された自己位置からの無人搬送車１の移動量と姿勢角度の変化量の計算、及びこれら変化量の自己位置への適用による自己位置情報の更新の処理を繰り返す。このため、記憶部２７には、前回の処理において推定された、大域地図上の最新の自己位置、すなわち前回位置が記憶されている。
オドメトリデータ取得部２３は、ＳＬＡＭ自己位置推定部２２または後述する自己位置推定部２６により最後に推定されて記憶部２７に記憶された前回位置を取得する。
オドメトリデータ取得部２３は、第１エンコーダ１１Ａ、第２エンコーダ１１Ｂ、第１ポテンショメータ１２Ａ、第２ポテンショメータ１２Ｂの各々から、前回位置が推定された時点以降における、第１モータ回転信号、第２モータ回転信号、第１駆動輪旋回角信号、第２駆動輪旋回角信号をそれぞれ受信する。
オドメトリデータ取得部２３は、第１モータ回転信号、第２モータ回転信号、第１駆動輪旋回角信号、第２駆動輪旋回角信号を基に、前回位置が推定された時点以降における、無人搬送車１の移動量と、姿勢角度すなわち向きの変化量を計算する。
オドメトリデータ取得部２３は、前回位置に対して、前回位置が推定された時点以降における無人搬送車１の移動量と姿勢角度の変化量を適用し、現在の暫定的な自己位置である暫定位置を計算する。

図３は、大域地図及び無人搬送車１の座標系の説明図である。ｘ軸とｙ軸により示されるｘｙ座標系は、大域地図上の座標系である。また、ｘｙ座標系内に示されている、ｘ´軸とｙ´軸により示されるｘ´ｙ´座標系は、無人搬送車１を中心とした局所的な座標系である。ｘ´軸の延伸する方向は、前回位置を推定した時点における無人搬送車１の前方向Ｆに一致しており、ｙ´軸の延伸する方向は、図１に示される、前方向Ｆに対して直交する横方向Ｗに一致している。

図３において、前回位置はＰ_１（ｘ_１、ｙ_１、θ_１）として示されている。ここで、ｘ_１は、前回位置Ｐ_１の、大域地図上のｘｙ座標系におけるｘ座標、ｙ_１はｙ座標である。θ_１は、前回位置を推定した際における、ｘ軸と、無人搬送車１の前方向Ｆであるｘ´軸とのなす角度である。
図３において、暫定位置はＰ_２（ｘ_２、ｙ_２、θ_２）として示されている。ここで、ｘ_２は、暫定位置Ｐ_２の、大域地図上のｘｙ座標系におけるｘ座標、ｙ_２はｙ座標である。θ_２は、暫定位置を推定した際における、ｘ軸と、無人搬送車１の前方向Ｆである方向Ｄとのなす角度である。
ｘ´ｙ´座標系の原点は、前回位置Ｐ_１となっている。

オドメトリデータ取得部２３は、前回位置の大域地図上のｘｙ座標系における座標値Ｐ_１（ｘ_１、ｙ_１、θ_１）と、上記のように、無人搬送車１の移動量と姿勢角度の変化量を前回位置Ｐ_１に適用することにより計算された、暫定位置の座標値Ｐ_２（ｘ_２、ｙ_２、θ_２）から、無人搬送車１のオドメトリデータを取得する。
より詳細には、オドメトリデータ取得部２３は、次式により、オドメトリデータとして、無人搬送車１上のｘ´ｙ´局所座標系における、オドメトリデータを前回取得した前回時点からの、無人搬送車１の前回時点における前方向Ｆに向けての移動量である前方向移動量ｘ_ｄ´、前方向に直交する横方向に向けての移動量である横方向移動量ｙ_ｄ´、及び、無人搬送車の姿勢角度の変化量である角度変化量θ_ｄ´を取得する。
前方向移動量ｘ_ｄ´、横方向移動量ｙ_ｄ´、角度変化量θ_ｄ´は、換言すれば、無人搬送車１の位置姿勢の変化量を、前回位置Ｐ_１における無人搬送車１上の直交する２つの方向と角度の各々の変化量として分解したものである。

オドメトリデータ取得部２３は、上記のようにして取得したオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を、記憶部２７へ記憶する。

オドメトリデータ補正部２４は、オドメトリデータ取得部２３により記憶部２７に記憶されたオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を取得する。
オドメトリデータ補正部２４は、オドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を補正し、補正後のオドメトリデータを計算する。
オドメトリを用いた自己位置の推定においては、路面状況や駆動輪３のスリップ、駆動輪３の摩耗や、無人搬送車１の固体に依存した精度のばらつき等により誤差が生じることがある。オドメトリデータ取得部２３は、この誤差の低減を目的として、オドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を補正する。
オドメトリデータ補正部２４は、実際には、低減対象となる誤差を計算し、これをオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´に加算することで、補正後のオドメトリデータを計算する。

ここでは、無人搬送車１における連続する自己位置推定処理間の時間間隔が小さく、このため、無人搬送車１の前回位置Ｐ_１から暫定位置Ｐ_２までの移動量と角度変化量が微小であると仮定する。このような仮定の下で、オドメトリデータ補正部２４は、誤差を、上記位置姿勢の変化量と同様に、前方向誤差ε_ｘ、横方向誤差ε_ｙ、及び角度誤差ε_θの３種類の誤差として分解し、その各々の値を計算する。
これらの各誤差ε_ｘ、ε_ｙ、ε_θは、無人搬送車１の前回位置Ｐ_１から暫定位置Ｐ_２までの移動量ｌ´、すなわち前方向移動量ｘ_ｄ´と横方向移動量ｙ_ｄ´の二乗和の平方根に依存するものと考える。この場合に、各誤差ε_ｘ、ε_ｙ、ε_θは、前方向誤差ε_ｘに関する係数ａ_ｘ、横方向誤差ε_ｙに関する係数ａ_ｙ、及び角度誤差ε_θに関する係数ａ_θにより構成される、所定の補正パラメータａ_ｘ、ａ_ｙ、ａ_θを基に、次式により演算される。

補正パラメータ設定部２５は、補正パラメータａ_ｘ、ａ_ｙ、ａ_θを保持する。補正パラメータ設定部２５は、補正パラメータａ_ｘ、ａ_ｙ、ａ_θをオドメトリデータ補正部２４に送信し、オドメトリデータ補正部２４はこれを用いて上記数式２を計算する。
これら補正パラメータａ_ｘ、ａ_ｙ、ａ_θは、例えばシミュレーションにより事前に所定の値が設定されている。無人搬送車１の位置姿勢の変化量が分解された前方向移動量、横方向移動量、角度変化量は、角度変化量、前方向移動量、横方向移動量の順で、無人搬送車１の移動量や姿勢角度に大きく影響するものと考えられる。したがって、シミュレーションによりこれらの値を設定するに際し、影響が大きい順に、すなわち角度誤差ε_θに関する係数ａ_θ、前方向誤差ε_ｘに関する係数ａ_ｘ、横方向誤差ε_ｙに関する係数ａ_ｙの順に設定するのが望ましい。
このように、オドメトリデータ補正部２４は、シミュレーションにより事前に所定の値が設定された補正パラメータａ_ｘ、ａ_ｙ、ａ_θを基に誤差ε_ｘ、ε_ｙ、ε_θを計算し、この誤差ε_ｘ、ε_ｙ、ε_θを基に後述するようにオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を補正する。

また、各誤差ε_ｘ、ε_ｙ、ε_θにおいて使用されている変数ｆは、前方向移動量ｘ_ｄ´をこの絶対値で除算した値であり、無人搬送車１がｘ´方向に向けて前進したか、後進したかを判断するための符号である。上記のように、無人搬送車１における連続する自己位置推定処理間の時間間隔が小さいと仮定した場合においては、無人搬送車１の方向は大きく変わらないと想定できる。したがって、誤差ε_ｘ、ε_ｙ、ε_θを計算する際に、無人搬送車１の進行方向を考慮するには、上記のように前進か後進かが反映できれば十分である。
このように、オドメトリデータ補正部２４は、オドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を基に計算された無人搬送車１の移動量ｌ´と、無人搬送車１が前進中か後進中かを判定する前進後進判定変数ｆを基に、誤差ε_ｘ、ε_ｙ、ε_θを計算する。

オドメトリデータ補正部２４は、上記のように数式２により計算された誤差ε_ｘ、ε_ｙ、ε_θを用いて、次式により、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を計算する。

このように、オドメトリデータ補正部２４は、オドメトリデータの、すなわち前方向移動量ｘ_ｄ´、横方向移動量ｙ_ｄ´、及び角度変化量θ_ｄ´の各々の誤差ε_ｘ、ε_ｙ、ε_θを計算し、この誤差ε_ｘ、ε_ｙ、ε_θを前方向移動量ｘ_ｄ´、横方向移動量ｙ_ｄ´、及び角度変化量θ_ｄ´の各々に加算することで、前方向移動量ｘ_ｄ´、横方向移動量ｙ_ｄ´、及び角度変化量θ_ｄ´の各々を補正し、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を計算する。
図４は、図３として示した大域地図及び無人搬送車１の座標系の説明図に対し、各誤差ε_ｘ、ε_ｙ、ε_θ、及び補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を記載したグラフである。ｘ´ｙ´座標系において、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´に相当する座標はＰ_２ｃと記されており、ｘ´ｙ´座標系におけるこの座標が、無人搬送車１が実際に位置しているとオドメトリデータ補正部２４が計算した位置である。

オドメトリデータ補正部２４は、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を、記憶部２７へ記憶する。

自己位置推定部２６は、オドメトリデータ補正部２４により記憶部２７に記憶された補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を取得する。
自己位置推定部２６はまた、記憶部２７から、前回位置Ｐ_１（ｘ_１、ｙ_１、θ_１）を取得する。
自己位置推定部２６は、前回位置Ｐ_１（ｘ_１、ｙ_１、θ_１）をあらわすｘｙ座標系における各座標値に対して、次式のように、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´をｘｙ座標系に座標変換した値を加算することにより、座標系ｘｙにおける座標Ｐ_２ｃの座標値（ｘ_２ｃ、ｙ_２ｃ、θ_２ｃ）を計算し、補正された、大域地図データ上の自己位置を推定する。

このように、自己位置推定部２６は、補正された位置Ｐ_２ｃを計算し、自己位置として記憶部２７へ記憶する。
制御部２０は、上記の処理を繰り返す。すなわち、補正された位置Ｐ_２ｃを計算した所定時間の後には、補正された位置Ｐ_２ｃを計算した後に測定された第１モータ回転信号、第２モータ回転信号、第１駆動輪旋回角信号、第２駆動輪旋回角信号を受信する。このとき、最後に計算された補正された位置Ｐ_２ｃを前回位置とし、受信した各信号を基に新たにオドメトリデータ及び補正後のオドメトリデータを計算した上で、数式４により補正された位置を新たに計算する。
このように、自己位置推定部２６は、上記の処理を繰り返すことにより、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を積算して無人搬送車１の自己位置を推定する。

記憶部２７には、大域地図が予め格納されている。また、記憶部２７には、環境データ作成部２１、ＳＬＡＭ自己位置推定部２２、オドメトリデータ取得部２３、オドメトリデータ補正部２４、及び自己位置推定部２６の、各々の計算結果が記憶される。

本実施形態において、制御部２０は、基本的には、オドメトリデータ取得部２３により取得されたオドメトリによって、自己位置を推定する。
制御部２０は、環境データ作成部２１とＳＬＡＭ自己位置推定部２２による、地図のマッチングによる自己位置推定が可能である場合には、この結果を基に、オドメトリデータ取得部２３により取得されたオドメトリによる自己位置推定結果を補正する。
制御部２０は、地図のマッチングによる自己位置推定が可能でない場合に、オドメトリデータ補正部２４、補正パラメータ設定部２５、及び自己位置推定部２６を用いて説明した手法によって、オドメトリデータ取得部２３により取得されたオドメトリによる自己位置推定結果を補正する。

次に、図１～図４、及び図５を用いて、上記の無人搬送車の走行制御方法を説明する。図５は、無人搬送車の走行制御方法のフローチャートである。
本走行制御方法は、無人搬送車のオドメトリデータとして、オドメトリデータを前回取得した前回時点からの、無人搬送車の前回時点における前方向に向けての移動量である前方向移動量、前方向に直交する横方向に向けての移動量である横方向移動量、及び、無人搬送車の姿勢角度の変化量である角度変化量を取得し、前方向移動量、横方向移動量、及び角度変化量の各々を補正し、補正後のオドメトリデータを計算し、補正後のオドメトリデータを積算して無人搬送車の自己位置を推定する。

無人搬送車１が所定の時間走行すると（ステップＳ２）、オドメトリデータ取得部２３は、ＳＬＡＭ自己位置推定部２２または後述する自己位置推定部２６により最後に推定されて記憶部２７に記憶された前回位置Ｐ_１（ｘ_１、ｙ_１、θ_１）を取得する。
オドメトリデータ取得部２３は、第１エンコーダ１１Ａ、第２エンコーダ１１Ｂ、第１ポテンショメータ１２Ａ、第２ポテンショメータ１２Ｂの各々から、前回位置Ｐ_１が推定された時点以降における、第１モータ回転信号、第２モータ回転信号、第１駆動輪旋回角信号、第２駆動輪旋回角信号をそれぞれ受信する。
オドメトリデータ取得部２３は、第１モータ回転信号、第２モータ回転信号、第１駆動輪旋回角信号、第２駆動輪旋回角信号を基に、前回位置Ｐ_１が推定された時点以降における、無人搬送車１の移動量と、姿勢角度すなわち向きの変化量を計算する。
オドメトリデータ取得部２３は、前回位置Ｐ_１に対して、前回位置Ｐ_１が推定された時点以降における無人搬送車１の移動量と姿勢角度の変化量を適用し、現在の暫定的な自己位置である暫定位置Ｐ_２を計算する。
オドメトリデータ取得部２３は、前回位置Ｐ_１と暫定位置Ｐ_２を数式１に適用して取得したオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を、記憶部２７へ記憶する（ステップＳ４）。

オドメトリデータ補正部２４は、オドメトリデータ取得部２３により記憶部２７に記憶されたオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を取得する。
オドメトリデータ補正部２４は、補正パラメータ設定部２５に保持されている補正パラメータａ_ｘ、ａ_ｙ、ａ_θを基に、数式２により誤差ε_ｘ、ε_ｙ、ε_θを計算する（ステップＳ６）。

オドメトリデータ補正部２４は更に、誤差ε_ｘ、ε_ｙ、ε_θを用いて、数式３により、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を計算する（ステップＳ８）。
オドメトリデータ補正部２４は、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を、記憶部２７へ記憶する。

自己位置推定部２６は、オドメトリデータ補正部２４により記憶部２７に記憶された補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を取得する。
自己位置推定部２６はまた、記憶部２７から、前回位置Ｐ_１（ｘ_１、ｙ_１、θ_１）を取得する。
自己位置推定部２６は、前回位置Ｐ_１（ｘ_１、ｙ_１、θ_１）をあらわすｘｙ座標系における各座標値に対して、数式４のように、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´をｘｙ座標系に座標変換した値を加算することにより、座標系ｘｙにおける座標Ｐ_２ｃの座標値（ｘ_２ｃ、ｙ_２ｃ、θ_２ｃ）を計算し、補正された、大域地図データ上の自己位置を推定する（ステップＳ１０）。
自己位置推定部２６は、補正された位置Ｐ_２ｃを計算し、自己位置として記憶部２７へ記憶する。

制御部２０は、上記の処理を繰り返す。すなわち、補正された位置Ｐ_２ｃを計算した所定時間の後には、補正された位置Ｐ_２ｃを計算した後に測定された第１モータ回転信号、第２モータ回転信号、第１駆動輪旋回角信号、第２駆動輪旋回角信号を受信する。このとき、最後に計算された補正された位置Ｐ_２ｃを前回位置とし、受信した各信号を基に新たにオドメトリデータ及び補正後のオドメトリデータを計算した上で、数式４により補正された位置を新たに計算する（ステップＳ２へ遷移）。

次に、上記の無人搬送車の走行制御装置及び走行制御方法の効果について説明する。

本実施形態の無人搬送車１の走行制御装置１０は、無人搬送車１のオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´として、オドメトリデータを前回取得した前回時点からの、無人搬送車１の前回時点における前方向Ｆに向けての移動量である前方向移動量ｘ_ｄ´、前方向Ｆに直交する横方向Ｗに向けての移動量である横方向移動量ｙ_ｄ´、及び、無人搬送車の姿勢角度の変化量である角度変化量θ_ｄ´を取得するオドメトリデータ取得部２３と、前方向移動量ｘ_ｄ´、横方向移動量ｙ_ｄ´、及び角度変化量θ_ｄ´の各々を補正し、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を計算するオドメトリデータ補正部２４と、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を積算して無人搬送車１の自己位置を推定する自己位置推定部２６と、を備えた制御部２０を備えている。
上記のような構成によれば、制御部２０は、無人搬送車１のオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を取得してこれを補正し、補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を計算する。これにより、オドメトリによる自己位置推定の精度を向上させることが可能である。
特に、無人搬送車１の位置姿勢の変化量であるオドメトリデータを、前方向移動量ｘ_ｄ´、横方向移動量ｙ_ｄ´、及び角度変化量θ_ｄ´の各々の変化量として分解し、この各々を補正するため、オドメトリデータの精度の高い補正が可能である。
また、図１を用いて説明したような本実施形態の無人搬送車１は、互いに離間して設けられた２つの駆動輪３が、互いに独立して旋回することが可能な構成となっている。図６は、このような無人搬送車１において生じる横ずれの説明図である。図６において下の位置１Ａに位置する無人搬送車１が前方向Ｆに進行する際に、２つの駆動輪３の各々が、前方向Ｆから同じ方向に、例えば左方向に微妙に回転した位置に旋回している場合を考える。本来であれば、無人搬送車１は直進しようとすると、図６において位置１Ａの直上の位置１Ｂへと進行するはずである。しかし、上記のような場合においては、無人搬送車１の筐体２自体は、その長辺方向が前方向Ｆに対して平行に位置しているにもかかわらず、無人搬送車１は実際には、位置１Ａの直上の位置１Ｂではなく、位置１Ｂから左方向にわずかにずれた位置１Ｃへ移動してしまう。このように、無人搬送車１の移動に横ずれが生じることがある。
特に、上記のような構成においては、オドメトリデータの精度の高い補正が可能であるため、上記のように生じうる無人搬送車１の横ずれを検出し補正可能である。

また、オドメトリデータ補正部２４は、オドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を基に計算された無人搬送車１の移動量ｌ´と、無人搬送車が前進中か後進中かを判定する前進後進判定変数ｆを基に、オドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´の誤差ε_ｘ、ε_ｙ、ε_θを計算し、誤差ε_ｘ、ε_ｙ、ε_θをオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´に加算して補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´を計算し、自己位置推定部２６は、制御部２７に記憶された大域地図データ上の自己位置を推定する。
上記のような構成によれば、オドメトリデータの精度の高い補正が可能である。
また、大域地図データ上の、無人搬送車１の正確な位置を推定可能である。

また、オドメトリデータ補正部２４は、事前に所定の値が設定された補正パラメータａ_ｘ、ａ_ｙ、ａ_θを基に、オドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を補正する。
上記のような構成によれば、実際の無人搬送車１の挙動を基にオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を補正するため、オドメトリデータの精度の高い補正が可能である。

また、無人搬送車１は、互いに独立して旋回可能な、複数の駆動輪３を備えている。
このような構成の無人搬送車１は、特に、重量物を積載して搬送する場合に適した構成である。上記のような走行制御装置１０は、このような無人搬送車１に対して、有効に適用可能である。

［実施形態の第１変形例］
次に、図７を用いて、上記実施形態として示した無人搬送車の走行制御装置及び走行制御方法の第１変形例を説明する。図７は、本第１変形例における無人搬送車の走行制御装置のブロック図である。本第１変形例における走行制御装置３０は、制御部３１が、補正パラメータ設定部２５の代わりに補正パラメータ計算部３５を備え、この補正パラメータ計算部３５が、ＳＬＡＭ自己位置推定部２２により推定された自己位置を基に、補正パラメータａ_ｘ、ａ_ｙ、ａ_θを計算する点が異なっている。

本変形例においては、補正パラメータ計算部３５は、ＳＬＡＭ自己位置推定部２２により推定された自己位置を補正後のオドメトリデータｘ_ｃ´、ｙ_ｃ´、θ_ｃ´とみなし、これとオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を基に、数式１～４等を用いて補正パラメータａ_ｘ、ａ_ｙ、ａ_θを変数とした方程式を解くことにより、補正パラメータａ_ｘ、ａ_ｙ、ａ_θを計算する。

本変形例においては、無人搬送車１の調子や状態が変わった場合であっても、それに応じて補正パラメータａ_ｘ、ａ_ｙ、ａ_θを計算することにより、安定して、オドメトリデータを高い精度で補正することができる。

本第１変形例が、既に説明した実施形態と同様な他の効果を奏することは言うまでもない。

本変形例を実際に適用するにあたり、上記のような補正パラメータ計算部３５により算出した結果をそのまま補正パラメータａ_ｘ、ａ_ｙ、ａ_θとして使用すると、補正パラメータａ_ｘ、ａ_ｙ、ａ_θの値の変動が大きく、無人搬送車１を適切に制御できない可能性がある。
このような場合においては、例えば、現在から一定の過去にわたる時間範囲内で計算された補正パラメータａ_ｘ、ａ_ｙ、ａ_θを対象として、これらの平均を計算し、この平均値を補正パラメータａ_ｘ、ａ_ｙ、ａ_θとして採用することで、補正パラメータａ_ｘ、ａ_ｙ、ａ_θの値を安定させることができる。
その他にも、補正パラメータａ_ｘ、ａ_ｙ、ａ_θを計算した時点における無人搬送車１の速度や旋回角度を取得し、これら速度や旋回角度毎に、補正パラメータａ_ｘ、ａ_ｙ、ａ_θの計算結果をグループ化し、各グループにおいて計算結果の平均を計算することによって、補正パラメータａ_ｘ、ａ_ｙ、ａ_θの値を安定させることもできる。この場合においては、無人搬送車１の速度や旋回角度毎に異なる補正パラメータａ_ｘ、ａ_ｙ、ａ_θを用意されている状態となり、オドメトリデータ補正部２４は、無人搬送車１の状況に応じて使用する補正パラメータａ_ｘ、ａ_ｙ、ａ_θを変更し、補正後のオドメトリデータを計算する。

［実施形態の第２変形例］
次に、図８を用いて、上記実施形態として示した無人搬送車の走行制御装置及び走行制御方法の第２変形例を説明する。図８は、本第２変形例における無人搬送車の走行制御装置のブロック図である。第２変形例は、上記第１変形例の、更なる変形例である。より詳細には、本第２変形例における走行制御装置４０は、無人搬送車１の姿勢角度を検出する姿勢検出部４８を備えている点、及び、制御部４１のオドメトリデータ取得部４３が、姿勢検出部４８により検出された姿勢角度を基に、オドメトリデータを、特に角度変化量θ_ｄ´を、計算する点が、第１変形例とは異なっている。

本変形例においては、姿勢検出部４８は、例えばジャイロセンサ４８である。ジャイロセンサ４８は無人搬送車１の姿勢角度を検出可能であるため、オドメトリデータ取得部４３は、ジャイロセンサ４８の出力する情報を基に角度変化量θ_ｄ´を導出可能である。

本変形例においては、例えばスリップ等により、無人搬送車１が急激に角度を変えた場合であっても、ジャイロセンサ４８によってこれを検出可能であるため、オドメトリデータを高い精度で補正することができる。

本第２変形例が、既に説明した実施形態と同様な他の効果を奏することは言うまでもない。

なお、本発明の無人搬送車の走行制御装置及び走行制御方法は、図面を参照して説明した上述の実施形態及び各変形例に限定されるものではなく、その技術的範囲において他の様々な変形例が考えられる。

例えば、上記実施形態においては、補正パラメータ設定部２５は補正パラメータａ_ｘ、ａ_ｙ、ａ_θをそれぞれ１つずつ保持するように説明したが、これに限られない。例えば、第１変形例として説明したように、無人搬送車１の速度や旋回角度毎に異なる補正パラメータａ_ｘ、ａ_ｙ、ａ_θを予め用意し、オドメトリデータ補正部２４が無人搬送車１の状況に応じて使用する補正パラメータａ_ｘ、ａ_ｙ、ａ_θを変更するようにすれば、オドメトリデータのより精度の高い補正が可能となる。

また、上記実施形態においては、オドメトリデータ取得部２３は、第１エンコーダ１１Ａ、第２エンコーダ１１Ｂ、第１ポテンショメータ１２Ａ、第２ポテンショメータ１２Ｂの各々から受信した信号を基に、大域地図上のｘｙ座標系での暫定位置Ｐ_２を一旦計算したうえで、前回位置Ｐ_１と暫定位置Ｐ_２の座標値を基に、数式１に従ってオドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を計算したが、これに限られない。例えば、第１エンコーダ１１Ａ、第２エンコーダ１１Ｂ、第１ポテンショメータ１２Ａ、第２ポテンショメータ１２Ｂの各々から受信した信号から、直接、オドメトリデータｘ_ｄ´、ｙ_ｄ´、θ_ｄ´を計算してもよい。

また、上記実施形態及び変形例においては、地図のマッチングにより無人搬送車１の自己位置を推定するＳＬＡＭ自己位置推定部２２と、オドメトリにより自己位置を推定する自己位置推定部２６が個別に設けられていたが、これに限られない。例えば、これらの機能を統合した自己位置推定部を設け、これが上記２種類の方法を例えば選択しながら自己位置を推定するような構成としても構わないのは、言うまでもない。

これ以外にも、本発明の主旨を逸脱しない限り、上記実施形態及び各変形例で挙げた構成を取捨選択したり、他の構成に適宜変更したりすることが可能である。

１ 無人搬送車
３、３Ａ、３Ｂ 駆動輪
１０、３０、４０ 走行制御装置
２０、３１、４１ 制御部
２１ 環境データ作成部
２２ ＳＬＡＭ自己位置推定部
２３、４３ オドメトリデータ取得部
２４ オドメトリデータ補正部
２５ 補正パラメータ設定部
２６ 自己位置推定部
３５ 補正パラメータ計算部
４８ ジャイロセンサ（姿勢検出部）
Ｆ 前方向
Ｗ 横方向
ｘ_ｄ´ 前方向移動量（オドメトリデータ）
ｙ_ｄ´ 横方向移動量（オドメトリデータ）
θ_ｄ´ 角度変化量（オドメトリデータ）
ε_ｘ、ε_ｙ、ε_θ 誤差
ｆ 前進後進判定変数
ｌ´ 移動量
ｘ_ｃ´、ｙ_ｃ´、θ_ｃ´ 補正後のオドメトリデータ

Claims (5)

1. 無人搬送車の走行制御装置であって、
   前記無人搬送車のオドメトリデータとして、前記オドメトリデータを前回取得した前回時点からの、前記無人搬送車の前記前回時点における前方向に向けての移動量である前方向移動量、前記前方向に直交する横方向に向けての移動量である横方向移動量、及び、前記無人搬送車の姿勢角度の変化量である角度変化量を取得するオドメトリデータ取得部と、
   前記前方向移動量、前記横方向移動量、及び前記角度変化量の各々を自己位置推定の精度が向上するように補正し、補正後のオドメトリデータを計算するオドメトリデータ補正部と、
   前記補正後のオドメトリデータを積算して前記無人搬送車の自己位置を推定する自己位置推定部と、
   を備えた制御部を備え、
   前記オドメトリデータ補正部は、前記オドメトリデータを基に計算された前記無人搬送車の移動量と、前記無人搬送車が前進中か後進中かを判定する前進後進判定変数を基に、前記オドメトリデータの誤差を計算し、当該誤差を前記オドメトリデータに加算して前記補正後のオドメトリデータを計算し、
   前記自己位置推定部は、前記制御部に記憶された大域地図データ上の自己位置を推定する、無人搬送車の走行制御装置。
2. 前記オドメトリデータ補正部は、事前に所定の値が設定された補正パラメータを基に、前記オドメトリデータを補正する、請求項１に記載の無人搬送車の走行制御装置。
3. ＳＬＡＭ誘導方式を基に自己位置を推定するＳＬＡＭ自己位置推定部を備え、
   前記オドメトリデータと、前記ＳＬＡＭ自己位置推定部により推定された前記自己位置を基に、補正パラメータを計算する補正パラメータ計算部を備え、
   前記オドメトリデータ補正部は、前記補正パラメータを基に、前記オドメトリデータを補正する、請求項１に記載の無人搬送車の走行制御装置。
4. ）
   当該無人搬送車の姿勢角度を検出する姿勢検出部を備え、
   前記オドメトリデータ取得部は、前記姿勢検出部により検出された前記姿勢角度を基に、前記オドメトリデータを計算する、請求項１から３のいずれか一項に記載の無人搬送車の走行制御装置。
5. 無人搬送車の走行制御方法であって、
   前記無人搬送車のオドメトリデータとして、前記オドメトリデータを前回取得した前回時点からの、前記無人搬送車の前記前回時点における前方向に向けての移動量である前方向移動量、前記前方向に直交する横方向に向けての移動量である横方向移動量、及び、前記無人搬送車の姿勢角度の変化量である角度変化量を取得し、
   前記前方向移動量、前記横方向移動量、及び前記角度変化量の各々を自己位置推定の精度が向上するように補正し、補正後のオドメトリデータを計算し、
   前記補正後のオドメトリデータを積算して前記無人搬送車の自己位置を推定し、
   前記補正後のオドメトリデータの計算は、前記オドメトリデータを基に計算された前記無人搬送車の移動量と、前記無人搬送車が前進中か後進中かを判定する前進後進判定変数を基に、前記オドメトリデータの誤差を計算し、当該誤差を前記オドメトリデータに加算することにより行われ、
   前記自己位置の推定は、記憶された大域地図データ上の自己位置を推定する、無人搬送車の走行制御方法。

Images