(57)【特許請求の範囲】
【請求項１】
与えられた目標位置を基に直交座標系の速度指令を生成する速度指令生成部と、前記直交座標系の速度指令を各駆動軸の速度指令に変換する逆運動学変換部とを備えた移動ロボット制御装置において、
前記逆運動学変換部から出力された各駆動軸速度指令にゲインを乗じて調整した値を出力する速度ゲイン乗算部と、
移動ロボットから周囲物体までの距離を計測する環境認識センサと、
前記移動ロボットの駆動機構の情報を取得する内界センサと、
直交座標系の速度指令および前記環境認識センサおよび前記内界センサによって取得した情報から駆動軸速度補正量を計算する駆動軸速度補正量計算部と、
前記駆動軸速度補正量計算部から出力された補正量に基づき前記速度ゲイン乗算部の出力値の補正を行う駆動軸補正部と、を備えたことを特徴とする移動ロボット制御装置。
【請求項２】
逐次目標対象物教示モデルとのマッチングをとって目標位置に到達するようにモデル補正量を求めるモデル補正量計算部と、
前記モデル補正量計算部にて求められたモデル補正量により目標値を変更する目標値変更部と、を備えたことを特徴とする請求項１記載の移動ロボット制御装置。
【請求項３】
前記駆動軸速度補正量計算部は、前記環境認識センサおよび前記内界センサによって取得した情報から自己位置推定を行って求められた推定位置を微分した後に移動平均を施し、その結果得られた値と前記速度指令生成部からの出力値との偏差を計算し、前記偏差を積分したものに位置ループゲイン乗算、逆運動学変換、フィルタによる平滑化の処理を施して駆動軸速度補正量を出力することを特徴とする請求項１または２記載の移動ロボット制御装置。
【請求項４】
前記モデル補正量計算部は、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法により前記目標対象物教示モデルとのマッチングを行って補正量を計算することを特徴とする請求項２記載の移動ロボット制御装置。
【請求項５】
前記目標対象物教示モデルは、地図のノード情報に付帯したノード特徴データより抽出して前記モデル補正量計算部に入力することを特徴とする請求項２または４記載の移動ロボット制御装置。
【請求項６】
前記自己位置推定は、環境認識センサおよび内界センサの情報を用いてカルマンフィルタにより位置推定を行うことを特徴とする請求項３記載の移動ロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、オフィス、工場等の施設内において、環境情報を計測して移動する移動ロボットに搭載された移動ロボット制御装置に関する。
【背景技術】
【０００２】
移動ロボットは、移動手段である車輪と床面とのすべりなどにより目標位置からの位置誤差が発生する。そのため、従来の移動ロボット制御装置では、誤差の補正のために超音波センサなどの外界センサを用いてロボット位置を計測し、目標位置とのずれを補正する方法が取られている。例えば、往復直進運動に限定して、ジャイロやエンコーダなどの内界センサと外界計測用の超音波センサとにより位置ずれを計測して、その結果から補正量を算出し、移動制御に使う方法が提案されている（例えば、特許文献１参照）。
また、超音波センサおよびデッドレコニングにより、絶対位置補正地点にロボットが到着していると判断された場合、超音波センサで周囲の壁までの距離を測定し、その距離データの値が理想値と一致していない場合に、位置、方向の補正を行うものもある（例えば、特許文献２参照）。
【０００３】
図７（ａ）および（ｂ）は、特許文献１における移動ロボット１０１の装置構成を示す。移動ロボット１０１を移動させる駆動手段１０２と、ジャイロおよび積分器から構成され本体の方向を計測する方向計測手段１０３と、超音波センサから構成され本体周囲の物体までの距離を計測する測距手段１０４と、前記駆動手段１０２を制御し移動ロボット１０１の走行を制御する走行制御手段１０５と、エンコーダなどから構成され移動ロボット１０１の走行距離を計測する走行距離検出手段１０６を備え、前記走行制御手段１０５は、方向計測手段１０３の出力に基づいて移動ロボット１０１を所定の目標方向に直進走行させる直進手段１０７と、走行距離検出手段１０６と測距手段１０４の情報から本体車輪の滑りを求める演算手段１０８を有する。このような構成において、方向計測手段１０３の出力に基づいて直進手段１０７にて直進方向に制御し、演算手段１０８にて走行距離検出手段１０６と測距手段１０４の情報から制御周期毎に位置ずれを計測して、補正動作を行っている。
【０００４】
図８（ａ）および（ｂ）は、特許文献２における移動ロボット１１１の装置構成を示す。
移動ロボット１１１は、モータを有し走行回転速度やステアリング角が制御されて動作する走行用車輪１１２、本体周囲の物体までの距離を計測する超音波センサ１１３、センサデータ取り込み手段１１７、絶対位置補正手段１１８、経路追従手段１１９を有し車輪回転速度・方向制御装置１１５に指令を与える演算処理装置１１４、走行用車輪１１２を制御する車輪回転速度・方向制御装置１１５、走行用車輪１１２の回転数・方向から移動ロボット１１１の位置・姿勢を求めるデッドレコニング装置１１６から構成される。
演算処理装置１１４において、センサデータ取り込み手段１１７は超音波センサ１１３を起動し、周囲の空き空間距離データを取り込む。この取り込み後センサデータ取り込み手段１１７はデッドレコニング装置１１６からロボットの位置座標及び進行方向角を読み込み、距離データと共に経路追従手段１１９に伝達する。経路追従手段１１９は距離データ、ロボットの位置座標及び進行方向角を読み込み、これらデータに基づいて現在位置が予め設定してある絶対位置補正地点であるかどうかを判断する。絶対位置補正地点にロボットが到着していると判断された場合、絶対位置補正手段１１８は、超音波センサ１１３を起動して周囲の壁までの距離を測定させる。そして、その距離データの値が理想値を示していない場合には、位置、方向の補正動作を行っている。
このように、従来の移動ロボット制御装置は、すべりなどによる位置誤差を補正するために超音波センサなどの外界センサを用いて、ロボット位置を計測し、目標位置とのずれを補正しているのである。
【特許文献１】特開平０５−２５００３２号公報（第３−４頁、図１）
【特許文献２】特開平０６−１４９３５８号公報（第２−３頁、図１）
【発明の開示】
【発明が解決しようとする課題】
【０００５】
特許文献１の移動ロボット制御装置は、超音波センサと内界センサを用い、制御周期毎に距離誤差によりすべり量を計算し補正を行っているが、動作が直進動作に限定されカーブを曲がる動作など曲線動作には対応できず、汎用的な動作には適用できないという問題がある。
また、特許文献２の移動ロボット制御装置は、停止している状態から補正動作が実行されるため、シームレスな連続移動ができずに移動時間が長くなるという問題がある。また、超音波センサによる補正量を、１つの取り付け面にあるセンサに関し、平面までの計測距離の大小により決定しているため、正確な値を求めておらず補正量の精度が低下するという問題や、平面以外の一般的な形状の周囲物体との位置関係を指定できないという問題もある。
本発明はこのような問題点に鑑みてなされたものであり、動作パターンを限定したり一旦停止したりすることなく、シームレスな動作によって位置決め精度を向上する移動ロボット制御装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、与えられた目標位置を基に直交座標系の速度指令を生成する速度指令生成部と、前記直交座標系の速度指令を各駆動軸の速度指令に変換する逆運動学変換部とを備えた移動ロボット制御装置において、前記逆運動学変換部から出力された各駆動軸速度指令にゲインを乗じて調整した値を出力する速度ゲイン乗算部と、移動ロボットから周囲物体までの距離を計測する環境認識センサと、前記移動ロボットの駆動機構の情報を取得する内界センサと、直交座標系の速度指令および前記環境認識センサおよび前記内界センサによって取得した情報から駆動軸速度補正量を計算する駆動軸速度補正量計算部と、前記駆動軸速度補正量計算部から出力された補正量に基づき前記速度ゲイン乗算部の出力値の補正を行う駆動軸補正部とを備えたことを特徴とするものである。
また、請求項２に記載の発明は、逐次目標対象物教示モデルとのマッチングをとって目標位置に到達するようにモデル補正量を求めるモデル補正量計算部と、前記モデル補正量計算部にて求められたモデル補正量により目標値を変更する目標値変更部とを備えたことを特徴とするものである。
また、請求項３に記載の発明は、前記駆動軸速度補正量計算部は、前記環境認識センサおよび前記内界センサによって取得した情報から自己位置推定を行って求められた推定位置を微分した後に移動平均を施し、その結果得られた値と前記速度指令生成部からの出力値との偏差を計算し、前記偏差を積分したものに位置ループゲイン乗算、逆運動学変換、フィルタによる平滑化の処理を施して駆動軸速度補正量を出力することを特徴とするものである。
また、請求項４に記載の発明は、前記モデル補正量計算部は、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）法により前記目標対象物教示モデルとのマッチングを行って補正量を計算することを特徴とするものである。
また、請求項５に記載の発明は、前記目標対象物教示モデルは、地図のノード情報に付帯したノード特徴データより抽出して前記モデル補正量計算部に入力することを特徴とするものである。
また、請求項６に記載の発明は、前記自己位置推定は、環境認識センサおよび内界センサの情報を用いてカルマンフィルタにより位置推定を行うことを特徴とするものである。
【発明の効果】
【０００７】
請求項１、３、６に記載の発明によると、自己位置推定値から制御周期毎に常に位置補正を行うようにしているため、動作パターンを限定したり一旦停止したりすることなく、シームレスな動作によって位置決め精度を向上することができる。
請求項２、４、５に記載の発明によると、逐次目標対象物教示モデルとのマッチングをとって目標値の補正を行うようにしているため、目標位置に不確かさを持った簡易的な地図においても一般的な形状の周囲物体との位置関係を指定して正確な補正量を求め、動作パターンを限定したり一旦停止したりすることなく、シームレスな動作によって位置決め精度を向上することができる。
【発明を実施するための最良の形態】
【０００８】
以下、本発明の実施の形態について図を参照して説明する。
【実施例１】
【０００９】
図１は、本発明の移動ロボット制御装置の装置構成を示す図である。移動ロボット制御装置２は、例えば、ＣＰＵやメモリを内蔵した計算機であり、速度指令生成部１１、逆運動学変換部１２、速度ゲイン乗算部１３、駆動軸速度補正量計算部１４、駆動軸補正部１５から構成され、センサなどの情報を入力して駆動軸に対する速度指令を計算し出力する。
速度指令生成部１１は、上位の制御装置から与えられる目標位置の情報を基に補間・加減速パターン算出を行い、直交座標系速度指令を生成する。逆運動学変換部１２は、速度指令生成部１１から出力された直交座標系の速度指令を駆動軸の速度指令に変換する。速度ゲイン乗算部１３は、逆運動学変換部１２から出力された駆動軸速度指令を調整して駆動軸フィードフォワード速度指令を出力する。駆動軸速度補正量計算部１４は、直交座標系速度指令および環境認識センサ、内界センサによる情報から駆動軸速度補正量を計算する。
駆動軸補正部１５は、駆動軸速度補正量計算部１４から出力された駆動軸速度補正量に基づき速度ゲイン乗算部１３の出力値の補正を制御周期毎に行い、駆動軸に対する速度指令である駆動軸補正速度指令を出力する。
【００１０】
図２（ａ）および（ｂ）は、図１の移動ロボット制御装置２を搭載した移動ロボットの装置構成を示す図である。移動ロボット１は、移動ロボット制御装置２、モータ制御装置３、モータ４、エンコーダ５、駆動機構６、環境認識センサ７から構成される。
モータ制御装置３は、移動ロボット制御装置２からの指令を受けモータ４の駆動制御を行う。モータ４は駆動機構６を駆動する。エンコーダ５は、モータ４の回転量を計測し、内界センサ情報を生成する。また、内界センサ情報はエンコーダだけでなくジャイロセンサを移動ロボット１に組み込んで適用するようにしても良い。駆動機構６は、モータ４の出力軸に接続された車輪あるいは操舵軸を複数組有し移動ロボット１を駆動する。
環境認識センサ７は、例えば２次元レーザレンジファインダのような２次元スキャン型距離センサであり、そのレーザ光線が、左右に傾いていない平面（スキャン平面）内において、センサ取付位置を中心にして扇状にスキャンされ、壁などの周囲物体までの距離を計測する。
このような構成において、移動ロボット制御装置２は、環境認識センサ７およびエンコーダ５からの情報を取り込み、駆動軸の速度指令を補正しながらモータ制御装置３に指令を出力してモータ４、駆動機構６を介して移動ロボット１を制御しているのである。
本発明が従来技術と異なる部分は、速度ゲイン乗算部１３、駆動軸速度補正量計算部１４、駆動軸補正部１５を備えた部分である。
【００１１】
図３は移動ロボット制御装置２において駆動軸補正速度指令を計算する処理を示すブロック図である。この図を用いて本発明における処理を説明する。
速度指令生成部１１から出力された直交座標系速度指令（ベクトル）Ｖｒｅｆをもとに、逆運動学変換部１２、速度ゲイン乗算部１３により、駆動軸フィードフォワード速度指令（ベクトル）ｗrefを次式により計算する。
【００１２】
ｗref  ＝  Ｋｆ Ｊ−１ Ｖｒｅｆ                 （式１）
【００１３】
ここでＪ−１はヤコビアンの逆行列、Ｋｆは速度ゲインの行列（対角行列）を表す。
次に、駆動軸速度補正量計算部１４にて、直交座標系速度指令（ベクトル）Ｖｒｅｆおよびセンサ情報を基にカルマンフィルタにより自己位置推定された推定位置（ベクトル）Ｐｅをもとに、位置偏差（ベクトル）ｅｐｏｓを次式により計算する。
【００１４】
ｅｐｏｓ  ＝  （Ｖｒｅｆ−Ｆｍ（ｓＰｅ））／ｓ  （式２）
【００１５】
ここでｓは、微分器をラプラス変換したもの、１／ｓは積分器をラプラス変換したものを表す。Ｆｍ（ｘ）は、ｘの移動平均を表す。
さらに位置偏差（ベクトル）ｅｐｏｓから駆動軸速度補正量（ベクトル）Δｗを次式により計算する。
【００１６】
Δｗ  ＝  Ｊ−１ＫＰｅｐｏｓ／（ｓＴ＋１）      （式３）
【００１７】
ここでＪ−１は、ヤコビアンの逆行列、ＫＰは、位置ループゲインの行列（対角行列）を表す。１／（ｓＴ＋１）は、平滑化フィルタを表し、ここでは、１次遅れフィルタを使用している。
最後に、駆動軸補正部１５にて、駆動軸フィードフォワード速度指令（ベクトル）ｗrefおよび駆動軸速度補正量（ベクトル）Δｗより駆動軸補正速度指令（ベクトル）ｗ’refを次式のように計算し、モータ制御装置３に出力する。
【００１８】
ｗ’ref  ＝  ｗref  ＋  Δｗ                    （式４）
【００１９】
このように、自己位置推定値から制御周期毎に常に位置補正を行うようにしているため、動作パターンを限定したり一旦停止したりすることなく、シームレスな動作によって位置決め精度を向上することができる。
【実施例２】
【００２０】
実施例２の実施の形態について図４を参照して説明する。図４は、本発明の移動ロボット制御装置の装置構成を示す図である。図４において、実施例１と異なるのは、モデル補正量計算部１６および目標値変更部１７を付加した部分である。モデル補正量計算部１６は、地図データより抽出した目標対象物教示モデルおよび環境認識センサ７からの情報から逐次マッチングをとって目標位置に到達するようにモデル補正量を計算する。目標値変更部１７は、モデル補正量計算部にて求められたモデル補正量により目標値を変更する。
図５に示すようにモデル補正量計算部１６にて、ＩＣＰ（Ｉｔｅｒａｔｉｖｅ  Ｃｌｏｓｅｓｔ  Ｐｏｉｎｔ）法により逐次目標対象物教示モデルとのマッチングを行って、現在センサ情報と目標対象物教示モデルのずれ量を求め、ずれ量の符号を反転し座標変換を行って補正量を計算する。
目標対象物教示モデルの地図データからの抽出は、図６に示すように予め教示されたノード情報に付帯したノード特徴データより、目標とする移動ロボット１の姿勢およびセンサのスキャン領域から、一部を切り出すようにして行う。ノード特徴データは、移動ロボット１がそのノードに到達した時の壁などの周囲物体との位置関係を３６０°分登録したものであり、環境認識センサ７を用いて予め教示される。図６の場合、目標とする移動ロボット１の姿勢が９０°でセンサのスキャン領域−４５°から４５°までの場合の目標対象物教示モデルを切り出した様子を示している。このように地図より抽出された目標対象物教示モデルとのマッチングを逐次とり、補正量が求められた場合は目標値を変更する。補正量が閾値以下になったら目標値変更をやめ、その状態で位置偏差（ベクトル）ｅｐｏｓが閾値以下になったら移動完了とする。
【００２１】
このように、逐次目標対象物教示モデルとのマッチングをとって目標値の補正を行うようにしているため、目標位置に不確かさを持った簡易的な地図においても一般的な形状の周囲物体との位置関係を指定して正確な補正量を求め、動作パターンの限定や一旦停止することもなく、シームレスな動作によって位置決め精度を向上することができる。
【図面の簡単な説明】
【００２２】
【図１】本発明の第１の実施例における移動ロボット制御装置の装置構成図
【図２】本発明の移動ロボット制御装置を移動ロボットに搭載した場合の装置構成図
【図３】本発明の移動ロボット制御装置における処理を示すブロック図
【図４】本発明の第２の実施例における移動ロボット制御装置の装置構成図
【図５】本発明の第２の実施例におけるモデル補正量を求める処理を説明する説明図
【図６】本発明の第２の実施例における目標対象物教示モデル抽出処理を説明する説明図
【図７】従来例１の移動ロボットの装置構成図
【図８】従来例２の移動ロボットの装置構成図
【符号の説明】
【００２３】
１  移動ロボット
２  移動ロボット制御装置
３  モータ制御装置
４  モータ
５  エンコーダ
６  駆動機構
７  環境認識センサ
１１  速度指令生成部
１２  逆運動学変換部
１３  速度ゲイン乗算部
１４  駆動軸速度補正量計算部
１５  駆動軸補正部
１６  モデル補正量計算部
１７  目標値変更部
１０１  移動ロボット
１０２  駆動手段
１０３  方向計測手段
１０４  測距手段
１０５  走行制御手段
１０６  走行距離検出手段
１０７  直進手段
１０８  演算手段
１１１  移動ロボット
１１２  走行用車輪
１１３  超音波センサ
１１４  演算処理装置
１１５  車輪回転速度・方向制御装置
１１６  デッドレコニング装置
１１７  センサデータ取り込み手段
１１８  絶対位置補正手段
１１９  経路追従手段
【図１】
0005057224.tif 000002
【図２】
0005057224.tif 000003
【図３】
0005057224.tif 000004
【図４】
0005057224.tif 000005
【図５】
0005057224.tif 000006
【図６】
0005057224.tif 000007
【図７】
0005057224.tif 000008
【図８】
0005057224.tif 000009